[JTC] Proposal for custom "controller" plugins

[christophfroehlich]

Up to now there were only two possibilities of controlling the system with JTC:

• feedforward only
• feedback with one single-input-single-output PID per joint.

The aim of this PR is to create an interface for supporting new control strategies, and enable even creating custom controller plugins by users. This is done by plugins of base class TrajectoryControllerBase, loaded by the pluginlib.

Together with the change of #809 it will even be supported to integrate state-space controllers, where the system output does not have the same size as the system input: See the following swing-up of a cart-pole from this demo, where I implemented a LQR as plugin. The merge of both PRs can be found here.

swingup_lqr_gh.mp4

Why to change JTC in this way:

• Of course this could also be done by a new custom ros2_controller, but I'd really like to use the well-tested interfaces of JTC in my projects as well (and get updates to it). Overriding single methods does not work neither (see the verbose update() method for example).
• In principle, JTC could be enhanced by a chained controller. But: Then there would be no direct way to use the trajectory sent to JTC for calculating the control law, nor does it have the information about the start of the sent trajectory to interpolate the gains to the correct time instance.
• An alternative would be a JTC base class, with virtual methods for the "actual" trajectory controller. The current default JTC is then inheriting this class; users can inherit from the base class with their own methods. One drawback of this would be, that any upstream changes of the JTC base class would force the users to re-build their inherited JTC. This wouldn't be necessary with the proposed plugins. And the proposed controller plugins are faster to compile and unittest!

The API was designed around the following methods:

• computeControlLawNonRT: called when a new trajectory is received. This may take some time (e.g., calculating the gains for a LQR by solving Riccati equations). This blocks the execution of the new trajectory by JTC until the calculation is finished (see is_ready()). The user has to implement a realtime-buffer to switch the old control law to the new one, once it is started (see start()).
• In case of any abort (e.g., set_hold_position()), there is a computeControlLawRT which needs to finish within one RT loop.
• updateGainsRT: A fast update within the RT loop, e.g., update of the PID gains from reconfigured ROS parameters.
• computeCommands: Evaluate the control law inside the RT loop with earlier calculated/updated gains.

Notes:

• w.r.t. the change of [JTC] Accept larger number of joints than command_joints #809, the map for the gains should be command_joints. But most of the time this parameter is empty and copied over from joints in JTC. Because it is a static read-only variable, it is not possible to override the parameter for the plugin from JTC -> I had to make it reconfigurable from the generate_parameter_library, but added a callback afterwards to prohibit a later change from the user. If anyone knows a better way to solve this, please let me know.
• The naming of the plugin package, the plugin itself, or the base class is open for discussion.
• Important: The behavior of existing configurations should not break with the proposed changes.

[codecov]

Codecov Report

Attention: Patch coverage is 29.41176% with 48 lines in your changes are missing coverage. Please review.

Project coverage is 47.40%. Comparing base (0b43291) to head (ce9386a).

❗ Current head ce9386a differs from pull request most recent head 9c0252f. Consider uploading reports for the commit 9c0252f to get more accurate results

Additional details and impacted files

@@             Coverage Diff             @@
##           master     #885       +/-   ##
===========================================
- Coverage   71.86%   47.40%   -24.46%     
===========================================
  Files          41       41               
  Lines        3650     3894      +244     
  Branches     1794     1840       +46     
===========================================
- Hits         2623     1846      -777     
- Misses        707      777       +70     
- Partials      320     1271      +951     

Flag	Coverage Δ
unittests	47.40% <29.41%> (-24.46%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	0.00% <ø> (-100.00%)	⬇️
...ory_controller/src/joint_trajectory_controller.cpp	46.22% <29.41%> (-33.31%)	⬇️

... and 37 files with indirect coverage changes

[mergify]

This pull request is in conflict. Could you fix it @christophfroehlich?

[mergify]

This pull request is in conflict. Could you fix it @christophfroehlich?

[mergify]

This pull request is in conflict. Could you fix it @christophfroehlich?

[mergify]

This pull request is in conflict. Could you fix it @christophfroehlich?