Skip to content

Commit 81aed14

Browse files
th-skamth-skam
committed
[scene] Added regularizationTerms and changed problem parameters
1 parent 59534fb commit 81aed14

File tree

1 file changed

+89
-88
lines changed

1 file changed

+89
-88
lines changed

‎scenes/NeedleInsertionLayers.py‎

Lines changed: 89 additions & 88 deletions
Original file line numberDiff line numberDiff line change
@@ -1,37 +1,37 @@
11
import Sofa
22

3-
g_needleLength=0.100 #(m)
4-
g_needleNumberOfElems=20 #(# of edges)
5-
g_needleBaseOffset=[0.04,0.04,0]
3+
g_needleLength=0.400 #(m)
4+
g_needleNumberOfElems=40 #(# of edges)
5+
g_needleBaseOffset=[0.04,0.25,-0.2]
66
g_needleRadius = 0.001 #(m)
77
g_needleMechanicalParameters = {
88
"radius":g_needleRadius,
9-
"youngModulus":1e11,
9+
"youngModulus":4e12,
1010
"poissonRatio":0.3
1111
}
12-
g_needleTotalMass=0.01
12+
g_needleTotalMass=0.04
1313

1414
g_gelRegularGridParameters = [
1515
{
1616
"n":[6, 4, 4],
17-
"min":[-0.350, -0.050, -0.350],
18-
"max":[0.350, 0.0499, -0.100]
17+
"min":[-0.150, -0.050, -0.250],
18+
"max":[0.150, 0.0499, -0.100]
1919
},
2020
{
2121
"n":[6, 4, 4],
22-
"min":[-0.350, 0.0501, -0.350],
23-
"max":[0.350, 0.150, -0.100]
22+
"min":[-0.150, 0.0501, -0.250],
23+
"max":[0.150, 0.150, -0.100]
2424
}
2525
] #Again all in mm
2626
g_gelMechanicalParameters = {
27-
"youngModulus":8e7,
27+
"youngModulus":8e5,
2828
"poissonRatio":0.45,
2929
"method":"large"
3030
}
3131
g_gelTotalMass = 1
3232
g_cubeColor=[[0.8, 0.34, 0.34, 0.3],[0.6, 0.6, 0, 0.3]]
3333
g_wireColor=[[0.8, 0.34, 0.34, 1],[0.6, 0.6, 0, 1]]
34-
g_gelFixedBoxROI=[[-0.355, -0.055, -0.355, -0.345, 0.155, -0.095 ], [0.355, -0.055, -0.355, 0.345, 0.155, -0.095 ]]
34+
g_gelFixedBoxROI=[[-0.155, -0.055, -0.255, -0.145, 0.155, -0.095 ], [0.155, -0.055, -0.255, 0.145, 0.155, -0.095 ]]
3535

3636
# Function called when the scene graph is being created
3737
def createScene(root):
@@ -65,9 +65,9 @@ def createScene(root):
6565

6666

6767
root.addObject("ConstraintAttachButtonSetting")
68-
root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels hideCollisionModels hideMappings hideForceFields hideWireframe hideInteractionForceFields")
68+
root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields")
6969
root.addObject("FreeMotionAnimationLoop")
70-
root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000)
70+
root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000, regularizationTerm=0.001)
7171
root.addObject("CollisionLoop")
7272

7373
root.addObject("CollisionPipeline", name="pipeline", depth=6, verbose=0)
@@ -76,75 +76,76 @@ def createScene(root):
7676
root.addObject("CollisionResponse", response="FrictionContact")
7777
root.addObject("LocalMinDistance", name="proximity", alarmDistance=0.002, contactDistance=0.0005)
7878

79-
#needleBaseMaster = root.addChild("NeedleBaseMaster")
80-
#needleBaseMaster.addObject("MechanicalObject", name="mstate_baseMaster", position=[0.04, 0.04, 0, 0, 0, 0, 1], template="Rigid3d", showObjectScale=0.002, showObject=False, drawMode=1)
81-
#needleBaseMaster.addObject("LinearMovementProjectiveConstraint",indices=[0], keyTimes=[0,1,7,9],movements=
82-
# [ [0.04, 0.04,0,0,0,0]
83-
# , [0.04, 0.04,0.05,0,3.14/2,0]
84-
# , [0.04, 0.04,-0.07,0,3.14/2,0]
85-
# , [0.05, 0.04,-0.07,0,3.14/2 + 3.14/16,0]
86-
#],relativeMovements=False)
79+
needleBaseMaster = root.addChild("NeedleBaseMaster")
80+
needleBaseMaster.addObject("MechanicalObject", name="mstate_baseMaster", position=[0.04,0.25,-0.2, 0, 0, 0, 1], template="Rigid3d", showObjectScale=0.002, showObject=False, drawMode=1)
81+
needleBaseMaster.addObject("LinearMovementProjectiveConstraint",indices=[0], keyTimes=[0,0.5,1,7,12],movements=
82+
[ [0.04, 0.25,-0.2,0,0,0]
83+
, [0.04, 0.60,-0.2,0,0,0]
84+
, [0.04, 0.60,-0.2,0,0,-3.14/2]
85+
, [0.04, 0.40,-0.2,0,0,-3.14/2]
86+
, [0.05, 0.40,-0.2,0,0,-3.14/2 + 3.14/16]
87+
],relativeMovements=False)
8788

8889

8990

90-
#needle = root.addChild("Needle")
91-
#needle.addObject("EulerImplicitSolver", firstOrder=True)
92-
#needle.addObject("EigenSparseLU", name="LinearSolver", template="CompressedRowSparseMatrixd")
93-
#needle.addObject("EdgeSetTopologyContainer", name="Container", position=[[i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]] for i in range(g_needleNumberOfElems + 1)]
94-
# , edges=[[i, i+1] for i in range(g_needleNumberOfElems)])
91+
needle = root.addChild("Needle")
92+
needle.addObject("EulerImplicitSolver", firstOrder=True)
93+
needle.addObject("EigenSparseLU", name="LinearSolver", template="CompressedRowSparseMatrixd")
94+
needle.addObject("EdgeSetTopologyContainer", name="Container", position=[[i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]] for i in range(g_needleNumberOfElems + 1)]
95+
, edges=[[i, i+1] for i in range(g_needleNumberOfElems)])
9596

96-
#needle.addObject("EdgeSetTopologyModifier", name="modifier")
97-
#needle.addObject("PointSetTopologyModifier", name="modifier2")
97+
needle.addObject("EdgeSetTopologyModifier", name="modifier")
98+
needle.addObject("PointSetTopologyModifier", name="modifier2")
9899

99-
#needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.0002, showObject=False, drawMode=1)
100+
needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.0002, showObject=False, drawMode=1)
100101

101-
#needle.addObject("UniformMass", totalMass=g_needleTotalMass)
102-
#needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters)
103-
#needle.addObject("LinearSolverConstraintCorrection", printLog=False, linearSolver="@LinearSolver")
102+
needle.addObject("UniformMass", totalMass=g_needleTotalMass)
103+
needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters)
104+
needle.addObject("LinearSolverConstraintCorrection", printLog=False, linearSolver="@LinearSolver")
104105

105-
#needleBase = needle.addChild("needleBase")
106-
#needleBase.addObject("PointSetTopologyContainer", name="Container_base", position=[0, 0, 0])
107-
#needleBase.addObject("MechanicalObject",name="mstate_base", template="Rigid3d",)
108-
#needleBase.addObject("RestShapeSpringsForceField",points=[0],stiffness=1e8, angularStiffness=1e8,external_points=[0],external_rest_shape="@/NeedleBaseMaster/mstate_baseMaster")
106+
needleBase = needle.addChild("needleBase")
107+
needleBase.addObject("PointSetTopologyContainer", name="Container_base", position=[0, 0, 0])
108+
needleBase.addObject("MechanicalObject",name="mstate_base", template="Rigid3d",)
109+
needleBase.addObject("RestShapeSpringsForceField",points=[0],stiffness=1e8, angularStiffness=1e8,external_points=[0],external_rest_shape="@/NeedleBaseMaster/mstate_baseMaster")
109110

110-
#needleBase.addObject("SubsetMapping", indices="0")
111+
needleBase.addObject("SubsetMapping", indices="0")
111112

112-
#needleBodyCollision = needle.addChild("bodyCollision")
113-
#needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container_body", src="@../Container")
114-
#needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d",)
115-
#needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body")
116-
#needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body")
113+
needleBodyCollision = needle.addChild("bodyCollision")
114+
needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container_body", src="@../Container")
115+
needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d",)
116+
needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body")
117+
needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body")
117118

118-
#needleBodyCollision.addObject("IdentityMapping")
119+
needleBodyCollision.addObject("IdentityMapping")
119120

120121

121-
#needleTipCollision = needle.addChild("tipCollision")
122-
#needleTipCollision.addObject("MechanicalObject",name="mstate_tip",position=[g_needleLength+g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]],template="Vec3d",)
123-
#needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip")
124-
#needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems)
122+
needleTipCollision = needle.addChild("tipCollision")
123+
needleTipCollision.addObject("MechanicalObject",name="mstate_tip",position=[g_needleLength+g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]],template="Vec3d",)
124+
needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip")
125+
needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems)
125126

126127

127-
#needleVisual = needle.addChild("visual")
128-
#needleVisual.addObject("QuadSetTopologyContainer", name="Container_visu")
129-
#needleVisual.addObject("QuadSetTopologyModifier", name="Modifier")
130-
#needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@Container_visu")
128+
needleVisual = needle.addChild("visual")
129+
needleVisual.addObject("QuadSetTopologyContainer", name="Container_visu")
130+
needleVisual.addObject("QuadSetTopologyModifier", name="Modifier")
131+
needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@Container_visu")
131132

132-
#needleVisual.addObject("MechanicalObject", name="mstate_visu", showObjectScale=0.0002, showObject=True, drawMode=1)
133+
needleVisual.addObject("MechanicalObject", name="mstate_visu", showObjectScale=0.0002, showObject=True, drawMode=1)
133134

134-
#needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@mstate_visu")
135+
needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@mstate_visu")
135136

136-
#needleOGL = needleVisual.addChild("OGL")
137-
#needleOGL.addObject("OglModel", position="@../Container_visu.position",
138-
# vertices="@../Container_visu.position",
139-
# quads="@../Container_visu.quads",
140-
# color=[0.4, 0.34, 0.34],
141-
# material="texture Ambient 1 0.4 0.34 0.34 1.0 Diffuse 0 0.4 0.34 0.34 1.0 Specular 1 0.4 0.34 0.34 0.1 Emissive 1 0.5 0.54 0.54 .01 Shininess 1 20",
142-
# name="visualOgl")
143-
#needleOGL.addObject("IdentityMapping")
137+
needleOGL = needleVisual.addChild("OGL")
138+
needleOGL.addObject("OglModel", position="@../Container_visu.position",
139+
vertices="@../Container_visu.position",
140+
quads="@../Container_visu.quads",
141+
color=[0.4, 0.34, 0.34],
142+
material="texture Ambient 1 0.4 0.34 0.34 1.0 Diffuse 0 0.4 0.34 0.34 1.0 Specular 1 0.4 0.34 0.34 0.1 Emissive 1 0.5 0.54 0.54 .01 Shininess 1 20",
143+
name="visualOgl")
144+
needleOGL.addObject("IdentityMapping")
144145

145146

146147

147-
for i in range(2):
148+
for i in range(1,2):
148149
gelGridTopoName = "GelGridTopo" + str(i)
149150
gelTopo = root.addChild(gelGridTopoName)
150151
gelTopo.addObject("RegularGridTopology", name="HexaTop", **g_gelRegularGridParameters[i])
@@ -177,9 +178,9 @@ def createScene(root):
177178
volumeCollision.addObject("PhongTriangleNormalHandler", name="SurfaceTriangles", geometry="@geom_tri")
178179
volumeCollision.addObject("AABBBroadPhase",name="AABBTriangles",thread=1,nbox=[2,2,3])
179180

180-
volumeCollision.addObject("TriangleCollisionModel", name="colli_tri", group=i)
181-
volumeCollision.addObject("LineCollisionModel", name="colli_line", group=i)
182-
volumeCollision.addObject("PointCollisionModel", name="colli_point", group=i)
181+
#volumeCollision.addObject("TriangleCollisionModel", name="colli_tri", group=i)
182+
#volumeCollision.addObject("LineCollisionModel", name="colli_line", group=i)
183+
#volumeCollision.addObject("PointCollisionModel", name="colli_point", group=i)
183184

184185
volumeCollision.addObject("IdentityMapping", name="identityMappingToCollision", input="@../mstate_gel", output="@mstate_gelColi", isMechanical=True)
185186

@@ -198,26 +199,26 @@ def createScene(root):
198199
color=g_wireColor[i],name="volume_visu",template="Vec3d")
199200
volumeVisuWire.addObject("IdentityMapping")
200201

201-
root.addObject("NearestPointROI", template="Vec3d", name="attachROI", radius=0.0025,
202-
object1="@Layer0/mstate_gel", object2="@Layer1/mstate_gel")
203-
root.addObject("BilateralLagrangianConstraint", name="layerAttachment",
204-
first_point="@attachROI.indices1", second_point="@attachROI.indices2",
205-
object1="@Layer0/mstate_gel", object2="@Layer1/mstate_gel")
206-
207-
208-
#root.addObject("InsertionAlgorithm", name="InsertionAlgo",
209-
# tipGeom="@Needle/tipCollision/geom_tip",
210-
# surfGeom="@Volume/collision/geom_tri",
211-
# shaftGeom="@Needle/bodyCollision/geom_body",
212-
# volGeom="@Volume/geom_tetra",
213-
# punctureForceThreshold=2.,
214-
# tipDistThreshold=0.003,
215-
# drawcollision=True,
216-
# drawPointsScale=0.0001
217-
#)
218-
#root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01)
219-
#root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles")
220-
#root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001)
221-
222-
#root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams")
223-
#root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.05)
202+
#root.addObject("NearestPointROI", template="Vec3d", name="attachROI", radius=0.0025,
203+
# object1="@Layer0/mstate_gel", object2="@Layer1/mstate_gel")
204+
#root.addObject("BilateralLagrangianConstraint", name="layerAttachment",
205+
# first_point="@attachROI.indices1", second_point="@attachROI.indices2",
206+
# object1="@Layer0/mstate_gel", object2="@Layer1/mstate_gel")
207+
208+
209+
root.addObject("InsertionAlgorithm", name="InsertionAlgo",
210+
tipGeom="@Needle/tipCollision/geom_tip",
211+
surfGeom="@Layer1/collision/geom_tri",
212+
shaftGeom="@Needle/bodyCollision/geom_body",
213+
volGeom="@Layer1/geom_tetra",
214+
punctureForceThreshold=0.8,
215+
tipDistThreshold=0.005,
216+
drawcollision=True,
217+
drawPointsScale=0.0001
218+
)
219+
root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01)
220+
root.addObject("SecondDirection",name="punctureDirection",handler="@Layer1/collision/SurfaceTriangles")
221+
root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001)
222+
223+
root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams")
224+
root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.00)

0 commit comments

Comments
 (0)