[core] Restore constraint lambda in case of integration failure.
duburcqa opened this issue · 3 comments
Currently, the lambda multipliers of the constraints are not reset to their original value if the integration step failed. This is an issue because they are used as initial guess for the constraint solver. This coupling seems to be problematic on gym_jiminy.envs:atlas environment. Here is a snippet to reproduce the issue:
import numpy as np
import gymnasium as gym
env = gym.make("gym_jiminy.envs:atlas", debug=True)
env.reset()
env.simulator.step(0.73)
for _ in range(10):
print("-----")
env.simulator.step(1e-6)
An additional buffers with the values at the previous iteration should be stored next to contactForcesPrev_, fPrev_ and aPrev_.
It appears that the root cause of the exception (Too many successive constraint solving failures.) is not related to integration failure but rather inherent numerical instabilities of the contact solver, which is known to have poor convergence property. Here is a figure of the joint acceleration before raising the exception:
Smoothness of the accelerations is a major concern, in terms of simulation speed but also learning speed.
Two options at may improve convergence:
- implement SOR (successive over-relaxation with rfactor going from 2.0 to 0.0 linearly)
- implement SSOR (symmetric SOR) with factor fixed to 1.0
