[CoaxialModel] Add input parameter in navigation controller

Until now, the minimal distance to apply constraint parameter was automatically calculated from the increment distance input parameter. However, if the user defines a high increment distance, this method will cause constraints to be ignored for even somewhat distant frames. To fix this issue, we switch minimalDistanceForConstraint to an input parameter, which the user has to provide as well. We update all active test scenes accordingly.
SofaDefrost · Feb 28, 2023 · 32fbb1b · 32fbb1b
1 parent a37d257
commit 32fbb1b
Show file tree

Hide file tree

Showing 11 changed files with 50 additions and 46 deletions.
diff --git a/python3/cosserat/plasticity/CosseratNavigationController.py b/python3/cosserat/plasticity/CosseratNavigationController.py
@@ -68,6 +68,11 @@
 #    in instrument.py), to characterise each instrument properties
 #  - curvAbsTolerance : distance threshold used to determine if two close nodes
 #    should be merged (and considered as one)
+#  - minimalDistanceForConstraint: minimal distance to apply constraints on close
+#    pairs of coaxial frames (a 'pair' meaning two frames with the same curvilinear
+#    abscissa, on two different instruments). If the distance between two pairs
+#    of coaxial frames is lower than this threshold, we will apply constraints
+#    on only one of the two pairs.
 #  - nbIntermediateConstraintFrames : number of intermediate coaxial frames added
 #    when coaxial beam segments are detected. A higher number means a finer
 #    application of constraints on the coaxial beam segments.
@@ -106,6 +111,7 @@ def __init__(self, rootNode, solverNode,
                  incrementDirection,
                  instrumentList,
                  curvAbsTolerance,
+                 minimalDistanceForConstraint,
                  nbIntermediateConstraintFrames = 0,
                  constrainWithSprings = False,
                  outputFilename="",
@@ -121,8 +127,7 @@ def __init__(self, rootNode, solverNode,
         ### Reading the insertion velocity parameters ###
 
         self.incrementDistance = incrementDistance
-        # TO DO : pass the minimal distance for constraints as an input parameter ?
-        self.minimalDistanceForConstraint = incrementDistance * 5.0
+        self.minimalDistanceForConstraint = minimalDistanceForConstraint
         # TO DO: check that the number of coaxial frames provided is coherent with beam number and nbIntermediateConstraintFrames
         self.nbIntermediateConstraintFrames = nbIntermediateConstraintFrames
         self.incrementAngle = incrementAngle

diff --git a/...serat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_constraint_version.py b/...serat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_constraint_version.py
@@ -538,6 +538,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     # outputFilename = ""
     # inputFilename = "two_instruments_coaxial_close_frames.txt"
@@ -571,6 +572,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
                             constrainWithSprings=False,
                             outputFilename=outputFilename,

diff --git a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts.py
@@ -72,7 +72,7 @@ def createScene(rootNode):
     contactDistance = 1.0*largestSectionRadius
 
 
-    rootNode.addObject('DefaultPipeline', verbose="0")
+    rootNode.addObject('CollisionPipeline', verbose="0")
     rootNode.addObject('BruteForceBroadPhase', name="BroadPhase")
     rootNode.addObject('BVHNarrowPhase', name="NarrowPhase")
     rootNode.addObject('DefaultContactManager', response="FrictionContactConstraint",
@@ -624,6 +624,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     # outputFilename = ""
     # inputFilename = "two_instruments_coaxial_close_frames.txt"
@@ -655,6 +656,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
                             constrainWithSprings=False,
                             outputFilename=outputFilename,

diff --git a/...erat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts_FF_version.py b/...erat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts_FF_version.py
@@ -73,7 +73,7 @@ def createScene(rootNode):
     contactDistance = 1.0*largestSectionRadius
 
 
-    rootNode.addObject('DefaultPipeline', verbose="0")
+    rootNode.addObject('CollisionPipeline', verbose="0")
     rootNode.addObject('BruteForceBroadPhase', name="BroadPhase")
     rootNode.addObject('BVHNarrowPhase', name="NarrowPhase")
     rootNode.addObject('DefaultContactManager', response="FrictionContactConstraint",
@@ -685,6 +685,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     # outputFilename = ""
     # inputFilename = "two_instruments_coaxial_close_frames.txt"
@@ -717,6 +718,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
                             constrainWithSprings=True,
                             outputFilename=outputFilename,

diff --git a/...serat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_forcefield_version.py b/...serat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_forcefield_version.py
@@ -571,6 +571,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     # outputFilename = ""
     # inputFilename = "two_instruments_coaxial_close_frames.txt"
@@ -604,6 +605,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
                             constrainWithSprings=True,
                             outputFilename=outputFilename,

diff --git a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_keyPoints.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_keyPoints.py
@@ -48,24 +48,14 @@ def createScene(rootNode):
     rootNode.findData('dt').value = DT
     rootNode.findData('gravity').value = [0., 0., -GRAVITY]
 
-    rootNode.addObject('FreeMotionAnimationLoop')
+    rootNode.addObject('FreeMotionAnimationLoop',
+                       updateSceneAfterAnimateBeginEvent=True)
 
     # --- Constraint handling --- #
     rootNode.addObject('GenericConstraintSolver', tolerance=1e-8,
                        maxIterations=2e3, printLog=False)
 
 
-    # --- Collisions --- #
-    # rootNode.addObject('DefaultPipeline', verbose="0")
-    # rootNode.addObject('BruteForceBroadPhase', name="BroadPhase")
-    # rootNode.addObject('BVHNarrowPhase', name="NarrowPhase")
-    # rootNode.addObject('DefaultContactManager', response="FrictionContactConstraint",
-    #                    responseParams=frictionCoefficientParam)
-    # rootNode.addObject('LocalMinDistance', name="Proximity",
-    #                    alarmDistance=100.*sectionRadius,
-    #                    contactDistance=10.*sectionRadius, angleCone=0.1)
-
-
     # -------------------------------------------------------------------- #
     # -----                      Beam parameters                     ----- #
     # -------------------------------------------------------------------- #
@@ -150,9 +140,6 @@ def createScene(rootNode):
                                            name="RigidBaseMO", position=[0., 0., 0., 0, 0, 0, 1],
                                            showObject=False,
                                            showObjectScale=2.)
-    # rigidBaseNode0.addObject('RestShapeSpringsForceField', name='spring',
-    #                          stiffness="5.e8", angularStiffness="5.e8",
-    #                          external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d")
     rigidBaseNode0.addObject('RestShapeSpringsForceField', name='controlSpring',
                              stiffness="5.e8", angularStiffness="1.0e5",
                              external_rest_shape="@../../../controlPointNode0/controlPointMO",
@@ -227,14 +214,6 @@ def createScene(rootNode):
                                      length=beamLengths, poissonRatioList=beamPoissonRatioList,
                                      youngModulusList=beamYoungModulusList, innerRadius=0.4)
 
-    beamBendingMoment = 1.0e5
-    bendingForces = np.array([0, beamBendingMoment, beamBendingMoment])
-    # momentIndices = range(1, nbBeams0PlusStock)
-    momentIndices = [nbBeams0PlusStock-1]
-    # rateAngularDeformNode.addObject('ConstantForceField', name='Moment',
-    #                                 indices=momentIndices,
-    #                                 forces=bendingForces)
-
     # EXPERIMENTAL: navigation simulation
     # Adding constraints on the additional beams which are not meant to be
     # simulated at the beginning
@@ -335,9 +314,6 @@ def createScene(rootNode):
                                            name="RigidBaseMO", position=[0., 0., 0., 0, 0, 0, 1],
                                            showObject=False,
                                            showObjectScale=2.)
-    # rigidBaseNode1.addObject('RestShapeSpringsForceField', name='spring',
-    #                          stiffness="5.e8", angularStiffness="5.e8",
-    #                          external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d")
     rigidBaseNode1.addObject('RestShapeSpringsForceField', name='controlSpring',
                              stiffness="5.e8", angularStiffness="1.0e5",
                              external_rest_shape="@../../../controlPointNode1/controlPointMO",
@@ -412,14 +388,6 @@ def createScene(rootNode):
                                      length=beamLengths, poissonRatioList=beamPoissonRatioList,
                                      youngModulusList=beamYoungModulusList)
 
-    beamBendingMoment = 1.0e5
-    bendingForces = np.array([0, beamBendingMoment, beamBendingMoment])
-    # momentIndices = range(1, nbBeams1PlusStock)
-    momentIndices = [nbBeams1PlusStock-1]
-    # rateAngularDeformNode.addObject('ConstantForceField', name='Moment',
-    #                                 indices=momentIndices,
-    #                                 forces=bendingForces)
-
     # EXPERIMENTAL: navigation simulation
     # Adding constraints on the additional beams which are not meant to be
     # simulated at the beginning
@@ -534,6 +502,12 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
+
+    # outputFilename = ""
+    # inputFilename = ""
+    outputFilename = "two_instruments_coaxial_keyPoints_script.txt"
+    inputFilename = ""
 
     instrument0 = Instrument(instrumentNode=instrument0Node,
                              totalLength=totalLength0,
@@ -560,6 +534,10 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
-                            nbIntermediateConstraintFrames=nbIntermediateConstraintFrames))
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
+                            nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
+                            constrainWithSprings=False,
+                            outputFilename=outputFilename,
+                            inputFilename=inputFilename))
 
     return rootNode
diff --git a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic.py
@@ -557,6 +557,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     instrument0 = Instrument(instrumentNode=instrument0Node,
                              totalLength=totalLength0,
@@ -583,6 +584,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames))
 
     return rootNode
diff --git a/...rat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version.py b/...rat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version.py
@@ -38,7 +38,8 @@
                  ' Sofa.Component.Topology.Container.Dynamic' \
                  ' Sofa.Component.Collision.Detection.Algorithm' \
                  ' Sofa.Component.Collision.Detection.Intersection' \
-                 ' Sofa.Component.Collision.Response.Contact'
+                 ' Sofa.Component.Collision.Response.Contact' \
+                 ' Sofa.Component.Mapping.MappedMatrix'
 
 visualFlagList = 'showVisualModels showBehaviorModels showCollisionModels' \
                  ' hideBoundingCollisionModels hideForceFields' \
@@ -69,7 +70,7 @@ def createScene(rootNode):
     contactDistance = 1.0*largestSectionRadius
 
 
-    rootNode.addObject('DefaultPipeline', verbose="0")
+    rootNode.addObject('CollisionPipeline', verbose="0")
     rootNode.addObject('BruteForceBroadPhase', name="BroadPhase")
     rootNode.addObject('BVHNarrowPhase', name="NarrowPhase")
     rootNode.addObject('DefaultContactManager', response="FrictionContactConstraint",
@@ -622,6 +623,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     instrument0 = Instrument(instrumentNode=instrument0Node,
                              totalLength=totalLength0,
@@ -648,6 +650,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
                             constrainWithSprings=True))
 

diff --git a/.../plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version_CG.py b/.../plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version_CG.py
@@ -599,6 +599,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     instrument0 = Instrument(instrumentNode=instrument0Node,
                              totalLength=totalLength0,
@@ -625,6 +626,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames,
                             constrainWithSprings=True))
 

diff --git a/python3/cosserat/plasticity/testScene_navigation_obstacle_singleInstrument.py b/python3/cosserat/plasticity/testScene_navigation_obstacle_singleInstrument.py
@@ -57,7 +57,8 @@ def createScene(rootNode):
     rootNode.findData('dt').value = DT
     rootNode.findData('gravity').value = [0., 0., -GRAVITY]
 
-    rootNode.addObject('FreeMotionAnimationLoop')
+    rootNode.addObject('FreeMotionAnimationLoop',
+                       updateSceneAfterAnimateBeginEvent=True)
 
     # --- Constraint handling --- #
     rootNode.addObject('GenericConstraintSolver', tolerance=1e-8,
@@ -72,7 +73,7 @@ def createScene(rootNode):
     contactDistance = 1.0*largestSectionRadius
 
 
-    rootNode.addObject('DefaultPipeline', verbose="0")
+    rootNode.addObject('CollisionPipeline', verbose="0")
     rootNode.addObject('BruteForceBroadPhase', name="BroadPhase")
     rootNode.addObject('BVHNarrowPhase', name="NarrowPhase")
     rootNode.addObject('DefaultContactManager', response="FrictionContactConstraint",
@@ -355,6 +356,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     instrument0 = Instrument(instrumentNode=instrument0Node,
                              totalLength=totalLength0,
@@ -375,6 +377,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames))
 
     return rootNode
diff --git a/python3/cosserat/plasticity/testScene_navigation_obstacle_twoInstruments.py b/python3/cosserat/plasticity/testScene_navigation_obstacle_twoInstruments.py
@@ -57,7 +57,8 @@ def createScene(rootNode):
     rootNode.findData('dt').value = DT
     rootNode.findData('gravity').value = [0., 0., -GRAVITY]
 
-    rootNode.addObject('FreeMotionAnimationLoop')
+    rootNode.addObject('FreeMotionAnimationLoop',
+                       updateSceneAfterAnimateBeginEvent=True)
 
     # --- Constraint handling --- #
     rootNode.addObject('GenericConstraintSolver', tolerance=1e-8,
@@ -72,7 +73,7 @@ def createScene(rootNode):
     contactDistance = 1.0*largestSectionRadius
 
 
-    rootNode.addObject('DefaultPipeline', verbose="0")
+    rootNode.addObject('CollisionPipeline', verbose="0")
     rootNode.addObject('BruteForceBroadPhase', name="BroadPhase")
     rootNode.addObject('BVHNarrowPhase', name="NarrowPhase")
     rootNode.addObject('DefaultContactManager', response="FrictionContactConstraint",
@@ -292,7 +293,7 @@ def createScene(rootNode):
                               drawBeamSegments=True, nonColored=False, debug=0, printLog=False)
 
     # Collision model
-    # 
+    #
     # instrumentCollisionNode0 = mappedFrameNode.addChild('InstrumentCollisionNode')
     # instrumentCollisionNode0.addObject('EdgeSetTopologyContainer', name="collisEdgeSet",
     #                                    position=frames3DDoFs, edges=frameEdges)
@@ -593,6 +594,7 @@ def createScene(rootNode):
     incrementAngle=5.0
     incrementDirection = np.array([1., 0., 0.])
     curvAbsTolerance= 1.0e-4
+    minimalDistanceForConstraint = 0.5 # in cm
 
     instrument0 = Instrument(instrumentNode=instrument0Node,
                              totalLength=totalLength0,
@@ -620,6 +622,7 @@ def createScene(rootNode):
                             incrementDirection=incrementDirection,
                             instrumentList=instrumentList,
                             curvAbsTolerance=curvAbsTolerance,
+                            minimalDistanceForConstraint=minimalDistanceForConstraint,
                             nbIntermediateConstraintFrames=nbIntermediateConstraintFrames))
 
     return rootNode