Sanity check: Bilateral contacts

[andreadelprete]

To verify that the exponential integrator is working fine we can test it on a linear system with bilateral contacts. In this case the result of the integration should be independent of the integration time step.

I added an input parameter in the method addContactPoint to specify whether the contact should be unilateral. If that flag is set to false, then the contact is treated as bilateral. This means that, once that point collides with an object (whichever), the contact will never break. Moreover, all checks related to friction cones are disabled for bilateral contacts.

Using this new feature I then modified the point_mass_simulation.py script to compare exponential and Euler integrators in the case of a point mass system. To avoid errors due to the differences in collision detection, I started the simulation with the point mass already colliding with the ground (just an epsilon below the floor). The "outer" time step was set to 10 ms, but then I specified a different number of "inner" time steps, either 1, or 10, or 100 (which are the numbers in the figure legend). Here you can see the resulting trajectories (z axis).

As expected, the exponential integrators always give the same result, regardless of the number of inner time steps (1, 10, 100). "Euler 100" (i.e. with an inner time step of 0.1 ms) gave results quite similar to the exponential integrators. "Euler 10" instead gave a very different result. I didn't include "Euler 1" because it went unstable.

In conclusion, this test seems to validate the exponential integrators, and it confirms that they give exact results when applied to linear dynamical systems.

[andreadelprete]

@hammoudbilal can you check I didn't do anything stupid in this commit? 635f3863403ad2b7e050c490e07383a2fae5e251

[andreadelprete]

A note on computation time.

*** PROFILING RESULTS [ms]                                  min        avg        max        lastTime   nSamples   totalTime  ***
check_contact_state                                         0.001      0.000      0.041      0.001      4425       0.591     
compute_contact_forces                                      0.000      0.000      0.024      0.000      2202       0.773     
euler_simulator::step                                       0.037      0.192      0.404      0.038      40         7.681     
euler_simulator::substep                                    0.002      0.003      0.059      0.003      2200       6.990     
exponential_simulator::checkFrictionCone                    0.000      0.000      0.017      0.000      2220       0.400     
exponential_simulator::computeContactForces                 0.001      0.001      0.021      0.001      2220       1.554     
exponential_simulator::computeFrameAcceleration             0.000      0.000      0.019      0.000      2223       0.330     
exponential_simulator::computeIntegralXt                    0.003      0.004      0.033      0.007      2220       8.999     
exponential_simulator::resizeVectorsAndMatrices             0.076      0.433      1.010      1.010      3          1.300     
exponential_simulator::step                                 0.027      0.640      2.109      0.027      60         38.404    
exponential_simulator::substep                              0.014      0.017      0.102      0.026      2220       37.762    
pinocchio::aba                                              0.001      0.001      0.035      0.001      2200       1.914     
pinocchio::computeAllTerms                                  0.000      0.001      0.086      0.000      4425       2.324  

• the inner step (substep) of the euler simulator takes 3 us
• the inner step (substep) of the exponential simulator takes 17 us
• out of these 17 us, only 4 are taken by the matrix exponential

I think this suggests that the implementation of the exponential simulator has a lot of room for improvements because the main difference between exponential and euler simulators should be the computation of the matrix exponential, so I would expect the computation time of the exponential simulator to be roughly 7/8 us.

[hammoudbilal]

In the case of bilateral contact, we also call the double integral computation at every integration step ~ 7 us on the virtual machine I am using currently. also I switched the Minv_ computation back from pinocchio::computeMinverse() to Eigen::MatrixXd.inverse() and it seemed to improve from ~ 5 us to ~ 1 us

computeContactState common for both simulators ~ 1 us computeContactForces also takes approximately 1 us

resizing takes 19 us but it only gets called 3 times in the 2220 substeps computed, which makes it insignificant

[andreadelprete]

I hadn't noticed that the double integral was also computed. Anyway, benchmarking the code with the point mass is likely not a good idea. The state size is so small that many operations that should be negligible may become significant for such a small system. For instance, it's really weird that Pinocchio's computeMinverse is 5x slower than Eigen inverse, so I wonder whether this is just happening for such a small size.

[hammoudbilal]

Alright, I will do the same benchmark with the squatting quadruped, it should give us a better idea

[hammoudbilal]

@andreadelprete I just pushed a script for tsid example with consim, it seems that simulation fails with euler dt = 1.e-3 and ndt = 100 for both bilateral and unilateral contacts can you check "script/consim_py/demo/quadruped_consim.py" I am running tsid at 1 ms also. I just want to make sure I am not doing something stupid in tsid before actually debugging the simulator.

[hammoudbilal]

@andreadelprete I just pushed a script for tsid example with consim, it seems that simulation fails with euler dt = 1.e-3 and ndt = 100 for both bilateral and unilateral contacts can you check "script/consim_py/demo/quadruped_consim.py" I am running tsid at 1 ms also. I just want to make sure I am not doing something stupid in tsid before actually debugging the simulator.

never mind I found the bug , the simulator takes control on full acceleration vector it has no selection matrix on u, I fixed it.

[hammoudbilal]

Here's a follow up, I have had a bug in ExponentialSimulator for the past week that seems a bit hard to solve, I am working with a solo8 TSID base sinusoid example "consim_py/demos/quadruped_consim.py". Euler simulator works well with both unilateral and bilateral contacts for the Exponential, any simulation with doing full integration ( intx and int2x) diverges horribly, the contact velocities fluctuate between -40 m/s to 40 m/s between each integration step (0.1 ms or less).

I went over the signs and algebra multiple times so most likely this is not the issue.

the most interesting behavior appears when I force the integration scheme to use the average force computed instead of computing the double integration. To do this I set "coneflag=true" in ExponentialSimulator::checkFrictionCone() regardless of violating the cone or not. This way the integration loop is forced to use this part if the code to carry out the integration
https://github.com/andreadelprete/consim/blob/master/src/simulator.cpp#L284

if I only run the exponential simulator in my python script, I get the following base motion

now if I add the euler simultion to the same script, the behavior generated by the same exponential simulator differs significantly (green in this figure should match blue in the previous figure)

Can you take a quick look if I am not doing something completely stupid, I am guessing this might have something to do with memory leaks or the way I pass arguments to ComputeDoubleIntegralXt() ??

[andreadelprete]

This looks like a horrible bug to track. I'll take a look asap.

[hammoudbilal]

This was a combination of few bugs, I moved computing all the integration terms (Upsilon, JMinv ...etc) into a separate method and replaced JMinv.transpose() with JcT = Jc.transpose() Minv*JcT this seemed to solve the diverging contact force/velocity issue the other one was resetting the sinusoid offset for TSID, this was a minor fix.

[andreadelprete]

I have also independently fixed the same bug of the reset of the CoM offset (30 minutes ago). Results are still off however. Even the point-mass test now gives unexpected results.

[andreadelprete]

I' ve tried resetting cone_flag to false in line 421 of simulator.cpp, but this didn't solve the problem.

[hammoudbilal]

In my last push I already reset it back to false, I will push the code again

[andreadelprete]

Have you managed to find the problem? Is the point-mass simulation working again now?

[hammoudbilal]

yes, the bug was in this line https://github.com/andreadelprete/consim/blob/e7420cb08c797b1e7a72c2369f6e3365bc01bb06/src/simulator.cpp#L384

calling the contact model before updating the contact velocity in ExponentialSimulator:: computeFrameKinematics()

[andreadelprete]

ok, great that you found the bug. So how is it working now for the quadruped?

[hammoudbilal]

the mass matrix results I posted for the quadruped with TSID are after fixing this bug. Which means it works. Interesting enough we can simulate at 1 ms intervals with the exponential simulator.

one thing I am still not sure about is forcing the floor height to be zero https://github.com/andreadelprete/consim/blob/master/src/object.cpp#L27

this might create issues with integrating unilateral forces at large time steps, if the floor is penetrated few millimeters before activating the contact i see that for bilateral contacts you reset it back to the actual contact point position https://github.com/andreadelprete/consim/blob/master/src/simulator.cpp#L122

[andreadelprete]

Yes, I think forcing the contact height to zero might create problems, so I'd rather set the anchor point to the contact position at the moment contact was detected. I'll comment the results in the other issue then.

[andreadelprete]

I've run a test with the latest verison of consim (master branch) to verify that for the point-mass simulation, as the time step goes to zero, Euler converges to the ExponentialSimulator, and that seems to be the case!

[andreadelprete]

I've carried out some tests using the Python version of the simulator (the one I had developed last year). Using Solo as a robot and a TSID controller with reference squatting motion for the CoM, it seems that by decreasing the time step the two simulators (euler and exponential) tend towards the same solution, as you can see in this plot: ![Uploading consim_quadruped_squatting.png…]() The controller time step was 5 ms, and the number in the legend represents how many simulation steps were taken for each control loop.

[andreadelprete]

For some reason the upload did not work in my previous comment, so here it is:

[andreadelprete]

Running some more tests with the Python simulators it seems that Euler and Exponential give almost the same results when they use the same time step, if the time step is sufficiently small (<=0.1 ms). For larger time steps instead Euler becomes unstable, while Exponential remains stable.

What's really weird to me is that Exponential does not seem to converge towards the ground truth faster than Euler. The only benefit of Exponential seems that it's more stable, so it can be used with large time steps (but of course it's not gonna be very accurate with large time steps). I'm gonna keep investigating because this is fishy to me.

[hammoudbilal]

hi @andreadelprete

for the EulerSimulator I was using ABA to compute the acceleration before integrating, no matter how small I went with the time steps, it would still not match as shown in the figure,

switching from ABA to dv_= invM_*(tau - nle + J.T.f) however fixes the issue

for the point mass the choice of ABA or dv_= invM_*(tau - nle + J.T.f) didn't seem to make a difference, taking smaller steps with euler was enough to get the trajectories to match.

[andreadelprete]

Nice that you found the problem. Indeed in the python simulator I was not using ABA, so this makes sense. However, it's really fishy that ABA does not give the same result as using the inverse of M. Eventually we should try to reproduce the problem in a minimal working example and open an issue in Pinocchio.

[andreadelprete]

I believe this issue is now obsolete, so I close it.