What is the best way to have soft-rigid interactions

Hello everyone!

I am working with SOFA as a part of my bachelor’s thesis project. In this project, we are interested to design a prosthetic hand that is capable of grasping physical objects. I took the Trunk example of SoftRobots as a template and modified the code a bit to contract the cables.

I wanted to ask, what is the best way to develop the interactions between the soft material of the trunk and the rigid material of the cylinder? What kind of collision model, mesh topology, solver, etc. would you use?

I have noticed a few interesting features of the system:
1) As friction in SOFA is done using collisions, it’s obvious that the more collisions points there are between cylinder and trunk the better grasping is. I changed the collision model from ‘LocalMinDisntace’ to ‘MinProximityIntersection’. There are more collision points this way but the simulation stats to lag.
2) While grasping, sometimes the mesh of the trunk would overlap with the cylinder mesh, even though it’s not supposed to do so.

I hope this explains the issue well. My code is here:

from splib.objectmodel import SofaPrefab, SofaObject
from splib.numerics import Vec3, Quat
from splib.animation import animate, AnimationManager
from stlib.visuals import ShowGrid
from stlib.scene import MainHeader, ContactHeader
from stlib.physics.rigid import Cube, Floor, RigidObject
from math import cos, sin, sqrt
import Sofa

import os
path = os.path.dirname(os.path.abspath(__file__))+'/mesh/'
dirPath = os.path.dirname(os.path.abspath(__file__))+'/'

def printObjects(objects ):
    for o in objects:
        print "   ",o.getPathName(), type(o)

class TrunkController(Sofa.PythonScriptController):
    total_time = 0
    floor_transitioned = False
    def __init__(self, parentNode, trunkNode, automation=True):
        self.name = "TrunkController"
        self.parentNode = parentNode
        self.trunk = trunkNode
        self.automation = automation
        if self.trunk is not None:
            self.cables = self.trunk.getObjects("SearchDown", "CableConstraint")
            self.cablesConstraintValues = []
            for cable in self.cables:
                self.cablesConstraintValues.append(cable.findData('value'))

    def onKeyPressed(self,c):
        if len(self.cablesConstraintValues) != 0:
            # Cable number 1 | Numpad + and -
            if (c == "+"):
                cableConstraint = self.cablesConstraintValues[0]
                cableConstraint.value = cableConstraint.value[0][0] + 0.5
                print "Contracting cable number 1",

            elif (c == "-"):
                cableConstraint = self.cablesConstraintValues[0]
                displacement = cableConstraint.value[0][0] - 0.5
                if (displacement < 0):
                    displacement = 0
                cableConstraint.value = displacement
                print "Extending cable number 1",

            # Cable number 2 | Numpad 6 and 9
            elif(ord(c) == 20):
                cableConstraint = self.cablesConstraintValues[1]
                cableConstraint.value = cableConstraint.value[0][0] + 0.5
                print "Contracting cable number 2",
            elif (ord(c) == 22):
                cableConstraint = self.cablesConstraintValues[1]
                displacement = cableConstraint.value[0][0] - 0.5
                if (displacement < 0):
                    displacement = 0
                cableConstraint.value = displacement
                print "Extending cable number 2",

            # Cable number 3 | Numpad 5 and 8
            elif(ord(c) == 11):
                cableConstraint = self.cablesConstraintValues[2]
                cableConstraint.value = cableConstraint.value[0][0] + 0.5
                print "Contracting cable number 3",
            elif (ord(c) == 19):
                cableConstraint = self.cablesConstraintValues[2]
                displacement = cableConstraint.value[0][0] - 0.5
                if (displacement < 0):
                    displacement = 0
                cableConstraint.value = displacement
                print "Extending cable number 3",

            # Cable number 4 | Numpad 4 and 7
            elif(ord(c) == 18):
                cableConstraint = self.cablesConstraintValues[3]
                cableConstraint.value = cableConstraint.value[0][0] + 0.5
                print "Contracting cable number 4",
            elif (ord(c) == 16):
                cableConstraint = self.cablesConstraintValues[3]
                displacement = cableConstraint.value[0][0] - 0.5
                if (displacement < 0):
                    displacement = 0
                cableConstraint.value = displacement
                print "Extending cable number 4",

            print [cableConstraint.value[0][0] for cableConstraint in self.cablesConstraintValues]

        if ord(c) == 70:    # key F is pressed
            floor = self.parentNode.getChild("Floor")
            moList = floor.getObjects("SearchDown", "MechanicalObject", "mstate")
            positionValue = moList[0].findData("position").value
            positionValue[0][1] += -10
            moList[0].findData("position").value = positionValue

        if ord(c) == 77:    # key M is pressed
            cylinder = self.parentNode.getChild("Cylinder")
            mList = cylinder.getObjects("SearchDown", "UniformMass", "mass")
            mList[0].findData("totalMass").value -= 1
            print mList[0].findData("totalMass").value
        return 0

    def calculateDiffInCableLength(self):
            for cable in self.cables:
                cableInitialLength = cable.findData('cableInitialLength')
                cableLength = cable.findData('cableLength')
                print cableInitialLength.value - cableLength.value

    def onBeginAnimationStep(self, dt):
        self.total_time += dt

        if self.automation == True:
            cableConstraint = self.cablesConstraintValues[3]
            #print type(cableConstraint.value)
            if cableConstraint.value[0][0] <  80:
                cableConstraint.value = cableConstraint.value[0][0] + 1.
                print "Contracting cable number 4"
                print[cableConstraint.value[0][0] for cableConstraint in self.cablesConstraintValues]
            else:
                if not self.floor_transitioned:
                    floor = self.parentNode.getChild("Floor")
                    moList = floor.getObjects("SearchDown", "MechanicalObject", "mstate")
                    positionValue = moList[0].findData("position").value
                    positionValue[0][1] += -20
                    moList[0].findData("position").value = positionValue
                    self.floor_transitioned = True

        #print 'onBeginAnimatinStep (python) dt=%f total time=%f' % (dt, self.total_time)
        #self.calculateDiffInCableLength()
        return 0

    def reset(self):
        """
        This is not working for some reason

        for cableConstraint in self.cablesConstraintValues:
            cableConstraint.value[0][0] = 0.
            print cableConstraint.value[0][0]
        """
        cableConstraint = self.cablesConstraintValues[3]
        cableConstraint.value = 0.
        print 'Resetting the simulation...'
        self.total_time = 0
        return 0

@SofaPrefab
class Trunk(SofaObject):
    # Info on SofaPrefab and what it has
    """ This prefab is implementing a soft robot inspired by the elephant's trunk.
        The robot is entirely soft and actuated with 8 cables.

        The prefab is composed of:
        - a visual model
        - a collision model
        - a mechanical model for the deformable structure

        The prefab has the following parameters:
        - youngModulus
        - poissonRatio
        - totalMass

        Example of use in a Sofa scene:

        def createScene(root):
            ...
            trunk = Trunk(root)

            ## Direct access to the components
            trunk.displacements = [0., 0., 0., 0., 5., 0., 0., 0.]
    """

    def __init__(self, parentNode, youngModulus=450, poissonRatio=0.45, totalMass=0.042, minForce=0, maxForce=100):
        self.minForce = minForce
        self.maxForce = maxForce

        self.node = parentNode.createChild('Trunk')
        self.node.createObject('MeshVTKLoader', name='loader', filename=path+'trunk.vtk')
        self.node.createObject('MeshTopology', src='@loader', name='container')

        self.node.createObject('MechanicalObject', name='dofs',
                               template='Vec3', showIndices=False, showIndicesScale=4e-5)
        self.node.createObject('UniformMass', totalMass=totalMass)
        self.node.createObject('TetrahedronFEMForceField', template='Vec3', name='FEM',
                               method='large', poissonRatio=poissonRatio,  youngModulus=youngModulus)

        self.__addCables()

    def __addCables(self):
        length1 = 10.
        length2 = 2.
        lengthTrunk = 195.

        pullPoint = [[0., length1, 0.], [-length1, 0., 0.],
                     [0., -length1, 0.], [length1, 0., 0.]]
        direction = Vec3(0., length2-length1, lengthTrunk)
        direction.normalize()

        nbCables = 4

        for i in range(0, nbCables):
            theta = 1.57*i  # 180deg in rad -> 0 pi 2pi 3pi
            q = Quat(0., 0., sin(theta/2.), cos(theta/2.))

            position = [[0., 0., 0.]]*20

            # 10 iterations -> k = 0 2 4 6 8 10 12 14 16 18
            for k in range(0, 20, 2):
                v = Vec3(direction[0], direction[1]*17.5*(k/2) + length1, direction[2]*17.5*(k/2) + 21) #21
                position[k] = v.rotateFromQuat(q)

                v = Vec3(direction[0], direction[1]*17.5*(k/2) + length1, direction[2]*17.5*(k/2) + 27)
                position[k+1] = v.rotateFromQuat(q)

            cableL = self.node.createChild('cableL'+str(i))
            cableL.createObject('MechanicalObject', name='dofs', position=pullPoint[i]+[pos.toList() for pos in position])
            cableL.createObject('CableConstraint', template='Vec3', name="cable",
                                pullPoint=pullPoint[i],
                                indices=range(0, 21),
                                maxPositiveDisp=70,
                                maxDispVariation=1,
                                valueType='force',
                                minForce=self.minForce,
                                maxForce=self.maxForce)
            cableL.createObject('BarycentricMapping', name='mapping',  mapForces=False, mapMasses=False)
            #print pullPoint[i]

        # Plot half (10) of cable nodes
        """
        for i in range(0, nbCables):
            theta = 1.57*i
            q = Quat(0., 0., sin(theta/2.), cos(theta/2.))

            position = [[0., 0., 0.]]*10
            for k in range(0, 9, 2):
                v = Vec3(direction[0], direction[1]*17.5 *
                         (k/2)+length1, direction[2]*17.5*(k/2)+21)
                position[k] = v.rotateFromQuat(q)
                v = Vec3(direction[0], direction[1]*17.5 *
                         (k/2)+length1, direction[2]*17.5*(k/2)+27)
                position[k+1] = v.rotateFromQuat(q)

            cableS = self.node.createChild('cableS'+str(i))
            cableS.createObject('MechanicalObject', name='dofs', position=pullPoint[i]+[pos.toList() for pos in position])
            cableS.createObject('CableConstraint', template='Vec3', name="cable",
                                hasPullPoint=False,
                                indices=range(0, 10),
                                maxPositiveDisp=40,
                                maxDispVariation=1,
                                minForce=0)
            cableS.createObject('BarycentricMapping', name='mapping',  mapForces=False, mapMasses=False)
    """

    def addVisualModel(self, color=[1., 1., 1., 1.]):
        trunkVisu = self.node.createChild('VisualModel')
        trunkVisu.createObject('MeshSTLLoader', filename=path+"trunk.stl")
        trunkVisu.createObject('OglModel', color=color)
        trunkVisu.createObject('BarycentricMapping')

    def addCollisionModel(self, selfCollision=False):
        trunkColli = self.node.createChild('CollisionModel')
        for i in range(2):
            part = trunkColli.createChild("Part"+str(i+1))
            part.createObject('MeshSTLLoader', name="loader", filename=path+"trunk_colli"+str(i+1)+".stl")
            part.createObject('MeshTopology', src="@loader")
            part.createObject('MechanicalObject')
            part.createObject('TriangleCollisionModel', group=1 if not selfCollision else i)
            part.createObject('LineCollisionModel', group=1 if not selfCollision else i)
            part.createObject('PointCollisionModel', group=1 if not selfCollision else i)
            part.createObject('BarycentricMapping')

    def fixExtremity(self):
        self.node.createObject('BoxROI', name='boxROI', box=[[-20, -20, 0], [20, 20, 20]], drawBoxes=True)
        self.node.createObject('PartialFixedConstraint', fixedDirections=[1, 1, 1], indices="@boxROI.indices")

    # def addEffectors
    """
    def addEffectors(self, target, position=[0., 0., 195.]):
        effectors = self.node.createChild("Effectors")
        effectors.createObject("MechanicalObject", position=position)
        effectors.createObject("PositionEffector", indices=range(len(position)), effectorGoal=target)
        effectors.createObject("BarycentricMapping", mapForces=False, mapMasses=False)
    """

################ CREATE SCENE ####################
def createScene(rootNode):
    gravity = [0., -981., 0.]
    alarmDistance = 2
    contactDistance = 1

    # Independant variables
    friction = sqrt(0.8)*2          # sqrt(0.8)
    tentancleMass = 0.00001           # 0.025
    tentacleYoungModulus = 310      # 450
    tentanclePoissonRatio = 0.25    # 0.45
    objectMass = 0.5

    minPullForce = 1
    maxPullForce = 20

    # Rest of the code
    ShowGrid(rootNode)
    rootNode.createObject('RequiredPlugin', pluginName='SoftRobots SofaOpenglVisual SofaSparseSolver SofaPreconditioner SofaPython CGALPlugin') # SoftRobots.Inverse isn't publically available
    AnimationManager(rootNode)
    rootNode.createObject("VisualStyle", displayFlags="hideForceFields showInteractionForceFields showBehaviorModels")
    rootNode.gravity = gravity
    ContactHeader(rootNode, alarmDistance=alarmDistance, contactDistance=contactDistance, frictionCoef=friction)

    simulation = rootNode.createChild("Simulation")
    simulation.createObject('EulerImplicitSolver', name='odesolver', rayleighMass=0.1, rayleighStiffness=0.1)
    simulation.createObject('ShewchukPCGLinearSolver', name='linearSolver', iterations=500, tolerance=1.0e-18, preconditioners="precond")
    simulation.createObject('SparseLDLSolver', name='precond')
    simulation.createObject('GenericConstraintCorrection', solverName="precond")

    trunk = Trunk(simulation, totalMass=tentancleMass, youngModulus=tentacleYoungModulus, poissonRatio=tentanclePoissonRatio, minForce=minPullForce, maxForce=maxPullForce)
    trunk.addVisualModel(color=[0.5, 0.5, 1, 0.95])
    trunk.fixExtremity()
    trunk.addCollisionModel()
    TrunkController(rootNode, trunk, automation=False)

    #Cube(rootNode, translation=[35, 0, 115], color=[1, 1, 1, 1], uniformScale=7.5, totalMass = 0.01)
    #Cylinder(rootNode, translation=[35, -10, 115], color=[1, 1, 1, 1], uniformScale=7.5, totalMass = 1)
    RigidObject(rootNode, name='Cylinder', translation=[60, 10.0, 30], color=[1, 1, 1, 1], uniformScale=3, totalMass=objectMass, surfaceMeshFileName='mesh/cylinder.obj')
    floorObjet = Floor(rootNode, translation=[0.0, -10.0, 0.0], uniformScale=5.0, isAStaticObject=True, groupForCollision=1)