Bipedal-locomotion-framework: Port the matlab-whole-body-simulator in the repo

[GiulioRomualdi]

In this issue, we can discuss the points required to port the matlab-whole-body-simulator in blf. https://github.com/dic-iit/matlab-whole-body-simulator

Blf has already the machinery to compute the dynamics of the system and integrate it. Discussing with @Giulero implementing the whole-body-simulator he developed in C++ should not be so complex.

Before implementing the dynamics we need

• [ ] Add the possibility to reset the integrator transparently to the user. The dynamics designed in the matlab-whole-body-simulator is hybrid and so we should reset the integrator when an impact with the environment occurs.

cc @traversaro @S-Dafarra @nunoguedelha @Giulero

[traversaro]

[ ] Add the possibility to reset the integrator transparently to the user. The dynamics designed in the matlab-whole-body-simulator is hybrid and so we should reset the integrator when an impact with the environment occurs.

The tricky aspect here is that to integrate an hybrid system with a variable step integrator you need some kind of hybrid integrator, as the so-called zero-crossing detection is part of logic that the integrator uses to determine the simulation step. If we want to go down this path an alternative to roll our our own integrator is to use an existing integrator for this, such as the one provided by Sundials (see https://github.com/RobotLocomotion/drake/issues/1306#issuecomment-173923818 for a related comment).

A simple example of integrating an hybrid system (in particular, a bouncing ball) with Sundials can be found in https://github.com/CATIA-Systems/FMPy/blob/e1927a128a9552f20304587c54c716750e73b05f/tests/test_cvode.py#L12 (it is in Python, but it uses Sundials bindings that match 1:1 the C version).

Another possible source of integrators (or even of inspiration if we want to roll out our own integrators, given that its code is rather commented and compressible even if it is written in C++) is Drake: https://drake.mit.edu/doxygen_cxx/group__integrators.html .

[traversaro]

Sorry, I did not mentioned that for a first implementation we can avoid to do zero crossing detection, and just reset the integrator whenever the witness function/even trigger passes through zero.

[nunoguedelha]

* The dynamics designed in the `matlab-whole-body-simulator` is hybrid and so we should reset the integrator when an impact with the environment occurs.

Hi guys, quick question: what do you mean exactly by resetting the integrator?

Just for clarifying, here's what we're doing in the matlab-whole-body-simulator simulator:

• We are using the MATLAB ode45 integration method, integrating the velocity $\dot{s}(t)$ to obtain the updated position $s(t+dt)$, and integrating the acceleration $\ddot{s}(t)$ to obtain the updated velocity $\dot{s}(t+dt)$.
• On the next iteration, if an impact is detected (contact vertex z position < 0), we correct the joints velocities such that the linear velocity of that vertex is 0 (this should be improved, only the vertical component of the velocity should be 0 in case of 0% restitution). After that, the integration step is done with the corrected joints velocities, and thus the vertex height stays unchanged. So this velocity correction is a reset in some way, but there is like a tolerance: although the vertex motion is stopped (no further sinking on the ground), the position is not corrected (keeps its small negative value).

this shouldn't be a problem, but if it is in some context of the BLF, one possible solution would be to reset the joint positions to the previous values.

Side note: using a zero crossing detection approach here wouldn't be trivial since the violated limit does not concern the integrated variable but rather the output of the forward kinematics.

[GiulioRomualdi]

Hi guys, quick question: what do you mean exactly by resetting the integrator?

Exactly this.

On the next iteration, if an impact is detected (contact vertex z position < 0), we correct the joints velocities such that the linear velocity of that vertex is 0 (this should be improved, only the vertical component of the velocity should be 0 in case of 0% restitution). After that, the integration step is done with the corrected joints velocities, and thus the vertex height stays unchanged. So this velocity correction is a reset in some way, but there is like a tolerance: although the vertex motion is stopped (no further sinking on the ground), the position is not corrected (keeps its small negative value).