Skip to content

Commit dffc90c

Browse files
jnbrunetjnbrunet
committed
[Bindings] Add MechanicalOperations bindings
Signed-off-by: Jean-Nicolas Brunet <[email protected]>
1 parent 5a6c82f commit dffc90c

File tree

6 files changed

+246
-0
lines changed

6 files changed

+246
-0
lines changed

‎bindings/Modules/src/SofaPython3/SofaSimulationCore/Binding_MechanicalOperations.cpp

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
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: [email protected] *
19+
******************************************************************************/
20+
21+
#include <pybind11/pybind11.h>
22+
23+
#include <sofa/simulation/MechanicalOperations.h>
24+
25+
namespace py { using namespace pybind11; }
26+
27+
namespace sofapython3 {
28+
29+
void moduleAddMechanicalOperations(pybind11::module& m)
30+
{
31+
using namespace sofa::simulation::common;
32+
33+
py::class_<MechanicalOperations> c (m, "MechanicalOperations");
34+
c.def(py::init<const sofa::core::MechanicalParams*, sofa::core::objectmodel::BaseContext*, bool>(), py::arg("mparams"), py::arg("ctx"), py::arg("precomputedTraversalOrder")=false);
35+
36+
// Mechanical Vector operations
37+
c.def("propagateDx", &MechanicalOperations::propagateDx, py::arg("dx"), py::arg("ignore_flag") = false, "Propagate the given displacement through all mappings");
38+
c.def("propagateDxAndResetDf", &MechanicalOperations::propagateDxAndResetDf, py::arg("dx"), py::arg("df"), "Propagate the given displacement through all mappings and reset the current force delta");
39+
c.def("propagateX", &MechanicalOperations::propagateX, py::arg("x"), "Propagate the given position through all mappings");
40+
c.def("propagateV", &MechanicalOperations::propagateV, py::arg("v"), "Propagate the given velocity through all mappings");
41+
c.def("propagateXAndV", &MechanicalOperations::propagateXAndV, py::arg("x"), py::arg("v"), "Propagate the given position and velocity through all mappings");
42+
c.def("propagateXAndResetF", &MechanicalOperations::propagateXAndResetF, py::arg("x"), py::arg("f"), "Propagate the given position through all mappings and reset the current force delta");
43+
c.def("projectPosition", &MechanicalOperations::projectPosition, py::arg("x"), py::arg("time") = 0., "Apply projective constraints to the given position vector");
44+
c.def("projectVelocity", &MechanicalOperations::projectVelocity, py::arg("v"), py::arg("time") = 0., "Apply projective constraints to the given velocity vector");
45+
c.def("projectResponse", [](MechanicalOperations & self, sofa::core::MultiVecDerivId dx) {
46+
self.projectResponse(dx);
47+
}, py::arg("dx"), "Apply projective constraints to the given vector");
48+
c.def("projectPositionAndVelocity", &MechanicalOperations::projectPositionAndVelocity, py::arg("x"), py::arg("v"), py::arg("time") = 0., "Apply projective constraints to the given position and velocity vectors");
49+
c.def("addMdx", &MechanicalOperations::addMdx, py::arg("res"), py::arg("dx"), py::arg("factor") = 1.0, "res += factor M.dx");
50+
c.def("integrateVelocity", &MechanicalOperations::integrateVelocity, py::arg("res"), py::arg("x"), py::arg("v"), py::arg("dt"), "res = x + v.dt");
51+
c.def("accFromF", &MechanicalOperations::accFromF, py::arg("a"), py::arg("f"), "a = M^-1 . f");
52+
c.def("computeEnergy", &MechanicalOperations::computeEnergy, py::arg("kineticEnergy"), py::arg("potentialEnergy"), "Compute Energy");
53+
c.def("computeForce", py::overload_cast<sofa::core::MultiVecDerivId, bool, bool, bool>(&MechanicalOperations::computeForce), py::arg("result"), py::arg("clear")=true, py::arg("accumulate")=true, py::arg("neglectingCompliance")=true, "Compute the current force (given the latest propagated position and velocity)");
54+
c.def("computeForce", py::overload_cast<SReal, sofa::core::MultiVecDerivId, sofa::core::MultiVecCoordId, sofa::core::MultiVecDerivId, bool>(&MechanicalOperations::computeForce), py::arg("t"), py::arg("f"), py::arg("x"), py::arg("v"), py::arg("neglectingCompliance")=true, "Compute f(x,v) at time t. Parameters x and v not const due to propagation through mappings.");
55+
c.def("computeDf", &MechanicalOperations::computeDf, py::arg("df"), py::arg("clear")=true, py::arg("accumulate")=true, "Compute the current force delta (given the latest propagated displacement)");
56+
c.def("computeDfV", &MechanicalOperations::computeDfV, py::arg("df"), py::arg("clear")=true, py::arg("accumulate")=true, "Compute the current force delta (given the latest propagated velocity)");
57+
c.def("addMBKdx", &MechanicalOperations::addMBKdx, py::arg("df"), py::arg("m"), py::arg("b"), py::arg("k"), py::arg("clear")=true, py::arg("accumulate")=true, "accumulate $ df += (m M + b B + k K) dx $ (given the latest propagated displacement)");
58+
c.def("addMBKv", &MechanicalOperations::addMBKv, py::arg("df"), py::arg("m"), py::arg("b"), py::arg("k"), py::arg("clear")=true, py::arg("accumulate")=true, "accumulate $ df += (m M + b B + k K) velocity $");
59+
c.def("addSeparateGravity", &MechanicalOperations::addSeparateGravity, py::arg("dt"), py::arg("result") = sofa::core::VecDerivId::velocity(), "Add dt*Gravity to the velocity");
60+
c.def("computeContactForce", &MechanicalOperations::computeContactForce, py::arg("result"));
61+
c.def("computeContactDf", &MechanicalOperations::computeContactDf, py::arg("df"));
62+
c.def("computeAcc", &MechanicalOperations::computeAcc, py::arg("t"), py::arg("a"), py::arg("x"), py::arg("v"), "Compute a(x,v) at time t. Parameters x and v not const due to propagation through mappings.");
63+
c.def("computeContactAcc", &MechanicalOperations::computeContactAcc, py::arg("t"), py::arg("a"), py::arg("x"), py::arg("v"), "Parameters x and v not const due to propagation through mappings.");
64+
65+
// Matrix operations using LinearSolver components
66+
c.def("m_resetSystem", &MechanicalOperations::m_resetSystem);
67+
c.def("m_setSystemMBKMatrix", &MechanicalOperations::m_setSystemMBKMatrix, py::arg("m"), py::arg("b"), py::arg("k"));
68+
c.def("m_setSystemRHVector", &MechanicalOperations::m_setSystemRHVector, py::arg("f"));
69+
c.def("m_setSystemLHVector", &MechanicalOperations::m_setSystemLHVector, py::arg("dx"));
70+
c.def("m_solveSystem", &MechanicalOperations::m_solveSystem);
71+
72+
// Constraints
73+
c.def("solveConstraint", &MechanicalOperations::solveConstraint, py::arg("id"), py::arg("order"));
74+
75+
// Matrix operations
76+
c.def("getMatrixDimension", py::overload_cast<sofa::Size*, sofa::Size*, sofa::core::behavior::MultiMatrixAccessor*>(&MechanicalOperations::getMatrixDimension), py::arg("nrows"), py::arg("ncols"), py::arg("matrix") = nullptr);
77+
c.def("getMatrixDimension", py::overload_cast<sofa::core::behavior::MultiMatrixAccessor*>(&MechanicalOperations::getMatrixDimension), py::arg("matrix"));
78+
c.def("addMBK_ToMatrix", &MechanicalOperations::addMBK_ToMatrix, py::arg("matrix"), py::arg("mFact"), py::arg("bFact"), py::arg("kFact"));
79+
c.def("addSubMBK_ToMatrix", &MechanicalOperations::addSubMBK_ToMatrix, py::arg("matrix"), py::arg("subMatrixIndex"), py::arg("mFact"), py::arg("bFact"), py::arg("kFact"));
80+
c.def("multiVector2BaseVector", &MechanicalOperations::multiVector2BaseVector, py::arg("src"), py::arg("dest"), py::arg("matrix"));
81+
c.def("baseVector2MultiVector", &MechanicalOperations::baseVector2MultiVector, py::arg("src"), py::arg("dest"), py::arg("matrix"));
82+
c.def("multiVectorPeqBaseVector", &MechanicalOperations::multiVectorPeqBaseVector, py::arg("dest"), py::arg("src"), py::arg("matrix"));
83+
}
84+
85+
}

‎bindings/Modules/src/SofaPython3/SofaSimulationCore/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
project(Bindings.Modules.SofaSimulationCore)
22

33
set(SOURCE_FILES
4+
${CMAKE_CURRENT_SOURCE_DIR}/Binding_MechanicalOperations.cpp
45
${CMAKE_CURRENT_SOURCE_DIR}/Binding_VectorOperations.cpp
56
${CMAKE_CURRENT_SOURCE_DIR}/Module_SofaSimulationCore.cpp
67
)

‎bindings/Modules/src/SofaPython3/SofaSimulationCore/Module_SofaSimulationCore.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,10 +26,12 @@ namespace py { using namespace pybind11; }
2626
namespace sofapython3
2727
{
2828

29+
void moduleAddMechanicalOperations(pybind11::module& m);
2930
void moduleAddVectorOperations(pybind11::module& m);
3031

3132
PYBIND11_MODULE(SofaSimulationCore, m)
3233
{
34+
moduleAddMechanicalOperations(m);
3335
moduleAddVectorOperations(m);
3436
}
3537

‎bindings/Modules/tests/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ set(SOURCE_FILES
77
set(PYTHON_FILES
88
${CMAKE_CURRENT_SOURCE_DIR}/SofaDeformable/LinearSpring.py
99
${CMAKE_CURRENT_SOURCE_DIR}/SofaDeformable/SpringForceField.py
10+
${CMAKE_CURRENT_SOURCE_DIR}/SofaSimulationCore/MechanicalOperations.py
1011
${CMAKE_CURRENT_SOURCE_DIR}/SofaSimulationCore/VectorOperations.py
1112
)
1213

‎bindings/Modules/tests/SofaSimulationCore/MechanicalOperations.py

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
import unittest
2+
import Sofa
3+
from Sofa import SofaSimulationCore
4+
import numpy as np
5+
6+
"""
7+
In order to test out a good subset of the Mechanical operations, let's do here do a Newton-Raphson solver in python.
8+
"""
9+
10+
11+
class StaticOdeSolver (Sofa.Core.OdeSolver):
12+
def solve(self, _, dt, X, V):
13+
mparams = Sofa.Core.MechanicalParams()
14+
vop = SofaSimulationCore.VectorOperations(self.getContext())
15+
mop = SofaSimulationCore.MechanicalOperations(mparams, self.getContext())
16+
17+
# Allocate the solution vector
18+
dx = Sofa.Core.VecId.dx()
19+
vop.v_realloc(dx, interactionForceField=True, propagate=True)
20+
vop.v_clear(dx)
21+
22+
# Compute the residual
23+
F = Sofa.Core.VecId.force()
24+
mop.computeForce(result=F, clear=True, accumulate=True);
25+
mop.projectResponse(F)
26+
vop.v_dot(F, F)
27+
F_norm = np.sqrt(vop.finish())
28+
29+
# Assemble the system
30+
mop.m_setSystemMBKMatrix(m=0, b=0, k=-1)
31+
mop.m_setSystemRHVector(F)
32+
mop.m_setSystemLHVector(dx)
33+
34+
# Solve the system
35+
mop.m_solveSystem()
36+
vop.v_dot(dx, dx)
37+
dx_norm = np.sqrt(vop.finish())
38+
39+
# Propagate the solution
40+
vop.v_peq(X, dx) # X += dx
41+
42+
# Solve the constraints
43+
# todo: Bind ConstraintParams
44+
# mop.solveConstraint(X, ConstraintParams.ConstOrder.POS)
45+
46+
print(f"Solved with |F| = {F_norm} and |dx| = {dx_norm}")
47+
48+
49+
class MechanicalOperations(unittest.TestCase):
50+
def test_static_solver(self):
51+
root = Sofa.Core.Node()
52+
createScene(root)
53+
Sofa.Simulation.init(root)
54+
for _ in range(5):
55+
Sofa.Simulation.animate(root, root.dt.value)
56+
57+
middle_node_index = 52
58+
solution_of_middle_node = np.array([0, 10.0953, -0.285531])
59+
self.assertLess(np.linalg.norm(solution_of_middle_node - root.mechanics.neumann.mo.position.array()[middle_node_index])/np.linalg.norm(solution_of_middle_node), 1e-5)
60+
61+
62+
def createScene(root):
63+
w, l = 2, 10
64+
nx, ny = 3, 9
65+
dx, dy = w/nx/2, l/ny/2
66+
root.addObject('RequiredPlugin', pluginName='SofaLoader SofaBoundaryCondition SofaEngine SofaSimpleFem SofaImplicitOdeSolver SofaOpenglVisual SofaSparseSolver SofaTopologyMapping')
67+
root.addObject('MeshObjLoader', name='surface', filename='mesh/cylinder.obj')
68+
root.addObject('SparseGridTopology', src='@surface', name='grid', n=[nx, ny, nx])
69+
root.addChild('mechanics')
70+
root.mechanics.addObject(StaticOdeSolver(name='solver'))
71+
root.mechanics.addObject('SparseLDLSolver')
72+
root.mechanics.addObject('MechanicalObject', name='mo', src='@../grid')
73+
root.mechanics.addObject('HexahedronSetTopologyContainer', name='hexa_topology', hexahedra='@../grid.hexahedra')
74+
root.mechanics.addObject('HexahedronFEMForceField', youngModulus=3000, poissonRatio=0.3)
75+
root.mechanics.addObject('BoxROI', name='fixed_roi', box=[-1-dx,0-dx,-1-dx, 1+dx, 0+dx, 1+dx], drawBoxes=True)
76+
root.mechanics.addObject('FixedConstraint', indices='@fixed_roi.indices')
77+
root.mechanics.addChild('visual')
78+
root.mechanics.visual.addObject('OglModel', name='vm', src='@/surface')
79+
root.mechanics.visual.addObject('BarycentricMapping', applyRestPosition=True)
80+
root.mechanics.addChild('neumann')
81+
root.mechanics.neumann.addObject('MechanicalObject', name='mo')
82+
root.mechanics.neumann.addObject('QuadSetTopologyContainer', name='quad_topology')
83+
root.mechanics.neumann.addObject('QuadSetTopologyModifier')
84+
root.mechanics.neumann.addObject('QuadSetGeometryAlgorithms')
85+
root.mechanics.neumann.addObject('Hexa2QuadTopologicalMapping', input='@../hexa_topology', output='@quad_topology')
86+
root.mechanics.neumann.addObject('SubsetMapping', applyRestPosition=True, input='@../mo', output='@./mo', indices='@quad_topology.points')
87+
root.mechanics.neumann.addObject('BoxROI', name='top_roi', quad='@quad_topology.quads', src='@mo', box=[-1-dx,10-dx,-1-dx, 1+dx, 10+dx, 1+dx], drawBoxes=True)
88+
root.mechanics.neumann.addObject('QuadPressureForceField', pressure=[0, 0, -1], quadList='@top_roi.quadIndices')

‎examples/StaticSolver.py

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
import Sofa
2+
from Sofa import SofaSimulationCore
3+
import numpy as np
4+
5+
class StaticOdeSolver (Sofa.Core.OdeSolver):
6+
def solve(self, _, dt, X, V):
7+
mparams = Sofa.Core.MechanicalParams()
8+
vop = SofaSimulationCore.VectorOperations(self.getContext())
9+
mop = SofaSimulationCore.MechanicalOperations(mparams, self.getContext())
10+
11+
# Allocate the solution vector
12+
dx = Sofa.Core.VecId.dx()
13+
vop.v_realloc(dx, interactionForceField=True, propagate=True)
14+
vop.v_clear(dx)
15+
16+
# Compute the residual
17+
F = Sofa.Core.VecId.force()
18+
mop.computeForce(result=F, clear=True, accumulate=True);
19+
mop.projectResponse(F)
20+
vop.v_dot(F, F)
21+
F_norm = np.sqrt(vop.finish())
22+
23+
# Assemble the system
24+
mop.m_setSystemMBKMatrix(m=0, b=0, k=-1)
25+
mop.m_setSystemRHVector(F)
26+
mop.m_setSystemLHVector(dx)
27+
28+
# Solve the system
29+
mop.m_solveSystem()
30+
vop.v_dot(dx, dx)
31+
dx_norm = np.sqrt(vop.finish())
32+
33+
# Propagate the solution
34+
vop.v_peq(X, dx) # X += dx
35+
36+
# Solve the constraints
37+
# todo: Bind ConstraintParams
38+
# mop.solveConstraint(X, ConstraintParams.ConstOrder.POS)
39+
40+
print(f"Solved with |F| = {F_norm} and |dx| = {dx_norm}")
41+
42+
def createScene(root):
43+
w, l = 2, 10
44+
nx, ny = 3, 9
45+
dx, dy = w/nx/2, l/ny/2
46+
root.addObject('RequiredPlugin', pluginName='SofaLoader SofaBoundaryCondition SofaEngine SofaSimpleFem SofaImplicitOdeSolver SofaOpenglVisual SofaSparseSolver SofaTopologyMapping')
47+
root.addObject('MeshObjLoader', name='surface', filename='mesh/cylinder.obj')
48+
root.addObject('SparseGridTopology', src='@surface', name='grid', n=[nx, ny, nx])
49+
root.addChild('mechanics')
50+
# root.mechanics.addObject('StaticSolver', name='solver', printLog=False)
51+
root.mechanics.addObject(StaticOdeSolver(name='solver'))
52+
root.mechanics.addObject('SparseLDLSolver')
53+
root.mechanics.addObject('MechanicalObject', name='mo', src='@../grid')
54+
root.mechanics.addObject('HexahedronSetTopologyContainer', name='hexa_topology', hexahedra='@../grid.hexahedra')
55+
root.mechanics.addObject('HexahedronFEMForceField', youngModulus=3000, poissonRatio=0.3)
56+
root.mechanics.addObject('BoxROI', name='fixed_roi', box=[-1-dx,0-dx,-1-dx, 1+dx, 0+dx, 1+dx], drawBoxes=True)
57+
root.mechanics.addObject('FixedConstraint', indices='@fixed_roi.indices')
58+
root.mechanics.addChild('visual')
59+
root.mechanics.visual.addObject('OglModel', name='vm', src='@/surface')
60+
root.mechanics.visual.addObject('BarycentricMapping', applyRestPosition=True)
61+
root.mechanics.addChild('neumann')
62+
root.mechanics.neumann.addObject('MechanicalObject', name='mo')
63+
root.mechanics.neumann.addObject('QuadSetTopologyContainer', name='quad_topology')
64+
root.mechanics.neumann.addObject('QuadSetTopologyModifier')
65+
root.mechanics.neumann.addObject('QuadSetGeometryAlgorithms')
66+
root.mechanics.neumann.addObject('Hexa2QuadTopologicalMapping', input='@../hexa_topology', output='@quad_topology')
67+
root.mechanics.neumann.addObject('SubsetMapping', applyRestPosition=True, input='@../mo', output='@./mo', indices='@quad_topology.points')
68+
root.mechanics.neumann.addObject('BoxROI', name='top_roi', quad='@quad_topology.quads', src='@mo', box=[-1-dx,10-dx,-1-dx, 1+dx, 10+dx, 1+dx], drawBoxes=True)
69+
root.mechanics.neumann.addObject('QuadPressureForceField', pressure=[0, 0, -1], quadList='@top_roi.quadIndices')

0 commit comments

Comments
 (0)