Revision history [back]

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.