CartesianComplianceController - support setting stiffness via topic (instead of just parameter calls)

[firesurfer]

Problem statement and motivation

Note: I only refer to the ROS2 branch. I didnt take a closer look at the ROS1 branches.

The CartesianComplianceController currently only allows to configure the stiffness parameters via the ROS parameter mechanism. I would like to drive a circular trajectory with a robot arm where the arm shall always be rather stiff in the direction of the normal of the circle. Therefore I need to continuously adapt the stiffness parameters. Using the ROS parameter mechanism there is a lot of overhead involved negatively affecting the performance. (Also on ROS2 sometimes the setting/getting the parameters from a remote node tends to hang)

EDIT: I just realized that if I specify the end_effector correctly the stiffness can be left constant in the given example. Nevertheless I think this might be a wanted feature for certain applications.

Also this might improve overall control performance as each call in computeComplianceError

  tmp[0] = get_node()->get_parameter("stiffness.trans_x").as_double();
  tmp[1] = get_node()->get_parameter("stiffness.trans_y").as_double();
  tmp[2] = get_node()->get_parameter("stiffness.trans_z").as_double();
  tmp[3] = get_node()->get_parameter("stiffness.rot_x").as_double();
  tmp[4] = get_node()->get_parameter("stiffness.rot_y").as_double();
  tmp[5] = get_node()->get_parameter("stiffness.rot_z").as_double();

will lock at least one recursive mutex in the control realtime loop which is rather time consuming.

Goal of this feature

Allow configuring the stiffness with a higher update rate / low overhead

Approach and next steps

Add an additional topic (either with a custom message or just a Float64 array) that allows configuring the stiffness.

In general cache the stiffness value and only update them when necessary. For the topic interface interaction is pretty simple (just take the message and update the stiffness values), for the parameters interface hooking up to one of the parameter callbacks (e.g. add_on_set_parameters_callback) could be a viable solution.

[stefanscherzinger]

@firesurfer

Thanks for this suggestion. I'll have a look at performance/realtime and evaluate what to use best to update the stiffness.

In general cache the stiffness value and only update them when necessary.

Good point. I'll check.

EDIT: I just realized that if I specify the end_effector correctly the stiffness can be left constant in the given example.

True. The stiffness is specified with respect to the given compliance_ref_link.