[SOLVED] Using SOFA to build pneumatic actuated robot arm

Hi. I am trying to use SOFA and the softRobots plugin by SofaDeforst to build the pneumatic actuated robot arm.

For now I already created two pneumatic actuator and a connector connecting these two actuators.
I’ve used BilateralInteractionConstraint to connect two points on the actuators and connector together. However, when I apply pressure on the actuator and elongate the actuator, the position of the connector does not change. I don’t know what should I add to make the connector move along with the actuator.
Any insights would be helpful. I will attach the main part of the code in the following.

import Sofa
import math
import os

from stlib.physics.deformable import ElasticMaterialObject
from stlib.physics.rigid import *
from stlib.components import addOrientedBoxRoi
from splib.numerics import vec3
from stlib.scene import Scene, Interaction

youngModulusFingers = 500
youngModulusStiffLayerFingers = 1500

radius = 70
angle1 = 0*math.pi/180  # Angle between 1st and 2nd finger in radian

# red, green, blue
translateFingerX = [26, 26]
translateFingerY = [-170, -170]
translateFingerZ = [-40, 40]
translateFinger1 = str(translateFingerX[0]) + " " + str(translateFingerY[0]) + " " + str(translateFingerZ[0])
translateFinger2 = str(translateFingerX[1]) + " " + str(translateFingerY[1]) + " " + str(translateFingerZ[1])
translations= [translateFinger1,translateFinger2]
angles=[0,angle1]

def createScene(rootNode):

	rootNode.createObject('RequiredPlugin', pluginName='SoftRobots')
	rootNode.createObject('RequiredPlugin', name='SofaPython', pluginName='SofaPython')
	rootNode.createObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels hideCollisionModels hideBoundingCollisionModels hideForceFields showInteractionForceFields hideWireframe')
	rootNode.findData('gravity').value='-9810 0 0';
	rootNode.createObject('FreeMotionAnimationLoop')
	rootNode.createObject('GenericConstraintSolver', tolerance="1e-12", maxIterations="10000")
	rootNode.createObject('CollisionPipeline', verbose="0")
	rootNode.createObject('BruteForceDetection', name="N2")
	rootNode.createObject('CollisionResponse', response="FrictionContact", responseParams="mu=0.6")
	rootNode.createObject('LocalMinDistance', name="Proximity", alarmDistance="5", contactDistance="1", angleCone="0.00")

	rootNode.createObject('BackgroundSetting', color='0 0.168627 0.211765')
	rootNode.createObject('OglSceneFrame', style="Arrows", alignment="TopRight")
	rootNode.createObject('PythonScriptController', filename="pythonControllers/wholeGripperController.py", classname="controller")

	planeNode = rootNode.createChild('Plane')
	planeNode.createObject('MeshObjLoader', name='loader', filename="data/mesh/floorFlat.obj", triangulate="true", rotation="0 0 270", scale=10, translation="0 0 0")
	planeNode.createObject('Mesh', src="@loader")
	planeNode.createObject('MechanicalObject', src="@loader")
	planeNode.createObject('Triangle', simulated="0", moving="0")
	planeNode.createObject('Line', simulated="0", moving="0")
	planeNode.createObject('Point', simulated="0", moving="0")
	planeNode.createObject('OglModel',name="Visual", src="@loader", color="1 0 0 1")

	cube = rootNode.createChild('cube')
	cube.createObject('EulerImplicit', name='odesolver')
	cube.createObject('SparseLDLSolver', name='linearSolver')
	cube.createObject('MechanicalObject', template="Rigid3d", position='25 -80 0 0 0 0 0')
	cube.createObject('UniformMass', totalMass='0.1')
	cube.createObject('UncoupledConstraintCorrection')

	#collision
	cubeCollis = cube.createChild('cubeCollis')
	cubeCollis.createObject('MeshObjLoader', name="loader", filename="data/mesh/smCube27.obj", triangulate="true",  scale="6")
	cubeCollis.createObject('Mesh', src="@loader")
	cubeCollis.createObject('MechanicalObject')
	cubeCollis.createObject('Triangle')
	cubeCollis.createObject('Line')
	cubeCollis.createObject('Point')
	cubeCollis.createObject('RigidMapping')

	#visualization
	cubeVisu = cube.createChild('cubeVisu')
	cubeVisu.createObject('MeshObjLoader', name="loader", filename="data/mesh/smCube27.obj")
	cubeVisu.createObject('OglModel', name="Visual", src="@loader", color="0.0 0.1 0.5", scale="6.2")
	cubeVisu.createObject('RigidMapping')

	connector = rootNode.createChild('Connector')
	connector.createObject('MeshSTLLoader', name='loader', filename="data/mesh/connector.stl", rotation="0 90 90", translation=str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str((translateFingerZ[0] + translateFingerZ[1]) / 2))
	connector.createObject('MechanicalObject', template='Rigid3d', name="dofs")
	connector.createObject('OglModel',name="Visual", src="@loader")
	connector.createObject("EulerImplicit")
	connector.createObject("CGLinearSolver")
	connector.createObject('UncoupledConstraintCorrection')
	connector.createObject('UniformMass', totalMass="0.1")
	constraint = connector.createChild('Constraint')
	constraint.createObject('MechanicalObject', name="points", template="Vec3d", position=str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str(translateFingerZ[0] - 22.5) + " " + str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str(translateFingerZ[1] - 22.5) + " " + str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str(translateFingerZ[0] + 22.5) + " " + str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str(translateFingerZ[1] + 22.5), showObject="true", showObjectScale="5")

	for i in range(2):
		actuator = rootNode.createChild('actuator'+str(i+1))
		e = ElasticMaterialObject(actuator, volumeMeshFileName="data/mesh/actuator.vtk", translation=translations[i], rotation=[0,0,0],
								surfaceMeshFileName="data/mesh/actuator.stl", collisionMesh="data/mesh/actuator.stl", withConstrain=True,
								youngModulus=youngModulusFingers, poissonRatio=0.3, totalMass=0.1)
		e.createObject('TetrahedronSetTopologyModifier')
		e.createObject('TetrahedronSetTopologyAlgorithms', template='Vec3d')
		e.createObject('TetrahedronSetGeometryAlgorithms', template='Vec3d')

		# Bounding Box
		e.createObject('BoxROI', name='boxROI', box=str(translateFingerX[i] - 22.5) + " " + str(translateFingerY[i] - 25) + " " + str(translateFingerZ[i] - 22.5) + " " + str(translateFingerX[i] + 22.5) + " " + str(translateFingerY[i] - 20) + " " + str(translateFingerZ[i] + 22.5) + " ", drawBoxes='false', drawSize='1', doUpdate='0')
		e.createObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness='1e12', angularStiffness='1e12')

		# Stiff layer
		e.createObject('BoxROI', name='boxROISubTopo', box=str(translateFingerX[i] - 22.5) + " " + str(translateFingerY[i] - 25) + " " + str(translateFingerZ[i] - 22.5) + " " + str(translateFingerX[i] + 22.5) + " " + str(translateFingerY[i] - 20) + " " + str(translateFingerZ[i] + 22.5) + " ", drawBoxes='false', drawSize='1')
		e.createObject('BoxROI', name='boxROISubTopo2', box=str(translateFingerX[i] - 22.5) + " " + str(translateFingerY[i] + 25) + " " + str(translateFingerZ[i] - 22.5) + " " + str(translateFingerX[i] + 22.5) + " " + str(translateFingerY[i] + 20) + " " + str(translateFingerZ[i] + 22.5) + " ", drawBoxes='false', drawSize='1')
		e.createObject('LinearSolverConstraintCorrection')

		modelSubTopo = e.createChild('modelSubTopo')
		modelSubTopo2 = e.createChild('modelSubTopo2')
		modelSubTopo.createObject('TetrahedronSetTopologyContainer', position='@loader.position', tetrahedra="@boxROISubTopo.tetrahedraInROI", name='container')
		modelSubTopo2.createObject('TetrahedronSetTopologyContainer', position='@loader.position', tetrahedra="@boxROISubTopo2.tetrahedraInROI", name='container')
		modelSubTopo.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM', method='large', poissonRatio='0.3',  youngModulus=str(youngModulusStiffLayerFingers-youngModulusFingers))
		modelSubTopo2.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM', method='large', poissonRatio='0.3',  youngModulus=str(youngModulusStiffLayerFingers-youngModulusFingers))

		cavity = e.createChild('cavity')
		cavity.createObject('MeshSTLLoader', name='loader', filename='data/mesh/cavity.stl',translation = translations[i],rotation="0 0 0")
		cavity.createObject('Mesh', src='@loader', name='topo')
		cavity.createObject('MechanicalObject', name='cavity')
		cavity.createObject('SurfacePressureConstraint', name="SurfacePressureConstraint", template='Vec3d', value="0.0001", triangles='@topo.triangles', drawPressure='0', drawScale='0.0002', valueType="pressure")
		cavity.createObject('BarycentricMapping', name='mapping',  mapForces='false', mapMasses='false')

		constraint = e.createChild("Constraint")
		constraint.createObject('MechanicalObject', name="points", template="Vec3d", position=str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str(translateFingerZ[i] - 22.5)  + " " + str(translateFingerX[0]) + " " + str(translateFingerY[0] + 20) + " " + str(translateFingerZ[i] + 22.5))
		constraint.createObject("BarycentricMapping")

	rootNode.createObject('BilateralInteractionConstraint', template = "Vec3d", object1 = "@Connector/Constraint/points", object2 = "@actuator1/ElasticMaterialObject/Constraint/points", first_point = "0 2", second_point = "0 1")
	rootNode.createObject('BilateralInteractionConstraint', template = "Vec3d", object1 = "@Connector/Constraint/points", object2 = "@actuator2/ElasticMaterialObject/Constraint/points", first_point = "1 3", second_point = "0 1")
	return rootNode