[BuiltConstraintSolver] Add SVD based regularization strategy

Sofa/Component/AnimationLoop/src/sofa/component/animationloop/FreeMotionAnimationLoop.cpp

@@ -44,10 +44,12 @@
 #include <sofa/simulation/CollisionVisitor.h>
 #include <sofa/simulation/SolveVisitor.h>
 #include <sofa/simulation/MainTaskSchedulerFactory.h>
+#include <sofa/component/constraint/lagrangian/solver/ProjectedGaussSeidelConstraintSolver.h>
 
 #include <sofa/simulation/mechanicalvisitor/MechanicalVInitVisitor.h>
 using sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor;
 
+
 #include <sofa/simulation/mechanicalvisitor/MechanicalBeginIntegrationVisitor.h>
 using sofa::simulation::mechanicalvisitor::MechanicalBeginIntegrationVisitor;
 
@@ -67,7 +69,7 @@ using namespace core::behavior;
 using namespace sofa::simulation;
 using sofa::helper::ScopedAdvancedTimer;
 
-using DefaultConstraintSolver = sofa::component::constraint::lagrangian::solver::GenericConstraintSolver;
+using DefaultConstraintSolver = sofa::component::constraint::lagrangian::solver::ProjectedGaussSeidelConstraintSolver;
 
 FreeMotionAnimationLoop::FreeMotionAnimationLoop() :
     d_solveVelocityConstraintFirst(initData(&d_solveVelocityConstraintFirst , false, "solveVelocityConstraintFirst", "solve separately velocity constraint violations before position constraint violations"))

Sofa/Component/Constraint/Lagrangian/Model/tests/scenes_test/BilateralInteractionConstraint.scn

@@ -2,7 +2,7 @@
 <Node name="root" dt="0.001" gravity="0 -981 0">
     <VisualStyle displayFlags="showForceFields" />
     <FreeMotionAnimationLoop />
-    <GenericConstraintSolver maxIterations="100" tolerance="1e-3" />
+    <ProjectedGaussSeidelConstraintSolver maxIterations="100" tolerance="1e-3" />
     <CollisionPipeline  depth="6" verbose="0" draw="0" />
     <BruteForceBroadPhase/>
     <BVHNarrowPhase/>

Sofa/Component/Constraint/Lagrangian/Solver/CMakeLists.txt

@@ -8,7 +8,13 @@ set(HEADER_FILES
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.h
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.h
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.h
+
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.h
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintSolver.h
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.h
 
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/visitors/ConstraintStoreLambdaVisitor.h
@@ -20,7 +26,13 @@ set(SOURCE_FILES
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/init.cpp
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ConstraintSolverImpl.cpp
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintProblem.cpp
+
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/GenericConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/BuiltConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/NNCGConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/ProjectedGaussSeidelConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltConstraintSolver.cpp
+    ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/UnbuiltGaussSeidelConstraintSolver.cpp
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/LCPConstraintSolver.cpp
 
     ${SOFACOMPONENTCONSTRAINTLAGRANGIANSOLVER_SOURCE_DIR}/visitors/ConstraintStoreLambdaVisitor.cpp

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

@@ -0,0 +1,192 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+
+#include <sofa/component/constraint/lagrangian/solver/BuiltConstraintSolver.h>
+
+#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"))
+{}
+
+void BuiltConstraintSolver::init()
+{
+    Inherit1::init();
+    if(d_multithreading.getValue())
+    {
+        simulation::MainTaskSchedulerFactory::createInRegistry()->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);
+            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))/100.0)
+                {
+                    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)) > 0.001;
+            }
+
+            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);
+    SCOPED_TIMER_VARNAME(getComplianceTimer, "Get Compliance");
+    dmsg_info() <<" computeCompliance in "  << l_constraintCorrections.size()<< " constraintCorrections" ;
+
+    const bool multithreading = d_multithreading.getValue();
+
+    const simulation::ForEachExecutionPolicy execution = multithreading ?
+        simulation::ForEachExecutionPolicy::PARALLEL :
+        simulation::ForEachExecutionPolicy::SEQUENTIAL;
+
+    simulation::TaskScheduler* taskScheduler = simulation::MainTaskSchedulerFactory::createInRegistry();
+    assert(taskScheduler);
+
+    //Used to prevent simultaneous accesses to the main compliance matrix
+    std::mutex mutex;
+
+    //Visits all constraint corrections to compute the compliance matrix projected
+    //in the constraint space.
+    simulation::forEachRange(execution, *taskScheduler,  l_constraintCorrections.begin(),  l_constraintCorrections.end(),
+        [&cParams, this, &multithreading, &mutex, problem](const auto& range)
+        {
+            ComplianceWrapper compliance(problem->W, multithreading);
+
+            for (auto it = range.start; it != range.end; ++it)
+            {
+                core::behavior::BaseConstraintCorrection* cc = *it;
+                if (cc->isActive())
+                {
+                    cc->addComplianceInConstraintSpace(cParams, &compliance.matrix());
+                }
+            }
+
+            std::lock_guard guard(mutex);
+            compliance.assembleMatrix();
+        });
+
+    addRegularization(problem->W,  d_regularizationTerm.getValue());
+    dmsg_info() << " computeCompliance_done "  ;
+}
+
+
+BuiltConstraintSolver::ComplianceWrapper::ComplianceMatrixType& BuiltConstraintSolver::ComplianceWrapper::matrix()
+{
+    if (m_isMultiThreaded)
+    {
+        if (!m_threadMatrix)
+        {
+            m_threadMatrix = std::make_unique<ComplianceMatrixType>();
+            m_threadMatrix->resize(m_complianceMatrix.rowSize(), m_complianceMatrix.colSize());
+        }
+        return *m_threadMatrix;
+    }
+    return m_complianceMatrix;
+}
+
+void BuiltConstraintSolver::ComplianceWrapper::assembleMatrix() const
+{
+    if (m_threadMatrix)
+    {
+        for (linearalgebra::BaseMatrix::Index j = 0; j < m_threadMatrix->rowSize(); ++j)
+        {
+            for (linearalgebra::BaseMatrix::Index l = 0; l < m_threadMatrix->colSize(); ++l)
+            {
+                m_complianceMatrix.add(j, l, m_threadMatrix->element(j,l));
+            }
+        }
+    }
+}
+
+}

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

@@ -0,0 +1,70 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: contact@sofa-framework.org                             *
+******************************************************************************/
+#pragma once
+
+#include <sofa/component/constraint/lagrangian/solver/GenericConstraintSolver.h>
+
+namespace sofa::component::constraint::lagrangian::solver
+{
+/**
+ *  \brief This component implements a generic way of building system for solvers that use a built
+ *  version of the constraint matrix. Any solver that uses a build matrix should inherit from this.
+ *  This component is purely virtual because doSolve is not defined and needs to be defined in the
+ *  inherited class
+ */
+class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API BuiltConstraintSolver : public GenericConstraintSolver
+{
+
+
+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
+
+    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:
+
+    struct ComplianceWrapper
+    {
+        using ComplianceMatrixType = sofa::linearalgebra::LPtrFullMatrix<SReal>;
+
+        ComplianceWrapper(ComplianceMatrixType& complianceMatrix, bool isMultiThreaded)
+        : m_isMultiThreaded(isMultiThreaded), m_complianceMatrix(complianceMatrix) {}
+
+        ComplianceMatrixType& matrix();
+
+        void assembleMatrix() const;
+
+    private:
+        bool m_isMultiThreaded { false };
+        ComplianceMatrixType& m_complianceMatrix;
+        std::unique_ptr<ComplianceMatrixType> m_threadMatrix;
+    };
+};
+}

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

@@ -51,11 +51,11 @@ void ConstraintProblem::clear(int nbConstraints)
     dFree.resize(nbConstraints);
     f.resize(nbConstraints);
 
-    static std::atomic<unsigned int> counter = 0;
+    static std::atomic<unsigned> counter = 0;
     problemId = counter.fetch_add(1, std::memory_order_relaxed);
 }
 
-unsigned int ConstraintProblem::getProblemId()
+unsigned ConstraintProblem::getProblemId() const
 {
     return problemId;
 }

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

@@ -59,18 +59,19 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintProblem
 
     // Returns the number of scalar constraints, or equivalently the number of Lagrange multipliers
     int getDimension() const { return dimension; }
+    void setDimension(int dim) { dimension = dim; }
 
     SReal** getW() { return W.lptr(); }
     SReal* getDfree() { return dFree.ptr(); }
     SReal* getF() { return f.ptr(); }
 
     virtual void solveTimed(SReal tolerance, int maxIt, SReal timeout) = 0;
 
-    unsigned int getProblemId();
+    unsigned getProblemId() const;
 
 protected:
     int dimension;
-    unsigned int problemId;
+    unsigned problemId;
 };
 
 
@@ -93,16 +94,17 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_SOLVER_API ConstraintSolverImpl : pub
 
     void removeConstraintCorrection(core::behavior::BaseConstraintCorrection *s) override;
 
+    MultiLink< ConstraintSolverImpl,
+        core::behavior::BaseConstraintCorrection,
+        BaseLink::FLAG_STOREPATH> l_constraintCorrections;
+
 protected:
 
     void postBuildSystem(const core::ConstraintParams* cParams) override;
     void postSolveSystem(const core::ConstraintParams* cParams) override;
 
     void clearConstraintCorrections();
 
-    MultiLink< ConstraintSolverImpl,
-        core::behavior::BaseConstraintCorrection,
-        BaseLink::FLAG_STOREPATH> l_constraintCorrections;
 
     /// Calls the method resetConstraint on all the mechanical states and BaseConstraintSet
     /// In the case of a MechanicalObject, it clears the constraint jacobian matrix