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:
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 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:
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: Each interacting object is solved independently, i.e. as no constraint law is defined, while setting . This so-called free motion aims at finding the change in velocity and for each object from the resolution of:
Step 2: now, the constraints are taken into account while considering . We are looking for a corrective change in velocity and for each object from the resolution of:
Defining 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:
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 Gauss-Seidel algorithm. After resolution of this new linear system, the motion can be corrected as follows:
- with and
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.
The first step triggered by the FreeMotionAnimationLoop 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 FreeMotionAnimationLoop, you will find the following functions responsible for the free motion:
The result of the resolution of the linear system is noted : .
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 FreeMotionAnimationLoop, the constraint resolution simply appears as:
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
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);
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 Gauss-Seidel algorithm. Depending on the type of ConstraintSolver used, two implementations are available:
using a LCPConstraintSolver, the Gauss-Seidel implementation running is implemented in sofa::helper::GaussSeidel
using a GenericConstraintSolver, the Gauss-Seidel 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.
As explained above, a ConstraintCorrection is required in the simulation to define the way the compliance matrix is computed. Different classes of ConstraintCorrection 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 matrix-matrix 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 matrix-vector multiplication, for each line of the 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 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
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 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:
You can also look at examples in the scenes of SOFA like:
Last modified: 12 May 2021