OpenRAVE Documentation

tutorial_iklookat_multiple Module

Shows how to use lookat inverse kinematics to have multiple robots maintain line of sight while avoiding collisions.

../../_images/tutorial_iklookat_multiple.jpg

Running the Example:

openrave.py --example tutorial_iklookat_multiple

Command-line

Usage: openrave.py [options]

Shows how to use different IK solutions for arms with few joints.

Options:
  -h, --help            show this help message and exit
  --scene=SCENE         Scene file to load (default=data/pr2wam_test1.env.xml)

  OpenRAVE Environment Options:
    --loadplugin=_LOADPLUGINS
                        List all plugins and the interfaces they provide.
    --collision=_COLLISION
                        Default collision checker to use
    --physics=_PHYSICS  physics engine to use (default=none)
    --viewer=_VIEWER    viewer to use (default=qtcoin)
    --server=_SERVER    server to use (default=None).
    --serverport=_SERVERPORT
                        port to load server on (default=4765).
    --module=_MODULES   module to load, can specify multiple modules. Two
                        arguments are required: "name" "args".
    -l _LEVEL, --level=_LEVEL, --log_level=_LEVEL
                        Debug level, one of
                        (fatal,error,warn,info,debug,verbose,verifyplans)
    --testmode          if set, will run the program in a finite amount of
                        time and spend computation time validating results.
                        Used for testing

Main Python Code

def main(env,options):
    "Main example code."
    env.Load(options.scene)
    robots = env.GetRobots()
    manips = [env.GetRobot('pr2').GetManipulator('head_torso'), env.GetRobot('BarrettWAM').GetManipulator('arm')]
    ikmodels = []
    freeindices=[None,manips[1].GetArmIndices()[:-2]]
    for imanip,manip in enumerate(manips):
        # generate the ik solver
        manip.GetRobot().SetActiveManipulator(manip)
        ikmodel = databases.inversekinematics.InverseKinematicsModel(manip.GetRobot(), iktype=IkParameterization.Type.Lookat3D,freeindices=freeindices[imanip])
        if not ikmodel.load():
            ikmodel.autogenerate()
        ikmodels.append(ikmodel)

    while True:
        maxsolutions = 40
        goodsolutions = []
        with env:
            # move the robot in a random collision-free position and call the IK
            while True:
                target=ikmodel.manip.GetTransform()[0:3,3]+(random.rand(3)-0.5)
                robotsolutions = []
                for ikmodel in ikmodels:
                    for ikmodel2 in ikmodels:
                        ikmodel2.robot.Enable(ikmodel==ikmodel2)
                    solutions = ikmodel.manip.FindIKSolutions(IkParameterization(target,IkParameterization.Type.Lookat3D),IkFilterOptions.CheckEnvCollisions)
                    if len(solutions) == 0:
                        break
                    robotsolutions.append(solutions)
                for ikmodel2 in ikmodels:
                    ikmodel2.robot.Enable(True)
                if len(robotsolutions) == len(robots):
                    print 'found solutions for all manipulators, search for a joint collision-free one'
                    goodsolutions = []
                    # permute randomly to get more interesting solutions
                    allsols = [sols for sols in sequence_cross_product(*robotsolutions)]
                    stdrandom.shuffle(allsols)
                    for sols in allsols:
                        for ikmodel,sol in izip(ikmodels,sols):
                            ikmodel.robot.SetDOFValues(sol,ikmodel.manip.GetArmIndices())
                        if not any([ikmodel.robot.CheckSelfCollision() or env.CheckCollision(ikmodel.robot) for ikmodel in ikmodels]):
                            goodsolutions.append(sols)
                            if len(goodsolutions) >= maxsolutions:
                                break
                    if len(goodsolutions) > 0: # found solutions, so break!
                        break
                    
        handles = [env.plot3(array([target]),20.0)]
        for sols in goodsolutions:
            handlerays = []
            with env:
                for ikmodel,sol in izip(ikmodels,sols):
                    ikmodel.robot.SetDOFValues(sol,ikmodel.manip.GetArmIndices())
                    T = ikmodel.manip.GetTransform()
                    globaldir = numpy.dot(T[0:3,0:3],ikmodel.manip.GetDirection())
                    dist = linalg.norm(T[0:3,3]-target)+0.4
                    handlerays.append(env.drawlinelist(array([T[0:3,3], T[0:3,3]+dist*globaldir]),5,colors=[0.1,0.1,1]))
                env.UpdatePublishedBodies()
            time.sleep(0.1)

Class Definitions

openravepy.examples.tutorial_iklookat_multiple.main(env, options)[source]

Main example code.

openravepy.examples.tutorial_iklookat_multiple.run(*args, **kwargs)[source]

Command-line execution of the example.

Parameters:args – arguments for script to parse, if not specified will use sys.argv

Questions/Feedback

Having problems with OpenRAVE?