Add optionnal end effector transform offset for the cartesian force controller

[captain-yoshi]

Ability to add an offset to the end effector link. Usefull when the end effector link is not part of the robot kinematic chain, e.g. when a gripper takes a screw driver. Tested on a real UR3 :)

Adds a generic SpatialPose config and PoseParameterHandle for handling updates of a pose through the dynamic reconfigure server. A getter to retrieve the pose and an atomic query method to see if a new pose is available.

Use the PoseParameterHandle in the cartesian force controller. Enables the offset to bet set at runtime at the start of the loop to avoid having a potential different offset for the FT sensor reference frame and the target wrench.

Setting the pose from within RQT is not recommanded, because we can only adjust one parameter at a time. But you can use the dynamic reconfigure client to set the pose offset parameters in one shot. Can also be configured in the yaml config.

An example of the optionnal offset parameter in the config:

my_cartesian_force_controller:
  type: "position_controllers/CartesianForceController"
  robot_base_link: "ur3_base_link"
  end_effector_link: "robotiq_2f140_tcp"
  ft_sensor_ref_link: "robotiq_ft300_frame_id"
  hand_frame_control: true

  end_effector_transform_offset:
    px: 0
    py: 0
    pz: 0
    # Rotate -pi/2 wrt. the end_effector_link Y axis
    qx: 0
    qy: -0.7071068
    qz: 0
    qw: 0.7071068

Drawback: Adds 1 matrix multiplication when m_hand_frame_control = False, otherwise 2 matrix multiplications. Also retriggers the setFtSensorReferenceFrame on each new pose.

---

It's totaly fine if you don't want this feature as it adds some overhead to the controller.

[captain-yoshi]

Closed in favor of #189.