Contact using lagrange multiplier for deformable body

[shizi-19]

Hi Hugo, @hugtalbot

Thanks for your post in #3812.

I have further question to your answer in this post about getting the contact force.

Constraint matrix H 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. SO BY runing your code, I can get the contact force F.

This code works for the rigid cube in #4225. I have successfully tested it.

However, when it comes to a soft deformable object contact, the Jocobian matrix H has more than one contact direction for each element in λ.

For example, if the λ is [1, 2, 3] N, you have the following Jocobian matrix in #3812.

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

For each row, it is a constraint direction, but it has two array for each row, e.g. 1st line, it has [0.487691 -0.305337 0.146266], [ 0.333767 -0.208967 0.100102].

Which array you are using to multiply λ[1], which is [1]N?

For 2nd row, which array you are using to multiply λ[2], which is [2] N?

The code

        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
        contactforce_x = 0
        contactforce_y = 0
        contactforce_z = 0
        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))

        print('pointid', pointId)

        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
        
          print('indice', i, indice)
        
        for i in range (nbDofs):
            contactforce_x += forces[i][0]
            contactforce_y += forces[i][1]
            contactforce_z += forces[i][2]
            
      
        print('nbDof', nbDofs)
        print('force', forces)
        print('contactforce', contactforce_x, contactforce_y, contactforce_z)

        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

only considers the first array for each row, which would not be applicable if there are two array in each raw.

Thanks for your time.

[bakpaul]

The main difference between the soft body contact and the rigid example given by @hugtalbot is that soft bodies collision models are sometimes more than just points. Some times you want to use higher degrees elements such as edges, triangles, quats. In those cases, the contact point may arise inside of the element and not exactly on one of its points. When this happens, in order to ensure a mechanically sound behavior, the contact force is distributed on every points of the element (three for the triangle, two for the edge etc...). This distribution is not homogeneous but take into account the distance between the application point and each point of the element using barycentric coordinates.

Here in your example, because you have two points affected by each constrain, you are certainly using edges for you collision. The two normals given by constraints are the normals used to apply the contact force to the corresponding point of the element. As I told you, the forces are distributed, in fact the two normal are collinear and have the same direction, only the norms differs and are related to the barycentric coordinates. But you don't mind about that, what is important is that, the force applied at the point 58 by the constraint 0 in your example is λ[1]/dt*[0.487691 -0.305337 0.146266] and the force applied at the point 59 is λ[1]/dt*[ 0.333767 -0.208967 0.100102].

Now for the script, a more generic way of going through the H matrix would be to take into account the number of point affected by the constraint and loop over it. It would look more like that (I didn't run it on my side, but it should work fine) :

constraint = self.rootNode.Cube.collision.MechanicalObject.constraint.value
dt = self.rootNode.dt.value
constraintMatrixInline = np.fromstring(constraint, sep='  ')

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

forcesNorm = self.rootNode.GCS.constraintForces.value
contactforce_x = 0
contactforce_y = 0
contactforce_z = 0

while index < len(constraintMatrixInline):
    nbConstraint   = int(constraintMatrixInline[index+1])
    currConstraintID = int(constraintMatrixInline[index])
    for pts in range(nbConstraint):
        currIDX = index+2+pts*4
        pointId = np.append(pointId, constraintMatrixInline[currIDX])
        constraintId.append(currConstraintID)
        constraintDirections.append([constraintMatrixInline[currIDX+1],constraintMatrixInline[currIDX+2],constraintMatrixInline[currIDX+3]])
    index = index + 2 + nbConstraint*4

nbDofs = len(self.rootNode.Cube.collision.MechanicalObject.position.value)
forces = np.zeros((nbDofs,3))


print('pointid', pointId)
print('constraintId', constraintId)


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

    print('indice', i, indice)

for i in range (nbDofs):
    contactforce_x += forces[i][0]
    contactforce_y += forces[i][1]
    contactforce_z += forces[i][2]


print('nbDof', nbDofs)
print('force', forces)
print('contactforce', contactforce_x, contactforce_y, contactforce_z)

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

Don't hesitate to give me feedback on how it worked and if you had to fix it.

[shizi-19]

Thanks @bakpaul for answering my question. It makes sense now.

But after checking the collision, I am using point, line, and triangle as the collision models.

objectCollis.addObject('TriangleCollisionModel', moving=False, simulated=False, group=collisionGroup) objectCollis.addObject('LineCollisionModel', moving=False, simulated=False, group=collisionGroup) objectCollis.addObject('PointCollisionModel', moving=False, simulated=False, group=collisionGroup)

The number of constraints is 2 at maximum (this should be the line collision model). Does that mean the triangle collision is not in effect?

[em-ni]

Hello,
I've just started using v23.12 and I noticed that the constraint format is changed with respect to v22.12 from something like this:
0 3 182 0 0 0.0204268 187 0 0 0.0686725 188 0 0 0.910901 1 3 182 0.0204268 -0 -0 187 0.0686725 -0 -0 188 0.910901 -0 -0 2 3 182 -0 -0.0204268 -0 187 -0 -0.0686725 -0 188 -0 -0.910901 -0 3 3 182 0 -1.21112e-16 0.0340899 187 0 -8.70841e-17 0.024512 188 0 -3.34452e-15 0.941398 4 3 182 0.0340899 -4.30275e-31 -1.52864e-45 187 0.024512 -3.09385e-31 -1.09916e-45 188 0.941398 -1.18821e-29 -4.22138e-44 5 3 182 -4.30275e-31 -0.0340899 -1.21112e-16 187 -3.09385e-31 -0.024512 -8.70841e-17 188 -1.18821e-29 -0.941398 -3.34452e-15

to something like this:
Constraint ID : 0 dof ID : 179 value : 0 -4.01807e-17 0.0126877 dof ID : 182 value : 0 -1.08679e-16 0.0343173 dof ID : 188 value : 0 -3.01803e-15 0.952995 Constraint ID : 1 dof ID : 179 value : 0.0126877 -1.27248e-31 -4.0298e-46 dof ID : 182 value : 0.0343173 -3.44176e-31 -1.08997e-45 dof ID : 188 value : 0.952995 -9.55778e-30 -3.02685e-44 Constraint ID : 2 dof ID : 179 value : -1.27248e-31 -0.0126877 -4.01807e-17 dof ID : 182 value : -3.44176e-31 -0.0343173 -1.08679e-16 dof ID : 188 value : -9.55778e-30 -0.952995 -3.01803e-15 Constraint ID : 3 dof ID : 182 value : 0 -6.88612e-17 0.0193827 dof ID : 187 value : 0 -2.69186e-16 0.0757691 dof ID : 188 value : 0 -3.21467e-15 0.904848 Constraint ID : 4 dof ID : 182 value : 0.0193827 -2.44644e-31 -8.6915e-46 dof ID : 187 value : 0.0757691 -9.56341e-31 -3.3976e-45 dof ID : 188 value : 0.904848 -1.14208e-29 -4.05748e-44 Constraint ID : 5 dof ID : 182 value : -2.44644e-31 -0.0193827 -6.88612e-17 dof ID : 187 value : -9.56341e-31 -0.0757691 -2.69186e-16 dof ID : 188 value : -1.14208e-29 -0.904848 -3.21467e-15 Constraint ID : 6 dof ID : 182 value : 0 4.86011e-16 0.0342 dof ID : 187 value : 0 4.30079e-16 0.0302641 dof ID : 188 value : 0 1.32948e-14 0.935536

So I tried to update the code accordingly, however, I have a different output, the force vectors are drawn in the wrong position and the contact force values are different.
This is the updated code I'm using

`
def onAnimateEndEvent(self, event):
# This function is called at each end of animation step.
if self.debug : print("\nContactListener: onAnimateEndEvent")
# Retrieve the constraint value from the MechanicalObject of the collision model.
# This contains information about the constraints applied during the simulation step.
constraint = self.root_node.getChild(self.object_name).getChild(self.collision_name).MechanicalObject.constraint.value
# Parse constraint data (needed to pass from v22.12 to v23.12)
constraint = parse_constraint_data(constraint)
if self.debug : print(f"ContactListener: Constraint: {constraint}")
if self.debug : print(f"ContactListener: type(constraint): {type(constraint)}, len(constraint): {len(constraint)}")
# Get the simulation time step value for force computation.
dt = self.root_node.dt.value

    # Convert the constraint string into a numpy array for easier manipulation.
    constraintMatrixInline = np.fromstring(constraint, sep=' ')
    if self.debug : print(f"ContactListener: constraintMatrixInline: {constraintMatrixInline}")


    # Initialize variables to store point IDs and constraint directions.
    pointId = []
    constraintId = []
    constraintDirections = []
    index = 0
    i = 0

    # Retrieve the norm of forces applied by the constraints from the solver.
    forcesNorm = self.root_node.GenericConstraintSolver.constraintForces.value
    contactforce_x = 0
    contactforce_y = 0
    contactforce_z = 0

    # Prepare an array to store constraint directions, initialized to zeros.
    #if len(constraintMatrixInline) > 0:
        #constraintDirections = np.zeros((int(len(constraintMatrixInline)/6), 3))

    # Loop through the constraint matrix to extract constraint information.
    while index < len(constraintMatrixInline):
        if self.debug : print(f"ContactListener: index: {index}")
        nbConstraint   = int(constraintMatrixInline[index+1])
        currConstraintID = int(constraintMatrixInline[index])
        if self.debug : print(f"ContactListener: nbConstraint: {nbConstraint}, currConstraintID: {currConstraintID}")
        for pts in range(nbConstraint):
            currIDX = index+2+pts*4
            pointId = np.append(pointId, constraintMatrixInline[currIDX])
            constraintId.append(currConstraintID)
            constraintDirections.append([constraintMatrixInline[currIDX+1],constraintMatrixInline[currIDX+2],constraintMatrixInline[currIDX+3]])
            if self.debug : print(f"ContactListener: pointId: {pointId}, constraintId: {constraintId}, constraintDirections: {constraintDirections}")
        index = index + 2 + nbConstraint*4

    # Debugging print to check the extracted constraint directions.
    if self.debug : print(f"\nContactListener: Constraint Directions: {constraintDirections}")

    # Determine the number of degrees of freedom (DOFs) for the object.
    nbDofs = len(self.root_node.getChild(self.object_name).getChild(self.collision_name).MechanicalObject.position.value)
    # Initialize a matrix to store the computed forces for each DOF.
    forces = np.zeros((nbDofs, 3))

    # After retrieving forcesNorm
    if self.debug: print(f"ContactListener: forcesNorm size: {len(forcesNorm)}")

    for i in range(len(pointId)):
        indice = int(pointId[i])
        
        # Additional debug to ensure indices are within bounds
        if i < len(constraintDirections) and constraintId[i] < len(forcesNorm):
            if indice < nbDofs:
                forces[indice][0] += constraintDirections[i][0] * forcesNorm[constraintId[i]] / dt
                forces[indice][1] += constraintDirections[i][1] * forcesNorm[constraintId[i]] / dt
                forces[indice][2] += constraintDirections[i][2] * forcesNorm[constraintId[i]] / dt
            else:
                if self.debug: print(f"ContactListener: Warning: indice {indice} out of bounds for forces array with size {nbDofs}")
        else:
            if self.debug: print(f"ContactListener: Warning: Access out of bounds. i: {i}, constraintId[i]: {constraintId[i]}, len(constraintDirections): {len(constraintDirections)}, len(forcesNorm): {len(forcesNorm)}")

    for i in range (nbDofs):
        contactforce_x += forces[i][0]
        contactforce_y += forces[i][1]
        contactforce_z += forces[i][2]
        
  
    if self.debug : print('\nContactListener: nbDof: ', nbDofs)

    # Visualize the computed forces in the simulation.
    if len(constraintMatrixInline) > 0:
        if self.debug : print(f"ContactListener: indices: {list(range(0,nbDofs,1))}")
        if self.debug : print(f"ContactListener: forces: {forces}")
        if self.debug : print(f"ContactListener: position: {self.root_node.getChild(self.object_name).getChild(self.collision_name).MechanicalObject.position.value}")
        self.root_node.drawNode.drawForceFF.indices.value = list(range(0,nbDofs,1)) 
        self.root_node.drawNode.drawForceFF.forces.value = forces
        self.root_node.drawNode.drawPositions.position.value = self.root_node.getChild(self.object_name).getChild(self.collision_name).MechanicalObject.position.value

    if self.debug : print('\nContactListener: contactforce: ', contactforce_x, contactforce_y, contactforce_z)

def parse_constraint_data(constraint_str):
# Regular expression to extract constraint data
pattern = re.compile(r"Constraint ID : (\d+) +dof ID : (\d+) +value : ([-\d.e]+) ([-\d.e]+) ([-\d.e]+)")
matches = pattern.findall(constraint_str)

# Group data by Constraint ID
constraints = defaultdict(list)
for match in matches:
    constraint_id, dof_id, *values = match
    constraints[int(constraint_id)].append((int(dof_id), [float(val) for val in values]))

# Sort and format the data
formatted_constraints = []
for constraint_id, dofs in sorted(constraints.items()):
    dofs.sort(key=lambda x: x[0])  # Sort by dof ID, if necessary
    formatted_constraint = f"{constraint_id} {len(dofs)}"
    for dof_id, values in dofs:
        formatted_constraint += f" {dof_id} {' '.join(map(str, values))}"
    formatted_constraints.append(formatted_constraint)

return "\n".join(formatted_constraints)

`

what am I doing wrong?
Thanks in advance

[Slykkey]

Hello @Emanuele-n ,

I also encountered this, I wrote a small script to just extract forces. I do get the values so if you would like to test it, here is the script I wrote. I didn't draw the forces so you might have to add that.

constraint = self.rootNode.Instrument.CollisionModel.CollisionDOFs.constraint.value

 lamb = self.rootNode.GCS.constraintForces.value
 nDOFs = len(self.rootNode.Instrument.CollisionModel.CollisionDOFs.position.value)
 splitconst = constraint.split('\n')
 contactForces = np.zeros((nDOFs,3))
 for x in splitconst:
     final_list = re.findall(r'-?\ *[0-9]+\.?[0-9]*(?:[Ee]\ *-?\ *[0-9]+)?', x)
     if len(final_list)<6 & len(final_list)>0:
           dofID = int(final_list[1])
                constID = int(final_list[0])
                contactForces[dofID, 0] = contactForces[dofID, 0] + (lamb[constID] * float(final_list[2]) / dt)
                contactForces[dofID, 1] = contactForces[dofID, 1] + (lamb[constID] * float(final_list[3]) / dt)
                contactForces[dofID, 2] = contactForces[dofID, 2] + (lamb[constID] * float(final_list[4]) / dt)
           elif len(final_list)>6:
                dofID = int(final_list[1])
                dofID_2 = int(final_list[5])
                constID = int(final_list[0])
                contactForces[dofID, 0] = contactForces[dofID, 0] + (lamb[constID] * float(final_list[2]) / dt)
                contactForces[dofID, 1] = contactForces[dofID, 1] + (lamb[constID] * float(final_list[3]) / dt)
                contactForces[dofID, 2] = contactForces[dofID, 2] + (lamb[constID] * float(final_list[4]) / dt)
                contactForces[dofID_2, 0] = contactForces[dofID_2, 0] + (lamb[constID] * float(final_list[6]) / dt)
                contactForces[dofID_2, 1] = contactForces[dofID_2, 1] + (lamb[constID] * float(final_list[7]) / dt)
                contactForces[dofID_2, 2] = contactForces[dofID_2, 2] + (lamb[constID] * float(final_list[8]) / dt)
 totaly = 0
 totalx = 0
 totalz = 0
 for x in contactForces[:,0]:
     if x!=0:
              totalx = totalx + x
 for y in contactForces[:,1]:
      if y!=0:
             totaly = totaly + y
 for z in contactForces[:,2]:
     if z!=0:
             totalz = totalz + z

Let me know if this doesn't work or if you have any questions.

[pasqualeferr94]

Dear @Slykkey thanks for posting your solution, as also @Emanuele-n I have the same problem.
I tried your code, but it gives me a problem on this function:
final_list = re.findall(r'-?\ [0-9]+.?[0-9](?:[Ee]\ *-?\ *[0-9]+)?', x)

How did you defined the variable "re"? I think is a misintg information from your code.
Cheers

[Slykkey]

Hello @pasqualeferr94 ,

Try importing re, it's a package on Python which helps you search through strings with specific conditions. This helps me separate the numbers from the constraint values. From here on, I take only the constraint ID, dof ID and the X Y Z values and use the appropriate multipliers to save it to the array in X, Y, Z respectively. Let me know if you have any other doubts.

[bhattner143]

Hello everyone

Did anyone get the final code? I have been trying to implement the code on the original cube-floor program, but the "splitconst" is always empty and hence I am getting zero for all the forces.

[zhanglepy]

hello,
May I ask if anyone has a complete open source project or final code that can be shared.

[hugtalbot]

Hi guys @shizi-19 @zhanglepy @bhattner143 @Slykkey

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

[Paolo-Susini]

Hi @hugtalbot,

I am very interested in this topic, I looked on the example you made, but I cannot replicate because I don't have the VisualVectorField component. I'm working with SOFA 24.12 and I saw that the VisualVectorField is available only inside the master branch.
I am trying to use this version with the ConstantForceField.

What are the differences? Is the new component going to be included or added in some way?

I do have another question, is there a way to detect the contact area between two deformable objects during contact? (i was looking at [this discussion`(https://github.com//discussions/5631), but it seems not resolved yet).

Regards