Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Exponential VS Rigid-contacts #34

Open
andreadelprete opened this issue Mar 24, 2021 · 9 comments
Open

Exponential VS Rigid-contacts #34

andreadelprete opened this issue Mar 24, 2021 · 9 comments

Comments

@andreadelprete
Copy link
Owner

andreadelprete commented Mar 24, 2021

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:
expo_vs_rigid_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):
expo_vs_rigid_rtf
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
Copy link
Owner Author

I've repeated the tests above with a larger controller time step, 30 ms instead of 10 ms.
expo_vs_rigid_dt
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.
expo_vs_rigid_rtf
In terms of real-time factor, in this case the range for which Expo is the best choice it's larger than before!

@andreadelprete
Copy link
Owner Author

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
Copy link

cmastalli commented Mar 29, 2021

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
Copy link

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

@andreadelprete
Copy link
Owner Author

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
Copy link

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
Copy link
Owner Author

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
Copy link

nmansard commented Mar 30, 2021 via email

@andreadelprete
Copy link
Owner Author

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants