Exponential VS Rigid-contacts

[andreadelprete]

Today I've run some tests on a classic rigid-contact simulator, without much hope to find anything interesting, but I was wrong.

I've implemented a simple simulator that uses the pinocchio::forwardDynamics function to compute the accelerations of a robot subject to rigid contact constraints J*dv = -dJ*v + PD, where PD is a proportional-derivative contact stabilization term. Then these accelerations are integrated using explicit Euler. I've assumed contacts cannot break (i.e. bilateral contacts), and I've used the "solo squat" test to compare the accuracy-speed trade-off of this RigidEuler simulator against our good-old Exponential simulator. I was expecting RigidEuler to be better because the rigid contact assumption doesn't lead to stiff differential equations as the viscoelastic model.

First of all, I've seen that it's very important to properly tune the PD contact stabilization gains to get good results, otherwise RigidEuler can perform terribly for large time steps. The best tuning rule I've found is to set kd=1/dt (which tries to bring the contact point velocity to zero in one time step) and kp=0.5*kd^2 (which makes the PD critically damped). Using this tuning I got the following results plotting integration error VS dt: The two simulators performed very similarly for dt<=2.5, but for larger dt (5 and 10 ms) RigidEuler deteriorated. Looking at the results in the viewer I could see the robot's feet penetrating into the ground with some very fast movements before going back in place.

Plotting the integration error VS real-time factor (as in the paper): RigidEuler benefits from being faster for 1 iteration (about 2x), but we can see there is a small range of RT-factors where Exponential seems the best choice. Most importantly, this happens in the high-speed low-accuracy range, which is what we are interested in for MPC.

This is of course a small win, but considering I started these tests expecting not to find anything worth reporting, it's kind-of a big result.

[andreadelprete]

I've repeated the tests above with a larger controller time step, 30 ms instead of 10 ms. In the first plot we can see that Expo works fine even with an integration step of 30 ms, whereas RigidEuler already breaks at 4 ms. In terms of real-time factor, in this case the range for which Expo is the best choice it's larger than before!

[andreadelprete]

I'd be curious to know what @nmansard and @cmastalli think about this. Have you ever seen issues with the rigid-contact model with PD stabilization when using large time steps (>=5ms)?

[cmastalli]

I would assume "optimal control stability" when you refer to "stability".

The optimal control solution works reasonably well for up to 30msec. I also observed that it is much stable for point contacts rather than 6d contacts.

As a reference, I use 10msec as step integration in my MPC algorithm.

[cmastalli]

Another thing, we have implemented a simpletic Euler integrator, which it is better than the explicit one.

[andreadelprete]

With "stability" I refer to "simulation stability", which basically means that the robot doesn't do crazy things. The tested motion is simply the Solo robot, standing on 4 feet and squatting up and down, being controlled with TSID at 30 ms. I've also extensively tested semi-implicit Euler (which I think is what you call simplectic Euler) and it's almost identical to explicit Euler.

I remember you told me you typically used 10 ms integration steps for MPC, which is why I am so surprised it works so bad, and for a motion that is so simple, such as squatting with no contact switches. You can take a look at the code here to double check I made no mistakes.

[cmastalli]

I have a couple questions about your code:

1. Why are computing the quantities (Jc and a0) using LOCAL_WORLD_ALIGNED? (see the implementation in Crocoddyl: https://github.com/loco-3d/crocoddyl/blob/master/include/crocoddyl/multibody/contacts/contact-3d.hxx#L27-L43)
2. Why do you use a regularization value for Jc^MJc?

The point 1 adds an unnecessary computation for both: Jacobian and constrained accelerations. The point 2 might be important as it modifies the contact dynamics. Unfortunately, I have not seriously study this regularization, but I have to confess that it doesn't convince me. I would suggest you to study again the integrator without this regularization value (btw, it is also a sort of big value; if I would like to include this regularization, then I would use 1e-12 as default).

[andreadelprete]

1. Why are computing the quantities (Jc and a0) using LOCAL_WORLD_ALIGNED? (see the implementation in Crocoddyl: https://github.com/loco-3d/crocoddyl/blob/master/include/crocoddyl/multibody/contacts/contact-3d.hxx#L27-L43)

If I remember correctly the reason was to simplify the friction cone constraints, which are constant in world-aligned coordinates. I could compute contact forces in local coordinates, but then I would need anyway to express them in world-aligned coordinates to check the friction cones. Maybe this would be more efficient, but I guess it really depends on the simulation method you use and how accurately you discretize the friction cone constraints (e.g., 4-sided VS 8-sided pyramids).

2. Why do you use a regularization value for Jc^MJc?

Because for a quadruped on 4 feet the contact Jacobian is not full row rank, so you need some regularization. I've repeated the test with 1e-12 and it doesn't change anything.

I will run more tests to see whether this issue is specific of this scenario or it happens in other cases as well.

[nmansard]

I confirm that we are using 10 to 30ms timesteps without any problem. The PD stabilization of the contact is only a D in practice. And it is even kind of working without any stabilization at all.

Yet we are only considering relatively short movements (but high dynamics such as walking and jumping).

I agree with Carlos that regularizing the matrix inversion is likely to disturb a lot the quality of your contact (although I did not check the code yet).

[andreadelprete]

I've just found the issue. Contacts were not set as bilateral, so with RigidEuler sometimes the feet lost contact, which triggered instability. This doesn't happen with soft contacts because the feet are always a bit inside the ground.

If I use bilateral contacts then RigidEuler works well, even with dt=30 ms, and even without stabilization, as you guys reported. Exponential and RigidEuler give very similar integration errors for the same dt. The only difference is computation time, which is 3x larger for Exponential.

This is what I initially expected to find. Thanks for the inputs.

PS: The regularization of the KKT matrix doesn't seem to lead to any problem.