PositionManipulator support for using a different base/world frame to express TCP poses.

[Victorlouisdg]

Describe the feature you'd like
I would like the PositionManipulator class to have an optional constructor argument: arm_base_in_world (X_W_B). This would then internally be used by the class to transform all given TCP poses (expressed in world) to the frame the control box of the robot expects.

Use cases
For anyone not working in the base frame defined by the UR control box, this would save the effort of multiplying each X_W_TCP by np.linalg.inv(X_W_B) before calling e.g. move_to_tcp_pose(X_W_TCP).

When using ROS-industrial compliant URDFs, you almost immediately run into this issue, because the URDFs define +X to be the direction the robot points towards with all-zero joints, while for the control box (and the stickers we've put on the bases) that is the -X direction. See the screenshot below for the difference.

Also, if we get a different brand robot in the future, we can easily reuse TCP poses by changing only the X_W_B in the constructors.

Possible implementation
We could add this functionality in a backward-compatible way by setting the default value of robot_base_in_world to np.identity(4). Additionally we could add a similar helper functions as the DualArmPositionManipulator called .transform_pose_to_arm_base().

We should probably also adapt the DualArmPositionManipulator class/constructor, because after this change, conversion from world to arm base is handled by each arm individually.

[Victorlouisdg]

Alternative solution: maybe this shouldn't be solved in the PositionManipulator. For example with Drake, can we solve this by welding the arm's base_link to the world in a specific way? Or would that have limitations?

[tlpss]

@Victorlouisdg I think decoupling world from robot base frame makes sense. You can probably draw inspiration from the DualArm class. Make sure to adapt the dual arm class to reuse the single arm functionality as much as possible