Moveit control gripper instead of panda_link8 EFF
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.
- Transform the grasp pose (using the tf2 package) in such a way that the
gripper centre
aligns with the computed grasp pose. - Modify the
urdfs/srdf
provided 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_p8
transformation 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 ...
What sort of
Quaternion
is that? Eigen?why do you "have to"? I'm not saying it's going to necessarily solve everything for you, but this is open-source. Clone the sources and add the end-effector to the moveit configuration.
And can't you use
doTransform(..)
instead of manually transforming thePose
object? See #q261419 fi and the linked tutorial.Ah ofcourse, sorry. It is the Quaternion class of the pyquaternion package. I also tried it with the
tf_conversions
package:but as expected this gives me the same result.
Your right, sorry for that, I could have said that differently. I tried changing the
urdfs
andsrdfs
, but I resorted to the second method since I ran into some problems. I will first reference the sources you send me thanks a lot!With all your edits I'm no longer sure you weren't already using
tf2_geometry_msgs.do_transform_pose(..)
. If so: sorry for the noise.@gvdhoorn No, thank you for your reply! I was updating the original code using the suggestions you made as the manual transform could indeed be replaced by `tf2_geometry_msgs.do_transform_pose(). But had to leave in the middle of it. Sorry for the confusion. The new code already gives the right EFF pose in some cases (See updated answer). I, therefore, think to start there was an error in the way I handled the quaternions.
@gvdhoorn I managed to fix my problem by adding an extra virtual joint to the
urdf
andsrds
in the panda_moveit_config and franka_ros. I am, however curious what goes wrong with my reasoning while using the tf2 library to correct for the fact that I am not planning in thepanda_gripper_center
frame but thepanda_link8
frame. I updated my question to depict my reasoning. Let me know if you see some immediate flaws in my reasoning. Otherwise, I will open a new ticket in the future if needed.