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_position(self, joint_positions, des_frame):
"""
Given joint angles, compute the pose of desired_frame with respect
to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame
must be in self.arm_link_names
:param joint_positions: joint angles
:param des_frame: desired frame
:type joint_positions: np.ndarray
:type des_frame: string
:return: translational vector and rotational matrix
:rtype: np.ndarray, np.ndarray
"""
joint_positions = joint_positions.flatten()
assert joint_positions.size == self.arm_dof
kdl_jnt_angles = prutil.joints_to_kdl(joint_positions)
kdl_end_frame = kdl.Frame()
idx = self.arm_link_names.index(des_frame) + 1
fg = self.fk_solver_pos.JntToCart(kdl_jnt_angles,
kdl_end_frame,
idx)
assert fg == 0, 'KDL Pos JntToCart error!'
pose = prutil.kdl_frame_to_numpy(kdl_end_frame)
pos = pose[:3, 3].reshape(-1, 1)
rot = pose[:3, :3]
return pos, rot