[Feature] Export chainable state interfaces from chainable controllers

[saikishor]

Hello,

This PR adds a new feature to the chainable controllers so that they are able to export the expected interfaces (in the form of state interfaces) that can be used by multiple controllers at the same time. Please take a look at the following image for better understanding:

There could be a use case, where you have a controller that wants to share information with multiple controllers, for instance, a controller like a state estimator, then using this feature you will be able to share the state as estimated interfaces so that other following controllers can use it as a state interface.

Closes #995

[destogl]

There could be a use case, where you have a controller that wants to share information with multiple controllers, for instance, a controller like a state estimator, then using this feature you will be able to share the state as estimated interfaces so that other following controllers can use it as a state interface.

Can you add in the figure connection between robot localization and position tracking? Also, I would remove odom_publisher no need to have it, since controllers can publish things directly.

[saikishor]

Can you add in the figure connection between robot localization and position tracking? Also, I would remove odom_publisher no need to have it, since controllers can publish things directly.

Thank you @destogl for the reply. Sure, I can add the connection from the robot_localization to position_tracking. I agree that the odom_publisher is not needed as it is very evident, however, I would leave it for the clarity of the same interface resource being utilized by multiple controllers (that was my main motivation in this publisher controller). What do you think? Do you still want me to remove it?.

[codecov-commenter]

Codecov Report

Attention: Patch coverage is 92.28070% with 44 lines in your changes missing coverage. Please review.

Project coverage is 88.05%. Comparing base (fbb893b) to head (7d5efeb).

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1021      +/-   ##
==========================================
+ Coverage   87.70%   88.05%   +0.34%     
==========================================
  Files         102      103       +1     
  Lines        8704     9191     +487     
  Branches      780      803      +23     
==========================================
+ Hits         7634     8093     +459     
- Misses        790      816      +26     
- Partials      280      282       +2     

Flag	Coverage Δ
unittests	88.05% <92.28%> (+0.34%)	⬆️

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

Files	Coverage Δ
...oller_interface/chainable_controller_interface.hpp	100.00% <ø> (ø)
...lude/controller_interface/controller_interface.hpp	100.00% <ø> (ø)
...controller_interface/controller_interface_base.hpp	87.50% <ø> (ø)
...rface/test/test_chainable_controller_interface.cpp	100.00% <100.00%> (ø)
...rface/test/test_chainable_controller_interface.hpp	86.95% <100.00%> (+3.17%)	⬆️
.../include/controller_manager/controller_manager.hpp	100.00% <ø> (ø)
...chainable_controller/test_chainable_controller.hpp	100.00% <ø> (ø)
...r_manager/test/test_controller/test_controller.cpp	95.34% <100.00%> (+0.61%)	⬆️
...r_manager/test/test_controller/test_controller.hpp	100.00% <ø> (ø)
...ller_manager/test/test_controller_manager_srvs.cpp	99.25% <100.00%> (+<0.01%)	⬆️
... and 7 more

[olivier-stasse]

Thanks a lot for this contribution.
The tests are very much appreciated.
I tested the branch and the tests on rolling and this is working.

My main suggestion will be to not use estimated_interfaces which I found misleading.
MPC could provide gains, future state, so for me we should either having a class of interfaces and then derived from it, or stick to state_interface terminology.

[olivier-stasse]

estimated is an unfortunate name. Some controllers, for instance model predictive controller have an horizon on the future which can be used for interpolation. Hence estimated_interface will be a very misleading name. To be consistent with the rest of the architecture, I would vote for using state_interfaces.

[destogl]

The issue is that we have already state_interfaces here – those are interfaces the controller uses as input.

Maybe we have another issue, maybe we should have “command” and “feedback” interfaces for classical controllers and then "reference" and "state" for this chaining features. Or should this other interfaces be "feedback" since they provide feedback for other controllers?

@christophebedard @fmauch @bmagyar @livanov93 what do you think about this?

[destogl]

What do you think? Do you still want me to remove it?.

We should work on a realistic example, i.e. have real use-case in docs. Otherwise people start to re-create whatever they see.

[olivier-stasse]

What do you think? Do you still want me to remove it?.

We should work on a realistic example, i.e. have real use-case in docs. Otherwise people start to re-create whatever they see.

Dear @destogl to give more concrete details, in humanoid robotics a recent nice explanation of the most common control design pattern is described here:
https://hal.archives-ouvertes.fr/hal-01875387/document
In Fig. 2 you will find a DCM and a ZMP observer which are used by two controllers.
The video of the real experiment is available here:
https://scaron.info/publications/icra-2019.html

You will see that this was implemented in HRP-4 using OpenRTM and mc_rtc (two others conrol framework using various middleware).
We reimplemented 4 or 5 variations in my lab with our own custom architecture.

Another real life example tested by one of our master student similar to the one given by @saikishor can be found in this document https://hal.laas.fr/hal-02964034v1. In p15 you will find a more detailed and complex example of what @saikishor is describing. The state estimator is used for:

• the DCM Controller
• the CoM admittance controller
• the Generalized Inverted Kinematics

We reimplemented the same design pattern for a conference paper in Humanoids 2022 https://hal.laas.fr/LAAS-GEPETTO/hal-03452196 mixing our new in house controller and PAL robotics Inverse dynamics (see Fig.3 of this paper) . It allowed us to perform torque control on TALOS (https://youtu.be/NdZpPIR-hKM).

During ICRA 2023 I learned during a discussion, that another group (DFKI) reimplemented another variation of the same design pattern.
We still have issue with some of the state estimators. This has a drastic impact on the quality of the control.
Being able to change it would be awesome as it has a strong impact on the other parts of the control architecture.

I hope this convince you that this is a realistic example, or at least one of the humanoid robotics community.

My hope is that by using ros2_control chainable architecture we will be able to exchange some of these control blocks.

IMHO the examples given in the previously cited papers are more complex than the theoretical one provided by @saikishor.
But I can create a PR providing links to the previously cited examples to give more weight to the argument.

[saikishor]

Hello @olivier-stasse,

Thank you so much for your review.

[saikishor]

We should work on a realistic example, i.e. have real use-case in docs. Otherwise people start to re-create whatever they see.

Hello @destogl,

I'm sorry that I couldn't reply to you sooner as I was busy with ICRA conference. I agree with you that others might recreate things if the examples are not so realistic enough. However, I would like to have one more in the diagram, to clearly show the paradigm of the newly added estimated interfaces as we wanted them to have a realistic example. How about changing the name from odom_publisher to slip_detection? or any other name that's meaningful and makes sense? Please let me know.

Regarding the review of @olivier-stasse, what do we do with the naming estimated to state?, can we have a discussion on it?. I will be waiting for your review.

Best Regards,
Sai

[christophfroehlich]

Regarding the review of @olivier-stasse, what do we do with the naming estimated to state?, can we have a discussion on it?. I will be waiting for your review.

I agree with Olivier that we shouldn't use estimated here. From my understanding of control theory this is clearly related to the estimation of a dynamical system's state. In the chained control loop there could be some kinematic calculations in the backward path (odometry,..), or a block performing signal processing (filtering, outlier detection..) -> no one would call them estimator.

After merging #985 we should also add docs about the state interfaces in there.

[mergify]

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

[jakewelde]

I just wanted to echo support for this feature improvement. I'm using ros2_control for aerial vehicles, and have a situation in which one hardware component provides state_interfaces for the measurements from an IMU (gyroscope and accelerometer) and another provides lower bandwidth pose estimates (e.g. a motion capture system, a 3rd party visual-inertial odometry system). I would like to run an "estimator" controller that fuses these measurements into a high bandwidth pose and twist estimate, and then chain that controller with the actual controller that takes the system's kinematic states as its state input, but at present the flow of information in chained controllers is essentially one-way.

If this gets merged in, I will definitely make use of it! Thanks!

[mergify]

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

[jakewelde]

Hey @saikishor, thanks again for your hard work on these features! I was just wondering if you have a sense of the approximate horizon over which exporting state interfaces from controllers is likely to get merged in? I'd love to make use of this functionality, but may make some different architectural decisions if this is slated more towards the long term development plan. Any general guidance you can offer is a great help. Thank you!

[JohannesPankert]

Same here, this is a feature that we very much need and we are happy if this will be merged soon.

[saikishor]

Requesting docs (here and into the release notes) 😇 or at least a follow-up issue

@christophfroehlich The docs and the release notes are updated :)

Thank you!

[christophfroehlich]

Am I missing something, or should we briefly introduce what a state interface or reference interface is?
We introduced "reference" here but in the context of chainable controllers, this distinction is not yet very clear imho.

[saikishor]

Am I missing something, or should we briefly introduce what a state interface or reference interface is? We introduced "reference" here but in the context of chainable controllers, this distinction is not yet very clear imho.

@christophfroehlich I've added a bit more of the documentation! Thank you!

[destogl]

Just two more comments.

[destogl]

I am not sure about that. I would the functionatliy of state interface not at all sort into chain mode, as it doesn't influence the functionaliy in any way.

[destogl]

Can we move this to controller_interface.hpp? Then every controller could expose its data.

And I would add defalut implementation to the controller_interface.cpp because then we don't push anyone to implement this, only if they want. And we keep API compatibility.

How would this influence cycling graph search?

[saikishor]

Are you talking about this on_export_state_interfaces method? or the export_state_interfaces method?

If it is export_state_interfaces method, then as you mentioned it already exists in the controller_interface_base.hpp and a default implementation is done inside controller_interface.cpp as it is done similar to the export_reference_interfaces

[destogl]

Thanks for following up on this!!!