[Lagrangian] Add regularization term in lagrangian constraints

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/GenericConstraintCorrection.cpp

@@ -52,6 +52,7 @@ GenericConstraintCorrection::GenericConstraintCorrection()
 : l_linearSolver(initLink("linearSolver", "Link towards the linear solver used to compute the compliance matrix, requiring the inverse of the linear system matrix"))
 , l_ODESolver(initLink("ODESolver", "Link towards the ODE solver used to recover the integration factors"))
 , d_complianceFactor(initData(&d_complianceFactor, 1.0_sreal, "complianceFactor", "Factor applied to the position factor and velocity factor used to calculate compliance matrix"))
+, d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints"))
 {
 }
 
@@ -166,7 +167,8 @@ void GenericConstraintCorrection::addComplianceInConstraintSpace(const Constrain
 
     factor *= complianceFactor;
     // use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
-    l_linearSolver.get()->buildComplianceMatrix(cparams, W, factor);
+    l_linearSolver.get()->buildComplianceMatrix(cparams, W, factor, d_regularizationTerm.getValue());
+
 }
 
 void GenericConstraintCorrection::computeMotionCorrectionFromLambda(const ConstraintParams* cparams, MultiVecDerivId dx, const linearalgebra::BaseVector * lambda)

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/GenericConstraintCorrection.h

@@ -62,6 +62,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API GenericConstraintCorre
     SingleLink<GenericConstraintCorrection, sofa::core::behavior::LinearSolver, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_linearSolver; ///< Link towards the linear solver used to compute the compliance matrix, requiring the inverse of the linear system matrix
     SingleLink<GenericConstraintCorrection, sofa::core::behavior::OdeSolver, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_ODESolver; ///< Link towards the ODE solver used to recover the integration factors
     Data< SReal > d_complianceFactor; ///< Factor applied to the position factor and velocity factor used to calculate compliance matrix
+    Data< SReal > d_regularizationTerm; ///< add regularizationTerm*Id to W when solving for constraints
 
 protected:
     GenericConstraintCorrection();

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/LinearSolverConstraintCorrection.h

@@ -66,6 +66,7 @@ class LinearSolverConstraintCorrection : public sofa::core::behavior::Constraint
 public:
     void init() override;
 
+    void addRegularization(linearalgebra::BaseMatrix* W);
 
     void addComplianceInConstraintSpace(const sofa::core::ConstraintParams *cparams, linearalgebra::BaseMatrix* W) override;
 
@@ -81,6 +82,7 @@ class LinearSolverConstraintCorrection : public sofa::core::behavior::Constraint
 
     void rebuildSystem(SReal massFactor, SReal forceFactor) override;
 
+
     /// @name Deprecated API
     /// @{
 
@@ -94,6 +96,7 @@ class LinearSolverConstraintCorrection : public sofa::core::behavior::Constraint
     /// @{
 
     Data< bool > wire_optimization; ///< constraints are reordered along a wire-like topology (from tip to base)
+    Data< SReal > d_regularizationTerm; ///< add regularization*Id to W when solving for constraints
     SingleLink<LinearSolverConstraintCorrection, sofa::core::behavior::LinearSolver, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_linearSolver; ///< Link towards the linear solver used to compute the compliance matrix, requiring the inverse of the linear system matrix
     SingleLink<LinearSolverConstraintCorrection, sofa::core::behavior::OdeSolver, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_ODESolver; ///< Link towards the ODE solver used to recover the integration factors
 

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/LinearSolverConstraintCorrection.inl

@@ -47,6 +47,7 @@ template<class DataTypes>
 LinearSolverConstraintCorrection<DataTypes>::LinearSolverConstraintCorrection(sofa::core::behavior::MechanicalState<DataTypes> *mm)
 : Inherit(mm)
 , wire_optimization(initData(&wire_optimization, false, "wire_optimization", "constraints are reordered along a wire-like topology (from tip to base)"))
+, d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints"))
 , l_linearSolver(initLink("linearSolver", "Link towards the linear solver used to compute the compliance matrix, requiring the inverse of the linear system matrix"))
 , l_ODESolver(initLink("ODESolver", "Link towards the ODE solver used to recover the integration factors"))
 {
@@ -168,6 +169,23 @@ void LinearSolverConstraintCorrection<TDataTypes>::convertConstraintMatrix(const
     }
 }
 
+template<class DataTypes>
+void LinearSolverConstraintCorrection<DataTypes>::addRegularization(linearalgebra::BaseMatrix* W)
+{
+    SReal regularization = d_regularizationTerm.getValue();
+    if (regularization > std::numeric_limits<SReal>::epsilon())
+    {
+        for (auto rowIt = m_constraintJacobian.begin(); rowIt != m_constraintJacobian.end(); ++rowIt)
+        {
+            if (rowIt->second.size() != 0)
+            {
+                W->add(rowIt->first,rowIt->first,regularization);
+            }
+        }
+    }
+
+}
+
 template<class DataTypes>
 void LinearSolverConstraintCorrection<DataTypes>::addComplianceInConstraintSpace(const sofa::core::ConstraintParams *cparams, sofa::linearalgebra::BaseMatrix* W)
 {
@@ -202,6 +220,8 @@ void LinearSolverConstraintCorrection<DataTypes>::addComplianceInConstraintSpace
     // use the Linear solver to compute J*inv(M)*Jt, where M is the mechanical linear system matrix
     l_linearSolver->setSystemLHVector(sofa::core::MultiVecDerivId::null());
     l_linearSolver->addJMInvJt(W, &m_constraintJacobian, factor);
+
+    addRegularization(W);
 }
 
 
@@ -768,6 +788,9 @@ void LinearSolverConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(lin
     {
         Vec_I_list_dof[i] = list_dof;
     }
+
+    addRegularization(W);
+
 }
 
 } //namespace sofa::component::constraint::lagrangian::correction

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/PrecomputedConstraintCorrection.h

@@ -82,6 +82,7 @@ class PrecomputedConstraintCorrection : public sofa::core::behavior::ConstraintC
     Data<bool> d_restRotations;
 
     Data<bool> d_recompute; ///< if true, always recompute the compliance
+    Data<SReal> d_regularizationTerm; ///< add regularization*Id to W when solving for constraints
     Data<SReal> d_debugViewFrameScale; ///< Scale on computed node's frame
     sofa::core::objectmodel::DataFileName d_fileCompliance; ///< Precomputed compliance matrix data file
     Data<std::string> d_fileDir; ///< If not empty, the compliance will be saved in this repertory

Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/PrecomputedConstraintCorrection.inl

@@ -56,6 +56,7 @@ PrecomputedConstraintCorrection<DataTypes>::PrecomputedConstraintCorrection(sofa
     , d_rotations(initData(&d_rotations, false, "rotations", ""))
     , d_restRotations(initData(&d_restRotations, false, "restDeformations", ""))
     , d_recompute(initData(&d_recompute, false, "recompute", "if true, always recompute the compliance"))
+    , d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints"))
     , d_debugViewFrameScale(initData(&d_debugViewFrameScale, 1.0_sreal, "debugViewFrameScale", "Scale on computed node's frame"))
     , d_fileCompliance(initData(&d_fileCompliance, "fileCompliance", "Precomputed compliance matrix data file"))
     , d_fileDir(initData(&d_fileDir, "fileDir", "If not empty, the compliance will be saved in this repertory"))
@@ -524,6 +525,7 @@ void PrecomputedConstraintCorrection< DataTypes >::addComplianceInConstraintSpac
     }
 
     unsigned int curConstraint = 0;
+    const SReal regularization = d_regularizationTerm.getValue();
 
     for (MatrixDerivRowConstIterator rowIt = c.begin(); rowIt != rowItEnd; ++rowIt)
     {
@@ -548,12 +550,16 @@ void PrecomputedConstraintCorrection< DataTypes >::addComplianceInConstraintSpac
 
                 if (indexCurRowConst != indexCurColConst)
                     W->add(indexCurColConst, indexCurRowConst, w);
+                else
+                    W->add(indexCurColConst, indexCurRowConst,regularization);
+
 
                 curColConst++;
             }
         }
         curConstraint++;
     }
+
 }
 
 template<class DataTypes>
@@ -1371,6 +1377,7 @@ void PrecomputedConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(line
 
 #else
 
+    const SReal regularizationTerm = d_regularizationTerm.getValue();
     for (int id1 = begin; id1<=end; id1++)
     {
         int c1 = id_to_localIndex[id1];
@@ -1382,6 +1389,8 @@ void PrecomputedConstraintCorrection<DataTypes>::getBlockDiagonalCompliance(line
             W->add(id1, id2, w);
             if (id1 != id2)
                 W->add(id2, id1, w);
+            else
+                W->add(id2, id1, regularizationTerm);
         }
     }
 

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.cpp

@@ -67,6 +67,7 @@ GenericConstraintSolver::GenericConstraintSolver()
     , d_maxIt(initData(&d_maxIt, 1000, "maxIterations", "maximal number of iterations of the Gauss-Seidel algorithm"))
     , d_tolerance(initData(&d_tolerance, 0.001_sreal, "tolerance", "residual error threshold for termination of the Gauss-Seidel algorithm"))
     , d_sor(initData(&d_sor, 1.0_sreal, "sor", "Successive Over Relaxation parameter (0-2)"))
+    , d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints"))
     , d_scaleTolerance(initData(&d_scaleTolerance, true, "scaleTolerance", "Scale the error tolerance with the number of constraints"))
     , d_allVerified(initData(&d_allVerified, false, "allVerified", "All constraints must be verified (each constraint's error < tolerance)"))
     , d_newtonIterations(initData(&d_newtonIterations, 100, "newtonIterations", "Maximum iteration number of Newton (for the NonsmoothNonlinearConjugateGradient solver only)"))
@@ -244,6 +245,18 @@ bool GenericConstraintSolver::buildSystem(const core::ConstraintParams *cParams,
     return true;
 }
 
+void GenericConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W)
+{
+    const SReal regularization =  d_regularizationTerm.getValue();
+    if (regularization>std::numeric_limits<SReal>::epsilon())
+    {
+        for (int i=0; i<W.rowSize(); ++i)
+        {
+            W.add(i,i,regularization);
+        }
+    }
+}
+
 void GenericConstraintSolver::buildSystem_matrixFree(unsigned int numConstraints)
 {
     for (const auto& cc : l_constraintCorrections)
@@ -300,6 +313,10 @@ void GenericConstraintSolver::buildSystem_matrixFree(unsigned int numConstraints
     current_cp->change_sequence = false;
     if(current_cp->constraints_sequence.size() == nbObjects)
         current_cp->change_sequence=true;
+
+    addRegularization(current_cp->W);
+    addRegularization(current_cp->Wdiag);
+
 }
 
 GenericConstraintSolver::ComplianceWrapper::ComplianceMatrixType& GenericConstraintSolver::
@@ -368,6 +385,7 @@ void GenericConstraintSolver::buildSystem_matrixAssembly(const core::ConstraintP
             compliance.assembleMatrix();
         });
 
+    addRegularization(current_cp->W);
     dmsg_info() << " computeCompliance_done "  ;
 }
 

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h

@@ -56,6 +56,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver :
     bool prepareStates(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override;
     bool buildSystem(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override;
     void rebuildSystem(SReal massFactor, SReal forceFactor) override;
+    void addRegularization(linearalgebra::BaseMatrix & W);
     bool solveSystem(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override;
     bool applyCorrection(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2=MultiVecId::null()) override;
     void computeResidual(const core::ExecParams* /*params*/) override;
@@ -118,6 +119,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver :
     Data<int> d_maxIt; ///< maximal number of iterations of the Gauss-Seidel algorithm
     Data<SReal> d_tolerance; ///< residual error threshold for termination of the Gauss-Seidel algorithm
     Data<SReal> d_sor; ///< Successive Over Relaxation parameter (0-2)
+    Data< SReal > d_regularizationTerm; ///< add regularization*Id to W when solving for constraints
     Data<bool> d_scaleTolerance; ///< Scale the error tolerance with the number of constraints
     Data<bool> d_allVerified; ///< All constraints must be verified (each constraint's error < tolerance)
     Data<int> d_newtonIterations; ///< Maximum iteration number of Newton (for the NonsmoothNonlinearConjugateGradient solver only)

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/LCPConstraintSolver.cpp

@@ -48,6 +48,7 @@ LCPConstraintSolver::LCPConstraintSolver()
     , d_build_lcp(initData(&d_build_lcp, true, "build_lcp", "LCP is not fully built to increase performance in some case."))
     , d_tol(initData(&d_tol, 0.001_sreal, "tolerance", "residual error threshold for termination of the Gauss-Seidel algorithm"))
     , d_maxIt(initData(&d_maxIt, 1000, "maxIt", "maximal number of iterations of the Gauss-Seidel algorithm"))
+    , d_regularizationTerm(initData(&d_regularizationTerm, 0.0_sreal, "regularizationTerm", "Add regularization factor times the identity matrix to the compliance W when solving constraints"))
     , d_mu(initData(&d_mu, 0.6_sreal, "mu", "Friction coefficient"))
     , d_minW(initData(&d_minW, 0.0_sreal, "minW", "If not zero, constraints whose self-compliance (i.e. the corresponding value on the diagonal of W) is smaller than this threshold will be ignored"))
     , d_maxF(initData(&d_maxF, 0.0_sreal, "maxF", "If not zero, constraints whose response force becomes larger than this threshold will be ignored"))
@@ -128,6 +129,19 @@ bool LCPConstraintSolver::prepareStates(const core::ConstraintParams * /*cParams
     return true;
 }
 
+
+void LCPConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W)
+{
+    const SReal regularization =  d_regularizationTerm.getValue();
+    if (regularization>std::numeric_limits<SReal>::epsilon())
+    {
+        for (int i=0; i<W.rowSize(); ++i)
+        {
+            W.add(i,i,regularization);
+        }
+    }
+}
+
 bool LCPConstraintSolver::buildSystem(const core::ConstraintParams * /*cParams*/, MultiVecId res1, MultiVecId res2)
 {
     SOFA_UNUSED(res1);
@@ -346,6 +360,8 @@ void LCPConstraintSolver::addComplianceInConstraintSpace(core::ConstraintParams
         cc->addComplianceInConstraintSpace(&cparams, _W);
     }
 
+
+    addRegularization(* _W);
     dmsg_info() << "W=" << *_W ;
 }
 
@@ -866,6 +882,7 @@ int LCPConstraintSolver::nlcp_gaussseidel_unbuilt(SReal *dfree, SReal *f, std::v
         dmsg_info() <<" "<<msgendl;
     }
 
+    addRegularization(_Wdiag);
 
 
     // allocation of the inverted system 3x3
@@ -885,6 +902,7 @@ int LCPConstraintSolver::nlcp_gaussseidel_unbuilt(SReal *dfree, SReal *f, std::v
         W33[c1].compute(w[0], w[1] , w[2], w[3], w[4] , w[5]);
     }
 
+
     dmsg_info() <<" Compliance In constraint Space : \n W ="<<(* _W)<<msgendl
                 <<"getBlockDiagonalCompliance   \n Wdiag = "<< _Wdiag ;
 
@@ -1111,6 +1129,8 @@ int LCPConstraintSolver::lcp_gaussseidel_unbuilt(SReal *dfree, SReal *f, std::ve
         }
     }
 
+    addRegularization(_Wdiag);
+
     unbuilt_W11.resize(numContacts);
     SReal *W11 = &(unbuilt_W11[0]);
     for (c1=0; c1<numContacts; c1++)

Sofa/Component/Constraint/Lagrangian/Solver/src/sofa/component/constraint/lagrangian/solver/LCPConstraintSolver.h

@@ -137,6 +137,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API LCPConstraintSolver : publ
     Data<bool> d_build_lcp; ///< LCP is not fully built to increase performance in some case.
     Data<SReal> d_tol; ///< residual error threshold for termination of the Gauss-Seidel algorithm
     Data<int> d_maxIt; ///< maximal number of iterations of the Gauss-Seidel algorithm
+    Data<SReal> d_regularizationTerm; ///< add regularization*Id to W when solving for constraints
     Data<SReal> d_mu; ///< Friction coefficient
     Data<SReal> d_minW; ///< If not zero, constraints whose self-compliance (i.e. the corresponding value on the diagonal of W) is smaller than this threshold will be ignored
     Data<SReal> d_maxF; ///< If not zero, constraints whose response force becomes larger than this threshold will be ignored
@@ -161,6 +162,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API LCPConstraintSolver : publ
     void lockConstraintProblem(sofa::core::objectmodel::BaseObject* from, ConstraintProblem* p1, ConstraintProblem* p2=nullptr) override; ///< Do not use the following LCPs until the next call to this function. This is used to prevent concurrent access to the LCP when using a LCPForceFeedback through an haptic thread
 
 private:
+    void addRegularization(linearalgebra::BaseMatrix& W);
     void computeInitialGuess();
     void keepContactForcesValue();
 

Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/MatrixLinearSolver.h

@@ -298,7 +298,7 @@ class MatrixLinearSolver<Matrix,Vector,NoThreadManager> : public BaseMatrixLinea
 
     bool addMInvJt(linearalgebra::BaseMatrix* result, linearalgebra::BaseMatrix* J, SReal fact) override;
 
-    bool buildComplianceMatrix(const core::ConstraintParams* cparams, linearalgebra::BaseMatrix* result, SReal fact) override;
+    bool buildComplianceMatrix(const core::ConstraintParams* cparams, linearalgebra::BaseMatrix* result, SReal fact, SReal regularizationTerm) override;
 
     void applyConstraintForce(const sofa::core::ConstraintParams* cparams, sofa::core::MultiVecDerivId dx, const linearalgebra::BaseVector* f) override;
 
@@ -473,7 +473,7 @@ extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API bool MatrixLinearSolve
 extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API bool MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::addMInvJt(linearalgebra::BaseMatrix*, linearalgebra::BaseMatrix*, SReal);
 extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API bool MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::addJMInvJtLocal(GraphScatteredMatrix*, ResMatrixType*, const JMatrixType*, SReal);
 extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API bool MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::addMInvJtLocal(GraphScatteredMatrix*, ResMatrixType*, const  JMatrixType*, SReal);
-extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API bool MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::buildComplianceMatrix(const core::ConstraintParams*, linearalgebra::BaseMatrix*, SReal);
+extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API bool MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::buildComplianceMatrix(const core::ConstraintParams*, linearalgebra::BaseMatrix*, SReal, SReal);
 extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API MatrixInvertData* MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::getMatrixInvertData(linearalgebra::BaseMatrix * m);
 extern template SOFA_COMPONENT_LINEARSOLVER_ITERATIVE_API MatrixInvertData* MatrixLinearSolver<GraphScatteredMatrix,GraphScatteredVector,NoThreadManager>::createInvertData();
 

Sofa/Component/LinearSolver/Iterative/src/sofa/component/linearsolver/iterative/MatrixLinearSolver.inl

@@ -577,7 +577,7 @@ bool MatrixLinearSolver<Matrix,Vector>::addMInvJt(linearalgebra::BaseMatrix* res
 }
 
 template<class Matrix, class Vector>
-bool MatrixLinearSolver<Matrix,Vector>::buildComplianceMatrix(const sofa::core::ConstraintParams* cparams, linearalgebra::BaseMatrix* result, SReal fact)
+bool MatrixLinearSolver<Matrix,Vector>::buildComplianceMatrix(const sofa::core::ConstraintParams* cparams, linearalgebra::BaseMatrix* result, SReal fact, SReal regularizationTerm)
 {
     JMatrixType * j_local = internalData.getLocalJ();
     j_local->clear();
@@ -590,7 +590,20 @@ bool MatrixLinearSolver<Matrix,Vector>::buildComplianceMatrix(const sofa::core::
 
     executeVisitor(MechanicalGetConstraintJacobianVisitor(cparams, j_local));
 
-    return addJMInvJt(result, j_local, fact);
+    bool boolRes = addJMInvJt(result, j_local, fact);
+
+    if (boolRes && regularizationTerm > std::numeric_limits<SReal>::epsilon())
+    {
+        for (auto rowIt = j_local->begin(); rowIt != j_local->end(); ++rowIt)
+        {
+            if (rowIt->second.size() != 0)
+            {
+                result->add(rowIt->first,rowIt->first,regularizationTerm);
+            }
+        }
+    }
+
+    return boolRes;
 }
 
 template<class Matrix, class Vector>