Exponential VS Rigid-contacts about consim HOT 9 OPEN

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!

from consim.

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)?

from consim.

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.

from consim.

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

from consim.

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.

from consim.

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

from consim.

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.

from consim.

from consim.

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.

from consim.