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.

General presentation of the 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:

$Constraint problem$

that can be written in a simpler way as:

$Shortened constraint problem$

where $Constraint forces$ is the vector of constraint forces contribution with $Constraint matrix$ matrix containing the constraint directions and $Lagrange multipliers$ are the so-called 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:

$Constraint law1$

$Constraint law2$

where $Phi$ represents the bilateral interaction laws (attachments, sliding joints, etc.) whereas $Psi$ represents unilateral interaction laws (contact, needle puncture, friction, etc.). These functions can be nonlinear. In the constrained system presented above, the constraint matrix $Constraint matrix$ appeared. The definition of the constraint laws $Phi$ and $Psi$ allows to define:

$H1$ $H2$

Note that $Constraint matrix$ 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:

$Shortened constraint problem1$
$Shortened constraint problem2$

However, this system will not be solved directly. It will be decomposed into two steps:

Step 1: Each interacting object is solved independently, i.e. as no constraint law is defined, while setting $Lagrange multipliers$ . This so-called free motion aims at finding the change in velocity $Free motion 1$ and $Free motion 2$ for each object from the resolution of:

$Free motion of object1$
$Free motion of object2$

Step 2: now, the constraints are taken into account while considering $No force condition$ . We are looking for a corrective change in velocity $Corrective motion 1$ and $Corrective motion 2$ for each object from the resolution of:

$Free motion of object1$
$Free motion of object2$

Defining $Lagrange multipliers$ the Lagrange multipliers, as the forces to be applied in the constraint space to satisfy all constraint laws, the constrained system can therefore be presented as:

$Constraint problem$

where $Compliance matrix$ is the matrix of our linearized constraint system, this matrix $Compliance matrix$ is homogeneous to a compliance. $Constraint violation$ is the constraint violation (here in velocity), that can be directly obtained from the expression of our constraint laws $Phi$ and $Psi$ .

Finally, the resolution of the constraint problem is done using the Gauss-Seidel algorithm. After resolution of this new linear system, the motion can be corrected as follows:

$Correction1$
$Correction2$ with $Corrective displacement1$ and $Corrective displacement2$

FreeMotionAnimationLoop

To solve such complex constraint-based interactions, the simulation requires a specific animation loop: the FreeMotionAnimationLoop. 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 FreeMotionAnimationLoop is the free motion step. It consists in the resolution of the unconstrained (free) system $Linear 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 FreeMotionAnimationLoop, 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 $Linear system$ is noted : $Free motion solution$ .

Constraint-based correction

Once the free motion $Free motion solution$ 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 FreeMotionAnimationLoop, 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 $Free motion solution$ . 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 $Corrective displacement$ and the Lagrange multipliers $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 $Corrective displacement$ and applies this corrective motion to the free motion $Correction$

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 $Phi$ and $Psi$ (see the Constraint law section).

The following steps are processed one after another:

reset the constraint matrix $Constraint matrix$ . The associated visitor is MechanicalResetConstraintVisitor
build a new constraint matrix (or Jacobian matrix) $Constraint matrix$ depending on the constraint laws available in the scene $Phi$ and $Psi$ . The associated visitor is MechanicalBuildConstraintMatrix
accumulate additional contributions to the constraint matrix (or Jacobian matrix) $Constraint matrix$ coming from underlying mappings. The associated visitor is MechanicalAccumulateMatrixDeriv
take into account the projective constraint $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 $Free motion$ (computed at Step 1) into the constraint space $Projected free motion$ where $Violation$ 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 $Compliance matrix$ based on the previously computed matrices. This task is performed by the ConstraintCorrection. The detail of the assembly of $Compliance matrix$ is given below in the ConstraintCorrection section. The associated function of the ConstraintCorrection is addComplianceInConstraintSpace()
store $Projected lambda$ which corresponds to the projection of the Lagrange multipliers $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, &current_cp->dFree).execute(context);
MechanicalGetConstraintResolutionVisitor(cParams, current_cp->constraintsResolutions).execute(context);

cc->addComplianceInConstraintSpace(cParams, &current_cp->W);

Solve system

The resolution of the system will be processed when the solveSystem() function of the ConstraintSolver is called. In SOFA, two different ConstraintSolver implementations exist in SOFA:

LCPConstraintSolver: this solvers targets on collision constraints, contacts with frictions which corresponds to unilateral constraints. This solver proposes one implementation of a Gauss-Seidel algorithm available in in sofa::helper::GaussSeidel
GenericConstraintSolver: this solver handles all kind of constraints, i.e. works with any constraint resolution algorithm. This solver proposes three different resolution methods:
- a Projective Gauss-Seidel algorithm (PGS) algorithm implemented in GenericConstraintSolver::GaussSeidel. To use this PGS algorithm, you must select the data resolutionMethod as ProjectedGaussSeidel
- an Unbuilt (matrix-free) approach of the Gauss-Seidel algorithm (UGS) algorithm is also available, thus avoiding the assembly of the constraint system. To use this UGS algorithm, you must select the data resolutionMethod as UnbuiltGaussSeidel
- a Non-smooth Non-linear Conjugate Gradient (NNCG) algorithm implemented in GenericConstraintSolver::NNCG(). To use this NNCG algorithm, you must select the data resolutionMethod as NonsmoothNonlinearConjugateGradient. Using the data newtonIterations, you can define the maximum number of iterations for the classical Newton method, solving the generic roots search problem.

NB: 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.

The output of the constraint resolution is the corrected motion $Corrective displacement$ for each object involved.

ConstraintCorrection

As explained above, a ConstraintCorrection is required in the simulation to define the way the compliance matrix $Compliance matrix$ is computed. Different classes of ConstraintCorrection exist in SOFA corresponding to different approaches:

UncoupledConstraintCorrection: makes the approximation that the compliance matrix $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 $Compliance matrix$ where $Inverse of A$ comes from a direct solver associated to the object. Since the direct solvers in SOFA factorize the matrix $System matrix$ (for instance using a LDL factorization if you use the LDLSolver), the factorization is reused to compute the compliance matrix. The matrix-matrix multiplication $Compliance matrix$ is not possible in case of a matrix-free solver, since the assembled inverse matrix $Inverse of A$ is not available. From the factorization of $System matrix$ , the computation of $Compliance matrix$ done in the function addJMInvJt() requires to call the solve() function from the direct solver, computing a matrix-vector multiplication, for each line of the constraint matrix $Constraint matrix$ , i.e. for each constraint. This approach can therefore be very computationally-demanding if you have many constraints. Note that this ConstraintCorrection proposes an optimization for wire-like structures (boolean option)
PrecomputedConstraintCorrection: instead of computing $Inverse of A$ at each time step, this constraint correction precomputes once the inverse of $System matrix$ 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 $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 $Compliance matrix$ , whereas the previously described constraint 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 $Psi$ function described above in the Constraint problem section), that fits for instance contact and collision cases
BilateralInteractionConstraint: constraint of equality (like the $Phi$ function described above in the Constraint problem section), that fits for instance interactions, attachments 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 x-direction)

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/Component/AnimationLoop/FreeMotionAnimationLoop.scn
examples/Component/Constraint/Lagrangian/SlidingConstraint.scn

Last modified: 1 March 2024

Constraint based on Lagrange Multipliers

General presentation of the constraint problem

FreeMotionAnimationLoop

Free motion

Constraint-based correction

ConstraintSolver

Build system

Solve system

ConstraintCorrection

Constraint laws

More about Lagrange multipliers and constraints

Getting Started

Binaries

Build

Video Tutorials

Using SOFA

Performances

Simulation Principles

System Resolution

Multi-Model Representation

Components

AnimationLoop

Collision

Detection

Algorithm

BroadPhases

NarrowPhases

Intersection

Geometry

CollisionGroupManagers

Constraint

Projective

Lagrangian

Engine

Generate

Select

Transform

SolidMechanics

FEM

Spring

MechanicalLoad

ODESolver

Forward

Backward

LinearSolver

Iterative

Direct

IO

Mesh

Mappings

Mass

Rendering

Visual

UI

Plugins

Usual plugins

Programming with SOFA

API overview

Contributing to SOFA