Skip to content

Commit 3bec75d

Browse files
hugtalbothugtalbot
authored
Merge pull request #262 from alxbilger/stiffnessmassmatrix
Bindings to access matrices from force fields and masses
2 parents 4c9945b + 1e20738 commit 3bec75d

13 files changed

Lines changed: 465 additions & 10 deletions

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp‎

Lines changed: 52 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,10 @@
2828
#include <sofa/core/behavior/MechanicalState.h>
2929
#include <sofa/core/behavior/ForceField.h>
3030
#include <sofa/core/MechanicalParams.h>
31-
#include <sofa/core/behavior/MultiMatrixAccessor.h>
31+
#include <sofa/core/behavior/DefaultMultiMatrixAccessor.h>
32+
#include <sofa/linearalgebra/CompressedRowSparseMatrix.h>
33+
34+
#include <pybind11/eigen.h>
3235

3336
#include <SofaPython3/PythonEnvironment.h>
3437
using sofapython3::PythonEnvironment;
@@ -49,6 +52,7 @@ namespace sofapython3
4952
using sofa::defaulttype::Vec3dTypes;
5053
using sofa::defaulttype::Vec2dTypes;
5154
using sofa::defaulttype::Vec1dTypes;
55+
using sofa::defaulttype::Vec6dTypes;
5256
using sofa::defaulttype::Rigid3dTypes;
5357
using sofa::defaulttype::Rigid2dTypes;
5458

@@ -177,10 +181,10 @@ namespace sofapython3
177181

178182

179183
template<class TDOFType>
180-
void declare_forcefield(py::module &m, std::string typestr) {
181-
std::string pyclass_name = std::string("ForceField") + typestr;
184+
void declare_forcefield(py::module &m) {
185+
const std::string pyclass_name = std::string("ForceField") + TDOFType::Name();
182186
py::class_<ForceField<TDOFType>, BaseObject, ForceField_Trampoline<TDOFType>, py_shared_ptr<ForceField<TDOFType>>> f(m, pyclass_name.c_str(), py::dynamic_attr(), py::multiple_inheritance(), sofapython3::doc::forceField::forceFieldClass);
183-
187+
184188
f.def(py::init([](py::args &args, py::kwargs &kwargs) {
185189
auto ff = sofa::core::sptr<ForceField_Trampoline<TDOFType>> (new ForceField_Trampoline<TDOFType>());
186190

@@ -204,15 +208,54 @@ namespace sofapython3
204208
}
205209
return ff;
206210
}));
211+
212+
using Real = typename TDOFType::Real;
213+
using EigenSparseMatrix = Eigen::SparseMatrix<typename TDOFType::Real, Eigen::RowMajor>;
214+
using EigenMatrixMap = Eigen::Map<EigenSparseMatrix>;
215+
216+
f.def("assembleKMatrix", [](ForceField<TDOFType>& self) -> EigenSparseMatrix
217+
{
218+
sofa::linearalgebra::CompressedRowSparseMatrix<Real> matrix;
219+
220+
if (const auto* mstate = self.getMState())
221+
{
222+
const auto matrixSize = static_cast<sofa::linearalgebra::BaseMatrix::Index>(mstate->getMatrixSize());
223+
matrix.resize(matrixSize, matrixSize);
224+
225+
sofa::core::behavior::DefaultMultiMatrixAccessor accessor;
226+
accessor.addMechanicalState(mstate);
227+
accessor.setGlobalMatrix(&matrix);
228+
229+
auto mparams = *MechanicalParams::defaultInstance();
230+
mparams.setKFactor(1.).setMFactor(0.).setBFactor(0.);
231+
232+
self.addKToMatrix(&mparams, &accessor);
233+
}
234+
matrix.compress();
235+
236+
if (matrix.getColsValue().empty() || matrix.rowBegin.empty() || matrix.colsIndex.empty())
237+
return {};
238+
239+
return EigenMatrixMap(matrix.rows(), matrix.cols(), matrix.getColsValue().size(),
240+
(typename EigenMatrixMap::StorageIndex*)matrix.rowBegin.data(),
241+
(typename EigenMatrixMap::StorageIndex*)matrix.colsIndex.data(),
242+
matrix.colsValue.data());
243+
}, sofapython3::doc::forceField::assembleKMatrix);
244+
245+
PythonFactory::registerType<ForceField<TDOFType>>([](sofa::core::objectmodel::Base* object)
246+
{
247+
return py::cast(dynamic_cast<ForceField<TDOFType>*>(object));
248+
});
207249
}
208250

209251

210252
void moduleAddForceField(py::module &m) {
211-
declare_forcefield<Vec3dTypes>(m, "Vec3d");
212-
declare_forcefield<Vec2dTypes>(m, "Vec2d");
213-
declare_forcefield<Vec1dTypes>(m, "Vec1d");
214-
declare_forcefield<Rigid3dTypes>(m, "Rigid3d");
215-
declare_forcefield<Rigid2dTypes>(m, "Rigid2d");
253+
declare_forcefield<Vec3dTypes>(m);
254+
declare_forcefield<Vec2dTypes>(m);
255+
declare_forcefield<Vec1dTypes>(m);
256+
declare_forcefield<Vec6dTypes>(m);
257+
declare_forcefield<Rigid3dTypes>(m);
258+
declare_forcefield<Rigid2dTypes>(m);
216259
}
217260

218261
} // namespace sofapython3

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
namespace sofapython3 {
2929

3030
template<class TDOFType>
31-
class ForceField_Trampoline : public sofa::core::behavior::ForceField<TDOFType> {
31+
class ForceField_Trampoline : public sofa::core::behavior::ForceField<TDOFType> {
3232
public:
3333
SOFA_CLASS(ForceField_Trampoline, SOFA_TEMPLATE(sofa::core::behavior::ForceField, TDOFType));
3434
using sofa::core::behavior::ForceField<TDOFType>::mstate;

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField_doc.h‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,4 +25,20 @@ namespace sofapython3::doc::forceField
2525
static auto forceFieldClass = R"(
2626
An overridable class to create your own customized force field.
2727
)";
28+
29+
static constexpr const char* assembleKMatrix = R"(
30+
Assemble the stiffness matrix of a force field.
31+
32+
Note that this function is not free. It assembles the stiffness matrix whether or not it has been assembled previously
33+
to add it into the global system matrix. Besides, the function does not prevent side effects of a second matrix assembly
34+
in a single time step.
35+
36+
Typical usage example:
37+
FEM = root.addObject('HexahedronFEMForceField', name="FEM", youngModulus="4000", poissonRatio="0.3", method="large")
38+
...
39+
stiffness_matrix = self.force_field.assembleKMatrix()
40+
41+
Returns:
42+
A scipy.sparse.csr_matrix object representing the stiffness matrix of the force field
43+
)";
2844
}

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_Mass.cpp‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
/******************************************************************************
2+
* SofaPython3 plugin *
3+
* (c) 2021 CNRS, University of Lille, INRIA *
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: contact@sofa-framework.org *
19+
******************************************************************************/
20+
21+
#include <pybind11/pybind11.h>
22+
#include <SofaPython3/Sofa/Core/Binding_Base.h>
23+
#include <SofaPython3/Sofa/Core/Binding_Mass.h>
24+
#include <SofaPython3/Sofa/Core/Binding_Mass_doc.h>
25+
#include <SofaPython3/PythonFactory.h>
26+
27+
#include <sofa/core/behavior/MechanicalState.h>
28+
#include <sofa/core/behavior/Mass.h>
29+
#include <sofa/core/MechanicalParams.h>
30+
#include <sofa/core/behavior/DefaultMultiMatrixAccessor.h>
31+
#include <sofa/linearalgebra/CompressedRowSparseMatrix.h>
32+
33+
#include <pybind11/eigen.h>
34+
35+
/// Makes an alias for the pybind11 namespace to increase readability.
36+
namespace py { using namespace pybind11; }
37+
38+
namespace sofapython3
39+
{
40+
41+
using sofa::core::behavior::Mass;
42+
43+
template<class TDOFType>
44+
void declare_mass(py::module &m) {
45+
const std::string pyclass_name = std::string("Mass") + TDOFType::Name();
46+
py::class_<Mass<TDOFType>, sofa::core::behavior::ForceField<TDOFType>, py_shared_ptr<Mass<TDOFType>>> f(m, pyclass_name.c_str(), sofapython3::doc::mass::massClass);
47+
48+
using Real = typename TDOFType::Real;
49+
using EigenSparseMatrix = Eigen::SparseMatrix<typename TDOFType::Real, Eigen::RowMajor>;
50+
using EigenMatrixMap = Eigen::Map<EigenSparseMatrix>;
51+
52+
f.def("assembleMMatrix", [](Mass<TDOFType>& self) -> EigenSparseMatrix
53+
{
54+
sofa::linearalgebra::CompressedRowSparseMatrix<Real> matrix;
55+
56+
if (const auto* mstate = self.getMState())
57+
{
58+
const auto matrixSize = static_cast<sofa::linearalgebra::BaseMatrix::Index>(mstate->getMatrixSize());
59+
matrix.resize(matrixSize, matrixSize);
60+
61+
sofa::core::behavior::DefaultMultiMatrixAccessor accessor;
62+
accessor.addMechanicalState(mstate);
63+
accessor.setGlobalMatrix(&matrix);
64+
65+
auto mparams = *sofa::core::MechanicalParams::defaultInstance();
66+
mparams.setKFactor(0.).setMFactor(1.).setBFactor(0.);
67+
68+
self.addMToMatrix(&mparams, &accessor);
69+
}
70+
matrix.compress();
71+
72+
if (matrix.getColsValue().empty() || matrix.rowBegin.empty() || matrix.colsIndex.empty())
73+
return {};
74+
75+
return EigenMatrixMap(matrix.rows(), matrix.cols(), matrix.getColsValue().size(),
76+
(typename EigenMatrixMap::StorageIndex*)matrix.rowBegin.data(),
77+
(typename EigenMatrixMap::StorageIndex*)matrix.colsIndex.data(),
78+
matrix.colsValue.data());
79+
}, sofapython3::doc::mass::assembleMMatrix);
80+
81+
PythonFactory::registerType<Mass<TDOFType>>([](sofa::core::objectmodel::Base* object)
82+
{
83+
return py::cast(dynamic_cast<Mass<TDOFType>*>(object));
84+
});
85+
}
86+
87+
88+
void moduleAddMass(py::module &m) {
89+
declare_mass<sofa::defaulttype::Vec3dTypes>(m);
90+
declare_mass<sofa::defaulttype::Vec2dTypes>(m);
91+
declare_mass<sofa::defaulttype::Vec1dTypes>(m);
92+
declare_mass<sofa::defaulttype::Vec6dTypes>(m);
93+
declare_mass<sofa::defaulttype::Rigid3dTypes>(m);
94+
declare_mass<sofa::defaulttype::Rigid2dTypes>(m);
95+
}
96+
97+
} // namespace sofapython3

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_Mass.h‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
/******************************************************************************
2+
* SofaPython3 plugin *
3+
* (c) 2021 CNRS, University of Lille, INRIA *
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: contact@sofa-framework.org *
19+
******************************************************************************/
20+
21+
#pragma once
22+
23+
#include <pybind11/pybind11.h>
24+
25+
namespace sofapython3 {
26+
27+
void moduleAddMass(pybind11::module &m);
28+
29+
} /// namespace sofapython3

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_Mass_doc.h‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
/******************************************************************************
2+
* SofaPython3 plugin *
3+
* (c) 2021 CNRS, University of Lille, INRIA *
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: contact@sofa-framework.org *
19+
******************************************************************************/
20+
21+
#pragma once
22+
23+
namespace sofapython3::doc::mass
24+
{
25+
static auto massClass = R"(
26+
Mass is an API dedicated to the control of a mass in SOFA.
27+
)";
28+
29+
static constexpr const char* assembleMMatrix = R"(
30+
Assemble the mass matrix of a mass.
31+
32+
Note that this function is not free. It assembles the mass matrix whether or not it has been assembled previously
33+
to add it into the global system matrix. Besides, the function does not prevent side effects of a second matrix assembly
34+
in a single time step.
35+
36+
Typical usage example:
37+
mass = root.addObject('MeshMatrixMass', name="mass", totalMass="320")
38+
...
39+
mass_matrix = mass.assembleMMatrix()
40+
41+
Returns:
42+
A scipy.sparse.csr_matrix object representing the mass matrix of the mass
43+
)";
44+
}

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@ set(HEADER_FILES
1919
${CMAKE_CURRENT_SOURCE_DIR}/Binding_DataEngine_doc.h
2020
${CMAKE_CURRENT_SOURCE_DIR}/Binding_ForceField.h
2121
${CMAKE_CURRENT_SOURCE_DIR}/Binding_ForceField_doc.h
22+
${CMAKE_CURRENT_SOURCE_DIR}/Binding_Mass.h
23+
${CMAKE_CURRENT_SOURCE_DIR}/Binding_Mass_doc.h
2224
${CMAKE_CURRENT_SOURCE_DIR}/Binding_ObjectFactory.h
2325
${CMAKE_CURRENT_SOURCE_DIR}/Binding_ObjectFactory_doc.h
2426
${CMAKE_CURRENT_SOURCE_DIR}/Binding_Node.h
@@ -58,6 +60,7 @@ set(SOURCE_FILES
5860
${CMAKE_CURRENT_SOURCE_DIR}/Data/Binding_DataLink.cpp
5961
${CMAKE_CURRENT_SOURCE_DIR}/Data/Binding_DataVectorString.cpp
6062
${CMAKE_CURRENT_SOURCE_DIR}/Binding_ForceField.cpp
63+
${CMAKE_CURRENT_SOURCE_DIR}/Binding_Mass.cpp
6164
${CMAKE_CURRENT_SOURCE_DIR}/Binding_ObjectFactory.cpp
6265
${CMAKE_CURRENT_SOURCE_DIR}/Binding_Node.cpp
6366
${CMAKE_CURRENT_SOURCE_DIR}/Binding_NodeIterator.cpp

‎bindings/Sofa/src/SofaPython3/Sofa/Core/Submodule_Core.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ using sofa::helper::logging::Message;
2929
#include <SofaPython3/Sofa/Core/Binding_BaseData.h>
3030
#include <SofaPython3/Sofa/Core/Binding_BaseCamera.h>
3131
#include <SofaPython3/Sofa/Core/Binding_ForceField.h>
32+
#include <SofaPython3/Sofa/Core/Binding_Mass.h>
3233
#include <SofaPython3/Sofa/Core/Binding_ContactListener.h>
3334
#include <SofaPython3/Sofa/Core/Binding_Context.h>
3435
#include <SofaPython3/Sofa/Core/Binding_Controller.h>
@@ -131,6 +132,7 @@ PYBIND11_MODULE(Core, core)
131132
moduleAddController(core);
132133
moduleAddDataEngine(core);
133134
moduleAddForceField(core);
135+
moduleAddMass(core);
134136
moduleAddObjectFactory(core);
135137
moduleAddNode(core);
136138
moduleAddNodeIterator(core);

‎bindings/Sofa/tests/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ set(PYTHON_FILES
1616
${CMAKE_CURRENT_SOURCE_DIR}/Core/BaseObject.py
1717
${CMAKE_CURRENT_SOURCE_DIR}/Core/Controller.py
1818
${CMAKE_CURRENT_SOURCE_DIR}/Core/ForceField.py
19+
${CMAKE_CURRENT_SOURCE_DIR}/Core/Mass.py
1920
${CMAKE_CURRENT_SOURCE_DIR}/Core/DataEngine.py
2021
${CMAKE_CURRENT_SOURCE_DIR}/Core/MyRestShapeForceField.py
2122
${CMAKE_CURRENT_SOURCE_DIR}/Core/BaseLink.py

‎bindings/Sofa/tests/Core/ForceField.py‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,3 +90,49 @@ def test_2_implicit_direct(self):
9090
root.particle.MO.rest_position.value - root.particle.MO.position.value), 0.26,
9191
"Passed threshold on step " + str(i) + ".")
9292
return
93+
94+
@staticmethod
95+
def simulate_beam(linear_solver_template):
96+
root = Sofa.Core.Node("rootNode")
97+
98+
root.addObject('DefaultAnimationLoop')
99+
100+
root.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward')
101+
root.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct')
102+
root.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select')
103+
root.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Projective')
104+
root.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic')
105+
root.addObject('RequiredPlugin', name='Sofa.Component.Mass')
106+
107+
root.addObject('EulerImplicitSolver', rayleighStiffness="0.1", rayleighMass="0.1")
108+
root.addObject('SparseLDLSolver', applyPermutation="false", template=linear_solver_template)
109+
110+
root.addObject('MechanicalObject', name="DoFs")
111+
root.addObject('UniformMass', name="mass", totalMass="320")
112+
root.addObject('RegularGridTopology', name="grid", nx="4", ny="4", nz="20", xmin="-9", xmax="-6", ymin="0", ymax="3", zmin="0", zmax="19")
113+
root.addObject('BoxROI', name="box", box=[-10, -1, -0.0001, -5, 4, 0.0001])
114+
root.addObject('FixedConstraint', indices="@box.indices")
115+
root.addObject('HexahedronFEMForceField', name="FEM", youngModulus="4000", poissonRatio="0.3", method="large")
116+
117+
Sofa.Simulation.init(root)
118+
Sofa.Simulation.animate(root, 0.0001)
119+
120+
return root
121+
122+
def test_stiffness_matrix_access_scalar(self):
123+
124+
root = self.simulate_beam("CompressedRowSparseMatrixd")
125+
K = root.FEM.assembleKMatrix()
126+
127+
self.assertEqual(K.ndim, 2)
128+
self.assertEqual(K.shape, (960, 960))
129+
self.assertEqual(K.nnz, 52200)
130+
131+
def test_stiffness_matrix_access_blocks3x3(self):
132+
133+
root = self.simulate_beam("CompressedRowSparseMatrixMat3x3d")
134+
K = root.FEM.assembleKMatrix()
135+
136+
self.assertEqual(K.ndim, 2)
137+
self.assertEqual(K.shape, (960, 960))
138+
self.assertEqual(K.nnz, 52200)

0 commit comments

Comments
 (0)