From 21b4cdfb55033e66110a961719914d3bbb46312f Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Mon, 14 Oct 2024 18:31:10 +0200 Subject: [PATCH] Refactor CosseratBase and tutorial scenes to update parameter handling and fix typos - Changed 'beam_params' to 'params' in CosseratBase for better clarity. - Updated parameter references in tutorial scenes (tuto_4.py, tuto_5.py) to match new naming conventions. - Refined BeamPhysicsParameters to use NoInertia where applicable. - Fixed typos (e.g., 'rotateFromEuler' comment and variable names like 'nbFrames' to 'nb_frames'). - Added rigid base spring force field in CosseratBase for better beam control. - Organized imports and cleaned up unused variables in the tutorial scenes. --- tutorial/tuto_scenes/tuto_4.py | 45 ++++++++++++++++++++++------------ tutorial/tuto_scenes/tuto_5.py | 5 ++-- 2 files changed, 32 insertions(+), 18 deletions(-) diff --git a/tutorial/tuto_scenes/tuto_4.py b/tutorial/tuto_scenes/tuto_4.py index 9c73a44b..2ea454f1 100644 --- a/tutorial/tuto_scenes/tuto_4.py +++ b/tutorial/tuto_scenes/tuto_4.py @@ -10,7 +10,7 @@ __date__ = "October, 26 2021" from useful.header import addHeader, addSolverNode -from useful.params import BeamPhysicsParameters, BeamGeometryParameters, SimulationParameters +from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters, SimulationParameters from useful.params import Parameters from cosserat.CosseratBase import CosseratBase from math import sqrt @@ -18,14 +18,17 @@ import Sofa from math import pi -geoParams = BeamGeometryParameters(init_pos=[0., 0., 0.], beamLength=30., showFramesObject=1, - nbSection=6, nbFrames=12, buildCollisionModel=0) -physicsParams = BeamPhysicsParameters(beamMass=1., youngModulus=1.0e4, poissonRatio=0.38, beamRadius=0.5, - beamLength=30) +_beam_radius = 0.5 +_beam_length = 30. +_nb_section = 32 +force_null = [0., 0., 0., 0., 0., 0.] # N +geoParams = BeamGeometryParameters(beam_length=30., + nb_section=_nb_section, nb_frames=_nb_section, build_collision_model=0) +physicsParams = BeamPhysicsParametersNoInertia(beam_mass=1., young_modulus=1.0e4, poisson_ratio=0.38, + beam_radius=_beam_radius, beam_length=30) simuParams = SimulationParameters() -Params = Parameters(beamGeoParams=geoParams, beamPhysicsParams=physicsParams, simuParams=simuParams) +Params = Parameters(beam_geo_params=geoParams, beam_physics_params=physicsParams, simu_params=simuParams) -force_null = [0., 0., 0., 0., 0., 0.] # N class ForceController(Sofa.Core.Controller): def __init__(self, *args, **kwargs): @@ -35,7 +38,7 @@ def __init__(self, *args, **kwargs): self.force_type = kwargs['force_type'] self.tip_controller = kwargs['tip_controller'] - self.size = geoParams.nbFrames + self.size = geoParams.nb_frames self.applyForce = True self.forceCoeff = 0. self.theta = 0.1 @@ -60,7 +63,7 @@ def onAnimateEndEvent(self, event): def compute_force(self): with self.forceNode.forces.writeable() as force: - vec = [0., 0., 0., 0., self.forceCoeff/sqrt(2), self.forceCoeff/sqrt(2)] + vec = [0., 0., 0., 0., self.forceCoeff / sqrt(2), self.forceCoeff / sqrt(2)] for i, v in enumerate(vec): force[0][i] = v @@ -82,7 +85,7 @@ def rotate_force(self): last_frame = self.frames.position[self.size] vec = Quat(last_frame[3], last_frame[4], last_frame[5], last_frame[6]) # get the orientation - vec.rotateFromEuler([self.theta, 0., 0.]) # apply rotation arround x-axis + vec.rotateFromEuler([self.theta, 0., 0.]) # apply rotation around x-axis vec.normalize() for i, v in enumerate(vec): position[0][i + 3] = v @@ -95,7 +98,8 @@ def onKeypressedEvent(self, event): self.applyForce = False -controller_type = 1 +controller_type: int = 1 + def createScene(root_node): addHeader(root_node) @@ -105,22 +109,33 @@ def createScene(root_node): # create cosserat Beam cosserat_beam = solver_node.addChild(CosseratBase(parent=solver_node, params=Params)) + cosserat_beam.rigidBaseNode.addObject( + "RestShapeSpringsForceField", + name="spring", + stiffness=1e8, + angularStiffness=1.0e8, + external_points=0, + points=0, + template="Rigid3d" + ) cosserat_frames = cosserat_beam.cosseratFrame # this constance force is used only in the case we are doing force_type 1 or 2 - const_force_node = cosserat_frames.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-8, indices=geoParams.nbFrames, forces=force_null) + const_force_node = cosserat_frames.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-8, + indices=geoParams.nb_frames, forces=force_null) # The effector is used only when force_type is 3 # create a rigid body to control the end effector of the beam tip_controller = root_node.addChild('tip_controller') controller_state = tip_controller.addObject('MechanicalObject', template='Rigid3d', name="controlEndEffector", - showObjectScale=0.3, position=[geoParams.beamLength, 0, 0, 0, 0, 0, 1], + showObjectScale=0.3, position=[geoParams.beam_length, 0, 0, 0, 0, 0, 1], showObject=True) if controller_type == 3: cosserat_frames.addObject('RestShapeSpringsForceField', name='spring', stiffness=0., angularStiffness=1.e8, external_points=0, external_rest_shape=controller_state.getLinkPath(), - points=geoParams.nbFrames, template="Rigid3d") + points=geoParams.nb_frames, template="Rigid3d") - solver_node.addObject(ForceController(forceNode=const_force_node, frame_node=cosserat_frames, force_type=controller_type, tip_controller=controller_state)) + solver_node.addObject(ForceController(forceNode=const_force_node, frame_node=cosserat_frames, + force_type=controller_type, tip_controller=controller_state)) return root_node diff --git a/tutorial/tuto_scenes/tuto_5.py b/tutorial/tuto_scenes/tuto_5.py index 71fdcecc..c6867441 100644 --- a/tutorial/tuto_scenes/tuto_5.py +++ b/tutorial/tuto_scenes/tuto_5.py @@ -9,8 +9,8 @@ __copyright__ = "(c) 2021,Inria" __date__ = "October, 26 2021" -from useful.header import addHeader, addSolverNode, addVisual -from useful.params import BeamPhysicsParametersNoInertia , BeamGeometryParameters, SimulationParameters +from useful.header import addHeader, addSolverNode, Finger +from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters, SimulationParameters from useful.params import Parameters from cosserat.CosseratBase import CosseratBase from math import sqrt @@ -18,7 +18,6 @@ import Sofa import os from math import pi -from useful.header import addHeader, addVisual, addSolverNode, Finger from controller import FingerController from numpy import array