OpenRAVE Documentation

inversekinematicspick Module

Creates an object whose transform is used to solve IK solutions for each robot manipulator.

../../_images/inversekinematicspick.jpg

Running the Example:

openrave.py --example inversekinematicspick

Description

Will create a picker for each manipulator in each robot. Can change the scene with:

openrave.py --example inversekinematicspick --scene=data/pr2test1.env.xml

Can also change the inverse kinematics type to use by:

openrave.py --example inversekinematicspick --scene=data/katanatable.env.xml --iktype=translation3d

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)
  --iktype=IKTYPE       The ik type to build the solver current types are:
                        VelocityDataBit, Transform6D, Rotation3D,
                        Translation3D, Direction3D, Ray4D, Lookat3D,
                        TranslationDirection5D, TranslationXY2D,
                        TranslationXYOrientation3D, TranslationLocalGlobal6D,
                        TranslationXAxisAngle4D, TranslationYAxisAngle4D,
                        TranslationZAxisAngle4D, TranslationXAxisAngleZNorm4D,
                        TranslationYAxisAngleXNorm4D,
                        TranslationZAxisAngleYNorm4D, Translation3DVelocity,
                        Direction3DVelocity, Ray4DVelocity,
                        Transform6DVelocity, Lookat3DVelocity,
                        TranslationDirection5DVelocity,
                        TranslationXY2DVelocity,
                        TranslationXYOrientation3DVelocity,
                        TranslationLocalGlobal6DVelocity, Rotation3DVelocity,
                        TranslationXAxisAngle4DVelocity,
                        TranslationYAxisAngle4DVelocity,
                        TranslationZAxisAngle4DVelocity,
                        TranslationXAxisAngleZNorm4DVelocity,
                        TranslationYAxisAngleXNorm4DVelocity, CustomDataBit,
                        TranslationZAxisAngleYNorm4DVelocity, UniqueIdMask

  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."
    if options.iktype is not None:
        # cannot use .names due to python 2.5 (or is it boost version?)
        for value,type in IkParameterization.Type.values.iteritems():
            if type.name.lower() == options.iktype.lower():
                iktype = type
                break
    else:
        iktype = IkParameterizationType.Transform6D
        
    env.Load(options.scene)
    with env:
        # load the Transform6D IK models
        ikmodels = []
        for robot in env.GetRobots():
            for manip in robot.GetManipulators():
                print manip
                try:
                    robot.SetActiveManipulator(manip)
                    ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
                    if not ikmodel.load():
                        ikmodel.autogenerate()
                        if not ikmodel.has():
                            continue
                    ikmodels.append(ikmodel)
                except Exception, e:
                    print 'failed manip %s'%manip, e
                
        if len(ikmodels) == 0:
            raveLogWarn('no manipulators found that can be loaded with iktype %s'%str(iktype))
            
        # create the pickers
        pickers = []
        for ikmodel in ikmodels:
            raveLogInfo('creating picker for %s'%str(ikmodel.manip))
            picker = RaveCreateKinBody(env,'')
            picker.InitFromBoxes(array([ [0.05,0,0,0.05,0.01,0.01], [0,0.05,0,0.01,0.05,0.01], [0,0,0.05,0.01,0.01,0.05] ]),True)
            for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
                color = zeros(3)
                color[igeom] = 1
                geom.SetDiffuseColor(color)
            picker.SetName(ikmodel.manip.GetName())
            env.Add(picker,True)
            # have to disable since not part of collision 
            picker.Enable(False)
            picker.SetTransform(ikmodel.manip.GetTransform())
            pickers.append([picker,ikmodel.manip])
              
    raveLogInfo('pickers loaded, try moving them')
    def PickerThread(env,pickers,iktype):
        """this function runs in a separate thread monitoring each of the pickers
        """
        Tpickers = [None]*len(pickers)
        while True:
            if env.GetViewer() is None:
                break
            with env:
                for ipicker,(picker,manip) in enumerate(pickers):
                    T=picker.GetTransform()
                    if Tpickers[ipicker] is not None and sum(abs(Tpickers[ipicker]-T).flatten()) <= 1e-10:
                        continue
                    data = None
                    if iktype == IkParameterizationType.Direction3D:
                        data = T[0:3,2]
                    elif iktype == IkParameterizationType.Lookat3D:
                        data = T[0:3,3]
                    elif iktype == IkParameterizationType.Ray4D:
                        data = Ray(T[0:3,3],T[0:3,2])
                    elif iktype == IkParameterizationType.Rotation3D:
                        data = T[0:3,0:3]
                    elif iktype == IkParameterizationType.Transform6D:
                        data = T
                    elif iktype == IkParameterizationType.Translation3D:
                        data = T[0:3,3]
                    elif iktype == IkParameterizationType.TranslationDirection5D:
                        ikparam = Ray(T[0:3,3],T[0:3,2])
                    elif iktype == IkParameterizationType.TranslationLocalGlobal6D:
                        ikparam = [T[0:3,3],zeros(3)]
                    else:
                        raveLogWarn('iktype %s unsupported'%str(iktype))
                        continue
                    sol = manip.FindIKSolution(IkParameterization(data,iktype),IkFilterOptions.CheckEnvCollisions)
                    if sol is not None:
                        robot.SetDOFValues(sol,manip.GetArmIndices())
                        # have to update all other pickers since two manipulators can be connected to the same joints (torso)
                        for ipicker2, (picker2,manip2) in enumerate(pickers):
                            Tpickers[ipicker2] = manip2.GetTransform()
                            picker2.SetTransform(manip2.GetTransform())
                    # changing color produces bugs with qtcoin
#                     for igeom,geom in enumerate(picker.GetLinks()[0].GetGeometries()):
#                         color = zeros(3)
#                         color[igeom] = 0.4 if sol is None else 1.0
#                         geom.SetDiffuseColor(color)
                    Tpickers[ipicker] = T

            # update rate of 0.05s
            time.sleep(0.05)

    # create the thread
    t = threading.Thread(target=PickerThread,args=[env,pickers,iktype])
    t.start()
    while True:
        t.join(1)
    

Class Definitions

openravepy.examples.inversekinematicspick.RaveCreateKinBody((Environment)env, (str)name) → KinBody :

OPENRAVE_API KinBodyPtr RaveCreateKinBody(EnvironmentBasePtr penv, const std::string & name = “” )

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

Main example code.

openravepy.examples.inversekinematicspick.raveLogInfo((str)log) → None :

Send an info log to the openrave system

openravepy.examples.inversekinematicspick.raveLogWarn((str)log) → None :

Send a warn log to the openrave system

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