def forward_velocity_kinematics(self, joint_velocities=None): """Solve forward vel-kine : joint-vel -> ee-vels """ end_frame = PyKDL.FrameVel() self._fk_v_kdl.JntToCart( self.joints_to_kdl('velocities', joint_velocities), end_frame) return end_frame.GetTwist()
def compute_fk_velocity(self, joint_positions, joint_velocities, des_frame): """ Given joint_positions and joint velocities, compute the velocities of des_frame with respect to the base frame :param joint_positions: joint positions :param joint_velocities: joint velocities :param des_frame: end frame :type joint_positions: np.ndarray :type joint_velocities: np.ndarray :type des_frame: string :return: translational and rotational velocities (vx, vy, vz, wx, wy, wz) :rtype: np.ndarray """ kdl_end_frame = kdl.FrameVel() kdl_jnt_angles = prutil.joints_to_kdl(joint_positions) kdl_jnt_vels = prutil.joints_to_kdl(joint_velocities) kdl_jnt_qvels = kdl.JntArrayVel(kdl_jnt_angles, kdl_jnt_vels) idx = self.arm_link_names.index(des_frame) + 1 fg = self.fk_solver_vel.JntToCart(kdl_jnt_qvels, kdl_end_frame, idx) assert fg == 0, 'KDL Vel JntToCart error!' end_twist = kdl_end_frame.GetTwist() return np.array([end_twist[0], end_twist[1], end_twist[2], end_twist[3], end_twist[4], end_twist[5]])
def compute_fk_velocity(self, jpos, jvel, tgt_frame): """ Given joint_positions and joint velocities, compute the velocities of tgt_frame with respect to the base frame. Args: jpos (list or flattened np.ndarray): joint positions. jvel (list or flattened np.ndarray): joint velocities. tgt_frame (str): target link frame. Returns: np.ndarray: translational velocity and rotational velocity (vx, vy, vz, wx, wy, wz) (shape: :math:`[6,]`). """ if isinstance(jpos, list): jpos = np.array(jpos) if isinstance(jvel, list): jvel = np.array(jvel) kdl_end_frame = kdl.FrameVel() kdl_jnt_angles = joints_to_kdl(jpos) kdl_jnt_vels = joints_to_kdl(jvel) kdl_jnt_qvels = kdl.JntArrayVel(kdl_jnt_angles, kdl_jnt_vels) idx = self.arm_link_names.index(tgt_frame) + 1 fg = self._fk_solver_vel.JntToCart(kdl_jnt_qvels, kdl_end_frame, idx) if fg < 0: raise ValueError('KDL Vel JntToCart error!') end_twist = kdl_end_frame.GetTwist() return np.array([end_twist[0], end_twist[1], end_twist[2], end_twist[3], end_twist[4], end_twist[5]])
def forward_velocity_kinematics(self, joint_values=None, joint_velocities=None): end_frame = PyKDL.FrameVel() kdl_JntArray_pos = self.joints_to_kdl('positions', joint_values) kdl_JntArray_vel = self.joints_to_kdl('velocities', joint_velocities) kdl_JntArrayVel = PyKDL.JntArrayVel(kdl_JntArray_pos, kdl_JntArray_vel) self._fk_v_kdl.JntToCart(kdl_JntArrayVel, end_frame) return end_frame.GetTwist()
def joint_states_cb(self, msg): state_msg = EndpointState() # Forward Position Kinematics q, qd, eff = self.kinematics.extract_joint_state(msg) T06 = self.kinematics.forward(q) state_msg.pose = PoseConv.to_pose_msg(T06) # Forward Velocity Kinematics end_frame = PyKDL.FrameVel() q_vel = PyKDL.JntArrayVel(joint_list_to_kdl(q), joint_list_to_kdl(qd)) self.fk_vel_solver.JntToCart(q_vel, end_frame) state_msg.twist = TwistKDLToMsg(end_frame.GetTwist()) state_msg.header.frame_id = self.frame_id state_msg.header.stamp = rospy.Time.now() try: self.state_pub.publish(state_msg) except: pass
def kdl_inv_vel_kine(self, cur_joint_pos, ee_twist=None): """ inverse velocity kinematics using the solver in PyKDL instance : self.ik_v_kdl refer ik_p_kdl-> ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: PyKDL -> ee frame velocity? KDL::FrameVel v_out; tf::poseMsgToKDL(cart_pose, pose_kdl); // cur_jnt_position, ee_vel CartToJntVel(JointVelList[i].q, end_effector_vel.GetTwist(), NullSpaceBias[i], result_vel) kin.ik_solver->CartTo """ frame_vel = PyKDL.FrameVel() _joint_vels = self.joints_to_kdl(type='velocities', values=None) # args : CartToJnt-> (cur_joint_pos, targ_ee_vel, result_joint_vel) if self.ik_v_kdl.CartToJnt(cur_joint_pos, ee_twist, _joint_vels) >= 0: return list(_joint_vels) else: return None
def forward_velocity_kinematics(self, joint_values=None, joint_velocities=None): if joint_values is None: joint_values = self.joint_angles if joint_velocities is None: joint_values = self.joint_velocities q = PyKDL.JntArray(self.n_joints) q_dot = PyKDL.JntArray(self.n_joints) for idx in range(len(self.joint_names)): q[idx] = joint_values[self.joint_names[idx]] q_dot[idx] = joint_velocities[self.joint_names[idx]] vel_array = PyKDL.JntArrayVel(q, q_dot) end_frame = PyKDL.FrameVel() self._fk_v_kdl.JntToCart(vel_array, end_frame) return end_frame.GetTwist()
def update_ee_state(self, joint_pos, joint_vel): end_frame = PyKDL.Frame() kdl_array = PyKDL.JntArray(len(joint_pos)) for idx, val in enumerate(joint_pos): kdl_array[idx] = val self._fk_p_kdl.JntToCart(kdl_array, end_frame) self._ee_frame = end_frame pos = end_frame.p rot = PyKDL.Rotation(end_frame.M) rot = rot.GetQuaternion() self._ee_pose = np.array([pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]]) end_frame = PyKDL.FrameVel() kdl_array = PyKDL.JntArray(len(joint_vel)) for idx, val in enumerate(joint_vel): kdl_array[idx] = val kdl_array = PyKDL.JntArrayVel(kdl_array) self._fk_v_kdl.JntToCart(kdl_array, end_frame) self._ee_twist = end_frame.GetTwist() #not numpy array...
def forward_velocity_kinematics(self, joint_velocities=None): end_frame = PyKDL.FrameVel() self._fk_v_kdl.JntToCart( self.joints_to_kdl('velocities2', joint_velocities), end_frame) return end_frame.GetTwist()