Sanity check: Bilateral contacts about consim HOT 25 CLOSED

@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.

from consim.

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

from consim.

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.

from consim.

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

from consim.

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.

from consim.

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

from consim.

@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.

from consim.

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 "cone_flag_=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() ??

from consim.

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

from consim.

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.

from consim.

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.

from consim.

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

from consim.

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

from consim.

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

from consim.

yes, the bug was in this line

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

from consim.

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

from consim.

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

from consim.

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.

from consim.

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!

from consim.

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:

The controller time step was 5 ms, and the number in the legend represents how many simulation steps were taken for each control loop.

from consim.

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

from consim.

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.

from consim.

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.

from consim.

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.

from consim.

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

from consim.