Skip to content

Commit 6dace03

Browse files
th-skamth-skam
authored
[scene] Fix force feedback for haptics and insertion - addition of 2 scenes (#58)
* [scene] Fixed the lack of force feedback when using the needle with haptics * [scene] Added a simpler scene with haptics - insertion in a box
1 parent 94abacd commit 6dace03

File tree

2 files changed

+252
-25
lines changed

2 files changed

+252
-25
lines changed

‎scenes/InsertionHaptics_Box.py‎

Lines changed: 211 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,211 @@
1+
import Sofa
2+
3+
g_needleLength=0.100 #(m)
4+
g_needleNumberOfElems=20 #(# of edges)
5+
g_needleBaseOffset=[0.15,0.04,0.04]
6+
g_needleRadius = 0.001 #(m)
7+
g_needleMechanicalParameters = {
8+
"radius":g_needleRadius,
9+
"youngModulus":5e11,
10+
"poissonRatio":0.45
11+
}
12+
g_needleTotalMass=0.01
13+
14+
g_gelRegularGridParameters = {
15+
"n":[8, 8, 8],
16+
"min":[-0.125, -0.125, -0.350],
17+
"max":[0.125, 0.125, -0.100]
18+
} #Again all in mm
19+
g_gelMechanicalParameters = {
20+
"youngModulus":9e5,
21+
"poissonRatio":0.3,
22+
"method":"large"
23+
}
24+
g_gelTotalMass = 1
25+
g_cubeColor=[0.8, 0.34, 0.34, 0.3]
26+
g_gelFixedBoxROI=[-0.130, -0.130, -0.360, 0.130, 0.130, -0.300 ]
27+
28+
# Function called when the scene graph is being created
29+
def createScene(root):
30+
root.gravity=[0,0,-9.81]
31+
root.dt = 0.01
32+
33+
root.addObject("RequiredPlugin",pluginName=['Sofa.Component.AnimationLoop',
34+
'Sofa.Component.Constraint.Lagrangian.Solver',
35+
'Sofa.Component.ODESolver.Backward',
36+
'Sofa.Component.Visual',
37+
'Sofa.Component.Constraint.Lagrangian.Correction',
38+
'Sofa.Component.Constraint.Lagrangian.Model',
39+
'Sofa.Component.LinearSolver.Direct',
40+
'Sofa.Component.Mapping.Linear',
41+
'Sofa.Component.Mass',
42+
'Sofa.Component.SolidMechanics.FEM.Elastic',
43+
'Sofa.Component.StateContainer',
44+
'Sofa.Component.Topology.Container.Dynamic',
45+
'Sofa.Component.Topology.Mapping',
46+
'Sofa.Component.Mapping.NonLinear',
47+
'Sofa.Component.Topology.Container.Grid',
48+
'Sofa.Component.Constraint.Projective',
49+
'Sofa.Component.SolidMechanics.Spring',
50+
'Sofa.GL.Component.Rendering3D',
51+
'Sofa.GUI.Component',
52+
'Sofa.Component.Engine.Select',
53+
'MultiThreading',
54+
'CollisionAlgorithm',
55+
'ConstraintGeometry',
56+
'Geomagic',
57+
'Sofa.Component.Haptics',
58+
])
59+
60+
61+
root.addObject("ConstraintAttachButtonSetting")
62+
root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" )
63+
root.addObject("FreeMotionAnimationLoop")
64+
root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000)
65+
root.addObject("CollisionLoop")
66+
67+
toolController = root.addChild("ToolController")
68+
toolController.addObject("GeomagicDriver"
69+
, name='GeomagicDevice'
70+
, deviceName='Default Device'
71+
, scale=0.02
72+
, drawDeviceFrame=False
73+
, drawDevice=False
74+
, manualStart=False
75+
, positionBase=[0.12, 0, 0]
76+
, orientationBase=[0, 0.174, 0, -0.985]
77+
)
78+
toolController.addObject("MechanicalObject", name="mstate_baseMaster"
79+
, position="@GeomagicDevice.positionDevice"
80+
, template="Rigid3d"
81+
, showObjectScale=0.01
82+
, showObject=False
83+
, drawMode=1
84+
)
85+
86+
needle = root.addChild("Needle")
87+
needle.addObject("EulerImplicitSolver", firstOrder=True)
88+
needle.addObject("EigenSparseLU", name="LinearSolver", template="CompressedRowSparseMatrixd")
89+
needle.addObject("EdgeSetTopologyContainer", name="Container"
90+
, position=[[g_needleBaseOffset[0], g_needleBaseOffset[1], -(i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[2])] for i in range(g_needleNumberOfElems + 1)]
91+
, edges=[[i, i+1] for i in range(g_needleNumberOfElems)]
92+
)
93+
needle.addObject("EdgeSetTopologyModifier", name="modifier")
94+
needle.addObject("PointSetTopologyModifier", name="modifier2")
95+
needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.0002, showObject=False, drawMode=1)
96+
needle.addObject("UniformMass", totalMass=g_needleTotalMass)
97+
needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters)
98+
needle.addObject("LinearSolverConstraintCorrection", printLog=False, linearSolver="@LinearSolver")
99+
needle.addObject("RestShapeSpringsForceField",points=[0], stiffness=1e8, angularStiffness=1e8, external_points=[0], external_rest_shape="@/ToolController/mstate_baseMaster")
100+
101+
needleBase = needle.addChild("needleBase")
102+
needleBase.addObject("PointSetTopologyContainer", name="Container_base", position=[0, 0, 0])
103+
needleBase.addObject("MechanicalObject",name="mstate_base", template="Rigid3d",)
104+
needleBase.addObject("SubsetMapping", indices="0")
105+
106+
needleBodyCollision = needle.addChild("bodyCollision")
107+
needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container_body", src="@../Container")
108+
needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d",)
109+
needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body")
110+
needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body")
111+
needleBodyCollision.addObject("IdentityMapping")
112+
113+
needleTipCollision = needle.addChild("tipCollision")
114+
needleTipCollision.addObject("PointSetTopologyContainer", name="Container_tip"
115+
, position=[g_needleBaseOffset[0], g_needleBaseOffset[1], -(g_needleLength+g_needleBaseOffset[2])])
116+
needleTipCollision.addObject("MechanicalObject",name="mstate_tip",template="Vec3d", showObject=False, showObjectScale=20)
117+
needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip")
118+
needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems)
119+
120+
needleVisual = needle.addChild("visual")
121+
needleVisual.addObject("QuadSetTopologyContainer", name="Container_visu")
122+
needleVisual.addObject("QuadSetTopologyModifier", name="Modifier")
123+
needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@Container_visu")
124+
needleVisual.addObject("MechanicalObject", name="mstate_visu", showObjectScale=0.0002, showObject=True, drawMode=1)
125+
needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@mstate_visu")
126+
127+
needleOGL = needleVisual.addChild("OGL")
128+
needleOGL.addObject("OglModel", position="@../Container_visu.position",
129+
vertices="@../Container_visu.position",
130+
quads="@../Container_visu.quads",
131+
color=[0.4, 0.34, 0.34],
132+
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",
133+
name="visualOgl")
134+
needleOGL.addObject("IdentityMapping")
135+
136+
FF = root.addChild("ForceFeedback")
137+
FF.addObject("MechanicalObject", name="mstate_lcp", template="Rigid3d"
138+
, showObject=False, src="@../Needle/needleBase/mstate_base")
139+
FF.addObject("LCPForceFeedback", name="lcp_ff", activate=1, forceCoef=0.25)
140+
FFBody = FF.addChild("Body")
141+
FFBody.addObject("EdgeSetTopologyContainer", name="Container", src="@../../Needle/bodyCollision/Container_body")
142+
FFBody.addObject("MechanicalObject", name="mstate_coli", constraint="@../../Needle/bodyCollision/mstate_body.constraint")
143+
FFBody.addObject("RigidMapping")
144+
FFTip = FF.addChild("Tip")
145+
FFTip.addObject("PointSetTopologyContainer", name="Container", src="@../../Needle/tipCollision/Container_tip")
146+
FFTip.addObject("MechanicalObject", name="mstate_coli", constraint="@../../Needle/tipCollision/mstate_tip.constraint")
147+
FFTip.addObject("RigidMapping")
148+
149+
gelTopo = root.addChild("GelGridTopo")
150+
gelTopo.addObject("RegularGridTopology", name="HexaTop", **g_gelRegularGridParameters)
151+
152+
volume = root.addChild("Volume")
153+
volume.addObject("EulerImplicitSolver")
154+
volume.addObject("EigenSimplicialLDLT", name="LinearSolver", template='CompressedRowSparseMatrixMat3x3d')
155+
volume.addObject("TetrahedronSetTopologyContainer", name="TetraContainer", position="@../GelGridTopo/HexaTop.position")
156+
volume.addObject("TetrahedronSetTopologyModifier", name="TetraModifier")
157+
volume.addObject("Hexa2TetraTopologicalMapping", input="@../GelGridTopo/HexaTop", output="@TetraContainer", swapping=False)
158+
159+
volume.addObject("MechanicalObject", name="mstate_gel", template="Vec3d")
160+
volume.addObject("TetrahedronGeometry", name="geom_tetra", mstate="@mstate_gel", topology="@TetraContainer", draw=False)
161+
volume.addObject("AABBBroadPhase",name="AABBTetra",geometry="@geom_tetra",nbox=[3,3,3],thread=1)
162+
volume.addObject("TetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters)
163+
volume.addObject("MeshMatrixMass", name="Mass",totalMass=g_gelTotalMass)
164+
165+
volume.addObject("BoxROI",name="BoxROI",box=g_gelFixedBoxROI)
166+
volume.addObject("RestShapeSpringsForceField", stiffness=1e6,points="@BoxROI.indices" )
167+
168+
volume.addObject("LinearSolverConstraintCorrection", printLog=False, linearSolver="@LinearSolver")
169+
170+
volumeCollision = volume.addChild("collision")
171+
volumeCollision.addObject("TriangleSetTopologyContainer", name="TriContainer")
172+
volumeCollision.addObject("TriangleSetTopologyModifier", name="TriModifier")
173+
volumeCollision.addObject("Tetra2TriangleTopologicalMapping", name="mapping", input="@../TetraContainer", output="@TriContainer", flipNormals=False)
174+
volumeCollision.addObject("MechanicalObject", name="mstate_gelColi",position="@../TetraContainer.position")
175+
volumeCollision.addObject("TriangleGeometry", name="geom_tri", mstate="@mstate_gelColi", topology="@TriContainer",draw=False)
176+
volumeCollision.addObject("PhongTriangleNormalHandler", name="SurfaceTriangles", geometry="@geom_tri")
177+
volumeCollision.addObject("AABBBroadPhase",name="AABBTriangles",thread=1,nbox=[2,2,3])
178+
179+
volumeCollision.addObject("IdentityMapping", name="identityMappingToCollision", input="@../mstate_gel", output="@mstate_gelColi", isMechanical=True)
180+
181+
volumeVisu = volumeCollision.addChild("visu")
182+
volumeVisu.addObject("OglModel", position="@../TriContainer.position",
183+
vertices="@../TriContainer.position",
184+
triangles="@../TriContainer.triangles",
185+
color=g_cubeColor,name="volume_visu",template="Vec3d")
186+
volumeVisu.addObject("IdentityMapping")
187+
188+
volumeVisuWire = volume.addChild("visu_wire")
189+
volumeVisuWire.addObject("OglModel", position="@../TetraContainer.position",
190+
vertices="@../TetraContainer.position",
191+
triangles="@../TetraContainer.triangles",
192+
color=[1, 0, 1, 1],name="volume_visu",template="Vec3d")
193+
volumeVisuWire.addObject("IdentityMapping")
194+
195+
196+
root.addObject("InsertionAlgorithm", name="InsertionAlgo",
197+
tipGeom="@Needle/tipCollision/geom_tip",
198+
surfGeom="@Volume/collision/geom_tri",
199+
shaftGeom="@Needle/bodyCollision/geom_body",
200+
volGeom="@Volume/geom_tetra",
201+
punctureForceThreshold=5.,
202+
tipDistThreshold=0.003,
203+
drawcollision=True,
204+
drawPointsScale=0.0001
205+
)
206+
root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01)
207+
root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles")
208+
root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001, mu=0.003)
209+
210+
root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams")
211+
root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.001)

0 commit comments

Comments
 (0)