Accessing Contact forces in Collision Detectors

[AbhiBjee]

Hello SOFA Developers !

I am a PhD student from Purdue Mechanical Engineering.

I am very new to SOFA and currently working on understanding surgical tool tissue interactions for palpation of tumors, especially for tumor mapping.

I am currently exploring both Penalty based as well as Lagrange Multiplier based collision models for contact detection.
My primary objective is to retrieve contact forces and deformations of a Soft Body from a rigid body to soft body collision and use it for calculating local point stiffness estimation for robot control policy learning aimed for tumor mapping.

Tumor Model Liposarcoma (Musculoskeletal) and Hepatocellular sarcoma (Liver Tumor)

I am using Sofa Python3 on Windows with NVIDIA RTX 4070 8 gigs and version: SOFA 24.06 .

I am trying to understand how to extract forces from the collision detectors.
Going through the discussion forum I found this conversation thread

This thread shows a very useful example of detecting contact forces through constraint correction using Lagrange Multipliers.

The code: constraint = self.rootNode.Cube.collision.MechanicalObject.constraint.value should output in the format given in the discussion as:

0 2 58 0.487691 -0.305337 0.146266 59 0.333767 -0.208967 0.100102
1 2 58 0.336271 0.407063 -0.271458 59 0.230138 0.278586 -0.185781
2 2 58 0.0393245 0.305838 0.50733 59 0.026913 0.20931 0.347208

However, on my end it reads:

Constraint ID : 0 dof ID : 1 value : 0 1 -1.27615e-13
Constraint ID : 1 dof ID : 1 value : -1.27615e-13 1.27615e-13 1
Constraint ID : 2 dof ID : 1 value : 1 1.62857e-26 1.27615e-13
Constraint ID : 3 dof ID : 2 value : -1.33509e-15 1 -4.6728e-15
Constraint ID : 4 dof ID : 2 value : -4.6728e-15 4.6728e-15 1
Constraint ID : 5 dof ID : 2 value : 1 1.33509e-15 4.6728e-15
Constraint ID : 6 dof ID : 3 value : -5.10462e-13 1 -5.10462e-13
Constraint ID : 7 dof ID : 3 value : -5.10462e-13 5.10462e-13 1
Constraint ID : 8 dof ID : 3 value : 1 5.10462e-13 5.10462e-13

Constraint ID : 0 dof ID : 3 value : -7.81589e-15 1 4.26321e-15
Constraint ID : 1 dof ID : 3 value : 4.26321e-15 -4.26321e-15 1
Constraint ID : 2 dof ID : 3 value : 1 7.81589e-15 -4.26321e-15

I feel this makes the drawForces() Class to fail.

Can you please help me run this example on my end?
I would also like to request you to help me understand what the following code is doing:

class drawForces(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        # These are needed (and the normal way to override from a python class)
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.rootNode = kwargs.get("rootNode")

    def onAnimateEndEvent(self, event): # called at each end of animation step
        constraint = self.rootNode.Cube.collision.MechanicalObject.constraint.value
        #print (f"ConstraintForce: {constraint}")
        print (constraint)
        dt = self.rootNode.dt.value
        constraintMatrixInline = np.fromstring(constraint, sep=' ')

        print (f"ConstraintMatrixInline: {constraintMatrixInline}")

        pointId = []
        constraintDirections = []
        index = 0
        i = 0

        forcesNorm = self.rootNode.GCS.constraintForces.value
        print (f"ForceNorms: {forcesNorm}")

        constraintDirections = np.zeros((1,1))

        if len(constraintMatrixInline) > 0:
          constraintDirections = np.zeros((int(len(constraintMatrixInline)/6),3))

        while index < len(constraintMatrixInline):
          nbConstraint   = constraintMatrixInline[index+1]
          pointId = np.append(pointId, constraintMatrixInline[index+2])
          constraintDirections[i][0] = constraintMatrixInline[index+3]
          constraintDirections[i][1] = constraintMatrixInline[index+4]
          constraintDirections[i][2] = constraintMatrixInline[index+5]
          index = index + 6
          i = i + 1
        
        nbDofs = len(self.rootNode.Cube.collision.MechanicalObject.position.value)
        forces = np.zeros((nbDofs,3))

        for i in range(int(len(constraintMatrixInline)/6)):
          indice = int(pointId[i])
          forces[indice][0] = forces[indice][0] + constraintDirections[i][0] * forcesNorm[i] / dt
          forces[indice][1] = forces[indice][1] + constraintDirections[i][1] * forcesNorm[i] / dt
          forces[indice][2] = forces[indice][2] + constraintDirections[i][2] * forcesNorm[i] / dt
        

        if len(constraintMatrixInline) > 0:
          self.rootNode.drawNode.drawForceFF.indices.value = list(range(0,nbDofs,1)) 
          self.rootNode.drawNode.drawForceFF.forces.value = forces
          self.rootNode.drawNode.drawPositions.position.value = self.rootNode.Cube.collision.MechanicalObject.position.value

How can I update it on my end based on the new format of the "Constraint" String.

Are there other approaches to extract contact forces to compute reaction forces of a rigid body in contact with soft body ?

Any help would be highly appreciated.
Thanks in advance.

[bakpaul]

Hello !

I am sorry, but it seems to me that you are not asking any SOFA-related question. Your are asking for an update of a python script regarding some input constraints. I am honestly sure that you are able to do it by yourself.

Copy-pasting your post on ChatGPT outputted a quite comprehensive answer. I didn't check the syntax or even if it was working. But anyway, it shows that you are not asking SOFA question but really a python question ;)

Please, feel free to come back when you start digging more into SOFA API !

[AbhiBjee]

Hi Paul Baksic,

Thank you for your feedback. I was able to come up with a python parser to get the code from thread #3812 running on my end finally.

I share my updated Code below (for observing contact force directions at contact dofs in SOFA version 24.06):

import Sofa.Core
import numpy as np
import re
from stlib3.physics.rigid import Floor, Cube

# Choose in your script to activate or not the GUI
USE_GUI = True


class drawForces(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.rootNode = kwargs.get("rootNode")

    def get_resultant_force_with_max(self, F):
        """
        Compute the resultant force and find the maximum force component with its index.

        Parameters:
        F (array-like): Array of forces [F1, F2, F3]

        Returns:
        tuple: (Resultant force, Maximum force component, Index of max force)
        """
        F = np.array(F)  # Ensure input is a NumPy array

        # Compute the resultant force
        F_resultant = np.linalg.norm(F)  # Equivalent to sqrt(F1^2 + F2^2 + F3^2)

        # Find the max force and its index
        max_force = np.max(np.abs(F))  # Absolute value to consider negative forces
        max_index = np.argmax(np.abs(F))  # Get the index of the max absolute force
        sign = np.sign(F[max_index])  # Get sign of that value

        F_resultant = F_resultant * sign

        return F_resultant, max_force, max_index

    def get_DofNodes_ForceVectors(self, dofs, F_Matrix):

        dofs = dofs.tolist()
        last_dof_id = -1
        dof_nodes = []
        forceVec_arr = []
        forceVec_dir = []
        Fx = 0.0
        Fy = 0.0
        Fz = 0.0

        for i in range(len(dofs)):
            if last_dof_id != dofs[i]:
                dof_nodes.append(dofs[i])

            f_rez,_, idx = self.get_resultant_force_with_max(F_Matrix[i])

            if idx == 0:
                Fx = f_rez.item()# change numpy value to normal
            elif idx == 1:
                Fy = f_rez.item()# change numpy value to normal
            elif idx == 2:
                Fz = f_rez.item()# change numpy value to normal

            if (i+1)%3 == 0:
                forceVec = [Fx, Fy, Fz]

                vector = np.array(forceVec)  # Ensure input is a NumPy array
                magnitude = np.linalg.norm(vector)  # Compute magnitude (Euclidean norm)
                
                if magnitude == 0:
                    forceVecDir = np.zeros_like(vector)# Return zero vector if input is zero
                else:
                    forceVecDir = vector / magnitude  # Normalize vector

                vec_Dir = forceVecDir.tolist()
                vec_Dir = [5.0 * v for v in vec_Dir]
                    
                forceVec_dir.append(vec_Dir)                            
                forceVec_arr.append(forceVec)
                
                Fx = 0.0
                Fy = 0.0
                Fz = 0.0

            last_dof_id = dofs[i]
            
        return dof_nodes, forceVec_dir, forceVec_arr
    

    def parse_constraints(self, constraint_text):
        """Parses constraint data and returns point IDs and force directions."""
        pattern = r"Constraint ID : (\d+)\s+dof ID : (\d+)\s+value : ([\d\.\-e\s]+)"
        matches = re.findall(pattern, constraint_text)

        pointId = []
        constraintDirections = []
        last_dof_id = -1
        dof_arr = []

        #print ("Start Parsing")

        for match in matches:
            constraint_id = int(match[0])
            dof_id = int(match[1])
            values = np.array(match[2].split(), dtype=float)

            pointId.append(constraint_id)  # Using Constraint ID as point reference
            constraintDirections.append(values)
            dof_arr.append(dof_id)

##            if last_dof_id != dof_id:
##                dof_arr.append(dof_id)
##
##            last_dof_id = dof_id               

            #print (f"Parsed Contraint ID : {constraint_id}, Dof: {dof_id} direction is : {values}")

        #print ("End Parsing")

        return np.array(pointId, dtype=int), np.array(dof_arr), np.array(constraintDirections)

    def onAnimateEndEvent(self, event):  # Called at each end of animation step
        constraint_text = self.rootNode.Cube.collision.MechanicalObject.constraint.value
        dt = self.rootNode.dt.value

        # Parse the new format constraints
        pointId, dofs, constraintDirections = self.parse_constraints(constraint_text)

        forcesNorm = self.rootNode.GCS.constraintForces.value
        dofNodes  = self.rootNode.Cube.collision.MechanicalObject.position.value
        nbDofs = len(dofNodes)
        #forces = np.zeros((nbDofs, 3))
        forceMatrix = np.zeros((len(pointId), 3))
        #print (f"len point ids:  {len(pointId)} #nbDofs: {nbDofs}")
        #print (f"Forces Norm : {forcesNorm}")

        # Compute force Matrix -- dim [#pointID X 3]
        for i in range(len(pointId)):
            index = pointId[i]
            for j in range(len(constraintDirections[i])):
                if j < 3:  # Ensure it's a 3D force vector
                    forceMatrix[index][j] += constraintDirections[i][j] * forcesNorm[i] / dt

        #print (f"Forces are : {forceMatrix}")

        dof_nodes, forceVec_dir, forceVec_arr = self.get_DofNodes_ForceVectors(dofs, forceMatrix)

        #forceVec_dir = [50.0 * v for v in forceVec_dir]

        print (f" Dof Nodes in contact : {dof_nodes}")

        print (f"Force Vectors of Dof Nodes: {forceVec_arr}")
        print (f"Force Directions of Dof Nodes: {forceVec_dir}")

        # Update visualization
        if len(pointId) > 0:
            #self.rootNode.drawNode.drawForceFF.indices.value = list(range(len(nbDofs)))
            self.rootNode.drawNode.drawForceFF.indices.value = dof_nodes
            self.rootNode.drawNode.drawForceFF.forces.value = forceVec_dir
            self.rootNode.drawNode.drawPositions.position.value = self.rootNode.Cube.collision.MechanicalObject.position.value


def createScene(rootNode):
    rootNode.addObject("RequiredPlugin", pluginName=['Sofa.Component.AnimationLoop',
                                                '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.Constraint.Lagrangian.Correction',
                                                'Sofa.Component.Constraint.Lagrangian.Solver',                                                
                                                'Sofa.Component.Constraint.Lagrangian.Model',
                                                'Sofa.Component.IO.Mesh',
                                                'Sofa.Component.LinearSolver.Iterative',
                                                'Sofa.Component.Mapping.Linear',
                                                'Sofa.Component.Mass',
                                                'Sofa.Component.MechanicalLoad',
                                                'Sofa.Component.ODESolver.Backward',
                                                'Sofa.Component.SolidMechanics.FEM.Elastic',
                                                'Sofa.Component.StateContainer',
                                                'Sofa.Component.Topology.Container.Dynamic',
                                                'Sofa.Component.Topology.Container.Grid',
                                                'Sofa.Component.Topology.Mapping', 
                                                'Sofa.Component.Visual',
                                                'Sofa.GL.Component.Rendering3D'
                                                ])
    rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight")
    rootNode.addObject('VisualStyle', displayFlags='showVisual showCollisionModels showWireFrame showInteractionForceFields showForceFields')
    rootNode.addObject('GenericConstraintSolver', name="GCS", maxIt=1000, tolerance=1e-6, computeConstraintForces=True)
    rootNode.addObject('FreeMotionAnimationLoop')
    rootNode.addObject('DefaultPipeline', depth=15, verbose=0, draw=0)
    rootNode.addObject('BruteForceBroadPhase')
    rootNode.addObject('BVHNarrowPhase')
    rootNode.addObject('LocalMinDistance', name='Proximity', alarmDistance=10, contactDistance=5, useLMDFilters=0)
    rootNode.addObject('DefaultContactManager', name='Response', response='FrictionContactConstraint')
    rootNode.addObject('EulerImplicitSolver', name='odesolver', firstOrder=False, rayleighMass=0.1, rayleighStiffness=0.1)
    rootNode.gravity = [0, -981.0, 0]

    #rootNode.addObject("VisualGrid", nbSubdiv=100, size=100)

    cube = Cube(rootNode, name="Cube", translation=[0.0, 0.0, 0.0], uniformScale=5.0, totalMass=1.0, volume=1.0)
    cube.addObject('UncoupledConstraintCorrection')

    floor = Floor(rootNode, name="Floor", translation=[0.0, -100.0, 0.0], uniformScale=1.0, isAStaticObject=True)

    rootNode.addObject(drawForces(name="PythonController", rootNode=rootNode))

    drawNode = rootNode.addChild('drawNode')
    MOdraw = drawNode.addObject('MechanicalObject', name="drawPositions", position="@/rootNode/Cube/collision/MechanicalObject.position", size=3)
    drawNode.addObject('ConstantForceField', name="drawForceFF", forces=[0, 0, 0], showArrowSize=1)

    return rootNode

def main():
    import SofaRuntime
    import Sofa.Gui

    root = Sofa.Core.Node("root")
    createScene(root)
    Sofa.Simulation.init(root)

    if not USE_GUI:
        for iteration in range(1000):
            Sofa.Simulation.animate(root, root.dt.value)
    else:
        Sofa.Gui.GUIManager.Init("myscene", "qglviewer")
        Sofa.Gui.GUIManager.createGUI(root, __file__)
        Sofa.Gui.GUIManager.SetDimension(1280, 720)
        Sofa.Gui.GUIManager.MainLoop(root)
        Sofa.Gui.GUIManager.closeGUI()

if __name__ == '__main__':
    main()

Here is the video:

SOFA.v24.06.00.-.D__SOFA.Learning_Default.Tutorials.from.SOFA_TestContactForces_forumCubeEdited.py.2025-03-18.12-34-52.mp4

The only error I am facing is on the Terminal stating:

[ERROR] [ConstantForceField(drawForceFF)] Size mismatch: indices > system size
[ERROR] [ConstantForceField(drawForceFF)] Size mismatch: indices > system size
[ERROR] [ConstantForceField(drawForceFF)] Size mismatch: indices > system size
[ERROR] [ConstantForceField(drawForceFF)] Size mismatch: indices > system size

So the Total number of nodes in the cube is 8 i.e. 8 corners which are indexed from 0-7. Also, the lines
dofNodes = self.rootNode.Cube.collision.MechanicalObject.position.value
nbDofs = len(dofNodes) shows that there are only 8 dofs. I am reading only 3 to 4 indices for the drawForce.indices.value and their corresponding Force vectors drawForceFF.forces.value which can either be the force direction vector forceVec_dir or the force magnitude vector forceVec_arr during Cube-Floor contacts. I am unable to understand why I am getting this Size Mismatch error.

Could you please help me understand this error and troubleshoot it? It will be much appreciated.

Thanks in advance.

[bakpaul]

Hello !

Great to see this script working ! Congrat'
Regarding you last issue: is this error thrown each timestep or only for the first one or at init ?

Reading the source code, this error is thrown when the data drawForceFF.indices (which is a list of indices) has a size that is greater than the number of points in the mstate. Please make sure this vector size never exceed 8. If the error is only thrown at the loading of the scene or in the first timestamp, don't mind it, it certainly comes from a wrong initialization.

Please tell me how it goes, so we know if the object need some fixing.

Anyway, looking at your scene I can see some strange things, for instance you are specifying a data size=3 to the MechanicalObject. I guess what you wanted to do is to say that you have a Vec3 and not a Rigid (3DOFS instead of 6). If so, don't use this parameter and add template="Vec3" instead.

[AbhiBjee]

Hello Paul;

I am sorry for the late response. Yes, the error is thrown at each timestep not only on the first timestep.

Can you please specify if we can also use the object Vec3 also for rigid bodies without using Rigid3D?

[SangbaekYoo]

Hi, I am also trying to get contact force and approaching to this recent discussion.

I am currently using SOFA v24.12 (the most recent version), and the expression of constraint = self.rootNode.Cube.collision.MechanicalObject.constraint.value is also different with previous versions.
when I run below code,

print(self.mechanical_object.constraint.linkpath)
print('dtype: ' + str(constraint_matrix.dtype))
print('shape: ' + str(constraint_matrix.shape))
print('ndim: ' + str(constraint_matrix.ndim))
print(constraint_matrix)

the result is:
@/defoDOF.constraint
dtype: float64
shape: (19, 60)
ndim: 2
(0, 3) 1.0
(0, 4) 0.0
(0, 5) 0.0
(1, 3) -1.0
(1, 4) 0.0
(1, 5) 0.0
(1, 6) 1.0
(1, 7) 0.0
(1, 8) 0.0
(2, 6) -1.0
(2, 7) 0.0
(2, 8) 0.0
(2, 9) 1.0
(2, 10) 0.0
(2, 11) 0.0
(3, 9) -1.0
(3, 10) 0.0
(3, 11) 0.0
(3, 12) 1.0
(3, 13) 0.0
(3, 14) 0.0
(4, 12) -1.0
(4, 13) 0.0
(4, 14) 0.0
(4, 15) 1.0
: :
(14, 47) 0.0
(15, 45) -1.0
(15, 46) 0.0
(15, 47) 0.0
(15, 48) 1.0
(15, 49) 0.0
(15, 50) 0.0
(16, 48) -1.0
(16, 49) 0.0
(16, 50) 0.0
(16, 51) 1.0
(16, 52) 0.0
(16, 53) 0.0
(17, 51) -1.0
(17, 52) 0.0
(17, 53) 0.0
(17, 54) 1.0
(17, 55) 0.0
(17, 56) 0.0
(18, 54) -1.0
(18, 55) 0.0
(18, 56) 0.0
(18, 57) 1.0
(18, 58) 0.0
(18, 59) 0.0
This result is very different with previous format, so I am suffering to guess what is meaning. Can I get some help about this?

[Zentetsu]

Hello,

Concerning your problem, I faced the same issue a few days ago.
You can find a solution in the snippet of code I created in this discussion.

[hugtalbot]

I close this discussion @AbhiBjee @Zentetsu
If anything more is needed, do not hesitate to reopen it (if in the scope of @AbhiBjee 's discussion) or open a new discussion!