Our LCP's are only guaranteed solvable if A is positive-semidefinite and there are no force bounds. To increase stability of our LCPs, we can do a trick called "Constraint Force Mixing". In practice, this means multiplying the elements on the diagonal of
1.0 + eps, where
eps is some small positive value. This has the effect of ensuring
A isn't singular, and in general reducing "singular-ness" of
You can turn constraint force mixing on and off with
void World::setConstraintForceMixingEnabled(bool enable). Currently, by default CFM is turned off, because our Jacobians don't support it.
This ticket is about supporting it.
A is computed in
dart/constrant/BoxedLcpConstraintSolver.cpp in the method
A matrix it computes is in Open Dynamics Engine format, which means row-major order, where each row's length is rounded up to the nearest multiple of 4 (to allow vectorization) and any padding entries are ignored. This calls out to individual constraints to populate
A. For each constraint, it applies a unit impulse, and then measures the change in relative velocity at the constraint. The method that applies the CFM is
ContactConstraint::getVelocityChange(), towards the bottom.
Supporting this in our differentiation means tracking all the CFM constants for each element of the diagonal of
A, and storing them for later. These are constants wrt differentation, but we need to ensure that we scale
A's diagonals, and the gradient of
A's diagonals, by these constants wherever it is computed.