How to setup collision between soft tissue and rigid body using Free Motion AL

[Lorenzo-Mazza]

Hi everyone,

I have been trying to implement a collision model between a soft tissue (a liver) and a rigid body (a surgical hook).
My goal is to control the surgical hook via a position-based controller and actively create deformations of the soft tissue.
For the controller, I based myself upon the templates from the 'sofa_env' project by @ScheiklP (https://github.com/ScheiklP/sofa_env/tree/main).
However I have been struggling to find a plausible solution.
The only examples I found in the SOFA codebase regarding a liver and a FreeMotionAnimationLoop are the following:

https://github.com/sofa-framework/sofa/blob/88320b856fea453ee6311d922f888446acda8018/applications/plugins/Geomagic/scenes/Geomagic-FEMLiver.scn

https://github.com/sofa-framework/sofa/blob/88320b856fea453ee6311d922f888446acda8018/applications/plugins/MultiThreading/examples/livers.scn

https://github.com/sofa-framework/sofa/blob/88320b856fea453ee6311d922f888446acda8018/applications/plugins/Haption/examples/haption1.scn

I tried to use the same liver parameters as in the examples, but they result in unstable results. The liver seems way too soft, even when tweaking the params and choosing a very high Young Modulus.

I am not able to implement a realistic collision so far. I am not sure whether the issue lies in the position-based controller, in my scene definition or in something else.
Following a suggestion by @ScheiklP, I tried to tweak the tool controller to slow down the hook velocity when moving (by reducing the position offset after every OnKeyPressedEvent). Unfortunately the effect is the same. When the objects collide, either the simulation becomes unstable or the liver is still too soft and deformed in unrealistic ways.
https://github.com/user-attachments/assets/623bd272-848a-4771-aca0-649f1963da13

Below, I tried to re-write the code to show a minimally understandable example (in my actual code I am using a lot of helper functions coming from sofa_env). I hope I did not forget anything.

Feel free to have a look. I would really appreciate if someone had an intuition on what I am doing wrong.
The scale of the meshes is in meters.
SOFA version is 24.06.

Thanks,

Lorenzo Mazza

    rootNode.dt = 0.01  # I tried 0.001 as well
    rootNode.gravity = [0, 0, 0]  # No gravity for testing

    rootNode.addObject('FreeMotionAnimationLoop')
    root_node.addObject('LCPConstraintSolver', tolerance=1e-2, maxIt=100, initial_guess=False, build_lcp=True, mu=0.2)
    rootNode.addObject('DefaultVisualManagerLoop')
    rootNode.addObject('VisualStyle', displayFlags="showVisual")

    rootNode.addObject('CollisionPipeline', name="CollisionPipeline")
    rootNode.addObject('BruteForceBroadPhase')
    rootNode.addObject('BVHNarrowPhase')
    rootNode.addObject('CollisionResponse', name="CollisionResponse", response="FrictionContactConstraint")
    rootNode.addObject('LocalMinDistance', name="Proximity", alarmDistance=0.05, contactDistance=0.001)


    liver = rootNode.addChild('liver')
    

    liver.addObject('EulerImplicitSolver',
                   name='odesolver',
                   rayleighStiffness=0.1,
                   rayleighMass=0.1)
    
    liver.addObject('CGLinearSolver',
                   name='linearSolver',
                   iterations=25,
                   tolerance=1e-8,
                   threshold=1e-8,
                   template="CompressedRowSparseMatrixMat3x3d")

    liver.addObject('MeshVTKLoader',
                   name='loader',
                   filename='liver_volume.vtk',
                   scale=1)
    
    liver.addObject('TetrahedronSetTopologyContainer',
                   src='@loader',
                   name='topo')
    
    liver.addObject('MechanicalObject',
                   name='dofs',
                   template='Vec3d',
                   src='@loader')
    
    liver.addObject('TetrahedronSetGeometryAlgorithms')
    liver.addObject('MeshMatrixMass',
                   massDensity=1080)  # kg/m^3
    
    liver.addObject('TetrahedralCorotationalFEMForceField',
                   name='FEM',
                   youngModulus=80000,  # Pa
                   poissonRatio=0.48,
                   method='large')
    
    liver.addObject('PrecomputedConstraintCorrection')

    liver.addObject('FixedProjectiveConstraint',
                   name="FixedConstraint",
                   indices="3 39 64")

    visual = liver.addChild('visual')
    visual.addObject('MeshSTLLoader',
                    name='loader',
                    filename='liver_surface.stl',
                    scale=1)
    visual.addObject('OglModel',
                    src='@loader')
    visual.addObject('BarycentricMapping')

    collision = liver.addChild('collision')
    collision.addObject('MeshSTLLoader',
                       name='loader',
                       filename='liver_surface.stl',
                       scale=1)
    collision.addObject('MeshTopology',
                       src='@loader')
    collision.addObject('MechanicalObject')
    collision.addObject('TriangleCollisionModel')
    collision.addObject('LineCollisionModel')
    collision.addObject('PointCollisionModel')
    collision.addObject('BarycentricMapping')
    
    # Rigid body
    hook = scene.addChild('hook')
    
    hook.addObject('EulerImplicitSolver',
                  name='odesolver',)
    hook.addObject('CGLinearSolver',
                  name='linearSolver',
                  iterations=25,
                  tolerance=1e-10,
                  threshold=1e-10)

    # Create motion control node
    hook_motion = hook.addChild('hook_motion')
    hook_motion.addObject('MechanicalObject',
                         template='Rigid3d',
                         position=[0, 0.5, 0, 0, 0, 0, 0],  # some example position)

    # Create physical body node
    hook_body = hook.addChild('hook_body')
    hook_body.addObject('MechanicalObject',
                       template='Rigid3d',
                       position=[0, 0.5, 0, 0, 0, 0, 0])
    
    hook_mass = 0.1
    hook_body.addObject('UniformMass', totalMass=hook_mass)
    hook_body.addObject('RestShapeSpringsForceField',
                       stiffness=2e10,
                       angularStiffness=2e10,
                       external_rest_shape='@../hook_motion/DOFs')
    
    hook_body.addObject('UncoupledConstraintCorrection', compliance=1.0 / hook_mass)

    hook_visual = hook_body.addChild('hook_visual')
    hook_visual.addObject('MeshSTLLoader',
                         name='loader',
                         filename='Hook.stl',
                         scale=1)
    hook_visual.addObject('OglModel',
                         src='@loader')
    hook_visual.addObject('RigidMapping')

    hook_collision = hook_body.addChild('hook_collision')
    hook_collision.addObject('MeshSTLLoader',
                           name='loader',
                           filename='Hook.stl',
                           scale=1)
    hook_collision.addObject('MeshTopology',
                           src='@loader')
    hook_collision.addObject('MechanicalObject')
    hook_collision.addObject('TriangleCollisionModel')
    hook_collision.addObject('LineCollisionModel')
    hook_collision.addObject('PointCollisionModel')
    hook_collision.addObject('RigidMapping')

The controller updates the location of the tool in a similar manner:

    def onKeypressedEvent(self, event):
        key = event["key"]
        if ord(key) == 20:  # right
            self.do_action(np.array([0.05, 0.0, 0.0, 0.0])) #Pan-Tilt-Spin-Depth
        elif ord(key) == 18:  # left
            self.do_action(np.array([-0.05, 0.0, 0.0, 0.0]))
        elif ord(key) == 19:  # up
            self.do_action(np.array([0.0, 0.05, 0.0, 0.0]))
        elif ord(key) == 21:  # down
            self.do_action(np.array([0.0, -0.05, 0.0, 0.0]))
          ...

Where method do_action simply converts Pan-Tilt-Spin-Depth coordinates to cartesian coordinates and adds them to the tool current cartesian pose in the following way (taken by sofa_env) :

state = ptsd_state + action
pose = self.pivot_transform(state)   # conversion to cartesian coordinate system
with hook_motion.position.writeable() as sofa_pose:
    sofa_pose[:] = pose