From a5e9503d920d91f05d2ec505356fb86fb39d4b47 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 9 Oct 2024 23:05:17 +0200 Subject: [PATCH] adding new scens Refactor event key handling and add new cable-driven Cosserat beam scene - Updated key event handling in geo_cable_driven_cosserat_beam.py to use string-based key comparison for + and - keys. - Fixed minor formatting issue by adding missing comma in RestShapeSpringsForceField. - Added new scene file geo_cosserat_cable_driven_cosserat_beam.py for simulating cable-driven Cosserat beams, with detailed parameter definitions for beam geometry and physics. - Introduced PullingCosseratCable controller in pulling_cosserat_cable.py to handle cable pulling mechanics. - Replaced BeamPhysicsParameters with BeamPhysicsParametersNoInertia in tuto_5.py for more specific physical simulations. --- .../geo_cable_driven_cosserat_beam.py | 6 +- ...geo_cosserat_cable_driven_cosserat_beam.py | 129 ++++++++++++++++++ tutorial/tuto_scenes/git_msg.md | 7 + .../tuto_scenes/pulling_cosserat_cable.py | 46 +++++++ tutorial/tuto_scenes/tuto_5.py | 6 +- 5 files changed, 188 insertions(+), 6 deletions(-) create mode 100644 tutorial/tuto_scenes/geo_cosserat_cable_driven_cosserat_beam.py create mode 100644 tutorial/tuto_scenes/git_msg.md create mode 100644 tutorial/tuto_scenes/pulling_cosserat_cable.py diff --git a/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py b/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py index 6ca71f6..d4bebc9 100644 --- a/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py +++ b/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py @@ -45,10 +45,10 @@ def __init__(self, *args, **kwargs): def onKeypressedEvent(self, event): displacement = self.cable.CableConstraint.value[0] - if event["key"] == Key.plus: + if event["key"] == "+": displacement += 1. - elif event["key"] == Key.minus: + elif event["key"] == "-": displacement -= 1. if displacement < 0: displacement = 0 @@ -68,7 +68,7 @@ def createScene(root_node): "RestShapeSpringsForceField", name="spring", stiffness=1e8, - angularStiffness=1.0e8 + angularStiffness=1.0e8, external_points=0, points=0, template="Rigid3d" diff --git a/tutorial/tuto_scenes/geo_cosserat_cable_driven_cosserat_beam.py b/tutorial/tuto_scenes/geo_cosserat_cable_driven_cosserat_beam.py new file mode 100644 index 0000000..a7e28c8 --- /dev/null +++ b/tutorial/tuto_scenes/geo_cosserat_cable_driven_cosserat_beam.py @@ -0,0 +1,129 @@ +# -*- coding: utf-8 -*- +""" + Cosserat class in SofaPython3. +""" + +__authors__ = "adagolodjo" +__contact__ = "adagolodjo@protonmail.com" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "October, 09 2024" + +from useful.header import addHeader, addSolverNode +from useful.params import Parameters +from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters, \ + ContactParameters +from cosserat.CosseratBase import CosseratBase +import Sofa + +# Define the beam parameters +beam_length = 1. +geoParams = BeamGeometryParameters(beam_length=beam_length, nb_section=12, nb_frames=12, build_collision_model=0) +physicsParams = BeamPhysicsParametersNoInertia(beam_mass=0.03, young_modulus=1.0e5, poisson_ratio=0.4, beam_radius=0.04, + beam_length=beam_length) +contactParams = ContactParameters() +Params = Parameters(beam_geo_params=geoParams, beam_physics_params=physicsParams, contact_params=contactParams) + +# Define Cable parameters +cable_length = 1.2 +cable_geo_params = BeamGeometryParameters(beam_length=cable_length, nb_section=16, nb_frames=16, + build_collision_model=1) +cable_physics_params = BeamPhysicsParametersNoInertia(beam_mass=0.03, young_modulus=1.0e7, poisson_ratio=0.4, + beam_radius=0.005, beam_length=cable_length) +cable_params = Parameters(beam_geo_params=cable_geo_params, beam_physics_params=cable_physics_params) + +def add_mecha_points_with_skinng_maps(node_name, parent_node, positions, _show=True): + node = parent_node.addChild(node_name) + meca_node = node.addObject('MechanicalObject', position=positions, template="Vec3d", name=node_name+"_mo") + node.addObject('SkinningMapping', nbRef="1", name="skinning_mapping") + return meca_node + + +def show_mecha_visual(node, show=True): + node.showObject = show + node.showIndices = show + + +class CableController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, args, kwargs) + self.cable = args[0] + self.move = True + + def onAnimateEndEvent(self, event): + if self.move: + self.pull_cable() + + def pull_cable(self): + with self.cable.rest_position.writeable() as pos: + pos[0][0] -= 0.01 + + def onKeypressedEvent(self, event): + if event["key"] == "+": + self.move = True + elif event["key"] == "-": + self.move -= False + + +def createScene(root_node): + + addHeader(root_node, is_constrained=1, is_contact=1, contact_params=Params) + root_node.gravity = [0, -9.81, 0.] + solver_node = addSolverNode(root_node, name="solver_node") + + solver_node.addObject('GenericConstraintCorrection') + # create cosserat Beam + beam = solver_node.addChild(CosseratBase(parent=solver_node, beam_params=Params)) + # Attach beam base using a spring force field + beam.rigidBaseNode.addObject( + "RestShapeSpringsForceField", + name="spring", + stiffness=1e8, + angularStiffness=1.0e8, + external_points=0, + points=0, + template="Rigid3d" + ) + + # add points inside the beam + frame_node = beam.rigidBaseNode.cosseratInSofaFrameNode + sliding_position = [[0, 0, 0.02], [0.2, 0, 0.02], [0.4, 0, 0.02], [0.6, 0, 0.02], [0.8, 0, 0.02], [1, 0, 0.02]] + sliding_mo = add_mecha_points_with_skinng_maps(node_name='mapped_poind', parent_node=frame_node, positions=sliding_position) + + # Add cable to the scene + cable_solver = addSolverNode(root_node, name="cable_solver") + cable_node = cable_solver.addChild(CosseratBase(parent=cable_solver, translation=[-0.2, 0, 0.02], + beam_params=cable_params, name="cable")) + cable_solver.addObject('GenericConstraintCorrection') + cable_frames_node = cable_node.cosseratFrame + + # This creates a new node in the scene. This node is appended to the finger's node. + cable_state_node = cable_frames_node.addChild('cable_state_node') + + # This creates a MechanicalObject, a component holding the degree of freedom of our + # mechanical modelling. In the case of a cable it is a set of positions specifying + # the points where the cable is passing by. + cable_state = cable_state_node.addObject('MechanicalObject', name="cablePos", + position=cable_node.frames3D) + show_mecha_visual(cable_state, show=True) + cable_state_node.addObject('IdentityMapping') + + """ These positions are in fact the distance between the 3D points mapped inside the Beam and the cable points""" + distance_node = cable_state_node.addChild('distance_node') + beam.cosseratFrame.addChild(distance_node) + + distance = distance_node.addObject('MechanicalObject', template='Vec3d', position=sliding_position, + name="distancePointsMO", showObject='1', showObjectScale='1') + + """The controller of the cable is added to the scene""" + # cable_state_node.addObject(CableController(cable_node.rigidBaseNode.RigidBaseMO)) + + inputCableMO = cable_state.getLinkPath() + sliding_points = sliding_mo.getLinkPath() + outputPointMO = distance.getLinkPath() + """ This constraint is used to compute the distance between the cable and the fem points""" + distance_node.addObject('QPSlidingConstraint', name="QPConstraint") + distance_node.addObject('DifferenceMultiMapping', name="pointsMulti", input1=sliding_points, indices="5", + input2=inputCableMO, output=outputPointMO, direction="@../../FramesMO.position") + + return root_node \ No newline at end of file diff --git a/tutorial/tuto_scenes/git_msg.md b/tutorial/tuto_scenes/git_msg.md new file mode 100644 index 0000000..edffbc0 --- /dev/null +++ b/tutorial/tuto_scenes/git_msg.md @@ -0,0 +1,7 @@ +Refactor event key handling and add new cable-driven Cosserat beam scene + +- Updated key event handling in geo_cable_driven_cosserat_beam.py to use string-based key comparison for + and - keys. +- Fixed minor formatting issue by adding missing comma in RestShapeSpringsForceField. +- Added new scene file geo_cosserat_cable_driven_cosserat_beam.py for simulating cable-driven Cosserat beams, with detailed parameter definitions for beam geometry and physics. +- Introduced PullingCosseratCable controller in pulling_cosserat_cable.py to handle cable pulling mechanics. +- Replaced BeamPhysicsParameters with BeamPhysicsParametersNoInertia in tuto_5.py for more specific physical simulations. diff --git a/tutorial/tuto_scenes/pulling_cosserat_cable.py b/tutorial/tuto_scenes/pulling_cosserat_cable.py new file mode 100644 index 0000000..da655b1 --- /dev/null +++ b/tutorial/tuto_scenes/pulling_cosserat_cable.py @@ -0,0 +1,46 @@ +import Sofa.Core +from splib3.numerics import Quat + + +class PullingCosseratCable(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.cable = kwargs['frame_node'].FramesMO + self.tip_controller = kwargs['tip_controller'] + + def onAnimateEndEvent(self, event): + pass + + def pull_cable(self): + with self.cable.restPosition.writeable() as pos: + pos[0][0] -= self.rate + + def compute_orthogonal_force(self): + position = self.frames.position[self.size] # get the last rigid of the cosserat frame + orientation = Quat(position[3], position[4], position[5], position[6]) # get the orientation + # Calculate the direction of the force in order to remain orthogonal to the x axis of the last frame of the beam. + with self.forceNode.forces.writeable() as force: + vec = orientation.rotate([0., self.forceCoeff * 5.e-2, 0.]) + vec.normalize() + # print(f' The new vec is : {vec}') + for count in range(3): + force[0][count] = vec[count] + + def rotate_force(self): + if self.forceCoeff <= 100. * pi: + with self.tip_controller.position.writeable() as position: + 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.normalize() + for i, v in enumerate(vec): + position[0][i + 3] = v + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "+": + self.applyForce = True + elif key == "-": + self.applyForce = False + diff --git a/tutorial/tuto_scenes/tuto_5.py b/tutorial/tuto_scenes/tuto_5.py index 32a5617..71fdcec 100644 --- a/tutorial/tuto_scenes/tuto_5.py +++ b/tutorial/tuto_scenes/tuto_5.py @@ -10,7 +10,7 @@ __date__ = "October, 26 2021" from useful.header import addHeader, addSolverNode, addVisual -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 @@ -27,7 +27,7 @@ geoParams = BeamGeometryParameters(init_pos=[0., 0., 0.], beamLength=81., showFramesObject=1, nbSection=12, nbFrames=32, buildCollisionModel=0) -physicsParams = BeamPhysicsParameters(beamMass=1., youngModulus=5.e6, poissonRatio=0.4, beamRadius=0.5, +physicsParams = BeamPhysicsParametersNoInertia(beamMass=1., youngModulus=5.e6, poissonRatio=0.4, beamRadius=0.5, beamLength=30) simuParams = SimulationParameters() @@ -119,7 +119,7 @@ def createScene(root_node): # This creates a new node in the scene. This node is appended to the finger's node. cable_state_node = cosserat_frames_node.addChild('cable_state_node') - # This creates a MechanicalObject, a componant holding the degree of freedom of our + # This creates a MechanicalObject, a componante holding the degree of freedom of our # mechanical modelling. In the case of a cable it is a set of positions specifying # the points where the cable is passing by. cable_state = cable_state_node.addObject('MechanicalObject', name="cablePos",