# -*- 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)