Skip to content

Commit e858992

Browse files
alxbilgerhugtalbot
alxbilger
and
hugtalbot
authored
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]>
1 parent a17d1e4 commit e858992

File tree

5 files changed

+196
-0
lines changed

5 files changed

+196
-0
lines changed

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

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2021 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Contact information: [email protected] *
19+
******************************************************************************/
20+
#include <SofaPython3/Sofa/Types/Binding_CompressedRowSparseMatrix.h>
21+
22+
#include <sofa/core/objectmodel/BaseData.h>
23+
#include <sofa/core/objectmodel/Data.h>
24+
#include <sofa/defaulttype/RigidTypes.h>
25+
#include <sofa/linearalgebra/CompressedRowSparseMatrixConstraint.h>
26+
#include <pybind11/eigen.h>
27+
#include <sofa/linearalgebra/CompressedRowSparseMatrixConstraintEigenUtils.h>
28+
#include <sofa/type/trait/is_specialization_of.h>
29+
#include <SofaPython3/PythonFactory.h>
30+
31+
32+
namespace py { using namespace pybind11; }
33+
34+
namespace sofapython3
35+
{
36+
37+
template<class TBlock>
38+
typename sofa::linearalgebra::CompressedRowSparseMatrixToEigenSparseVec<TBlock>::EigenSparseMatrix
39+
toEigen(const sofa::linearalgebra::CompressedRowSparseMatrixConstraint<TBlock>& matrix)
40+
{
41+
static sofa::linearalgebra::CompressedRowSparseMatrixToEigenSparseVec<TBlock> convert;
42+
return convert(matrix);
43+
}
44+
45+
template<class MatrixDeriv>
46+
void bindCompressedRowSparseMatrixConstraint(pybind11::module& m)
47+
{
48+
static_assert(sofa::type::trait::is_specialization_of_v<MatrixDeriv, sofa::linearalgebra::CompressedRowSparseMatrixConstraint>,
49+
"Template argument must be a specialization of CompressedRowSparseMatrixConstraint");
50+
51+
py::class_<sofa::Data<MatrixDeriv>, sofa::core::objectmodel::BaseData,
52+
std::unique_ptr<sofa::Data<MatrixDeriv>, pybind11::nodelete> > crsmc(m, MatrixDeriv::Name() );
53+
54+
crsmc.def_property_readonly("value", [](sofa::Data<MatrixDeriv>& self)
55+
{
56+
sofa::helper::ReadAccessor accessor(self);
57+
return toEigen(accessor.ref());
58+
});
59+
60+
PythonFactory::registerType(MatrixDeriv::Name(), [](sofa::core::BaseData* data) -> py::object {
61+
auto matrix = reinterpret_cast<sofa::Data<MatrixDeriv>*>(data);
62+
return py::cast(matrix);
63+
});
64+
}
65+
66+
/**
67+
* Given a template parameter pack, create the bindings of all the templates in
68+
* the pack, making sure that there are no duplicate types.
69+
*/
70+
template <typename T, typename... Args>
71+
void bindAllCompressedRowSparseMatrixConstraintTypes(pybind11::module& m)
72+
{
73+
static_assert(!(std::is_same_v<T, Args> || ...), "Error: Duplicate types found!");
74+
75+
bindCompressedRowSparseMatrixConstraint<T>(m);
76+
if constexpr (sizeof...(Args) > 0)
77+
{
78+
bindAllCompressedRowSparseMatrixConstraintTypes<Args...>(m);
79+
}
80+
}
81+
82+
void moduleAddCompressedRowSparseMatrix(pybind11::module& m)
83+
{
84+
using namespace sofa::defaulttype;
85+
86+
bindAllCompressedRowSparseMatrixConstraintTypes<
87+
Vec1Types::MatrixDeriv,
88+
Vec2Types::MatrixDeriv,
89+
Vec3Types::MatrixDeriv,
90+
Vec6Types::MatrixDeriv,
91+
Rigid2Types::MatrixDeriv,
92+
Rigid3Types::MatrixDeriv
93+
>(m);
94+
}
95+
96+
}

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

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2021 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Contact information: [email protected] *
19+
******************************************************************************/
20+
21+
#pragma once
22+
23+
#include <pybind11/pybind11.h>
24+
25+
namespace sofapython3 {
26+
27+
void moduleAddCompressedRowSparseMatrix(pybind11::module& m);
28+
29+
} // namespace sofapython3

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,12 @@ project(Bindings.Sofa.Types)
22

33
set(HEADER_FILES
44
${CMAKE_CURRENT_SOURCE_DIR}/Binding_BoundingBox.h
5+
${CMAKE_CURRENT_SOURCE_DIR}/Binding_CompressedRowSparseMatrix.h
56
)
67

78
set(SOURCE_FILES
89
${CMAKE_CURRENT_SOURCE_DIR}/Submodule_Types.cpp
10+
${CMAKE_CURRENT_SOURCE_DIR}/Binding_CompressedRowSparseMatrix.cpp
911
${CMAKE_CURRENT_SOURCE_DIR}/Binding_BoundingBox.cpp
1012
)
1113

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,8 @@
2222
#include <sofa/defaulttype/init.h>
2323

2424
#include <SofaPython3/Sofa/Types/Binding_BoundingBox.h>
25+
#include <SofaPython3/Sofa/Types/Binding_CompressedRowSparseMatrix.h>
26+
2527

2628
namespace sofapython3 {
2729
/// The first parameter must be named the same as the module file to load.
@@ -42,6 +44,7 @@ PYBIND11_MODULE(Types, types)
4244
)doc";
4345

4446
moduleAddBoundingBox(types);
47+
moduleAddCompressedRowSparseMatrix(types);
4548
}
4649

4750
} // namespace sofapython3

‎examples/access_constraint_matrix.py‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
# Required import for python
2+
import Sofa
3+
import SofaRuntime
4+
import numpy as np
5+
6+
7+
# Function called when the scene graph is being created
8+
def createScene(root):
9+
root.addObject("RequiredPlugin", pluginName=["Sofa.Component.AnimationLoop",
10+
"Sofa.Component.Constraint.Lagrangian.Correction",
11+
"Sofa.Component.Constraint.Lagrangian.Model",
12+
"Sofa.Component.Constraint.Lagrangian.Solver",
13+
"Sofa.Component.Constraint.Projective",
14+
"Sofa.Component.IO.Mesh",
15+
"Sofa.Component.LinearSolver.Direct",
16+
"Sofa.Component.Mapping.MappedMatrix",
17+
"Sofa.Component.Mass",
18+
"Sofa.Component.ODESolver.Backward",
19+
"Sofa.Component.Topology.Container.Dynamic",
20+
"Sofa.Component.Mapping.NonLinear",
21+
"Sofa.Component.StateContainer"
22+
])
23+
24+
root.addObject("FreeMotionAnimationLoop", solveVelocityConstraintFirst=True)
25+
root.addObject("GenericConstraintSolver", tolerance=1e-9, maxIterations=1000)
26+
root.addObject("StringMeshCreator", name="loader", resolution="20")
27+
28+
root.addObject("EulerImplicitSolver")
29+
root.addObject("EigenSimplicialLLT", template='CompressedRowSparseMatrixMat3x3d')
30+
root.addObject("GenericConstraintCorrection")
31+
32+
root.addObject("EdgeSetTopologyContainer", name="topo", position="@loader.position", edges="@loader.edges")
33+
mechanical_object = root.addObject("MechanicalObject", name="defoDOF", template="Vec3d")
34+
root.addObject("EdgeSetGeometryAlgorithms", drawEdges=True)
35+
root.addObject("FixedProjectiveConstraint", indices=[0])
36+
root.addObject("MeshMatrixMass", name="mass", totalMass="1e-3", lumping=True)
37+
38+
ext = root.addChild("extensionsNode")
39+
ext.addObject("MechanicalObject", template="Vec1d", name="extensionsDOF")
40+
ext.addObject("DistanceMapping", name="distanceMapping", topology="@topo")
41+
ext.addObject("UniformLagrangianConstraint", template="Vec1d", iterative=True)
42+
43+
root.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', mechanical_object=mechanical_object))
44+
45+
46+
class MatrixAccessController(Sofa.Core.Controller):
47+
48+
def __init__(self, *args, **kwargs):
49+
Sofa.Core.Controller.__init__(self, *args, **kwargs)
50+
self.mechanical_object = kwargs.get("mechanical_object")
51+
52+
def onBuildConstraintSystemEndEvent(self, event):
53+
constraint_matrix = self.mechanical_object.constraint.value
54+
55+
print(self.mechanical_object.constraint.linkpath)
56+
57+
print('====================================')
58+
print('Constraint matrix in constraint space')
59+
print('====================================')
60+
print('dtype: ' + str(constraint_matrix.dtype))
61+
print('shape: ' + str(constraint_matrix.shape))
62+
print('ndim: ' + str(constraint_matrix.ndim))
63+
64+
print(constraint_matrix)
65+
66+

0 commit comments

Comments
 (0)