Access to the constraint matrix

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("DiagonalMass", name="mass", totalMass="1e-3")
+
+ 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)
+
+