def load_ikfast(robot,
iktype,
freejoints=['J6'],
freeinc=[0.01],
autogenerate=True):
"""
Load the IKFast solver
Parameters
----------
robot: orpy.Robot
The OpenRAVE robot
iktype: orpy.IkParameterizationType
Inverse kinematics type to be used
freeinc: list
The increment (discretization) to be used for the free DOF when the target
`iktype` is `TranslationDirection5D`
autogenerate: bool, optional
If true, auto-generate the IKFast solver
Returns
-------
success: bool
`True` if succeeded, `False` otherwise
"""
# Improve code readability
from openravepy.databases.inversekinematics import InverseKinematicsModel
# Initialize the ikmodel
if iktype == orpy.IkParameterizationType.TranslationDirection5D:
ikmodel = InverseKinematicsModel(robot,
iktype=iktype,
freejoints=freejoints)
elif iktype == orpy.IkParameterizationType.Transform6D:
ikmodel = InverseKinematicsModel(robot, iktype=iktype)
else:
raise TypeError('Unsupported ikmodel: {}'.format(iktype.name))
# Load or generate
if not ikmodel.load() and autogenerate:
print 'Generating IKFast {0}. Will take few minutes...'.format(
iktype.name)
if iktype == orpy.IkParameterizationType.Transform6D:
ikmodel.autogenerate()
elif iktype == orpy.IkParameterizationType.TranslationDirection5D:
ikmodel.generate(iktype=iktype, freejoints=freejoints)
ikmodel.save()
else:
ikmodel.autogenerate()
print 'IKFast {0} has been successfully generated'.format(iktype.name)
if iktype == orpy.IkParameterizationType.TranslationDirection5D:
success = ikmodel.load(freeinc=freeinc)
elif iktype == orpy.IkParameterizationType.Transform6D:
success = ikmodel.load()
return success
def _SetupIK(self, iktype):
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
iktype=iktype)
if not self.ikmodel.load():
self.ikmodel.generate(iktype=iktype, precision=4,
freeindices=[ self.GetIndices()[2] ])
self.ikmodel.save()
def load_ikfast(self, freeinc=np.pi / 6.):
success = False
from openravepy.databases.inversekinematics import InverseKinematicsModel
manipulators = [self.left_manip, self.right_manip]
if None not in manipulators:
for manip in manipulators:
ikmodel = InverseKinematicsModel(self.robot,
iktype=self.iktype,
manip=manip)
success = ikmodel.load(freeinc=[freeinc])
if not success:
break
return success
def _SetupIK(self, iktype):
""" creates Ikfast file for chosen manipulator
"""
from openravepy.databases.inversekinematics import InverseKinematicsModel
robot = self.GetRobot()
self.ikmodel = InverseKinematicsModel(robot=robot,
manip=self,
iktype=iktype)
if not self.ikmodel.load():
if (self.namespace == 'arm'):
self.ikmodel.generate(iktype=iktype,
precision=4,
freeindices=[self.GetIndices()[2]])
self.ikmodel.save()
else:
self.ikmodel.generate(
iktype=iktype,
precision=4,
freeindices=[self.GetIndices()[4],
self.GetIndices()[3]])
self.ikmodel.save()
