From 97d79077e49263cbf63bf93d5ce171000ceec919 Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 12 Oct 2026 14:50:22 +0200 Subject: [PATCH 1/2] Full force field example in Python --- examples/example-forcefield.py | 46 +++++++++++++++++++++------------- 1 file changed, 29 insertions(+), 17 deletions(-) diff --git a/examples/example-forcefield.py b/examples/example-forcefield.py index 58c59ded..daef3b71 100644 --- a/examples/example-forcefield.py +++ b/examples/example-forcefield.py @@ -14,19 +14,24 @@ def __init__(self, ks=1.0, kd=1.0, *args, **kwargs): def init(self): mstate = self.getContext().mechanical self.initpos = mstate.position.array().copy() - self.k = np.zeros((1,1)) - self.f = [] - self.d = 0.5 def addForce(self, m, out_force, pos, vel): with out_force.writeableArray() as wa: wa[:] += ( (self.initpos-pos.value) * self.ks.value ) - + def addDForce(self, params, df, dx): - pass - - #def addKToMatrix(self, params, number_of_nodes, number_of_dofs_per_node): - # print(" Python::addKToMatrix: ", a, " ", b) + kFactor = params['kFactor'] + with df.writeableArray() as dfwa: + dfwa[:] -= dx.value * self.kd.value * kFactor + + def addKToMatrix(self, params, number_of_nodes, number_of_dofs_per_node): + K = [] + kFactor = params['kFactor'] + for n in range(0, number_of_nodes): + for d in range(0, number_of_dofs_per_node): + K.append([number_of_dofs_per_node * n + d, number_of_dofs_per_node * n + d, -kFactor * self.kd.value]) + return np.asarray(K) + def createScene(root): @@ -42,17 +47,24 @@ def createScene(root): root.addObject("LineAxis") root.addObject("DefaultAnimationLoop", name="loop") - root.addObject("EulerImplicitIntegrationScheme") - root.addObject("CGLinearSolver", tolerance=1e-12, threshold=1e-12, iterations=25) - o = root.addChild("Object") - c = o.addObject("MechanicalObject", name="mechanical", position=[0.0,0.0,0.0, 1.0,0.0,0.0]) - c.showObject = True - c.showColor = [1.0,0.0,0.0,1.0] - c.drawMode = 1 + def add_spring_simulation(node, start_position, color): + node.addObject("EulerImplicitIntegrationScheme", rayleighStiffness=0.1, rayleighMass=0.1) + + o = node.addChild("Object") + c = o.addObject("MechanicalObject", name="mechanical", position=[start_position + 0.0,0.0,0.0, start_position + 1.0,0.0,0.0], showColor=color) + c.showObject = True + c.drawMode = 1 + + o.addObject("UniformMass", name="mass", totalMass=[0.1]) + o.addObject( RestShapeForceField(name="spring", ks=2.0, kd=0.1)) - o.addObject("UniformMass", name="mass", totalMass=[0.1]) - o.addObject( RestShapeForceField(name="CPPObject", ks=2.0, kd=0.1)) + with root.addChild("Direct") as direct: + add_spring_simulation(direct, 0.0, [0,1,0,1]) + direct.addObject("SparseLDLSolver", template="CompressedRowSparseMatrixd") + with root.addChild("Iterative") as iterative: + add_spring_simulation(iterative, 0.2, [1,0,0,1]) + iterative.addObject("CGLinearSolver", tolerance=1e-12, threshold=1e-12, iterations=25) return root From 3f36e493832906aaaf59570415925ecdb2f011cf Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Mon, 12 Oct 2026 21:05:31 +0200 Subject: [PATCH 2/2] Implement `buildStiffnessMatrix` in Python --- .../Sofa/Core/Binding_ForceField.cpp | 71 ++++++++++++++++++- .../Sofa/Core/Binding_ForceField.h | 5 ++ examples/example-forcefield.py | 10 ++- 3 files changed, 82 insertions(+), 4 deletions(-) diff --git a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp index 326b44dc..0ddc88c6 100644 --- a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp +++ b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include @@ -136,7 +137,7 @@ namespace sofapython3 void ForceField_Trampoline::addKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* dfId) { PythonEnvironment::gil acquire; - + MultiMatrixAccessor::MatrixRef mref = dfId->getMatrix(this->mstate); sofa::linearalgebra::BaseMatrix* mat = mref.matrix; @@ -183,8 +184,74 @@ namespace sofapython3 } - template + py::object ForceField_Trampoline::_buildStiffnessMatrix(int nIndices, int nDofs) + { + PYBIND11_OVERRIDE_PURE(py::object, ForceField, buildStiffnessMatrix, nIndices, nDofs); + } + + template + void ForceField_Trampoline::buildStiffnessMatrix( + sofa::core::behavior::StiffnessMatrix *matrix) + { + PythonEnvironment::gil acquire; + + // nNodes is the number of nodes (positions) of the object whose K matrix we're computing + const int nNodes = static_cast(this->mstate->getSize()); + // nDofs is the number of degrees of freedom per-element of the object whose K matrix we're computing + static constexpr int nDofs = Coord::total_size; + + if (py::get_overload(this, "addKToMatrix") + && !py::get_overload(this, "buildStiffnessMatrix")) + { + msg_deprecated_once() << "The method `addKToMatrix` is deprecated. Please use `buildStiffnessMatrix` instead."; + + // Runs the compatibility layer with `addKToMatrix` + ForceField::buildStiffnessMatrix(matrix); + return; + } + + py::object ret = _buildStiffnessMatrix(nNodes, nDofs); + + if(!py::isinstance(ret)) + { + throw py::type_error("Can't read return value of `buildStiffnessMatrix`. A numpy array is expected"); + } + + auto dfdx = + matrix->getForceDerivativeIn(this->mstate) + .withRespectToPositionsIn(this->mstate); + + // if ret is numpy array + auto r = py::cast(ret); + if (r.ndim() == 3 && r.shape(2) == 1) + { + // read K as a plain 2D matrix + auto kMatrix = r.unchecked(); + for (size_t x = 0 ; x < size_t(kMatrix.shape(0)) ; ++x) + { + for (size_t y = 0 ; y < size_t(kMatrix.shape(1)) ; ++y) + { + dfdx(x, y) += kMatrix(x,y, 0); + } + } + } + else if (r.ndim() == 2 && r.shape(1) == 3) + { + // consider ret to be a list of tuples [(i,j,[val])] + auto kMatrix = r.unchecked(); + for (auto x = 0 ; x < kMatrix.shape(0) ; ++x) + { + dfdx(int(size_t(kMatrix(x,0))), int(size_t(kMatrix(x,1)))) += kMatrix(x,2); + } + } + else + { + throw py::type_error("Can't read return value of `buildStiffnessMatrix`. The method should return either a plain 2D matrix or a vector of tuples (i, j, val)"); + } + } + + template void declare_forcefield(py::module &m) { const std::string pyclass_name = std::string("ForceField") + TDOFType::Name(); py::class_, BaseComponent, ForceField_Trampoline, py_shared_ptr>> f(m, pyclass_name.c_str(), py::dynamic_attr(), py::multiple_inheritance(), sofapython3::doc::forceField::forceFieldClass); diff --git a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h index 3d34f3f9..d2f4f6d7 100644 --- a/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h +++ b/bindings/Sofa/src/SofaPython3/Sofa/Core/Binding_ForceField.h @@ -50,8 +50,13 @@ class ForceField_Trampoline : public sofa::core::behavior::ForceField pybind11::object _addKToMatrix(const sofa::core::MechanicalParams* mparams, int nNodes, int nDofs); void addKToMatrix(const sofa::core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* dfId) override; + void buildStiffnessMatrix(sofa::core::behavior::StiffnessMatrix* matrix) override; + SReal getPotentialEnergy(const sofa::core::MechanicalParams* /*mparams*/, const DataVecCoord& /*x*/) const override { return 0.0; } +private: + + pybind11::object _buildStiffnessMatrix(int nIndices, int nDofs); }; void moduleAddForceField(pybind11::module &m); diff --git a/examples/example-forcefield.py b/examples/example-forcefield.py index daef3b71..96a983b2 100644 --- a/examples/example-forcefield.py +++ b/examples/example-forcefield.py @@ -17,12 +17,12 @@ def init(self): def addForce(self, m, out_force, pos, vel): with out_force.writeableArray() as wa: - wa[:] += ( (self.initpos-pos.value) * self.ks.value ) + wa[:] += ( (self.initpos-pos.value) * self.ks.value ) def addDForce(self, params, df, dx): kFactor = params['kFactor'] with df.writeableArray() as dfwa: - dfwa[:] -= dx.value * self.kd.value * kFactor + dfwa[:] -= dx.value * self.ks.value * kFactor def addKToMatrix(self, params, number_of_nodes, number_of_dofs_per_node): K = [] @@ -32,6 +32,12 @@ def addKToMatrix(self, params, number_of_nodes, number_of_dofs_per_node): K.append([number_of_dofs_per_node * n + d, number_of_dofs_per_node * n + d, -kFactor * self.kd.value]) return np.asarray(K) + def buildStiffnessMatrix(self, number_of_nodes, number_of_dofs_per_node): + K = [] + for n in range(0, number_of_nodes): + for d in range(0, number_of_dofs_per_node): + K.append([number_of_dofs_per_node * n + d, number_of_dofs_per_node * n + d, -self.ks.value]) + return np.asarray(K) def createScene(root):