How to access the contact forces when using constraint resolution based on Lagrange multipliers

[hugtalbot]

Further to several discussions ( #2871 #3228 #3275 ), it appeared that a summary on "how to access contact forces when using constraint resolution based on Lagrange multipliers" was necessary.

Context

This post therefore addresses the case of collision solved as a constraint problem. When solving constraints (here we address the case of contacts) using the Lagrange multipliers, your SOFA scene should contain:

• a FreeMotionAnimationLoop
• a ConstraintSolver : either LCPConstraintSolver or GenericConstraintSolver
• a DefaultContactManager with its data response="FrictionContactConstraint" (handling both friction or frictionless contacts)
• each simulated object should also have its own ConstraintCorrection

In this context, let's detail how to access the contact forces.

Context

To do so, the ConstraintSolver you use (either LCPConstraintSolver or GenericConstraintSolver) should define the data computeConstraintForces="1". This boolean data will activate the computation of a constraintForces data. Resulting from the constraint resolution, this data constraintForces saves the constraint impulse : the time step where is the vector of constraint forces (contact forces in case of contacts) in the constraint space. In the case of frictional contacts in 3d, it means each contact force is a vector of 3 values corresponding to the force intensities along the normal and the two tangential directions:

Instead, what most of the users are interested in are the contact forces expressed in the world coordinates (x,yz):

To compute these forces, we need to compute the back-transformation from the constraint space towards the world coordinate space :

where is the constraint Jacobian matrix containing the constraint directions, i.e. the contact directions. This matrix is the Jacobian of the transformation between the world coordinate system and the constraint space. This constraint Jacobian matrix is available in a constraint data structure (C++ map of map) of the MechanicalObject of the collision model of your object.

This map-map structure of the data constraint corresponding to can be misleading.
Here is an example:

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

The above data means that we have 3 constraints (3 lines), each of them affecting 2 degrees of freedom (nodes) of the collision model. These constraints correspond to frictional contacts since each constraint is a vector of 3 floating values (n, t1, t2). The structure of the constraint data field can be generalized as follows:

constraintID = 0	nbConstraint = 2	pointIDconcernedByConstraint1=58	contactDirection_x1	contactDirection_y1	contactDirection_z1	pointIDconcernedByConstraint2=59	contactDirection_x2	contactDirection_y2	contactDirection_z2
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

Example with a python script

Click to expand the example 👇

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


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
        dt = self.rootNode.dt.value
        constraintMatrixInline = np.fromstring(constraint, sep='  ')

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

        forcesNorm = self.rootNode.GCS.constraintForces.value

        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




def createScene(rootNode):
    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]

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

    floor = Floor(rootNode,
                  name="Floor",
                  translation=[0.0, -300.0, 0.0],
                  uniformScale=5.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=8)
    drawNode.addObject('ConstantForceField', name="drawForceFF", force=[0,0,0], showArrowSize=1)


    return rootNode

If this helps you in getting the forces, please upvote this discussion using t he "up" ⬆️ button below.
@nhnhan92 @DanteAngio8 @noteddeh @@oystebje @camilla-agabiti @NotJed317 @satimmer @peyronq1 @BrunoB81HK

Cheers

[oystebje]

Thanks Hugo! Much appreciated.

[hugtalbot]

Please note a major update in the above explanation @oystebje

In short. Resulting from the constraint resolution, this data constraintForces saves the constraint impulse : the time step where is the vector of constraint forces (contact forces in case of contacts) in the constraint space. To access constraint forces in the constraint space, the values of the data constraintForces should therefore be divided by the time step (see the updated python example)

[bakpaul]

To get a more generic script not only working with rigid but with deformable bodies also, see #4415

[hugtalbot]

For your information, I just created an example as a PR in the SofaPython3 repository describing how to easily access (and visualize) contact forces, when using the response method "FrictionContactConstraint".

See ➡️ https://github.com/sofa-framework/SofaPython3/pull/514/files