Transformation between camera pose and end effector with tf2

asked 2018-12-20 10:00:29 -0500

maxwell_equations gravatar image

updated 2018-12-27 18:25:06 -0500

Hello!

I am trying to grip objects using my UR5 and an Asus Xtion Pro Life. The camera is mounted on the end effector of the robot (hand-eye). The camera is going to find the object and return a pose of the found object. I used the easy_handeye repository to calcualte the transformation between the ee_link and the camera.

This is the transformation result:

eye_on_hand: true
robot_effector_frame: ee_link
tracking_base_frame: primesense_link
transformation: {qw: 0.12315000298790331, qx: -0.989347167929719, qy: -0.07459459276842946,
qz: 0.02149197066897385, x: 0.04013691589594168, y: -0.13677498325817272, z: 0.08220400023605642}

The transformation seems to be correct. So the next step was to transform my pose with the known transformation.

First of all here is my data:

grasp_camera_pose:

 frame_id: "object_link"
 child_frame_id: "primesense_link"
   position: 
       x: -0.0987664492927
       y: -0.0910226523694
       z: 0.560733250459
  orientation: 
       x: 0.631932686942
       y: 0.31727130216
       z: 0.631932686942
       w: -0.31727130216
  q_value: 0.678789377213
  grasp_type: 0
  center_px: [226.52753499783466, 153.77785122466588]
  angle: -0.640199793927
  depth: 0.560733250459
  ...

camera_ee_transform:

header: 
seq: 0
frame_id: "ee_link"
child_frame_id: "primesense_link"
transform: 
translation: 
    x: 0.040137
    y: -0.136775
    z: 0.082204
rotation: 
    x: -0.989347006868672
    y: -0.0745944036699313
    z: 0.021491953634446583
    w: 0.12315016789268264

Then I used t2f to calculae the grasp in the ee pose

grasp_ee_pose = tf2_geometry_msgs.do_transform_pose(grasp_camera_pose, camera_ee_transform)

The result is incorrect. Here are the expected results and my actual results for the translation:

grasp_ee_pose_translation_actual:

x = -0.104539997152
y  = 0.0702126802005
z = -0.436145472381

grasp_ee_pose_translation_expected:

x =  0.362407802611
y =  0.667193262606
z =  0.050243921518

And I don't know where the mistake is. I would be very happy if someone could help me solve this problem. Thank you!


EDIT:

To clear things up. The grasp_camera_pose is the pose between object_link->primesense_link

Therefore, I need to invert grasp_camera_pose and use the transformation I got earlier.

ret = self.get_transform(grasp_camera_pose.invert(), 'primesense_link', 'ee_link')

This is the tf_frame_tree:

world->base_link->shoulder_link_upper_arm_link->forarm_link->wrist_1_link->wrist_2_link->wrist_3_link->ee_link->primesense_link->primesense_rgb_frame->primsense_rgb_optical_frame

Since I got all the transformations I used a buffer and the Transformlistener and then calculate the transformation from primesense_link -> ee_link

self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))  # tf buffer length
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)


def get_transform(self, pose_stamped, source_frame, target_frame):
    try:
        transform = self.tf_buffer.lookup_transform(target_frame,
                                   source_frame, 
                                   rospy.Time(0), #get the tf at first available time
                                   rospy.Duration(1.0)) #wait for 1 second

        pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
        return pose_transformed
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, 
                    tf2_ros.ExtrapolationException):
        rospy.logerror("Error getting transform")

It still does not work but at least the new pose looks to be in the right direction.

edit retag flag offensive close merge delete

Comments

If I'm not mistaken, the frame on the object from the camera makes sense, but the transform is almost a 180° rotation on the x-axis, so the camera is pointing the opposite direction to the end effector...seems weird you're expecting to go forwards. What frames are the grasp poses in?

aPonza gravatar imageaPonza ( 2018-12-21 06:31:16 -0500 )edit

Thank you for your help!

The camera is mounted with the top facing the end effector (so the cable does not interfere with the mounting), that's why they point in opposite directions I guess.

To your second question: I guess you mean what frame the grasp_camera_pose is? -> I edited my question.

maxwell_equations gravatar imagemaxwell_equations ( 2018-12-21 12:30:36 -0500 )edit
1

I meant what are the frame_ids of grasp_camera_pose, and the 2 grasp_ee_pose_.... They should all be something like world or your arm's base link, but I'm betting they aren't since you expect to move forwards, which is away from the camera's direction, since the frames are opposing.

aPonza gravatar imageaPonza ( 2018-12-27 05:18:23 -0500 )edit

I think I am close to the solution. Thanks for the tip. I had to use the inverted pose and then use the transformListener. There is still something wrong. Maybe I have to use another transformation.

maxwell_equations gravatar imagemaxwell_equations ( 2018-12-27 18:27:40 -0500 )edit
1

There is no reason to do this in code. The recommended technique is to broadcast a static transform from the ee_frame to the camera frame. Then you can automatically transform geometry in the camera frame to any other frame you want.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-12-29 08:07:20 -0500 )edit