Support Gazebo simulation embedded in the WB-Toolbox block, communicating over standard YARP C++ device interfaces

[traversaro]

Summary

Support Gazebo simulation embedded in the WB-Toolbox block, communicating over standard YARP C++ device interfaces.

Motivation

Users will just need to press play to launch a simulation, without the need to launch Gazebo separately. In particular, I remember seeing once how @FabioBergonti workflow was extremely complicated due to this.

Additional context

From the user point of view, I think that feature can be implemented by creating a new type of "configuration" block, that takes in input all the usual parameters, with these differences:

• No parameters related to YARP ports/YARP wrappers: i.e. no robotName (that is actually the port prefix) and the controlBoardsNames. This parameters will be replaced with some parameters necessary to get the correct devices from the "device database" available in the GazeboYarpPlugins::Handler singleton class: https://github.com/robotology/gazebo-yarp-plugins/blob/master/libraries/singleton/include/GazeboYarpPlugins/Handler.hh#L117 (device name should be sufficient, but this probably will need to be revised to support multi robot).
• Some parameters for Gazebo configuration:at the minimum, a .world file to specify the scenario to simulate. The .world could probably also exploit the recent features added in gazebo-yarp-plugins to ensure flexible composition of yarp-based models.

From the implementation point of view, it is probably necessary to refactor the wbt::RobotInterface and the wbt::Configuration.

[traversaro]

Ref : https://github.com/robotology/gazebo-yarp-plugins/issues/391#issuecomment-441965101 .

@traversaro Let me figure out if I understood what you proposed. Assuming you are talking about Gazebo < 11, are you proposing to execute gazebo in a new thread spawned by Simulink? In this way, it would be possible to get the pointers of the plugins that expose the YARP interfaces (e.g. the controlboards) allowing attaching directly to them.

Yes! Note that the thread spawning is actually the only way to use the Gazebo < 11 API (see https://bitbucket.org/osrf/gazebo/issues/2509/running-physics-world-step-wise). A good example (even better than Gazebo official examples, as it includes also the sensors) of how to run Gazebo < 11 in this way is https://github.com/kuka-isir/rtt_gazebo_embedded/blob/master/src/rtt_gazebo_embedded.cc#L552 (courtesy of good old @ahoarau )

It would be very cool for what concern the simulations and I don't think it would require a massive amount of work. However this might not be compatible to the usage of the real robots. How should the block behave in such a way that the simulated robot exposes the same interfaces and communicates in the same way of the real one? Right now, always in the case of the controlboards, we read the same ports with different prefixes, but going in this direction we would loose this interchangeability, is this right?

I think that switching from the embedded Gazebo simulation to an external robot communicating over YARP ports (both gazebo standalone or real robot) should be as simple as switching from one Configuration block to another, but probably I am missing some technical aspect of the existing implementation.

P.s. Bonus point: starting with Gazebo >= 11, we can even embed gazebo directly inside a block ;) Yes, we can do it also right now but it would be more involved.

Indeed! On the other hand, keeping the communication over YARP C++ interfaces (and hence using the existing WB-Toolbox blocks) ensure that we maintain compatibility with all the existing controllers and we maintain easy interoperability with communicating with a robot over the YARP network. Furthermore, it can also simplify exploiting all the core of the machine.

[traversaro]

A tricky aspect is how to provide visualization capabilities (assuming Gazebo < 11 in the following). As in the proposed configuration WB-Toolbox will run a normal Gazebo server (code equivalent to gzserver process) it should be sufficient for the user to manually spawn a gzclient to visualize the simulation going on on the Simulink side.

This manual step can be avoided if we add the option for spawning automatically (and closing) the gzclient process from the WB-Toolbox itself, using one of the existing libraries to spawn process in a platform-independent way:

• https://gitlab.com/eidheim/tiny-process-library
• https://github.com/sheredom/process.h
• https://github.com/DaanDeMeyer/reproc

If the gzclient is launched by the WB-Toolbox, it is also possible to specify a custom GAZEBO_MASTER_URI (http://gazebosim.org/tutorials?tut=components) to avoid interfering with other running Gazebo simulation in the system.

[diegoferigo]

I think that switching from the embedded Gazebo simulation to an external robot communicating over YARP ports (both gazebo standalone or real robot) should be as simple as switching from one Configuration block to another, but probably I am missing some technical aspect of the existing implementation.

If YARP ports are involved yes, it would be straightforward. Though, I thought that in the simulated case you proposed, WB-Toolbox would attach directly to the interfaces, without the need of wrappers. Actually, all the blocks that access robot resources pass through the RobotInterface class to get the YARP interfaces in a lazy-mode. And the RobotInterface is created out of a Configuration object. If the Configuration object has a flag Run Gazebo, the associated RobotInterface can either open a RemoteControlBoardRemapper if it is false, or get the pointers of the interfaces from Gazebo.

From the implementation point of view, it is probably necessary to refactor the wbt::RobotInterface and the wbt::Configuration.

Probably the best architecture would be transforming the RobotInterface to a C++ interface with two implementations: gazebo-based and remotecontrolboardremapper-based. The Run Gazebo configuration flag would select one of the two. This approach would not require any edit to any other block.

More in details, the implementation of the gazebo-based RobotInterface can initialize the gzserver with a world passed from the Configuration block, and a modified version of the SimulatorSynchronizer can get the pointer of gzserver out of the RobotInterface (dynamic_casting it during the first run) and perform the simulation step. This approach would also remove the need of the ClockRpc thrift.

@traversaro Would this be feasible? I never looked how to execute gazebo < 11 in this way.

[traversaro]

If YARP ports are involved yes, it would be straightforward. Though, I thought that in the simulated case you proposed, WB-Toolbox would attach directly to the interfaces, without the need of wrappers. Actually, all the blocks that access robot resources pass through the RobotInterface class to get the YARP interfaces in a lazy-mode. And the RobotInterface is created out of a Configuration object. If the Configuration object has a flag Run Gazebo, the associated RobotInterface can either open a RemoteControlBoardRemapper if it is false, or get the pointers of the interfaces from Gazebo.

Yes, this is exactly what I meant. If I understand correctly, once this is implemented, switching between Gazebo-embedded and YARP-ports will be just changing the configuration block (or enabling/disabling two different configuration). The implementation may be complex, but once implemented the user experience will be quite straightforward, or I am missing something?

[traversaro]

Probably the best architecture would be transforming the RobotInterface to a C++ interface with two implementations: gazebo-based and remotecontrolboardremapper-based. The Run Gazebo configuration flag would select one of the two. This approach would not require any edit to any other block.

Yes! However just having two classes will have a lot of duplication in the iDynTree-related RobotInterface's classes, so it coould make sense to keep them in a third class, or something like that. Another thing that it could make sense to have is two Configuration blocks, one for RemoteControlBoardRemapper based communications, and one for embedded Gazebo one.

More in details, the implementation of the gazebo-based RobotInterface can initialize the gzserver with a world passed from the Configuration block, and a modified version of the SimulatorSynchronizer can get the pointer of gzserver out of the RobotInterface (dynamic_casting it during the first run) and perform the simulation step. This approach would also remove the need of the ClockRpc thrift.

Yes! However, the co-simulation with an external Gazebo can still have a lot of use cases, so I would not remove it.

[diegoferigo]

Yes, this is exactly what I meant. If I understand correctly, once this is implemented, switching between Gazebo-embedded and YARP-ports will be just changing the configuration block (or enabling/disabling two different configuration). The implementation may be complex, but once implemented the user experience will be quite straightforward, or I am missing something?

Yet it would be straightforward.

Yes! However just having two classes will have a lot of duplication in the iDynTree-related RobotInterface's classes, so it coould make sense to keep them in a third class, or something like that. Another thing that it could make sense to have is two Configuration blocks, one for RemoteControlBoardRemapper based communications, and one for embedded Gazebo one.

I would keep a single Configuration block, and playing around with showing / hiding elements in the Simulink mask.

About the RobotInterface, what we can do instead of having a pure virtual class is providing a class that implements the model logic and exposes virtual methods for the YARP interfaces. Then the two implementation will implement only them.

Yes! However, the co-simulation with an external Gazebo can still have a lot of use cases, so I would not remove it.

Sure!

[traversaro]

I think that the refactor of wbt::base::RobotInterface discussed in this issue would probably also be useful to be able to run the Simulink-generated source code as part of YARP device that is running as part of the main yarprobotinterface of the robot in the icub-head, in which the pointers to the device to which to connect are passed via the IMultipleWrapper::attachAll(const PolyDriverList & p) method.

[diegoferigo]

I suspect that in the use case you described we might have the need to pass through a singleton in order to pass those pointers to the wbt::base::RobotInterface class.

More in detail, what I am asking myself is how to pass these pointers from the attachAll method to the autogenerated class, which has only few methods exposed (configure, step, terminate, etc).

[traversaro]

I suspect that in the use case you described we might have the need to pass through a singleton in order to pass those pointers to the wbt::base::RobotInterface class.

More in detail, what I am asking myself is how to pass these pointers from the attachAll method to the autogenerated class, which has only few methods exposed (configure, step, terminate, etc).

I tought that we had some freedom on how the autogenerated class is generated (some template, etc etc) but probably I was just confused.

Indeed, the easiest solution (if the generated parameters API is not working as expected, see https://github.com/robotology/blockfactory/issues/42#issuecomment-461453346) would be to set them in some sort of singleton, possibly using some kind of identifier string for the autogenerated model, if we eventually want to support to have multiple different autogenerated models inside the same yarprobotinterface.

[diegoferigo]

I tought that we had some freedom on how the autogenerated class is generated (some template, etc etc) but probably I was just confused.

For sure we can add what we want in the cpp file. I am pretty sure we can also add classes and structs in the header of the autogenerated source, but I never did it (only headers are included right now). However, I never read anything about the possibility to operate on the declaration of the autogenerated class.

Alternatively to the singleton, if we can add our own structs in the headers where we might store there the pointers to the interfaces, but this would mean that the TLC provided by blockfactory will not be anymore general.

[diegoferigo]

Since this issue is being linked quite a lot around our repos, it is worth adding a link to the GazeboWrapper class I implemented for another project. It wraps the new Ignition Gazebo simulator used as a library, solution that provides a fully reproducible simulation. The wrapper is already functional, though both simulation and our robot models need improvements before considering to start drafting the wb-toolbox inclusion.

[traversaro]

Interesting example of a similar approach by the Yonohub company: https://towardsdatascience.com/yonohub-autonomous-vehicles-using-blocks-ef4a1838d92c .

[traversaro]

Something that we may want to evaluate for linking Gazebo libraries with Matlab/Simulink, is that apparently Matlab ships its own vendored Protobuf, that could create problems if it is not the same version of the system's Protobuf used by Gazebo.