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