Force prediction accuracy

[andreadelprete]

I open this issue to discuss the accuracy of the exponential simulator, which in my latest tests with the Python simulator does not seem to exceed the accuracy of the Euler simulator.

As a first test I've started investigating the accuracy of the force prediction based on the linear dynamical system, which is at the core of the exponential simulator. I've run some tests with the quadruped performing a sinusoidal squatting motion with the CoM. Looking at this plot, the accuracy seems rather good (at least for time steps of 5 ms): In this test I was integrating with the Exponential simulator (using dt=5ms and ndt=50), and using the matrix exponential computed at the first inner step to predict the contact forces at the next 49 inner steps. The blue circles are the forces at the beginning of each time step, used as a starting point for the prediction. The red triangles are the predicted forces, and the blue crosses are the real forces. We can see that the prediction matches rather well the real forces, especially in the first part.

[andreadelprete]

In the test shown above the matrix exponential was used for the prediction, but not really for the integration (actually it was used also for the integration, but with such a small time step 0.1ms that it hardly played a role). In the next test, I should instead use the exponential for integrating with a large time step, to see whether the overall state integration is accurate. If it's not (as I expect, given my previous tests reported in the issue #9 ) then it means that some of the other terms in the dynamics are changing too fast for the time step used. Once I've identified which terms are causing this problem, I could consider having intermediate steps in which I only update these terms, without recomputing the matrix exponential.

[andreadelprete]

Here another few examples of nontrivial force predictions using the matrix exponential. All force predictions are over an horizon of 5 ms, whereas the ground truth is computed with a time step of 0.1 ms. In these examples we can appreciate how the matrix exponential is able to predict complex force trajectories that have varying trends (e.g., first increase and then decrease).

This one is very nice and corresponds to the beginning of the simulation, where we start with zero contact force and the matrix exponential predicts correctly the sudden rise of the contact forces. I think this is encouraging for the use of exponential integrators in case of impacts:

In this example the force first decreases rapidly, then it increases slowly, and then in decreases again, but slowly, so we have 3 trends in only 5 ms. The force prediction matches quite well the first two trends, but then fails to predict the last decrease.