Transformation between camera pose and end effector with tf2

edit

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.