Transform a points reference frame for object scanning

Question

I am trying to create a path for a robot to move a camera around an object at a specified distance and angle. I have a defined object_link with world as parent and I have a robot_base_link with world as parent as well. I have managed to create a path for robots tool0 around the object_link by looking up a transform between the object_link and tool0 for the current tool0 location as I am moving my robot by that. Then creating that as a PoseStamped pose, setting it to the required position around the object, then transforming that position to the robots reference frame and then moving the robot.

object_to_robot_transform = tfBuffer.lookupTransform("object_link", "tool0", ros::Time(0), ros::Duration(1.0));

scan_object = convert(object_to_robot_transform, scan_object);

/*
set the poseStamped to required position
*/

object_to_world_transform = tfBuffer.lookupTransform("world", "object_link", ros::Time(0));

tf2::doTransform(scan_object, scan_object, object_to_world_transform);

move_group.setPoseTarget(scan_object.pose);
move_group.move();

After these transforms, the pose in scan_object is the one I need the robot to move to (world,tool0).

But problems arise when I try to move a camera_frame that is at the end of the robot. My idea of how it would work was, that I would find the transformation between the object and camera(object,camera). Set the PoseStamped to the required position. Then transform it to (world,tool0).

object_to_camera_transform = tfBuffer.lookupTransform("object_link", "camera", ros::Time(0), ros::Duration(1.0));
/*
convert object_to_camera_transform to poseStamped and set it to required position
*/

robot_to_object_transform = tfBuffer.lookupTransform("tool0", "object_link", ros::Time(0), ros::Duration(1.0));
tf2::doTransform(scan_object, scan_object, robot_to_object_transform);
camera_to_world_transform = tfBuffer.lookupTransform("camera", "world", ros::Time(0), ros::Duration(1.0));
tf2::doTransform(scan_object, scan_object, camera_to_world_transform);

move_group.setPoseTarget(scan_object.pose);
move_group.move();

But sadly, this does not give me the results I'm going for so I was wondering if there is a better method of transforming, maybe with only one transform operation?

P.S. I am aware of changing the tip_link in the .srdf file to the camera frame, but that would make this very tedious for usability. P.P.S. This is my first question!

---

Originally posted by sniegs on ROS Answers with karma: 65 on 2021-03-24

Post score: 0

Answer 1

As I have found out, doing transformations as suggested in my question isn't the best way to go about this problem and a better way is to add the camera_frame as a sort of gripper or just an end effector extension in the srdf file. So what you do is create a group in the srdf as such:

<group name="camera">
    <link name="camera_frame" /> 
</group>

and afterwards, define an end effector:

<end_effector name="camera_frame" parent_link="wrist_3_link" group="camera" parent_group="manipulator" />

The parent_link is the link of the robot, which the camera is attached to in the xacro/urdf. Now you can just set the end effector in code with

move_group.setEndEffectorLink("camera_frame");

and use a single transformation to move the camera around a point of interest.

---

Originally posted by sniegs with karma: 65 on 2021-06-08

This answer was ACCEPTED on the original site

Post score: 0