From 4a99254e32393c6234882833e5e2c5c7fbd01744 Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 26 Aug 2025 16:10:46 +0200 Subject: [PATCH 1/2] Apply changes from SOFA PR 5666 regarding GenericConstraintSolver --- .../python3/NeedleInsertion-predefinedPath.py | 2 +- examples/python3/NeedleInsertion.py | 2 +- examples/python3/actuators/fingerActuation.py | 2 +- examples/python3/cosserat/cosseratObject.py | 456 ++++++------------ .../python3/cosserat/nonLinearCosserat.py | 19 +- .../tutorial/formation/chiba/Actuator.py | 2 +- .../tutorial/formation/chiba/actuator_v1.py | 2 +- examples/python3/tutorial/scene_w9991.py | 2 +- examples/python3/useful/header.py | 2 +- .../wip/completDirectNeedleInsertion.py | 32 +- examples/python3/wip/needleInteractionTest.py | 6 +- 11 files changed, 174 insertions(+), 353 deletions(-) diff --git a/examples/python3/NeedleInsertion-predefinedPath.py b/examples/python3/NeedleInsertion-predefinedPath.py index aa81171d..84736c64 100644 --- a/examples/python3/NeedleInsertion-predefinedPath.py +++ b/examples/python3/NeedleInsertion-predefinedPath.py @@ -96,7 +96,7 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showBehaviorModels hideCollisionModels hideBoundingCollisionModels ' 'showForceFields hideInteractionForceFields showWireframe') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', + rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0") gravity = [0, 0, 0] diff --git a/examples/python3/NeedleInsertion.py b/examples/python3/NeedleInsertion.py index 9d614381..5916de95 100644 --- a/examples/python3/NeedleInsertion.py +++ b/examples/python3/NeedleInsertion.py @@ -41,7 +41,7 @@ def createScene(rootNode): coneFactor=ContactParams.coneFactor, angleCone=0.1) rootNode.addObject('FreeMotionAnimationLoop') - generic = rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", + generic = rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance="1e-20", maxIterations="500", computeConstraintForces=1, printLog="0") gravity = [0, 0, 0] diff --git a/examples/python3/actuators/fingerActuation.py b/examples/python3/actuators/fingerActuation.py index 59e4248f..546d5329 100644 --- a/examples/python3/actuators/fingerActuation.py +++ b/examples/python3/actuators/fingerActuation.py @@ -32,7 +32,7 @@ def createScene(rootNode): addHeader(rootNode, isConstrained=False) rootNode.addObject('FreeMotionAnimationLoop', parallelCollisionDetectionAndFreeMotion=False, parallelODESolving=False) - rootNode.addObject('GenericConstraintSolver', name='ConstraintSolver', tolerance=1e-20, maxIterations=1000, + rootNode.addObject('ProjectedGaussSeidelConstraintSolver', name='ConstraintSolver', tolerance=1e-20, maxIterations=1000, multithreading=False) rootNode.findData('gravity').value = [0., 0., 0.] diff --git a/examples/python3/cosserat/cosseratObject.py b/examples/python3/cosserat/cosseratObject.py index 619e82c1..d3786cff 100644 --- a/examples/python3/cosserat/cosseratObject.py +++ b/examples/python3/cosserat/cosseratObject.py @@ -1,177 +1,99 @@ # -*- coding: utf-8 -*- """ -Cosserat class in SofaPython3. + Cosserat class in SofaPython3. """ -__authors__ = "adagolodjo" +__authors__ = "younesssss" __contact__ = "adagolodjo@protonmail.com" __version__ = "1.0.0" __copyright__ = "(c) 2021,Inria" __date__ = "October, 26 2021" +# from dataclasses import dataclass import Sofa from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry -from useful.utils import addEdgeCollision, addPointsCollision -import warnings +from splib3.numerics import Quat +from cosserat.utils import addEdgeCollision, addPointsCollision +cosserat_config = {'init_pos': [0., 0., 0.], 'tot_length': 6, 'nbSectionS': 6, + 'nbFramesF': 12, 'buildCollisionModel': 1, 'beamMass': 0.22} -cosserat_config = { - "init_pos": [0.0, 0.0, 0.0], - "tot_length": 6, - "nbSectionS": 6, - "nbFramesF": 12, - "buildCollisionModel": 1, - "beamMass": 0.22, -} # @dataclass + + class Cosserat(Sofa.Prefab): - """Cosserat beam prefab class. It is a prefab class that allows to create a Cosserat beam in SOFA. - Parameters: - -parent: node where the ServoArm will be attached - - translation the position in space of the structure - - eulerRotation the orientation of the structure - - attachingTo (MechanicalObject) a rest shape force field will constraint the object - to follow arm position - Structure: - Node : { - name : 'Cosserat' - Node0 MechanicalObject : // Rigid position of the base of the beam - Node1 MechanicalObject : // Vec3d, The rate angular composed of the twist and the bending along y and z - Node1 ForceField // - MechanicalObject // The child of the two precedent nodes, Rigid positions - Cosserat Mapping // it allow the transfer from the local to the global frame - } + """ActuatedArm is a reusable sofa model of a S90 servo motor and the tripod actuation arm. + Parameters: + -parent: node where the ServoArm will be attached + - translation the position in space of the structure + - eulerRotation the orientation of the structure + - attachingTo (MechanicalObject) a rest shape force field will constraint the object + to follow arm position + Structure: + Node : { + name : 'Cosserat' + Node0 MechanicalObject : // Rigid position of the base of the beam + Node1 MechanicalObject : // Vec3d, The rate angular composed of the twist and the bending along y and z + Node1 ForceField // + MechanicalObject // The child of the two precedent nodes, Rigid positions + Cosserat Mapping // it allow the transfer from the local to the global frame + } """ - prefabParameters = [ - { - "name": "name", - "type": "string", - "help": "Node name", "default": "Cosserat" - }, - { - "name": "position", - "type": "Rigid3d::VecCoord", - "help": "Cosserat base position", - "default": [[0.0, 0.0, 0.0, 0, 0, 0, 1.0]], - }, - { - "name": "translation", - "type": "Vec3d", - "help": "Cosserat base Rotation", - "default": [0.0, 0.0, 0.0], - }, - { - "name": "rotation", - "type": "Vec3d", - "help": "Cosserat base Rotation", - "default": [0.0, 0.0, 0.0], - }, - { - "name": "youngModulus", - "type": "double", - "help": "Beam Young modulus", - "default": 1.0e6, - }, - { - "name": "poissonRatio", - "type": "double", - "help": "Beam poisson ratio", - "default": 0.4, - }, - { - "name": "shape", - "type": "string", - "help": "beam section", - "default": "circular", - }, - { - "name": "radius", - "type": "double", - "help": "the radius in case of circular section", - "default": 0.02, - }, - { - "name": "length_Y", - "type": "double", - "help": "the radius in case of circular section", - "default": 1.0, - }, - { - "name": "length_Z", - "type": "double", - "help": "the radius in case of circular section", - "default": 1.0, - }, - { - "name": "rayleighStiffness", - "type": "double", - "help": "Rayleigh damping - stiffness matrix coefficient", - "default": 0.0, - }, - { - "name": "attachingToLink", - "type": "string", - "help": "a rest shape force field will constraint the object " - "to follow arm position", - "default": "1", - }, - { - "name": "showObject", - "type": "string", - "help": " Draw object arrow ", - "default": "0", - }, - ] + {'name': 'name', 'type': 'string', + 'help': 'Node name', 'default': 'Cosserat'}, + {'name': 'position', 'type': 'Rigid3d::VecCoord', 'help': 'Cosserat base position', + 'default': [[0., 0., 0., 0, 0, 0, 1.]]}, + {'name': 'translation', 'type': 'Vec3d', + 'help': 'Cosserat base Rotation', 'default': [0., 0., 0.]}, + {'name': 'rotation', 'type': 'Vec3d', + 'help': 'Cosserat base Rotation', 'default': [0., 0., 0.]}, + {'name': 'youngModulus', 'type': 'double', + 'help': 'Beam Young modulus', 'default': 1.e6}, + {'name': 'poissonRatio', 'type': 'double', + 'help': 'Beam poisson ratio', 'default': 0.4}, + {'name': 'shape', 'type': 'string', + 'help': 'beam section', 'default': "circular"}, + {'name': 'radius', 'type': 'double', + 'help': 'the radius in case of circular section', 'default': 0.02}, + {'name': 'length_Y', 'type': 'double', + 'help': 'the radius in case of circular section', 'default': 1.0}, + {'name': 'length_Z', 'type': 'double', + 'help': 'the radius in case of circular section', 'default': 1.0}, + {'name': 'rayleighStiffness', 'type': 'double', 'help': 'Rayleigh damping - stiffness matrix coefficient', + 'default': 0.0}, + {'name': 'attachingToLink', 'type': 'string', 'help': 'a rest shape force field will constraint the object ' + 'to follow arm position', 'default': '1'}, + {'name': 'showObject', 'type': 'string', 'help': ' Draw object arrow ', 'default': '0'}] def __init__(self, *args, **kwargs): Sofa.Prefab.__init__(self, *args, **kwargs) - self.cosseratGeometry = kwargs["cosseratGeometry"] - self.beamMass = self.cosseratGeometry["beamMass"] - self.parent = kwargs.get("parent") + self.cosseratGeometry = kwargs['cosseratGeometry'] + self.beamMass = self.cosseratGeometry['beamMass'] + self.parent = kwargs.get('parent') self.useInertiaParams = False - self.radius = kwargs.get( - "radius", - ) - - print ('===============>DEPRECATED<============================\n') - - warnings.warn( - "\n====> DEPRECATED: This prefab CosseratObject class will be removed in a future version. " - "Use CosseratBase instead. For examples and tutorials, please refer to " - "the 'tutorial' folder located at plugin.Cosserat/tutoria/tuto_scenes .\n" - ) - print("========================> DEPRECATED<============================\n") - - + self.radius = kwargs.get('radius', ) if self.parent.hasObject("EulerImplicitSolver") is False: - print("The code does not have parent EulerImplicite") + print('The code does not have parent EulerImplicite') self.solverNode = self.addSolverNode() else: self.solverNode = self.parent - if "inertialParams" in kwargs: + if 'inertialParams' in kwargs: self.useInertiaParams = True - self.inertialParams = kwargs["inertialParams"] + self.inertialParams = kwargs['inertialParams'] self.rigidBaseNode = self.addRigidBaseNode() - [ - positionS, - curv_abs_inputS, - sectionsLength, - framesF, - curv_abs_outputF, - self.frames3D, - ] = BuildCosseratGeometry(self.cosseratGeometry) + [positionS, curv_abs_inputS, longeurS, framesF, curv_abs_outputF, self.frames3D] = \ + BuildCosseratGeometry(self.cosseratGeometry) self.cosseratCoordinateNode = self.addCosseratCoordinate( - positionS, sectionsLength - ) + positionS, longeurS) self.cosseratFrame = self.addCosseratFrame( - framesF, curv_abs_inputS, curv_abs_outputF - ) + framesF, curv_abs_inputS, curv_abs_outputF) + # print(f'=== > {curv_abs_inputS}') def init(self): pass @@ -180,218 +102,132 @@ def addCollisionModel(self): tab_edges = buildEdges(self.frames3D) return addEdgeCollision(self.cosseratFrame, self.frames3D, tab_edges) - def addPointCollisionModel(self, nodeName="CollisionPoints"): + def addPointCollisionModel(self, nodeName='CollisionPoints'): tab_edges = buildEdges(self.frames3D) - return addPointsCollision( - self.cosseratFrame, self.frames3D, tab_edges, nodeName - ) + return addPointsCollision(self.cosseratFrame, self.frames3D, tab_edges, nodeName) def addSlidingPoints(self): - slidingPoint = self.cosseratFrame.addChild("slidingPoint") - slidingPoint.addObject( - "MechanicalObject", - name="slidingPointMO", - position=self.frames3D, - showObject="0", - showIndices="0", - ) - slidingPoint.addObject("IdentityMapping") + slidingPoint = self.cosseratFrame.addChild('slidingPoint') + slidingPoint.addObject('MechanicalObject', name="slidingPointMO", position=self.frames3D, + showObject="0", showIndices="0") + slidingPoint.addObject('IdentityMapping') return slidingPoint def addSlidingPointsWithContainer(self): - slidingPoint = self.cosseratFrame.addChild("slidingPoint") - slidingPoint.addObject("PointSetTopologyContainer") - slidingPoint.addObject("PointSetTopologyModifier") - slidingPoint.addObject( - "MechanicalObject", - name="slidingPointMO", - position=self.frames3D, - showObject="1", - showIndices="0", - ) - slidingPoint.addObject("IdentityMapping") + slidingPoint = self.cosseratFrame.addChild('slidingPoint') + container = slidingPoint.addObject("PointSetTopologyContainer") + modifier = slidingPoint.addObject("PointSetTopologyModifier") + slidingPoint.addObject('MechanicalObject', name="slidingPointMO", position=self.frames3D, + showObject="1", showIndices="0") + slidingPoint.addObject('IdentityMapping') return slidingPoint def addSolverNode(self): - solverNode = self.parent - # solverNode = self.addChild('solverNode') - solverNode.addObject( - "EulerImplicitSolver", rayleighStiffness="0.2", rayleighMass="0.1" - ) - solverNode.addObject( - "SparseLDLSolver", name="solver", template="CompressedRowSparseMatrixd" - ) - solverNode.addObject("GenericConstraintCorrection") + solverNode = self.addChild('solverNode') + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness="0.2", rayleighMass='0.1') + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") + solverNode.addObject('GenericConstraintCorrection') return solverNode def addRigidBaseNode(self): - rigidBaseNode = self.addChild("rigidBase") + rigidBaseNode = self.addChild('rigidBase') # trans = [t for t in self.translation.value] - trans = self.translation.value - rot = self.rotation.value - + trans = list(self.translation.value) + rot = list(self.rotation.value) # @todo converter - positions = [pos for pos in self.position.value] - - rigidBaseNode.addObject( - "MechanicalObject", - template="Rigid3d", - name="RigidBaseMO", - showObjectScale=0.2, - translation=trans, - position=positions, - rotation=rot, - showObject=int(self.showObject.value), - ) + positions = [list(pos) for pos in self.position.value] + + rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO", showObjectScale=0.2, + translation=trans, position=positions, rotation=rot, + showObject=int(self.showObject.value)) # one can choose to set this to false and directly attach the beam base # to a control object in order to be able to drive it. if int(self.attachingToLink.value): print("Adding the rest shape to the base") - rigidBaseNode.addObject( - "RestShapeSpringsForceField", - name="spring", - stiffness=1e8, - angularStiffness=1.0e8, - external_points=0, - mstate="@RigidBaseMO", - points=0, - template="Rigid3d", - ) + rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness=1e8, angularStiffness=1.e8, + external_points=0, mstate="@RigidBaseMO", points=0, template="Rigid3d") return rigidBaseNode def addCosseratCoordinate(self, bendingStates, listOfSectionsLength): - cosseratCoordinateNode = self.addChild("cosseratCoordinate") - cosseratCoordinateNode.addObject( - "MechanicalObject", - template="Vec3d", - name="cosseratCoordinateMO", - position=bendingStates, - showIndices=0, - ) + cosseratCoordinateNode = self.addChild('cosseratCoordinate') + cosseratCoordinateNode.addObject('MechanicalObject', + template='Vec3d', name='cosseratCoordinateMO', + position=bendingStates, + showIndices=0) if self.useInertiaParams is False: - cosseratCoordinateNode.addObject( - "BeamHookeLawForceField", - crossSectionShape=self.shape.value, - length=listOfSectionsLength, - radius=self.radius.value, - youngModulus=self.youngModulus.value, - poissonRatio=self.poissonRatio.value, - rayleighStiffness=self.rayleighStiffness.value, - lengthY=self.length_Y.value, - lengthZ=self.length_Z.value, - ) + cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, + length=listOfSectionsLength, radius=self.radius.value, + youngModulus=self.youngModulus.value, poissonRatio=self.poissonRatio.value, + rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.length_Y.value, lengthZ=self.length_Z.value) else: - self._extracted_from_addCosseratCoordinate_15( - cosseratCoordinateNode, listOfSectionsLength - ) + GA = self.inertialParams['GA'] + GI = self.inertialParams['GI'] + EA = self.inertialParams['EA'] + EI = self.inertialParams['EI'] + print(f'{GA}') + cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, + length=listOfSectionsLength, radius=self.radius.value, useInertiaParams=True, + GI=GI, GA=GA, EI=EI, EA=EA, rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.length_Y.value, lengthZ=self.length_Z.value) return cosseratCoordinateNode - # TODO Rename this here and in `addCosseratCoordinate` - def _extracted_from_addCosseratCoordinate_15( - self, cosseratCoordinateNode, listOfSectionsLength - ): - GA = self.inertialParams["GA"] - GI = self.inertialParams["GI"] - EA = self.inertialParams["EA"] - EI = self.inertialParams["EI"] - print(f"{GA}") - cosseratCoordinateNode.addObject( - "BeamHookeLawForceField", - crossSectionShape=self.shape.value, - length=listOfSectionsLength, - radius=self.radius.value, - useInertiaParams=True, - GI=GI, - GA=GA, - EI=EI, - EA=EA, - rayleighStiffness=self.rayleighStiffness.value, - lengthY=self.length_Y.value, - lengthZ=self.length_Z.value, - ) - def addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): + cosseratInSofaFrameNode = self.rigidBaseNode.addChild( - "cosseratInSofaFrameNode") + 'cosseratInSofaFrameNode') self.cosseratCoordinateNode.addChild(cosseratInSofaFrameNode) framesMO = cosseratInSofaFrameNode.addObject( - "MechanicalObject", - template="Rigid3d", - name="FramesMO", - position=framesF, - showObject=1, - showObjectScale=0.001, - ) - if self.beamMass != 0.0: + 'MechanicalObject', template='Rigid3d', name="FramesMO", position=framesF, showObject=0, showObjectScale=0.1) + if self.beamMass != 0.: cosseratInSofaFrameNode.addObject( - "UniformMass", totalMass=self.beamMass, showAxisSizeFactor="0" - ) - - cosseratInSofaFrameNode.addObject( - "DiscreteCosseratMapping", - curv_abs_input=curv_abs_inputS, - curv_abs_output=curv_abs_outputF, - name="cosseratMapping", - input1=self.cosseratCoordinateNode.cosseratCoordinateMO.getLinkPath(), - input2=self.rigidBaseNode.RigidBaseMO.getLinkPath(), - output=framesMO.getLinkPath(), - debug=0, - radius=self.radius.value, - ) + 'UniformMass', totalMass=self.beamMass, showAxisSizeFactor='0') + + cosseratInSofaFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=curv_abs_inputS, + curv_abs_output=curv_abs_outputF, name='cosseratMapping', + input1=self.cosseratCoordinateNode.cosseratCoordinateMO.getLinkPath(), + input2=self.rigidBaseNode.RigidBaseMO.getLinkPath(), + output=framesMO.getLinkPath(), debug=0, radius=self.radius.value) return cosseratInSofaFrameNode def createScene(rootNode): - rootNode.addObject( - "RequiredPlugin", - name="plugins", - pluginName=pluginList, - ) - rootNode.addObject( - "VisualStyle", - displayFlags="showVisualModels showBehaviorModels hideCollisionModels " - "hideBoundingCollisionModels hideForceFields " - "hideInteractionForceFields hideWireframe showMechanicalMappings", - ) - rootNode.findData("dt").value = 0.01 - rootNode.findData("gravity").value = [0.0, -9.81, 0.0] - rootNode.addObject("BackgroundSetting", color="0 0.168627 0.211765") - rootNode.addObject("FreeMotionAnimationLoop") - rootNode.addObject("GenericConstraintSolver", + rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hideForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.findData('dt').value = 0.01 + rootNode.findData('gravity').value = [0., -9.81, 0.] + rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + rootNode.addObject('FreeMotionAnimationLoop') + rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance=1e-5, maxIterations=5e2) - rootNode.addObject("Camera", position="-35 0 280", lookAt="0 0 0") - - solverNode = rootNode.addChild("solverNode") - solverNode.addObject( - "EulerImplicitSolver", rayleighStiffness="0.2", rayleighMass="0.1" - ) - solverNode.addObject( - "SparseLDLSolver", name="solver", template="CompressedRowSparseMatrixd" - ) - solverNode.addObject("GenericConstraintCorrection") - - cosserat = Cosserat( - parent=solverNode, - cosseratGeometry=cosserat_config, - name="cosserat", - radius=0.15, - ) + rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") + + solverNode = rootNode.addChild('solverNode') + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness="0.2", rayleighMass='0.1') + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") + solverNode.addObject('GenericConstraintCorrection') + + cosserat = solverNode.addChild( + Cosserat(parent=solverNode, cosseratGeometry=cosserat_config, name="cosserat", radius=0.15)) # use this to add the collision if the beam will interact with another object - cosserat.addCollisionModel() + collisionModel = cosserat.addCollisionModel() # Attach a force at the beam tip, - # we can attach this force to a non-mechanical node to control the beam in order to be able to drive it. + # we can attach this force to non mechanical node thanks to the MechanicalMatrixMapper component beamFrame = cosserat.cosseratFrame - beamFrame.addObject( - "ConstantForceField", - name="constForce", - showArrowSize=1.0e-2, - indices=12, - forces=[0.0, -100.0, 0.0, 0.0, 0.0, 0.0], - ) + beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-2, indices=12, + forces=[0., -100., 0., 0., 0., 0.]) return rootNode diff --git a/examples/python3/cosserat/nonLinearCosserat.py b/examples/python3/cosserat/nonLinearCosserat.py index 90e3022e..06d0f3e9 100644 --- a/examples/python3/cosserat/nonLinearCosserat.py +++ b/examples/python3/cosserat/nonLinearCosserat.py @@ -51,7 +51,7 @@ class NonLinearCosserat(Sofa.Prefab): Cosserat Mapping // it allow the transfer from the local to the global frame } """ - prefabParameters = [ + properties = [ {'name': 'name', 'type': 'string', 'help': 'Node name', 'default': 'Cosserat'}, {'name': 'position', 'type': 'Rigid3d::VecCoord', 'help': 'Cosserat base position', 'default': [[0., 0., 0., 0, 0, 0, 1.]]}, @@ -67,7 +67,7 @@ class NonLinearCosserat(Sofa.Prefab): 'default': 0.0}, {'name': 'attachingToLink', 'type': 'string', 'help': 'a rest shape force field will constraint the object ' 'to follow arm position', 'default': '1'}, - {'name': 'showObject', 'type': 'string', 'help': ' Draw object arrow ', 'default': '1'}] + {'name': 'showObject', 'type': 'string', 'help': ' Draw object arrow ', 'default': '0'}] def __init__(self, *args, **kwargs): Sofa.Prefab.__init__(self, *args, **kwargs) @@ -193,17 +193,17 @@ def addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): def createScene(rootNode): rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, - ['Sofa.Component.Visual']]) + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' - 'hideBoundingCollisionModels hideForceFields ' + 'hideBoundingCollisionModels hireForceFields ' 'hideInteractionForceFields hideWireframe') - rootNode.addObject('DefaultAnimationLoop') rootNode.findData('dt').value = 0.01 # rootNode.findData('gravity').value = [0., -9.81, 0.] rootNode.findData('gravity').value = [0., 0., 0.] rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') # rootNode.addObject('FreeMotionAnimationLoop') - # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + # rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance=1e-5, maxIterations=5e2) rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") solverNode = rootNode.addChild('solverNode') @@ -213,11 +213,12 @@ def createScene(rootNode): # solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) needCollisionModel = 0 # use this if the collision model if the beam will interact with another object - nonLinearCosserat = NonLinearCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, - useCollisionModel=needCollisionModel, name="cosserat", radius=0.1, legendreControlPoints=initialStrain, order=3) + nonLinearCosserat = solverNode.addChild( + NonLinearCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, + name="cosserat", radius=0.1, legendreControlPoints=initialStrain, order=3)) beamFrame = nonLinearCosserat.cosseratFrame beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-8, indices=12, - forces=[0., 0., 0., 0., 0., 450.]) + force=[0., 0., 0., 0., 0., 450.]) return rootNode diff --git a/examples/python3/tutorial/formation/chiba/Actuator.py b/examples/python3/tutorial/formation/chiba/Actuator.py index b1a52af5..975913a9 100644 --- a/examples/python3/tutorial/formation/chiba/Actuator.py +++ b/examples/python3/tutorial/formation/chiba/Actuator.py @@ -10,7 +10,7 @@ def createScene(rootNode): rootNode.addObject('VisualStyle',displayFlags='showBehavior') rootNode.gravity.value = [-9810, 0, 0] rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance=1e-9, maxIterations=50000) + rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance=1e-9, maxIterations=50000) rootNode.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.01, rayleighMass=0.01) finger = rootNode.addChild('finger') diff --git a/examples/python3/tutorial/formation/chiba/actuator_v1.py b/examples/python3/tutorial/formation/chiba/actuator_v1.py index 22ddca89..b1f6cd57 100644 --- a/examples/python3/tutorial/formation/chiba/actuator_v1.py +++ b/examples/python3/tutorial/formation/chiba/actuator_v1.py @@ -10,7 +10,7 @@ def createScene(root_node): root_node.gravity.value = [-9810, 0, 0] # root_node.addObject('FreeMotionAnimationLoop') - # root_node.addObject('GenericConstraintSolver', tolerance=1e-9, maxIterations=50000) + # root_node.addObject('ProjectedGaussSeidelConstraintSolver', tolerance=1e-9, maxIterations=50000) root_node.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.01, rayleighMass=0.01) finger = root_node.addChild('finger') diff --git a/examples/python3/tutorial/scene_w9991.py b/examples/python3/tutorial/scene_w9991.py index 0a81c81e..2cdf439e 100644 --- a/examples/python3/tutorial/scene_w9991.py +++ b/examples/python3/tutorial/scene_w9991.py @@ -82,7 +82,7 @@ def createScene(root_node): root_node.addObject('FreeMotionAnimationLoop') root_node.addObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="0.0", rayleighMass="0.0") - root_node.addObject('GenericConstraintSolver', maxIterations="500", tolerance="1e-20", computeConstraintForces=1, printLog="0") + root_node.addObject('ProjectedGaussSeidelConstraintSolver', maxIterations="500", tolerance="1e-20", computeConstraintForces=1, printLog="0") root_node.addObject('SparseLDLSolver', name="solver", template="CompressedRowSparseMatrixd") base_node = _add_rigid_base(root_node) diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index cdd2cb70..2318f047 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -78,7 +78,7 @@ def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=F parent_node.addObject('QPInverseProblemSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, multithreading=multithreading, epsilon=1) else: - parent_node.addObject('GenericConstraintSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, + parent_node.addObject('ProjectedGaussSeidelConstraintSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, multithreading=multithreading) if is_contact: diff --git a/examples/python3/wip/completDirectNeedleInsertion.py b/examples/python3/wip/completDirectNeedleInsertion.py index 48a0fabd..9ddbc879 100644 --- a/examples/python3/wip/completDirectNeedleInsertion.py +++ b/examples/python3/wip/completDirectNeedleInsertion.py @@ -17,28 +17,12 @@ from splib3.numerics import Quat sys.path.append('../') -from cosserat.createFemRegularGrid import createFemCube -from cosserat.usefulFunctions import BuildCosseratGeometry, AddPointProcess - -pluginNameList = """Cosserat - Sofa.Component.AnimationLoop - Sofa.Component.Collision.Geometry - Sofa.Component.Constraint.Lagrangian.Correction - Sofa.Component.Constraint.Lagrangian.Solver - Sofa.Component.Engine.Select - Sofa.Component.LinearSolver.Direct - Sofa.Component.Mapping.Linear - Sofa.Component.ODESolver.Backward - Sofa.Component.Setting - Sofa.Component.SolidMechanics.FEM.Elastic - Sofa.Component.SolidMechanics.Spring - Sofa.Component.StateContainer - Sofa.Component.Topology.Container.Dynamic - Sofa.Component.Topology.Container.Grid - Sofa.Component.Topology.Mapping - Sofa.Component.Visual - Sofa.GL.Component.Rendering3D -""" +from createFemRegularGrid import createFemCube +from usefulFunctions import BuildCosseratGeometry, AddPointProcess + +pluginNameList = 'SofaConstraint SofaDeformable SofaImplicitOdeSolver SofaMeshCollision SofaPreconditioner' \ + ' SofaGeneralTopology SofaOpenglVisual SofaGeneralRigid SoftRobots SofaSparseSolver' \ + ' Cosserat SofaBoundaryCondition' class Animation(Sofa.Core.Controller): @@ -87,7 +71,7 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showBehaviorModels hideCollisionModels hideBoundingCollisionModels ' 'showForceFields hideInteractionForceFields showWireframe') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0") + rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0") gravity = [0, 0, 0] rootNode.gravity.value = gravity @@ -98,7 +82,7 @@ def createScene(rootNode): ############### cableNode = rootNode.addChild('cableNode') cableNode.addObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="0.1", rayleighMass='0.1') - cableNode.addObject('EigenSimplicialLDLT', name='solver', template='CompressedRowSparseMatrixd') + cableNode.addObject('SparseLUSolver', name='solver') cableNode.addObject('GenericConstraintCorrection') # ############### diff --git a/examples/python3/wip/needleInteractionTest.py b/examples/python3/wip/needleInteractionTest.py index 2975b56c..ea3dc725 100644 --- a/examples/python3/wip/needleInteractionTest.py +++ b/examples/python3/wip/needleInteractionTest.py @@ -156,7 +156,7 @@ def createScene(rootNode): coneFactor=params.contact.coneFactor, angleCone=0.1) rootNode.addObject('FreeMotionAnimationLoop') - generic = rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", + generic = rootNode.addObject('ProjectedGaussSeidelConstraintSolver', tolerance="1e-20", maxIterations="500", computeConstraintForces=1, printLog="0") gravity = [0, 0, 0] @@ -168,7 +168,7 @@ def createScene(rootNode): ############### solverNode = rootNode.addChild('solverNode') solverNode.addObject('EulerImplicitSolver', - rayleighStiffness=params.Physics.rayleigh_stiffness) + rayleighStiffness=params.Physics.rayleighStiffness) solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") solverNode.addObject('GenericConstraintCorrection') @@ -176,7 +176,7 @@ def createScene(rootNode): needle = solverNode.addChild( Cosserat(parent=solverNode, cosseratGeometry=needleGeometryConfig, radius=params.Geometry.radius, name="needle", youngModulus=params.Physics.youngModulus, poissonRatio=params.Physics.poissonRatio, - rayleighStiffness=params.Physics.rayleigh_stiffness)) + rayleighStiffness=params.Physics.rayleighStiffness)) needleCollisionModel = needle.addPointCollisionModel() slidingPoint = needle.addSlidingPoints() From 3d632755fe2ba9e10a95a819f7783168fff859fb Mon Sep 17 00:00:00 2001 From: Paul Baksic Date: Tue, 26 Aug 2025 16:12:11 +0200 Subject: [PATCH 2/2] Add missing example --- examples/python3/cosserat/CosseratBase.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index 1b27c9b7..70e0f042 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -229,7 +229,7 @@ def createScene(rootNode): rootNode.findData("gravity").value = [0.0, -9.81, 0.0] rootNode.addObject("BackgroundSetting", color="0 0.168627 0.211765") rootNode.addObject("FreeMotionAnimationLoop") - rootNode.addObject("GenericConstraintSolver", tolerance=1e-5, maxIterations=5e2) + rootNode.addObject("ProjectedGaussSeidelConstraintSolver", tolerance=1e-5, maxIterations=5e2) rootNode.addObject("Camera", position="-35 0 280", lookAt="0 0 0") solverNode = rootNode.addChild("solverNode")