diff --git a/python3/cosserat/plasticity/CosseratNavigationController.py b/python3/cosserat/plasticity/CosseratNavigationController.py index 665fd1e..9ecfdc1 100644 --- 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/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_constraint_version.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_constraint_version.py index 3be8148..0e16620 100644 --- a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_constraint_version.py +++ b/python3/cosserat/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 index 055435d..8812145 100644 --- 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/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts_FF_version.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts_FF_version.py index da77f6f..c4fc7ac 100644 --- a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_contacts_FF_version.py +++ b/python3/cosserat/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/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_forcefield_version.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_forcefield_version.py index cb7ed6e..2b13294 100644 --- a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_forcefield_version.py +++ b/python3/cosserat/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 index 936d787..a070234 100644 --- 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 index 5272786..60057ad 100644 --- 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/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version.py index 4253a9a..e91da5f 100644 --- a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version.py +++ b/python3/cosserat/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/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version_CG.py b/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version_CG.py index 9e4d9f7..8b21258 100644 --- a/python3/cosserat/plasticity/testScene_coaxialBeamModel_two_instruments_coaxial_realistic_FF_version_CG.py +++ b/python3/cosserat/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 index cffc0f3..1194881 100644 --- 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 index 2209921..a104c3b 100644 --- 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