Add beam structure with points and controller for cable actuation

Let me know if you'd like to refine this further!

examples/python3/cosserat/CosseratBase.py

@@ -10,15 +10,14 @@
 __date__ = "October, 26 2021"
 
 import Sofa
-from useful.utils import addEdgeCollision, addPointsCollision, _create_rigid_node
-from useful.header import addHeader, addVisual, addSolverNode
+from useful.utils import addEdgeCollision, addPointsCollision, create_rigid_node
+from useful.header import addHeader, addVisual
 from useful.params import Parameters, BeamGeometryParameters
 from useful.geometry import CosseratGeometry, generate_edge_list
 from numpy import array
 from typing import List
 
 
-
 class CosseratBase(Sofa.Prefab):
     """
     CosseratBase model prefab class. It is a prefab class that allow to create a cosserat beam/rod in Sofa.
@@ -54,14 +53,15 @@ class CosseratBase(Sofa.Prefab):
 
     def __init__(self, *args, **kwargs):
         Sofa.Prefab.__init__(self, *args, **kwargs)
-        self.params = kwargs.get(
-            "beam_params", Parameters()
-        )  # Use the Parameters class with default values
+        self.params = kwargs.get("beam_params")  # Use the Parameters class with default values
+
+        self.beamPhysicsParams = self.params.beam_physics_params
+        self.beam_mass = self.beamPhysicsParams.beam_mass  # self.cosseratGeometry['beamMass']
+        self.use_inertia_params = self.beamPhysicsParams.useInertia  # False
+        self.radius = self.beamPhysicsParams.beam_radius  # kwargs.get('radius')
 
-        beamPhysicsParams = self.params.beam_physics_params
-        self.beam_mass = beamPhysicsParams.beam_mass  # self.cosseratGeometry['beamMass']
-        self.use_inertia_params = beamPhysicsParams.useInertia  # False
-        self.radius = beamPhysicsParams.beam_radius  # kwargs.get('radius')
+        print(f' ====> The beam mass is : {self.beam_mass}')
+        print(f' ====> The beam radius is : {self.radius}')
 
         self.solverNode = kwargs.get("parent")
 
@@ -110,11 +110,11 @@ def _addSlidingPointsWithContainer(self):
         return slidingPoint
 
     def _addRigidBaseNode(self):
-        rigidBaseNode = _create_rigid_node(self, "RigidBase",
-                           self.translation, self.rotation)
+        rigidBaseNode = create_rigid_node(self, "RigidBase",
+                                          self.translation, self.rotation)
         return rigidBaseNode
 
-    def _add_cosserat_coordinate(self, initial_curvature: List[float], section_lengths: List[float]) -> None:
+    def _add_cosserat_coordinate(self, initial_curvature: List[float], section_lengths: List[float]):
         """
         Adds a cosserat coordinate node with a BeamHookeLawForceField object to the graph.
 
@@ -161,7 +161,7 @@ def _add_beam_hooke_law_without_inertia(self, cosserat_coordinate_node: None,
             lengthZ=self.params.beam_physics_params.length_Z,
         )
 
-    def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, section_lengths: List[float]) -> None:
+    def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node, section_lengths: List[float]) -> None:
         """
         Adds a BeamHookeLawForceField object to the cosserat coordinate node with inertia parameters.
 
@@ -173,7 +173,7 @@ def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, secti
         GI = self.params.beam_physics_params.GI
         EA = self.params.beam_physics_params.EA
         EI = self.params.beam_physics_params.EI
-        cosseratCoordinateNode.addObject(
+        cosserat_coordinate_node.addObject(
             "BeamHookeLawForceField",
             crossSectionShape=self.params.beam_physics_params.beam_shape,
             length=section_lengths,
@@ -190,7 +190,6 @@ def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, secti
 
     # TODO Rename this here and in `addCosseratCoordinate`
 
-
     def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF):
         cosseratInSofaFrameNode = self.rigidBaseNode.addChild("cosseratInSofaFrameNode")
         self.cosseratCoordinateNode.addChild(cosseratInSofaFrameNode)
@@ -245,16 +244,15 @@ def createScene(rootNode):
     # Create a
     cosserat = solverNode.addChild(CosseratBase(parent=solverNode, beam_params=Params))
     cosserat.rigidBaseNode.addObject(
-            "RestShapeSpringsForceField",
-            name="spring",
-            stiffness=1e8,
-            angularStiffness=1.0e8,
-            external_points=0,
-            # mstate="@RigidBaseMO",
-            points=0,
-            template="Rigid3d"
-        )
-
+        "RestShapeSpringsForceField",
+        name="spring",
+        stiffness=1e8,
+        angularStiffness=1.0e8,
+        external_points=0,
+        # mstate="@RigidBaseMO",
+        points=0,
+        template="Rigid3d"
+    )
 
     # use this to add the collision if the beam will interact with another object
     cosserat.addCollisionModel()

examples/python3/useful/header.py

@@ -18,8 +18,8 @@
 from useful.params import ContactParameters as DefaultContactParams
 
 
-def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=False, is_contact=False, contact_params=None):
-
+def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=False, is_contact=False,
+              contact_params=None):
     """
     Adds default headers for a simulation with contact to the parent node.
     Also adds and returns three nodes: Settings, Modelling, Simulation.
@@ -34,7 +34,7 @@ def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=F
 
     Returns:
         A tuple containing the settings, modelling, and simulation nodes.
-    """    
+    """
 
     settings = parent_node.addChild('Settings')
     settings.addObject('RequiredPlugin', pluginName=[
@@ -65,8 +65,8 @@ def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=F
     settings.addObject('BackgroundSetting', color=[1, 1, 1, 1])
 
     parent_node.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels showCollisionModels '
-                                                     'hideBoundingCollisionModels hideForceFields '
-                                                     'showInteractionForceFields hideWireframe showMechanicalMappings')
+                                                      'hideBoundingCollisionModels hideForceFields '
+                                                      'showInteractionForceFields hideWireframe showMechanicalMappings')
 
     if is_constrained:
         parent_node.addObject('FreeMotionAnimationLoop', parallelCollisionDetectionAndFreeMotion=multithreading,
@@ -85,22 +85,24 @@ def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=F
 
 # components needed for contact modeling
 
-def contactHeader(parent_node, contact_params = DefaultContactParams):
+def contactHeader(parent_node, contact_params=DefaultContactParams):
     """
     Adds components for contact simulation to the parent node.
 
     Args:
         parent_node: The parent node to add the components to.
         contact_params: Optional contact parameters (default: None).
     """
-    
+
     parent_node.addObject('CollisionPipeline')
     parent_node.addObject("DefaultVisualManagerLoop")
     parent_node.addObject('BruteForceBroadPhase')
     parent_node.addObject('BVHNarrowPhase')
 
-    parent_node.addObject('RuleBasedContactManager', responseParams=contact_params.responseParams, response='FrictionContactConstraint')
-    parent_node.addObject('LocalMinDistance', alarmDistance=contact_params.alarmDistance, contactDistance=contact_params.contactDistance)
+    parent_node.addObject('RuleBasedContactManager', responseParams=contact_params.responseParams,
+                          response='FrictionContactConstraint')
+    parent_node.addObject('LocalMinDistance', alarmDistance=contact_params.alarmDistance,
+                          contactDistance=contact_params.contactDistance)
 
 
 def addVisual(node):
@@ -118,7 +120,8 @@ def addVisual(node):
     return node
 
 
-def addSolverNode(parent_node, name='solverNode', template='CompressedRowSparseMatrixd', rayleighMass=0., rayleighStiffness=0.,
+def addSolverNode(parent_node, name='solverNode', template='CompressedRowSparseMatrixd', rayleighMass=0.,
+                  rayleighStiffness=0.,
                   firstOrder=False,
                   iterative=False, isConstrained=False):
     """
@@ -139,7 +142,7 @@ def addSolverNode(parent_node, name='solverNode', template='CompressedRowSparseM
     """
     solver_node = parent_node.addChild(name)
     solver_node.addObject('EulerImplicitSolver', firstOrder=firstOrder, rayleighStiffness=rayleighStiffness,
-                         rayleighMass=rayleighMass)
+                          rayleighMass=rayleighMass)
     if iterative:
         solver_node.addObject('CGLinearSolver', name='Solver', template=template)
     else:
@@ -155,10 +158,10 @@ def addFEMObject(parent_node, path):
 
     # Load a VTK tetrahedral mesh and expose the resulting topology in the scene .
     loader = finger_solver.addObject('MeshVTKLoader', name='loader', filename=f'{path}finger.vtk',
-                                    translation="-17.5 -12.5 7.5",
-                                    rotation="0 180 0")
+                                     translation="-17.5 -12.5 7.5",
+                                     rotation="0 180 0")
     finger_solver.addObject('TetrahedronSetTopologyContainer', position=loader.position.getLinkPath(),
-                           tetras=loader.tetras.getLinkPath(), name='container')
+                            tetras=loader.tetras.getLinkPath(), name='container')
     # Create a MechanicalObject component to stores the DoFs of the model
     finger_solver.addObject('MechanicalObject', template='Vec3', name='dofs')
 
@@ -168,11 +171,11 @@ def addFEMObject(parent_node, path):
     # solved using the Finite Element Method on
     # tetrahedrons.
     finger_solver.addObject('TetrahedronFEMForceField', template='Vec3d', name='forceField', method='large',
-                           poissonRatio='0.45', youngModulus='500')
+                            poissonRatio='0.45', youngModulus='500')
 
     finger_solver.addObject('BoxROI', name='ROI1', box='-18 -15 -8 2 -3 8', drawBoxes='true')
     finger_solver.addObject('RestShapeSpringsForceField',
-                           points='@ROI1.indices', stiffness='1e12')
+                            points='@ROI1.indices', stiffness='1e12')
     ##########################################
     # Cable points                           #
     ##########################################
@@ -182,7 +185,7 @@ def addFEMObject(parent_node, path):
     FEMpos = [" 0.0 0 0 15 0 0 30 0 0 45 0 0 60 0 0 66 0 0 81 0.0 0.0"]
     fem_points = finger_solver.addChild('femPoints')
     fem_points.addObject('MechanicalObject', name="pointsInFEM", position=FEMpos, showObject="1",
-                        showIndices="1")
+                         showIndices="1")
     fem_points.addObject('BarycentricMapping')
 
 

examples/python3/useful/params.py

@@ -20,6 +20,7 @@ def validate(self):
         assert self.nb_frames > 0, "Number of frames must be positive"
         assert self.nb_frames >= self.nb_section, "Number of frames must be positive"
 
+
 @dataclass
 class BeamPhysicsBaseParameters:
     """Base class for Cosserat Beam Physics parameters"""
@@ -36,7 +37,7 @@ class BeamPhysicsBaseParameters:
 
     def validate(self):
         assert self.young_modulus > 0, "Young's modulus must be positive"
-        assert self.poisson_ratio > 0 and self.poisson_ratio < 0.5, "Poisson's ratio must be between 0 and 0.5"
+        assert 0 < self.poisson_ratio < 0.5, "Poisson's ratio must be between 0 and 0.5"
         assert self.beam_mass > 0, "Beam mass must be positive"
         assert self.beam_radius > 0, "Beam radius must be positive"
         assert self.beam_length > 0, "Beam length must be positive"

examples/python3/useful/utils.py

@@ -83,11 +83,11 @@ def computeNegativeAlongXDistanceBetweenPoints(constraintPointPos, slidingPointP
         return 0
 
 
-def _create_rigid_node(parent_node, name, translation, rotation,
-                       positions=[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]):
+def create_rigid_node(parent_node, name, translation, rotation,
+                      positions=[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]):
     rigidBaseNode = parent_node.addChild(name)
 
-    rigidBaseNodeMo = rigidBaseNode.addObject(
+    rigidBaseNode.addObject(
         "MechanicalObject",
         template="Rigid3d",
         name=name+"MO",

tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py

@@ -0,0 +1,86 @@
+# -*- coding: utf-8 -*-
+"""
+    Cosserat class in SofaPython3.
+"""
+
+__authors__ = "adagolodjo"
+__contact__ = "[email protected]"
+__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
+from softrobots.actuators import PullingCable
+import Sofa
+
+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)
+
+
+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")
+    node.addObject('SkinningMapping', nbRef="1", name="skinning_mapping")
+    show_mecha_visual(meca_node, show=_show)
+    return node
+
+
+def show_mecha_visual(node, show=True):
+    node.showObject = show
+    node.showIndices = show
+
+
+class FingerController(Sofa.Core.Controller):
+    def __init__(self, *args, **kwargs):
+        Sofa.Core.Controller.__init__(self, args, kwargs)
+        self.cable = args[0]
+
+    def onKeypressedEvent(self, event):
+        displacement = self.cable.CableConstraint.value[0]
+        if event["key"] == Key.plus:
+            displacement += 1.
+
+        elif event["key"] == Key.minus:
+            displacement -= 1.
+            if displacement < 0:
+                displacement = 0
+        self.cable.CableConstraint.value = [displacement]
+
+
+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
+    cable_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]]
+    add_mecha_points_with_skinng_maps(node_name='mapped_poind', parent_node=frame_node, positions=cable_position)
+
+    return root_node
+
+    PullingCable(frame_node, cableGeometry=cable_position, name="cable")
+
+    return root_node