[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.

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:

• compute_control_law_non_rt: 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 compute_control_law_rt which needs to finish within one RT loop.
• update_gains_rt: A fast update within the RT loop, e.g., update of the PID gains from reconfigured ROS parameters.
• compute_commands: 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

❌ Patch coverage is 83.75451% with 45 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.18%. Comparing base (7b7ecb7) to head (8e4de92).

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	66.66%	24 Missing and 6 partials ⚠️
...y_controller_plugins/src/pid_trajectory_plugin.cpp	82.45%	7 Missing and 3 partials ⚠️
...ntroller/test/test_trajectory_controller_utils.hpp	66.66%	4 Missing ⚠️
...ry_controller_plugins/test/test_pid_trajectory.hpp	90.90%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #885      +/-   ##
==========================================
- Coverage   85.26%   85.18%   -0.08%     
==========================================
  Files         143      149       +6     
  Lines       13793    13990     +197     
  Branches     1196     1215      +19     
==========================================
+ Hits        11760    11917     +157     
- Misses       1636     1665      +29     
- Partials      397      408      +11     

Flag	Coverage Δ
unittests	85.18% <83.75%> (-0.08%)	⬇️

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

Files with missing lines	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	50.00% <ø> (ø)
...ory_controller/test/test_trajectory_controller.cpp	99.80% <100.00%> (+<0.01%)	⬆️
...ctory_controller_plugins/pid_trajectory_plugin.hpp	100.00% <100.00%> (ø)
..._controller_plugins/trajectory_controller_base.hpp	100.00% <100.00%> (ø)
...ntroller_plugins/test/test_load_pid_trajectory.cpp	100.00% <100.00%> (ø)
...ry_controller_plugins/test/test_pid_trajectory.cpp	100.00% <100.00%> (ø)
...ry_controller_plugins/test/test_pid_trajectory.hpp	90.90% <90.90%> (ø)
...ntroller/test/test_trajectory_controller_utils.hpp	84.66% <66.66%> (+0.13%)	⬆️
...y_controller_plugins/src/pid_trajectory_plugin.cpp	82.45% <82.45%> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	81.27% <66.66%> (-2.74%)	⬇️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.