def createScene(rootNode):
from stlib3.scene import MainHeader
from stlib3.physics.deformable import ElasticMaterialObject
MainHeader(rootNode)
# Tetrahedron and triangle subtopology
target = ElasticMaterialObject(volumeMeshFileName="mesh/liver2.msh",
totalMass=0.5,
attachedTo=rootNode)
target.addObject('BoxROI',
name='boxROI',
box=[-20, -20, -20, 20, 20, 20],
drawBoxes=True)
SubTopology(attachedTo=target,
containerLink='@../container.position',
boxRoiLink='@../boxROI.tetrahedraInROI',
name='Default-tetrahedron')
SubTopology(attachedTo=target,
containerLink='@../container.position',
linkType='triangle',
boxRoiLink='@../boxROI.trianglesInROI',
name='Triangles')
def createScene(rootNode):
from stlib3.scene import MainHeader
from stlib3.physics.rigid import Floor
MainHeader(rootNode,
plugins=["SofaPython"],
dt=1.,
gravity=[0., -9810., 0.])
rootNode.VisualStyle.displayFlags = "showVisual showBehavior"
floor = Floor(rootNode,
name="Plane",
color=[1., 0., 1.],
isAStaticObject=True,
uniformScale=10)
addOrientedBoxRoi(floor,
name="MyBoxRoi",
position=[[50, 0, 0], [15, 15, 0], [60, 70, 25]],
scale=[100, 100, 100])
myOrientedBox = getOrientedBoxFromTransform(translation=[400, 100, 100],
eulerRotation=[0, 65, 0],
scale=[400, 400, 800])
floor.addObject("BoxROI", orientedBox=myOrientedBox, drawBoxes=True)
def createScene(rootNode):
from stlib3.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):
from stlib3.scene import MainHeader
from stlib3.solver import DefaultSolver
MainHeader(rootNode)
DefaultSolver(rootNode)
Cube(rootNode, translation=[5.0,0.0,0.0])
Sphere(rootNode, translation=[-5.0,0.0,0.0])
Floor(rootNode, translation=[0.0,-1.0,0.0])
def createScene(node):
from stlib3.scene import MainHeader
from stlib3.physics.deformable import ElasticMaterialObject
MainHeader(node, plugins=["SoftRobots"])
target = ElasticMaterialObject(volumeMeshFileName="mesh/liver.msh",
totalMass=0.5,
attachedTo=node)
PullingCable(target)
def createScene(rootNode):
from stlib3.scene import MainHeader
from stlib3.physics.deformable import ElasticMaterialObject
from stlib3.physics.constraints import FixedBox
MainHeader(rootNode)
target = ElasticMaterialObject(volumeMeshFileName="mesh/liver.msh",
totalMass=0.5,
attachedTo=rootNode)
FixedBox(atPositions=[-4, 0, 0, 5, 5, 4],
applyTo=target,
doVisualization=True)
def createScene(rootNode):
from stlib3.scene import MainHeader
MainHeader(rootNode, gravity=[0, 0, 0])
rootNode.addChild(
ElasticMaterialObject(name='ElasticMaterialObject1',
volumeMeshFileName="mesh/liver.msh",
translation=[3.0, 0.0, 0.0]))
rootNode.addChild(
ElasticMaterialObject(name='ElasticMaterialObject2',
volumeMeshFileName="mesh/liver.msh",
translation=[-3, 0, 0],
surfaceMeshFileName="mesh/liver.obj",
surfaceColor=[1.0, 0.0, 0.0]))
def createScene(rootNode):
from stlib3.scene import MainHeader
from stlib3.physics.deformable import ElasticMaterialObject
from stlib3.physics.constraints import PartiallyFixedBox
MainHeader(rootNode)
target = ElasticMaterialObject(fromVolumeMesh="mesh/liver.msh",
withTotalMass=0.5,
attachedTo=rootNode)
PartiallyFixedBox(box=[-4, 0, 0, 5, 5, 4],
attachedTo=target,
drawBoxes=True,
fixedAxis=[0, 1, 0])
def createScene(rootNode):
"""
"""
from stlib3.scene import MainHeader
from stlib3.physics.deformable import ElasticMaterialObject
from stlib3.physics.mixedmaterial import Rigidify
from splib3.objectmodel import setData
MainHeader(rootNode,
plugins=[
"SofaSparseSolver", "SofaImplicitOdeSolver",
"SofaMiscMapping", "SofaRigid", "SofaBoundaryCondition"
])
rootNode.VisualStyle.displayFlags = "showBehavior"
modelNode = rootNode.addChild("Modeling")
elasticobject = ElasticMaterialObject(volumeMeshFileName="mesh/liver.msh",
name="ElasticMaterialObject")
modelNode.addChild(elasticobject)
# Rigidification of the elasticobject for given indices with given frameOrientations.
o = Rigidify(modelNode,
elasticobject,
name="RigidifiedStructure",
frames=[[0., 0., 0], [0., 0., 0]],
groupIndices=[[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10],
[48, 49, 50, 51]])
# Activate some rendering on the rigidified object.
setData(o.RigidParts.dofs, showObject=True, showObjectScale=1, drawMode=2)
setData(o.RigidParts.RigidifiedParticules.dofs,
showObject=True,
showObjectScale=0.1,
drawMode=1,
showColor=[1., 1., 0., 1.])
setData(o.DeformableParts.dofs,
showObject=True,
showObjectScale=0.1,
drawMode=2)
o.RigidParts.addObject("FixedConstraint", indices=0)
simulationNode = rootNode.addChild("Simulation")
simulationNode.addObject("EulerImplicitSolver")
simulationNode.addObject("CGLinearSolver")
simulationNode.addChild(o)
return rootNode
def createScene(rootNode):
from stlib3.scene import MainHeader
MainHeader(rootNode,
plugins=[
"SoftRobots", "SoftRobots.Inverse", "SofaSparseSolver",
'SofaDeformable', "SofaPreconditioner", "SofaOpenglVisual",
"CosseratPlugin", "BeamAdapter", "SofaGeneralRigid",
"SofaImplicitOdeSolver"
],
repositoryPaths=[os.getcwd()])
rootNode.addObject(
'VisualStyle',
displayFlags='showVisualModels hideBehaviorModels showCollisionModels '
'hideBoundingCollisionModels hideForceFields showInteractionForceFields '
'showWireframe')
rootNode.addObject('FreeMotionAnimationLoop')
qp_solver = rootNode.createObject('QPInverseProblemSolver', printLog='0')
rootNode.addObject('CollisionPipeline', depth="6", verbose="0", draw="1")
rootNode.addObject('BruteForceDetection', name="N2")
rootNode.addObject('DefaultContactManager',
response="FrictionContact",
responseParams="mu=0.65")
rootNode.addObject('LocalMinDistance',
name="Proximity",
alarmDistance="0.6",
contactDistance="0.44",
angleCone="0.01")
rootNode.gravity = [0.0, 0.0, 0.0]
rootNode.dt = 0.01
rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight")
##########################################
# Effector goal for interactive control #
##########################################
goal = rootNode.addChild('goal')
goal.addObject('EulerImplicitSolver', firstOrder='1')
goal.addObject('CGLinearSolver',
iterations='100',
tolerance="1e-5",
threshold="1e-5")
goalPos = goal.addObject('MechanicalObject',
name='goalMO',
position='90.0 3.0 0.35857')
goal.addObject('SphereCollisionModel', radius='2', color="red")
goal.addObject('UncoupledConstraintCorrection')
# #######################################
# New adds to use the sliding Actuator #
#########################################
cableNode = rootNode.addChild('cableNode')
cableNode.addObject('EulerImplicitSolver',
firstOrder="0",
rayleighStiffness="1.0",
rayleighMass='0.1')
cableNode.addObject('SparseLUSolver', name='solver')
cableNode.addObject('GenericConstraintCorrection')
#################################
## RigidBase ##
#################################
rigidBaseNode = cableNode.addChild('rigidBase')
RigidBaseMO = rigidBaseNode.addObject('MechanicalObject',
template='Rigid3d',
name="RigidBaseMO",
position="0 0 0 0 0 0 1",
showObject='1',
showObjectScale='0.1')
rigidBaseNode.addObject('RestShapeSpringsForceField',
name='spring',
stiffness="5",
angularStiffness="5",
external_points="0",
mstate="@RigidBaseMO",
points="0",
template="Rigid3d")
#################################################################
## Sliding actuator to guide the base of the needle, only the ##
## translation are tacking into account here.
#################################################################
for j in range(0, 3):
direction = [0, 0, 0, 0, 0, 0]
direction[j] = 1
rigidBaseNode.addObject('SlidingActuator',
name="SlidingActuator" + str(j),
template='Rigid3d',
direction=direction,
indices=0,
maxForce='100000',
minForce='-30000')
#############################################
# Rate of angular Deformation (2 sections)
#############################################
# Define: the number of section, the total lenght and the lenght of each beam.
nbSectionS = 6
totalLength = 80.0
lengthS = totalLength / nbSectionS
# Define: the length of each beam in a list, the positions of eahc beam
# (flexion, torsion), the abs of each section
positionS = []
longeurS = []
temp = 0.
curv_abs_inputS = [0.0]
for i in range(nbSectionS):
positionS.append([0, 0, 0])
longeurS.append((((i + 1) * lengthS) - i * lengthS))
temp += longeurS[i]
curv_abs_inputS.append(temp)
curv_abs_inputS[nbSectionS] = totalLength
rateAngularDeformNode = cableNode.addChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.addObject(
'MechanicalObject',
template='Vec3d',
name='rateAngularDeformMO',
position=positionS)
BeamHookeLawForce = rateAngularDeformNode.addObject(
'BeamHookeLawForceField',
crossSectionShape='circular',
length=longeurS,
radius='0.5',
youngModulus='5e6')
for i in range(1, 6):
rateAngularDeformNode.addObject('SlidingActuator',
name="SlidingActuatory" + str(i),
template='Vec3d',
direction='0 1 0 ',
indices=i,
maxForce='100000',
minForce='-30000')
for i in range(1, 6):
rateAngularDeformNode.addObject('SlidingActuator',
name="SlidingActuatorz" + str(i),
template='Vec3d',
direction='0 0 1',
indices=i,
maxForce='100000',
minForce='-30000')
##############
# Frames #
##############
# Define: the number of frame and the length between each frame.
nbFramesF = 16
lengthF = totalLength / nbFramesF
# Define: the abs of each frame and the position of each frame.
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([totalLength, 0, 0, 0, 0, 0, 1])
cable_positionF.append([totalLength, 0, 0])
curv_abs_outputF.append(totalLength)
# the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform
mappedFrameNode = rigidBaseNode.addChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.addObject('MechanicalObject',
template='Rigid3d',
name="FramesMO",
position=framesF,
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.addObject('DiscreteCosseratMapping',
curv_abs_input=curv_abs_inputS,
output=outputMO,
curv_abs_output=curv_abs_outputF,
input1=inputMO,
input2=inputMO_rigid,
debug='0',
max=6.e-2,
deformationAxis=1,
nonColored="0",
radius=5)
# #Mapped mechanical points
# This create a new node in the scene. This node is appended to the finger's node.
slidingPoint = mappedFrameNode.addChild('slidingPoint')
slidingPoint.addObject('MechanicalObject',
name="cablePos",
position=cable_positionF,
showObject="1",
showIndices="0")
slidingPoint.addObject('IdentityMapping')
##########################################
# Effector #
##########################################
effector = mappedFrameNode.addChild('fingertip')
effMO = effector.addObject('MechanicalObject', position=[81., 0., 0.35857])
effector.addObject('PositionEffector',
template='Vec3d',
indices="0",
effectorGoal="@../../../../goal/goalMO.position")
effector.addObject('SkinningMapping',
nbRef='1',
mapForces='false',
mapMasses='false')
return rootNode
def createScene(node):
from stlib3.scene import MainHeader
MainHeader(node, plugins=["SoftRobots"])
node.addObject('MechanicalObject')
PneumaticCavity(surfaceMeshFileName="mesh/cube.obj",
attachedAsAChildOf=node)
def createScene(rootNode):
from stlib3.scene import MainHeader
MainHeader(rootNode)
f = Node(rootNode, "Finger")
StringSensor(f)
def createScene(rootNode):
from stlib3.scene import MainHeader
MainHeader(rootNode,
plugins=[
"SoftRobots", "SoftRobots.Inverse", "SofaSparseSolver",
'SofaDeformable', "SofaPreconditioner", "SofaOpenglVisual",
"CosseratPlugin", "BeamAdapter", "SofaGeneralRigid",
"SofaImplicitOdeSolver", 'SofaEngine', 'SofaMeshCollision',
'SofaSimpleFem', 'SofaTopologyMapping', 'SofaConstraint'
],
repositoryPaths=[os.getcwd()])
rootNode.addObject(
'VisualStyle',
displayFlags='showVisualModels hideBehaviorModels showCollisionModels '
'hideBoundingCollisionModels hideForceFields showInteractionForceFields '
'showWireframe')
rootNode.addObject('FreeMotionAnimationLoop')
rootNode.createObject('QPInverseProblemSolver',
printLog='0',
epsilon=0.0002)
rootNode.addObject('CollisionPipeline', depth="6", verbose="0", draw="1")
rootNode.addObject('BruteForceDetection', name="N2")
rootNode.addObject('DefaultContactManager',
response="FrictionContact",
responseParams="mu=0.65")
rootNode.addObject('LocalMinDistance',
name="Proximity",
alarmDistance="0.6",
contactDistance="0.44",
angleCone="0.01")
rootNode.gravity = [0.0, 0.0, 0.0]
rootNode.dt = 0.01
rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight")
# #######################################
# New adds to use the sliding Actuator #
#########################################
cableNode = rootNode.addChild('cableNode')
cableNode.addObject('EulerImplicitSolver',
firstOrder="0",
rayleighStiffness="1.0",
rayleighMass='0.1')
cableNode.addObject('SparseLUSolver', name='solver')
cableNode.addObject('GenericConstraintCorrection')
#################################
## RigidBase ##
#################################
rigidBaseNode = cableNode.addChild('rigidBase')
RigidBaseMO = rigidBaseNode.addObject('MechanicalObject',
template='Rigid3d',
name="RigidBaseMO",
position="0 0 0 0 0 0 1",
showObject='1',
showObjectScale='0.1')
rigidBaseNode.addObject('RestShapeSpringsForceField',
name='spring',
stiffness="5",
angularStiffness="5",
external_points="0",
mstate="@RigidBaseMO",
points="0",
template="Rigid3d")
#################################################################
## Sliding actuator to guide the base of the needle, only the ##
## translation are tacking into account here.
#################################################################
for j in range(0, 6):
direction = [0, 0, 0, 0, 0, 0]
direction[j] = 1
rigidBaseNode.addObject('SlidingActuator',
name="SlidingActuator" + str(j),
template='Rigid3d',
direction=direction,
indices=0,
maxForce='1e6',
minForce='-30000')
[positionS, curv_abs_inputS, longeurS, framesF, curv_abs_outputF, cable_positionF] = \
BuildCosseratGeometry(nbSection=8, nbFrames=16, totalLength=80)
#############################################
# Rate of angular Deformation (2 sections)
#############################################
rateAngularDeformNode = cableNode.addChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.addObject(
'MechanicalObject',
template='Vec3d',
name='rateAngularDeformMO',
position=positionS)
rateAngularDeformNode.addObject('BeamHookeLawForceField',
crossSectionShape='circular',
length=longeurS,
radius='0.5',
youngModulus='1.0e12')
# for i in range(0, nbSectionS):
# rateAngularDeformNode.addObject('SlidingActuator', name="SlidingActuatorY" + str(i), template='Vec3d',
# direction='0 1 0 ', indices=i, maxForce='100000', minForce='-30000')
# for i in range(0, nbSectionS):
# rateAngularDeformNode.addObject('SlidingActuator', name="SlidingActuatorZ" + str(i), template='Vec3d',
# direction='0 0 1', indices=i, maxForce='100000', minForce='-30000')
##############
# Frames #
##############
# the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform
mappedFrameNode = rigidBaseNode.addChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.addObject('MechanicalObject',
template='Rigid3d',
name="FramesMO",
position=framesF,
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.addObject('DiscreteCosseratMapping',
curv_abs_input=curv_abs_inputS,
output=outputMO,
curv_abs_output=curv_abs_outputF,
input1=inputMO,
input2=inputMO_rigid,
debug='0',
max=6.e-2,
deformationAxis=1,
nonColored="0",
radius=5)
# #Mapped mechanical points
# This create a new node in the scene. This node is appended to the finger's node.
framePoints = mappedFrameNode.addChild('framePoints')
framePointsMO = framePoints.addObject('MechanicalObject',
name="cablePos",
position=cable_positionF,
showObject="1",
showIndices="0")
framePoints.addObject('IdentityMapping')
##########################################
# ## create FEM grid
##########################################
cubeNode = createFemCube(rootNode)
####################################################################################
# Attached the target to FEM model
# targetNode = cubeNode.gelNode.addChild('target')
# targetNode.addObject('VisualStyle', displayFlags='showCollisionModels ')
# targetMO = targetNode.addObject('MechanicalObject', name="targetMO", position='85.0 1.5 0.35857', showObject="1",
# showIndices="1", template="Vec3d")
# targetNode.addObject('SphereCollisionModel', radius='2', color="red")
# targetNode.addObject('BarycentricMapping')
[targetNode, targetMO] = effectorTarget(rootNode)
####################################################################################
# Create constraint Points inside the volume of the deformable object
# These points are created
femPos = [" 41.0 0 0 45 0 0 50 0 0 55 0 0 "]
gelNode = cubeNode.getChild('gelNode')
femPoints = gelNode.addChild('femPoints')
inputFEMCable = femPoints.addObject('MechanicalObject',
name="pointsInFEM",
position=femPos,
showIndices="1")
femPoints.addObject('BarycentricMapping')
####################################################################################
# Effector constraint,
# first define the tip on then needle then attach the effector constraint which
# leads the tip of the needle to the defined target #
####################################################################################
effector = mappedFrameNode.addChild('fingertip')
effMO = effector.addObject('MechanicalObject', position=[80., 0., 0.35857])
posEffector = effector.addObject(
'PositionEffector',
template='Vec3d',
indices="0",
effectorGoal=targetMO.getLinkPath() +
'.position') # "@../../../../FemNode/gelNode/target/targetMO.position"
targetNode.addObject(MoveTargetProcess(posEffector))
effector.addObject('SkinningMapping',
nbRef='1',
mapForces='false',
mapMasses='false')
mappedPointsNode = framePoints.addChild('MappedPoints')
mappedPoints = mappedPointsNode.addObject('MechanicalObject',
template='Vec3d',
position=femPos,
name="FramesMO",
showObject='1',
showIndices='1',
showObjectScale='1')
mappedPointsNode.addObject('CosseratEquality',
name="QPConstraint",
eqDisp='0.0',
lastPointIsFixed="false")
## Get the tree mstate links for the mapping
inputCableMO = framePointsMO.getLinkPath()
inputFEMCableMO = inputFEMCable.getLinkPath()
outputPointMO = mappedPoints.getLinkPath()
femPoints.addChild(mappedPointsNode)
mappedPointsNode.addObject('DifferenceMultiMapping',
name="pointsMulti",
input1=inputFEMCableMO,
lastPointIsFixed=0,
input2=inputCableMO,
output=outputPointMO,
direction="@../../FramesMO.position")
def createScene(rootNode):
from stlib3.scene import MainHeader
# from stlib3.physics.collision import CollisionMesh
MainHeader(rootNode, plugins=["SoftRobots", "SofaSparseSolver", 'SofaDeformable', 'SofaEngine',
'SofaImplicitOdeSolver', 'SofaLoader', 'SofaSimpleFem', "SofaPreconditioner",
"SofaOpenglVisual", "CosseratPlugin", "BeamAdapter", "SofaConstraint"],
repositoryPaths=[os.getcwd()])
rootNode.addObject('VisualStyle', displayFlags='showVisualModels showInteractionForceFields showWireframe')
rootNode.addObject('FreeMotionAnimationLoop')
rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0")
# ContactHeader(rootNode, alarmDistance=2.5, contactDistance=2, frictionCoef=0.08)
gravity = [0, 0, 0]
rootNode.gravity.value = gravity
rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight")
##########################################
# FEM Model #
##########################################
finger = rootNode.addChild('finger')
finger.addObject('EulerImplicitSolver', name='odesolver', firstOrder='0', rayleighMass=0.1, rayleighStiffness=0.1)
finger.addObject('SparseLDLSolver', name='preconditioner')
# Add a componant to load a VTK tetrahedral mesh and expose the resulting topology in the scene .
finger.addObject('MeshVTKLoader', name='loader', filename=path + 'finger.vtk', translation="-17.5 -12.5 7.5",
rotation="0 180 0")
finger.addObject('TetrahedronSetTopologyContainer', src='@loader', name='container')
finger.addObject('TetrahedronSetTopologyModifier')
# Create a mechanicaobject component to stores the DoFs of the model
finger.addObject('MechanicalObject', name='tetras', template='Vec3d', showIndices='false', showIndicesScale='4e-5',
rx='0', dz='0')
# Gives a mass to the model
finger.addObject('UniformMass', totalMass='0.075')
# Add a TetrahedronFEMForceField component which implement an elastic material model
# solved using the Finite Element Method on
# tetrahedrons.
finger.addObject('TetrahedronFEMForceField', template='Vec3d',
name='FEM', method='large', poissonRatio='0.45', youngModulus='500')
finger.addObject('BoxROI', name='ROI1', box='-18 -15 -8 2 -3 8', drawBoxes='true')
finger.addObject('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"]
femPoints = finger.addChild('femPoints')
inputFEMCable = femPoints.addObject('MechanicalObject', name="pointsInFEM", position=FEMpos, showObject="1",
showIndices="1")
femPoints.addObject('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.addObject('LinearSolverConstraintCorrection')
##########################################
# Visualization #
##########################################
fingerVisu = finger.addChild('visu')
fingerVisu.addObject(
'MeshSTLLoader', filename=path+"finger.stl", name="loader", translation="-17.5 -12.5 7.5",
rotation="0 180 0")
fingerVisu.addObject('OglModel', src="@loader", template='ExtVec3f', color="0.0 0.7 0.7")
fingerVisu.addObject('BarycentricMapping')
# ###############
# New adds to use the sliding Actuator
###############
cableNode = rootNode.addChild('cableNode')
cableNode.addObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="0.1", rayleighMass='0.1')
cableNode.addObject('SparseLUSolver', name='solver')
cableNode.addObject('GenericConstraintCorrection')
# ###############
# RigidBase
###############
rigidBaseNode = cableNode.addChild('rigidBase')
RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO",
position="0 0 0 0 0 0 1", showObject='1', showObjectScale='5.')
rigidBaseNode.addObject('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.addChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', template='Vec3d',
name='rateAngularDeformMO', position=position,
showIndices="1")
BeamHookeLawForce = rateAngularDeformNode.addObject('BeamHookeLawForceField', crossSectionShape='circular',
length=longeur, radius='0.50', youngModulus='5e6')
################################
# Animation (to move the dofs) #
################################
animate = Animation(RigidBaseMO, rateAngularDeformMO)
rootNode.addObject(animate)
##############
# 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.addChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.addObject(
'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.addObject('DiscreteCosseratMapping', 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.addObject('DiscreteCosseratMapping', 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.addObject('actuators')
# actuator0 = actuators.addObject('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.addChild('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.addObject('MechanicalObject', name="cablePos",
position=cable_position, showObject="1", showIndices="0")
slidingPoint.addObject('IdentityMapping')
mappedPointsNode = slidingPoint.addChild('MappedPoints')
femPoints.addChild(mappedPointsNode)
mappedPoints = mappedPointsNode.addObject('MechanicalObject', template='Vec3d', position=FEMpos,
name="FramesMO", showObject='1', showObjectScale='1')
inputCableMO = slidingPointMO.getLinkPath()
inputFEMCableMO = inputFEMCable.getLinkPath()
outputPointMO = mappedPoints.getLinkPath()
mappedPointsNode.addObject('QPSlidingConstraint', name="QPConstraint")
mappedPointsNode.addObject('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):
MainHeader(rootNode, plugins=["SoftRobots", "SofaSparseSolver", "SofaPreconditioner",
"SofaOpenglVisual", "CosseratPlugin", "BeamAdapter", "SofaDeformable",
"SofaImplicitOdeSolver", 'SofaEngine', 'SofaMeshCollision', 'SofaSimpleFem',
'SofaConstraint', 'SofaTopologyMapping', 'NeedleConstraintPlugin'],
repositoryPaths=[os.getcwd()])
# rootNode.addObject('VisualStyle', displayFlags='showVisualModels showInteractionForceFields')
rootNode.addObject('VisualStyle', displayFlags='showBehaviorModels hideCollisionModels hideBoundingCollisionModels '
'showForceFields hideInteractionForceFields showWireframe')
rootNode.addObject('FreeMotionAnimationLoop')
rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0")
gravity = [0, 0, 0]
rootNode.gravity.value = gravity
rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765')
rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight")
# ###############
# New adds to use the sliding Actuator
###############
cableNode = rootNode.addChild('cableNode')
cableNode.addObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="0.1", rayleighMass='0.1')
cableNode.addObject('SparseLUSolver', name='solver')
cableNode.addObject('GenericConstraintCorrection')
# ###############
# RigidBase
###############
rigidBaseNode = cableNode.addChild('rigidBase')
RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO",
position="0 0 0 0 0 0 1", showObject='1', showObjectScale='5.')
rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="50000",
angularStiffness="50000", external_points="0", mstate="@RigidBaseMO", points="0",
template="Rigid3d")
###############
# Rate of angular Deformation (2 sections)
###############
[positionS, curv_abs_inputS, longeurS, framesF, curv_abs_outputF, cable_positionF] = \
BuildCosseratGeometry(nbSection=8, nbFrames=16, totalLength=80)
rateAngularDeformNode = cableNode.addChild('rateAngularDeform')
rateAngularDeformMO = rateAngularDeformNode.addObject(
'MechanicalObject', template='Vec3d', name='rateAngularDeformMO', position=positionS, showIndices="1")
rateAngularDeformNode.addObject(
'BeamHookeLawForceField', crossSectionShape='circular', length=longeurS, radius='2.0', youngModulus='1.e12')
################################
# Animation (to move the dofs) #
################################
rootNode.addObject(Animation(RigidBaseMO, rateAngularDeformMO))
##############
# Frames #
##############
# the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform
mappedFrameNode = rigidBaseNode.addChild('MappedFrames')
rateAngularDeformNode.addChild(mappedFrameNode)
framesMO = mappedFrameNode.addObject(
'MechanicalObject', template='Rigid3d', name="FramesMO", position=framesF, 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.addObject('DiscreteCosseratMapping', 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=1, nonColored="0", radius=5)
slidingPoint = mappedFrameNode.addChild('slidingPoint')
edgeList = "0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8 8 9 9 10 10 11 11 12 12 13 13 14 14 15 15 16"
slidingPoint.addObject('EdgeSetTopologyContainer', name="Container", position=cable_positionF, edges=edgeList)
slidingPointMO = slidingPoint.addObject('MechanicalObject', name="cablePos", position=cable_positionF,
showObject="1", showIndices="0")
slidingPoint.addObject('NeedleGeometry', name='Needle') #### NeedleConstriantPlugin
slidingPoint.addObject('IdentityMapping')
# Create FEM Node
femPos = [" 41.0 0 0 45 0 0 50 0 0 55 0 0 60 0 0 60 0 0 70 0 0 80 0 0 "]
cubeNode = createFemCube(rootNode)
gelNode = cubeNode.getChild('gelNode')
Trajectory = gelNode.addChild('Trajectory')
Trajectory.addObject('EdgeSetTopologyContainer', name="Container")
Trajectory.addObject('EdgeSetTopologyModifier', name='Modifier')
inputFEMCable = Trajectory.addObject('MechanicalObject', name="pointsInFEM", position=femPos, showIndices="1")
Trajectory.addObject('NeedleTrajectoryGeometry', name="trajectory", entryDist="1", constraintDist="3",
clearTrajectory="true")
Trajectory.addObject('BarycentricMapping')
mappedPointsNode = slidingPoint.addChild('MappedPoints')
Trajectory.addChild(mappedPointsNode)
mappedPoints = mappedPointsNode.addObject('MechanicalObject', template='Vec3d', position=femPos, name="FramesMO")
inputCableMO = slidingPointMO.getLinkPath()
inputFEMCableMO = inputFEMCable.getLinkPath()
outputPointMO = mappedPoints.getLinkPath()
mappedPointsNode.addObject('CosseratNeedleSlidingConstraint', name="QPConstraint")
mappedPointsNode.addObject('DifferenceMultiMapping', name="pointsMulti", input1=inputFEMCableMO, lastPointIsFixed=0,
input2=inputCableMO, output=outputPointMO, direction="@../../FramesMO.position")
return rootNode
