From 6c7d6364647e524b37e237d666ed257c3cacfc86 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 7 Aug 2025 09:49:01 +0200 Subject: [PATCH 1/2] [scene] Fixed the lack of force feedback when using the needle with haptics --- ...nGeomagic.py => InsertionHaptics_Liver.py} | 66 ++++++++++++------- 1 file changed, 41 insertions(+), 25 deletions(-) rename scenes/{InsertionGeomagic.py => InsertionHaptics_Liver.py} (83%) diff --git a/scenes/InsertionGeomagic.py b/scenes/InsertionHaptics_Liver.py similarity index 83% rename from scenes/InsertionGeomagic.py rename to scenes/InsertionHaptics_Liver.py index 4eaacc77..fae1189e 100644 --- a/scenes/InsertionGeomagic.py +++ b/scenes/InsertionHaptics_Liver.py @@ -1,13 +1,13 @@ import Sofa -g_needleLength=0.100 #(m) -g_needleNumberOfElems=20 #(# of edges) +g_needleLength=0.200 #(m) +g_needleNumberOfElems=40 #(# of edges) g_needleBaseOffset=[0.15,0.04,0.04] g_needleRadius = 0.001 #(m) g_needleMechanicalParameters = { "radius":g_needleRadius, - "youngModulus":1e11, - "poissonRatio":0.3 + "youngModulus":2e13, + "poissonRatio":0.45 } g_needleTotalMass=0.01 @@ -17,8 +17,8 @@ "max":[0.125, 0.125, -0.100] } #Again all in mm g_gelMechanicalParameters = { - "youngModulus":9e4, - "poissonRatio":0.45, + "youngModulus":4e4, + "poissonRatio":0.3, "method":"large" } g_gelTotalMass = 1 @@ -62,10 +62,11 @@ def createScene(root): root.addObject("ConstraintAttachButtonSetting") root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" ) root.addObject("FreeMotionAnimationLoop") - root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000) + root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000, regularizationTerm=0.001) root.addObject("CollisionLoop") - root.addObject("GeomagicDriver" + toolController = root.addChild("ToolController") + toolController.addObject("GeomagicDriver" , name='GeomagicDevice' , deviceName='Default Device' , scale=0.02 @@ -75,30 +76,34 @@ def createScene(root): , positionBase=[0.12, 0, 0] , orientationBase=[0, 0.174, 0, -0.985] ) - - toolController = root.addChild("ToolController") - toolController.addObject("MechanicalObject", name="mstate_baseMaster", position="@GeomagicDevice.positionDevice", template="Rigid3d", showObjectScale=0.01, showObject=False, drawMode=1) + toolController.addObject("MechanicalObject", name="mstate_baseMaster" + , position="@GeomagicDevice.positionDevice" + , template="Rigid3d" + , showObjectScale=0.01 + , showObject=False + , drawMode=1 + ) needle = root.addChild("Needle") needle.addObject("EulerImplicitSolver", firstOrder=True) needle.addObject("EigenSparseLU", name="LinearSolver", template="CompressedRowSparseMatrixd") - needle.addObject("EdgeSetTopologyContainer", name="Container", position=[[g_needleBaseOffset[0], g_needleBaseOffset[1], -(i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[2])] for i in range(g_needleNumberOfElems + 1)] - , edges=[[i, i+1] for i in range(g_needleNumberOfElems)]) - + needle.addObject("EdgeSetTopologyContainer", name="Container" + , position=[[g_needleBaseOffset[0], g_needleBaseOffset[1], -(i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[2])] for i in range(g_needleNumberOfElems + 1)] + , edges=[[i, i+1] for i in range(g_needleNumberOfElems)] + ) needle.addObject("EdgeSetTopologyModifier", name="modifier") needle.addObject("PointSetTopologyModifier", name="modifier2") - - needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.002, showObject=False, drawMode=1) + needle.addObject("MechanicalObject", name="mstate", template="Rigid3d" + , showObjectScale=0.002, showObject=False, drawMode=1) needle.addObject("UniformMass", totalMass=g_needleTotalMass) needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters) needle.addObject("LinearSolverConstraintCorrection", linearSolver="@LinearSolver") - needle.addObject("LCPForceFeedback", activate=1, forceCoef=0.01) + needle.addObject("RestShapeSpringsForceField",points=[0],stiffness=1e9, angularStiffness=1e11,external_points=[0],external_rest_shape="@/ToolController/mstate_baseMaster") needleBase = needle.addChild("needleBase") needleBase.addObject("PointSetTopologyContainer", name="Container_base", position="@../mstate.position") needleBase.addObject("MechanicalObject",name="mstate_base", template="Rigid3d") - needleBase.addObject("RestShapeSpringsForceField",points=[0],stiffness=1e9, angularStiffness=1e9,external_points=[0],external_rest_shape="@/ToolController/mstate_baseMaster") needleBase.addObject("SubsetMapping", indices=0) needleBodyCollision = needle.addChild("bodyCollision") @@ -106,11 +111,12 @@ def createScene(root): needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d", drawMode=0, showObject=False, showObjectScale=10) needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body") needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body") - needleBodyCollision.addObject("IdentityMapping") needleTipCollision = needle.addChild("tipCollision") - needleTipCollision.addObject("MechanicalObject",name="mstate_tip",position=[g_needleBaseOffset[0], g_needleBaseOffset[1], -(g_needleLength+g_needleBaseOffset[2])],template="Vec3d", showObject=False, showObjectScale=20) + needleTipCollision.addObject("PointSetTopologyContainer", name="Container_tip" + , position=[g_needleBaseOffset[0], g_needleBaseOffset[1], -(g_needleLength+g_needleBaseOffset[2])]) + needleTipCollision.addObject("MechanicalObject",name="mstate_tip",template="Vec3d", showObject=False, showObjectScale=20) needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip") needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems) @@ -119,9 +125,7 @@ def createScene(root): needleVisual.addObject("QuadSetTopologyContainer", name="Container_visu") needleVisual.addObject("QuadSetTopologyModifier", name="Modifier") needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@Container_visu") - needleVisual.addObject("MechanicalObject", name="mstate_visu", showObjectScale=0.0002, showObject=False, drawMode=1) - needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@mstate_visu") needleOGL = needleVisual.addChild("OGL") @@ -133,6 +137,18 @@ def createScene(root): name="visualOgl") needleOGL.addObject("IdentityMapping") + FF = root.addChild("ForceFeedback") + FF.addObject("MechanicalObject", name="mstate_lcp", template="Rigid3d" + , showObject=False, src="@../Needle/needleBase/mstate_base") + FF.addObject("LCPForceFeedback", name="lcp_ff", activate=1, forceCoef=1) + FFCollision = FF.addChild("Collision") + FFCollision.addObject("EdgeSetTopologyContainer", name="Container", src="@../../Needle/bodyCollision/Container_body") + FFCollision.addObject("MechanicalObject", name="mstate_coli", constraint="@../../Needle/bodyCollision/mstate_body.constraint") + FFCollision.addObject("RigidMapping") + FFTip = FF.addChild("Tip") + FFTip.addObject("PointSetTopologyContainer", name="Container", src="@../../Needle/tipCollision/Container_tip") + FFTip.addObject("MechanicalObject", name="mstate_coli", constraint="@../../Needle/tipCollision/mstate_tip.constraint") + FFTip.addObject("RigidMapping") volume = root.addChild("Volume") volume.addObject("EulerImplicitSolver") @@ -184,12 +200,12 @@ def createScene(root): surfGeom="@Volume/collision/geom_tri", shaftGeom="@Needle/bodyCollision/geom_body", volGeom="@Volume/geom_tetra", - punctureForceThreshold=2., - tipDistThreshold=0.003, + punctureForceThreshold=1., + tipDistThreshold=0.01, drawcollision=True, drawPointsScale=0.0001 ) - root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) + root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.02) root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001, mu=0.001) From 7baf3cca704370c7e684c656c5d5f07397b83011 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 7 Aug 2025 10:56:22 +0200 Subject: [PATCH 2/2] [scene] Added a simpler scene with haptics - insertion in a box --- scenes/InsertionHaptics_Box.py | 211 +++++++++++++++++++++++++++++++++ 1 file changed, 211 insertions(+) create mode 100644 scenes/InsertionHaptics_Box.py diff --git a/scenes/InsertionHaptics_Box.py b/scenes/InsertionHaptics_Box.py new file mode 100644 index 00000000..2f118abc --- /dev/null +++ b/scenes/InsertionHaptics_Box.py @@ -0,0 +1,211 @@ +import Sofa + +g_needleLength=0.100 #(m) +g_needleNumberOfElems=20 #(# of edges) +g_needleBaseOffset=[0.15,0.04,0.04] +g_needleRadius = 0.001 #(m) +g_needleMechanicalParameters = { + "radius":g_needleRadius, + "youngModulus":5e11, + "poissonRatio":0.45 +} +g_needleTotalMass=0.01 + +g_gelRegularGridParameters = { + "n":[8, 8, 8], + "min":[-0.125, -0.125, -0.350], + "max":[0.125, 0.125, -0.100] +} #Again all in mm +g_gelMechanicalParameters = { + "youngModulus":9e5, + "poissonRatio":0.3, + "method":"large" +} +g_gelTotalMass = 1 +g_cubeColor=[0.8, 0.34, 0.34, 0.3] +g_gelFixedBoxROI=[-0.130, -0.130, -0.360, 0.130, 0.130, -0.300 ] + +# Function called when the scene graph is being created +def createScene(root): + root.gravity=[0,0,-9.81] + root.dt = 0.01 + + root.addObject("RequiredPlugin",pluginName=['Sofa.Component.AnimationLoop', + 'Sofa.Component.Constraint.Lagrangian.Solver', + 'Sofa.Component.ODESolver.Backward', + 'Sofa.Component.Visual', + 'Sofa.Component.Constraint.Lagrangian.Correction', + 'Sofa.Component.Constraint.Lagrangian.Model', + 'Sofa.Component.LinearSolver.Direct', + 'Sofa.Component.Mapping.Linear', + 'Sofa.Component.Mass', + 'Sofa.Component.SolidMechanics.FEM.Elastic', + 'Sofa.Component.StateContainer', + 'Sofa.Component.Topology.Container.Dynamic', + 'Sofa.Component.Topology.Mapping', + 'Sofa.Component.Mapping.NonLinear', + 'Sofa.Component.Topology.Container.Grid', + 'Sofa.Component.Constraint.Projective', + 'Sofa.Component.SolidMechanics.Spring', + 'Sofa.GL.Component.Rendering3D', + 'Sofa.GUI.Component', + 'Sofa.Component.Engine.Select', + 'MultiThreading', + 'CollisionAlgorithm', + 'ConstraintGeometry', + 'Geomagic', + 'Sofa.Component.Haptics', + ]) + + + root.addObject("ConstraintAttachButtonSetting") + root.addObject("VisualStyle", displayFlags="showVisualModels hideBehaviorModels showCollisionModels hideMappings hideForceFields showWireframe showInteractionForceFields" ) + root.addObject("FreeMotionAnimationLoop") + root.addObject("GenericConstraintSolver", tolerance=0.00001, maxIt=5000) + root.addObject("CollisionLoop") + + toolController = root.addChild("ToolController") + toolController.addObject("GeomagicDriver" + , name='GeomagicDevice' + , deviceName='Default Device' + , scale=0.02 + , drawDeviceFrame=False + , drawDevice=False + , manualStart=False + , positionBase=[0.12, 0, 0] + , orientationBase=[0, 0.174, 0, -0.985] + ) + toolController.addObject("MechanicalObject", name="mstate_baseMaster" + , position="@GeomagicDevice.positionDevice" + , template="Rigid3d" + , showObjectScale=0.01 + , showObject=False + , drawMode=1 + ) + + needle = root.addChild("Needle") + needle.addObject("EulerImplicitSolver", firstOrder=True) + needle.addObject("EigenSparseLU", name="LinearSolver", template="CompressedRowSparseMatrixd") + needle.addObject("EdgeSetTopologyContainer", name="Container" + , position=[[g_needleBaseOffset[0], g_needleBaseOffset[1], -(i * g_needleLength/(g_needleNumberOfElems) + g_needleBaseOffset[2])] for i in range(g_needleNumberOfElems + 1)] + , edges=[[i, i+1] for i in range(g_needleNumberOfElems)] + ) + needle.addObject("EdgeSetTopologyModifier", name="modifier") + needle.addObject("PointSetTopologyModifier", name="modifier2") + needle.addObject("MechanicalObject", name="mstate", template="Rigid3d", showObjectScale=0.0002, showObject=False, drawMode=1) + needle.addObject("UniformMass", totalMass=g_needleTotalMass) + needle.addObject("BeamFEMForceField", name="FEM", **g_needleMechanicalParameters) + needle.addObject("LinearSolverConstraintCorrection", printLog=False, linearSolver="@LinearSolver") + needle.addObject("RestShapeSpringsForceField",points=[0], stiffness=1e8, angularStiffness=1e8, external_points=[0], external_rest_shape="@/ToolController/mstate_baseMaster") + + needleBase = needle.addChild("needleBase") + needleBase.addObject("PointSetTopologyContainer", name="Container_base", position=[0, 0, 0]) + needleBase.addObject("MechanicalObject",name="mstate_base", template="Rigid3d",) + needleBase.addObject("SubsetMapping", indices="0") + + needleBodyCollision = needle.addChild("bodyCollision") + needleBodyCollision.addObject("EdgeSetTopologyContainer", name="Container_body", src="@../Container") + needleBodyCollision.addObject("MechanicalObject",name="mstate_body", template="Vec3d",) + needleBodyCollision.addObject("EdgeGeometry",name="geom_body",mstate="@mstate_body", topology="@Container_body") + needleBodyCollision.addObject("EdgeNormalHandler", name="NeedleBeams", geometry="@geom_body") + needleBodyCollision.addObject("IdentityMapping") + + needleTipCollision = needle.addChild("tipCollision") + needleTipCollision.addObject("PointSetTopologyContainer", name="Container_tip" + , position=[g_needleBaseOffset[0], g_needleBaseOffset[1], -(g_needleLength+g_needleBaseOffset[2])]) + needleTipCollision.addObject("MechanicalObject",name="mstate_tip",template="Vec3d", showObject=False, showObjectScale=20) + needleTipCollision.addObject("PointGeometry",name="geom_tip",mstate="@mstate_tip") + needleTipCollision.addObject("RigidMapping",globalToLocalCoords=True,index=g_needleNumberOfElems) + + needleVisual = needle.addChild("visual") + needleVisual.addObject("QuadSetTopologyContainer", name="Container_visu") + needleVisual.addObject("QuadSetTopologyModifier", name="Modifier") + needleVisual.addObject("Edge2QuadTopologicalMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../Container", output="@Container_visu") + needleVisual.addObject("MechanicalObject", name="mstate_visu", showObjectScale=0.0002, showObject=True, drawMode=1) + needleVisual.addObject("TubularMapping", nbPointsOnEachCircle=8, radius=g_needleRadius, input="@../mstate", output="@mstate_visu") + + needleOGL = needleVisual.addChild("OGL") + needleOGL.addObject("OglModel", position="@../Container_visu.position", + vertices="@../Container_visu.position", + quads="@../Container_visu.quads", + color=[0.4, 0.34, 0.34], + 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", + name="visualOgl") + needleOGL.addObject("IdentityMapping") + + FF = root.addChild("ForceFeedback") + FF.addObject("MechanicalObject", name="mstate_lcp", template="Rigid3d" + , showObject=False, src="@../Needle/needleBase/mstate_base") + FF.addObject("LCPForceFeedback", name="lcp_ff", activate=1, forceCoef=0.25) + FFBody = FF.addChild("Body") + FFBody.addObject("EdgeSetTopologyContainer", name="Container", src="@../../Needle/bodyCollision/Container_body") + FFBody.addObject("MechanicalObject", name="mstate_coli", constraint="@../../Needle/bodyCollision/mstate_body.constraint") + FFBody.addObject("RigidMapping") + FFTip = FF.addChild("Tip") + FFTip.addObject("PointSetTopologyContainer", name="Container", src="@../../Needle/tipCollision/Container_tip") + FFTip.addObject("MechanicalObject", name="mstate_coli", constraint="@../../Needle/tipCollision/mstate_tip.constraint") + FFTip.addObject("RigidMapping") + + gelTopo = root.addChild("GelGridTopo") + gelTopo.addObject("RegularGridTopology", name="HexaTop", **g_gelRegularGridParameters) + + volume = root.addChild("Volume") + volume.addObject("EulerImplicitSolver") + volume.addObject("EigenSimplicialLDLT", name="LinearSolver", template='CompressedRowSparseMatrixMat3x3d') + volume.addObject("TetrahedronSetTopologyContainer", name="TetraContainer", position="@../GelGridTopo/HexaTop.position") + volume.addObject("TetrahedronSetTopologyModifier", name="TetraModifier") + volume.addObject("Hexa2TetraTopologicalMapping", input="@../GelGridTopo/HexaTop", output="@TetraContainer", swapping=False) + + volume.addObject("MechanicalObject", name="mstate_gel", template="Vec3d") + volume.addObject("TetrahedronGeometry", name="geom_tetra", mstate="@mstate_gel", topology="@TetraContainer", draw=False) + volume.addObject("AABBBroadPhase",name="AABBTetra",geometry="@geom_tetra",nbox=[3,3,3],thread=1) + volume.addObject("TetrahedronFEMForceField", name="FF",**g_gelMechanicalParameters) + volume.addObject("MeshMatrixMass", name="Mass",totalMass=g_gelTotalMass) + + volume.addObject("BoxROI",name="BoxROI",box=g_gelFixedBoxROI) + volume.addObject("RestShapeSpringsForceField", stiffness=1e6,points="@BoxROI.indices" ) + + volume.addObject("LinearSolverConstraintCorrection", printLog=False, linearSolver="@LinearSolver") + + volumeCollision = volume.addChild("collision") + volumeCollision.addObject("TriangleSetTopologyContainer", name="TriContainer") + volumeCollision.addObject("TriangleSetTopologyModifier", name="TriModifier") + volumeCollision.addObject("Tetra2TriangleTopologicalMapping", name="mapping", input="@../TetraContainer", output="@TriContainer", flipNormals=False) + volumeCollision.addObject("MechanicalObject", name="mstate_gelColi",position="@../TetraContainer.position") + volumeCollision.addObject("TriangleGeometry", name="geom_tri", mstate="@mstate_gelColi", topology="@TriContainer",draw=False) + volumeCollision.addObject("PhongTriangleNormalHandler", name="SurfaceTriangles", geometry="@geom_tri") + volumeCollision.addObject("AABBBroadPhase",name="AABBTriangles",thread=1,nbox=[2,2,3]) + + volumeCollision.addObject("IdentityMapping", name="identityMappingToCollision", input="@../mstate_gel", output="@mstate_gelColi", isMechanical=True) + + volumeVisu = volumeCollision.addChild("visu") + volumeVisu.addObject("OglModel", position="@../TriContainer.position", + vertices="@../TriContainer.position", + triangles="@../TriContainer.triangles", + color=g_cubeColor,name="volume_visu",template="Vec3d") + volumeVisu.addObject("IdentityMapping") + + volumeVisuWire = volume.addChild("visu_wire") + volumeVisuWire.addObject("OglModel", position="@../TetraContainer.position", + vertices="@../TetraContainer.position", + triangles="@../TetraContainer.triangles", + color=[1, 0, 1, 1],name="volume_visu",template="Vec3d") + volumeVisuWire.addObject("IdentityMapping") + + + root.addObject("InsertionAlgorithm", name="InsertionAlgo", + tipGeom="@Needle/tipCollision/geom_tip", + surfGeom="@Volume/collision/geom_tri", + shaftGeom="@Needle/bodyCollision/geom_body", + volGeom="@Volume/geom_tetra", + punctureForceThreshold=5., + tipDistThreshold=0.003, + drawcollision=True, + drawPointsScale=0.0001 + ) + root.addObject("DistanceFilter",algo="@InsertionAlgo",distance=0.01) + root.addObject("SecondDirection",name="punctureDirection",handler="@Volume/collision/SurfaceTriangles") + root.addObject("ConstraintUnilateral",input="@InsertionAlgo.collisionOutput",directions="@punctureDirection",draw_scale=0.001, mu=0.003) + + root.addObject("FirstDirection",name="bindDirection", handler="@Needle/bodyCollision/NeedleBeams") + root.addObject("ConstraintInsertion",input="@InsertionAlgo.insertionOutput", directions="@bindDirection",draw_scale=0.002, frictionCoeff=0.001)