From ef81e005ea756e7d12658dadaa529f8776632567 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Thu, 3 Aug 2023 01:01:08 +0200 Subject: [PATCH] Revamped ISIR robot scene with enhanced functionality. The discs are now seamlessly attached to the central rod of the robot using the RigidMapping. The scene has been updated to utilize the newly implemented python class for cosserat beams, , which takes advantage of dataclasses for improved data handling and organization. --- examples/python3/actuators/example1.py | 60 ++++++++------------- examples/python3/cosserat/CosseratBase.py | 26 +++++---- examples/python3/cosserat/cosseratObject.py | 2 +- examples/python3/useful/params.py | 2 + 4 files changed, 42 insertions(+), 48 deletions(-) diff --git a/examples/python3/actuators/example1.py b/examples/python3/actuators/example1.py index 1dffd2a1..22ed2f97 100644 --- a/examples/python3/actuators/example1.py +++ b/examples/python3/actuators/example1.py @@ -1,18 +1,15 @@ # -*- coding: utf-8 -*- """ - Cosserat class in SofaPython3. + Scene robot ISIR. """ - - __authors__ = "Yinoussa" __contact__ = "adagolodjo@protonmail.com" __version__ = "1.0.0" __copyright__ = "(c) 2021,Inria" __date__ = "July, 20 2023" -from useful.header import addHeader +from useful.header import addHeader, addSolverNode from useful.params import BeamPhysicsParameters, BeamGeometryParameters, SimulationParameters -from cosserat.cosseratObject import Cosserat from useful.params import Parameters from cosserat.CosseratBase import CosseratBase @@ -47,23 +44,19 @@ # [@info] ================ Unit: N, cm, g, Pa ================ """ - - -# @todo use CableGeometryParameters & CableParameters in Cosserat class for both geometry and physics parameters -geoParams = BeamGeometryParameters(init_pos=[0., 0., 0.], beamLength=65.5, - nbSection=5, nbFrames=30, buildCollisionModel=0) -physicsParams = BeamPhysicsParameters(beamMass=1., youngModulus= 1.0e8, poissonRatio=0.38, beamRadius=6.2e-1/2., +# @todo +geoParams = BeamGeometryParameters(init_pos=[0., 0., 0.], beamLength=65.5, showFramesObject=1, + nbSection=5, nbFrames=26, buildCollisionModel=0) +physicsParams = BeamPhysicsParameters(beamMass=5., youngModulus= 1.0e8, poissonRatio=0.38, beamRadius=6.2e-1/2., beamLength=65.5) simuParams = SimulationParameters(rayleighStiffness=1.e-3) - -isCollisionModel = 0 - +Params = Parameters(beamGeoParams=geoParams, beamPhysicsParams=physicsParams, simuParams=simuParams) -def createIntermediateNode(parent, rigidCentral=None, baseName="rigidState"): +def createIntermediateNode(parent, rigidCentral=None, baseName="rigidState",rigidIndex=2): """ The intermediateRigid construction """ if rigidCentral is None: - rigidCentral = [3, 0., 0, 0, 0, 0, 1] + rigidCentral = [0, 0., 0, 0, 0, 0, 1] q0 = Quat() q1 = Quat() q2 = Quat() @@ -74,15 +67,15 @@ def createIntermediateNode(parent, rigidCentral=None, baseName="rigidState"): intermediateRigid = parent.addChild('intermediateRigid') intermediateRigid.addObject('MechanicalObject', name=baseName, template='Rigid3d', position=rigidCentral, showObject=True, showObjectScale=0.4) - intermediateRigid.addObject('RestShapeSpringsForceField', name='rigid', stiffness=1.e1, template="Rigid3d", - angularStiffness=1.e1, external_points=0, points=0) + # @TODO add the mass of the disk + intermediateRigid.addObject('RigidMapping', name="interRigidMap", index=rigidIndex) + """Create intermediate kids""" interRigidChild = intermediateRigid.addChild('interRigidChild') interRigidChild.addObject('MechanicalObject', name="interRigidChildMo", template='Rigid3d', position=interChildPos, showObject=True, showObjectScale=0.4) interRigidChild.addObject('RigidMapping', name="interRigidMap") - """ Add disk to the scene, actually this disk is only use for the visualisation """ loadDisk(intermediateRigid) @@ -96,34 +89,27 @@ def loadDisk(parentNode): diskMapping.addObject('RigidMapping', name="diskMap") -def createRigidDisk(rootNode, length=65.5): # @todo add a parameter for the number of disk - diskSolverNode = rootNode.addChild('diskSolverNode') - diskSolverNode.addObject('EulerImplicitSolver', firstOrder=0, rayleighStiffness=0.01, rayleighMass=0.) - diskSolverNode.addObject('SparseLDLSolver', name='solver') - +def createRigidDisk(parentNode): # @todo add a parameter for the number of disk + """ Create the rigid disk nodes """ for i in range(1, 14): """ Create the rigid disk nodes """ - createIntermediateNode(diskSolverNode, rigidCentral=[i*(length/13.), 0., 0, 0, 0, 0, 1], baseName=f"rigidState{i}") - - # """ The intermediateRigid construction """ - # intermediateRigid = createIntermediateNode(diskSolverNode) - return diskSolverNode - + createIntermediateNode(parentNode, rigidCentral=[0, 0., 0, 0, 0, 0, 1], + baseName=f"rigidState{i}", rigidIndex=i*2) -Params = Parameters(beamGeoParams=geoParams, beamPhysicsParams=physicsParams, simuParams=simuParams) def createScene(rootNode): addHeader(rootNode) rootNode.gravity = [0., -9.81, 0.] - stemNode = rootNode.addChild('stemNode') - stemNode.addChild(CosseratBase(parent=stemNode, params=Params)) - # Cosserat(parent=stemNode, cosseratGeometry=nonLinearConfig, inertialParams=inertialParams, radius=Rb, - # useCollisionModel=isCollisionModel, name="cosserat", youngModulus=YM, poissonRatio=PR, - # rayleighStiffness=rayleighStiffness)) + stemNode = addSolverNode(rootNode, name="stemNode") + cosserat = stemNode.addChild(CosseratBase(parent=stemNode, params=Params)) # create the rigid disk node - # createRigidDisk(rootNode) + createRigidDisk(cosserat.cosseratFrame) + return + + + diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index af283793..88865af9 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -144,15 +144,19 @@ def _addCosseratCoordinate(self, bendingStates, listOfSectionsLength): # 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, useInertiaParams=True, - GI=GI, GA=GA, EI=EI, EA=EA, rayleighStiffness=self.rayleighStiffness.value, - lengthY=self.length_Y.value, lengthZ=self.length_Z.value) + GA = self.params.beamPhysicsParams.GA + GI = self.params.beamPhysicsParams.GI + EA = self.params.beamPhysicsParams.EA + EI = self.params.beamPhysicsParams.EI + cosseratCoordinateNode.addObject('BeamHookeLawForceField', + crossSectionShape=self.params.beamPhysicsParams.beamShape, + length=listOfSectionsLength, + radius=self.params.beamPhysicsParams.beamRadius, + useInertiaParams=True, + GI=GI, GA=GA, EI=EI, EA=EA, + rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.params.beamPhysicsParams.length_Y, + lengthZ=self.params.beamPhysicsParams.length_Z) def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): @@ -160,7 +164,9 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): 'cosseratInSofaFrameNode') self.cosseratCoordinateNode.addChild(cosseratInSofaFrameNode) framesMO = cosseratInSofaFrameNode.addObject( - 'MechanicalObject', template='Rigid3d', name="FramesMO", position=framesF, showObject=1, showObjectScale=0.001) + 'MechanicalObject', template='Rigid3d', name="FramesMO", position=framesF, + showIndices=self.params.beamGeoParams.showFramesObject, + showObject=self.params.beamGeoParams.showFramesObject, showObjectScale=0.1) if self.beamMass != 0.: cosseratInSofaFrameNode.addObject( 'UniformMass', totalMass=self.beamMass, showAxisSizeFactor='0') diff --git a/examples/python3/cosserat/cosseratObject.py b/examples/python3/cosserat/cosseratObject.py index 6f353960..ec97050b 100644 --- a/examples/python3/cosserat/cosseratObject.py +++ b/examples/python3/cosserat/cosseratObject.py @@ -11,7 +11,7 @@ import Sofa from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry -from cosserat.utils import addEdgeCollision, addPointsCollision +from useful.utils import addEdgeCollision, addPointsCollision cosserat_config = {'init_pos': [0., 0., 0.], 'tot_length': 6, 'nbSectionS': 6, 'nbFramesF': 12, 'buildCollisionModel': 1, 'beamMass': 0.22} diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index a9ac6f69..cbc1aada 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -12,6 +12,8 @@ class BeamGeometryParameters: nbFrames: int = 30 # number of frames along the beam buildCollisionModel: int = 0 init_pos: List[float] = field(default_factory=lambda: [0., 0., 0.]) # The beam rigid base position as a list [x, y, z] + + """Parameters for the visualisation of the beam""" showFramesObject: int = 1 showRigidBaseObject: int = 1