Skip to content

Commit

Permalink
Refactor CosseratBase and tutorial scenes to update parameter handlin…
Browse files Browse the repository at this point in the history
…g 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.
  • Loading branch information
adagolodjo committed Oct 14, 2024
1 parent a5e9503 commit 21b4cdf
Show file tree
Hide file tree
Showing 2 changed files with 32 additions and 18 deletions.
45 changes: 30 additions & 15 deletions tutorial/tuto_scenes/tuto_4.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,22 +10,25 @@
__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
from splib3.numerics import Quat
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):
Expand All @@ -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
Expand All @@ -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

Expand All @@ -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
Expand All @@ -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)
Expand All @@ -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
5 changes: 2 additions & 3 deletions tutorial/tuto_scenes/tuto_5.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,16 +9,15 @@
__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
from splib3.numerics import Quat
import Sofa
import os
from math import pi
from useful.header import addHeader, addVisual, addSolverNode, Finger
from controller import FingerController
from numpy import array

Expand Down

0 comments on commit 21b4cdf

Please sign in to comment.