Custom controllers: base interfaces, plugins, and fixed-base Computed Torque example

[diegoferigo]

This PR adds the initial support to insert custom controllers in the simulation as model plugins.

Controllers could inherit from the following interfaces:

• scenario::controllers::Controller Used to initialize / step / terminate a controller.
• scenario::controllers::SetJointReferences Used to set the joint references.
• scenario::controllers::SetBaseReferences Used to set the base references.
• scenario::controllers::UseScenarioModel Controllers could optionally use internally a scenario::gazebo::Model object to get / set data. This interface streamlines how the pointer to the model object is handled.

An example of a fixed-base controller implemented with some of these interfaces is scenario::controllers::ComputedTorqueFixedBase.

Then, there is a Gazebo plugin scenario::plugins::gazebo::ControllerRunner that can be inserted during runtime and executes controllers that implement those interfaces. The configuration of the controller is passed from the plugin as an XML configuration.

Right now we do not load the controller classes as plugins (since it would be a plugin loading a plugin). In order to keep things simple, I have implemented a controller factory that for the moment allows executing only the controllers part of the project.

Note that this design choice allows using the custom controllers also with ScenarI/O APIs implemented for a real robot (e.g. for YARP or ROS). In that case, a secondary thread / process will run the controller, and depending about how the ScenarI/O APIs will be implemented, it will read the references from either shared memory of pub-sub. After each call, the controller will send the command to the robot likely through a middleware.

[diegoferigo]

Ready to be reviewed @traversaro @paolo-viceconte @raffaello-camoriano

[traversaro]

This is quite nice. I think a short description of the architecture somewhere (even something extremly simple for example a sentence that says that controllers are mapped to ignition-gazebo's Systems, and that input/output interconnections are realized by reading from and writing to components buffers) and some examples or tests would be great.

My only question is: both whole body and joint level controllers get updated during the PreUpdate . As far as I understood from your previous comments at the moment in the ignition-gazebo documentation there is no information or guarantee on when which one will be actually executed before, but in your practical experience did you checked what is the execution order?

[diegoferigo]

As far as I understood from your previous comments at the moment in the ignition-gazebo documentation there is no information or guarantee on when which one will be actually executed before, but in your practical experience did you checked what is the execution order?

I just gave it a try. I have only the Physics and ControllerRunner systems right now, and the order of execution of the callbacks seem to follow the order of plugin loading.

This means that if I insert first Physics and then ControllerRunner, I get the following execution order:

1. Physics::PreUpdate
2. ControllerRunner::PreUpdate
3. Physics::Update
4. ControllerRunner::Update
5. Concurrent execution of Physics::PostUpdate ControllerRunner::PostUpdate in different threads

I validated this behaviour switching the order of plugins loading.

This is quite nice. I think a short description of the architecture somewhere (even something extremly simple for example a sentence that says that controllers are mapped to ignition-gazebo's Systems, and that input/output interconnections are realized by reading from and writing to components buffers) and some examples or tests would be great.

I agree that it would be nice having some documentation. I have almost ready a simple Sphinx-based website. I will add this piece of information there.

I already have tests running locally for controllers and also part of ScenarI/O APIs. I will open a PR to add them all together. The final PR refactoring -> devel must have green CI.

[traversaro]

And the order of plugins loading is given by the order in which they are found in the SDF file?

[diegoferigo]

And the order of plugins loading is given by the order in which they are found in the SDF file?

I load the plugins from Python code, not SDF. I'm quite sure though that you would get the same behaviour also from an SDF file.

[diegoferigo]

I remember I read long time ago a discussion about it, I managed to find it:

ignitionrobotics/ign-gazebo#100

Edit: now that I read that PR, I realize that the problems I faced when I tried to implement the non-deterministic mode in #158 were strongly related to this concurrent access to components and the solution prosed in that PR could solve them. I was thinking to a protection with mutexes, but I couldn't find any simple implementation. I believe that having support directly form upstream would greatly simplify it.

[diegoferigo]

Cleaned the history. Merging.