I'm currently trying to execute a grasp pose, which I computed using a CNN, on the Panda Emika Franka Robot using Moveit. Unfortunately, the panda_moveit_config and franka_ros packages didn't specify the panda_hand link as a move_group end effector in their urdf's/srdf's. I, therefore, have to control the robot using the panda_link8 joint instead. Based on the following topics how-to-plan-moves-with-gripper-attached-to-arm and openclose-end-effector-with-moveit-rviz. I found two ways of solving my problem.

1. Transform the grasp pose (using the tf2 package) in such a way that the gripper centre aligns with the computed grasp pose.
2. Modify the urdfs/srdfprovided by panda_moveit_config and franka_ros to include an extra (virtual) link that ends in the middle of the gripper, making controlling the arm plus gripper easier. Virtual in the sense that it has no mass, inertia, visual geometry and collision geometry.

UPDATE: I am currently using method 2.

Method 1: Use the tf2 package

Using the tf2 package, I try to correct for the fact that I am planning using the panda_link8 frame as the end effector. I do this by calculating the transformation between the panda_link8 frame and the gripper_center frame (trans_grip_center_p8). I then transform the grasping pose which was computed initially in the kinect2_rgb_optical_Frame to the panda_link8 frame. After the pose is expressed in the panda_link8 frame I transform both the pose and the trans_grip_center_p8 transformation message into a 4x4 homogeneous transformation matrix. I following take the inverse of the trans_grip_center_p8transformation matrix and multiply this with the pose. Lastly, I transform this new pose to the panda_link0 frame, which is used as the reference frame in my move_group. Unfortunately, I did something wrong in my reasoning since using the new corrected pose (depicted in purple) the end effector still does not end up in the right pose (Depicted in blue).

What I tried: - Not taking the inverse. - Using the self.tf2_buffer.transform directly. And afterward setting the pose.header.frame_id to the old frame (Before the transform).

Wrong pose

](/upfiles/15708151267919294.png)

Code

Main code

## Get transform between panda_link8 and gripper centre ##
    try:
        trans_grip_center_p8 = self.tf2_buffer.lookup_transform("panda_link8", "gripper_center", rospy.Time(0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        return False

    ## Get pose expressed in the panda_link8 frame ##
    try:
        pose_msg = self.tf2_buffer.transform(self.pose_msg, "panda_link8", rospy.Duration(1))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        return False

    ## Transform grasp pose such that it aligns with the gripper_center frame ##
    # This is needed since the move_group uses the panda_link8 frame as its
    # end-effector.

    ## Transform transformation to 4x4 homogeneous matrix ##
    H_trans = conversions.transform_stamped_2_matrix(trans_grip_center_p8)
    H_trans = tf_conversions.transformations.inverse_matrix(H_trans) # Use inverse to correct for planning in the wrong EEF
    H_pose = conversions.pose_msg_stamped_2_matrix(pose_msg)

    ## New Pose ##
    pose_new = np.dot(H_trans, H_pose)
    trans = tf_conversions.transformations.translation_from_matrix(pose_new)
    rot = tf_conversions.transformations.quaternion_from_matrix(pose_new)
    pose_msg.pose.position.x = trans[0]
    pose_msg.pose.position.y = trans[1]
    pose_msg.pose.position.z = trans[2]
    pose_msg.pose.orientation.x = rot[0]
    pose_msg.pose.orientation.y = rot[1]
    pose_msg.pose.orientation.z = rot[2]
    pose_msg.pose.orientation ...