OpenRAVE Documentation

Source code for openravepy.databases.inversekinematics

# -*- coding: utf-8 -*-
# Copyright (C) 2009-2012 Rosen Diankov (rosen.diankov@gmail.com)
# 
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#     http://www.apache.org/licenses/LICENSE-2.0
# 
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Manages compiled inverse kinematics files for robots using ikfast.

.. image:: ../../images/databases/inversekinematics.jpg
  :width: 640

`[source] <../_modules/openravepy/databases/inversekinematics.html>`_

**Running:**

.. code-block:: bash

  openrave.py --database inversekinematics

Usage
-----

First set the active manipulator, and then instantiate the InverseKinematicsModel class specifying the iktype and free indices.

.. code-block:: python

  robot.SetActiveManipulator(...)
  ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterizationType.Transform6D)
  if not ikmodel.load():
      ikmodel.autogenerate()

The supported types are defined by `IkParameterizationType` and are propagated throughout the entire OpenRAVE framework. All solve methods take in a `IkParameterization` structure, which handles each IK type's serialization, distances metrics, derivatives, and transformation.

To show the manipulator and IK results do:

.. image:: ../../images/databases/inversekinematics_pr2show.jpg
  :height: 200

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --manipname=leftarm --show

It is also possible to test the IK on a scene:

.. code-block:: bash

  openrave.py --database inversekinematics --robot=data/pr2test1.env.xml --manipname=leftarm --show
  
Description
-----------

This database generator uses :ref:`ikfast_compiler` to generate optimized and stable analytic
inverse kinematics solvers for any robot manipulator. The manipulator's arm joints are used for
obtaining the joints to solve for. The user can specify the IK type (Rotation, Translation, Full 6D,
Ray 4D, etc), the free joints of the kinematics, and the precision. For example, generating the
right arm 6D IK for the PR2 robot where the free joint is the first joint and the free increment is
0.01 radians is:

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --manipname=rightarm  --freejoint=r_shoulder_pan_joint --freeinc=0.01

Generating the 3d rotation IK for the stage below is:

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/rotation_stage.robot.xml --iktype=rotation3d


.. image:: ../../images/databases/inversekinematics_rotation_stage.jpg
  :height: 200

Generating the ray inverse kinematics for the 4 degrees of freedom barrett wam is:

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/barrettwam4.robot.xml --iktype=ray4d
  openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --iktype=ray4d --manipname=rightarm_camera

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/neuronics-katana.zae --iktype=translationdirection5d --manipname=arm

The filename that the code is saved in can be retrieved by 

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/neuronics-katana.zae --iktype=translationdirection5d --manipname=arm --getfilename

Testing
-------

Every IK solver should be tested with the robot using ``--iktests=XXX``. However, calling
``inversekinematics`` will always re-generate the IK, even if one already exists. In order to just
run tests, it is possible to specify the ``--usecached`` option to prevent re-generation and
specifically test:

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/barrettwam.robot.xml --usecached --iktests=100

This will give the success rate along with information whether the IK gives a wrong results or fails
to find a solution.

If there are a lot of free joints in the IK solver, then their discretization can greatly affect
whether solutions are found or not. In this case, it is advisable to reduce the discretization
threshold by using the ``--freeinc`` option.

Loading from C++
----------------

It is possible to use the auto-generation process through c++ by loading the IKFast problem and
calling LoadIKFastSolver command.

`ikfastloader.cpp`_ - example for loading IK in C++.

Reference
---------

* :ref:`ikfast-database` - statistics and performance results of ikfast for many robots

* :ref:`ikfast_compiler` - details on the technology behind IKFast

.. _`ikfastloader.cpp`: ../../coreapihtml/ikfastloader_8cpp-example.html

Command-line
------------

.. shell-block:: openrave.py --database inversekinematics --help

Class Definitions
-----------------
"""
from __future__ import with_statement # for python 2.5
__author__ = 'Rosen Diankov'
__copyright__ = 'Copyright (C) 2009-2012 Rosen Diankov <rosen.diankov@gmail.com>'
__license__ = 'Apache License, Version 2.0'

if not __openravepy_build_doc__:
    from numpy import *
else:
    from numpy import array

from ..openravepy_ext import openrave_exception, RobotStateSaver
from ..openravepy_int import RaveCreateModule, RaveCreateIkSolver, IkParameterization, IkParameterizationType, RaveFindDatabaseFile, RaveDestroy, Environment, openravepyCompilerVersion, IkFilterOptions
from . import DatabaseGenerator
from ..misc import relpath, TSP
import time,platform,shutil,sys
import os.path
from os import getcwd, remove
import distutils
from distutils import ccompiler
from optparse import OptionParser

try:
    import cPickle as pickle
except:
    import pickle

import logging
log = logging.getLogger('openravepy.'+__name__.split('.',2)[-1])

[docs]class InverseKinematicsModel(DatabaseGenerator): """Generates analytical inverse-kinematics solutions, compiles them into a shared object/DLL, and sets the robot's iksolver. Only generates the models for the robot's active manipulator. To generate IK models for each manipulator in the robot, mulitple InverseKinematicsModel classes have to be created. """
[docs] class ArmVisibility: """When 'entered' will hide all the non-arm links in order to facilitate visiblity of the gripper""" def __init__(self,manip,transparency=1): self.manip = manip self.robot = self.manip.GetRobot() self.hiddengeoms = [] self.transparency = transparency def __enter__(self): self.hiddengeoms = [] with self.robot.GetEnv(): childlinks = self.robot.GetChain(self.manip.GetBase().GetIndex(),self.manip.GetEndEffector().GetIndex(),returnjoints=False) for link in self.robot.GetLinks(): if link not in childlinks: for geom in link.GetGeometries(): self.hiddengeoms.append((geom,geom.IsDraw(),geom.GetTransparency())) if self.transparency >= 1: geom.SetDraw(False) else: geom.SetDraw(True) geom.SetTransparency(self.transparency) def __exit__(self,type,value,traceback): with self.robot.GetEnv(): for geom,isdraw,tr in self.hiddengeoms: geom.SetDraw(isdraw) geom.SetTransparency(tr)
def __init__(self,robot=None,iktype=None,forceikfast=False,freeindices=None,freejoints=None,manip=None): """ :param robot: if not None, will use the robot's active manipulator :param manip: if not None, will the manipulator, takes precedence over robot :param forceikfast: if set will always force the ikfast solver """ self.ikfastproblem = None if manip is not None: robot = manip.GetRobot() else: manip = robot.GetActiveManipulator() DatabaseGenerator.__init__(self,robot=robot) self.manip = manip # check if robot manipulator has no static links (except the base) for link in robot.GetChain(manip.GetBase().GetIndex(),manip.GetEndEffector().GetIndex(),returnjoints=False)[1:]: for rigidlyattached in link.GetRigidlyAttachedLinks(): if rigidlyattached.IsStatic(): raise ValueError('link %s part of IK chain cannot be declared static'%str(link)) try: self.ikfast = __import__('openravepy.ikfast',fromlist=['openravepy']) except ImportError,e: log.warn('failed to import ikfast, so reverting to older version: %s',e) self.ikfast = __import__('openravepy.ikfast_sympy0_6',fromlist=['openravepy']) for handler in log.handlers: self.ikfast.log.addHandler(handler) self.ikfastproblem = RaveCreateModule(self.env,'ikfast') if self.ikfastproblem is not None: self.env.Add(self.ikfastproblem) self.iktype = iktype self.iksolver = None self.freeinc = None if freeindices is not None: self.freeindices = freeindices elif freejoints is not None: self.freeindices = self.getIndicesFromJointNames(freejoints) else: self.freeindices = None if self.freeindices is None: self.solveindices = None else: if not all([ifree in manip.GetArmIndices() for ifree in self.freeindices]): raise ValueError('not all free indices %s are part of the manipulator indices %s'%(self.freeindices,manip.GetArmIndices())) self.solveindices = [i for i in manip.GetArmIndices() if not i in self.freeindices] self.forceikfast = forceikfast self.ikfeasibility = None # if not None, ik is NOT feasibile and contains the error message self.statistics = dict() def __del__(self): if self.ikfastproblem is not None: try: self.env.Remove(self.ikfastproblem) except openrave_exception, e: log.debug('__del__ %s',e)
[docs] def clone(self,envother): clone = DatabaseGenerator.clone(self,envother) clone.ikfastproblem = RaveCreateModule(envother,'ikfast') if clone.ikfastproblem is not None: envother.Add(clone.ikfastproblem) if self.has(): clone.setrobot(self.freeinc) return clone
[docs] def has(self): return self.iksolver is not None and self.manip.GetIkSolver() is not None and self.manip.GetIkSolver().Supports(self.iktype)
[docs] def save(self): statsfilename=self.getstatsfilename(False) try: os.makedirs(os.path.split(statsfilename)[0]) except OSError: pass pickle.dump((self.getversion(),self.statistics,self.ikfeasibility,self.solveindices,self.freeindices,self.freeinc), open(statsfilename, 'w')) log.info('inversekinematics generation is done, compiled shared object: %s',self.getfilename(False))
[docs] def load(self,freeinc=None,checkforloaded=True,*args,**kwargs): try: filename = self.getstatsfilename(True) if len(filename) == 0: return checkforloaded and self.manip.GetIkSolver() is not None and self.manip.GetIkSolver().Supports(self.iktype) # might have ik already loaded modelversion,self.statistics,self.ikfeasibility,self.solveindices,self.freeindices,self.freeinc = pickle.load(open(filename, 'r')) if modelversion != self.getversion(): log.warn('version is wrong %s!=%s',modelversion,self.getversion()) return checkforloaded and self.manip.GetIkSolver() is not None and self.manip.GetIkSolver().Supports(self.iktype) # might have ik already loaded except Exception,e: log.warn(e) return checkforloaded and self.manip.GetIkSolver() is not None and self.manip.GetIkSolver().Supports(self.iktype) # might have ik already loaded if self.ikfeasibility is not None: # ik is infeasible, but load successfully completed, so return success return True return self.setrobot(freeinc,*args,**kwargs)
[docs] def getversion(self): return int(self.ikfast.__version__)
[docs] def getikname(self): return 'ikfast ikfast.%s.%s.%s'%(self.manip.GetKinematicsStructureHash(),str(self.iktype),self.manip.GetName())
[docs] def setrobot(self,freeinc=None): """Sets the ik solver on the robot. freeinc is a list of the delta increments of the freejoint values that can override the default values. """ if freeinc is not None: self.freeinc=freeinc if self.freeinc is not None: try: iksuffix = ' ' + ' '.join(str(f) for f in self.freeinc) except TypeError: # possibly just a float iksuffix = ' %f'%self.freeinc else: iksuffix = ' ' + ' '.join(str(f) for f in self.getDefaultFreeIncrements(0.1, 0.01)) # if self.manip.GetIkSolver() is not None: # self.iksolver = RaveCreateIkSolver(self.env,self.manip.GetIKSolverName()+iksuffix) if self.iksolver is None: with self.env: ikname = self.getikname() iktype = self.ikfastproblem.SendCommand('AddIkLibrary %s %s'%(ikname.split()[1],self.getfilename(True))) if iktype is None: if self.forceikfast: return False self.iksolver = RaveCreateIkSolver(self.env,self.manip.GetIkSolver().GetXMLId().split(' ',1)[0]+iksuffix) if self.manip.GetIkSolver() is not None else None else: if int(self.iktype) != int(iktype): raise ValueError('ik does not match types %s!=%s'%(self.iktype,iktype)) self.iksolver = RaveCreateIkSolver(self.env,ikname+iksuffix) if self.iksolver is not None and self.iksolver.Supports(self.iktype): success = self.manip.SetIKSolver(self.iksolver) if success and self.manip.GetIkSolver() is not None and self.freeinc is not None: freeincvalue = 0.01 try: if len(self.freeinc) > 0: freeincvalue = self.freeinc[0] except TypeError: freeincvalue = float(self.freeinc) self.manip.GetIkSolver().SendCommand('SetFreeIncrements %f 100 %f 10'%(freeincvalue,pi/8)) return success return self.has()
[docs] def getDefaultFreeIncrements(self,freeincrot, freeinctrans): """Returns a list of delta increments appropriate for each free index """ with self.env: values = [] eetrans = self.manip.GetEndEffectorTransform()[0:3,3] armlength = 0 orderedarmindices = [j for j in self.robot.GetDependencyOrderedJoints() if j.GetJointIndex() in self.manip.GetArmIndices()] for j in orderedarmindices[::-1]: armlength += sqrt(sum((eetrans-j.GetAnchor())**2)) eetrans = j.GetAnchor() freeinc = [] for index in self.freeindices: joint = self.robot.GetJointFromDOFIndex(index) if joint.IsRevolute(index-joint.GetDOFIndex()): freeinc.append(freeincrot) elif joint.IsPrismatic(index-joint.GetDOFIndex()): freeinc.append(freeinctrans*armlength) else: log.warn('cannot set increment for joint type %s'%joint.GetType()) return freeinc
[docs] def GetDefaultIndices(self): """Returns a default set of free indices if the robot has more joints than required by the IK. In the futrue, this function will contain heuristics in order to select the best indices candidates. """ if self.iktype is None: raise ValueError('ik type is not set') freeindices = [] dofexpected = IkParameterization.GetDOFFromType(self.iktype) remainingindices = list(self.manip.GetArmIndices()) if len(remainingindices) > dofexpected: N = len(remainingindices) # need to choose a free index so that # 1. the IK can be computed # 2. the IK has the most success rate (ie choose joint with least impact on performance) # # the compatiblity of the IK depends a lot on what axes are intersecting, and whether they are orthogonal with each other # In general, take from the top or the bottom depending on the complexity of the arm. robot=self.manip.GetRobot() jointanchors = [] jointaxes = [] for i,index in enumerate(self.manip.GetArmIndices()): joint=robot.GetJointFromDOFIndex(index) jointanchors.append(joint.GetAnchor()) jointaxes.append(joint.GetAxis(index-joint.GetDOFIndex())) intersectingaxes = eye(N) for i in range(N): for j in range(i+1,N): norm = cross(jointaxes[j], jointaxes[i]) diff = jointanchors[j]-jointanchors[i] if sum(norm**2) > 1e-7: # axes are not parallel if abs(dot(norm, diff)) <= 1e-5: # axes intersectingaxes[i,j] = intersectingaxes[j,i] = 1 else: # axes are parallel if sum(cross(jointaxes[i],diff)**2) <= 1e-10: intersectingaxes[i,j] = intersectingaxes[j,i] = 1 intersecting3axes = [0]*N num3intersecting = 0 for i in range(1,N-1): if intersectingaxes[i-1,i] and intersectingaxes[i,i+1] and intersectingaxes[i-1,i+1]: # have to check if they intersect at a common point intersection = jointanchors[i] + jointaxes[i] * dot(jointaxes[i], jointanchors[i-1]-jointanchors[i]) distfromintersection = sum(cross(jointaxes[i+1],intersection - jointanchors[i+1])**2) if distfromintersection < 1e-10: intersecting3axes[i-1] |= 1 << num3intersecting intersecting3axes[i] |= 1 << num3intersecting intersecting3axes[i+1] |= 1 << num3intersecting log.info('found 3-intersection centered on index %d', remainingindices[i]) num3intersecting += 1 for i in range(N - dofexpected): # by default always choose first indextopop = 0 if self.iktype == IkParameterizationType.Transform6D: if num3intersecting > 0: # try to preserve the intersecting axes # only choose wrist if wrist isn't intersecting and [2] is if intersecting3axes[2] > 0 and intersecting3axes[-1] == 0: indextopop = len(intersecting3axes)-1 else: hasother = False # prioritize 2 by checking if there exists other intersecting axes for j in range(len(intersecting3axes)-1,-1,-1): if (intersecting3axes[j] & ~intersecting3axes[2]) > 0: hasother = True if hasother: indextopop = 2 else: # prioritize the first index that is not in intersecting for j in range(len(intersecting3axes)-1,-1,-1): if intersecting3axes[j] == 0: indextopop = j break else: # already complicated enough, so take from the bottom in order to avoid variables coming inside the kinematics indextopop = 0 elif self.iktype == IkParameterizationType.Lookat3D: # usually head (rotation joints) are at the end #freeindices = remainingindices[len(remainingindices)-2:] #remainingindices=remainingindices[:-2] #len(remainingindices) indextopop = len(remainingindices)-1 elif self.iktype == IkParameterizationType.TranslationDirection5D: # check if ray aligns with furthest axis dirfromanchor = self.manip.GetTransform()[0:3,3]-jointanchors[-1] if abs(dot(jointaxes[-1],dot(self.manip.GetTransform()[0:3,0:3],self.manip.GetLocalToolDirection()))) > 0.99999 and abs(dot(jointaxes[-1],dirfromanchor)) > 0.99999*linalg.norm(dirfromanchor): # have to take the last index since last axis aligns indextopop = len(remainingindices)-1 else: indextopop = 0 else: # self.iktype == IkParameterizationType.Translation3D or self.iktype == IkParameterizationType.TranslationLocalGlobal6D # if not 6D, then don't need to worry about intersecting joints # so remove the least important joints indextopop = len(remainingindices)-1 freeindices.append(remainingindices.pop(indextopop)) jointanchors.pop(indextopop) jointaxes.pop(indextopop) # have to clear any intersecting axes mask = intersecting3axes.pop(indextopop) for j in range(len(intersecting3axes)): intersecting3axes[j] &= ~mask solveindices = [i for i in self.manip.GetArmIndices() if not i in freeindices] return solveindices,freeindices
[docs] def getfilename(self,read=False): if self.iktype is None: raise ValueError('ik type is not set') if self.solveindices is None or self.freeindices is None: solveindices, freeindices = self.GetDefaultIndices() else: solveindices, freeindices = self.solveindices, self.freeindices index = -1 allfreeindices = None while True: basename = 'ikfast%s.%s.%s.'%(self.getversion(),self.iktype,platform.machine()) + '_'.join(str(ind) for ind in sorted(solveindices)) if len(freeindices)>0: basename += '_f'+'_'.join(str(ind) for ind in sorted(freeindices)) filename = RaveFindDatabaseFile(os.path.join('kinematics.'+self.manip.GetKinematicsStructureHash(),ccompiler.new_compiler().shared_object_filename(basename=basename)),read) if not read or len(filename) > 0 or self.freeindices is not None: break # user did not specify a set of freeindices, so the expected behavior is to search for the next loadable one index += 1 dofexpected = IkParameterization.GetDOFFromType(self.iktype) if allfreeindices is None: allfreeindices = [f for f in self.ikfast.permutations(self.manip.GetArmIndices(),len(self.manip.GetArmIndices())-dofexpected)] if index >= len(allfreeindices): break freeindices = allfreeindices[index] solveindices = [i for i in self.manip.GetArmIndices() if not i in freeindices] return filename
[docs] def getsourcefilename(self,read=False,outputlang='cpp'): if self.iktype is None: raise ValueError('ik type is not set') if self.solveindices is None or self.freeindices is None: solveindices, freeindices = self.GetDefaultIndices() else: solveindices, freeindices = self.solveindices, self.freeindices basename = 'ikfast%s.%s.'%(self.getversion(),self.iktype) basename += '_'.join(str(ind) for ind in sorted(solveindices)) if len(freeindices)>0: basename += '_f'+'_'.join(str(ind) for ind in sorted(freeindices)) basename += '.' + outputlang return RaveFindDatabaseFile(os.path.join('kinematics.'+self.manip.GetKinematicsStructureHash(),basename),read)
[docs] def getstatsfilename(self,read=False): if self.iktype is None: raise ValueError('ik type is not set') if self.solveindices is None or self.freeindices is None: solveindices, freeindices = self.GetDefaultIndices() else: solveindices, freeindices = self.solveindices, self.freeindices index = -1 while True: freeindicesstrings = [] if len(freeindices)>0: for _freeindices in self.ikfast.permutations(freeindices): freeindicesstrings.append(['_f'+'_'.join(str(ind) for ind in sorted(_freeindices)),_freeindices]) else: freeindicesstrings.append(['',[]]) for freeindicesstring, fi in freeindicesstrings: basename = 'ikfast%s.%s.'%(self.getversion(),self.iktype) basename += '_'.join(str(ind) for ind in sorted(solveindices)) basename += freeindicesstring basename += '.pp' filename = RaveFindDatabaseFile(os.path.join('kinematics.'+self.manip.GetKinematicsStructureHash(),basename),read) if not read or len(filename) > 0 or self.freeindices is not None: self.freeindices = fi return filename # user did not specify a set of freeindices, so the expected behavior is to search for the next loadable one index += 1 dofexpected = IkParameterization.GetDOFFromType(self.iktype) allfreeindices = [f for f in self.ikfast.combinations(self.manip.GetArmIndices(),len(self.manip.GetArmIndices())-dofexpected)] if index >= len(allfreeindices): break freeindices = allfreeindices[index] solveindices = [i for i in self.manip.GetArmIndices() if not i in freeindices] return filename
[docs] def autogenerate(self,options=None): freejoints = None iktype = self.iktype precision = None forceikbuild = True outputlang = None ipython = None freeinc = None if options is not None: forceikbuild=options.force precision=options.precision if options.freejoints is not None: freejoints=options.freejoints outputlang=options.outputlang ipython=options.ipython if options.freeinc is not None: freeinc = [float64(s) for s in options.freeinc] if self.manip.GetKinematicsStructureHash() == 'f17f58ee53cc9d185c2634e721af7cd3': # wam 4dof if iktype is None: iktype=IkParameterizationType.Translation3D if iktype == IkParameterizationType.Translation3D and freejoints is None: freejoints = ['Shoulder_Roll'] elif self.manip.GetKinematicsStructureHash() == 'bfc61bd497e9993b85f1ab511ee7bdbc': # stage if iktype is None: iktype=IkParameterizationType.Rotation3D elif self.manip.GetKinematicsStructureHash() == 'c363859a2d7a151a22dc1e251d6d8669' or self.manip.GetKinematicsStructureHash() == '12ceb0aaa06143fe305efa6e48faae0b': # pr2 if iktype == None: iktype=IkParameterizationType.Transform6D if iktype == IkParameterizationType.Transform6D and freejoints is None: # take the torso and roll joint freejoints=[self.robot.GetJoints()[self.manip.GetArmIndices()[ind]].GetName() for ind in [0,3]] elif self.manip.GetKinematicsStructureHash()=='a1e9aea0dc0fda631ca376c03d500927' or self.manip.GetKinematicsStructureHash()=='ceb6be51bd14f345e22997cc0bca9f2f': # pr2 cameras if iktype is None: iktype=IkParameterizationType.Ray4D if freejoints is None: # take the torso joint freejoints=[self.robot.GetJoints()[self.manip.GetArmIndices()[0]].GetName()] elif self.manip.GetKinematicsStructureHash()=='2640ae411e0c87b03f56bf289296f9d8' and iktype == IkParameterizationType.Lookat3D: # pr2 head_torso if freejoints is None: freejoints=[self.robot.GetJoints()[self.manip.GetArmIndices()[0]].GetName()] elif self.manip.GetKinematicsStructureHash()=='ab9d03903279e44bc692e896791bcd05' or self.manip.GetKinematicsStructureHash()=='afe50514bf09aff5f2a84beb078bafbd': # katana if iktype==IkParameterizationType.Translation3D or (iktype==None and self.iktype==IkParameterizationType.Translation3D): freejoints = [self.robot.GetJoints()[ind].GetName() for ind in self.manip.GetArmIndices()[3:]] if iktype==None: iktype == IkParameterizationType.TranslationDirection5D self.generate(iktype=iktype,freejoints=freejoints,precision=precision,forceikbuild=forceikbuild,outputlang=outputlang,ipython=ipython) self.save()
[docs] def getIndicesFromJointNames(self,freejoints): freeindices = [] for jointname in freejoints: if type(jointname) == int: freeindices.append(jointname) else: # find the correct joint index dofindices = [joint.GetDOFIndex() for joint in self.robot.GetJoints() if joint.GetName()==jointname] if len(dofindices) == 0: raise LookupError("cannot find '%s' joint in %s robot"%(jointname,self.robot.GetName())) if not dofindices[0] in self.manip.GetArmIndices(): raise LookupError("cannot find joint '%s(%d)' in solve joints: %s"%(jointname,dofindices[0],self.manip.GetArmIndices())) freeindices.append(dofindices[0]) print 'getIndicesFromJointNames',freeindices,freejoints return freeindices
[docs] def generate(self,iktype=None,freejoints=None,freeinc=None,freeindices=None,precision=None,forceikbuild=True,outputlang=None,ipython=False): self.iksolver = None if iktype is not None: self.iktype = iktype if self.iktype is None: self.iktype = iktype = IkParameterizationType.Transform6D if self.iktype == IkParameterizationType.Rotation3D: Rbaseraw=self.manip.GetLocalToolTransform()[0:3,0:3] def solveFullIK_Rotation3D(*args,**kwargs): kwargs['Rbaseraw'] = Rbaseraw return self.ikfast.IKFastSolver.solveFullIK_Rotation3D(*args,**kwargs) solvefn=solveFullIK_Rotation3D elif self.iktype == IkParameterizationType.Direction3D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) def solveFullIK_Direction3D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir return self.ikfast.IKFastSolver.solveFullIK_Direction3D(*args,**kwargs) solvefn=solveFullIK_Direction3D elif self.iktype == IkParameterizationType.Ray4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_Ray4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos return self.ikfast.IKFastSolver.solveFullIK_Ray4D(*args,**kwargs) solvefn=solveFullIK_Ray4D elif self.iktype == IkParameterizationType.TranslationDirection5D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationDirection5D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos return self.ikfast.IKFastSolver.solveFullIK_TranslationDirection5D(*args,**kwargs) solvefn=solveFullIK_TranslationDirection5D elif self.iktype == IkParameterizationType.Translation3D: rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_Translation3D(*args,**kwargs): kwargs['rawbasepos'] = rawbasepos return self.ikfast.IKFastSolver.solveFullIK_Translation3D(*args,**kwargs) solvefn=solveFullIK_Translation3D elif self.iktype == IkParameterizationType.TranslationXY2D: rawbasepos=self.manip.GetLocalToolTransform()[0:2,3] def solveFullIK_TranslationXY2D(*args,**kwargs): kwargs['rawbasepos'] = rawbasepos return self.ikfast.IKFastSolver.solveFullIK_TranslationXY2D(*args,**kwargs) solvefn=solveFullIK_TranslationXY2D elif self.iktype == IkParameterizationType.TranslationXYOrientation3D: rawbasepos=self.manip.GetLocalToolTransform()[0:2,3] rawangle=normalizeAxisRotation([0,0,1],-self.manip.GetLocalToolTransform()[0:3,0:3])[0] def solveFullIK_TranslationXYOrientation3D(*args,**kwargs): kwargs['rawbasepos'] = rawbasepos kwargs['rawangle'] = rawangle return self.ikfast.IKFastSolver.solveFullIK_TranslationXYOrientation3D(*args,**kwargs) solvefn=solveFullIK_TranslationXYOrientation3D elif self.iktype == IkParameterizationType.Transform6D: Tgripperraw=self.manip.GetLocalToolTransform() def solveFullIK_6D(*args,**kwargs): kwargs['Tgripperraw'] = Tgripperraw return self.ikfast.IKFastSolver.solveFullIK_6D(*args,**kwargs) solvefn=solveFullIK_6D elif self.iktype == IkParameterizationType.Lookat3D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_Lookat3D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos return self.ikfast.IKFastSolver.solveFullIK_Lookat3D(*args,**kwargs) solvefn=solveFullIK_Lookat3D elif self.iktype == IkParameterizationType.TranslationLocalGlobal6D: Tgripperraw=self.manip.GetLocalToolTransform() def solveFullIK_TranslationLocalGlobal6D(*args,**kwargs): kwargs['Tgripperraw'] = Tgripperraw return self.ikfast.IKFastSolver.solveFullIK_TranslationLocalGlobal6D(*args,**kwargs) solvefn=solveFullIK_TranslationLocalGlobal6D elif self.iktype == IkParameterizationType.TranslationXAxisAngle4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationXAxisAngle4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos kwargs['rawglobaldir'] = [1.0,0.0,0.0] return self.ikfast.IKFastSolver.solveFullIK_TranslationAxisAngle4D(*args,**kwargs) solvefn=solveFullIK_TranslationXAxisAngle4D elif self.iktype == IkParameterizationType.TranslationYAxisAngle4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationYAxisAngle4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos kwargs['rawglobaldir'] = [0.0,1.0,0.0] return self.ikfast.IKFastSolver.solveFullIK_TranslationAxisAngle4D(*args,**kwargs) solvefn=solveFullIK_TranslationYAxisAngle4D elif self.iktype == IkParameterizationType.TranslationZAxisAngle4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationZAxisAngle4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos kwargs['rawglobaldir'] = [0.0,0.0,1.0] return self.ikfast.IKFastSolver.solveFullIK_TranslationAxisAngle4D(*args,**kwargs) solvefn=solveFullIK_TranslationZAxisAngle4D elif self.iktype == IkParameterizationType.TranslationXAxisAngleZNorm4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationXAxisAngleZNorm4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos kwargs['rawglobaldir'] = [1.0,0.0,0.0] kwargs['rawnormaldir'] = [0.0,0.0,1.0] return self.ikfast.IKFastSolver.solveFullIK_TranslationAxisAngle4D(*args,**kwargs) solvefn=solveFullIK_TranslationXAxisAngleZNorm4D elif self.iktype == IkParameterizationType.TranslationYAxisAngleXNorm4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationYAxisAngleXNorm4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos kwargs['rawglobaldir'] = [0.0,1.0,0.0] kwargs['rawnormaldir'] = [1.0,0.0,0.0] return self.ikfast.IKFastSolver.solveFullIK_TranslationAxisAngle4D(*args,**kwargs) solvefn=solveFullIK_TranslationYAxisAngleXNorm4D elif self.iktype == IkParameterizationType.TranslationZAxisAngleYNorm4D: rawbasedir=dot(self.manip.GetLocalToolTransform()[0:3,0:3],self.manip.GetDirection()) rawbasepos=self.manip.GetLocalToolTransform()[0:3,3] def solveFullIK_TranslationZAxisAngleYNorm4D(*args,**kwargs): kwargs['rawbasedir'] = rawbasedir kwargs['rawbasepos'] = rawbasepos kwargs['rawglobaldir'] = [0.0,0.0,1.0] kwargs['rawnormaldir'] = [0.0,1.0,0.0] return self.ikfast.IKFastSolver.solveFullIK_TranslationAxisAngle4D(*args,**kwargs) solvefn=solveFullIK_TranslationZAxisAngleYNorm4D else: raise ValueError('bad type') dofexpected = IkParameterization.GetDOFFromType(self.iktype) if freeindices is not None: self.freeindices = freeindices if self.freeindices is None: if freejoints is not None: self.freeindices = self.getIndicesFromJointNames(freejoints) else: self.solveindices,self.freeindices = self.GetDefaultIndices() self.solveindices = [i for i in self.manip.GetArmIndices() if not i in self.freeindices] if len(self.solveindices) != dofexpected: raise ValueError('number of joints to solve for is not equal to required joints %d!=%d'%(len(self.solveindices),dofexpected)) if freeinc is not None: self.freeinc = freeinc if self.freeinc is None: self.freeinc = self.getDefaultFreeIncrements(0.1,0.01) log.info('Generating inverse kinematics for manip %s: %s %s (this might take up to 10 min)',self.manip.GetName(),self.iktype,self.solveindices) if outputlang is None: outputlang = 'cpp' sourcefilename = self.getsourcefilename(False,outputlang) statsfilename = self.getstatsfilename(False) output_filename = self.getfilename(False) sourcedir = os.path.split(sourcefilename)[0] if forceikbuild or not os.path.isfile(sourcefilename): log.info('creating ik file %s',sourcefilename) try: os.makedirs(sourcedir) except OSError: pass solver = self.ikfast.IKFastSolver(kinbody=self.robot,kinematicshash=self.manip.GetKinematicsStructureHash(),precision=precision) if self.iktype == IkParameterizationType.TranslationXAxisAngle4D or self.iktype == IkParameterizationType.TranslationYAxisAngle4D or self.iktype == IkParameterizationType.TranslationZAxisAngle4D or self.iktype == IkParameterizationType.TranslationXAxisAngleZNorm4D or self.iktype == IkParameterizationType.TranslationYAxisAngleXNorm4D or self.iktype == IkParameterizationType.TranslationZAxisAngleYNorm4D: solver.useleftmultiply = False baselink=self.manip.GetBase().GetIndex() eelink=self.manip.GetEndEffector().GetIndex() if ipython: # requires ipython v0.11+ IPython = __import__('IPython') if IPython.__version__.startswith("0.10"): ipshell = IPython.Shell.IPShellEmbed(argv='',banner = 'inversekinematics dropping into ipython',exit_msg = 'Leaving Interpreter and continuing solver.') ipshell(local_ns=locals()) else: m=__import__('IPython.config.loader',fromlist=['Config']) Config = getattr(m,'Config') cfg = Config() cfg.InteractiveShellEmbed.local_ns = locals() cfg.InteractiveShellEmbed.global_ns = globals() IPython.embed(config=cfg, banner2 = 'inversekinematics dropping into ipython') from IPython.frontend.terminal.embed import InteractiveShellEmbed ipshell = InteractiveShellEmbed(config=cfg) reload(self.ikfast) # in case changes occurred try: generationstart = time.time() chaintree = solver.generateIkSolver(baselink=baselink,eelink=eelink,freeindices=self.freeindices,solvefn=solvefn) self.ikfeasibility = None code = solver.writeIkSolver(chaintree,lang=outputlang) if len(code) == 0: raise ValueError('failed to generate ik solver for robot %s:%s'%(self.robot.GetName(),self.manip.GetName())) self.statistics['generationtime'] = time.time()-generationstart self.statistics['usinglapack'] = solver.usinglapack open(sourcefilename,'w').write(code) try: from pkg_resources import resource_filename shutil.copyfile(resource_filename('openravepy','ikfast.h'), os.path.join(sourcedir,'ikfast.h')) except ImportError,e: log.warn(e) except self.ikfast.IKFastSolver.IKFeasibilityError, e: self.ikfeasibility = str(e) log.warn(e) if self.ikfeasibility is None: if outputlang == 'cpp': # compile the code and create the shared object compiler,compile_flags = self.getcompiler() try: output_dir = os.path.relpath('/',getcwd()) except AttributeError: # python 2.5 does not have os.path.relpath output_dir = relpath('/',getcwd()) platformsourcefilename = os.path.splitext(output_filename)[0]+'.cpp' # needed in order to prevent interference with machines with different architectures shutil.copyfile(sourcefilename, platformsourcefilename) objectfiles=[] try: objectfiles = compiler.compile(sources=[platformsourcefilename],macros=[('IKFAST_CLIBRARY',1),('IKFAST_NO_MAIN',1)],extra_postargs=compile_flags,output_dir=output_dir) # because some parts of ikfast require lapack, always try to link with it try: iswindows = sys.platform.startswith('win') or platform.system().lower() == 'windows' libraries = None if self.statistics.get('usinglapack',False) or not iswindows: libraries = ['lapack'] compiler.link_shared_object(objectfiles,output_filename=output_filename, libraries=libraries) except distutils.errors.LinkError,e: log.warn(e) if libraries is not None and 'lapack' in libraries: libraries.remove('lapack') if len(libraries) == 0: libraries = None log.info('linking again with %r... (MSVC bug?)',libraries) compiler.link_shared_object(objectfiles,output_filename=output_filename, libraries=libraries) if not self.setrobot(): return ValueError('failed to generate ik solver') finally: # cleanup intermediate files if os.path.isfile(platformsourcefilename): remove(platformsourcefilename) for objectfile in objectfiles: try: remove(objectfile) except: pass else: log.warn('cannot continue further if outputlang %s is not cpp',outputlang)
[docs] def perftiming(self,num): with self.env: results = self.ikfastproblem.SendCommand('PerfTiming num %d %s'%(num,self.getfilename(True))) return [double(s)*1e-9 for s in results.split()]
[docs] def testik(self,iktests): """Tests the iksolver. """ if self.ikfeasibility is not None: raise ValueError('ik is infeasible') with self.robot: self.robot.SetActiveManipulator(self.manip) # set base to identity to avoid complications when reporting errors self.robot.SetTransform(dot(linalg.inv(self.manip.GetBase().GetTransform()),self.robot.GetTransform())) cmd = 'DebugIK robot %s '%self.robot.GetName() if iktests.isdigit(): assert(int(iktests) > 0) cmd += 'numtests %d '%int(iktests) else: cmd += 'readfile %s '%iktests res = self.ikfastproblem.SendCommand(cmd).split() numtested = float(res[0]) successrate = float(res[1])/numtested solutionresults = [] index = 2 numvalues=1+IkParameterization.GetNumberOfValuesFromType(self.iktype)+self.manip.GetIkSolver().GetNumFreeParameters() for iresults in range(3): num = int(res[index]) index += 1 samples = reshape(array([float64(s) for s in res[index:(index+num*numvalues)]]),(num,numvalues)) solutionresults.append(samples) index += num*numvalues wrongrate = len(solutionresults[0])/numtested log.info('success rate: %f, wrong solutions: %f, no solutions: %f, missing solution: %f',float(res[1])/numtested,wrongrate,len(solutionresults[1])/numtested,len(solutionresults[2])/numtested) return successrate, wrongrate
[docs] def show(self,delay=0.1,options=None,forceclosure=True): if self.env.GetViewer() is None: self.env.SetViewer('qtcoin') time.sleep(0.4) # give time for viewer to initialize with RobotStateSaver(self.robot): with self.ArmVisibility(self.manip,0.95): time.sleep(3) # let viewer load self.setrobot(0.05) while True: with self.env: lower,upper = self.robot.GetDOFLimits(self.manip.GetArmIndices()) self.robot.SetDOFValues(lower+random.rand(len(lower))*(upper-lower),self.manip.GetArmIndices()) ikparam = self.manip.GetIkParameterization(self.iktype) sols = self.manip.FindIKSolutions(ikparam,IkFilterOptions.CheckEnvCollisions) weights = self.robot.GetDOFWeights(self.manip.GetArmIndices()) log.info('found %d solutions'%len(sols)) sols = TSP(sols,lambda x,y: sum(weights*(x-y)**2)) # find shortest route for sol in sols: self.robot.SetDOFValues(sol,self.manip.GetArmIndices()) self.env.UpdatePublishedBodies() time.sleep(delay)
@staticmethod
[docs] def getcompiler(): compiler = ccompiler.new_compiler() compile_flags = [] if compiler.compiler_type == 'msvc': compile_flags.append('/Ox') try: # make sure it is correct version! cname,cver = openravepyCompilerVersion().split() if cname == 'msvc': majorVersion = int(cver)/100-6 minorVersion = mod(int(cver),100)/10.0 if abs(compiler._MSVCCompiler__version - majorVersion+minorVersion) > 0.001: log.warn('default compiler v %s the same version as openrave compiler v %f, look for a different compiler',compiler._MSVCCompiler__version, majorVersion+minorVersion); distutils.msvc9compiler.VERSION = majorVersion + minorVersion newcompiler = ccompiler.new_compiler() if newcompiler is not None: compiler = newcompiler except Exception, e: log.warn(e) else: compiler.add_library('stdc++') if compiler.compiler_type == 'unix': compile_flags.append('-O3') compile_flags.append('-fPIC') return compiler,compile_flags
@staticmethod
[docs] def CreateOptionParser(): parser = DatabaseGenerator.CreateOptionParser() parser.description='Uses ikfast to compute the closed-form inverse kinematics equations of a robot manipulator, generates a C++ file, and compiles this file into a shared object which can then be loaded by OpenRAVE.' parser.usage='openrave.py --database inversekinematics [options]' parser.add_option('--freejoint', action='append', type='string', dest='freejoints',default=None, help='Optional joint name specifying a free parameter of the manipulator. The value of a free joint is known at runtime, but not known at IK generation time. If nothing specified, assumes all joints not solving for are free parameters. Can be specified multiple times for multiple free parameters.') parser.add_option('--precision', action='store', type='int', dest='precision',default=8, help='The precision to compute the inverse kinematics in, (default=%default).') parser.add_option('--usecached', action='store_false', dest='force',default=True, help='If set, will always try to use the cached ik c++ file, instead of generating a new one.') parser.add_option('--freeinc', action='append', type='float', dest='freeinc',default=None, help='The discretization value of freejoints.') parser.add_option('--numiktests','--iktests',action='store',type='string',dest='iktests',default=None, help='Will test the ik solver and return the success rate. IKTESTS can be an integer to specify number of random tests, it can also be a filename to specify the joint values of the manipulator to test. The formst of the filename is #numiktests [dof values]*') parser.add_option('--perftiming', action='store',type='int',dest='perftiming',default=None, help='Number of IK calls for measuring the internal ikfast solver.') parser.add_option('--outputlang', action='store',type='string',dest='outputlang',default=None, help='If specified, will output the generated code in that language (ie --outputlang=cpp).') parser.add_option('--ipython', '-i',action="store_true",dest='ipython',default=False, help='if true will drop into the ipython interpreter right before ikfast is called') parser.add_option('--iktype', action='store',type='string',dest='iktype',default=None, help='The ik type to build the solver current types are: %s'%(', '.join(iktype.name for iktype in IkParameterizationType.values.values() if not int(iktype) & IkParameterizationType.VelocityDataBit ))) return parser
@staticmethod
[docs] def RunFromParser(Model=None,parser=None,args=None,**kwargs): if parser is None: parser = InverseKinematicsModel.CreateOptionParser() (options, leftargs) = parser.parse_args(args=args) if options.iktype is not None: # cannot use .names due to python 2.5 (or is it boost version?) for value,type in IkParameterizationType.values.iteritems(): if type.name.lower() == options.iktype.lower(): iktype = type break else: iktype = IkParameterizationType.Transform6D Model = lambda robot: InverseKinematicsModel(robot=robot,iktype=iktype,forceikfast=True) robotatts={} if not options.show: robotatts = {'skipgeometry':'1'} model = DatabaseGenerator.RunFromParser(Model=Model,parser=parser,robotatts=robotatts,args=args,**kwargs) if options.iktests is not None or options.perftiming is not None: log.info('testing the success rate of robot %s ',options.robot) env = Environment() try: robot = env.ReadRobotXMLFile(options.robot,{'skipgeometry':'1'}) env.Add(robot) manip = None if options.manipname is None else robot.GetManipulator(options.manipname) ikmodel = InverseKinematicsModel(robot,iktype=model.iktype,forceikfast=True,freeindices=model.freeindices,manip=manip) if not ikmodel.load(freeinc=options.freeinc): raise ValueError('failed to load ik') if options.iktests is not None: successrate, wrongrate = ikmodel.testik(iktests=options.iktests) if wrongrate > 0: raise ValueError('wrong rate %f > 0!'%wrongrate) elif options.perftiming: results = array(ikmodel.perftiming(num=options.perftiming)) log.info('running time mean: %fs, median: %fs, min: %fs, max: %fs', mean(results),median(results),min(results),max(results)) finally: env.Destroy() RaveDestroy()
[docs]def run(*args,**kwargs): """Command-line execution of the example. ``args`` specifies a list of the arguments to the script. """ InverseKinematicsModel.RunFromParser(*args,**kwargs)

Questions/Feedback

Having problems with OpenRAVE?