def createScene(rootNode):
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0])
ContactHeader(rootNode, alarmDistance=8, contactDistance=5)
ShowGrid(rootNode)
Floor(rootNode,
translation=[0.0, -160.0, 0.0],
uniformScale=5.0,
isAStaticObject=True)
Floor(rootNode,
name="FloorObstacle",
translation=[0.0, -80.0, 0.0],
color=[0.0, 1.0, 0.0],
uniformScale=0.8,
isAStaticObject=True)
for c in range(7):
Cube(rootNode,
name="Cube" + str(-210 + c * 70),
translation=[-210 + c * 70, 0.0, 0.0],
color=[c / 10.0, c * 0.7 / 10.0, 0.9],
uniformScale=20.0)
return rootNode
def createScene(rootNode):
# main header (rootNode + gravity)
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0])
# contact header
ContactHeader(rootNode, alarmDistance=8, contactDistance=5)
# shows the grid outline
ShowGrid(rootNode)
# base floor
Floor(rootNode,
translation=[0.0, -180.0, 0.0],
uniformScale=5.0,
isAStaticObject=True)
# obstacle floor
Floor(rootNode,
name="Floor Obstacle",
translation=[0.0, -90.0, 0.0],
color=[0.0, 1.0, 0.0],
uniformScale=0.8,
isAStaticObject=True)
# creates 7 cubes
for i in range(7):
Cube(rootNode,
name="Cube" + str(-210 + c * 70),
translation=[-210 + c * 70, 0.0, 0.0],
color=[c / 10.0, c * 0.7 / 10.0, 0.9],
uniformScale=20.0)
return rootNode
def createScene(rootNode):
from stlib.scene import MainHeader, ContactHeader
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0], plugins=["SoftRobots"])
ContactHeader(rootNode, alarmDistance=4, contactDistance=3, frictionCoef=0.08)
Finger(rootNode, translation=[1.0,0.0,0.0])
return rootNode
def createScene(rootNode):
"""Setting up a simple scene"""
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0])
ContactHeader(rootNode, alarmDistance=5, contactDistance=1)
Cube(rootNode, translation=[0.0, 60.0, 10.0], uniformScale=2.0)
Floor(rootNode, translation=[0.0, -160.0, 0.0], isAStaticObject=True)
Prostate = ElasticMaterialObject(
rootNode,
name="Prostate",
volumeMeshFileName="/Users/pedro/Downloads/mac/Data/Prosta_2_2.msh",
surfaceMeshFileName="/Users/pedro/Downloads/mac/Data/Prosta_2_2.stl",
collisionMesh="/Users/pedro/Downloads/mac/Data/Prosta_2_2.stl",
withConstrain=True,
surfaceColor=[0.0, 0.70, 1.0],
scale=[0.9, 0.9, 0.9],
poissonRatio=0.49,
youngModulus=500,
translation=[10.0, 0.0, 0.0])
# Bulbo = ElasticMaterialObject(rootNode, name="Bulbo",
# volumeMeshFileName="/Users/pedro/Downloads/mac/Data/Prosta_16_16.msh",
# surfaceMeshFileName="/Users/pedro/Downloads/mac/Data/Prosta_16_16.stl",
# collisionMesh = "/Users/pedro/Downloads/mac/Data/Prosta_16_16.stl",
# withConstrain=True,
# surfaceColor=[0.2, 0.9, 0.0],
# scale=[0.9, 0.9, 0.9],
# poissonRatio=0.49,
# youngModulus=90000,
# translation=[0.0,60.0,35.0])
fixingBox = [0.0, 0.0, 0.0]
BoxROICoordinates = [-5, 0, -5, 5, 1, 5]
FixedBox(Prostate,
atPositions=[-10.0, -1.0, -20.0, 60.0, 60.0, 5.0],
doVisualization=True)
# scene = Scene(rootNode, gravity=[0.0, -981.0, 0.0])
# scene.dt = 0.025
# scene.VisualStyle.displayFlags = "showBehavior"
#
# scene.Config.createObject("MeshSTLLoader", name="loader", filename="/Users/pedro/Downloads/mac/SOFA_v19.06.99_custom_MacOS_v11/plugins/SoftRobots/docs/tutorials/Tripod/details/data/mesh/blueprint.stl")
# scene.Config.createObject("OglModel", src="@loader")
# body = ElasticBody(scene.Modelling)
# fix = FixingBox(scene.Modelling,
# body.ElasticMaterialObject,
# eulerRotation=[0,0,0],
# translation=[0.0, .0, 0.0],
# scale=[30., 30., 30.])
#
# # Changing the property of the Box ROI so that the constraint area appears on screen.
# fix.BoxROI.drawBoxes = True
#
return rootNode
def createScene(rootNode):
MainHeader(rootNode,
plugins=['SofaPython', 'SoftRobots', 'SofaOpenglVisual'],
gravity=[0.0, -9810, 0.0])
ContactHeader(rootNode,
alarmDistance=5,
contactDistance=1,
frictionCoef=0.7)
GripperController(rootNode)
Floor(rootNode, **floorParam)
Cube(rootNode, **cubeParam)
# Put treshold in rigibObject construction param ?
rootNode.Cube.Solver.threshold = 1e-6
for i in range(len(fingersParameters)):
finger = ElasticMaterialObject(
attachedTo=rootNode,
volumeMeshFileName=fingersVolumeMesh,
name=fingersParameters[i]['name'],
rotation=fingersParameters[i]['rotation'],
translation=fingersParameters[i]['translation'],
surfaceMeshFileName=fingersSurfaceAndCollisionMesh,
collisionMesh=fingersSurfaceAndCollisionMesh,
withConstrain=True,
surfaceColor=fingersColor,
poissonRatio=poissonRatioFingers,
youngModulus=youngModulusFingers,
totalMass=fingersMass)
finger.integration.rayleighStiffness = 0.1
finger.integration.rayleighMass = 0.1
finger.createObject('BoxROI',
name='boxROI',
box=fingersParameters[i]['ROIBox'],
drawBoxes=True,
doUpdate=False)
finger.createObject('RestShapeSpringsForceField',
points='@../finger1/boxROI.indices',
stiffness=1e12,
angularStiffness=1e12)
PneumaticCavity(surfaceMeshFileName=fingersCavitySurfaceMesh,
attachedAsAChildOf=finger,
name='cavity',
rotation=fingersParameters[i]['rotation'],
translation=fingersParameters[i]['translation'],
initialValue=cavitiesInitialValue,
valueType='pressure')
def createScene(rootNode):
"""You can load the finger only by typing runSofa finger.py"""
from stlib.scene import MainHeader, ContactHeader
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0], plugins=["SoftRobots"])
ContactHeader(rootNode,
alarmDistance=4,
contactDistance=3,
frictionCoef=0.08)
Finger(rootNode, translation=[1.0, 0.0, 0.0])
return rootNode
def createScene(rootNode):
"""This is my first scene"""
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0])
ContactHeader(rootNode, alarmDistance=15, contactDistance=10)
#ShowGrid(rootNode)
Floor(rootNode, translation=[0.0, -160.0, 0.0], isAStaticObject=True)
Cube(rootNode, translation=[0.0, 0.0, 0.0], uniformScale=20.0)
return rootNode
def createScene(rootNode):
"""Setting up a simple scene"""
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0])
ContactHeader(rootNode, alarmDistance=15, contactDistance=5)
Cube(rootNode,
translation=[0.0, 60.0, 10.0],
rotation=[0.0, 60.0, 10.0],
uniformScale=20.0)
Floor(rootNode, translation=[0.0, -160.0, 0.0], isAStaticObject=True)
return rootNode
def createScene(rootNode):
"""Setting up a simple scene"""
MainHeader(rootNode, gravity = [0.0, -981.0, 0.0])
ContactHeader(rootNode, alarmDistance = 5, contactDistance = 1)
#cube(rootNode, translation = [10.1,60.0,10.0], uniformScale = 1.0)
Floor(rootNode, translation = [0.0,-160.0,0.0], isAStaticObject = True)
Prostate = ElasticMaterialObject(rootNode, name="Prostate",
volumeMeshFileName="/Users/pedro/Downloads/mac/Data/Prosta_2_2.msh",
surfaceMeshFileName="/Users/pedro/Downloads/mac/Data/Prosta_2_2.stl",
collisionMesh = "/Users/pedro/Downloads/mac/Data/Prosta_2_2.stl",
withConstrain=True,
surfaceColor=[0.0, 0.70, 1.0],
scale=[0.9, 0.9, 0.9],
poissonRatio=0.49,
youngModulus=500,
translation=[10.0,0.0,0.0])
Bulbo = ElasticMaterialObject(rootNode, name="Bulbo",
volumeMeshFileName="/Users/pedro/Downloads/mac/Data/needle_s.msh",
surfaceMeshFileName="/Users/pedro/Downloads/mac/Data/needle_s.stl",
collisionMesh = "/Users/pedro/Downloads/mac/Data/needle_s.stl",
withConstrain=True,
surfaceColor=[0.2, 0.9, 0.0],
scale=[0.9, 0.9, 0.9],
poissonRatio=0.33,
youngModulus=100000,
translation=[0.0,160.0,60.0],
rotation=[90.0,0.0,0.0])
fixingBox=[0.0,0.0,0.0]
BoxROICoordinates=[-5, 0, -5, 5, 1, 5]
FixedBox(Prostate, atPositions=[-10.0,-1.0,-20.0,60.0,60.0,5.0], doVisualization=True)
# fixingBox=[0.0,0.0,0.0]
# BoxROICoordinates=[-5, 0, -5, 5, 1, 5]
# PartiallyFixedBox(attachedTo=Bulbo, fixedAxis=[1, 0,1], box=[-10.0,140.0,-20.0,10.0,170.0,25.0], drawBoxes = True )
return rootNode
def createScene(rootNode):
surfaceMeshFileNameDiamond = 'surface.stl'
surfaceMeshFileNameStrafish = 'quadriped_collision.stl'
surfaceMeshFileNameSofia = 'sofia_leg.stl'
MainHeader(rootNode,
plugins=["SofaPython", "SoftRobots", "ModelOrderReduction"],
dt=1,
gravity=[0.0, -9810, 0.0])
ContactHeader(rootNode,
alarmDistance=5,
contactDistance=1,
frictionCoef=0.7)
Floor(rootNode,
name="Plane",
color=[1.0, 0.0, 1.0],
isAStaticObject=True,
uniformScale=10)
Reduced_diamond(rootNode,
name="Reduced_diamond_white",
rotation=[-90, 0.0, 0.0],
translation=[0, 50.0, 0.0],
surfaceColor=[0.5, 0.5, 0.5, 0.5],
surfaceMeshFileName=surfaceMeshFileNameDiamond)
Reduced_starfish(rootNode,
name="Reduced_starfish_red",
rotation=[0, 90.0, 120.0],
translation=[300, 400.0, 100.0],
surfaceColor=[1, 0.0, 0.0, 0.5],
surfaceMeshFileName=surfaceMeshFileNameStrafish)
Reduced_SofiaLeg(rootNode,
name="Reduced_sofiaLeg_blue",
rotation=[0, 0.0, 0.0],
translation=[-400, 60.0, 100.0],
surfaceColor=[0.0, 0.0, 1, 0.5],
surfaceMeshFileName=surfaceMeshFileNameSofia)
def createScene(rootNode):
"""This is my first scene"""
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0], plugins=["SoftRobots"])
ContactHeader(rootNode,
alarmDistance=4,
contactDistance=3,
frictionCoef=0.08)
Gripper(rootNode)
Floor(rootNode,
color=[1.0, 0.0, 0.0],
translation=[0.0, -160.0, 0.0],
isAStaticObject=True)
Cube(rootNode,
uniformScale=20.0,
color=[1.0, 1.0, 0.0],
totalMass=0.03,
volume=20,
inertiaMatrix=[1000.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 1000.0],
translation=[0.0, -130.0, 10.0])
return rootNode
def createScene(rootNode):
from stlib.scene import MainHeader, ContactHeader
#from stlib.physics.deformable import ElasticMaterialObject
MainHeader(rootNode, plugins=["CosseratPlugin"], gravity=[0., 0., 0.])
# rootNode.createObject(
# 'VisualStyle', displayFlags='showVisualModels hideBehaviorModels showCollisionModels hideBoundingCollisionModels hideForceFields showInteractionForceFields showWireframe')
rootNode.createObject(
'VisualStyle',
displayFlags='showVisualModels showInteractionForceFields')
# rootNode.createObject('FreeMotionAnimationLoop')
# rootNode.createObject('GenericConstraintSolver', tolerance="1e-20", maxIterations="5000", printLog="0")
ContactHeader(rootNode,
alarmDistance=4,
contactDistance=3,
frictionCoef=0.08)
rootNode.gravity = "0 -981 0"
rootNode.createObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.createObject('OglSceneFrame',
style="Arrows",
alignment="TopRight")
cableNode = rootNode.createChild('cosseratNode')
cableNode.createObject('EulerImplicitSolver',
firstOrder="0",
rayleighStiffness="1.0",
rayleighMass='0.1')
cableNode.createObject('SparseLUSolver', name='solver')
cableNode.createObject('GenericConstraintCorrection')
cosFinger1 = CosseratFinger(
rootNode=rootNode,
cableNode=cableNode,
name="cosseratF1",
rotation=[0., 0, -120.],
translation=[-5., 70., 0.0],
)
#trans = cosFinger1.trans
cosFinger2 = CosseratFinger(rootNode=rootNode,
cableNode=cableNode,
name="cosseratF2",
rotation=[180., 0, -60.],
translation=[5., 70., 10.0])
cosFinger3 = CosseratFinger(rootNode=rootNode,
cableNode=cableNode,
name="cosseratF3",
rotation=[180., 0, -60.],
translation=[5., 70., -10.0])
GripperController(rootNode,
fingers=[cosFinger1, cosFinger2, cosFinger3],
angles=[[0., 0, -120.], [180., 0, -60.], [180., 0,
-60.]])
Floor(rootNode,
color=[1.0, 0.0, 0.0],
translation=[0.0, -25.0, 0.0],
rotation=[0.0, 0.0, 0.0],
isAStaticObject=True)
Cube(
rootNode,
uniformScale=20.0,
color=[1.0, 1.0, 0.0],
totalMass=0.03,
volume=20,
inertiaMatrix=[1000.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 1000.0],
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
)
def createScene(rootNode):
MainHeader(rootNode, plugins=["SoftRobots", "SoftRobots.Inverse", "SofaPython", "SofaSparseSolver",
"SofaPreconditioner", "SofaOpenglVisual", "CosseratPlugin", "BeamAdapter"],
repositoryPaths=[os.getcwd()])
# rootNode.createObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels showCollisionModels '
# 'hideBoundingCollisionModels hideForceFields '
# 'showInteractionForceFields showWireframe')
rootNode.createObject('VisualStyle', displayFlags='showVisualModels showInteractionForceFields showWireframe')
rootNode.createObject('FreeMotionAnimationLoop')
# rootNode.createObject('QPInverseProblemSolver', printLog='0')
rootNode.createObject('GenericConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0")
ContactHeader(rootNode, alarmDistance=2.5, contactDistance=2, frictionCoef=0.08)
rootNode.gravity = "0 0 0"
rootNode.createObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.createObject('OglSceneFrame', style="Arrows",
alignment="TopRight")
showObject = "0"
showIndices = "0"
##########################################
# FEM Model #
##########################################
finger = rootNode.createChild('finger')
finger.createObject('EulerImplicitSolver', name='odesolver', firstOrder='0', rayleighMass=0.1, rayleighStiffness=0.1)
finger.createObject('SparseLDLSolver', name='preconditioner')
# Add a componant to load a VTK tetrahedral mesh and expose the resulting topology in the scene .
finger.createObject('MeshVTKLoader', name='loader', filename=path +'finger.vtk', translation="-17.5 -12.5 7.5",
rotation="0 180 0")
#finger.createObject('MeshExporter', name='loader', filename=path +'transFinger.vtk', exportAtEnd="true")
finger.createObject('TetrahedronSetTopologyContainer', src='@loader', name='container')
finger.createObject('TetrahedronSetTopologyModifier')
# finger.createObject('TetrahedronSetTopologyAlgorithms', template='Vec3d')
# finger.createObject('TetrahedronSetGeometryAlgorithms', template='Vec3d')
# Create a mechanicaobject component to stores the DoFs of the model
finger.createObject('MechanicalObject', name='tetras', template='Vec3d', showIndices='false',
showIndicesScale='4e-5', rx='0', dz='0')
# Gives a mass to the model
finger.createObject('UniformMass', totalMass='0.075')
# Add a TetrahedronFEMForceField componant which implement an elastic material model
# solved using the Finite Element Method on
# tetrahedrons.
finger.createObject('TetrahedronFEMForceField', template='Vec3d',
name='FEM', method='large', poissonRatio='0.45', youngModulus='500')
finger.createObject('BoxROI', name='ROI1',
box='-18 -15 -8 2 -3 8', drawBoxes='true')
finger.createObject('RestShapeSpringsForceField',
points='@ROI1.indices', stiffness='1e12')
##########################################
# Cable points #
##########################################
# Mappe points inside the meca, this points will be use for the bilateral mapping
FEMpos = [" 0.0 0 0 15 0 0 30 0 0 45 0 0 60 0 0 66 0 0 81 0.0 0.0"]
# FEMpos = [" 81 0.0 0.0"]
femPoints = finger.createChild('femPoints')
inputFEMCable = femPoints.createObject('MechanicalObject', name="pointsInFEM",
position=FEMpos, showObject="1", showIndices="1")
femPoints.createObject('BarycentricMapping')
##########################################
# Finger auto-Collision #
##########################################
# CollisionMesh(finger,
# surfaceMeshFileName="mesh/fingerCollision_part1.stl",
# name="CollisionMeshAuto1", translation="-17.5 -12.5 7.5", rotation="0 180 0", collisionGroup=[1])
#
# CollisionMesh(finger,
# surfaceMeshFileName="mesh/fingerCollision_part2.stl",
# name="CollisionMeshAuto2", translation="-17.5 -12.5 7.5", rotation="0 180 0", collisionGroup=[2])
finger.createObject('LinearSolverConstraintCorrection')
##########################################
# Visualization #
##########################################
fingerVisu = finger.createChild('visu')
fingerVisu.createObject(
'MeshSTLLoader', filename=path+"finger.stl", name="loader", translation="-17.5 -12.5 7.5",
rotation="0 180 0")
#fingerVisu.createObject('STLExporter', filename=path+"transFinger", exportAtEnd="true")
fingerVisu.createObject('OglModel', src="@loader",
template='ExtVec3f', color="0.0 0.7 0.7")
fingerVisu.createObject('BarycentricMapping')
# ###############
# New adds to use the sliding Actuator
###############
cableNode = rootNode.createChild('cableNode')
cableNode.createObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="0.1", rayleighMass='0.1')
cableNode.createObject('SparseLUSolver', name='solver')
cableNode.createObject('GenericConstraintCorrection')
# ###############
# RigidBase
###############
rigidBaseNode = cableNode.createChild('rigidBase')
RigidBaseMO = rigidBaseNode.createObject('MechanicalObject', template='Rigid3d',
name="RigidBaseMO", position="0 0 0 0 0 0 1", showObject='1',
showObjectScale='5.')
rigidBaseNode.createObject('RestShapeSpringsForceField', name='spring', stiffness="50000",
angularStiffness="50000", external_points="0", mstate="@RigidBaseMO", points="0",
template="Rigid3d")
###############
# Rate of angular Deformation (2 sections)
###############
position = ["0 0 0 " + "0 0 0 " + "0 0 0 " +
"0 0 0 " + "0 0 0 " + "0 0 0 "]
longeur = '15 15 15 15 6 15' # beams size
rateAngularDeformNode = cableNode.createChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.createObject(
'MechanicalObject', template='Vec3d', name='rateAngularDeformMO', position=position, showIndices="1")
BeamHookeLawForce = rateAngularDeformNode.createObject(
'BeamHookeLawForceField', crossSectionShape='circular', length=longeur, radius='0.50', youngModulus='5e6')
# BeamHookeLawForce = rateAngularDeformNode.createObject('CosseratInternalActuation', name="BeamHookeLawForce",
# crossSectionShape='circular', radius='0.5', youngModulus='5e6')
################################
# Animation (to move the dofs) #
################################
anim = Animation(rigidBaseNode, rateAngularDeformNode)
##############
# Frames #
##############
frames = ["0.0 0 0 0 0 0 1 5 0 0 0 0 0 1 10.0 0 0 0 0 0 1 15.0 0 0 0 0 0 1 20.0 0 0 0 0 0 1" +
" 30.0 0 0 0 0 0 1 35.0 0 0 0 0 0 1 40.0 0 0 0 0 0 1 45.0 0 0 0 0 0 1 55.0 0 0 0 0 0 1 60.0 0 0 0 0 0 1" +
" 66.0 0 0 0 0 0 1 71.0 0 0 0 0 0 1 76.0 0 0 0 0 0 1 81.0 0 0 0 0 0 1"]
# the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform
mappedFrameNode = rigidBaseNode.createChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.createObject(
'MechanicalObject', template='Rigid3d', name="FramesMO", position=frames, showObject='1', showObjectScale='1')
# The mapping has two inputs: RigidBaseMO and rateAngularDeformMO
# one output: FramesMO
inputMO = rateAngularDeformMO.getLinkPath()
inputMO_rigid = RigidBaseMO.getLinkPath()
outputMO = framesMO.getLinkPath()
curv_abs_input = '0 15 30 45 60 66 81'
curv_abs_output = '0.0 5 10 15 20 30 35 40 45 55 60 66 71 76 81'
# mappedFrameNode.createObject('DiscretCosseratMapping', curv_abs_input=curv_abs_input,
# curv_abs_output=curv_abs_output, input1=inputMO, input2=inputMO_rigid,
# output=outputMO, debug='0', max=2.e-3, deformationAxis=1)
mappedFrameNode.createObject('DiscretCosseratMapping', curv_abs_input=curv_abs_input,
curv_abs_output=curv_abs_output, input1=inputMO, input2=inputMO_rigid,
output=outputMO, debug='0', max=6.e-2, deformationAxis=1, nonColored="0", radius=5)
# actuators = mappedFrameNode.createChild('actuators')
# actuator0 = actuators.createObject('SlidingActuator', name="SlidingActuator0", template='Rigid3d',
# direction='0 0 0 1 0 0', indices=1, maxForce='100000', minForce='-30000')
cable_position = [[0.0, 0.0, 0.0], [5.0, 0.0, 0.0], [10.0, 0.0, 0.0], [15.0, 0.0, 0.0], [20.0, 0.0, 0.0],
[30.0, 0.0, 0.0], [35.0, 0.0, 0.0], [40.0, 0.0, 0.0], [45.0, 0.0, 0.0],
[55.0, 0.0, 0.0], [60.0, 0.0, 0.0], [66.0, 0.0, 0.0], [71.0, 0.0, 0.0], [76.0, 0.0, 0.0],
[81.0, 0.0, 0.0]]
# This create a new node in the scene. This node is appended to the finger's node.
slidingPoint = mappedFrameNode.createChild('slidingPoint')
# This create a MechanicalObject, a componant holding the degree of freedom of our
# mechanical modelling. In the case of a cable it is a set of positions specifying
# the points where the cable is passing by.
slidingPointMO = slidingPoint.createObject('MechanicalObject', name="cablePos",
position=cable_position, showObject="1", showIndices="0")
slidingPoint.createObject('IdentityMapping')
mappedPointsNode = slidingPoint.createChild('MappedPoints')
femPoints.addChild(mappedPointsNode)
mappedPoints = mappedPointsNode.createObject('MechanicalObject', template='Vec3d', position=FEMpos,
name="FramesMO", showObject='1', showObjectScale='1')
inputCableMO = slidingPointMO.getLinkPath()
inputFEMCableMO = inputFEMCable.getLinkPath()
outputPointMO = mappedPoints.getLinkPath()
mappedPointsNode.createObject('QPSlidingConstraint', name="QPConstraint")
mappedPointsNode.createObject('DifferenceMultiMapping', name="pointsMulti", input1=inputFEMCableMO,
input2=inputCableMO, output=outputPointMO, direction="@../../FramesMO.position")
## Get the tree mstate links for the mapping
return rootNode
def createScene(rootNode):
# main header (rootNode + gravity)
MainHeader(rootNode, gravity=[])
# contact header
ContactHeader(rootNode, alarmDistance=0,
contactDistance=)
# shows the grid outline
ShowGrid(rootNode)
# base floor
Floor(rootNode,
translation=[],
uniformScale=0.0,
isAStaticObject=True)
# obstacle floor
Floor(rootNode,
name="Floor Obstacle",
translation=[],
color=[],
uniformScale=0.0,
isAStaticObject=True)
# creates cubes
for i in range(0):
Cube(rootNode,
name=,
translation=[],
color=[],
uniformScale=0.0)
#---------------------------------------------------------------------------------#
# creating children (ie. a cube) for the root node
cube = rootNode.createChild("Cube")
# Mechanical Model
totalMass = 0.0
volume = 0.0
inertiaMatrix = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# create an Object
cube.createObject('ObjectName', name="name",
template="Rigid", translation=[0.0, 0.0, 0.0],
rotation = [0.0, 0.0, 0.0])
# create a Uniform Mass Object
cube.createObject('UniformMass', name="Mass",
mass=[totalMass, volume, inertiaMatrix[:]])
# Time Integration Scheme
cube.createObject('EulerImplicit', name="")
# Solving Method
cube.createObject('CGLinearSolver', name="")
# create a visual object of child object (ie. cube)
visual = cube.createChild("Cube Visual")
# Visual Ogl Model
visual.createObject('OglModel', name="Visual",
fileMesh="mesh/name.ob",
colour=[0.0,0.0,0.0],
scale=0.0)
# Rigid Mapping Object
visual.createObject('RigidMapping')
# Collision Model for the Cube
collision = cube.createChild("Cube Collision Model")
# Creating Collision Objects
collision.createObject('MeshObjLoader', name="",
filename="mesh/name.obj",
triangulate="true", scale=0.0)
# example collision objects to add
collision.createObject('Mesh', src="@loader")
collision.createObject('MechanicalObject')
collision.createObject('Triangle')
collision.createObject('Line')
collision.createObject('Point')
collision.createObject('RigidMapping')
return rootNode
def createScene(rootNode):
import os
from stlib.scene import MainHeader, ContactHeader
MainHeader(rootNode,
plugins=[
"SoftRobots", "SofaPython", "SofaSparseSolver",
"SofaPreconditioner", "SofaOpenglVisual", "CosseratPlugin",
"BeamAdapter"
],
repositoryPaths=[os.getcwd()])
# rootNode.createObject('RequiredPlugin', pluginName='SoftRobots SofaPython SofaSparseSolver ')
rootNode.createObject(
'VisualStyle',
displayFlags=
'showVisualModels hideBehaviorModels showCollisionModels hideBoundingCollisionModels hideForceFields showInteractionForceFields showWireframe'
)
rootNode.gravity = "0 -0 0"
rootNode.dt = "0.01"
ContactHeader(rootNode,
alarmDistance=4,
contactDistance=3,
frictionCoef=0.08)
rootNode.createObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.createObject('OglSceneFrame',
style="Arrows",
alignment="TopRight")
rootNode.createObject('FreeMotionAnimationLoop')
rootNode.createObject('GenericConstraintSolver',
tolerance="1e-5",
maxIterations="100",
printLog="1")
##########################################
# FEM Model #
##########################################
finger = rootNode.createChild('finger')
finger.createObject('EulerImplicitSolver',
name='odesolver',
firstOrder='0',
rayleighMass="0.1",
rayleighStiffness="0.1")
finger.createObject('SparseLDLSolver', name='preconditioner')
# Add a componant to load a VTK tetrahedral mesh and expose the resulting topology in the scene .
# finger.createObject('MeshVTKLoader', name='loader', filename=path+'finger.vtk', translation="-10.5 -3.0 7.5", rotation="0 180 0")
# finger.createObject('MeshVTKLoader', name='loader', filename=path+'transformFinger0.vtu', translation="10.4 0.0 0.0")
finger.createObject('MeshGmshLoader',
name='loader',
filename=path + 'transformFinger_2_2.msh')
finger.createObject('TetrahedronSetTopologyContainer',
src='@loader',
name='container')
finger.createObject('TetrahedronSetTopologyModifier')
finger.createObject('TetrahedronSetTopologyAlgorithms', template='Vec3d')
finger.createObject('TetrahedronSetGeometryAlgorithms', template='Vec3d')
finger.createObject('MechanicalObject',
name='tetras',
template='Vec3d',
showIndices='false',
showIndicesScale='4e-5',
rx='0',
dz='0')
finger.createObject('UniformMass', totalMass='0.00001')
# finger.createObject('VTKExporter', filename=path+"transformFinger_2", edges="1", tetras="1", triangles="1", listening="true", exportAtEnd="true")
finger.createObject('TetrahedronFEMForceField',
template='Vec3d',
name='FEM',
method='large',
poissonRatio='0.48',
youngModulus='100')
# finger.createObject('BoxROI', name='ROI1', box='-18 -15 -8 2 -3 8', drawBoxes='false')
# finger.createObject('RestShapeSpringsForceField', points='@ROI1.indices', stiffness='1e12')
boxeElement = finger.createObject('BoxROI',
name='commonPoints',
box='-1.0 -3 -10 95 3 10',
drawBoxes='0',
drawSize="1")
# boxeElement.init()
boxeElement.bwdInit()
indices = boxeElement.findData('indices').value
print("Indices : ", indices)
#Visu Finger
visuFinger = finger.createChild('visuFinger')
visuFinger.createObject('MeshSTLLoader',
name='surfLoader',
filename=path + 'transformFinger_2_2.stl')
visuFinger.createObject('TriangleSetTopologyContainer',
name='container',
src='@surfLoader')
visuFinger.createObject('TriangleSetTopologyModifier', name="modifier")
visuFinger.createObject('MechanicalObject',
name='finger_mstate',
position="@container.position")
visuFinger.createObject('Triangle')
visuFinger.createObject('BarycentricMapping')
################################################
#### Common points between finger (FEM) and the Beam (Cosserat)
################################################
points = finger.createChild('points')
points.createObject('MechanicalObject',
name="pointsInFEM",
position="@../commonPoints.pointsInROI",
showObject="1",
showIndices="1")
points.createObject('BarycentricMapping')
# CollisionMesh(finger, surfaceMeshFileName="mesh/finger.stl", name="part0", translation="-10.5 -3.0 7.5", rotation="0 180 0", collisionGroup=[1, 2])
# finger.createObject('LinearSolverConstraintCorrection')
finger.createObject('GenericConstraintCorrection')
# return rootNode
##########################################
# Cosserat Model #
##########################################
cableNode = rootNode.createChild('cableNode')
cableNode.createObject('EulerImplicitSolver',
firstOrder="0",
rayleighStiffness="0.01",
rayleighMass='0.01')
cableNode.createObject('SparseLUSolver', name='solver')
cableNode.createObject('GenericConstraintCorrection')
###############hresho
## RigidBase
###############
rigidBaseNode = cableNode.createChild('rigidBase')
RigidBaseMO = rigidBaseNode.createObject('MechanicalObject',
template='Rigid3d',
name="RigidBaseMO",
position="0 0 0 0 0 0 1",
showObject='1',
showObjectScale='0.1')
rigidBaseNode.createObject('RestShapeSpringsForceField',
name='spring',
stiffness="50000",
angularStiffness="50000",
external_points="0",
mstate="@RigidBaseMO",
points="0",
template="Rigid3d")
###############
## Rate of angular Deformation (2 sections)
###############
rateAngularDeformNode = cableNode.createChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.createObject(
'MechanicalObject',
template='Vec3d',
name='rateAngularDeformMO',
position=position)
# BeamHookeLawForce = rateAngularDeformNode.createObject('CosseratInternalActuation', name="BeamHookeLawForce", crossSectionShape='circular', length=longeur, radius='0.5',
# youngModulus='5.93e4',distance0=_distance, distance1=_distance, ddistance=_ddistance, tension=_tension)
BeamHookeLawForce = rateAngularDeformNode.createObject(
'CosseratInternalActuation',
name="BeamHookeLawForce",
crossSectionShape='rectangular',
length=longeur,
radius='0.5',
youngModulus='5.93e3',
distance0=_distance,
distance1=_distance,
ddistance=_ddistance,
tension=_tension,
lengthY='3',
lengthZ='10')
rateAngularDeformNode.createObject('PythonScriptController',
classname="Animation")
##############
## Frames
##############
frames = [
"0.0 0 0 0 0 0 1 5 0 0 0 0 0 1 10.0 0 0 0 0 0 1 15.0 0 0 0 0 0 1 20.0 0 0 0 0 0 1"
+
" 30.0 0 0 0 0 0 1 35.0 0 0 0 0 0 1 40.0 0 0 0 0 0 1 45.0 0 0 0 0 0 1 55.0 0 0 0 0 0 1 60.0 0 0 0 0 0 1"
+
" 66.0 0 0 0 0 0 1 71.0 0 0 0 0 0 1 76.0 0 0 0 0 0 1 85.0 0 0 0 0 0 1 90.0 0 0 0 0 0 1 95.0 0 0 0 0 0 1 100.0 0 0 0 0 0 1 103.0 0 0 0 0 0 1"
]
# the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform
mappedFrameNode = rigidBaseNode.createChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.createObject('MechanicalObject',
template='Rigid3d',
name="FramesMO",
position=frames,
showObject='1',
showObjectScale='1')
# The mapping has two inputs: RigidBaseMO and rateAngularDeformMO
# one output: FramesMO
inputMO = rateAngularDeformMO.getLinkPath()
inputMO_rigid = RigidBaseMO.getLinkPath()
outputMO = framesMO.getLinkPath()
mappedFrameNode.createObject('DiscretCosseratMapping',
curv_abs_input=curv_abs_input,
curv_abs_output=curv_abs_output,
input1=inputMO,
input2=inputMO_rigid,
output=outputMO,
debug='0')
# ##### CubeTopo
cubeTopo = mappedFrameNode.createChild('cubeTopo')
cubeTopo.createObject('CubeTopology',
name='loader',
internalPoints="0",
splitNormals="1",
nx="12",
ny="5",
nz="5",
min="0 -3 -10",
max="103 3 10",
drawEdges="0")
cubeTopo.createObject('TriangleSetTopologyContainer',
name='Container',
position="@loader.position")
cubeTopo.createObject('TriangleSetTopologyModifier', name="modifier")
cubeTopo.createObject('Quad2TriangleTopologicalMapping',
name="converter",
input="@loader",
output="@Container")
cubeTopo.createObject('MechanicalObject',
name='cube_mstate',
position="@Container.position")
cubeTopo.createObject('Triangle', color="0 1 0")
cubeTopo.createObject('SkinningMapping', nbRef='2')
pointsCosserat = cubeTopo.createChild('points')
element = pointsCosserat.createObject(
'MechanicalObject',
name="mstateCoss",
position="@../../../../../finger/commonPoints.pointsInROI",
showObject="0",
showIndices="0")
pointsCosserat.createObject('BarycentricMapping')
element.bwdInit()
listOfPoints = element.findData('position').value
print("=++==================> ", listOfPoints)
# cubeTopo.addChild(points)
pointsPairs = []
for i in range(0, 8):
pointsPairs.append(i)
# pointsPairs='0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15'
rootNode.createObject(
'BilateralInteractionConstraint',
template='Vec3d',
object2='@cableNode/rigidBase/MappedFrames/cubeTopo/points/mstateCoss',
object1='@finger/points/pointsInFEM',
first_point=pointsPairs,
second_point=pointsPairs,
merge="true")
return rootNode
def createScene(rootNode):
MainHeader(rootNode,
plugins=["SofaPython", "SoftRobots"],
gravity=[0.0, -9810, 0.0])
rootNode.getObject('GenericConstraintSolver').findData(
'maxIterations').value = '100000'
rootNode.getObject('GenericConstraintSolver').findData(
'tolerance').value = '1e-12'
ContactHeader(rootNode,
alarmDistance=5,
contactDistance=1,
frictionCoef=0.7)
GripperController(rootNode)
Floor(rootNode, **floorParam)
Cube(rootNode, **cubeParam)
# Put treshold in rigibObject construction param ?
rootNode.getChild('Cube').getObject('Solver').findData(
'threshold').value = '1e-6'
for i in range(len(fingersParameters)):
finger = ElasticMaterialObject(
attachedTo=rootNode,
volumeMeshFileName=fingersVolumeMesh,
name=fingersParameters[i]['name'],
rotation=fingersParameters[i]['rotation'],
translation=fingersParameters[i]['translation'],
surfaceMeshFileName=fingersSurfaceAndCollisionMesh,
collisionMesh=fingersSurfaceAndCollisionMesh,
withConstrain=True,
surfaceColor=fingersColor,
poissonRatio=poissonRatioFingers,
youngModulus=youngModulusFingers,
totalMass=fingersMass)
finger.getObject('EulerImplicit').findData(
'rayleighStiffness').value = 0.1
finger.getObject('EulerImplicit').findData('rayleighMass').value = 0.1
finger.createObject('BoxROI',
name='boxROI',
box=fingersParameters[i]['ROIBox'],
drawBoxes='true',
doUpdate='0')
finger.createObject('RestShapeSpringsForceField',
points='@../finger1/boxROI.indices',
stiffness='1e12',
angularStiffness='1e12')
SubTopology(attachedTo=finger,
topologiyContainer='container',
subTopologiyContainer=subTopoParameters[i]
['subTopologiyContainer'],
atPositions=subTopoParameters[i]['atPositions'],
poissonRatio=poissonRatioFingers,
youngModulus=youngModulusStiffLayerFingers -
youngModulusFingers)
PneumaticCavity(surfaceMeshFileName=fingersCavitySurfaceMesh,
attachedAsAChildOf=finger,
name='cavity',
rotation=fingersParameters[i]['rotation'],
translation=fingersParameters[i]['translation'],
initialValue=cavitiesInitialValue,
valueType='pressure')
def createScene(rootNode):
MainHeader(rootNode,
plugins=[
"SoftRobots", "SoftRobots.Inverse", "SofaPython",
"SofaSparseSolver", "SofaConstraint", "SofaPreconditioner",
"SofaOpenglVisual", "CosseratPlugin", "BeamAdapter"
],
repositoryPaths=[os.getcwd()])
# rootNode.createObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels showCollisionModels '
# 'hideBoundingCollisionModels hideForceFields '
# 'showInteractionForceFields showWireframe')
rootNode.createObject(
'VisualStyle',
displayFlags='showVisualModels showInteractionForceFields showWireframe'
)
rootNode.createObject('FreeMotionAnimationLoop')
# rootNode.createObject('QPInverseProblemSolver', printLog='0')
rootNode.createObject('GenericConstraintSolver',
tolerance="1e-20",
maxIterations="500",
printLog="0")
ContactHeader(rootNode,
alarmDistance=2.5,
contactDistance=2,
frictionCoef=0.08)
rootNode.gravity = "0 981 0"
rootNode.createObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.createObject('OglSceneFrame',
style="Arrows",
alignment="TopRight")
showObject = "0"
showIndices = "0"
##########################################
# FEM Model #
##########################################
finger = rootNode.createChild('finger')
finger.createObject('EulerImplicitSolver',
name='odesolver',
firstOrder='0',
rayleighMass=0.1,
rayleighStiffness=0.1)
finger.createObject('SparseLDLSolver', name='preconditioner')
# Add a componant to load a VTK tetrahedral mesh and expose the resulting topology in the scene .
finger.createObject('MeshVTKLoader',
name='loader',
filename=path + 'finger.vtk',
translation="-17.5 -12.5 7.5",
rotation="0 180 0")
# finger.createObject('MeshExporter', name='loader', filename=path +'transFinger.vtk', exportAtEnd="true")
finger.createObject('TetrahedronSetTopologyContainer',
src='@loader',
name='container')
finger.createObject('TetrahedronSetTopologyModifier')
# finger.createObject('TetrahedronSetTopologyAlgorithms', template='Vec3d')
# finger.createObject('TetrahedronSetGeometryAlgorithms', template='Vec3d')
# Create a mechanicaobject component to stores the DoFs of the model
finger.createObject('MechanicalObject',
name='tetras',
template='Vec3d',
showIndices='false',
showIndicesScale='4e-5',
rx='0',
dz='0')
# Gives a mass to the model
finger.createObject('UniformMass', totalMass='0.075')
# Add a TetrahedronFEMForceField componant which implement an elastic material model
# solved using the Finite Element Method on
# tetrahedrons.
finger.createObject('TetrahedronFEMForceField',
template='Vec3d',
name='FEM',
method='large',
poissonRatio='0.45',
youngModulus='1200')
finger.createObject('BoxROI',
name='ROI1',
box='-18 -15 -8 2 -3 8',
drawBoxes='true')
finger.createObject('RestShapeSpringsForceField',
points='@ROI1.indices',
stiffness='1e12')
##########################################
# Cable points #
##########################################
# Mappe points inside the meca, this points will be use for the bilateral mapping
FEMpos = [" 0.0 0 0 15 0 0 30 0 0 45 0 0 60 0 0 66 0 0 81 0.0 0.0"]
# FEMpos = [" 81 0.0 0.0"]
femPoints = finger.createChild('femPoints')
inputFEMCable = femPoints.createObject('MechanicalObject',
name="pointsInFEM",
position=FEMpos,
showObject="0",
showIndices="0")
femPoints.createObject('BarycentricMapping')
# spheres = ["30. 0 0 48. 0 0 66 0 0 81. 0.0 0.0"]
# spheresPos = finger.createChild('spheresPos')
# spheresPosMec = spheresPos.createObject('MechanicalObject', name="spheresGoal", position=spheres, showObject="0",
# showIndices="1")
# spheresPos.createObject('BarycentricMapping')
finger.createObject('LinearSolverConstraintCorrection')
##########################################
# Effector goal for interactive control #
##########################################
GoalPos = [[20.6307, 5.57305, -0.494896], [32.5759, 17.6405, -1.11956],
[35.3802, 28.458, -1.52895], [36.3606, 42.5902, -1.85686]]
goal = rootNode.createChild('goal')
goal.createObject('EulerImplicitSolver', firstOrder='1')
goal.createObject('CGLinearSolver',
iterations='100',
tolerance="1e-5",
threshold="1e-5")
# goal.createObject('MechanicalObject', name='goalMO', position='76.591 3.13521 0.35857')
goal.createObject('MechanicalObject', name='goalMO', position=GoalPos)
goal.createObject('SphereCollisionModel', radius='1.5')
goal.createObject('UncoupledConstraintCorrection')
goal.createObject(
'VisualStyle',
displayFlags=
'showVisualModels showInteractionForceFields showCollisionModels')
##########################################
# Visualization #
##########################################
fingerVisu = finger.createChild('visu')
fingerVisu.createObject('MeshSTLLoader',
filename=path + "finger.stl",
name="loader",
translation="-17.5 -12.5 7.5",
rotation="0 180 0")
# fingerVisu.createObject('STLExporter', filename=path+"transFinger", exportAtEnd="true")
fingerVisu.createObject('OglModel',
src="@loader",
template='ExtVec3f',
color="0.0 0.7 0.7")
fingerVisu.createObject('BarycentricMapping')
# ###############
# New adds to use the sliding Actuator
###############
cableNode = rootNode.createChild('cableNode')
cableNode.createObject('EulerImplicitSolver',
firstOrder="0",
rayleighStiffness="1.0",
rayleighMass='0.1')
cableNode.createObject('SparseLUSolver', name='solver')
cableNode.createObject('GenericConstraintCorrection')
# ###############
# RigidBase
###############
rigidBaseNode = cableNode.createChild('rigidBase')
RigidBaseMO = rigidBaseNode.createObject('MechanicalObject',
template='Rigid3d',
name="RigidBaseMO",
position="0 0 0 0 0 0 1",
showObject='1',
showObjectScale='5.')
rigidBaseNode.createObject('RestShapeSpringsForceField',
name='spring',
stiffness="50000",
angularStiffness="50000",
external_points="0",
mstate="@RigidBaseMO",
points="0",
template="Rigid3d")
###############
# Rate of angular Deformation (2 sections)
###############
nbSectionS = 14
lengthS = 80.0 / nbSectionS
positionS = []
longeurS = []
sum = 0.
# curv_abs_input = '0 15 30 45 60 66 81'
curv_abs_inputS = []
curv_abs_inputS.append(0.0)
for i in range(nbSectionS):
positionS.append([0, 0, 0])
longeurS.append((((i + 1) * lengthS) - i * lengthS))
sum += longeurS[i]
curv_abs_inputS.append(sum)
longeurS[nbSectionS - 1] = longeurS[nbSectionS - 1] + 1.
curv_abs_inputS[nbSectionS] = 81.
print("=============> positionS : ", positionS)
print("=============> longeurS : ", longeurS)
print("=============> curv_abs_inputS : ", curv_abs_inputS)
# position = ["0 0 0 " + "0 0 0 " + "0 0 0 " +
# "0 0 0 " + "0 0 0 " + "0 0 0 "]
# longeur = '15 15 15 15 6 15' # beams size
rateAngularDeformNode = cableNode.createChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.createObject(
'MechanicalObject',
template='Vec3d',
name='rateAngularDeformMO',
position=positionS,
showIndices="0")
BeamHookeLawForce = rateAngularDeformNode.createObject(
'BeamHookeLawForceField',
crossSectionShape='circular',
length=longeurS,
radius='0.50',
youngModulus='5e6')
################################
# Animation (to move the dofs) #
################################
anim = Animation(rigidBaseNode, rateAngularDeformNode)
##############
# Frames #
##############
nbFramesF = 28
lengthF = 80.0 / nbFramesF
framesF = []
curv_abs_outputF = []
cable_positionF = []
for i in range(nbFramesF):
sol = i * lengthF
framesF.append([sol, 0, 0, 0, 0, 0, 1])
cable_positionF.append([sol, 0, 0])
curv_abs_outputF.append(sol)
framesF.append([81., 0, 0, 0, 0, 0, 1])
cable_positionF.append([81., 0, 0])
curv_abs_outputF.append(81.)
# the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform
mappedFrameNode = rigidBaseNode.createChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.createObject('MechanicalObject',
template='Rigid3d',
name="FramesMO",
position=framesF,
showObject='0',
showObjectScale='1')
# The mapping has two inputs: RigidBaseMO and rateAngularDeformMO
# one output: FramesMO
inputMO = rateAngularDeformMO.getLinkPath()
inputMO_rigid = RigidBaseMO.getLinkPath()
outputMO = framesMO.getLinkPath()
mappedFrameNode.createObject('DiscretCosseratMapping',
curv_abs_input=curv_abs_inputS,
curv_abs_output=curv_abs_outputF,
input1=inputMO,
input2=inputMO_rigid,
output=outputMO,
debug='0',
max=6.e-2,
deformationAxis=2,
nonColored="0",
radius=5)
# This create a new node in the scene. This node is appended to the finger's node.
slidingPoint = mappedFrameNode.createChild('slidingPoint')
# This create a MechanicalObject, a componant holding the degree of freedom of our
# mechanical modelling. In the case of a cable it is a set of positions specifying
# the points where the cable is passing by.
slidingPointMO = slidingPoint.createObject('MechanicalObject',
name="cablePos",
position=cable_positionF,
showObject="0",
showIndices="0")
slidingPoint.createObject('IdentityMapping')
mappedPointsNode = slidingPoint.createChild('MappedPoints')
femPoints.addChild(mappedPointsNode)
mappedPoints = mappedPointsNode.createObject('MechanicalObject',
template='Vec3d',
position=FEMpos,
name="FramesMO",
showObject='0',
showObjectScale='0')
inputCableMO = slidingPointMO.getLinkPath()
inputFEMCableMO = inputFEMCable.getLinkPath()
outputPointMO = mappedPoints.getLinkPath()
mappedPointsNode.createObject('QPSlidingConstraint', name="QPConstraint")
mappedPointsNode.createObject('DifferenceMultiMapping',
name="pointsMulti",
input1=inputFEMCableMO,
input2=inputCableMO,
output=outputPointMO,
direction="@../../FramesMO.position")
## Get the tree mstate links for the mapping
return rootNode
def addContact(self, alarmDistance=5, contactDistance=2, frictionCoef=0.0):
ContactHeader(self, alarmDistance, contactDistance, frictionCoef)
def createScene(rootNode):
#Its helpful to be able to see the grid
ShowGrid(rootNode)
#Adding the Soft-Robotics Plugin and the SofaPython plugin
rootNode.createObject('RequiredPlugin', name='soft', pluginName='SoftRobots')
rootNode.createObject('RequiredPlugin', name='SofaPython', pluginName='SofaPython')
rootNode.createObject('RequiredPlugin', name='SofaSparseSolver', pluginName='SofaSparseSolver')
#Gravity is automatically defined but we can redefine it
rootNode.findData('gravity').value='0 0 -981'
#It is nice to have the useful visualizatioin visible upon reloading the scene
#you can find the options in the view tab in the GUI
rootNode.createObject('VisualStyle', displayFlags='showForceFields showBehaviorModels')
#Add BackgroundSetting in order not to have to reset it to white everytime you reload
rootNode.createObject('BackgroundSetting', color='1 1 1')
#Add the axis in the upper right corner automatically
rootNode.createObject('OglSceneFrame', style="Arrows", alignment="TopRight")
#The standard Global Bounding Box is very small [-1,-1,-1,1,1,1]
#in comparison to our object so we make it bigger
rootNode.findData('bbox').value= '-200 -200 -200 200 200 200'
#Let's add the Child node for the Mesh
cube = rootNode.createChild('cube')
#Adding the Mesh file and make sure it finds it in the location directed to
cube.createObject('MeshVTKLoader', name='loader', filename= pa 'mesh_cube20.vtk')
#Now let's load the Mehs and make it a MechObj, which stores and is set to show the degrees of freedom of our box
cube.createObject('Mesh', src='@loader', name='container')
cube.createObject('MechanicalObject', name='tetras', template='Vec3d', showObject='true')
#Next we want to make the material soft so we add a forcefield
#We use TetrahedronFEMForceField in this case because of the nature of our mesh
#Here we also determine the Young's Modulus and Poisson's ratio for the material
#method is related to either small or large displacements
cube.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM', method='large', poissonRatio='0.49', youngModulus='200')
#Next we want to add the mass, since we want to uniformly distribute it we can either use
#vertexMass which is the mass at each node, stored in the MevhanicalObject
#or totalMass which is the total mass, which is then spread over the nodes.
#Beware these don't consider the geometry or topology of the system. More details, also on non-uniform masses:
#https://www.sofa-framework.org/community/doc/using-sofa/components/masses/uniformmass/
cube.createObject('UniformMass', totalMass='0.01')
#For initial try I will add a boundary condition for the bottom of the box
#This will fix the box in place if I set the stifness of the springs rather high
#As it creates springs between its initial and current position
#In this case I placed the box in the xy-plane going from 0-100 and up to 10 along the z-axis
#cube.createObject('BoxROI', name='boxROI', box='0 100 0 100 0 10', drawBoxes=True)
#cube.createObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness='1e12', angularStiffness='1e12')
#Now we add a time integration scene, allowing the system to be computed at each time step
#We will use the EulerImplicit solver where the forces are based off of
#the information of the next time step. Although being slower than the Explicit solver,
#they are more stable and are needed for stiff differential equations
cube.createObject('EulerImplicit', name='odesolver')
#And following that we add a Matrix solver, this one is a direct solver based off of A=LDL^T
#This one can be slow for very large systems but gives exact solutions
cube.createObject('SparseLDLSolver', name='directSolver')
#At this point it will deform a bit under gravity but not involve other dynamics
#Lets add the pneumatic actuators
#First let's create the cavity add the mesh and load it
cavity = cube.createChild('cavity')
cavity.createObject('MeshSTLLoader', name='loader', filename='mesh_cavity20.stl')
cavity.createObject('Mesh', src='@loader', name='topo')
#Now we must make the MechanicalObject of the surface mesh to store the degrees of freedom
#along the surface that will cause deformations
cavity.createObject('MechanicalObject', name='cavity')
#Now we will add a SurfacePressureConstraint
#This adds a constant pressure on a surface
#Such that each point of the mesh receives the pressure of the elements it is a part of
#You need to specify:
#are you using triangles or quads
#value
#valueIndex (default is {0})
#valueType; pressure or volumeGrowth
#For more info: https://project.inria.fr/softrobot/documentation/constraint/surface-pressure-constraint/
cavity.createObject('SurfacePressureConstraint', name='SurfacePressureConstraint',
template='Vec3d', value='0.0001', triangles='@topo.triangles', valueType='pressure')
#Now we need to use a BarycentricMapping to map the deformation of the cavity onto our 3d Mesh
cavity.createObject('BarycentricMapping', name='mapping')
#Now as a first test we will use the prewritten controller program which uses +/- to inflate and deflate
rootNode.createObject('PythonScriptController', filename='oscillationController.py', classname="controller")
#Now we will add an animationloop, the easiest is to add the FreeMotionAnimationLoop
rootNode.createObject('FreeMotionAnimationLoop')
#Furthermore we will add to solvers to take various constraints into account
rootNode.createObject('GenericConstraintSolver', maxIterations='10000', tolerance='1e-3')
cube.createObject('LinearSolverConstraintCorrection', solverName='directSolver')
#Let's add a visual model for the vube
visualCube = cube.createChild('visualCube')
visualCube.createObject('MeshSTLLoader', name='loader', filename='mesh_cube20.stl')
visualCube.createObject('OglModel', src='@loader', template='ExtVec3d', color='0.4 0.9 0.9')
visualCube.createObject('BarycentricMapping')
#In order to visualize and export the scene we can use the monitor component
#This is added to an indice of the component it is directly monitoring
#cube.createObject('Monitor', name="monitor-39", indices="178 228 278 303 253 203", template="Vec3d",
# showPositions=True, PositionsColor="1 0 1 1", ExportPositions=False,
# showVelocities=True, VelocitiesColor="0.5 0.5 1 1", ExportVelocities=False,
# showForces=True, ForcesColor="0.8 0.2 0.2 1", ExportForces=False,
# showTrajectories=False, TrajectoriesPrecision="0.1", TrajectoriesColor="0 1 1 1", sizeFactor="0.5")
#Now let's add a floor on which the cube will rest. We will use a rigid, preset floor from SOFA for the floor
#It is a preset with preset totalMass=1.0 and totalVolume=1.0. We will set it to be static.
#Furthermore it already includes its own collisionmodel.
#This includes the Mesh of the outer surface of the floor, A mapping between this mesh and that of the volume mesh of the floor
#and it tells all triangles, lines and points how to interact upon collision. (By not moving)
Floor(rootNode, translation=[0.0, 0.0, 0.0], rotation=[90.0, 0.0, 0.0], isAStaticObject=True, uniformScale=8.0)
#Now let's add the collisionmodel to the cube
collisionCube = cube.createChild("collisionCube")
#First we will load and store the surface mesh of the cube, and turn it into a mechanicalObject
#You could think about the translation of this object when wanting to add more cubes but for now it is not necessary
collisionCube.createObject('MeshSTLLoader', name='loader', filename='mesh_cube20.stl')
collisionCube.createObject('Mesh', src='@loader', name='topo')
collisionCube.createObject('MechanicalObject', name='collisMech')
#Now we add the behavior for the triangles, lines and points for the collision mesh
collisionCube.createObject('Triangle', selfCollision=False)
collisionCube.createObject('Line', selfCollision=False)
collisionCube.createObject('Point', selfCollision=False)
#Finally we add the mapping of this mesh to that of its parant node the cube
collisionCube.createObject('BarycentricMapping')
#We add the contactmodel and the corresponding friction coefficient
ContactHeader(rootNode, alarmDistance=5, contactDistance=1, frictionCoef=0.0)