Constraint based on Lagrange Multipliers
SOFA allows the use of Lagrange multipliers to handle complex constraints, such as contacts and joints between moving objets that can not be straightforwarly implemented using projection matrices.
Constraint problem
To solve the dynamic of two constrained objects, we use a Lagrange Multipliers approach and a single linearization by time step. From the physical system to solve, the constraint problem can be expressed with the linear system as:
that can be written in a simpler way as:
where is the vector of constraint forces contribution with matrix containing the constraint directions and are the socalled Lagrange multipliers. Both holonomic and nonholonomic constraints can be used to model the various mechanical interactions involved in the simulation. For each constraint, a constraint law is assigned, which depends on the relative position of the interacting objects:
where represents the bilateral interaction laws (attachments, sliding joints, etc.) whereas represents unilateral interaction laws (contact, needle puncture, friction, etc.). These functions can be nonlinear. In the constrained system presented above, the constraint matrix appeared. The definition of the constraint laws and allows to define:
Note that the matrix containing the constraint directions can be considered as the Jacobian of the mapping between the physics space and the constraint space. The constraint will always be linearized in SOFA. For two interacting objects (object 1 and object 2), the complete constrained system therefore corresponds to:
However, this system will not be solved diretly. It will be decomposed into two steps:
Step 1: interacting objects are solved independently while setting . We obtain what we call the free motion and for each object.
Step 2: now, the constraints are taken into account while considering . The constrained system can therefore be presented as:
where is the matrix of our linearized constraint system, this matrix is homogeneous to a compliance. is the constraint violation (here in velocity), that can be directly obtained from the expression of our constraint laws and .
Finally, the resolution of the constraint problem is done using the GaussSeidel algorithm. After resolution of this new linear system, the motion can be corrected as follows:
 with and
FreeAnimationLoop
To solve such complex constraintbased interactions, the simulation requires a specific animation loop: the FreeAnimationLoop. This animation loop divides each simulation step into two successive resolution steps: the free motion and a corrective motion.
Free motion
The first step triggered by the FreeAnimationLoop is the free motion step. It consists in the resolution of the unconstrained (free) system as described in the System Resolution section. Note that this free resolution may also include projective constraints that will be projected on the linear system. In the same way, collision might also be detected and a response would be created.
In the solve() function of the FreeAnimationLoop, you will find the following functions responsible for the free motion:
///Solve visitor is triggered to solve the free motion
simulation::SolveVisitor freeMotion(params, dt, true);
///Apply the projective constraint if any
mop.projectResponse(freeVel);
mop.propagateDx(freeVel, true);
///Detect and respond to collision
computeCollision(params);
The result of the resolution of the linear system is noted : .
Constraintbased correction
Once the free motion has been computed, the animation loop will look for an existing ConstraintSolver in the scene graph. If one is found, it will handle the entire constraint process: computation of the constraint system, resolution and application of the corrective motion ensuring valid constraints.
In the solve() function of the FreeAnimationLoop, the constraint resolution simply appears as:
///if a ConstraintSolver is in the simulation, trigger the constraint pipeline
if (constraintSolver)
{
constraintSolver>solveConstraint(&cparams, pos, vel);
}
ConstraintSolver
A ConstraintSolver is called by the AnimationLoop within the step() function. The solveConstraint() function of the ConstraintSolver organizes and rules all the steps of the resolution of the constraint problem. It builds the constraint system, solves it and applies a correction to find a corrected solution based on the free motion . In the code of any ConstraintSolver, you find the following functions:
bool prepareStates(const core::ConstraintParams * , MultiVecId res1, MultiVecId res2=MultiVecId::null());
bool buildSystem(const core::ConstraintParams * , MultiVecId res1, MultiVecId res2=MultiVecId::null());
bool solveSystem(const core::ConstraintParams * , MultiVecId res1, MultiVecId res2=MultiVecId::null());
bool applyCorrection(const core::ConstraintParams * , MultiVecId res1, MultiVecId res2=MultiVecId::null());
Each of these functions corresponds to a step described below:

Prepare states: allocates in memory vectors corresponding to the corrective motion and the Lagrange multipliers

Build system: ensures itself the construction of the constraint matrix system

Solve system: the constraint resolution finds a solution for the constraint problem

Apply the correction: recovers the result and applies this corrective motion to the free motion
The step of building the system (see the Build system, Constraint laws and ConstraintCorrection sections) and solving it will now be detailed.
Build system
This is the denser part of the constraint resolution. Most steps done to build the constraint problem are triggered using visitors browsing the simulation graph. All the following functions are actually not implemented by ConstraintSolver but by the constraint laws available in the scene and (see the Constraint law section).
The following steps are processed one after another:

reset the constraint matrix . The associated visitor is MechanicalResetConstraintVisitor

build a new constraint matrix (or Jacobian matrix) depending on the constraint laws available in the scene and . The associated visitor is MechanicalBuildConstraintMatrix

acculutate additional contributions to the constraint matrix (or Jacobian matrix) coming from underlying mappings. The associated visitor is MechanicalAccumulateMatrixDeriv

take into account the projective constraint . This step removes the constraints that are affecting the degrees of freedom currently concerned by a project constraint. The associated visitor is MechanicalProjectJacobianMatrixVisitor

clear previous values of the Lagrange multipliers

project the free motion (computed at Step 1) into the constraint space where is the violation in velocity. See the visitor MechanicalGetConstraintViolationVisitor

select which method will be used to solve the constraint problem. The associated visitor is MechanicalGetConstraintResolutionVisitor

finally build the “compliance” matrix based on the previously computed matrices. This task is performed by the ConstraintCorrection. The detail of the assembly of is given below in the ConstraintCorrection section. The associated function of the ConstraintCorrection is addComplianceInConstraintSpace()

store which corresponds to the projection of the Lagrange multipliers into the physics space, and is homogeneous to forces. This vector is made available with the function storeLambda(). This will be finally used to compute the corrective motion, resulting from the constraint resolution
In the code, the buildSystem() function performs each of the steps just described and looks as follows:
simulation::MechanicalResetConstraintVisitor(cParams).execute(context);
simulation::MechanicalBuildConstraintMatrix(cParams, cParams>j(), numConstraints).execute(context);
simulation::MechanicalAccumulateMatrixDeriv(cParams, cParams>j(), reverseAccumulateOrder.getValue()).execute(context);
simulation::MechanicalProjectJacobianMatrixVisitor(&mparams).execute(context);
current_cp>clear(numConstraints);
MechanicalGetConstraintViolationVisitor(cParams, ¤t_cp>dFree).execute(context);
MechanicalGetConstraintResolutionVisitor(cParams, current_cp>constraintsResolutions).execute(context);
cc>addComplianceInConstraintSpace(cParams, ¤t_cp>W);
Solve system
The resolution of the system will be processed when the solveSystem() function of the ConstraintSolver is called (see). In SOFA, the current resolution always relies on a GaussSeidel algorithm. Depending on the type of ConstraintSolver used, two implementations are available:

using a LCPConstraintSolver, the GaussSeidel implementation running is implemented in sofa::helper::GaussSeidel

using a GenericConstraintSolver, the GaussSeidel algorithm triggered is the one implemented internally in GenericConstraintSolver::GaussSeidel
The output of the constraint resolution is the corrected motion for each object involved.
ConstraintSolver in SOFA
Two different ConstraintSolver implementations exist in SOFA:

LCPConstraintSolver: this solvers targets on collision constraints, contacts with frictions which corresponds to unilateral constraints

GenericConstraintSolver: this solver handles all kind of constraints, i.e. works with any constraint resolution algorithm
Moreover, you may find the class ConstraintSolver. This class does not implement a real solver but actually just browses the graph in order to find and use one of the two implementations mentioned above.
ConstraintCorrection
As explained above, a ConstraintCorrection is required in the simulation to define the way the compliance matrix is computed. Different classes of exist in SOFA corresponding to different approaches:

UncoupledConstraintCorrection: makes the approximation that the compliance matrix is diagonal. This is as strong assumption since a diagonal matrix means that all constraints are independent from each other. Note that you can directly specify the compliance matrix values within the Data field “compliance”

LinearSolverConstraintCorrection: computes the compliance matrix where comes from a direct solver associated to the object. Since the direct solvers in SOFA factorize the matrix (for instance using a LDL factorization if you use the LDLSolver). The matrixmatrix multiplication is not possible since the assembled inverse matrix is not available. From the factorization of , the computation of done in the function addJMInvJt() requires to call the solve() function from the direct solver, computing a matrixvector multiplication, for each line of the constraint matrix , i.e. for each constraint. This approach can therefore be very computationallydemanding if you have many constraints. Note that this ConstraintCorrection proposes an optimization for wirelike structures (boolean option)

PrecomputedConstraintCorrection: instead of computing at each time step, this constraint correction precomputes once the inverse of at the initialization of the simulation and stores this matrix into a file. This speeds up the simulation but it can lead to a lack of accuracy in case the system matrix changes during the simulation

GenericConstraintCorrection: similar to the LinearSolverConstraintCorrection, it allows to declare only once all the direct solvers (one for each constraint object) used to compute the global , whereas the previously described contraint correction needs to be added for each object
Constraint laws
In SOFA, you can find several of interaction constraint laws available to include in your simulation. A lot of them is available in the SofaConstraint module, among them:

UnilateralInteractionConstraint: constraint of inequality (like the function described above in the Constraint problem section), that fits for instance contact and collision cases

BilateralInteractionConstraint: constraint of equality (like the function described above in the Constraint problem section), that fits for instance interactions, attachements between two paired objects

SlidingConstraint: constraint in equality, like the BilateralInteractionConstraint, but only active for some vectors of the physics space (for instance only the xdirection)
Classes defining constraints between a pair of objects inherit from the class PairInteractionConstraint. The associated API functions are:
/// Retrieve the associated MechanicalState of both paired objects
MechanicalState<DataTypes>* getMState1();
BaseMechanicalState* getMechModel1();
MechanicalState<DataTypes>* getMState2();
BaseMechanicalState* getMechModel2();
/// Construct the Constraint violations vector of each constraint
virtual void getConstraintViolation(const ConstraintParams* cParams, defaulttype::BaseVector *v);
/// Construct the Jacobian Matrix or constraint matrix H
virtual void buildConstraintMatrix(const ConstraintParams* cParams, MultiMatrixDerivId cId, unsigned int &cIndex);
More about Lagrange multipliers and constraints
To read more and go further regarding constraints relying on Lagrange multipliers, please read:

Pr. Duriez’s habilitation thesis

Pr. Baraff’s courses
You can also look at examples in the scenes of SOFA like:

examples/Components/animationloop/FreeMotionAnimationLoop.scn

examples/Components/constraint/SlidingConstraint.scn
Last modified: 5 May 2020