From dffc90c4555b9c58c4a38fa01ec6510f3b8c4a2c Mon Sep 17 00:00:00 2001 From: Jean-Nicolas Brunet Date: Thu, 27 May 2021 16:23:36 +0200 Subject: [PATCH] [Bindings] Add MechanicalOperations bindings Signed-off-by: Jean-Nicolas Brunet --- .../Binding_MechanicalOperations.cpp | 85 ++++++++++++++++++ .../SofaSimulationCore/CMakeLists.txt | 1 + .../Module_SofaSimulationCore.cpp | 2 + bindings/Modules/tests/CMakeLists.txt | 1 + .../MechanicalOperations.py | 88 +++++++++++++++++++ examples/StaticSolver.py | 69 +++++++++++++++ 6 files changed, 246 insertions(+) create mode 100644 bindings/Modules/src/SofaPython3/SofaSimulationCore/Binding_MechanicalOperations.cpp create mode 100644 bindings/Modules/tests/SofaSimulationCore/MechanicalOperations.py create mode 100644 examples/StaticSolver.py diff --git a/bindings/Modules/src/SofaPython3/SofaSimulationCore/Binding_MechanicalOperations.cpp b/bindings/Modules/src/SofaPython3/SofaSimulationCore/Binding_MechanicalOperations.cpp new file mode 100644 index 000000000..a78fa1434 --- /dev/null +++ b/bindings/Modules/src/SofaPython3/SofaSimulationCore/Binding_MechanicalOperations.cpp @@ -0,0 +1,85 @@ +/****************************************************************************** +* 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 * +******************************************************************************/ + +#include + +#include + +namespace py { using namespace pybind11; } + +namespace sofapython3 { + +void moduleAddMechanicalOperations(pybind11::module& m) +{ + using namespace sofa::simulation::common; + + py::class_ c (m, "MechanicalOperations"); + c.def(py::init(), py::arg("mparams"), py::arg("ctx"), py::arg("precomputedTraversalOrder")=false); + + // Mechanical Vector operations + c.def("propagateDx", &MechanicalOperations::propagateDx, py::arg("dx"), py::arg("ignore_flag") = false, "Propagate the given displacement through all mappings"); + c.def("propagateDxAndResetDf", &MechanicalOperations::propagateDxAndResetDf, py::arg("dx"), py::arg("df"), "Propagate the given displacement through all mappings and reset the current force delta"); + c.def("propagateX", &MechanicalOperations::propagateX, py::arg("x"), "Propagate the given position through all mappings"); + c.def("propagateV", &MechanicalOperations::propagateV, py::arg("v"), "Propagate the given velocity through all mappings"); + c.def("propagateXAndV", &MechanicalOperations::propagateXAndV, py::arg("x"), py::arg("v"), "Propagate the given position and velocity through all mappings"); + c.def("propagateXAndResetF", &MechanicalOperations::propagateXAndResetF, py::arg("x"), py::arg("f"), "Propagate the given position through all mappings and reset the current force delta"); + c.def("projectPosition", &MechanicalOperations::projectPosition, py::arg("x"), py::arg("time") = 0., "Apply projective constraints to the given position vector"); + c.def("projectVelocity", &MechanicalOperations::projectVelocity, py::arg("v"), py::arg("time") = 0., "Apply projective constraints to the given velocity vector"); + c.def("projectResponse", [](MechanicalOperations & self, sofa::core::MultiVecDerivId dx) { + self.projectResponse(dx); + }, py::arg("dx"), "Apply projective constraints to the given vector"); + 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"); + c.def("addMdx", &MechanicalOperations::addMdx, py::arg("res"), py::arg("dx"), py::arg("factor") = 1.0, "res += factor M.dx"); + c.def("integrateVelocity", &MechanicalOperations::integrateVelocity, py::arg("res"), py::arg("x"), py::arg("v"), py::arg("dt"), "res = x + v.dt"); + c.def("accFromF", &MechanicalOperations::accFromF, py::arg("a"), py::arg("f"), "a = M^-1 . f"); + c.def("computeEnergy", &MechanicalOperations::computeEnergy, py::arg("kineticEnergy"), py::arg("potentialEnergy"), "Compute Energy"); + c.def("computeForce", py::overload_cast(&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)"); + c.def("computeForce", py::overload_cast(&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."); + 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)"); + 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)"); + 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)"); + 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 $"); + c.def("addSeparateGravity", &MechanicalOperations::addSeparateGravity, py::arg("dt"), py::arg("result") = sofa::core::VecDerivId::velocity(), "Add dt*Gravity to the velocity"); + c.def("computeContactForce", &MechanicalOperations::computeContactForce, py::arg("result")); + c.def("computeContactDf", &MechanicalOperations::computeContactDf, py::arg("df")); + 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."); + 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."); + + // Matrix operations using LinearSolver components + c.def("m_resetSystem", &MechanicalOperations::m_resetSystem); + c.def("m_setSystemMBKMatrix", &MechanicalOperations::m_setSystemMBKMatrix, py::arg("m"), py::arg("b"), py::arg("k")); + c.def("m_setSystemRHVector", &MechanicalOperations::m_setSystemRHVector, py::arg("f")); + c.def("m_setSystemLHVector", &MechanicalOperations::m_setSystemLHVector, py::arg("dx")); + c.def("m_solveSystem", &MechanicalOperations::m_solveSystem); + + // Constraints + c.def("solveConstraint", &MechanicalOperations::solveConstraint, py::arg("id"), py::arg("order")); + + // Matrix operations + c.def("getMatrixDimension", py::overload_cast(&MechanicalOperations::getMatrixDimension), py::arg("nrows"), py::arg("ncols"), py::arg("matrix") = nullptr); + c.def("getMatrixDimension", py::overload_cast(&MechanicalOperations::getMatrixDimension), py::arg("matrix")); + c.def("addMBK_ToMatrix", &MechanicalOperations::addMBK_ToMatrix, py::arg("matrix"), py::arg("mFact"), py::arg("bFact"), py::arg("kFact")); + c.def("addSubMBK_ToMatrix", &MechanicalOperations::addSubMBK_ToMatrix, py::arg("matrix"), py::arg("subMatrixIndex"), py::arg("mFact"), py::arg("bFact"), py::arg("kFact")); + c.def("multiVector2BaseVector", &MechanicalOperations::multiVector2BaseVector, py::arg("src"), py::arg("dest"), py::arg("matrix")); + c.def("baseVector2MultiVector", &MechanicalOperations::baseVector2MultiVector, py::arg("src"), py::arg("dest"), py::arg("matrix")); + c.def("multiVectorPeqBaseVector", &MechanicalOperations::multiVectorPeqBaseVector, py::arg("dest"), py::arg("src"), py::arg("matrix")); +} + +} \ No newline at end of file diff --git a/bindings/Modules/src/SofaPython3/SofaSimulationCore/CMakeLists.txt b/bindings/Modules/src/SofaPython3/SofaSimulationCore/CMakeLists.txt index 61c04e8e5..bf261d979 100644 --- a/bindings/Modules/src/SofaPython3/SofaSimulationCore/CMakeLists.txt +++ b/bindings/Modules/src/SofaPython3/SofaSimulationCore/CMakeLists.txt @@ -1,6 +1,7 @@ project(Bindings.Modules.SofaSimulationCore) set(SOURCE_FILES + ${CMAKE_CURRENT_SOURCE_DIR}/Binding_MechanicalOperations.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Binding_VectorOperations.cpp ${CMAKE_CURRENT_SOURCE_DIR}/Module_SofaSimulationCore.cpp ) diff --git a/bindings/Modules/src/SofaPython3/SofaSimulationCore/Module_SofaSimulationCore.cpp b/bindings/Modules/src/SofaPython3/SofaSimulationCore/Module_SofaSimulationCore.cpp index 40c26618a..b222b7746 100644 --- a/bindings/Modules/src/SofaPython3/SofaSimulationCore/Module_SofaSimulationCore.cpp +++ b/bindings/Modules/src/SofaPython3/SofaSimulationCore/Module_SofaSimulationCore.cpp @@ -26,10 +26,12 @@ namespace py { using namespace pybind11; } namespace sofapython3 { +void moduleAddMechanicalOperations(pybind11::module& m); void moduleAddVectorOperations(pybind11::module& m); PYBIND11_MODULE(SofaSimulationCore, m) { + moduleAddMechanicalOperations(m); moduleAddVectorOperations(m); } diff --git a/bindings/Modules/tests/CMakeLists.txt b/bindings/Modules/tests/CMakeLists.txt index a18eb58d5..01374e6d0 100644 --- a/bindings/Modules/tests/CMakeLists.txt +++ b/bindings/Modules/tests/CMakeLists.txt @@ -7,6 +7,7 @@ set(SOURCE_FILES set(PYTHON_FILES ${CMAKE_CURRENT_SOURCE_DIR}/SofaDeformable/LinearSpring.py ${CMAKE_CURRENT_SOURCE_DIR}/SofaDeformable/SpringForceField.py + ${CMAKE_CURRENT_SOURCE_DIR}/SofaSimulationCore/MechanicalOperations.py ${CMAKE_CURRENT_SOURCE_DIR}/SofaSimulationCore/VectorOperations.py ) diff --git a/bindings/Modules/tests/SofaSimulationCore/MechanicalOperations.py b/bindings/Modules/tests/SofaSimulationCore/MechanicalOperations.py new file mode 100644 index 000000000..502b5f9cd --- /dev/null +++ b/bindings/Modules/tests/SofaSimulationCore/MechanicalOperations.py @@ -0,0 +1,88 @@ +import unittest +import Sofa +from Sofa import SofaSimulationCore +import numpy as np + +""" +In order to test out a good subset of the Mechanical operations, let's do here do a Newton-Raphson solver in python. +""" + + +class StaticOdeSolver (Sofa.Core.OdeSolver): + def solve(self, _, dt, X, V): + mparams = Sofa.Core.MechanicalParams() + vop = SofaSimulationCore.VectorOperations(self.getContext()) + mop = SofaSimulationCore.MechanicalOperations(mparams, self.getContext()) + + # Allocate the solution vector + dx = Sofa.Core.VecId.dx() + vop.v_realloc(dx, interactionForceField=True, propagate=True) + vop.v_clear(dx) + + # Compute the residual + F = Sofa.Core.VecId.force() + mop.computeForce(result=F, clear=True, accumulate=True); + mop.projectResponse(F) + vop.v_dot(F, F) + F_norm = np.sqrt(vop.finish()) + + # Assemble the system + mop.m_setSystemMBKMatrix(m=0, b=0, k=-1) + mop.m_setSystemRHVector(F) + mop.m_setSystemLHVector(dx) + + # Solve the system + mop.m_solveSystem() + vop.v_dot(dx, dx) + dx_norm = np.sqrt(vop.finish()) + + # Propagate the solution + vop.v_peq(X, dx) # X += dx + + # Solve the constraints + # todo: Bind ConstraintParams + # mop.solveConstraint(X, ConstraintParams.ConstOrder.POS) + + print(f"Solved with |F| = {F_norm} and |dx| = {dx_norm}") + + +class MechanicalOperations(unittest.TestCase): + def test_static_solver(self): + root = Sofa.Core.Node() + createScene(root) + Sofa.Simulation.init(root) + for _ in range(5): + Sofa.Simulation.animate(root, root.dt.value) + + middle_node_index = 52 + solution_of_middle_node = np.array([0, 10.0953, -0.285531]) + 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) + + +def createScene(root): + w, l = 2, 10 + nx, ny = 3, 9 + dx, dy = w/nx/2, l/ny/2 + root.addObject('RequiredPlugin', pluginName='SofaLoader SofaBoundaryCondition SofaEngine SofaSimpleFem SofaImplicitOdeSolver SofaOpenglVisual SofaSparseSolver SofaTopologyMapping') + root.addObject('MeshObjLoader', name='surface', filename='mesh/cylinder.obj') + root.addObject('SparseGridTopology', src='@surface', name='grid', n=[nx, ny, nx]) + root.addChild('mechanics') + root.mechanics.addObject(StaticOdeSolver(name='solver')) + root.mechanics.addObject('SparseLDLSolver') + root.mechanics.addObject('MechanicalObject', name='mo', src='@../grid') + root.mechanics.addObject('HexahedronSetTopologyContainer', name='hexa_topology', hexahedra='@../grid.hexahedra') + root.mechanics.addObject('HexahedronFEMForceField', youngModulus=3000, poissonRatio=0.3) + root.mechanics.addObject('BoxROI', name='fixed_roi', box=[-1-dx,0-dx,-1-dx, 1+dx, 0+dx, 1+dx], drawBoxes=True) + root.mechanics.addObject('FixedConstraint', indices='@fixed_roi.indices') + root.mechanics.addChild('visual') + root.mechanics.visual.addObject('OglModel', name='vm', src='@/surface') + root.mechanics.visual.addObject('BarycentricMapping', applyRestPosition=True) + root.mechanics.addChild('neumann') + root.mechanics.neumann.addObject('MechanicalObject', name='mo') + root.mechanics.neumann.addObject('QuadSetTopologyContainer', name='quad_topology') + root.mechanics.neumann.addObject('QuadSetTopologyModifier') + root.mechanics.neumann.addObject('QuadSetGeometryAlgorithms') + root.mechanics.neumann.addObject('Hexa2QuadTopologicalMapping', input='@../hexa_topology', output='@quad_topology') + root.mechanics.neumann.addObject('SubsetMapping', applyRestPosition=True, input='@../mo', output='@./mo', indices='@quad_topology.points') + 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) + root.mechanics.neumann.addObject('QuadPressureForceField', pressure=[0, 0, -1], quadList='@top_roi.quadIndices') diff --git a/examples/StaticSolver.py b/examples/StaticSolver.py new file mode 100644 index 000000000..25b956822 --- /dev/null +++ b/examples/StaticSolver.py @@ -0,0 +1,69 @@ +import Sofa +from Sofa import SofaSimulationCore +import numpy as np + +class StaticOdeSolver (Sofa.Core.OdeSolver): + def solve(self, _, dt, X, V): + mparams = Sofa.Core.MechanicalParams() + vop = SofaSimulationCore.VectorOperations(self.getContext()) + mop = SofaSimulationCore.MechanicalOperations(mparams, self.getContext()) + + # Allocate the solution vector + dx = Sofa.Core.VecId.dx() + vop.v_realloc(dx, interactionForceField=True, propagate=True) + vop.v_clear(dx) + + # Compute the residual + F = Sofa.Core.VecId.force() + mop.computeForce(result=F, clear=True, accumulate=True); + mop.projectResponse(F) + vop.v_dot(F, F) + F_norm = np.sqrt(vop.finish()) + + # Assemble the system + mop.m_setSystemMBKMatrix(m=0, b=0, k=-1) + mop.m_setSystemRHVector(F) + mop.m_setSystemLHVector(dx) + + # Solve the system + mop.m_solveSystem() + vop.v_dot(dx, dx) + dx_norm = np.sqrt(vop.finish()) + + # Propagate the solution + vop.v_peq(X, dx) # X += dx + + # Solve the constraints + # todo: Bind ConstraintParams + # mop.solveConstraint(X, ConstraintParams.ConstOrder.POS) + + print(f"Solved with |F| = {F_norm} and |dx| = {dx_norm}") + +def createScene(root): + w, l = 2, 10 + nx, ny = 3, 9 + dx, dy = w/nx/2, l/ny/2 + root.addObject('RequiredPlugin', pluginName='SofaLoader SofaBoundaryCondition SofaEngine SofaSimpleFem SofaImplicitOdeSolver SofaOpenglVisual SofaSparseSolver SofaTopologyMapping') + root.addObject('MeshObjLoader', name='surface', filename='mesh/cylinder.obj') + root.addObject('SparseGridTopology', src='@surface', name='grid', n=[nx, ny, nx]) + root.addChild('mechanics') +# root.mechanics.addObject('StaticSolver', name='solver', printLog=False) + root.mechanics.addObject(StaticOdeSolver(name='solver')) + root.mechanics.addObject('SparseLDLSolver') + root.mechanics.addObject('MechanicalObject', name='mo', src='@../grid') + root.mechanics.addObject('HexahedronSetTopologyContainer', name='hexa_topology', hexahedra='@../grid.hexahedra') + root.mechanics.addObject('HexahedronFEMForceField', youngModulus=3000, poissonRatio=0.3) + root.mechanics.addObject('BoxROI', name='fixed_roi', box=[-1-dx,0-dx,-1-dx, 1+dx, 0+dx, 1+dx], drawBoxes=True) + root.mechanics.addObject('FixedConstraint', indices='@fixed_roi.indices') + root.mechanics.addChild('visual') + root.mechanics.visual.addObject('OglModel', name='vm', src='@/surface') + root.mechanics.visual.addObject('BarycentricMapping', applyRestPosition=True) + root.mechanics.addChild('neumann') + root.mechanics.neumann.addObject('MechanicalObject', name='mo') + root.mechanics.neumann.addObject('QuadSetTopologyContainer', name='quad_topology') + root.mechanics.neumann.addObject('QuadSetTopologyModifier') + root.mechanics.neumann.addObject('QuadSetGeometryAlgorithms') + root.mechanics.neumann.addObject('Hexa2QuadTopologicalMapping', input='@../hexa_topology', output='@quad_topology') + root.mechanics.neumann.addObject('SubsetMapping', applyRestPosition=True, input='@../mo', output='@./mo', indices='@quad_topology.points') + 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) + root.mechanics.neumann.addObject('QuadPressureForceField', pressure=[0, 0, -1], quadList='@top_roi.quadIndices')