From 90c487db252ea45e656f2ef2b96601b83060224a Mon Sep 17 00:00:00 2001 From: hugtalbot Date: Thu, 10 Jul 2025 19:21:52 +0200 Subject: [PATCH 1/4] [Examples] Add a script to access contact forces --- examples/access_contact_forces.py | 133 ++++++++++++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 examples/access_contact_forces.py diff --git a/examples/access_contact_forces.py b/examples/access_contact_forces.py new file mode 100644 index 000000000..c754c61a8 --- /dev/null +++ b/examples/access_contact_forces.py @@ -0,0 +1,133 @@ +# Required import for python +import Sofa +import numpy as np +import math + + +# Choose in your script to activate or not the GUI +USE_GUI = True + + +def main(): + import SofaRuntime + import Sofa.Gui + + root = Sofa.Core.Node("root") + createScene(root) + Sofa.Simulation.initRoot(root) + + if not USE_GUI: + import SofaQt + + for iteration in range(10): + Sofa.Simulation.animate(root, root.dt.value) + else: + Sofa.Gui.GUIManager.Init("myscene", "qglviewer") + Sofa.Gui.GUIManager.createGUI(root, __file__) + Sofa.Gui.GUIManager.SetDimension(1080, 1080) + Sofa.Gui.GUIManager.MainLoop(root) + Sofa.Gui.GUIManager.closeGUI() + + +def createScene(root): + root.gravity=[0, -9.81, 0] + root.dt=0.02 + + root.addObject("RequiredPlugin", pluginName=[ 'Sofa.Component.Collision.Detection.Algorithm', + 'Sofa.Component.Collision.Detection.Intersection', 'Sofa.Component.Collision.Geometry', + 'Sofa.Component.Collision.Response.Contact', 'Sofa.Component.Constraint.Projective', + 'Sofa.Component.IO.Mesh','Sofa.Component.LinearSolver.Iterative', + 'Sofa.Component.Mapping.Linear', 'Sofa.Component.Mass', 'Sofa.Component.ODESolver.Backward', + 'Sofa.Component.SolidMechanics.FEM.Elastic','Sofa.Component.StateContainer', + 'Sofa.Component.Topology.Container.Dynamic','Sofa.Component.Visual', + 'Sofa.GL.Component.Rendering3D','Sofa.Component.Constraint.Lagrangian.Correction', + 'Sofa.Component.Constraint.Lagrangian.Solver', 'Sofa.Component.MechanicalLoad', + 'Sofa.Component.LinearSolver.Direct','Sofa.Component.AnimationLoop' + ]) + + root.addObject('FreeMotionAnimationLoop') + # Constraint solver computing the constraint/contact forces, stored in the constraint space (normal , tangential_1, tangential_2) + constraint_solver = root.addObject('GenericConstraintSolver', maxIterations=1000, tolerance=1e-6, computeConstraintForces=True) + + root.addObject('VisualStyle', displayFlags="showCollisionModels hideVisualModels showForceFields") + root.addObject('CollisionPipeline', name="CollisionPipeline") + root.addObject('BruteForceBroadPhase', name="BroadPhase") + root.addObject('BVHNarrowPhase', name="NarrowPhase") + root.addObject('DiscreteIntersection') + root.addObject('CollisionResponse', name="CollisionResponse", response="FrictionContactConstraint", responseParams="mu=0.1") + + root.addObject('MeshOBJLoader', name="LiverSurface", filename="mesh/liver-smooth.obj") + + liver = root.addChild('Liver') + liver.addObject('EulerImplicitSolver', name="cg_odesolver", rayleighStiffness=0.1, rayleighMass=0.1) + liver.addObject('SparseLDLSolver', name="linear_solver") + liver.addObject('MeshGmshLoader', name="meshLoader", filename="mesh/liver.msh") + liver.addObject('TetrahedronSetTopologyContainer', name="topo", src="@meshLoader") + # Liver MechanicalObject where the constraint/contact forces will be stored in the (x,y,z) coordinate system + liverMO = liver.addObject('MechanicalObject', name="dofs", src="@meshLoader") + liver.addObject('TetrahedronSetGeometryAlgorithms', template="Vec3d", name="GeomAlgo") + liver.addObject('DiagonalMass', name="Mass", massDensity=1.0) + liver.addObject('TetrahedralCorotationalFEMForceField', template="Vec3d", name="FEM", method="large", poissonRatio=0.3, youngModulus=3000, computeGlobalMatrix=False) + liver.addObject('FixedProjectiveConstraint', name="FixedConstraint", indices="3 39 64") + + # Forcefield only used for visualization purposes (of the contact forces) + CFF = liver.addObject('ConstantForceField', name="CFF", forces=[0,0,0], showArrowSize=10) + liver.addObject('LinearSolverConstraintCorrection') + + visu = liver.addChild('Visu') + visu.addObject('OglModel', name="VisualModel", src="@../../LiverSurface") + visu.addObject('BarycentricMapping', name="VisualMapping", input="@../dofs", output="@VisualModel") + + surf = liver.addChild('Surf') + surf.addObject('SphereLoader', name="sphereLoader", filename="mesh/liver.sph") + surf.addObject('MechanicalObject', name="spheres", position="@sphereLoader.position") + surf.addObject('SphereCollisionModel', name="CollisionModel", listRadius="@sphereLoader.listRadius") + surf.addObject('BarycentricMapping', name="CollisionMapping", input="@../dofs", output="@spheres") + + + particle = root.addChild('Particle') + particle.addObject('EulerImplicitSolver') + particle.addObject('CGLinearSolver', threshold='1e-09', tolerance='1e-09', iterations='200') + # Particle MechanicalObject where the constraint/contact forces will be stored in the (x,y,z) coordinate system + particleMO = particle.addObject('MechanicalObject', showObject=True, position=[-2, 10, 0, 0, 0, 0, 1], name=f'ParticleDoFs', template='Rigid3d') + particle.addObject('UniformMass', totalMass=1) + particle.addObject('ConstantForceField', name="CFF", totalForce=[0, -1, 0, 0, 0, 0] ) + particle.addObject('SphereCollisionModel', name="SCM", radius=1.0 ) + particle.addObject('UncoupledConstraintCorrection') + + # Python controller accessing and displaying the contact forces in the ConstantForceField + root.addObject(AccessContactForces('AccessContactForces', name='AccessContactForces', + constraint_solver=constraint_solver, + soft_liver=liverMO, + forcefield_visu=CFF, + root_node=root)) + + +class AccessContactForces(Sofa.Core.Controller): + + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.constraint_solver = kwargs.get("constraint_solver") + self.soft_liver = kwargs.get("soft_liver") + self.forcefield_visu = kwargs.get("forcefield_visu") + self.root_node = kwargs.get("root_node") + + + def onAnimateEndEvent(self, event): + + lambda_vector = self.constraint_solver.constraintForces.value + if(len(lambda_vector) > 0): + print("Forces in the contact space (n, t1, t2) = "+str(lambda_vector)+" (at time = "+str(round(self.root_node.time.value,3))+")") + + self.forcefield_visu.forces.value = -self.soft_liver.getData("lambda").value + visuScale = self.forcefield_visu.showArrowSize.value + fact =8.0/np.max(lambda_vector) + self.forcefield_visu.showArrowSize.value = fact + else: + self.forcefield_visu.forces.value = np.zeros((181,3)) + + + +# Function used only if this script is called from a python environment +if __name__ == '__main__': + main() From 3416b8fe87bc56131b0e363b60c208c1d3157df4 Mon Sep 17 00:00:00 2001 From: Hugo Date: Thu, 24 Jul 2025 16:14:23 +0200 Subject: [PATCH 2/4] Apply suggestions from code review Co-authored-by: Damien Marchal --- examples/access_contact_forces.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/access_contact_forces.py b/examples/access_contact_forces.py index c754c61a8..f04d40e5c 100644 --- a/examples/access_contact_forces.py +++ b/examples/access_contact_forces.py @@ -68,7 +68,7 @@ def createScene(root): liver.addObject('TetrahedronSetGeometryAlgorithms', template="Vec3d", name="GeomAlgo") liver.addObject('DiagonalMass', name="Mass", massDensity=1.0) liver.addObject('TetrahedralCorotationalFEMForceField', template="Vec3d", name="FEM", method="large", poissonRatio=0.3, youngModulus=3000, computeGlobalMatrix=False) - liver.addObject('FixedProjectiveConstraint', name="FixedConstraint", indices="3 39 64") + liver.addObject('FixedProjectiveConstraint', name="FixedConstraint", indices=[3,39,64]) # Forcefield only used for visualization purposes (of the contact forces) CFF = liver.addObject('ConstantForceField', name="CFF", forces=[0,0,0], showArrowSize=10) @@ -87,7 +87,7 @@ def createScene(root): particle = root.addChild('Particle') particle.addObject('EulerImplicitSolver') - particle.addObject('CGLinearSolver', threshold='1e-09', tolerance='1e-09', iterations='200') + particle.addObject('CGLinearSolver', threshold=1e-09, tolerance=1e-09, iterations=200) # Particle MechanicalObject where the constraint/contact forces will be stored in the (x,y,z) coordinate system particleMO = particle.addObject('MechanicalObject', showObject=True, position=[-2, 10, 0, 0, 0, 0, 1], name=f'ParticleDoFs', template='Rigid3d') particle.addObject('UniformMass', totalMass=1) @@ -117,7 +117,7 @@ def onAnimateEndEvent(self, event): lambda_vector = self.constraint_solver.constraintForces.value if(len(lambda_vector) > 0): - print("Forces in the contact space (n, t1, t2) = "+str(lambda_vector)+" (at time = "+str(round(self.root_node.time.value,3))+")") + print(f"Forces in the contact space (n, t1, t2) = {lambda_vector} (at time = {round(self.root_node.time.value,3))})") self.forcefield_visu.forces.value = -self.soft_liver.getData("lambda").value visuScale = self.forcefield_visu.showArrowSize.value From afd4d2413033f822277b150a24dd0c44165676ef Mon Sep 17 00:00:00 2001 From: hugtalbot Date: Thu, 24 Jul 2025 21:17:26 +0200 Subject: [PATCH 3/4] use VisualVectorField to render contact forces and take other reviews into account --- examples/access_contact_forces.py | 78 +++++++++++++++---------------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/examples/access_contact_forces.py b/examples/access_contact_forces.py index f04d40e5c..48978cd13 100644 --- a/examples/access_contact_forces.py +++ b/examples/access_contact_forces.py @@ -4,12 +4,9 @@ import math -# Choose in your script to activate or not the GUI -USE_GUI = True - - -def main(): - import SofaRuntime +# Main function taking a boolean as parameter to choose whether or not to use the GUI +def main(use_gui=True): + import SofaImGui import Sofa.Gui root = Sofa.Core.Node("root") @@ -22,7 +19,7 @@ def main(): for iteration in range(10): Sofa.Simulation.animate(root, root.dt.value) else: - Sofa.Gui.GUIManager.Init("myscene", "qglviewer") + Sofa.Gui.GUIManager.Init("myscene", "imgui") Sofa.Gui.GUIManager.createGUI(root, __file__) Sofa.Gui.GUIManager.SetDimension(1080, 1080) Sofa.Gui.GUIManager.MainLoop(root) @@ -50,56 +47,59 @@ def createScene(root): constraint_solver = root.addObject('GenericConstraintSolver', maxIterations=1000, tolerance=1e-6, computeConstraintForces=True) root.addObject('VisualStyle', displayFlags="showCollisionModels hideVisualModels showForceFields") - root.addObject('CollisionPipeline', name="CollisionPipeline") - root.addObject('BruteForceBroadPhase', name="BroadPhase") - root.addObject('BVHNarrowPhase', name="NarrowPhase") + root.addObject('CollisionPipeline', name="collision_pipeline") + root.addObject('BruteForceBroadPhase', name="broad_phase") + root.addObject('BVHNarrowPhase', name="narrow_phase") root.addObject('DiscreteIntersection') - root.addObject('CollisionResponse', name="CollisionResponse", response="FrictionContactConstraint", responseParams="mu=0.1") + root.addObject('CollisionResponse', name="collision_response", response="FrictionContactConstraint", responseParams="mu=0.1") - root.addObject('MeshOBJLoader', name="LiverSurface", filename="mesh/liver-smooth.obj") + root.addObject('MeshOBJLoader', name="load_liver_surface", filename="mesh/liver-smooth.obj") liver = root.addChild('Liver') liver.addObject('EulerImplicitSolver', name="cg_odesolver", rayleighStiffness=0.1, rayleighMass=0.1) - liver.addObject('SparseLDLSolver', name="linear_solver") - liver.addObject('MeshGmshLoader', name="meshLoader", filename="mesh/liver.msh") - liver.addObject('TetrahedronSetTopologyContainer', name="topo", src="@meshLoader") + liver.addObject('SparseLDLSolver', name="linear_solver", template="CompressedRowSparseMatrixMat3x3d") + liver.addObject('MeshGmshLoader', name="loader_liver_volume", filename="mesh/liver.msh") + liver.addObject('TetrahedronSetTopologyContainer', name="topo", src="@loader_liver_volume") # Liver MechanicalObject where the constraint/contact forces will be stored in the (x,y,z) coordinate system - liverMO = liver.addObject('MechanicalObject', name="dofs", src="@meshLoader") - liver.addObject('TetrahedronSetGeometryAlgorithms', template="Vec3d", name="GeomAlgo") - liver.addObject('DiagonalMass', name="Mass", massDensity=1.0) + liverMO = liver.addObject('MechanicalObject', name="dofs", src="@loader_liver_volume") + liver.addObject('TetrahedronSetGeometryAlgorithms', template="Vec3d", name="geom_algo") + liver.addObject('DiagonalMass', name="mass", massDensity=1.0) liver.addObject('TetrahedralCorotationalFEMForceField', template="Vec3d", name="FEM", method="large", poissonRatio=0.3, youngModulus=3000, computeGlobalMatrix=False) - liver.addObject('FixedProjectiveConstraint', name="FixedConstraint", indices=[3,39,64]) - - # Forcefield only used for visualization purposes (of the contact forces) - CFF = liver.addObject('ConstantForceField', name="CFF", forces=[0,0,0], showArrowSize=10) + liver.addObject('FixedProjectiveConstraint', name="fixed_constraint", indices=[3,39,64]) liver.addObject('LinearSolverConstraintCorrection') + # Forcefield only used for visualization purposes (of the contact fborces) + contactVisu = liver.addChild('RenderingContactForces') + contactVisu.addObject('VisualStyle', displayFlags="showVisualModels") + renderingForces = contactVisu.addObject('VisualVectorField', name="drawing_contact_forces", vectorScale="100", vector="@../dofs.lambda", position="@../dofs.position", color="orange", drawMode="Arrow") + visu = liver.addChild('Visu') - visu.addObject('OglModel', name="VisualModel", src="@../../LiverSurface") - visu.addObject('BarycentricMapping', name="VisualMapping", input="@../dofs", output="@VisualModel") + visu.addObject('OglModel', name="visual_model", src="@../../load_liver_surface") + visu.addObject('BarycentricMapping', name="visual_mapping", input="@../dofs", output="@visual_model") surf = liver.addChild('Surf') - surf.addObject('SphereLoader', name="sphereLoader", filename="mesh/liver.sph") - surf.addObject('MechanicalObject', name="spheres", position="@sphereLoader.position") - surf.addObject('SphereCollisionModel', name="CollisionModel", listRadius="@sphereLoader.listRadius") - surf.addObject('BarycentricMapping', name="CollisionMapping", input="@../dofs", output="@spheres") + surf.addObject('SphereLoader', name="loader_sphere_model", filename="mesh/liver.sph") + surf.addObject('MechanicalObject', name="spheres", position="@loader_sphere_model.position") + surf.addObject('SphereCollisionModel', name="collision_model", listRadius="@loader_sphere_model.listRadius") + surf.addObject('BarycentricMapping', name="collision_mapping", input="@../dofs", output="@spheres") particle = root.addChild('Particle') particle.addObject('EulerImplicitSolver') particle.addObject('CGLinearSolver', threshold=1e-09, tolerance=1e-09, iterations=200) # Particle MechanicalObject where the constraint/contact forces will be stored in the (x,y,z) coordinate system - particleMO = particle.addObject('MechanicalObject', showObject=True, position=[-2, 10, 0, 0, 0, 0, 1], name=f'ParticleDoFs', template='Rigid3d') + particleMO = particle.addObject('MechanicalObject', showObject=True, position=[-2, 10, 0, 0, 0, 0, 1], name=f'particle_DoFs', template='Rigid3d') particle.addObject('UniformMass', totalMass=1) particle.addObject('ConstantForceField', name="CFF", totalForce=[0, -1, 0, 0, 0, 0] ) particle.addObject('SphereCollisionModel', name="SCM", radius=1.0 ) particle.addObject('UncoupledConstraintCorrection') + # Python controller accessing and displaying the contact forces in the ConstantForceField root.addObject(AccessContactForces('AccessContactForces', name='AccessContactForces', constraint_solver=constraint_solver, soft_liver=liverMO, - forcefield_visu=CFF, + forces_visu=renderingForces, root_node=root)) @@ -109,25 +109,25 @@ def __init__(self, *args, **kwargs): Sofa.Core.Controller.__init__(self, *args, **kwargs) self.constraint_solver = kwargs.get("constraint_solver") self.soft_liver = kwargs.get("soft_liver") - self.forcefield_visu = kwargs.get("forcefield_visu") + self.forces_visu = kwargs.get("forces_visu") self.root_node = kwargs.get("root_node") - + self.forces_visu.vector.value = np.zeros((181,3)) def onAnimateEndEvent(self, event): lambda_vector = self.constraint_solver.constraintForces.value if(len(lambda_vector) > 0): - print(f"Forces in the contact space (n, t1, t2) = {lambda_vector} (at time = {round(self.root_node.time.value,3))})") + print(f"At time = {round(self.root_node.time.value,3)}, forces in the contact space (n, t1, t2) equals:\n {lambda_vector} ") - self.forcefield_visu.forces.value = -self.soft_liver.getData("lambda").value - visuScale = self.forcefield_visu.showArrowSize.value - fact =8.0/np.max(lambda_vector) - self.forcefield_visu.showArrowSize.value = fact + self.forces_visu.vector.value = -self.soft_liver.getData("lambda").value + visuScale = self.forces_visu.vectorScale.value + fact = np.max(lambda_vector) + self.forces_visu.vectorScale.value = 10/fact else: - self.forcefield_visu.forces.value = np.zeros((181,3)) + self.forces_visu.vector.value = np.zeros((181,3)) # Function used only if this script is called from a python environment if __name__ == '__main__': - main() + main(True) From b113c04567c35f04b6abadd0725d4cde30c79e8c Mon Sep 17 00:00:00 2001 From: hugtalbot Date: Thu, 24 Jul 2025 21:23:39 +0200 Subject: [PATCH 4/4] add comments --- examples/access_contact_forces.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/examples/access_contact_forces.py b/examples/access_contact_forces.py index 48978cd13..fccf480cd 100644 --- a/examples/access_contact_forces.py +++ b/examples/access_contact_forces.py @@ -111,18 +111,25 @@ def __init__(self, *args, **kwargs): self.soft_liver = kwargs.get("soft_liver") self.forces_visu = kwargs.get("forces_visu") self.root_node = kwargs.get("root_node") + # Initialize the rendered vector with zero vec self.forces_visu.vector.value = np.zeros((181,3)) def onAnimateEndEvent(self, event): lambda_vector = self.constraint_solver.constraintForces.value + # If there is a contact if(len(lambda_vector) > 0): print(f"At time = {round(self.root_node.time.value,3)}, forces in the contact space (n, t1, t2) equals:\n {lambda_vector} ") + # Compute the inverse (reaction force) self.forces_visu.vector.value = -self.soft_liver.getData("lambda").value + + # Scale automatically the displayed force vector visuScale = self.forces_visu.vectorScale.value fact = np.max(lambda_vector) self.forces_visu.vectorScale.value = 10/fact + + # If no contact else: self.forces_visu.vector.value = np.zeros((181,3))