Contact-aware simulator as a library in Matlab

[gabrielenava]

We are thinking of rewriting the matlab balancing controller (mex-wholebodymodel) such that it will be able to deal with contact switching. This may be useful because most of our work on balancing requires to deal with contact switching and to model impacts (e.g. push recovery, yoga demo, ...).

Currently, we have two balancing controllers: WBI-Toolbox-controllers is developed in Simulink, while simulations are performed using Gazebo simulator. In mex-wholebodymodel, instead, both controller and simulator are developed in matlab, but the software is not able to model impacts and to deal with contact switching. We would like to use the C++ library drake to wrap its simulation capabilities in a C++ class in iDynTree, that then can be exposed in matlab bindings. Once we have such a class, we can migrate the controllers in mex-wholebodymodel to use such a class.

The main convenience of having such a "simulator as a library" is that we could programmatically launch several simulations one after another, with for example a little change if the initial conditions to test the properties of the controller in a more sistematic way. Such a workflow would be difficult to realize using the Gazebo simulator, as it would required to reset and relaunch the Gazebo enviroment from the Matlab code.

The amount of C++ code necessary to create a basic simulator for a single robot interacting with a flat terrain is minimal in Drake, see for example [1] and [2]. However wrapping such a code in a iDynTree class is necessary to provide a stable and reliable Matlab interface, as currently the work of the Matlab interface of Drake has been halted to concentrate the works on the C++ version (see the disclaimer on the main Drake documentation). Furthermore, if we find a lot of different conventions between Drake and our usual notation, we can concentrate the conversion functions in such a class.

[1] : https://github.com/RobotLocomotion/drake/blob/master/drake/examples/kuka_iiwa_arm/run_kuka_iiwa_arm_dynamics.cc#L51 [2] : https://github.com/RobotLocomotion/drake/blob/master/drake/examples/Valkyrie/valkyrie_simulation.cc#L43

[gabrielenava]

FYI @DanielePucci @iron76

[traversaro]

Just to clarify: implementing such a class could take quite a bit of effort (but it would be definitely a good C++ exercise for @gabrielenava) so this is mainly a proposal open for discussion. I think "simulator" as a library is definitely something that could simplify a lot of workflows.

[traversaro]

From the integration point of view: while C++ Drake has a lot of dependencies, all of them can be disabled, and the simulation classes just depend on an handful of libraries (Bullet, Eigen) on which we already depend, hence it would be trivial to eventually include drake in the codyco-superbuild (or just rely on drake binaries [1], whenever they become available).

[DanielePucci]

Ok, it sounds pretty nice to me. I've just one point: the mex-wholebodymodel based integrator was initially conceived for knowing exactly what the simulator was doing, and I think that such a feature may be useful even for looming applications. So, we should somehow retain the possibility of integrating the equations we want through specific integration classes.

[iron76]

I agree on all the points raised above. Before considering any software development, as a lab we need to be aware of the alternatives and current state of the art. Currently, I don't feel confident enough on the theoretical background behind numerical integration of non smooth dynamical systems (e.g. the work by Bernard Brogliato). In this sense, we should definitively take advantage of our collaboration with Alessandro Saccon who is interested in similar problems.

[traversaro]

Thanks to the effort of @S-Dafarra on the optimal control front, we now have a "simulation" infrastructure, complete with a few basic integrators, that could be extended to use also external integrators such as the one provided in Sundials or in Drake. Furthermore OpenRobotics is currently developing the Ignition Physics library https://bitbucket.org/ignitionrobotics/ign-physics that should provide the "simulator as a library" for all the physics engine supported in Gazebo. I think it make sense to close this issue, and if someone else is eventually interested to get something like this in iDynTree (for example for integration with the matlab bindings) he/she can open a new issue/reopen this one.

[traversaro]

cc @diegoferigo