00001
00002
00003
00004
00005
00006 from trac_ik_python.trac_ik_wrap import TRAC_IK
00007 import rospy
00008
00009
00010 class IK(object):
00011 def __init__(self, base_link, tip_link,
00012 timeout=0.005, epsilon=1e-5, solve_type="Speed",
00013 urdf_string=None):
00014 """
00015 Create a TRAC_IK instance and keep track of it.
00016
00017 :param str base_link: Starting link of the chain.
00018 :param str tip_link: Last link of the chain.
00019 :param float timeout: Timeout in seconds for the IK calls.
00020 :param float epsilon: Error epsilon.
00021 :param solve_type str: Type of solver, can be:
00022 Speed (default), Distance, Manipulation1, Manipulation2
00023 :param urdf_string str: Optional arg, if not given URDF is taken from
00024 the param server at /robot_description.
00025 """
00026 if urdf_string is None:
00027 urdf_string = rospy.get_param('/robot_description')
00028 self._urdf_string = urdf_string
00029 self._timeout = timeout
00030 self._epsilon = epsilon
00031 self._solve_type = solve_type
00032 self.base_link = base_link
00033 self.tip_link = tip_link
00034 self._ik_solver = TRAC_IK(self.base_link,
00035 self.tip_link,
00036 self._urdf_string,
00037 self._timeout,
00038 self._epsilon,
00039 self._solve_type)
00040 self.number_of_joints = self._ik_solver.getNrOfJointsInChain()
00041 self.joint_names = self._ik_solver.getJointNamesInChain(
00042 self._urdf_string)
00043 self.link_names = self._ik_solver.getLinkNamesInChain()
00044
00045 def get_ik(self, qinit,
00046 x, y, z,
00047 rx, ry, rz, rw,
00048 bx=1e-5, by=1e-5, bz=1e-5,
00049 brx=1e-3, bry=1e-3, brz=1e-3):
00050 """
00051 Do the IK call.
00052
00053 :param list of float qinit: Initial status of the joints as seed.
00054 :param float x: X coordinates in base_frame.
00055 :param float y: Y coordinates in base_frame.
00056 :param float z: Z coordinates in base_frame.
00057 :param float rx: X quaternion coordinate.
00058 :param float ry: Y quaternion coordinate.
00059 :param float rz: Z quaternion coordinate.
00060 :param float rw: W quaternion coordinate.
00061 :param float bx: X allowed bound.
00062 :param float by: Y allowed bound.
00063 :param float bz: Z allowed bound.
00064 :param float brx: rotation over X allowed bound.
00065 :param float bry: rotation over Y allowed bound.
00066 :param float brz: rotation over Z allowed bound.
00067
00068 :return: joint values or None if no solution found.
00069 :rtype: tuple of float.
00070 """
00071 if len(qinit) != self.number_of_joints:
00072 raise Exception("qinit has length %i and it should have length %i" % (
00073 len(qinit), self.number_of_joints))
00074 solution = self._ik_solver.CartToJnt(qinit,
00075 x, y, z,
00076 rx, ry, rz, rw,
00077 bx, by, bz,
00078 brx, bry, brz)
00079 if solution:
00080 return solution
00081 else:
00082 return None
00083
00084 def get_joint_limits(self):
00085 """
00086 Return lower bound limits and upper bound limits for all the joints
00087 in the order of the joint names.
00088 """
00089 lb = self._ik_solver.getLowerBoundLimits()
00090 ub = self._ik_solver.getUpperBoundLimits()
00091 return lb, ub
00092
00093 def set_joint_limits(self, lower_bounds, upper_bounds):
00094 """
00095 Set joint limits for all the joints.
00096
00097 :arg list lower_bounds: List of float of the lower bound limits for
00098 all joints.
00099 :arg list upper_bounds: List of float of the upper bound limits for
00100 all joints.
00101 """
00102 if len(lower_bounds) != self.number_of_joints:
00103 raise Exception("lower_bounds array size mismatch, it's size %i, should be %i" % (
00104 len(lower_bounds),
00105 self.number_of_joints))
00106
00107 if len(upper_bounds) != self.number_of_joints:
00108 raise Exception("upper_bounds array size mismatch, it's size %i, should be %i" % (
00109 len(upper_bounds),
00110 self.number_of_joints))
00111 self._ik_solver.setKDLLimits(lower_bounds, upper_bounds)