def kinem_chain(self, name_frame_end, name_frame_base='odom'):
# Transform URDF to Chain() for the joints between 'name_frame_end' and 'name_frame_base'
self.chain = kdl.Chain()
ik_lambda = 0.35
try:
self.joint_names = self.urdf_model.get_chain(name_frame_base,
name_frame_end,
links=False,
fixed=False)
self.name_frame_in = name_frame_base
self.name_frame_out = name_frame_end
# rospy.loginfo("Will control the following joints: %s" %(self.joint_names))
self.kdl_tree = kdl_tree_from_urdf_model(self.urdf_model)
self.chain = self.kdl_tree.getChain(name_frame_base,
name_frame_end)
self.kdl_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.kdl_ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain)
self.kdl_ikv_solver.setLambda(ik_lambda)
self.nJoints = self.chain.getNrOfJoints()
# Default Task and Joint weights
self.tweights = np.identity(6)
# weight matrix with 1 in diagonal to make use of all the joints.
self.jweights = np.identity(self.nJoints)
self.kdl_ikv_solver.setWeightTS(self.tweights.tolist())
self.kdl_ikv_solver.setWeightJS(self.jweights.tolist())
# Fill the list with the joint limits
self.joint_limits_lower = np.empty(self.nJoints)
self.joint_limits_upper = np.empty(self.nJoints)
self.joint_vel_limits = np.empty(self.nJoints)
for n, jnt_name in enumerate(self.joint_names):
jnt = self.urdf_model.joint_map[jnt_name]
if jnt.limit is not None:
if jnt.limit.lower is None:
self.joint_limits_lower[n] = -0.07
else:
self.joint_limits_lower[n] = jnt.limit.lower
if jnt.limit.upper is None:
self.joint_limits_upper[n] = -0.07
else:
self.joint_limits_upper[n] = jnt.limit.upper
self.joint_vel_limits[n] = jnt.limit.velocity
self.joint_vel_limits[0] = 0.3
self.joint_vel_limits[1] = 0.3
except (RuntimeError, TypeError, NameError):
rospy.logerr("Unexpected error:", sys.exc_info()[0])
rospy.logerr('Could not re-init the kinematic chain')
self.name_frame_out = ''
def kinem_chain(self,
name_frame_end,
name_frame_base='triangle_base_link'):
# Transform URDF to Chain() for the joints between 'name_frame_end' and 'name_frame_base'
self.chain = kdl.Chain()
try:
self.joint_names = self.urdf_model.get_chain(name_frame_base,
name_frame_end,
links=False,
fixed=False)
self.name_frame_in = name_frame_base
self.name_frame_out = name_frame_end
self.njoints = len(self.joint_names)
self.kdl_tree = kdl_tree_from_urdf_model(self.urdf_model)
self.chain = self.kdl_tree.getChain(name_frame_base,
name_frame_end)
self.kdl_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.kdl_ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain)
self.kdl_ikv_solver.setLambda(self.ik_lambda)
# Default Task and Joint weights
self.tweights = np.identity(6)
# weight matrix with 1 in diagonal to make use of all the joints.
self.jweights = np.identity(self.njoints)
self.kdl_ikv_solver.setWeightTS(self.tweights.tolist())
self.kdl_ikv_solver.setWeightJS(self.jweights.tolist())
# Fill the list with the joint limits
self.joint_limits_lower = []
self.joint_limits_upper = []
for jnt_name in self.joint_names:
jnt = self.urdf_model.joint_map[jnt_name]
if jnt.limit is not None:
self.joint_limits_lower.append(jnt.limit.lower)
self.joint_limits_upper.append(jnt.limit.upper)
self.nJoints = self.chain.getNrOfJoints()
except:
rospy.logerr("Unexpected error:", sys.exc_info()[0])
rospy.logerr('Could not re-init the kinematic chain')
self.name_frame_out = ''
return self.chain
def configure(self, tf_base_link_name, tf_end_link_name):
self.tf_base_link_name = tf_base_link_name
self.tf_end_link_name = tf_end_link_name
#Variables holding the joint information
self.robot = URDF.from_parameter_server()
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(self.tf_base_link_name,
self.tf_end_link_name)
self.kdl_kin = KDLKinematics(self.robot, self.tf_base_link_name,
self.tf_end_link_name)
self.arm_joint_names = self.kdl_kin.get_joint_names()
self.jnt_pos = kdl.JntArray(self.chain.getNrOfJoints())
self.fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain)
self.ikv_solver.setLambda(0.0001)
#self.ikv_solver.setWeightTS([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,0,0,0,0],[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1]])
self.ik_solver = kdl.ChainIkSolverPos_NR(self.chain, self.fk_solver,
self.ikv_solver)
def inverse_kinematics(self,
position,
orientation=None,
seed=None,
min_joints=None,
max_joints=None,
w_x=None,
w_q=None,
maxiter=500,
eps=1.0e-6,
with_st=False):
if w_x is None and w_q is None:
ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
else:
ik_v_kdl = PyKDL.ChainIkSolverVel_wdls(self._arm_chain)
if w_x is not None: ik_v_kdl.setWeightTS(w_x) #TS = Task Space
if w_q is not None: ik_v_kdl.setWeightJS(w_q) #JS = Joint Space
pos = PyKDL.Vector(position[0], position[1], position[2])
if orientation is not None:
rot = PyKDL.Rotation()
rot = rot.Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
# Populate seed with current angles if not provided
seed_array = PyKDL.JntArray(self._num_jnts)
if seed is not None:
seed_array.resize(len(seed))
for idx, jnt in enumerate(seed):
seed_array[idx] = jnt
else:
seed_array = self.joints_to_kdl('positions')
# Make IK Call
if orientation is not None:
goal_pose = PyKDL.Frame(rot, pos)
else:
goal_pose = PyKDL.Frame(pos)
result_angles = PyKDL.JntArray(self._num_jnts)
# Make IK solver with joint limits
if min_joints is None: min_joints = self.joint_limits_lower
if max_joints is None: max_joints = self.joint_limits_upper
mins_kdl = PyKDL.JntArray(len(min_joints))
for idx, jnt in enumerate(min_joints):
mins_kdl[idx] = jnt
maxs_kdl = PyKDL.JntArray(len(max_joints))
for idx, jnt in enumerate(max_joints):
maxs_kdl[idx] = jnt
ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl,
maxs_kdl, self._fk_p_kdl,
ik_v_kdl, maxiter, eps)
if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(list(result_angles))
if with_st: return True, result
else: return result
else:
if with_st:
result = np.array(list(result_angles))
return False, result
else:
return None
