Collision model works between a rigid and hex model but not between rigid models

[chz31]

Hi,

In my scene, I have two rigid objects and one hexdedron object. The collision detection and response works well between a rigid and the hex models. However, though collision detection works between two rigid models, but the response did not work. Therefore, one object simply penetrated another object:

Here is how I set up the collision pipeline:

Collision pipeline

root.addObject('CollisionPipeline')
root.addObject('ParallelBruteForceBroadPhase')
root.addObject('ParallelBVHNarrowPhase')
root.addObject('CollisionResponse', responseParams="mu=" + str(5.0), name='Response',
               response='FrictionContactConstraint')  # FrictionContactConstraint or PenaltyContact
root.addObject('LocalMinDistance', alarmDistance=2, contactDistance=0.1)

Here is how I set up the collision model for a rigid body. I tried both "Line" and "PointCollisionModel".

orbitNode = root.addChild("Orbit")
orbitNode.addObject('EulerImplicitSolver', name='odesolver')
orbitNode.addObject('CGLinearSolver', name='Solver', iterations=25, tolerance=1e-05, threshold=1e-05)
orbitNode.addObject('MechanicalObject', name="mstate", template="Rigid3", showObject=True)

orbitNode.addObject('BoxROI', name='orbitFixed', template='Rigid3',
                    position='@mstate.position', box='-1e6 -1e6 -1e6  1e6 1e6 1e6')
orbitNode.addObject('FixedProjectiveConstraint', indices='@orbitFixed.indices')
orbitNode.addObject('UniformMass', name="mass", vertexMass=[totalMass, volume, inertiaMatrix[:]])
orbitNode.addObject('UncoupledConstraintCorrection')  #LinearSolverConstraint for collision

# Add collision model.
orbitCollision = orbitNode.addChild('orbitCollisionModel')
orbitCollision.addObject('MeshTopology', src=orbitLoaderFine.getLinkPath())
orbitCollision.addObject('MechanicalObject', name='CollisionMO')
orbitCollision.addObject('TriangleCollisionModel', name='CollisionModel', contactStiffness='100')
orbitCollision.addObject('PointCollisionModel')
# orbitCollision.addObject('LineCollisionModel')
orbitCollision.addObject('RigidMapping')

Could you give me some suggestions? Thank you very much!

[bakpaul]

Hello !

Collision with a deformable object will most of the time have a solution with a local minimum of energy thank's to the deformable object being able to transform the contact energy and the inertia into deformation energy. Btu this is not the case with two rigid objects. With rigid object, no deformation occurs, and all energy transfert has to be done through inertia, leading to having a unique solution.

Now depending on how you move your object, this limitation might lead to an over-constrained problem where you'll get this kind of behavior.

How do you move your probe ?

[chz31]

@bakpaul Thanks for the response!

I moved the probe by pressing the keyboard using a controller class:

toolController = root.addChild('toolController')
toolCMState = toolController.addObject('MechanicalObject', name='controllerMO', position=scoopCenterOfMass,
                                       template='Rigid3')

root.addObject(ScoopController(name="ScoopController", mstate=toolCMState, delta=0.05))

Link to the probe node

toolMo = toolNode.addObject('MechanicalObject', template='Rigid3', name='MechanicalModel',
#
toolNode.addObject('BilateralLagrangianConstraint', template='Rigid3',
                   object1=toolMo.linkpath,
                   object2=toolCMState.linkpath,
                   first_point=0,
                   second_point=0)
#
toolNode.addObject('UncoupledConstraintCorrection')  # need for gravity filed #LinearSolverConstraint
toolCollision = toolNode.addChild('toolCollision')
toolCollision.addObject('MeshTopology', src=toolMeshLoaderFine.getLinkPath())
toolCollision.addObject('MechanicalObject', name='CollisionMO')
toolCollision.addObject('TriangleCollisionModel', name='CollisionModel', contactStiffness='100')
toolCollision.addObject('PointCollisionModel')
toolCollision.addObject('RigidMapping', globalToLocalCoords=True)

Here is the controller class:

class ScoopController(Sofa.Core.Controller):
"""
This Controller simply takes the cableActuator's value
and increases / decreases it depending on the pressed key ('+' or '-')
"""
    
    def __init__(self, mstate, delta=1.0, *a, **kw):
        Sofa.Core.Controller.__init__(self, *a, **kw)
        self.mstate = mstate
        self.delta = delta
        return

    def onKeypressedEvent(self, e):
        currentPos = self.mstate.position[0]
        if e["key"] == Sofa.constants.Key.KP_4:
            currentPos[1] += self.delta
        elif e["key"] == Sofa.constants.Key.KP_6:
            currentPos[1] -= self.delta
        elif e["key"] == Sofa.constants.Key.KP_8:  # Move on Z axis
            currentPos[2] += self.delta
        elif e["key"] == Sofa.constants.Key.KP_2:
            currentPos[2] -= self.delta
        self.mstate.position = [currentPos]

[bakpaul]

Ok so the behavior makes sense. When the two are colliding, if you have non zero interpenetration, then your constrained problem has no solution. To solve that you have 3 solutions:

1. Avoid collision between the two objects
2. Make the skull or the tool soft, but this is not wanted I guess
3. Don't move the tool with a lagrangian constraint and instead use a 6 dofs spring or a multi points 3 dofs spring to avoid having numerical issue with rotational forces.

If I were you, I would go for the 'multiple point, 3 dofs' spring. you could map surface points of the probe model to the external rigid 'master'. Then you could add spring between this point cloud and the same points on the real probe so when you move the master, the point cloud moves rigidly with it and moves the probe in a compliant way thanks to the springs.

[chz31]

Thanks!

I think the multi points 3 dofs spring makes sense. I hope I understand you correctly. Does it mean the master should be mapped to the controller's mechanical object controllerMo as well?

Here is my set up for the probe object. I created a masterCloud and toolCloud objects for the spring model.

toolNode = root.addChild("tool")
# Creating the toolNode object
toolNode.addObject('EulerImplicitSolver', name='odesolver')
toolNode.addObject('CGLinearSolver', name='Solver', iterations=25, tolerance=1e-05, threshold=1e-05)

toolMo = toolNode.addObject('MechanicalObject', template='Rigid3', name='MechanicalModel',
                            position=scoopCenterOfMass, showObject=True, showObjectScale=1.0)
toolNode.addObject('UniformMass', name="mass", totalMass=10)


toolNode.addObject('UncoupledConstraintCorrection')  # need for gravity filed #LinearSolverConstraint

##############################################################################3
#master point cloud (follows controllerMO) 
masterCloud = toolController.addChild('MasterCloud')  # parented to controller
masterCloud.addObject('MeshOBJLoader', name='ptsLoader',
                      filename='/home/chi/Documents/orbit_SOFA/soft_tissue-models/tool.obj')  # can be a sparse OBJ of sampled points
masterCloud.addObject('MechanicalObject', name='x', template='Vec3d', src='@ptsLoader')
masterCloud.addObject('RigidMapping', input='@../controllerMO', output='@x')  # master pose -> points


#Tool point cloud (follows actual tool rigid) 
toolCloud = toolNode.addChild('ToolCloud')
toolCloud.addObject('MeshOBJLoader', name='ptsLoader',
                    filename='/home/chi/Documents/orbit_SOFA/soft_tissue-models/tool.obj')  # same vertices/order
toolCloud.addObject('MechanicalObject', name='x', template='Vec3d', src='@ptsLoader')
toolCloud.addObject('RigidMapping', input='@../MechanicalModel', output='@x')  # tool pose -> points

#Use the spring model
root.addObject(CloudSpringInitializer(masterCloud, toolCloud, k=5e3, damping=1.0))

###########################################################################

toolCollision = toolNode.addChild('toolCollision')
toolCollision.addObject('MeshTopology', src=toolMeshLoaderFine.getLinkPath())
toolCollision.addObject('MechanicalObject', name='CollisionMO')
toolCollision.addObject('TriangleCollisionModel', name='CollisionModel', contactStiffness='100')
toolCollision.addObject('PointCollisionModel')
toolCollision.addObject('RigidMapping', globalToLocalCoords=True) 

I don't know how to set up the spring model you mentioned, so below is chatgpt generated:

class CloudSpringInitializer(Sofa.Core.Controller):
    def __init__(self, masterNode, toolNode, k=5e3, damping=1.0, *a, **kw):
        super().__init__(*a, **kw)
        self.masterX = masterNode.getObject('x')
        self.toolX   = toolNode.getObject('x')
        self.k = k
        self.damping = damping
        self._added = False

    def onAnimateBeginEvent(self, *_):
        if self._added:  # only once, after topology sizes are known
            return
        n = min(len(self.masterX.position.value), len(self.toolX.position.value))
        springs = " ".join(f"{i} {i} {self.k} {self.damping} 0.0" for i in range(n))
        self.getContext().addObject('StiffSpringForceField',
                                    name='CloudCoupling',
                                    template='Vec3d',
                                    object1=self.masterX.getLinkPath(),
                                    object2=self.toolX.getLinkPath(),
                                    spring=springs)
        self._added = True

The good news is that I can use the mouse to attach a spring to move the probe and the collision between the probe and the rigid bone is activated. However, the controller stopped working. I do want to have the controller to allow more subtle and stable control of the probe.

I am not sure if this is the correct way to set up the multipoint spring model. Could you give some suggestions and examples?

Thanks!

[chz31]

@bakpaul I tried to follow the example by adding the interface to the deformable eyeNode (using Hexa force field) first:

  #Create interface
  eyeNode.addObject('BoxROI', name='interfaceROI', template='Vec3d', position="@MechanicalModel.position",
                    box="0 0 0  -300 -300 300")
  mappedSoft = eyeNode.addChild('MappedInterface')  # deformable interface
  mappedSoft.addObject('MechanicalObject', template='Vec3d', name='MappedDoFs',
                       position='@../interfaceROI.pointsInROI', showObject=True, showObjectScale=5)
  # Make these points follow the deformable
  mappedSoft.addObject('BarycentricMapping', input='@../MechanicalModel', output='@MappedDoFs')

I then added the interface to the rigid toolNode:

toolMappedRigid = toolNode.addChild('toolMappedRigidInterface')  # rigid interface
toolMappedRigid.addObject('MechanicalObject', template='Vec3d', name='MappedDoFs',
                      # initialize at the SAME positions as the soft interface:
                      position='@/eye/MappedInterface/MappedDoFs.position',
                      showObject=True, showObjectScale=5)
toolMappedRigid.addObject('RigidMapping', input='@../MechanicalModel', output='@MappedDoFs',
                      globalToLocalCoords=True)

Also the bilateral constraint following the example:

idx_str = "0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35"
root.addObject('BilateralLagrangianConstraint',
               template='Vec3d',
               name='Attachment',
               object1="@Orbit/MappedInterface/MappedDoFs",
               object2= "@tool/toolMappedRigidInterface/MappedDoFs",
               first_point=idx_str,
               second_point=idx_str)

However, I got the error of not creating 'Vec3d' mechanical object in object 1 in the above bilateral constraint, which shouldn't be the case:

[ERROR]   [SofaPython3::SceneLoader] Unable to completely load the scene from file '/home/chi/Documents/slicersofa_sofa_scratches/test_ffddistancegridcollision_intermediate_points.py'.  
Python exception:   
  ValueError: Object type BilateralLagrangianConstraint<Vec3d> was not created  
The object is in the factory but cannot be created.  
Requested template : Vec3d  
Used template      : Vec3d  
Reason(s)          : Data attribute 'object1' does not point to a valid mechanical state of datatype 'Vec3d'.  
Also tried to create the object with the template 'CudaVec3d' but failed for the following reason(s):  
    1. Data attribute 'object1' does not point to a valid mechanical state of datatype 'CudaVec3d'.  
Also tried to create the object with the template 'CudaVec3d1' but failed for the following reason(s):  
    1. Data attribute 'object1' does not point to a valid mechanical state of datatype 'CudaVec3d1'.  
Also tried to create the object with the template 'CudaVec3f' but failed for the following reason(s):  
    1. Data attribute 'object1' does not point to a valid mechanical state of datatype 'CudaVec3f'.  
Also tried to create the object with the template 'CudaVec3f1' but failed for the following reason(s):  
    1. Data attribute 'object1' does not point to a valid mechanical state of datatype 'CudaVec3f1'.  
Also tried to create the object with the template 'Rigid3d' but failed for the following reason(s):  
    1. Data attribute 'object1' does not point to a valid mechanical state of datatype 'Rigid3d'.  
  


At:
  /home/chi/Documents/slicersofa_sofa_scratches/test_ffddistancegridcollision_intermediate_points.py(366): createScene

In the meantime, I had no problem in the collision between the Hexa eye object and the rigid tool or bone model in the original script. I only got issue between the two rigid models. In this case, should I create an interface between the orbit and tool object? I tried it, but it reported an analogous error of object1 not created as a 'Rigid3d'.

Thanks!

[bakpaul]

Hello !

As I told you, to manage contact with two rigid objects, you'll need spring attachment. So instead of trying to fix your scene I suggest replacing the BilateralConstraint by a RestShapeSpringforceField.

Tell me how it goes !

[chz31]

@bakpaul Sorry for the late response!

I hope I understand it correctly. How would I replace BilateralConstraint by a RestShapeSpringForceField? I thought RestShapeSpringForceField is only for adding spring models to return the object to its rest state?

Did you mean I should add a RestShapeSpringsModel to the external rigid 'master' you suggested previously to move the actual probe? I could add a spring model to move the probe and create an actual collision between the probe and the rigid orbit, but the controller then failed to move.

If I were you, I would go for the 'multiple point, 3 dofs' spring. you could map surface points of the probe model to the external rigid 'master'. Then you could add spring between this point cloud and the same points on the real probe so when you move the master, the point cloud moves rigidly with it and moves the probe in a compliant way thanks to the springs.

Or did you mean I should set the new rest state of the probe as the next position moved by the controller?

Thanks!

[hugtalbot]

Any update @chz31 ?

[chz31]

@hugtalbot Sorry for the delaying! I was working on a different simulation step. I have closed this issue for now. I will open a new one when I revisit this issue.