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:
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
return False

## Get pose expressed in the panda_link8 frame ##
try:
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 ...
edit retag close merge delete

1
quat_pose = Quaternion(pose_msg.pose.orientation.w, pose_msg.pose.orientation.x, pose_msg.pose.orientation.y, pose_msg.pose.orientation.z)


What sort of Quaternion is that? Eigen?

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

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 the Pose object? See #q261419 fi and the linked tutorial.

( 2019-10-08 11:50:10 -0600 )edit

What sort of Quaternion is that? Eigen?

Ah ofcourse, sorry. It is the Quaternion class of the pyquaternion package. I also tried it with the tf_conversions package:

quat_old = [pose_msg.pose.orientation.x, pose_msg.pose.orientation.y, pose_msg.pose.orientation.z, pose_msg.pose.orientation.w]
quat_diff = [trans_p8_grip_center.transform.rotation.x, trans_p8_grip_center.transform.rotation.y, trans_p8_grip_center.transform.rotation.z, trans_p8_grip_center.transform.rotation.w]
quat = tf_conversions.transformations.quaternion_multiply(quat_diff, quat_old)


but as expected this gives me the same result.

( 2019-10-08 12:46:23 -0600 )edit

why do you "have to"?

Your right, sorry for that, I could have said that differently. I tried changing the urdfs and srdfs, 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!

( 2019-10-08 12:50:25 -0600 )edit
1

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.

( 2019-10-08 13:35:36 -0600 )edit

@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.

( 2019-10-08 14:25:52 -0600 )edit

@gvdhoorn I managed to fix my problem by adding an extra virtual joint to the urdf and srds 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 the panda_gripper_center frame but the panda_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.

( 2019-10-11 12:20:18 -0600 )edit

Sort by » oldest newest most voted

In the end, I fixed my issue by using method 2. After changing the urdf and srdf files of panda_moveit_config and franka_ros packages as was suggested by @gvdhoorn, I was able to plan for the gripper_center. The changes I made can be found on the following branches:

With these changes one can now control the gripper relative to the end effector by setting the move_group end effector link to panda_gripper_center.

move_group = self.robot.get_group("panda_arm")

more

1

I had this problem in 2018 and solved it via what you call method 1. However, I'm now trying to use the second way for an unrelated thing and looking through the changes in both files I can see you didn't really add hand as an end effector, because that was already the case. You added gripper_center and gripper_center_xyz as a link in the hand. This is different than using the tf2 frame called panda_K, the center of the internal cartesian impedance, which the franka emika team provides, because it's not set at runtime. Are you still using this method or did you manage to use the runtime change?

( 2020-02-07 10:21:27 -0600 )edit

@aPonza Your correct, that is precisely what I did in the end and what I am still using now (see most recent version of my project). I first added an extra panda_gripper_center (virtual) link to the urdfs/srdf`provided by panda_moveit_config and franka_ros. Following, in my control script I set the movegroup end effector to this new (virtual) link. I did not yet try the runtime change you describe above. I updated the original question a little bit to make it more clear.

( 2020-02-18 02:00:55 -0600 )edit