Skip to content

Commit 27f9f55

Browse files
alxbilgeralxbilger
committed
Add the mass matrix access
1 parent 5c23226 commit 27f9f55

9 files changed

Lines changed: 253 additions & 2 deletions

File tree

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

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ namespace sofapython3
5252
using sofa::defaulttype::Vec3dTypes;
5353
using sofa::defaulttype::Vec2dTypes;
5454
using sofa::defaulttype::Vec1dTypes;
55+
using sofa::defaulttype::Vec6dTypes;
5556
using sofa::defaulttype::Rigid3dTypes;
5657
using sofa::defaulttype::Rigid2dTypes;
5758

@@ -232,6 +233,9 @@ namespace sofapython3
232233
}
233234
matrix.compress();
234235

236+
if (matrix.getColsValue().empty() || matrix.rowBegin.empty() || matrix.colsIndex.empty())
237+
return {};
238+
235239
return EigenMatrixMap(matrix.rows(), matrix.cols(), matrix.getColsValue().size(),
236240
(typename EigenMatrixMap::StorageIndex*)matrix.rowBegin.data(),
237241
(typename EigenMatrixMap::StorageIndex*)matrix.colsIndex.data(),
@@ -249,6 +253,7 @@ void moduleAddForceField(py::module &m) {
249253
declare_forcefield<Vec3dTypes>(m);
250254
declare_forcefield<Vec2dTypes>(m);
251255
declare_forcefield<Vec1dTypes>(m);
256+
declare_forcefield<Vec6dTypes>(m);
252257
declare_forcefield<Rigid3dTypes>(m);
253258
declare_forcefield<Rigid2dTypes>(m);
254259
}

‎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_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);

‎examples/access_mass_matrix.py‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
# Required import for python
2+
import Sofa
3+
import SofaRuntime
4+
from Sofa import SofaLinearSolver
5+
from scipy import sparse
6+
from scipy import linalg
7+
from matplotlib import pyplot as plt
8+
import numpy as np
9+
10+
exportCSV = False
11+
showImage = False
12+
13+
# Function called when the scene graph is being created
14+
def createScene(root):
15+
16+
root.addObject('VisualStyle', displayFlags="showBehaviorModels showForceFields")
17+
18+
root.addObject('DefaultAnimationLoop')
19+
root.addObject('DefaultVisualManagerLoop')
20+
21+
root.addObject('EulerImplicitSolver', rayleighStiffness="0.1", rayleighMass="0.1")
22+
root.addObject('SparseLDLSolver', applyPermutation="false", template="CompressedRowSparseMatrixd")
23+
24+
root.addObject('MechanicalObject', name="DoFs")
25+
mass = root.addObject('MeshMatrixMass', name="mass", totalMass="320")
26+
root.addObject('RegularGridTopology', name="grid", nx="4", ny="4", nz="20", xmin="-9", xmax="-6", ymin="0", ymax="3", zmin="0", zmax="19")
27+
root.addObject('BoxROI', name="box", box="-10 -1 -0.0001 -5 4 0.0001")
28+
root.addObject('FixedConstraint', indices="@box.indices")
29+
root.addObject('HexahedronFEMForceField', name="FEM", youngModulus="4000", poissonRatio="0.3", method="large")
30+
31+
root.addObject(MatrixAccessController('MatrixAccessor', name='matrixAccessor', mass=mass))
32+
33+
return root
34+
35+
36+
class MatrixAccessController(Sofa.Core.Controller):
37+
38+
39+
def __init__(self, *args, **kwargs):
40+
Sofa.Core.Controller.__init__(self, *args, **kwargs)
41+
self.mass = kwargs.get("mass")
42+
43+
def onAnimateEndEvent(self, event):
44+
mass_matrix = self.mass.assembleMMatrix()
45+
46+
print('====================================')
47+
print('Stiffness matrix')
48+
print('====================================')
49+
print('dtype: ' + str(mass_matrix.dtype))
50+
print('shape: ' + str(mass_matrix.shape))
51+
print('ndim: ' + str(mass_matrix.ndim))
52+
print('nnz: ' + str(mass_matrix.nnz))
53+
print('norm: ' + str(sparse.linalg.norm(mass_matrix)))
54+
55+
if exportCSV:
56+
np.savetxt('mass.csv', mass_matrix.toarray(), delimiter=',')
57+
if showImage:
58+
plt.imshow(mass_matrix.toarray(), interpolation='nearest', cmap='gist_gray')
59+
plt.show()
60+

‎examples/access_stiffness_matrix.py‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,11 @@
44
from Sofa import SofaLinearSolver
55
from scipy import sparse
66
from scipy import linalg
7+
from matplotlib import pyplot as plt
8+
import numpy as np
9+
10+
exportCSV = False
11+
showImage = False
712

813
# Function called when the scene graph is being created
914
def createScene(root):
@@ -17,7 +22,7 @@ def createScene(root):
1722
root.addObject('SparseLDLSolver', applyPermutation="false", template="CompressedRowSparseMatrixd")
1823

1924
root.addObject('MechanicalObject', name="DoFs")
20-
root.addObject('UniformMass', name="mass", totalMass="320")
25+
root.addObject('MeshMatrixMass', name="mass", totalMass="320")
2126
root.addObject('RegularGridTopology', name="grid", nx="4", ny="4", nz="20", xmin="-9", xmax="-6", ymin="0", ymax="3", zmin="0", zmax="19")
2227
root.addObject('BoxROI', name="box", box="-10 -1 -0.0001 -5 4 0.0001")
2328
root.addObject('FixedConstraint', indices="@box.indices")
@@ -47,3 +52,9 @@ def onAnimateEndEvent(self, event):
4752
print('nnz: ' + str(stiffness_matrix.nnz))
4853
print('norm: ' + str(sparse.linalg.norm(stiffness_matrix)))
4954

55+
if exportCSV:
56+
np.savetxt('stiffness.csv', stiffness_matrix.toarray(), delimiter=',')
57+
if showImage:
58+
plt.imshow(stiffness_matrix.toarray(), interpolation='nearest', cmap='gist_gray')
59+
plt.show()
60+

0 commit comments

Comments
 (0)