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()