Skip to content

Commit 00c14b7

Browse files
th-skamth-skam
authored
[Needle][algorithm] Enabled insertion-removal cycles in the algorithm (#41)
* [scene] Added scene to demonstrate an insertion cycle * [algorithm] Enabled the removal of proximities in the algorithm when exiting
1 parent 1358221 commit 00c14b7

2 files changed

Lines changed: 231 additions & 2 deletions

File tree

‎scenes/NeedleInsertionCycles.py‎

Lines changed: 216 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,216 @@
1+
import Sofa
2+
3+
g_needleLength=0.100 #(m)
4+
g_needleNumberOfElems=20 #(# of edges)
5+
g_needleBaseOffset=[0.04,0.04,0]
6+
g_needleRadius = 0.001 #(m)
7+
g_needleMechanicalParameters = {
8+
"radius":g_needleRadius,
9+
"youngModulus":1e11,
10+
"poissonRatio":0.3
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":8e5,
21+
"poissonRatio":0.45,
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+
])
57+
58+
59+
root.addObject("ConstraintAttachButtonSetting")
60+
root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" )
61+
root.addObject("FreeMotionAnimationLoop")
62+
root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000, printLog=False, computeConstraintForces=True)
63+
root.addObject("CollisionLoop")
64+
65+
needleBaseMaster = root.addChild("NeedleBaseMaster")
66+
needleBaseMaster.addObject("MechanicalObject", name="mstate_baseMaster", position=[0.04, 0.04, 0, 0, 0, 0, 1], template="Rigid3d", showObjectScale=0.002, showObject="true", drawMode=1)
67+
needleBaseMaster.addObject("LinearMovementProjectiveConstraint",indices=[0],
68+
keyTimes=[
69+
0, 1, 4, 4.5, 5, 8
70+
,8.5,9,12,12.5,13,16
71+
],
72+
movements=[
73+
[0.04, 0.04,0,0,0,0],
74+
[0.04, 0.04,0.05,0,3.14/2,0],
75+
[0.04, 0.04,-0.07,0,3.14/2,0],
76+
[0.05, 0.04,-0.07,0,3.14/2 + 3.14/16,0],
77+
[0.04, 0.04,-0.07,0,3.14/2,0],
78+
[0.04, 0.04,0.005,0,3.14/2,0],
79+
# Change to insertion at an angle
80+
[0.06, 0.04,0.005,0,3.14/2,0],
81+
[0.06, 0.04,0.005,0,3.14/2 + 3.14/8,0],
82+
[0.030866, 0.04,-0.04119,0,3.14/2 + 3.14/8,0],
83+
[0.030866, 0.04,-0.04119,0,3.14/2,0],
84+
[0.030866, 0.04,-0.04119,0,3.14/2 + 3.14/8,0],
85+
[0.06, 0.04,0.005,0,3.14/2 + 3.14/8,0]
86+
]
87+
,relativeMovements=False
88+
)
89+
90+
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)])
96+
97+
needle.addObject("EdgeSetTopologyModifier", name="modifier")
98+
needle.addObject("PointSetTopologyModifier", name="modifier2")
99+
100+
needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.0002, showObject="true", drawMode=1)
101+
102+
needle.addObject("UniformMass", totalMass=g_needleTotalMass)
103+
needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters)
104+
# needle.addObject("FixedLagrangianConstraint", indices="0" )
105+
needle.addObject("LinearSolverConstraintCorrection", printLog="false", linearSolver="@LinearSolver")
106+
107+
needleBase = needle.addChild("needleBase")
108+
needleBase.addObject("PointSetTopologyContainer", name="Container_base", position=[0, 0, 0])
109+
needleBase.addObject("MechanicalObject",name="mstate_base", template="Rigid3d",)
110+
needleBase.addObject("RestShapeSpringsForceField",points=[0],stiffness=1e8, angularStiffness=1e8,external_points=[0],external_rest_shape="@/NeedleBaseMaster/mstate_baseMaster")
111+
112+
needleBase.addObject("SubsetMapping", indices="0")
113+
114+
needleBodyCollision = needle.addChild("bodyCollision")
115+
needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container_body", src="@../Container")
116+
needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d",)
117+
needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body")
118+
needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body")
119+
120+
needleBodyCollision.addObject("IdentityMapping")
121+
122+
123+
needleTipCollision = needle.addChild("tipCollision")
124+
needleTipCollision.addObject("MechanicalObject",name="mstate_tip",position=[g_needleLength+g_needleBaseOffset[0], g_needleBaseOffset[1], g_needleBaseOffset[2]],template="Vec3d",)
125+
needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip")
126+
needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems)
127+
128+
129+
needleVisual = needle.addChild("visual")
130+
needleVisual.addObject("QuadSetTopologyContainer", name="Container_visu")
131+
needleVisual.addObject("QuadSetTopologyModifier", name="Modifier")
132+
needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@Container_visu")
133+
134+
needleVisual.addObject("MechanicalObject", name="mstate_visu", showObjectScale="0.0002", showObject="true", drawMode="1")
135+
136+
needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@mstate_visu")
137+
138+
needleOGL = needleVisual.addChild("OGL")
139+
needleOGL.addObject("OglModel", position="@../Container_visu.position",
140+
vertices="@../Container_visu.position",
141+
quads="@../Container_visu.quads",
142+
color="0.4 0.34 0.34",
143+
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",
144+
name="visualOgl")
145+
needleOGL.addObject("IdentityMapping")
146+
147+
148+
149+
gelTopo = root.addChild("GelGridTopo")
150+
gelTopo.addObject("RegularGridTopology", name="HexaTop", **g_gelRegularGridParameters)
151+
152+
153+
volume = root.addChild("Volume")
154+
volume.addObject("EulerImplicitSolver")
155+
volume.addObject("EigenSimplicialLDLT", name="LinearSolver", template='CompressedRowSparseMatrixMat3x3d')
156+
volume.addObject("TetrahedronSetTopologyContainer", name="TetraContainer", position="@../GelGridTopo/HexaTop.position")
157+
volume.addObject("TetrahedronSetTopologyModifier", name="TetraModifier")
158+
volume.addObject("Hexa2TetraTopologicalMapping", input="@../GelGridTopo/HexaTop", output="@TetraContainer", swapping="false")
159+
160+
volume.addObject("MechanicalObject", name="mstate_gel", template="Vec3d")
161+
volume.addObject("TetrahedronGeometry", name="geom_tetra", mstate="@mstate_gel", topology="@TetraContainer", draw=False)
162+
#volume.addObject("TriangleGeometry", name="tri_geom", mstate="@mstate_gel", topology="@TetraContainer",draw=True)
163+
volume.addObject("PhongTriangleNormalHandler", name="InternalTriangles", geometry="@geom_tetra")
164+
volume.addObject("AABBBroadPhase",name="AABBTetra",geometry="@geom_tetra",nbox=[3,3,3],thread=1)
165+
#volume.addObject("ParallelTetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters)
166+
volume.addObject("TetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters)
167+
volume.addObject("MeshMatrixMass", name="Mass",totalMass=g_gelTotalMass)
168+
169+
volume.addObject("BoxROI",name="BoxROI",box=g_gelFixedBoxROI)
170+
volume.addObject("RestShapeSpringsForceField", stiffness='1e6',points="@BoxROI.indices" )
171+
172+
volume.addObject("LinearSolverConstraintCorrection", printLog="false", linearSolver="@LinearSolver")
173+
174+
volumeCollision = volume.addChild("collision")
175+
volumeCollision.addObject("TriangleSetTopologyContainer", name="TriContainer")
176+
volumeCollision.addObject("TriangleSetTopologyModifier", name="TriModifier")
177+
volumeCollision.addObject("Tetra2TriangleTopologicalMapping", name="mapping", input="@../TetraContainer", output="@TriContainer", flipNormals=False)
178+
volumeCollision.addObject("MechanicalObject", name="mstate_gelColi",position="@../TetraContainer.position")
179+
volumeCollision.addObject("TriangleGeometry", name="geom_tri", mstate="@mstate_gelColi", topology="@TriContainer",draw=False)
180+
volumeCollision.addObject("PhongTriangleNormalHandler", name="SurfaceTriangles", geometry="@geom_tri")
181+
volumeCollision.addObject("AABBBroadPhase",name="AABBTriangles",thread=1,nbox=[2,2,3])
182+
183+
volumeCollision.addObject("IdentityMapping", name="identityMappingToCollision", input="@../mstate_gel", output="@mstate_gelColi", isMechanical=True)
184+
185+
volumeVisu = volumeCollision.addChild("visu")
186+
volumeVisu.addObject("OglModel", position="@../TriContainer.position",
187+
vertices="@../TriContainer.position",
188+
triangles="@../TriContainer.triangles",
189+
color=g_cubeColor,name="volume_visu",template="Vec3d")
190+
volumeVisu.addObject("IdentityMapping")
191+
192+
volumeVisuWire = volume.addChild("visu_wire")
193+
volumeVisuWire.addObject("OglModel", position="@../TetraContainer.position",
194+
vertices="@../TetraContainer.position",
195+
triangles="@../TetraContainer.triangles",
196+
color=[1, 0, 1, 1],name="volume_visu",template="Vec3d")
197+
volumeVisuWire.addObject("IdentityMapping")
198+
199+
200+
root.addObject("InsertionAlgorithm", name="InsertionAlgo",
201+
fromGeom="@Needle/tipCollision/geom_tip",
202+
destGeom="@Volume/collision/geom_tri",
203+
fromVol="@Needle/bodyCollision/geom_body",
204+
destVol="@Volume/geom_tetra",
205+
punctureThreshold=2.,
206+
slideDistance=0.003,
207+
drawcollision=True,
208+
sphereRadius=0.0001
209+
#projective=True
210+
)
211+
root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01)
212+
root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles")
213+
root.addObject("ConstraintUnilateral",input="@InsertionAlgo.output",directions="@punctureDirection",draw_scale="0.001")#, mu="0.001")
214+
215+
root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams")
216+
root.addObject("ConstraintInsertion",input="@InsertionAlgo.outputList", directions="@bindDirection",draw_scale="0.002", frictionCoeff=0.0023)

‎src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h‎

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
#include <sofa/component/statecontainer/MechanicalObject.h>
1010
#include <sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h>
1111

12-
#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
12+
#include <sofa/collisionAlgorithm/proximity/EdgeProximity.h>
1313

1414
namespace sofa::collisionAlgorithm {
1515

@@ -152,7 +152,20 @@ class InsertionAlgorithm : public BaseAlgorithm {
152152
auto createProximityOp = Operations::CreateCenterProximity::Operation::get(itfrom->getTypeInfo());
153153
auto pfrom = createProximityOp(itfrom->element());
154154

155-
const SReal dist = (pfrom->getPosition() - m_couplingPts.back()->getPosition()).norm();
155+
auto itfromVol = l_fromVol->begin(l_fromVol->getSize() - 2);
156+
auto createProximityOpVol = Operations::CreateCenterProximity::Operation::get(itfromVol->getTypeInfo());
157+
auto pfromVol = createProximityOpVol(itfromVol->element());
158+
const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(pfromVol);
159+
const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() - edgeProx->element()->getP0()->getPosition()).normalized();
160+
type::Vec3 ab = m_couplingPts.back()->getPosition() - pfrom->getPosition();
161+
const SReal dotProd = dot(ab, normal);
162+
if (dotProd > 0.0)
163+
{
164+
m_couplingPts.pop_back();
165+
m_needlePts.pop_back();
166+
}
167+
168+
const SReal dist = ab.norm();
156169
if(dist > d_slideDistance.getValue())
157170
{
158171
auto findClosestProxOp_vol = Operations::FindClosestProximity::Operation::get(l_destVol);

0 commit comments

Comments
 (0)