From 7f460e8041f657e75cb519183287c3c05b44aff4 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 20 Oct 2025 10:58:45 +0200 Subject: [PATCH 1/2] Support more types of linear systems --- .../SofaLinearSystem/Binding_LinearSystem.cpp | 139 +++++++++++------- .../SofaLinearSystem/Binding_LinearSystem.inl | 101 +++++++++++++ .../Binding_LinearSystem_doc.h | 32 +++- .../SofaLinearSystem/CMakeLists.txt | 1 + examples/access_matrix.py | 65 +++++--- 5 files changed, 258 insertions(+), 80 deletions(-) create mode 100644 bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.inl diff --git a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp index b0cfc4ae2..378862f63 100644 --- a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp +++ b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp @@ -17,14 +17,9 @@ ******************************************************************************* * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include -#include -#include +#include -#include -#include -#include #include #include @@ -39,12 +34,6 @@ using EigenSparseMatrix = Eigen::SparseMatrix; template using EigenMatrixMap = Eigen::Map >; -template -using Vector = Eigen::Matrix; - -template -using EigenVectorMap = Eigen::Map>; - template EigenSparseMatrix::Real> toEigen(sofa::linearalgebra::CompressedRowSparseMatrix& matrix) @@ -70,62 +59,102 @@ toEigen(sofa::linearalgebra::CompressedRowSparseMatrix& matrix) } } -template -void bindLinearSystems(py::module &m) +template +void bindMatrixAccessCRS(LinearSystemClass& c) { - using CRS = sofa::linearalgebra::CompressedRowSparseMatrix; - using Real = typename CRS::Real; - using CRSLinearSystem = sofa::component::linearsystem::TypedMatrixLinearSystem >; + using Real = typename TMatrix::Real; + using CRSLinearSystem = sofa::component::linearsystem::TypedMatrixLinearSystem; - const std::string typeName = CRSLinearSystem::GetClass()->className + CRSLinearSystem::GetCustomTemplateName(); - - py::class_ > c(m, typeName.c_str(), sofapython3::doc::linearsystem::linearSystemClass); - - c.def("A", [](CRSLinearSystem& self) -> EigenSparseMatrix - { - if (CRS* matrix = self.getSystemMatrix()) + const auto matrixAccess = + [](CRSLinearSystem& self) -> EigenSparseMatrix { - if (matrix->colsValue.empty()) //null matrix: contains no entries + if (TMatrix* matrix = self.getSystemMatrix()) { - return EigenSparseMatrix{matrix->rows(), matrix->cols()}; + if (matrix->colsValue.empty()) //null matrix: contains no entries + { + return EigenSparseMatrix{matrix->rows(), matrix->cols()}; + } + return toEigen(*matrix); } - return toEigen(*matrix); - } - return {}; - }, sofapython3::doc::linearsystem::linearSystem_A); + return {}; + }; - c.def("b", [](CRSLinearSystem& self) -> Vector - { - if (auto* vector = self.getRHSVector()) - { - return EigenVectorMap(vector->ptr(), vector->size()); - } - return {}; - }, sofapython3::doc::linearsystem::linearSystem_b); + c.def("A", matrixAccess, sofapython3::doc::linearsystem::linearSystem_CRS_A); + c.def("get_system_matrix", matrixAccess, sofapython3::doc::linearsystem::linearSystem_CRS_get_system_matrix); +} - c.def("x", [](CRSLinearSystem& self) -> Vector - { - if (auto* vector = self.getSolutionVector()) - { - return EigenVectorMap(vector->ptr(), vector->size()); - } - return {}; - }, sofapython3::doc::linearsystem::linearSystem_x); +template<> +void bindMatrixAccess(LinearSystemClass, sofa::linearalgebra::FullVector>& c) +{ + bindMatrixAccessCRS(c); +} +template<> +void bindMatrixAccess(LinearSystemClass>, sofa::linearalgebra::FullVector>& c) +{ + bindMatrixAccessCRS(c); +} - /// register the binding in the downcasting subsystem - PythonFactory::registerType([](sofa::core::objectmodel::Base* object) - { - return py::cast(dynamic_cast(object)); - }); +template<> +void bindMatrixAccess(LinearSystemClass>, sofa::linearalgebra::FullVector>& c) +{ + bindMatrixAccessCRS(c); +} + +template<> +void bindMatrixAccess(LinearSystemClass>, sofa::linearalgebra::FullVector>& c) +{ + bindMatrixAccessCRS(c); +} + +template<> +void bindMatrixAccess(LinearSystemClass>, sofa::linearalgebra::FullVector>& c) +{ + bindMatrixAccessCRS(c); +} + +template<> +void bindMatrixAccess(LinearSystemClass>, sofa::linearalgebra::FullVector>& c) +{ + bindMatrixAccessCRS(c); +} + +template +using DenseMatrix = Eigen::Matrix; + +template +using EigenDenseMatrixMap = Eigen::Map>; + +template<> +void bindMatrixAccess(LinearSystemClass, sofa::linearalgebra::FullVector>& c) +{ + using CRSLinearSystem = sofa::component::linearsystem::TypedMatrixLinearSystem, sofa::linearalgebra::FullVector>; + + const auto matrixAccess = + [](CRSLinearSystem& self) -> EigenDenseMatrixMap + { + sofa::linearalgebra::FullMatrix* matrix = self.getSystemMatrix(); + const auto row = matrix ? matrix->rows() : 0; + const auto col = matrix ? matrix->cols() : 0; + return EigenDenseMatrixMap(matrix ? matrix->ptr() : nullptr, row, col); + }; + c.def("A", matrixAccess, sofapython3::doc::linearsystem::linearSystem_FullMatrix_A); + c.def("get_system_matrix", matrixAccess, sofapython3::doc::linearsystem::linearSystem_FullMatrix_get_system_matrix); } void moduleAddLinearSystem(py::module &m) { - bindLinearSystems(m); - bindLinearSystems >(m); + bindLinearSystems, sofa::linearalgebra::FullVector >(m); + bindLinearSystems, sofa::linearalgebra::FullVector >(m); + bindLinearSystems, sofa::linearalgebra::FullVector >(m); + bindLinearSystems >, sofa::linearalgebra::FullVector >(m); + bindLinearSystems >, sofa::linearalgebra::FullVector >(m); + bindLinearSystems >, sofa::linearalgebra::FullVector >(m); + bindLinearSystems >, sofa::linearalgebra::FullVector >(m); + bindLinearSystems >, sofa::linearalgebra::FullVector >(m); + bindLinearSystems, sofa::linearalgebra::FullVector >(m); + bindLinearSystems, sofa::linearalgebra::FullVector >(m); + bindLinearSystems, sofa::linearalgebra::FullVector >(m); } } diff --git a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.inl b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.inl new file mode 100644 index 000000000..570567ccd --- /dev/null +++ b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.inl @@ -0,0 +1,101 @@ +/****************************************************************************** +* SofaPython3 plugin * +* (c) 2021 CNRS, University of Lille, INRIA * +* * +* 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 . * +******************************************************************************* +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include +#include +#include + +#include +#include +#include + +namespace py { using namespace pybind11; } + +namespace sofapython3 +{ + +template +using Vector = Eigen::Matrix; + +template +using EigenVectorMap = Eigen::Map>; + +template +Vector +getVector(TVector* vector) +{ + using Real = typename TVector::Real; + if (vector) + { + return EigenVectorMap(vector->ptr(), vector->size()); + } + return {}; +} + +template +Vector getRHSVector(sofa::component::linearsystem::TypedMatrixLinearSystem& linearSystem) +{ + return getVector(linearSystem.getRHSVector()); +} + +template +Vector getSolutionVector(sofa::component::linearsystem::TypedMatrixLinearSystem& linearSystem) +{ + return getVector(linearSystem.getSolutionVector()); +} + +template +using LinearSystemClass = + py::class_, + sofa::core::objectmodel::BaseObject, + sofapython3::py_shared_ptr> >; + +template +void bindMatrixAccess(LinearSystemClass& c) +{} + +template +void bindLinearSystems(py::module &m) +{ + using LinearSystem = sofa::component::linearsystem::TypedMatrixLinearSystem; + + const std::string typeName = + LinearSystem::GetClass()->className + + LinearSystem::GetCustomTemplateName(); + + LinearSystemClass c(m, typeName.c_str(), sofapython3::doc::linearsystem::linearSystemClass); + + c.def("b", getRHSVector, sofapython3::doc::linearsystem::linearSystem_b); + c.def("get_rhs_vector", getRHSVector, sofapython3::doc::linearsystem::linearSystem_b); + + c.def("x", getSolutionVector, sofapython3::doc::linearsystem::linearSystem_x); + c.def("get_solution_vector", getSolutionVector, sofapython3::doc::linearsystem::linearSystem_x); + + bindMatrixAccess(c); + + /// register the binding in the downcasting subsystem + PythonFactory::registerType([](sofa::core::objectmodel::Base* object) + { + return py::cast(dynamic_cast(object)); + }); +} + +} diff --git a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem_doc.h b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem_doc.h index 6c5786e8a..421fa221f 100644 --- a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem_doc.h +++ b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem_doc.h @@ -32,7 +32,7 @@ Linear system. Supports only CompressedRowSparseMatrix. linear_system = root.addObject('MatrixLinearSystem', template='CompressedRowSparseMatrixd') )"; -static auto linearSystem_A = +static auto linearSystem_CRS_A = R"( Returns the global system matrix as a scipy sparse matrix @@ -42,6 +42,36 @@ linear_system = root.addObject('MatrixLinearSystem', template='CompressedRowSpar matrix = linear_system.A() )"; +static auto linearSystem_CRS_get_system_matrix = +R"( +Returns the global system matrix as a scipy sparse matrix + +example: +------------ +linear_system = root.addObject('MatrixLinearSystem', template='CompressedRowSparseMatrixd') +matrix = linear_system.get_system_matrix() +)"; + +static auto linearSystem_FullMatrix_A = +R"( +Returns the global system matrix as a numpy array matrix + +example: +------------ +linear_system = root.addObject('MatrixLinearSystem', template='FullMatrix') +matrix = linear_system.A() +)"; + +static auto linearSystem_FullMatrix_get_system_matrix = +R"( +Returns the global system matrix as a numpy array matrix + +example: +------------ +linear_system = root.addObject('MatrixLinearSystem', template='FullMatrix') +matrix = linear_system.get_system_matrix() +)"; + static auto linearSystem_b = R"( Returns the global system right hand side as a numpy array diff --git a/bindings/Modules/src/SofaPython3/SofaLinearSystem/CMakeLists.txt b/bindings/Modules/src/SofaPython3/SofaLinearSystem/CMakeLists.txt index d6b64a130..fd0805488 100644 --- a/bindings/Modules/src/SofaPython3/SofaLinearSystem/CMakeLists.txt +++ b/bindings/Modules/src/SofaPython3/SofaLinearSystem/CMakeLists.txt @@ -2,6 +2,7 @@ project(Bindings.Modules.SofaLinearSystem) set(HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/Binding_LinearSystem.h + ${CMAKE_CURRENT_SOURCE_DIR}/Binding_LinearSystem.inl ${CMAKE_CURRENT_SOURCE_DIR}/Binding_LinearSystem_doc.h ) diff --git a/examples/access_matrix.py b/examples/access_matrix.py index 3cc33523f..aeb1fa21e 100644 --- a/examples/access_matrix.py +++ b/examples/access_matrix.py @@ -1,9 +1,27 @@ # Required import for python import Sofa import SofaRuntime +from Sofa import SofaLinearSystem from Sofa import SofaLinearSolver from scipy import sparse +def createBeam(root, matrix_type): + node = root.addChild(matrix_type) + node.addObject('EulerImplicitSolver', rayleighStiffness="0.1", rayleighMass="0.1") + linear_system = node.addObject('MatrixLinearSystem', template=matrix_type) + + node.addObject('MechanicalObject', name="DoFs") + node.addObject('UniformMass', name="mass", totalMass="320") + node.addObject('RegularGridTopology', name="grid", nx="4", ny="4", nz="20", xmin="-9", xmax="-6", ymin="0", ymax="3", zmin="0", zmax="19") + node.addObject('BoxROI', name="box", box="-10 -1 -0.0001 -5 4 0.0001") + node.addObject('FixedProjectiveConstraint', indices="@box.indices") + node.addObject('HexahedronFEMForceField', name="FEM", youngModulus="4000", poissonRatio="0.3", method="large") + + node.addObject(MatrixAccessController('MatrixAccessor', name=f'matrixAccessor{matrix_type}', linear_system=linear_system)) + + return node + + # Function called when the scene graph is being created def createScene(root): @@ -15,26 +33,22 @@ def createScene(root): 'Sofa.Component.Visual' ]) - root.addObject('DefaultAnimationLoop') + root.addObject('DefaultAnimationLoop', parallelODESolving=True) root.addObject('DefaultVisualManagerLoop') - root.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') - root.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') - root.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') - root.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Projective') - root.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') - - root.addObject('EulerImplicitSolver', rayleighStiffness="0.1", rayleighMass="0.1") - linear_solver = root.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixMat3x3d") + plugins = root.addChild('plugins') + plugins.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Projective') + plugins.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') + plugins.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') + plugins.addObject('RequiredPlugin', name='Sofa.Component.LinearSystem') + plugins.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') + plugins.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') - root.addObject('MechanicalObject', name="DoFs") - root.addObject('UniformMass', name="mass", totalMass="320") - root.addObject('RegularGridTopology', name="grid", nx="4", ny="4", nz="20", xmin="-9", xmax="-6", ymin="0", ymax="3", zmin="0", zmax="19") - root.addObject('BoxROI', name="box", box="-10 -1 -0.0001 -5 4 0.0001") - root.addObject('FixedProjectiveConstraint', indices="@box.indices") - root.addObject('HexahedronFEMForceField', name="FEM", youngModulus="4000", poissonRatio="0.3", method="large") + node_crs = createBeam(root, 'CompressedRowSparseMatrixMat3x3d') + node_crs.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixMat3x3d") - root.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', linear_solver=linear_solver)) + node_full = createBeam(root, 'FullMatrix') + node_full.addObject('CGLinearSolver', template="FullMatrix") return root @@ -43,31 +57,34 @@ class MatrixAccessController(Sofa.Core.Controller): def __init__(self, *args, **kwargs): + print('Initialize controller') Sofa.Core.Controller.__init__(self, *args, **kwargs) - self.linear_solver = kwargs.get("linear_solver") + self.linear_system = kwargs.get("linear_system") + print(f'Type linear system: {type(self.linear_system)}') def onAnimateEndEvent(self, event): - system_matrix = self.linear_solver.A() - rhs = self.linear_solver.b() - solution = self.linear_solver.x() + system_matrix = self.linear_system.get_system_matrix() + rhs = self.linear_system.get_rhs_vector() + solution = self.linear_system.get_solution_vector() print('====================================') - print('Global system matrix') + print(f'Global system matrix {self.getName()}') print('====================================') print('dtype: ' + str(system_matrix.dtype)) print('shape: ' + str(system_matrix.shape)) print('ndim: ' + str(system_matrix.ndim)) - print('nnz: ' + str(system_matrix.nnz)) + if hasattr(system_matrix, 'nnz'): #if the matrix is sparse + print('nnz: ' + str(system_matrix.nnz)) print('====================================') - print('System right hand side') + print(f'System right hand side {self.getName()}') print('====================================') print('dtype: ' + str(rhs.dtype)) print('shape: ' + str(rhs.shape)) print('ndim: ' + str(rhs.ndim)) print('====================================') - print('System solution') + print(f'System solution {self.getName()}') print('====================================') print('dtype: ' + str(solution.dtype)) print('shape: ' + str(solution.shape)) From 35627c60d54db381b5a2d18a0466daf27079573e Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 20 Oct 2025 11:04:45 +0200 Subject: [PATCH 2/2] add support for BTD --- .../src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp index 378862f63..d03773972 100644 --- a/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp +++ b/bindings/Modules/src/SofaPython3/SofaLinearSystem/Binding_LinearSystem.cpp @@ -21,6 +21,8 @@ #include +#include +#include #include @@ -155,6 +157,8 @@ void moduleAddLinearSystem(py::module &m) bindLinearSystems, sofa::linearalgebra::FullVector >(m); bindLinearSystems, sofa::linearalgebra::FullVector >(m); bindLinearSystems, sofa::linearalgebra::FullVector >(m); + + bindLinearSystems, sofa::linearalgebra::BlockVector<6, SReal> >(m); } }