[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 84.73%. Comparing base (c703235) to head (6371fc2).

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   84.79%   84.73%   -0.07%     
==========================================
  Files         151      157       +6     
  Lines       14607    14803     +196     
  Branches     1266     1284      +18     
==========================================
+ Hits        12386    12543     +157     
- Misses       1763     1792      +29     
- Partials      458      468      +10     

Flag	Coverage Δ
unittests	84.73% <83.75%> (-0.07%)	⬇️

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.92% <66.66%> (+0.13%)	⬆️
...y_controller_plugins/src/pid_trajectory_plugin.cpp	82.45% <82.45%> (ø)
...ory_controller/src/joint_trajectory_controller.cpp	81.16% <66.66%> (-2.73%)	⬇️

🚀 New features to boost your workflow:

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

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.