Pass controller manager update rate on the init of the controller interface

[saikishor]

This PR is done on top of the PR Pass URDF to controllers on init. This PR adds the controller manager update rate into the controller interface as this information is not known to the controllers and this might be very useful for controllers running at different frequencies than the controller manager to be able to interpolate their commands.

Industrial solutions like Elmo control boards tend to have very stiff position gains, and this means they expect a reference at every control cycle. We have a whole body control that runs at 500 Hz and a controller manager that runs at 2000Hz, the position reference that comes out of WBC only gets updated every 4 cycles (2000/500), and this is creating a jerky reference to the low-level control and hence noisy joints. Instead, if we clearly know this number of cycles, we could plug in an intermediate chainable controller, that accepts reference from the WBC and interpolates it for the next 4 cycles to send jerk-free references.

This PR also changes the default controller update rate to CM's update rate auto_declare<int>("update_rate", cm_update_rate);

Thanks to @christophfroehlich for the discussion and sharing insights on this PR

[codecov]

Codecov Report

Merging #1141 (7d5a77a) into master (b33e579) will decrease coverage by 0.03%.
The diff coverage is 40.00%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1141      +/-   ##
==========================================
- Coverage   47.68%   47.65%   -0.03%     
==========================================
  Files          41       41              
  Lines        3452     3452              
  Branches     1873     1873              
==========================================
- Hits         1646     1645       -1     
+ Misses        480      479       -1     
- Partials     1326     1328       +2     

Flag	Coverage Δ
unittests	47.65% <40.00%> (-0.03%)	⬇️

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

Files	Coverage Δ
...controller_interface/controller_interface_base.hpp	33.33% <ø> (ø)
...roller_interface/src/controller_interface_base.cpp	48.14% <0.00%> (ø)
controller_manager/src/controller_manager.cpp	38.69% <50.00%> (-0.09%)	⬇️

[christophfroehlich]

I had to leave earlier in the last WG meeting where you explained why you need to interpolate from the controllers. Could you maybe explain this again here, why this is necessary? Thanks!

[saikishor]

I had to leave earlier in the last WG meeting where you explained why you need to interpolate from the controllers. Could you maybe explain this again here, why this is necessary? Thanks!

Sure. I have updated the description

[christophfroehlich]

Industrial solutions like Elmo control boards tend to have very stiff position gains, and this means they expect a reference at every control cycle. We have a whole body control that runs at 500 Hz and a controller manager that runs at 2000Hz, the position reference that comes out of WBC only gets updated every 4 cycles (2000/500), and this is creating a jerky reference to the low-level control and hence noisy joints. Instead, if we clearly know this number of cycles, we could plug in an intermediate chainable controller, that accepts reference from the WBC and interpolates it for the next 4 cycles to send jerk-free references.

Does the chained controller run on 2000Hz now? Otherwise it could not interpolate in between. Then it has to add a delay of one update of the WBC (2ms) to interpolate between the last two references from the WBC, is this correct?
Which controller needs now the cm update rate in your example? The chained one needs to know his update rate and the one of the WBC, but what can he do with the info of the CM udpate rate?

[saikishor]

Does the chained controller run on 2000Hz now? Otherwise it could not interpolate in between. Then it has to add a delay of one update of the WBC (2ms) to interpolate between the last two references from the WBC, is this correct?
Which controller needs now the cm update rate in your example? The chained one needs to know his update rate and the one of the WBC, but what can he do with the info of the CM udpate rate?

You are right. In this exact case, we cannot do anything with the CM update rate, but also the problem is most of the cases when we have the controllers running at CM update rate, internally the get_update_rate method returns zero instead of the CM update rate, so there is no way I can compute as of now. Unless, I manually set the update_rate value to that controller. I also want to use this value to set that update_rate value internally.

Moreover, I want to use the CM update rate to raise warnings for controllers that cannot run more than a certain update rate. For instance, I want to return error on activation, if someone wants to start the WBC at 2kHZ, then I can raise an error that it cannot run at that frequency or in some cases, that this needs to be run with is_async or something like that.

What do you think?

[christophfroehlich]

but also the problem is most of the cases when we have the controllers running at CM update rate, internally the get_update_rate method returns zero instead of the CM update rate, so there is no way I can compute as of now.

To summarize, the problem with the current code is that the controller has no idea at what rate it runs if it is not set explicitly. Until it runs, because then the period is known from the update method argument.
We could then also set the parameter from the CM to the cm_update_rate, without the need for much change in the interfaces?

[saikishor]

We could then also set the parameter from the CM to the cm_update_rate, without the need for much change in the interfaces?

Well, I thought of doing the same, but in the end, I decided to go this way, as the cm_update_rate is not exactly a parameter to the controller itself semantically. If this is the case, we could have done the same with the URDF too right? However, if there is some opinion for it to be better as a parameter, I'm open to changing it.

Thank you

[saikishor]

We could then also set the parameter from the CM to the cm_update_rate, without the need for much change in the interfaces?

Regarding this, the controllers won't see any difference, as they will only need to implement the on_init method here. This init method is only called by the controller_manager. However, this change and Bence's change does break ABI.

[christophfroehlich]

LGTM now.
We thought that there is no need for the controller to get the cm_update_rate, if get_update_rate() always returns a meaningful value.