[BuiltConstraintSolver] Add SVD based regularization strategy

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

@@ -25,12 +25,16 @@
 #include <sofa/helper/ScopedAdvancedTimer.h>
 #include <sofa/simulation/MainTaskSchedulerFactory.h>
 #include <sofa/simulation/ParallelForEach.h>
+#include <Eigen/Eigenvalues>
 
 namespace sofa::component::constraint::lagrangian::solver
 {
 
 BuiltConstraintSolver::BuiltConstraintSolver()
 : d_multithreading(initData(&d_multithreading, false, "multithreading", "Build compliances concurrently"))
+, d_useSVDForRegularization(initData(&d_useSVDForRegularization, false, "useSVDForRegularization", "Use SVD decomposiiton of the compliance matrix to project singular values smaller than regularization to the regularization term. Only works with built"))
+, d_svdSingularValueNullSpaceCriteriaFactor(initData(&d_svdSingularValueNullSpaceCriteriaFactor, 0.01, "svdSingularValueNullSpaceCriteriaFactor", "Fraction of the highest singular value bellow which a singular value will be supposed to belong to the nullspace"))
+, d_svdSingularVectorNullSpaceCriteriaFactor(initData(&d_svdSingularVectorNullSpaceCriteriaFactor, 0.001, "svdSingularVectorNullSpaceCriteriaFactor", "Absolute value bellow which a component of a normalized base vector will be considered null"))
 {}
 
 void BuiltConstraintSolver::init()
@@ -42,6 +46,86 @@ void BuiltConstraintSolver::init()
     }
 }
 
+void BuiltConstraintSolver::addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization)
+{
+
+    if (d_useSVDForRegularization.getValue())
+    {
+        if (regularization>std::numeric_limits<SReal>::epsilon())
+        {
+            auto * FullW =  dynamic_cast<sofa::linearalgebra::LPtrFullMatrix<SReal> * >(&W);
+            if (FullW == nullptr)
+            {
+                msg_error()<<"BuiltConstraintSolver expect a LPtrFullMatrix for W but didn't receive one. The constraint problem is wrong. Please fix the code or just deactivate SVD regularization.";
+                return;
+            }
+            const size_t problemSize = FullW->rowSize();
+
+            Eigen::Map<Eigen::MatrixX<SReal>> EigenW(FullW->ptr(),problemSize, problemSize) ;
+            Eigen::JacobiSVD<Eigen::MatrixXd> svd( EigenW, Eigen::ComputeFullV | Eigen::ComputeFullU );
+
+
+
+            //Given the SVD, loop over all singular values, and those that are smaller than 1% of the highest one are considered to be the null space
+            std::vector<bool> nullSpaceIndicator(problemSize, false);
+            int nullSpaceBegin = -1;
+            for(size_t i=0; i<problemSize; i++)
+            {
+                if (fabs(svd.singularValues()(i)) < fabs(svd.singularValues()(0)) * d_svdSingularValueNullSpaceCriteriaFactor.getValue())
+                {
+                    nullSpaceBegin = i;
+                    break;
+                }
+            }
+
+            //Now for all vector of the null space basis, we look at the indices where the coefficient
+            //is greater than 1% of the norm of the vector, this is the constraints that
+            //belong to the null space and thus have other one that are antagonists
+            for(int i=nullSpaceBegin; (i != -1) && (i<problemSize); ++i)
+            {
+                for(size_t j=0; j<problemSize; j++)
+                    nullSpaceIndicator[j] = nullSpaceIndicator[j] ||  fabs(svd.matrixV().col(i)(j)) > d_svdSingularVectorNullSpaceCriteriaFactor.getValue();
+            }
+
+            if (f_printLog.getValue())
+            {
+                std::stringstream msg ;
+                msg <<"Unregularized diagonal : ";
+                for(size_t i=0; i<problemSize; i++)
+                    msg<<EigenW(i,i) << "  ";
+                msg_info()<< msg.str();
+            }
+
+            //Because the eigen matrix uses the buffer of W to store the matrix, this is sufficient to set the value.
+            //Now using the indicator vector, set the regularization for the constraints that belongs
+            //to the null space to the regularization term
+            for(size_t i=0; i<problemSize; i++)
+                EigenW(i,i) += nullSpaceIndicator[i]*d_regularizationTerm.getValue();
+
+            if (f_printLog.getValue())
+            {
+                std::stringstream msg ;
+                msg <<"Null space : ";
+                for(size_t i=0; i<problemSize; i++)
+                    msg<<nullSpaceIndicator[i] << "  ";
+                msg_info()<< msg.str();
+
+                msg.flush();
+                msg <<"Regularized diagonal : ";
+                for(size_t i=0; i<problemSize; i++)
+                    msg<<EigenW(i,i) << "  ";
+                msg_info()<< msg.str();
+            }
+
+        }
+    }
+    else
+    {
+        Inherit1::addRegularization(W, regularization);
+    }
+}
+
+
 void BuiltConstraintSolver::doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints)
 {
     SOFA_UNUSED(numConstraints);

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

@@ -38,13 +38,19 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : pu
 public:
     SOFA_CLASS(BuiltConstraintSolver, GenericConstraintSolver);
     Data<bool> d_multithreading; ///< Build compliances concurrently
+    Data<bool> d_useSVDForRegularization; ///< Use SVD decomposiiton of the compliance matrix to project singular values smaller than regularization to the regularization term. Only works with built
+    Data<SReal> d_svdSingularValueNullSpaceCriteriaFactor; ///< Fraction of the highest singular value bellow which a singular value will be supposed to belong to the nullspace
+    Data<SReal> d_svdSingularVectorNullSpaceCriteriaFactor; ///< Absolute value bellow which a component of a normalized base vector will be considered null
+
 
     BuiltConstraintSolver();
 
     virtual void init() override;
 
 protected:
     virtual void doBuildSystem( const core::ConstraintParams *cParams, GenericConstraintProblem * problem ,unsigned int numConstraints) override;
+    virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization) override;
+
 
 private:
 

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

@@ -124,8 +124,8 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API GenericConstraintSolver :
      */
     virtual void doSolve( GenericConstraintProblem * problem, SReal timeout = 0.0) = 0;
 
+    virtual void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization);
 
-    static void addRegularization(linearalgebra::BaseMatrix& W, const SReal regularization);
 
 
 private:

examples/Component/Constraint/Lagrangian/BilateralLagrangianConstraint_with_svd_regularization_solvable.scn

@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+<!-- BilateralLagrangianConstraint example -->
+<Node name="root" dt="0.001" gravity="0 0 -9.81">
+    <RequiredPlugin name="Sofa.Component.AnimationLoop"/> <!-- Needed to use components [FreeMotionAnimationLoop] -->
+    <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Correction"/> <!-- Needed to use components [LinearSolverConstraintCorrection] -->
+    <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Model"/> <!-- Needed to use components [BilateralLagrangianConstraint] -->
+    <RequiredPlugin name="Sofa.Component.Constraint.Lagrangian.Solver"/> <!-- Needed to use components [GenericConstraintSolver] -->
+    <RequiredPlugin name="Sofa.Component.Engine.Select"/> <!-- Needed to use components [BoxROI] -->
+    <RequiredPlugin name="Sofa.Component.LinearSolver.Direct"/> <!-- Needed to use components [SparseLDLSolver] -->
+    <RequiredPlugin name="Sofa.Component.Mass"/> <!-- Needed to use components [UniformMass] -->
+    <RequiredPlugin name="Sofa.Component.ODESolver.Backward"/> <!-- Needed to use components [EulerImplicitSolver] -->
+    <RequiredPlugin name="Sofa.Component.SolidMechanics.FEM.Elastic"/> <!-- Needed to use components [HexahedronFEMForceField] -->
+    <RequiredPlugin name="Sofa.Component.StateContainer"/> <!-- Needed to use components [MechanicalObject] -->
+    <RequiredPlugin name="Sofa.Component.Topology.Container.Grid"/> <!-- Needed to use components [RegularGridTopology] -->
+    <RequiredPlugin name="Sofa.Component.Visual"/> <!-- Needed to use components [VisualStyle] -->
+
+    <VisualStyle displayFlags="showForceFields showWireFrame showBehavior" />
+    <DefaultVisualManagerLoop />
+    <FreeMotionAnimationLoop />
+    <ProjectedGaussSeidelConstraintSolver tolerance="0.0001" maxIterations="1000" regularizationTerm="1e-3" useSVDForRegularization="true"/>
+
+
+    <Node name="TargetCubeExtrimities">
+        <RegularGridTopology name="grid" nx="7" ny="2" nz="2" xmin="-1" xmax="1" ymin="-0.16" ymax="0.16" zmin="-0.16" zmax="0.16" />
+        <MechanicalObject name="mstate" template="Vec3"/>
+        <BoxROI name="bottom" box="-1.1 -1.1 -1.1  -0.9 1.1 1.1" />
+        <BoxROI name="top" box="0.9 -1.1 -1.1  1.1 1.1 1.1" />
+    </Node>
+    <Node name="TargetCubeMid">
+        <RegularGridTopology name="grid" nx="7" ny="2" nz="2" xmin="-1" xmax="1" ymin="-0.16" ymax="0.16" zmin="-0.16" zmax="0.16" />
+        <MechanicalObject name="mstate" template="Vec3"/>
+        <BoxROI name="center" box="-0.1 -1.1 -1.1  0.1 1.1 1.1" drawPoints="True" drawSize="0.01"/>
+    </Node>
+    <Node name="TargetCubeMidMove">
+
+        <MechanicalObject name="mstate" template="Rigid3" position="0 0 0 0 0 0 1"/>
+        <LinearMovementProjectiveConstraint indices="0" keyTimes="0  1  3  5  7" movements="0 0 0 0 0 0   0 1.0 0 0 0 0   0 0 1.0 0 0 0  0 -1.0 0 0 0 0   0 0  -1.0 0  0 0" />
+        <Node name="Attach">
+            <MechanicalObject name="mstate" template="Vec3" position=" 0 -0.16 -0.16  0 0.16 -0.16  0 -0.16 0.16  0 0.16 0.16"/>
+            <RigidMapping/>
+        </Node>
+    </Node>
+    <Node name="DeformableCube0">
+
+        <VisualStyle displayFlags="showForceFields" />
+        <EulerImplicitSolver name="odesolver" printLog="false" />
+        <SparseLDLSolver name="linearSolver" template="CompressedRowSparseMatrixMat3x3d" />
+
+        <RegularGridTopology name="grid" nx="7" ny="2" nz="2" xmin="-1" xmax="1" ymin="-0.16" ymax="0.16" zmin="-0.16" zmax="0.16" />
+        <MechanicalObject name="mstate" template="Vec3" />
+        <HexahedronFEMForceField poissonRatio="0.49" youngModulus="1e7"/>
+        <UniformMass totalMass="10" />
+        <BoxROI name="bottom" box="-1.1 -1.1 -1.1  -0.9 1.1 1.1" />
+        <BoxROI name="top" box="0.9 -1.1 -1.1  1.1 1.1 1.1" />
+        <BoxROI name="center" box="-0.1 -1.1 -1.1  0.1 1.1 1.1" />
+
+        <LinearSolverConstraintCorrection linearSolver="@linearSolver"/>
+    </Node>
+
+
+    <BilateralLagrangianConstraint template="Vec3"
+                                   object1="@DeformableCube0/mstate" first_point="@DeformableCube0/top.indices"
+                                   object2="@TargetCubeExtrimities/mstate" second_point="@TargetCubeExtrimities/top.indices" />
+
+    <BilateralLagrangianConstraint template="Vec3"
+                                   object1="@DeformableCube0/mstate" first_point="@DeformableCube0/center.indices"
+                                   object2="@TargetCubeMid/mstate" second_point="@TargetCubeMid/center.indices" />
+
+    <BilateralLagrangianConstraint template="Vec3"
+                                   object1="@DeformableCube0/mstate" first_point="@DeformableCube0/center.indices"
+                                   object2="@TargetCubeMidMove/Attach/mstate" second_point="0 1 2 3" />
+
+    <BilateralLagrangianConstraint template="Vec3"
+                                   object1="@DeformableCube0/mstate" first_point="@DeformableCube0/bottom.indices"
+                                   object2="@TargetCubeExtrimities/mstate" second_point="@TargetCubeExtrimities/bottom.indices" />
+
+
+</Node>