Access to the constraint matrix (#459)

* Access to the constraint matrix

* Use of MeshMatrixMass instead of DiagonalMass

Co-authored-by: Hugo <[email protected]>

---------

Co-authored-by: Hugo <[email protected]>

bindings/Sofa/src/SofaPython3/Sofa/Types/Binding_CompressedRowSparseMatrix.cpp

@@ -0,0 +1,96 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2021 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/>.        *
+*******************************************************************************
+* Contact information: [email protected]                             *
+******************************************************************************/
+#include <SofaPython3/Sofa/Types/Binding_CompressedRowSparseMatrix.h>
+
+#include <sofa/core/objectmodel/BaseData.h>
+#include <sofa/core/objectmodel/Data.h>
+#include <sofa/defaulttype/RigidTypes.h>
+#include <sofa/linearalgebra/CompressedRowSparseMatrixConstraint.h>
+#include <pybind11/eigen.h>
+#include <sofa/linearalgebra/CompressedRowSparseMatrixConstraintEigenUtils.h>
+#include <sofa/type/trait/is_specialization_of.h>
+#include <SofaPython3/PythonFactory.h>
+
+
+namespace py { using namespace pybind11; }
+
+namespace sofapython3
+{
+
+template<class TBlock>
+typename sofa::linearalgebra::CompressedRowSparseMatrixToEigenSparseVec<TBlock>::EigenSparseMatrix
+toEigen(const sofa::linearalgebra::CompressedRowSparseMatrixConstraint<TBlock>& matrix)
+{
+    static sofa::linearalgebra::CompressedRowSparseMatrixToEigenSparseVec<TBlock> convert;
+    return convert(matrix);
+}
+
+template<class MatrixDeriv>
+void bindCompressedRowSparseMatrixConstraint(pybind11::module& m)
+{
+    static_assert(sofa::type::trait::is_specialization_of_v<MatrixDeriv, sofa::linearalgebra::CompressedRowSparseMatrixConstraint>,
+                  "Template argument must be a specialization of CompressedRowSparseMatrixConstraint");
+
+    py::class_<sofa::Data<MatrixDeriv>, sofa::core::objectmodel::BaseData,
+            std::unique_ptr<sofa::Data<MatrixDeriv>, pybind11::nodelete> > crsmc(m, MatrixDeriv::Name() );
+
+    crsmc.def_property_readonly("value", [](sofa::Data<MatrixDeriv>& self)
+    {
+        sofa::helper::ReadAccessor accessor(self);
+        return toEigen(accessor.ref());
+    });
+
+    PythonFactory::registerType(MatrixDeriv::Name(), [](sofa::core::BaseData* data) -> py::object {
+        auto matrix = reinterpret_cast<sofa::Data<MatrixDeriv>*>(data);
+        return py::cast(matrix);
+    });
+}
+
+/**
+ * Given a template parameter pack, create the bindings of all the templates in
+ * the pack, making sure that there are no duplicate types.
+ */
+template <typename T, typename... Args>
+void bindAllCompressedRowSparseMatrixConstraintTypes(pybind11::module& m)
+{
+    static_assert(!(std::is_same_v<T, Args> || ...), "Error: Duplicate types found!");
+
+    bindCompressedRowSparseMatrixConstraint<T>(m);
+    if constexpr (sizeof...(Args) > 0)
+    {
+        bindAllCompressedRowSparseMatrixConstraintTypes<Args...>(m);
+    }
+}
+
+void moduleAddCompressedRowSparseMatrix(pybind11::module& m)
+{
+    using namespace sofa::defaulttype;
+
+    bindAllCompressedRowSparseMatrixConstraintTypes<
+        Vec1Types::MatrixDeriv,
+        Vec2Types::MatrixDeriv,
+        Vec3Types::MatrixDeriv,
+        Vec6Types::MatrixDeriv,
+        Rigid2Types::MatrixDeriv,
+        Rigid3Types::MatrixDeriv
+    >(m);
+}
+
+}

bindings/Sofa/src/SofaPython3/Sofa/Types/Binding_CompressedRowSparseMatrix.h

@@ -0,0 +1,29 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2021 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/>.        *
+*******************************************************************************
+* Contact information: [email protected]                             *
+******************************************************************************/
+
+#pragma once
+
+#include <pybind11/pybind11.h>
+
+namespace sofapython3 {
+
+void moduleAddCompressedRowSparseMatrix(pybind11::module& m);
+
+}  // namespace sofapython3

bindings/Sofa/src/SofaPython3/Sofa/Types/CMakeLists.txt

@@ -2,10 +2,12 @@ project(Bindings.Sofa.Types)
 
 set(HEADER_FILES
     ${CMAKE_CURRENT_SOURCE_DIR}/Binding_BoundingBox.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/Binding_CompressedRowSparseMatrix.h
 )
 
 set(SOURCE_FILES
     ${CMAKE_CURRENT_SOURCE_DIR}/Submodule_Types.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/Binding_CompressedRowSparseMatrix.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/Binding_BoundingBox.cpp
 )
 

bindings/Sofa/src/SofaPython3/Sofa/Types/Submodule_Types.cpp

@@ -22,6 +22,8 @@
 #include <sofa/defaulttype/init.h>
 
 #include <SofaPython3/Sofa/Types/Binding_BoundingBox.h>
+#include <SofaPython3/Sofa/Types/Binding_CompressedRowSparseMatrix.h>
+
 
 namespace sofapython3 {
 /// The first parameter must be named the same as the module file to load.
@@ -42,6 +44,7 @@ PYBIND11_MODULE(Types, types)
        )doc";
 
     moduleAddBoundingBox(types);
+    moduleAddCompressedRowSparseMatrix(types);
 }
 
 }  // namespace sofapython3

examples/access_constraint_matrix.py

@@ -0,0 +1,66 @@
+# Required import for python
+import Sofa
+import SofaRuntime
+import numpy as np
+
+
+# Function called when the scene graph is being created
+def createScene(root):
+    root.addObject("RequiredPlugin", pluginName=["Sofa.Component.AnimationLoop",
+                                                 "Sofa.Component.Constraint.Lagrangian.Correction",
+                                                 "Sofa.Component.Constraint.Lagrangian.Model",
+                                                 "Sofa.Component.Constraint.Lagrangian.Solver",
+                                                 "Sofa.Component.Constraint.Projective",
+                                                 "Sofa.Component.IO.Mesh",
+                                                 "Sofa.Component.LinearSolver.Direct",
+                                                 "Sofa.Component.Mapping.MappedMatrix",
+                                                 "Sofa.Component.Mass",
+                                                 "Sofa.Component.ODESolver.Backward",
+                                                 "Sofa.Component.Topology.Container.Dynamic",
+                                                 "Sofa.Component.Mapping.NonLinear",
+                                                 "Sofa.Component.StateContainer"
+                                                 ])
+
+    root.addObject("FreeMotionAnimationLoop", solveVelocityConstraintFirst=True)
+    root.addObject("GenericConstraintSolver", tolerance=1e-9, maxIterations=1000)
+    root.addObject("StringMeshCreator", name="loader", resolution="20")
+
+    root.addObject("EulerImplicitSolver")
+    root.addObject("EigenSimplicialLLT", template='CompressedRowSparseMatrixMat3x3d')
+    root.addObject("GenericConstraintCorrection")
+
+    root.addObject("EdgeSetTopologyContainer", name="topo", position="@loader.position", edges="@loader.edges")
+    mechanical_object = root.addObject("MechanicalObject", name="defoDOF", template="Vec3d")
+    root.addObject("EdgeSetGeometryAlgorithms", drawEdges=True)
+    root.addObject("FixedProjectiveConstraint", indices=[0])
+    root.addObject("MeshMatrixMass", name="mass", totalMass="1e-3", lumping=True)
+
+    ext = root.addChild("extensionsNode")
+    ext.addObject("MechanicalObject", template="Vec1d", name="extensionsDOF")
+    ext.addObject("DistanceMapping", name="distanceMapping", topology="@topo")
+    ext.addObject("UniformLagrangianConstraint", template="Vec1d", iterative=True)
+
+    root.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', mechanical_object=mechanical_object))
+
+
+class MatrixAccessController(Sofa.Core.Controller):
+
+    def __init__(self, *args, **kwargs):
+        Sofa.Core.Controller.__init__(self, *args, **kwargs)
+        self.mechanical_object = kwargs.get("mechanical_object")
+
+    def onBuildConstraintSystemEndEvent(self, event):
+        constraint_matrix = self.mechanical_object.constraint.value
+
+        print(self.mechanical_object.constraint.linkpath)
+
+        print('====================================')
+        print('Constraint matrix in constraint space')
+        print('====================================')
+        print('dtype: ' + str(constraint_matrix.dtype))
+        print('shape: ' + str(constraint_matrix.shape))
+        print('ndim: ' + str(constraint_matrix.ndim))
+
+        print(constraint_matrix)
+
+