[JTC] Proposal for custom "controller" plugins #885

christophfroehlich · 2023-12-06T21:22:31Z

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:

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 · 2023-12-06T21:49:55Z

Codecov Report

Attention: Patch coverage is 84.68085% with 36 lines in your changes missing coverage. Please review.

Project coverage is 85.00%. Comparing base (d1844e6) to head (4dc4026).

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	65.27%	18 Missing and 7 partials ⚠️
...y_controller_plugins/src/pid_trajectory_plugin.cpp	79.59%	7 Missing and 3 partials ⚠️
...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.06%   85.00%   -0.07%     
==========================================
  Files         123      129       +6     
  Lines       11712    11880     +168     
  Branches      998     1017      +19     
==========================================
+ Hits         9963    10098     +135     
- Misses       1434     1457      +23     
- Partials      315      325      +10

Flag	Coverage Δ
unittests	`85.00% <84.68%> (-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	`100.00% <ø> (ø)`
...ory_controller/test/test_trajectory_controller.cpp	`99.78% <100.00%> (+<0.01%)`	⬆️
...ntroller/test/test_trajectory_controller_utils.hpp	`84.47% <100.00%> (+0.14%)`	⬆️
...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%> (ø)`
...y_controller_plugins/src/pid_trajectory_plugin.cpp	`79.59% <79.59%> (ø)`
...ory_controller/src/joint_trajectory_controller.cpp	`83.02% <65.27%> (-2.62%)`	⬇️

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

mergify · 2023-12-12T08:52:38Z