OpenRAVE Documentation

tutorial_iksolutions Module

Shows how to call an IK solver and render all the solutions.

../../_images/tutorial_iksolutions.jpg

Running the Example:

openrave.py --example tutorial_iksolutions

Description

First the inversekinematics database generator is called querying a Transform6D IK.

ikmodel = database.inversekinematics.InverseKinematicsModel(robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

Then a collision-free random configuration is set on the robot:

lower,upper = [v[ikmodel.manip.GetArmIndices()] for v in ikmodel.robot.GetDOFLimits()]
robot.SetDOFValues(random.rand()*(upper-lower)+lower,ikmodel.manip.GetArmIndices()) # set random values
if not robot.CheckSelfCollision():
    ...

Finally all the IK solutions are computed

solutions = ikmodel.manip.FindIKSolutions(ikmodel.manip.GetTransform(),True)

In order to render the ik solutions, create a new robot for every solution and make it trasparent

newrobot = RaveCreateRobot(env,robot.GetXMLId())
newrobot.Clone(robot,0)
for link in newrobot.GetLinks():
    for geom in link.GetGeometries():
        geom.SetTransparency(transparency)
env.Add(newrobot,True)
newrobot.SetTransform(robot.GetTransform())
newrobot.SetDOFValues(solution,ikmodel.manip.GetArmIndices())

Command-line

Usage: openrave.py [options]

Shows how to generate a 6D inverse kinematics solver and use it for getting
all solutions.

Options:
  -h, --help            show this help message and exit
  --scene=SCENE         Scene file to load (default=data/lab1.env.xml)
  --transparency=TRANSPARENCY
                        Transparency for every robot (default=0.8)
  --maxnumber=MAXNUMBER
                        Max number of robots to render (default=10)
  --manipname=MANIPNAME
                        name of manipulator to use (default=none)

  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)
    robot = env.GetRobots()[0]
    if options.manipname is not None:
        robot.SetActiveManipulator(options.manipname)
    newrobots = []
    for ind in range(options.maxnumber):
        newrobot = RaveCreateRobot(env,robot.GetXMLId())
        newrobot.Clone(robot,0)
        for link in newrobot.GetLinks():
            for geom in link.GetGeometries():
                geom.SetTransparency(options.transparency)
        newrobots.append(newrobot)
    for link in robot.GetLinks():
        for geom in link.GetGeometries():
            geom.SetTransparency(options.transparency)

    while True:
        # generate the ik solver
        ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=IkParameterization.Type.Transform6D)
        if not ikmodel.load():
            ikmodel.autogenerate()
        with env:
            # move the robot in a random collision-free position and call the IK
            while True:
                lower,upper = [v[ikmodel.manip.GetArmIndices()] for v in ikmodel.robot.GetDOFLimits()]
                robot.SetDOFValues(random.rand()*(upper-lower)+lower,ikmodel.manip.GetArmIndices()) # set random values
                if not robot.CheckSelfCollision():
                    solutions = ikmodel.manip.FindIKSolutions(ikmodel.manip.GetTransform(),IkFilterOptions.CheckEnvCollisions)
                    if solutions is not None and len(solutions) > 0: # if found, then break
                        break
            
            print 'found %d solutions, rendering solutions:'%len(solutions)
            if len(solutions) < options.maxnumber:
                inds = range(len(solutions))
            else:
                inds = array(linspace(0,len(solutions)-1,options.maxnumber),int)
            for i,ind in enumerate(inds):
                print ind
                newrobot = newrobots[i]
                env.Add(newrobot,True)
                newrobot.SetTransform(robot.GetTransform())
                newrobot.SetDOFValues(solutions[ind],ikmodel.manip.GetArmIndices())

        env.UpdatePublishedBodies()
        print('waiting...')
        time.sleep(20)
        # remove the robots
        for newrobot in newrobots:
            env.Remove(newrobot)
    del newrobots

Class Definitions

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

Main example code.

openravepy.examples.tutorial_iksolutions.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?