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',
                         name="DOFs",
                         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

[bakpaul]

Hello, I am sorry for the answer time.

The issue might come from the PreComputedConstraintCorrection this ConstraintCorrection will only work as long as the deformation is small without rotations. When you are touching an organ with a rigid object, the neighborhood of this contact point will deform a lot and be subject to rotation. I suggest to change it to LinearSolverConstraintCorrection

If the problem isn't solved by this, could you attach a video of the behavior ?

[Lorenzo-Mazza]

Hi Paul,
Thank you very much for the help and the provided suggestion.
I used LinearSolverConstraintCorrection and the stability seems to improve.
I attached 2 videos, in the first one the tool has a velocity of ca. 0.3 m/s (probably too fast) the instability seems to persist. In the second one, I reduced the velocity by a factor of 0.01 (by reducing the positional offset at each key pressed event) and the situation improved! There are still a few stability issues when the tool compenetrates the liver. Do you think it's just a problem of velocity, i.e. should I reduce it even more? Do the liver parameters look okay to you?

sandbox__r60_0023.mp4

sandbox__r60_0024.mp4

[Lorenzo-Mazza]

Hi Paul,
thank you for your answer! I had your approach in mind, but I was looking for a scalable way to tackle the problem, i.e. work for any type of surgical mesh and location in space. Defining fixed-location spheres on the mesh did not quite satisfy my goal, so I opted for a triangle/line/point collision model instead.
Do you think it's not feasible?

[ScheiklP]

The alternative would be to create a low-resolution mesh from the visual mesh. For example by creating a convex hull, but that might be invalid for the cauter hook. I feel for most of these things, there is a trade-off between how general you can write the scene at the cost of speed, and manually doing stuff to speed it up for specific cases. :D
For this case, you could also try skeletonizing the mesh to use that as the basis for where to place sphere collision models. https://github.com/navis-org/skeletor

[Lorenzo-Mazza]

Hi,
I tried to create a low resolution collision model by exploiting the cool library you linked.
Unfortunately, although the computation doesn't freeze altogether, there are still some physical problems in the soft body deformation.

This is the simplified collision model of the cautery hook, consisting only of a bunch of spheres along the visual mesh.

skeleton_mesh__r60_0002.mp4

This is what happens, it seems like the realistic deformation is not quite there yet.

[ScheiklP]

That still looks like a velocity problem. :D
What's the translational speed of the instrument tip?

[Lorenzo-Mazza]

Oh 😅
Currently in cartesian coordinates is
Linear velocity [m/s]: [-1.69949749e-03 5.71433690e-04 9.94613090e-08]