Force discontinuities when adding contact

[pFernbach]

Hello,
When adding a new contact, the contact forces of the other contacts computed by the InverseDynamicsFormulationAccForce solver are discontinuous. See the plot below that shows the forces along z of the 4 feet of a quadrupedal. At the beginning, only 3 feet are in contact, at t=6.10 the LF foot create contact.

As you can see, the total force is immediately (in one iteration) distributed between the 4 contacts, which create discontinuities.

Is there any way in the current implementation to have a 'smooth' change of the force distribution after adding a new contact or do I have to compute a force reference myself and send it with contact.setForceReference() ?

[andreadelprete]

I think there're two ways to achieve this.

Adjust the regularization weights

When you add a new contact you can specify the regularization weight. If you initially set this weight quite high (e.g., 1) with a zero reference, I expect the initial force is gonna be almost zero. Then you can smoothly change this weight to reach a small value (e.g., 1e-5), which will allow the contact force to increase.

Use a time-varying upper bound on normal force

This is already done when you remove contacts so it shouldn't be too hard to do the same when you add contacts, but it's still more work than the previous solution because it requires you to modify the code. The idea is to have a time-varying bound on the normal force, so that it's forced to change smoothly. For the case when you remove a force, what's currently implemented is an upper bound that goes from the current value to almost zero as a linear function of time. The user can decide how fast this transition is using the parameter transition_duration.

[pFernbach]

This is the result with your first solution:

It is working as expected, thanks. I had to add updateRigidContactWeights to the python API, I will make a PR.

[andreadelprete]

Nice!