From 64467b277ee81c8f7acf2b22e2da3fd1cb2f2fc1 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Thu, 22 Aug 2024 16:29:12 +0200 Subject: [PATCH 01/22] Resolve remaining bugs related to SoftRobot plugin compilation issues. --- src/Cosserat/constraint/CosseratActuatorConstraint.h | 3 ++- src/Cosserat/constraint/QPSlidingConstraint.h | 7 ++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/Cosserat/constraint/CosseratActuatorConstraint.h b/src/Cosserat/constraint/CosseratActuatorConstraint.h index fae4331d..d2283522 100644 --- a/src/Cosserat/constraint/CosseratActuatorConstraint.h +++ b/src/Cosserat/constraint/CosseratActuatorConstraint.h @@ -47,6 +47,7 @@ using sofa::core::ConstraintParams; using sofa::linearalgebra::BaseVector; using sofa::core::visual::VisualParams ; using sofa::core::behavior::ConstraintResolution; +using softrobots::constraint::CableModel; class MyCableForceConstraintResolution : public ConstraintResolution @@ -114,7 +115,7 @@ class MyCableForceConstraintResolution : public ConstraintResolution * */ template< class DataTypes > -class CosseratActuatorConstraint : public softrobots::constraint::CableModel +class CosseratActuatorConstraint : public CableModel { public: SOFA_CLASS(SOFA_TEMPLATE(CosseratActuatorConstraint,DataTypes), SOFA_TEMPLATE(CableModel,DataTypes)); diff --git a/src/Cosserat/constraint/QPSlidingConstraint.h b/src/Cosserat/constraint/QPSlidingConstraint.h index f04ea296..0531371a 100644 --- a/src/Cosserat/constraint/QPSlidingConstraint.h +++ b/src/Cosserat/constraint/QPSlidingConstraint.h @@ -48,6 +48,7 @@ using sofa::linearalgebra::BaseVector ; using sofa::core::ConstraintParams ; using sofa::helper::ReadAccessor ; using sofa::core::VecCoordId ; +using softrobots::constraint::CableModel; using sofa::core::behavior::ConstraintResolution ; //using sofa::component::constraint::lagrangian::model::ConstraintResolution; @@ -145,7 +146,7 @@ class QPSlidingConstraint : public softrobots::constraint::CableModel /// Bring m_state in the current lookup context. /// otherwise any access to the base::attribute would require /// using the "this->" approach. - using SoftRobotsConstraint::m_state ; + using softrobots::constraint::SoftRobotsConstraint::m_state ; ////////////////////////// Inherited attributes //////////////////////////// /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html /// Bring inherited attributes and function in the current lookup context. @@ -160,8 +161,8 @@ class QPSlidingConstraint : public softrobots::constraint::CableModel using CableModel::d_componentState ; //////////////////////////////////////////////////////////////////////////// /// \brief internalInit - using SoftRobotsConstraint::m_nbLines ; - using SoftRobotsConstraint::m_constraintIndex ; + using softrobots::constraint::SoftRobotsConstraint::m_nbLines ; + using softrobots::constraint::SoftRobotsConstraint::m_constraintIndex ; void internalInit(); }; From 79547ec0a0a468567bb85778dc0052c15ee245e1 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Thu, 5 Sep 2024 14:11:12 +0200 Subject: [PATCH 02/22] fix bugs due to Sofa Update --- src/Cosserat/constraint/CosseratActuatorConstraint.h | 3 ++- src/Cosserat/constraint/CosseratActuatorConstraint.inl | 8 ++++---- src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h | 2 +- .../constraint/CosseratNeedleSlidingConstraint.inl | 6 +++--- src/Cosserat/constraint/QPSlidingConstraint.h | 2 +- src/Cosserat/constraint/QPSlidingConstraint.inl | 6 +++--- 6 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/Cosserat/constraint/CosseratActuatorConstraint.h b/src/Cosserat/constraint/CosseratActuatorConstraint.h index d2283522..b2794522 100644 --- a/src/Cosserat/constraint/CosseratActuatorConstraint.h +++ b/src/Cosserat/constraint/CosseratActuatorConstraint.h @@ -175,7 +175,8 @@ class CosseratActuatorConstraint : public CableModel using CableModel::d_displacement ; using CableModel::d_componentState ; using CableModel::m_nbLines ; - using CableModel::m_constraintIndex ; + using CableModel::d_constraintIndex ; + using CableModel::m_state ; using CableModel::d_indices ; using CableModel::d_pullPoint; diff --git a/src/Cosserat/constraint/CosseratActuatorConstraint.inl b/src/Cosserat/constraint/CosseratActuatorConstraint.inl index 218e9967..b1ffb8b0 100644 --- a/src/Cosserat/constraint/CosseratActuatorConstraint.inl +++ b/src/Cosserat/constraint/CosseratActuatorConstraint.inl @@ -104,8 +104,8 @@ void CosseratActuatorConstraint::buildConstraintMatrix(const Constrai SOFA_UNUSED(cParams); - m_constraintIndex.setValue(cIndex); - const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); + d_constraintIndex.setValue(cIndex); + const auto& constraintIndex = sofa::helper::getReadAccessor(d_constraintIndex); MatrixDeriv& matrix = *cMatrix.beginEdit(); @@ -167,9 +167,9 @@ void CosseratActuatorConstraint::getConstraintViolation(const Constra Real dfree = Jdx->element(0) + d_cableInitialLength.getValue() - d_cableLength.getValue(); - + auto & constraintIndex = d_constraintIndex.getValue(); for (unsigned i=0;iset(m_constraintIndex.getValue(), dfree); + resV->set(constraintIndex, dfree); } } diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h index 3eec7ac4..81298b6f 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h @@ -136,7 +136,7 @@ namespace sofa::component::constraintset //////////////////////////////////////////////////////////////////////////// /// \brief internalInit unsigned m_nbLines ; - using Constraint::m_constraintIndex ; + using Constraint::d_constraintIndex; void internalInit(); }; diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl index d579bbb8..bd737d65 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl @@ -113,7 +113,7 @@ namespace sofa::component::constraintset SOFA_UNUSED(cParams); MatrixDeriv &matrix = *cMatrix.beginEdit(); VecCoord positions = x.getValue(); - m_constraintIndex.setValue(cIndex); + d_constraintIndex.setValue(cIndex); type::Vec<3, bool> use = d_useDirections.getValue(); @@ -131,7 +131,7 @@ namespace sofa::component::constraintset } } cMatrix.endEdit(); - m_nbLines = cIndex - m_constraintIndex.getValue(); + m_nbLines = cIndex - d_constraintIndex.getValue(); } @@ -151,7 +151,7 @@ namespace sofa::component::constraintset //ReadAccessor> positions = this->mstate->readPositions(); const VecCoord positions = x.getValue(); type::Vec<3, bool> use = d_useDirections.getValue(); - const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); + auto& constraintIndex = d_constraintIndex.getValue(); for (unsigned int i = 0; i < positions.size(); i++) { diff --git a/src/Cosserat/constraint/QPSlidingConstraint.h b/src/Cosserat/constraint/QPSlidingConstraint.h index 0531371a..88316ef0 100644 --- a/src/Cosserat/constraint/QPSlidingConstraint.h +++ b/src/Cosserat/constraint/QPSlidingConstraint.h @@ -162,7 +162,7 @@ class QPSlidingConstraint : public softrobots::constraint::CableModel //////////////////////////////////////////////////////////////////////////// /// \brief internalInit using softrobots::constraint::SoftRobotsConstraint::m_nbLines ; - using softrobots::constraint::SoftRobotsConstraint::m_constraintIndex ; + using softrobots::constraint::SoftRobotsConstraint::d_constraintIndex; void internalInit(); }; diff --git a/src/Cosserat/constraint/QPSlidingConstraint.inl b/src/Cosserat/constraint/QPSlidingConstraint.inl index 60a00a61..113b3dc1 100644 --- a/src/Cosserat/constraint/QPSlidingConstraint.inl +++ b/src/Cosserat/constraint/QPSlidingConstraint.inl @@ -118,8 +118,8 @@ void QPSlidingConstraint::buildConstraintMatrix(const ConstraintParam SOFA_UNUSED(cParams); MatrixDeriv& matrix = *cMatrix.beginEdit(); VecCoord positions = x.getValue(); - m_constraintIndex.setValue(cIndex); - const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); + d_constraintIndex.setValue(cIndex); + const auto& constraintIndex = sofa::helper::getReadAccessor(d_constraintIndex); for (unsigned int i=0; i::getConstraintViolation(const ConstraintPara SOFA_UNUSED(cParams); ReadAccessor> positions = m_state->readPositions(); - const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); + const auto& constraintIndex = sofa::helper::getReadAccessor(d_constraintIndex); if(Jdx->size()==0){ //std::cout << "Size JDX = "<< Jdx->size() << std::endl; From bd2d47eca1d245eaf6a16877505c5fdec767a942 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Thu, 5 Sep 2024 14:25:23 +0200 Subject: [PATCH 03/22] fix bugs due to Sofa Update --- src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h | 2 +- src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h index 81298b6f..a3ceaabb 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h @@ -135,7 +135,7 @@ namespace sofa::component::constraintset using Constraint::d_componentState; //////////////////////////////////////////////////////////////////////////// /// \brief internalInit - unsigned m_nbLines ; + unsigned m_nbLines{} ; using Constraint::d_constraintIndex; void internalInit(); diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl index bd737d65..b4f05398 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl @@ -67,8 +67,7 @@ namespace sofa::component::constraintset template CosseratNeedleSlidingConstraint::~CosseratNeedleSlidingConstraint() - { - } + = default; template void CosseratNeedleSlidingConstraint::init() From ef0220c6efe17545929e340d6a2b6cb2b9b10256 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 6 Sep 2024 19:34:15 +0200 Subject: [PATCH 04/22] Refactor Cosserat prefab to remove visualization parameters and simplify object creation --- examples/python3/cosserat/CosseratBase.py | 25 +++++------------------ examples/python3/useful/params.py | 6 ------ 2 files changed, 5 insertions(+), 26 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index d0948d82..4bef3a55 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -54,13 +54,7 @@ class CosseratBase(Sofa.Prefab): "help": "a rest shape force field will constraint the object " "to follow arm position", "default": "1", - }, - { - "name": "showObject", - "type": "string", - "help": " Draw object arrow ", - "default": "0", - }, + } ] def __init__(self, *args, **kwargs): @@ -117,8 +111,7 @@ def _addPointCollisionModel(self, nodeName="CollisionPoints"): def _addSlidingPoints(self): slidingPoint = self.cosseratFrame.addChild("slidingPoint") - slidingPoint.addObject("MechanicalObject", name="slidingPointMO", position=self.frames3D, - showObject="0", showIndices="0") + slidingPoint.addObject("MechanicalObject", name="slidingPointMO", position=self.frames3D) slidingPoint.addObject("IdentityMapping") return slidingPoint @@ -129,9 +122,7 @@ def _addSlidingPointsWithContainer(self): slidingPoint.addObject( "MechanicalObject", name="slidingPointMO", - position=self.frames3D, - showObject="1", - showIndices="0", + position=self.frames3D ) slidingPoint.addObject("IdentityMapping") return slidingPoint @@ -145,12 +136,10 @@ def _addRigidBaseNode(self): "MechanicalObject", template="Rigid3d", name="RigidBaseMO", - showObjectScale=0.2, position=positions, translation=self.translation, rotation=self.rotation ) - rigidBaseNodeMo.showObject.setParent(self.showObject) # @TODO: remove this hard coded. # one can choose to set this to false and directly attach the beam base @@ -175,8 +164,7 @@ def _addCosseratCoordinate(self, bendingStates, listOfSectionsLength): "MechanicalObject", template="Vec3d", name="cosseratCoordinateMO", - position=bendingStates, - showIndices=0, + position=bendingStates ) if self.useInertiaParams is False: @@ -227,10 +215,7 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): "MechanicalObject", template="Rigid3d", name="FramesMO", - position=framesF, - showIndices=self.params.beamGeoParams.show_frames_indices, - showObject=self.params.beamGeoParams.show_frames_object, - showObjectScale=1.8, # Todo: remove this hard code + position=framesF ) cosseratInSofaFrameNode.addObject( diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index 25e4805b..7cb01c1b 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -17,12 +17,6 @@ class BeamGeometryParameters: default_factory=lambda: [0.0, 0.0, 0.0] ) # The beam rigid base position as a list [x, y, z] - """Parameters for the visualisation of the beam""" - show_frames_object: bool = True - show_frames_indices: bool = False - showRigidBaseObject: int = 1 - - #Todo: two objects from the same base class to define different instead of useInertia @dataclass class BeamPhysicsParameters: From 1a439c82a1a5f621a64c26050aac1445808ffabf Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 6 Sep 2024 20:28:18 +0200 Subject: [PATCH 05/22] -----> qwen2 Simplify BeamGeometryParameters by removing unnecessary attributes and defaults. --- examples/python3/cosserat/CosseratBase.py | 65 ++++++++++------------- examples/python3/useful/geometry.py | 32 ++++++----- examples/python3/useful/params.py | 3 -- 3 files changed, 44 insertions(+), 56 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index 4bef3a55..a7e3b94e 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -17,6 +17,20 @@ from numpy import array +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( + "MechanicalObject", + template="Rigid3d", + name=name+"MO", + position=positions, + translation=translation, + rotation=rotation + ) + return rigidBaseNode + class CosseratBase(Sofa.Prefab): """ CosseratBase model prefab class. It is a prefab class that allow to create a cosserat beam/rod in Sofa. @@ -47,13 +61,6 @@ class CosseratBase(Sofa.Prefab): "type": "Vec3d", "help": "Cosserat base Rotation", "default": array([0.0, 0.0, 0.0]), - }, - { # @TODO: to be removed - "name": "attachingToLink", - "type": "string", - "help": "a rest shape force field will constraint the object " - "to follow arm position", - "default": "1", } ] @@ -128,36 +135,11 @@ def _addSlidingPointsWithContainer(self): return slidingPoint def _addRigidBaseNode(self): - rigidBaseNode = self.addChild("rigidBase") - # To be improved with classes in top - positions = [[self.params.beamGeoParams.init_pos] + [0.0, 0.0, 0.0, 1.0]] - - rigidBaseNodeMo = rigidBaseNode.addObject( - "MechanicalObject", - template="Rigid3d", - name="RigidBaseMO", - position=positions, - translation=self.translation, - rotation=self.rotation - ) - - # @TODO: remove this hard coded. - # one can choose to set this to false and directly attach the beam base - # to a control object in order to be able to drive it. - if int(self.attachingToLink.value): - print("Adding the rest shape to the base") - rigidBaseNode.addObject( - "RestShapeSpringsForceField", - name="spring", - stiffness=1e8, - angularStiffness=1.0e8, - external_points=0, - mstate="@RigidBaseMO", - points=0, - template="Rigid3d", - ) + rigidBaseNode = _create_rigid_node(self, "RigidBase", + self.translation, self.rotation) return rigidBaseNode + def _addCosseratCoordinate(self, bendingStates, listOfSectionsLength): cosseratCoordinateNode = self.addChild("cosseratCoordinate") cosseratCoordinateNode.addObject( @@ -236,7 +218,7 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): return cosseratInSofaFrameNode -Params = Parameters(beamGeoParams=BeamGeometryParameters(init_pos=[0, 0, 0])) +Params = Parameters(beamGeoParams=BeamGeometryParameters()) def createScene(rootNode): @@ -261,6 +243,17 @@ def createScene(rootNode): # Create a cosserat = solverNode.addChild(CosseratBase(parent=solverNode, params=Params)) + cosserat.rigidBaseNode.addObject( + "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() diff --git a/examples/python3/useful/geometry.py b/examples/python3/useful/geometry.py index 4bec898b..8bd3ba6b 100644 --- a/examples/python3/useful/geometry.py +++ b/examples/python3/useful/geometry.py @@ -4,14 +4,13 @@ def calculate_beam_parameters(beamGeoParams): # Data validation checks for beamGeoParams attributes - if not all(hasattr(beamGeoParams, attr) for attr in ['init_pos', 'beamLength', 'nbSection']): - raise ValueError("beamGeoParams must have 'init_pos', 'beamLength', and 'nbSection' attributes.") + if not all(hasattr(beamGeoParams, attr) for attr in ['beamLength', 'nbSection']): + raise ValueError("beamGeoParams must have, 'beamLength', and 'nbSection' attributes.") total_length = beamGeoParams.beamLength nb_sections = beamGeoParams.nbSection - x, y, z = beamGeoParams.init_pos - if not all(isinstance(val, (int, float)) for val in [x, y, z, total_length]): + if not all(isinstance(val, (float)) for val in [total_length]): raise ValueError("init_pos and beamLength in beamGeoParams must be numeric values.") if not isinstance(nb_sections, int) or nb_sections <= 0: @@ -20,29 +19,28 @@ def calculate_beam_parameters(beamGeoParams): length_s = total_length / nb_sections bendingState = [] listOfSectionsLength = [] - temp = x - curv_abs_input_s = [x] + temp = 0 + curv_abs_input_s = [0] for i in range(nb_sections): bendingState.append([0, 0, 0]) listOfSectionsLength.append((((i + 1) * length_s) - i * length_s)) temp += listOfSectionsLength[i] curv_abs_input_s.append(temp) - curv_abs_input_s[nb_sections] = total_length + x + curv_abs_input_s[nb_sections] = total_length return bendingState, curv_abs_input_s, listOfSectionsLength def calculate_frame_parameters(beamGeoParams): # Data validation checks for beamGeoParams attributes - if not all(hasattr(beamGeoParams, attr) for attr in ['init_pos', 'beamLength', 'nbFrames']): - raise ValueError("beamGeoParams must have 'init_pos', 'beamLength', and 'nbFrames' attributes.") + if not all(hasattr(beamGeoParams, attr) for attr in ['beamLength', 'nbFrames']): + raise ValueError("beamGeoParams must have 'beamLength', and 'nbFrames' attributes.") - x, y, z = beamGeoParams.init_pos total_length = beamGeoParams.beamLength nb_frames = beamGeoParams.nbFrames - if not all(isinstance(val, (int, float)) for val in [x, y, z, total_length]): + if not all(isinstance(val, (int, float)) for val in [total_length]): raise ValueError("init_pos and beamLength in beamGeoParams must be numeric values.") if not isinstance(nb_frames, int) or nb_frames <= 0: @@ -56,13 +54,13 @@ def calculate_frame_parameters(beamGeoParams): # @Todo: improve this for for i in range(nb_frames): sol = i * length_f - frames_f.append([sol + x, y, z, 0, 0, 0, 1]) - cable_position_f.append([sol + x, y, z]) - curv_abs_output_f.append(sol + x) + frames_f.append([sol, 0, 0, 0, 0, 0, 1]) + cable_position_f.append([sol, 0, 0]) + curv_abs_output_f.append(sol) - frames_f.append([total_length + x, y, z, 0, 0, 0, 1]) - cable_position_f.append([total_length + x, y, z]) - curv_abs_output_f.append(total_length + x) + frames_f.append([total_length , 0, 0, 0, 0, 0, 1]) + cable_position_f.append([total_length, 0, 0]) + curv_abs_output_f.append(total_length) return frames_f, curv_abs_output_f, cable_position_f diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index 7cb01c1b..c8e75558 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -13,9 +13,6 @@ class BeamGeometryParameters: nbSection: int = 5 # number of sections, sections along the beam length nbFrames: int = 30 # number of frames along the beam buildCollisionModel: int = 0 - init_pos: List[float] = field( - default_factory=lambda: [0.0, 0.0, 0.0] - ) # The beam rigid base position as a list [x, y, z] #Todo: two objects from the same base class to define different instead of useInertia @dataclass From f76833e622f2fad81d40df252667529a116c1682 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 6 Sep 2024 20:43:48 +0200 Subject: [PATCH 06/22] -----> qwen2 Refactor: Move rigid node creation to `utils.py` and simplify `CosseratBase`. --- examples/python3/cosserat/CosseratBase.py | 41 ++++------------------- examples/python3/useful/utils.py | 15 +++++++++ 2 files changed, 22 insertions(+), 34 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index a7e3b94e..63fc8c9a 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -10,26 +10,13 @@ __date__ = "October, 26 2021" import Sofa -from useful.utils import addEdgeCollision, addPointsCollision +from useful.utils import addEdgeCollision, addPointsCollision, _create_rigid_node from useful.header import addHeader, addVisual, addSolverNode from useful.params import Parameters, BeamGeometryParameters from useful.geometry import CosseratGeometry, generate_edge_list from numpy import array -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( - "MechanicalObject", - template="Rigid3d", - name=name+"MO", - position=positions, - translation=translation, - rotation=rotation - ) - return rigidBaseNode class CosseratBase(Sofa.Prefab): """ @@ -67,23 +54,15 @@ class CosseratBase(Sofa.Prefab): def __init__(self, *args, **kwargs): Sofa.Prefab.__init__(self, *args, **kwargs) self.params = kwargs.get( - "params", Parameters() + "beam_params", Parameters() ) # Use the Parameters class with default values - beamPhysicsParams = self.params.beamPhysicsParams - beamGeometryParams = self.params.beamGeoParams + beamPhysicsParams = self.params.beamPhysicsParams self.beamMass = beamPhysicsParams.beamMass # self.cosseratGeometry['beamMass'] - # Todo: add option in case None - self.parent = kwargs.get("parent") self.useInertiaParams = beamPhysicsParams.useInertia # False self.radius = beamPhysicsParams.beamRadius # kwargs.get('radius') - # @TODO: To be removed - if self.parent.hasObject("EulerImplicitSolver") is False: - print("The code does not have parent EulerImplicit") - self.solverNode = addSolverNode(self.parent) - else: - self.solverNode = self.parent + self.solverNode = kwargs.get("parent") if "inertialParams" in kwargs: self.useInertiaParams = True @@ -91,7 +70,7 @@ def __init__(self, *args, **kwargs): self.rigidBaseNode = self._addRigidBaseNode() - cosserat_geometry = CosseratGeometry(beamGeometryParams) + cosserat_geometry = CosseratGeometry(self.params.beamGeoParams) self.frames3D = cosserat_geometry.cable_positionF self.cosseratCoordinateNode = self._addCosseratCoordinate( @@ -123,15 +102,9 @@ def _addSlidingPoints(self): return slidingPoint def _addSlidingPointsWithContainer(self): - slidingPoint = self.cosseratFrame.addChild("slidingPoint") + slidingPoint = self._addSlidingPoints() slidingPoint.addObject("PointSetTopologyContainer") slidingPoint.addObject("PointSetTopologyModifier") - slidingPoint.addObject( - "MechanicalObject", - name="slidingPointMO", - position=self.frames3D - ) - slidingPoint.addObject("IdentityMapping") return slidingPoint def _addRigidBaseNode(self): @@ -242,7 +215,7 @@ def createScene(rootNode): solverNode.addObject("GenericConstraintCorrection") # Create a - cosserat = solverNode.addChild(CosseratBase(parent=solverNode, params=Params)) + cosserat = solverNode.addChild(CosseratBase(parent=solverNode, beam_params=Params)) cosserat.rigidBaseNode.addObject( "RestShapeSpringsForceField", name="spring", diff --git a/examples/python3/useful/utils.py b/examples/python3/useful/utils.py index 9e1fc061..ca9e6651 100644 --- a/examples/python3/useful/utils.py +++ b/examples/python3/useful/utils.py @@ -81,3 +81,18 @@ def computeNegativeAlongXDistanceBetweenPoints(constraintPointPos, slidingPointP else: # print("No constraint points yet") 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]]): + rigidBaseNode = parent_node.addChild(name) + + rigidBaseNodeMo = rigidBaseNode.addObject( + "MechanicalObject", + template="Rigid3d", + name=name+"MO", + position=positions, + translation=translation, + rotation=rotation + ) + return rigidBaseNode \ No newline at end of file From 87c10646deebb0ab9abb2fd3472b8d5923b7de2f Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 6 Sep 2024 23:08:15 +0200 Subject: [PATCH 07/22] Refactor naming consistency and update parameters in BeamPhysicsParameters and Cosserat prefab --- examples/python3/cosserat/CosseratBase.py | 96 +++++++++++++++-------- examples/python3/useful/params.py | 12 +-- 2 files changed, 68 insertions(+), 40 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index 63fc8c9a..d0afbded 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -15,6 +15,7 @@ from useful.params import Parameters, BeamGeometryParameters from useful.geometry import CosseratGeometry, generate_edge_list from numpy import array +from typing import List @@ -58,14 +59,14 @@ def __init__(self, *args, **kwargs): ) # Use the Parameters class with default values beamPhysicsParams = self.params.beamPhysicsParams - self.beamMass = beamPhysicsParams.beamMass # self.cosseratGeometry['beamMass'] - self.useInertiaParams = beamPhysicsParams.useInertia # False - self.radius = beamPhysicsParams.beamRadius # kwargs.get('radius') + self.beam_mass = beamPhysicsParams.beam_mass # self.cosseratGeometry['beamMass'] + self.use_inertia_params = beamPhysicsParams.useInertia # False + self.radius = beamPhysicsParams.beam_radius # kwargs.get('radius') self.solverNode = kwargs.get("parent") if "inertialParams" in kwargs: - self.useInertiaParams = True + self.use_inertia_params = True self.inertialParams = kwargs["inertialParams"] self.rigidBaseNode = self._addRigidBaseNode() @@ -73,9 +74,10 @@ def __init__(self, *args, **kwargs): cosserat_geometry = CosseratGeometry(self.params.beamGeoParams) self.frames3D = cosserat_geometry.cable_positionF - self.cosseratCoordinateNode = self._addCosseratCoordinate( + self.cosseratCoordinateNode = self._add_cosserat_coordinate( cosserat_geometry.bendingState, cosserat_geometry.sectionsLengthList ) + self.cosseratFrame = self._addCosseratFrame( cosserat_geometry.framesF, cosserat_geometry.curv_abs_inputS, @@ -112,57 +114,83 @@ def _addRigidBaseNode(self): self.translation, self.rotation) return rigidBaseNode + def _add_cosserat_coordinate(self, initial_curvature: List[float], section_lengths: List[float]) -> None: + """ + Adds a cosserat coordinate node with a BeamHookeLawForceField object to the graph. - def _addCosseratCoordinate(self, bendingStates, listOfSectionsLength): - cosseratCoordinateNode = self.addChild("cosseratCoordinate") - cosseratCoordinateNode.addObject( + Args: + initial_curvature: Initial curvature of the cosserat coordinate. + section_lengths: Length of each section in the cosserat coordinate. + + Returns: + The cosserat coordinate node added to the model. + """ + cosserat_coordinate_node = self.addChild("cosseratCoordinate") + cosserat_coordinate_node.addObject( "MechanicalObject", template="Vec3d", name="cosseratCoordinateMO", - position=bendingStates + position=initial_curvature ) - if self.useInertiaParams is False: - cosseratCoordinateNode.addObject( - "BeamHookeLawForceField", - crossSectionShape=self.params.beamPhysicsParams.beamShape, - length=listOfSectionsLength, - radius=self.radius, - youngModulus=self.params.beamPhysicsParams.youngModulus, - poissonRatio=self.params.beamPhysicsParams.poissonRatio, - rayleighStiffness=self.params.simuParams.rayleighStiffness, - lengthY=self.params.beamPhysicsParams.length_Y, - lengthZ=self.params.beamPhysicsParams.length_Z, - ) + if not self.use_inertia_params: + self._add_beam_hooke_law_without_inertia(cosserat_coordinate_node, section_lengths) else: - self._extracted_from_addCosseratCoordinate_15( - cosseratCoordinateNode, listOfSectionsLength - ) - return cosseratCoordinateNode + self._add_beam_hooke_law_with_inertia(cosserat_coordinate_node, section_lengths) - # TODO Rename this here and in `addCosseratCoordinate` - def _extracted_from_addCosseratCoordinate_15( - self, cosseratCoordinateNode, listOfSectionsLength - ): + return cosserat_coordinate_node + + def _add_beam_hooke_law_without_inertia(self, cosserat_coordinate_node: None, + section_lengths: list[float]) -> None: + """ + Adds a BeamHookeLawForceField object to the cosserat coordinate node without inertia parameters. + + Args: + cosserat_coordinate_node: The cosserat coordinate node to add the object to. + section_lengths: Length of each section in the cosserat coordinate. + """ + cosserat_coordinate_node.addObject( + "BeamHookeLawForceField", + crossSectionShape=self.params.beamPhysicsParams.beam_shape, + length=section_lengths, + radius=self.params.beamPhysicsParams.beam_radius, + youngModulus=self.params.beamPhysicsParams.young_modulus, + poissonRatio=self.params.beamPhysicsParams.poisson_ratio, + rayleighStiffness=self.params.simuParams.rayleighStiffness, + lengthY=self.params.beamPhysicsParams.length_Y, + lengthZ=self.params.beamPhysicsParams.length_Z, + ) + + def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, section_lengths: List[float]) -> None: + """ + Adds a BeamHookeLawForceField object to the cosserat coordinate node with inertia parameters. + + Args: + cosserat_coordinate_node: The cosserat coordinate node to add the object to. + section_lengths: Length of each section in the cosserat coordinate. + """ 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, + crossSectionShape=self.params.beamPhysicsParams.beam_shape, + length=section_lengths, + radius=self.params.beamPhysicsParams.beam_radius, useInertiaParams=True, GI=GI, GA=GA, EI=EI, EA=EA, - rayleighStiffness=self.rayleighStiffness.value, + rayleighStiffness=self.params.simuParams.rayleighStiffness, lengthY=self.params.beamPhysicsParams.length_Y, lengthZ=self.params.beamPhysicsParams.length_Z, ) + # 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) @@ -174,7 +202,7 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): ) cosseratInSofaFrameNode.addObject( - "UniformMass", totalMass=self.beamMass, showAxisSizeFactor="0" + "UniformMass", totalMass=self.beam_mass, showAxisSizeFactor="0" ) cosseratInSofaFrameNode.addObject( diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index c8e75558..dc85ba39 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -20,8 +20,8 @@ class BeamPhysicsParameters: """Cosserat Beam Physics parameters""" """First set of parameters""" - youngModulus: float = 1.205e8 - poissonRatio: float = 0.3 + young_modulus: float = 1.205e8 + poisson_ratio: float = 0.3 """Second set of parameters""" useInertia: bool = False @@ -31,11 +31,11 @@ class BeamPhysicsParameters: EA: float = 3.1416e4 """Common parameters used in both cases""" - beamMass: float = 1.0 - beamRadius: float = 0.02 / 2.0 # beam radius in m - beamLength: float = 1.0 # beam length in m along the X axis + beam_mass: float = 1.0 + beam_radius: float = 0.02 / 2.0 # beam radius in m + beam_length: float = 1.0 # beam length in m along the X axis # Todo : add complex beam shape - beamShape: str = "circular" # beam shape, circular or rectangular + beam_shape: str = "circular" # beam shape, circular or rectangular """"If using rectangular beam shape""" length_Y: float = 0.1 # length of the beam in the Y direction length_Z: float = 0.1 # length of the beam in the Z direction From 8f84fabf8a0d626f940b1fbf60072f6ab27026b6 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Sat, 7 Sep 2024 00:24:33 +0200 Subject: [PATCH 08/22] Rename parameter data class for prefab use case" --- ...seratBeamFallingUnderTheEffectOfGravity.py | 4 +- examples/python3/cosserat/CosseratBase.py | 38 ++++---- examples/python3/useful/header.py | 2 +- examples/python3/useful/params.py | 96 +++++++++++++------ examples/python3/wip/needleInteractionTest.py | 4 +- 5 files changed, 92 insertions(+), 52 deletions(-) diff --git a/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py b/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py index 11c917d0..968e8e22 100644 --- a/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py +++ b/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py @@ -69,8 +69,8 @@ class CableGeometryParameters: isCollisionModel = 0 -Params = Parameters(beamGeoParams=BeamGeometryParameters(init_pos=[0, 0, 0], beamLength=length, - nbSection=5, nbFrames=30, buildCollisionModel=0)) +Params = Parameters(beam_geo_params=BeamGeometryParameters(init_pos=[0, 0, 0], beamLength=length, + nbSection=5, nbFrames=30, buildCollisionModel=0)) def createScene(rootNode): addHeader(rootNode) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index d0afbded..5e057571 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -58,7 +58,7 @@ def __init__(self, *args, **kwargs): "beam_params", Parameters() ) # Use the Parameters class with default values - beamPhysicsParams = self.params.beamPhysicsParams + 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') @@ -71,7 +71,7 @@ def __init__(self, *args, **kwargs): self.rigidBaseNode = self._addRigidBaseNode() - cosserat_geometry = CosseratGeometry(self.params.beamGeoParams) + cosserat_geometry = CosseratGeometry(self.params.beam_geo_params) self.frames3D = cosserat_geometry.cable_positionF self.cosseratCoordinateNode = self._add_cosserat_coordinate( @@ -151,14 +151,14 @@ def _add_beam_hooke_law_without_inertia(self, cosserat_coordinate_node: None, """ cosserat_coordinate_node.addObject( "BeamHookeLawForceField", - crossSectionShape=self.params.beamPhysicsParams.beam_shape, + crossSectionShape=self.params.beam_physics_params.beam_shape, length=section_lengths, - radius=self.params.beamPhysicsParams.beam_radius, - youngModulus=self.params.beamPhysicsParams.young_modulus, - poissonRatio=self.params.beamPhysicsParams.poisson_ratio, - rayleighStiffness=self.params.simuParams.rayleighStiffness, - lengthY=self.params.beamPhysicsParams.length_Y, - lengthZ=self.params.beamPhysicsParams.length_Z, + radius=self.params.beam_physics_params.beam_radius, + youngModulus=self.params.beam_physics_params.young_modulus, + poissonRatio=self.params.beam_physics_params.poisson_ratio, + rayleighStiffness=self.params.simu_params.rayleigh_stiffness, + lengthY=self.params.beam_physics_params.length_Y, + 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: @@ -169,23 +169,23 @@ def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, secti cosserat_coordinate_node: The cosserat coordinate node to add the object to. section_lengths: Length of each section in the cosserat coordinate. """ - GA = self.params.beamPhysicsParams.GA - GI = self.params.beamPhysicsParams.GI - EA = self.params.beamPhysicsParams.EA - EI = self.params.beamPhysicsParams.EI + GA = self.params.beam_physics_params.GA + GI = self.params.beam_physics_params.GI + EA = self.params.beam_physics_params.EA + EI = self.params.beam_physics_params.EI cosseratCoordinateNode.addObject( "BeamHookeLawForceField", - crossSectionShape=self.params.beamPhysicsParams.beam_shape, + crossSectionShape=self.params.beam_physics_params.beam_shape, length=section_lengths, - radius=self.params.beamPhysicsParams.beam_radius, + radius=self.params.beam_physics_params.beam_radius, useInertiaParams=True, GI=GI, GA=GA, EI=EI, EA=EA, - rayleighStiffness=self.params.simuParams.rayleighStiffness, - lengthY=self.params.beamPhysicsParams.length_Y, - lengthZ=self.params.beamPhysicsParams.length_Z, + rayleighStiffness=self.params.simu_params.rayleigh_stiffness, + lengthY=self.params.beam_physics_params.length_Y, + lengthZ=self.params.beam_physics_params.length_Z, ) # TODO Rename this here and in `addCosseratCoordinate` @@ -219,7 +219,7 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): return cosseratInSofaFrameNode -Params = Parameters(beamGeoParams=BeamGeometryParameters()) +Params = Parameters(beam_geo_params=BeamGeometryParameters()) def createScene(rootNode): diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index aceac414..24495214 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -81,7 +81,7 @@ def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=Fal multithreading=multithreading, printLog=1) if isContact: - contactHeader(parentNode, _contact_params=params.contactParams) + contactHeader(parentNode, _contact_params=params.contact_params) # components needed for contact modeling diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index dc85ba39..d4785bcd 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -1,7 +1,7 @@ # @todo use this dataclass to create the cosserat object from dataclasses import dataclass, field -from typing import List +from typing import List, Literal # @TODO: Improve this function, remove hard coding. @@ -14,49 +14,81 @@ class BeamGeometryParameters: nbFrames: int = 30 # number of frames along the beam buildCollisionModel: int = 0 -#Todo: two objects from the same base class to define different instead of useInertia -@dataclass -class BeamPhysicsParameters: - """Cosserat Beam Physics parameters""" + def validate(self): + assert self.beamLength > 0, "Beam length must be positive" + assert self.nbSection > 0, "Number of sections must be positive" + assert self.nbFrames > 0, "Number of frames must be positive" + assert self.nbFrames >= self.nbSection, "Number of frames must be positive" + + +class BeamPhysicsBaseParameters: + """Base class for Cosserat Beam Physics parameters""" - """First set of parameters""" young_modulus: float = 1.205e8 poisson_ratio: float = 0.3 - - """Second set of parameters""" + beam_mass: float = 1.0 + beam_radius: float = 0.01 # default radius of 0.02 / 2.0 + beam_length: float = 1.0 # default length along the X axis + beam_shape: Literal['circular', 'rectangular'] = 'circular' + length_Y: float = 0.1 # length in Y direction for rectangular beam + length_Z: float = 0.1 # length in Z direction for rectangular beam useInertia: bool = False + + 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 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" + + +@dataclass +class BeamPhysicsParametersNoInertia(BeamPhysicsBaseParameters): + """Parameters for a Cosserat Beam without inertia""" + pass + + +@dataclass +class BeamPhysicsParametersWithInertia(BeamPhysicsBaseParameters): + """Parameters for a Cosserat Beam with inertia""" + GI: float = 1.5708 GA: float = 3.1416e4 EI: float = 0.7854 EA: float = 3.1416e4 - """Common parameters used in both cases""" - beam_mass: float = 1.0 - beam_radius: float = 0.02 / 2.0 # beam radius in m - beam_length: float = 1.0 # beam length in m along the X axis - # Todo : add complex beam shape - beam_shape: str = "circular" # beam shape, circular or rectangular - """"If using rectangular beam shape""" - length_Y: float = 0.1 # length of the beam in the Y direction - length_Z: float = 0.1 # length of the beam in the Z direction + def validate(self): + super().validate() + assert self.GI > 0, "GI must be positive" + assert self.GA > 0, "GA must be positive" + assert self.EI > 0, "EI must be positive" + assert self.EA > 0, "EA must be positive" @dataclass class SimulationParameters: """Simulation parameters""" - rayleighStiffness: float = 0.2 - rayleighMass: float = 0.1 + rayleigh_stiffness: float = 0.2 + rayleigh_mass: float = 0.1 firstOrder: bool = False + def validate(self): + assert self.rayleigh_stiffness >= 0, "Rayleigh stiffness must be non-negative" + assert self.rayleigh_mass >= 0, "Rayleigh mass must be non-negative" + @dataclass class VisualParameters: """Visual parameters""" showObject: int = 1 - showObjectScale: float = 1.0 - showObjectColor: List[float] = field(default_factory=lambda: [1.0, 0.0, 0.0, 1.0]) + show_object_scale: float = 1.0 + show_object_color: List[float] = field(default_factory=lambda: [1.0, 0.0, 0.0, 1.0]) + + def validate(self): + assert len(self.show_object_color) == 4, "Color must have four components (RGBA)" + assert all(0.0 <= x <= 1.0 for x in self.show_object_color), "Color components must be in range [0, 1]" @dataclass @@ -77,16 +109,24 @@ class ContactParameters: class Parameters: """Parameters for the Cosserat Beam""" - beamPhysicsParams: BeamPhysicsParameters = field( - default_factory=BeamPhysicsParameters - ) - simuParams: SimulationParameters = field( + # beamPhysicsParams: BeamPhysicsParameters = field( + # default_factory=BeamPhysicsParameters + # ) + beam_physics_params: BeamPhysicsBaseParameters = field(default_factory=BeamPhysicsParametersNoInertia) + simu_params: SimulationParameters = field( default_factory=SimulationParameters ) # SimulationParameters() - contactParams: ContactParameters = field( + contact_params: ContactParameters = field( default_factory=ContactParameters ) # ContactParameters() - beamGeoParams: BeamGeometryParameters = field( + beam_geo_params: BeamGeometryParameters = field( default_factory=BeamGeometryParameters ) - visualParams: VisualParameters = field(default_factory=VisualParameters) + visual_params: VisualParameters = field(default_factory=VisualParameters) + + def validate(self): + self.beam_physics_params.validate() + self.simu_params.validate() + self.contact_params.validate() + self.beam_geo_params.validate() + self.visual_params.validate() \ No newline at end of file diff --git a/examples/python3/wip/needleInteractionTest.py b/examples/python3/wip/needleInteractionTest.py index 9d402bbc..2975b56c 100644 --- a/examples/python3/wip/needleInteractionTest.py +++ b/examples/python3/wip/needleInteractionTest.py @@ -168,7 +168,7 @@ def createScene(rootNode): ############### solverNode = rootNode.addChild('solverNode') solverNode.addObject('EulerImplicitSolver', - rayleighStiffness=params.Physics.rayleighStiffness) + rayleighStiffness=params.Physics.rayleigh_stiffness) solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") solverNode.addObject('GenericConstraintCorrection') @@ -176,7 +176,7 @@ def createScene(rootNode): needle = solverNode.addChild( Cosserat(parent=solverNode, cosseratGeometry=needleGeometryConfig, radius=params.Geometry.radius, name="needle", youngModulus=params.Physics.youngModulus, poissonRatio=params.Physics.poissonRatio, - rayleighStiffness=params.Physics.rayleighStiffness)) + rayleighStiffness=params.Physics.rayleigh_stiffness)) needleCollisionModel = needle.addPointCollisionModel() slidingPoint = needle.addSlidingPoints() From 609dd3372371ed93024a7e56b0adbc5084182cfa Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 6 Sep 2024 20:43:48 +0200 Subject: [PATCH 09/22] Refactoring Refactor: Move rigid node creation to `utils.py` and simplify `CosseratBase`. --- examples/python3/cosserat/CosseratBase.py | 41 ++++------------------- examples/python3/useful/utils.py | 15 +++++++++ 2 files changed, 22 insertions(+), 34 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index a7e3b94e..63fc8c9a 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -10,26 +10,13 @@ __date__ = "October, 26 2021" import Sofa -from useful.utils import addEdgeCollision, addPointsCollision +from useful.utils import addEdgeCollision, addPointsCollision, _create_rigid_node from useful.header import addHeader, addVisual, addSolverNode from useful.params import Parameters, BeamGeometryParameters from useful.geometry import CosseratGeometry, generate_edge_list from numpy import array -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( - "MechanicalObject", - template="Rigid3d", - name=name+"MO", - position=positions, - translation=translation, - rotation=rotation - ) - return rigidBaseNode class CosseratBase(Sofa.Prefab): """ @@ -67,23 +54,15 @@ class CosseratBase(Sofa.Prefab): def __init__(self, *args, **kwargs): Sofa.Prefab.__init__(self, *args, **kwargs) self.params = kwargs.get( - "params", Parameters() + "beam_params", Parameters() ) # Use the Parameters class with default values - beamPhysicsParams = self.params.beamPhysicsParams - beamGeometryParams = self.params.beamGeoParams + beamPhysicsParams = self.params.beamPhysicsParams self.beamMass = beamPhysicsParams.beamMass # self.cosseratGeometry['beamMass'] - # Todo: add option in case None - self.parent = kwargs.get("parent") self.useInertiaParams = beamPhysicsParams.useInertia # False self.radius = beamPhysicsParams.beamRadius # kwargs.get('radius') - # @TODO: To be removed - if self.parent.hasObject("EulerImplicitSolver") is False: - print("The code does not have parent EulerImplicit") - self.solverNode = addSolverNode(self.parent) - else: - self.solverNode = self.parent + self.solverNode = kwargs.get("parent") if "inertialParams" in kwargs: self.useInertiaParams = True @@ -91,7 +70,7 @@ def __init__(self, *args, **kwargs): self.rigidBaseNode = self._addRigidBaseNode() - cosserat_geometry = CosseratGeometry(beamGeometryParams) + cosserat_geometry = CosseratGeometry(self.params.beamGeoParams) self.frames3D = cosserat_geometry.cable_positionF self.cosseratCoordinateNode = self._addCosseratCoordinate( @@ -123,15 +102,9 @@ def _addSlidingPoints(self): return slidingPoint def _addSlidingPointsWithContainer(self): - slidingPoint = self.cosseratFrame.addChild("slidingPoint") + slidingPoint = self._addSlidingPoints() slidingPoint.addObject("PointSetTopologyContainer") slidingPoint.addObject("PointSetTopologyModifier") - slidingPoint.addObject( - "MechanicalObject", - name="slidingPointMO", - position=self.frames3D - ) - slidingPoint.addObject("IdentityMapping") return slidingPoint def _addRigidBaseNode(self): @@ -242,7 +215,7 @@ def createScene(rootNode): solverNode.addObject("GenericConstraintCorrection") # Create a - cosserat = solverNode.addChild(CosseratBase(parent=solverNode, params=Params)) + cosserat = solverNode.addChild(CosseratBase(parent=solverNode, beam_params=Params)) cosserat.rigidBaseNode.addObject( "RestShapeSpringsForceField", name="spring", diff --git a/examples/python3/useful/utils.py b/examples/python3/useful/utils.py index 9e1fc061..ca9e6651 100644 --- a/examples/python3/useful/utils.py +++ b/examples/python3/useful/utils.py @@ -81,3 +81,18 @@ def computeNegativeAlongXDistanceBetweenPoints(constraintPointPos, slidingPointP else: # print("No constraint points yet") 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]]): + rigidBaseNode = parent_node.addChild(name) + + rigidBaseNodeMo = rigidBaseNode.addObject( + "MechanicalObject", + template="Rigid3d", + name=name+"MO", + position=positions, + translation=translation, + rotation=rotation + ) + return rigidBaseNode \ No newline at end of file From 14119199d31ecbdc868b0be238fee5cb1377e079 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 6 Sep 2024 23:08:15 +0200 Subject: [PATCH 10/22] Refactor naming consistency and update parameters in BeamPhysicsParameters and Cosserat prefab --- examples/python3/cosserat/CosseratBase.py | 96 +++++++++++++++-------- examples/python3/useful/params.py | 12 +-- 2 files changed, 68 insertions(+), 40 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index 63fc8c9a..d0afbded 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -15,6 +15,7 @@ from useful.params import Parameters, BeamGeometryParameters from useful.geometry import CosseratGeometry, generate_edge_list from numpy import array +from typing import List @@ -58,14 +59,14 @@ def __init__(self, *args, **kwargs): ) # Use the Parameters class with default values beamPhysicsParams = self.params.beamPhysicsParams - self.beamMass = beamPhysicsParams.beamMass # self.cosseratGeometry['beamMass'] - self.useInertiaParams = beamPhysicsParams.useInertia # False - self.radius = beamPhysicsParams.beamRadius # kwargs.get('radius') + self.beam_mass = beamPhysicsParams.beam_mass # self.cosseratGeometry['beamMass'] + self.use_inertia_params = beamPhysicsParams.useInertia # False + self.radius = beamPhysicsParams.beam_radius # kwargs.get('radius') self.solverNode = kwargs.get("parent") if "inertialParams" in kwargs: - self.useInertiaParams = True + self.use_inertia_params = True self.inertialParams = kwargs["inertialParams"] self.rigidBaseNode = self._addRigidBaseNode() @@ -73,9 +74,10 @@ def __init__(self, *args, **kwargs): cosserat_geometry = CosseratGeometry(self.params.beamGeoParams) self.frames3D = cosserat_geometry.cable_positionF - self.cosseratCoordinateNode = self._addCosseratCoordinate( + self.cosseratCoordinateNode = self._add_cosserat_coordinate( cosserat_geometry.bendingState, cosserat_geometry.sectionsLengthList ) + self.cosseratFrame = self._addCosseratFrame( cosserat_geometry.framesF, cosserat_geometry.curv_abs_inputS, @@ -112,57 +114,83 @@ def _addRigidBaseNode(self): self.translation, self.rotation) return rigidBaseNode + def _add_cosserat_coordinate(self, initial_curvature: List[float], section_lengths: List[float]) -> None: + """ + Adds a cosserat coordinate node with a BeamHookeLawForceField object to the graph. - def _addCosseratCoordinate(self, bendingStates, listOfSectionsLength): - cosseratCoordinateNode = self.addChild("cosseratCoordinate") - cosseratCoordinateNode.addObject( + Args: + initial_curvature: Initial curvature of the cosserat coordinate. + section_lengths: Length of each section in the cosserat coordinate. + + Returns: + The cosserat coordinate node added to the model. + """ + cosserat_coordinate_node = self.addChild("cosseratCoordinate") + cosserat_coordinate_node.addObject( "MechanicalObject", template="Vec3d", name="cosseratCoordinateMO", - position=bendingStates + position=initial_curvature ) - if self.useInertiaParams is False: - cosseratCoordinateNode.addObject( - "BeamHookeLawForceField", - crossSectionShape=self.params.beamPhysicsParams.beamShape, - length=listOfSectionsLength, - radius=self.radius, - youngModulus=self.params.beamPhysicsParams.youngModulus, - poissonRatio=self.params.beamPhysicsParams.poissonRatio, - rayleighStiffness=self.params.simuParams.rayleighStiffness, - lengthY=self.params.beamPhysicsParams.length_Y, - lengthZ=self.params.beamPhysicsParams.length_Z, - ) + if not self.use_inertia_params: + self._add_beam_hooke_law_without_inertia(cosserat_coordinate_node, section_lengths) else: - self._extracted_from_addCosseratCoordinate_15( - cosseratCoordinateNode, listOfSectionsLength - ) - return cosseratCoordinateNode + self._add_beam_hooke_law_with_inertia(cosserat_coordinate_node, section_lengths) - # TODO Rename this here and in `addCosseratCoordinate` - def _extracted_from_addCosseratCoordinate_15( - self, cosseratCoordinateNode, listOfSectionsLength - ): + return cosserat_coordinate_node + + def _add_beam_hooke_law_without_inertia(self, cosserat_coordinate_node: None, + section_lengths: list[float]) -> None: + """ + Adds a BeamHookeLawForceField object to the cosserat coordinate node without inertia parameters. + + Args: + cosserat_coordinate_node: The cosserat coordinate node to add the object to. + section_lengths: Length of each section in the cosserat coordinate. + """ + cosserat_coordinate_node.addObject( + "BeamHookeLawForceField", + crossSectionShape=self.params.beamPhysicsParams.beam_shape, + length=section_lengths, + radius=self.params.beamPhysicsParams.beam_radius, + youngModulus=self.params.beamPhysicsParams.young_modulus, + poissonRatio=self.params.beamPhysicsParams.poisson_ratio, + rayleighStiffness=self.params.simuParams.rayleighStiffness, + lengthY=self.params.beamPhysicsParams.length_Y, + lengthZ=self.params.beamPhysicsParams.length_Z, + ) + + def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, section_lengths: List[float]) -> None: + """ + Adds a BeamHookeLawForceField object to the cosserat coordinate node with inertia parameters. + + Args: + cosserat_coordinate_node: The cosserat coordinate node to add the object to. + section_lengths: Length of each section in the cosserat coordinate. + """ 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, + crossSectionShape=self.params.beamPhysicsParams.beam_shape, + length=section_lengths, + radius=self.params.beamPhysicsParams.beam_radius, useInertiaParams=True, GI=GI, GA=GA, EI=EI, EA=EA, - rayleighStiffness=self.rayleighStiffness.value, + rayleighStiffness=self.params.simuParams.rayleighStiffness, lengthY=self.params.beamPhysicsParams.length_Y, lengthZ=self.params.beamPhysicsParams.length_Z, ) + # 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) @@ -174,7 +202,7 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): ) cosseratInSofaFrameNode.addObject( - "UniformMass", totalMass=self.beamMass, showAxisSizeFactor="0" + "UniformMass", totalMass=self.beam_mass, showAxisSizeFactor="0" ) cosseratInSofaFrameNode.addObject( diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index c8e75558..dc85ba39 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -20,8 +20,8 @@ class BeamPhysicsParameters: """Cosserat Beam Physics parameters""" """First set of parameters""" - youngModulus: float = 1.205e8 - poissonRatio: float = 0.3 + young_modulus: float = 1.205e8 + poisson_ratio: float = 0.3 """Second set of parameters""" useInertia: bool = False @@ -31,11 +31,11 @@ class BeamPhysicsParameters: EA: float = 3.1416e4 """Common parameters used in both cases""" - beamMass: float = 1.0 - beamRadius: float = 0.02 / 2.0 # beam radius in m - beamLength: float = 1.0 # beam length in m along the X axis + beam_mass: float = 1.0 + beam_radius: float = 0.02 / 2.0 # beam radius in m + beam_length: float = 1.0 # beam length in m along the X axis # Todo : add complex beam shape - beamShape: str = "circular" # beam shape, circular or rectangular + beam_shape: str = "circular" # beam shape, circular or rectangular """"If using rectangular beam shape""" length_Y: float = 0.1 # length of the beam in the Y direction length_Z: float = 0.1 # length of the beam in the Z direction From ff18262c750be00bc46f11eba7cffc867afe8c3d Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Sat, 7 Sep 2024 00:24:33 +0200 Subject: [PATCH 11/22] Rename parameter data class for prefab use case" --- ...seratBeamFallingUnderTheEffectOfGravity.py | 4 +- examples/python3/cosserat/CosseratBase.py | 38 ++++---- examples/python3/useful/header.py | 2 +- examples/python3/useful/params.py | 96 +++++++++++++------ examples/python3/wip/needleInteractionTest.py | 4 +- 5 files changed, 92 insertions(+), 52 deletions(-) diff --git a/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py b/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py index 11c917d0..968e8e22 100644 --- a/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py +++ b/examples/python3/CosseratBeamFallingUnderTheEffectOfGravity.py @@ -69,8 +69,8 @@ class CableGeometryParameters: isCollisionModel = 0 -Params = Parameters(beamGeoParams=BeamGeometryParameters(init_pos=[0, 0, 0], beamLength=length, - nbSection=5, nbFrames=30, buildCollisionModel=0)) +Params = Parameters(beam_geo_params=BeamGeometryParameters(init_pos=[0, 0, 0], beamLength=length, + nbSection=5, nbFrames=30, buildCollisionModel=0)) def createScene(rootNode): addHeader(rootNode) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index d0afbded..5e057571 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -58,7 +58,7 @@ def __init__(self, *args, **kwargs): "beam_params", Parameters() ) # Use the Parameters class with default values - beamPhysicsParams = self.params.beamPhysicsParams + 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') @@ -71,7 +71,7 @@ def __init__(self, *args, **kwargs): self.rigidBaseNode = self._addRigidBaseNode() - cosserat_geometry = CosseratGeometry(self.params.beamGeoParams) + cosserat_geometry = CosseratGeometry(self.params.beam_geo_params) self.frames3D = cosserat_geometry.cable_positionF self.cosseratCoordinateNode = self._add_cosserat_coordinate( @@ -151,14 +151,14 @@ def _add_beam_hooke_law_without_inertia(self, cosserat_coordinate_node: None, """ cosserat_coordinate_node.addObject( "BeamHookeLawForceField", - crossSectionShape=self.params.beamPhysicsParams.beam_shape, + crossSectionShape=self.params.beam_physics_params.beam_shape, length=section_lengths, - radius=self.params.beamPhysicsParams.beam_radius, - youngModulus=self.params.beamPhysicsParams.young_modulus, - poissonRatio=self.params.beamPhysicsParams.poisson_ratio, - rayleighStiffness=self.params.simuParams.rayleighStiffness, - lengthY=self.params.beamPhysicsParams.length_Y, - lengthZ=self.params.beamPhysicsParams.length_Z, + radius=self.params.beam_physics_params.beam_radius, + youngModulus=self.params.beam_physics_params.young_modulus, + poissonRatio=self.params.beam_physics_params.poisson_ratio, + rayleighStiffness=self.params.simu_params.rayleigh_stiffness, + lengthY=self.params.beam_physics_params.length_Y, + 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: @@ -169,23 +169,23 @@ def _add_beam_hooke_law_with_inertia(self, cosserat_coordinate_node: None, secti cosserat_coordinate_node: The cosserat coordinate node to add the object to. section_lengths: Length of each section in the cosserat coordinate. """ - GA = self.params.beamPhysicsParams.GA - GI = self.params.beamPhysicsParams.GI - EA = self.params.beamPhysicsParams.EA - EI = self.params.beamPhysicsParams.EI + GA = self.params.beam_physics_params.GA + GI = self.params.beam_physics_params.GI + EA = self.params.beam_physics_params.EA + EI = self.params.beam_physics_params.EI cosseratCoordinateNode.addObject( "BeamHookeLawForceField", - crossSectionShape=self.params.beamPhysicsParams.beam_shape, + crossSectionShape=self.params.beam_physics_params.beam_shape, length=section_lengths, - radius=self.params.beamPhysicsParams.beam_radius, + radius=self.params.beam_physics_params.beam_radius, useInertiaParams=True, GI=GI, GA=GA, EI=EI, EA=EA, - rayleighStiffness=self.params.simuParams.rayleighStiffness, - lengthY=self.params.beamPhysicsParams.length_Y, - lengthZ=self.params.beamPhysicsParams.length_Z, + rayleighStiffness=self.params.simu_params.rayleigh_stiffness, + lengthY=self.params.beam_physics_params.length_Y, + lengthZ=self.params.beam_physics_params.length_Z, ) # TODO Rename this here and in `addCosseratCoordinate` @@ -219,7 +219,7 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): return cosseratInSofaFrameNode -Params = Parameters(beamGeoParams=BeamGeometryParameters()) +Params = Parameters(beam_geo_params=BeamGeometryParameters()) def createScene(rootNode): diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index aceac414..24495214 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -81,7 +81,7 @@ def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=Fal multithreading=multithreading, printLog=1) if isContact: - contactHeader(parentNode, _contact_params=params.contactParams) + contactHeader(parentNode, _contact_params=params.contact_params) # components needed for contact modeling diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index dc85ba39..d4785bcd 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -1,7 +1,7 @@ # @todo use this dataclass to create the cosserat object from dataclasses import dataclass, field -from typing import List +from typing import List, Literal # @TODO: Improve this function, remove hard coding. @@ -14,49 +14,81 @@ class BeamGeometryParameters: nbFrames: int = 30 # number of frames along the beam buildCollisionModel: int = 0 -#Todo: two objects from the same base class to define different instead of useInertia -@dataclass -class BeamPhysicsParameters: - """Cosserat Beam Physics parameters""" + def validate(self): + assert self.beamLength > 0, "Beam length must be positive" + assert self.nbSection > 0, "Number of sections must be positive" + assert self.nbFrames > 0, "Number of frames must be positive" + assert self.nbFrames >= self.nbSection, "Number of frames must be positive" + + +class BeamPhysicsBaseParameters: + """Base class for Cosserat Beam Physics parameters""" - """First set of parameters""" young_modulus: float = 1.205e8 poisson_ratio: float = 0.3 - - """Second set of parameters""" + beam_mass: float = 1.0 + beam_radius: float = 0.01 # default radius of 0.02 / 2.0 + beam_length: float = 1.0 # default length along the X axis + beam_shape: Literal['circular', 'rectangular'] = 'circular' + length_Y: float = 0.1 # length in Y direction for rectangular beam + length_Z: float = 0.1 # length in Z direction for rectangular beam useInertia: bool = False + + 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 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" + + +@dataclass +class BeamPhysicsParametersNoInertia(BeamPhysicsBaseParameters): + """Parameters for a Cosserat Beam without inertia""" + pass + + +@dataclass +class BeamPhysicsParametersWithInertia(BeamPhysicsBaseParameters): + """Parameters for a Cosserat Beam with inertia""" + GI: float = 1.5708 GA: float = 3.1416e4 EI: float = 0.7854 EA: float = 3.1416e4 - """Common parameters used in both cases""" - beam_mass: float = 1.0 - beam_radius: float = 0.02 / 2.0 # beam radius in m - beam_length: float = 1.0 # beam length in m along the X axis - # Todo : add complex beam shape - beam_shape: str = "circular" # beam shape, circular or rectangular - """"If using rectangular beam shape""" - length_Y: float = 0.1 # length of the beam in the Y direction - length_Z: float = 0.1 # length of the beam in the Z direction + def validate(self): + super().validate() + assert self.GI > 0, "GI must be positive" + assert self.GA > 0, "GA must be positive" + assert self.EI > 0, "EI must be positive" + assert self.EA > 0, "EA must be positive" @dataclass class SimulationParameters: """Simulation parameters""" - rayleighStiffness: float = 0.2 - rayleighMass: float = 0.1 + rayleigh_stiffness: float = 0.2 + rayleigh_mass: float = 0.1 firstOrder: bool = False + def validate(self): + assert self.rayleigh_stiffness >= 0, "Rayleigh stiffness must be non-negative" + assert self.rayleigh_mass >= 0, "Rayleigh mass must be non-negative" + @dataclass class VisualParameters: """Visual parameters""" showObject: int = 1 - showObjectScale: float = 1.0 - showObjectColor: List[float] = field(default_factory=lambda: [1.0, 0.0, 0.0, 1.0]) + show_object_scale: float = 1.0 + show_object_color: List[float] = field(default_factory=lambda: [1.0, 0.0, 0.0, 1.0]) + + def validate(self): + assert len(self.show_object_color) == 4, "Color must have four components (RGBA)" + assert all(0.0 <= x <= 1.0 for x in self.show_object_color), "Color components must be in range [0, 1]" @dataclass @@ -77,16 +109,24 @@ class ContactParameters: class Parameters: """Parameters for the Cosserat Beam""" - beamPhysicsParams: BeamPhysicsParameters = field( - default_factory=BeamPhysicsParameters - ) - simuParams: SimulationParameters = field( + # beamPhysicsParams: BeamPhysicsParameters = field( + # default_factory=BeamPhysicsParameters + # ) + beam_physics_params: BeamPhysicsBaseParameters = field(default_factory=BeamPhysicsParametersNoInertia) + simu_params: SimulationParameters = field( default_factory=SimulationParameters ) # SimulationParameters() - contactParams: ContactParameters = field( + contact_params: ContactParameters = field( default_factory=ContactParameters ) # ContactParameters() - beamGeoParams: BeamGeometryParameters = field( + beam_geo_params: BeamGeometryParameters = field( default_factory=BeamGeometryParameters ) - visualParams: VisualParameters = field(default_factory=VisualParameters) + visual_params: VisualParameters = field(default_factory=VisualParameters) + + def validate(self): + self.beam_physics_params.validate() + self.simu_params.validate() + self.contact_params.validate() + self.beam_geo_params.validate() + self.visual_params.validate() \ No newline at end of file diff --git a/examples/python3/wip/needleInteractionTest.py b/examples/python3/wip/needleInteractionTest.py index 9d402bbc..2975b56c 100644 --- a/examples/python3/wip/needleInteractionTest.py +++ b/examples/python3/wip/needleInteractionTest.py @@ -168,7 +168,7 @@ def createScene(rootNode): ############### solverNode = rootNode.addChild('solverNode') solverNode.addObject('EulerImplicitSolver', - rayleighStiffness=params.Physics.rayleighStiffness) + rayleighStiffness=params.Physics.rayleigh_stiffness) solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") solverNode.addObject('GenericConstraintCorrection') @@ -176,7 +176,7 @@ def createScene(rootNode): needle = solverNode.addChild( Cosserat(parent=solverNode, cosseratGeometry=needleGeometryConfig, radius=params.Geometry.radius, name="needle", youngModulus=params.Physics.youngModulus, poissonRatio=params.Physics.poissonRatio, - rayleighStiffness=params.Physics.rayleighStiffness)) + rayleighStiffness=params.Physics.rayleigh_stiffness)) needleCollisionModel = needle.addPointCollisionModel() slidingPoint = needle.addSlidingPoints() From 60add9d1d318164df57cee3f436e7a05301c1040 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Sun, 8 Sep 2024 12:19:13 +0200 Subject: [PATCH 12/22] cleanning prefab Cleanning the prefab class and update the test scene regarding --- .../python3/cosserat/utils/basecosserat.py | 129 ++++++++++++++++++ src/Cosserat/Binding/testScene.py | 2 +- 2 files changed, 130 insertions(+), 1 deletion(-) create mode 100644 examples/python3/cosserat/utils/basecosserat.py diff --git a/examples/python3/cosserat/utils/basecosserat.py b/examples/python3/cosserat/utils/basecosserat.py new file mode 100644 index 00000000..9bf45ff7 --- /dev/null +++ b/examples/python3/cosserat/utils/basecosserat.py @@ -0,0 +1,129 @@ +from scripts.utils.baseobject import BaseObject +from splib3.numerics import Quat, Vec3, vsub +from math import pi +import numpy as np +from scipy.linalg import logm, inv +from scipy.spatial.transform import Rotation +import Sofa + + +def getStrainFromQuat(frame, curvAbs, gXp): + """ + + Args: + frame: + curvAbs: abscissa curve + gXp: previous matrix + + Returns: + + """ + + gX = np.zeros((4, 4), dtype=float) + gX[0:3, 0:3] = Rotation.from_quat(frame[3:7]).as_matrix() + gX[0:3, 3] = frame[0:3] + gX[3, 3] = 1 + + if curvAbs <= 0.: + xi = [0., 0., 0.] + else: + gXpInv = inv(gXp) + xiHat = 1 / curvAbs * logm(np.dot(gXpInv, gX)) + xi = [xiHat[2][1], xiHat[0][2], xiHat[1][0], xiHat[0][3], xiHat[1][3], xiHat[2][3]] + + return xi[0:3], gX + + +class BaseCosserat(BaseObject): + """ + Base rod, based on Cosserat. With a visual and a collision model. + """ + + deformabletemplate = 'Vec3' + + def __init__(self, modelling, simulation, params, positions, length, name='BaseCosserat', collisionGroup=0): + super().__init__(modelling, simulation, params, positions, length, name, collisionGroup) + self.__addRod() + self.addCylinderTopology() + self.addVisualModel() + + def __addRod(self): + self.node = self.modelling.addChild(self.name) + self.simulation.addChild(self.node) + self.node.addObject('RequiredPlugin', pluginName=['Cosserat']) + + nbSections = self.params.nbSections + nbPoints = nbSections + 1 + + self.base = self.node.addChild('RigidBase') + self.base.addObject('MechanicalObject', template='Rigid3', position=self.positions[0]) + + rod = self.base.addChild('Rod') + self.rod = rod + self.deformable = self.node.addChild('Deformable') + self.deformable.addChild(rod) + + rod.addObject('EdgeSetTopologyContainer', + position=[pos[0:3] for pos in self.positions], + edges=[[i, i+1] for i in range(nbSections)]) + rod.addObject('MechanicalObject', template='Rigid3', + position=self.positions, showIndices=False, showIndicesScale=0.005, showObject=False, + showObjectScale=0.05) + rod.addObject("BeamInterpolation") + + # Convert Rigid3 orientation description to Cosserat bending description + # [[torsion strain, y_bending strain, z_bending strain]] + gX = np.zeros((4, 4), dtype=float) + gX[0:3, 0:3] = Rotation.from_quat(Quat(self.positions[0][3:7])).as_matrix() + gX[0:3, 3] = self.positions[0][0:3] + gX[3, 3] = 1 + + lengths = [] + strain = [] + for i in range(len(self.positions) - 1): + length = Vec3(vsub(self.positions[i][0:3], self.positions[i + 1][0:3])).getNorm() + lengths.append(length) + xi, gX = getStrainFromQuat(self.positions[i + 1], length, gX) + strain.append(xi) + + self.deformable.addObject('MechanicalObject', position=strain, rest_position=[0, 0, 0] * nbSections) + self.deformable.addObject('BeamHookeLawForceField', + youngModulus=self.params.youngModulus, + poissonRatio=self.params.poissonRatio, + radius=self.params.radius, + length=lengths) + self.node.addData(name="indexExtremity", type='int', value=nbPoints - 1) + + totalMass = self.params.density * self.length * self.params.radius * self.params.radius * pi + rod.addObject('UniformMass', totalMass=totalMass, showAxisSizeFactor=0.01) + l = 0 + curv_abs = [l] + for length in lengths: + l += length + curv_abs.append(l) + rod.addObject('DiscreteCosseratMapping', + curv_abs_input=curv_abs, + curv_abs_output=curv_abs, + input1=self.deformable.MechanicalObject.getLinkPath(), + input2=self.base.MechanicalObject.getLinkPath(), + output=rod.getLinkPath(), + debug=False, baseIndex=0) + + return rod + + +# Test scene +def createScene(rootnode): + from scripts.utils.header import addHeader, addSolvers + import params + + settings, modelling, simulation = addHeader(rootnode) + rootnode.VisualStyle.displayFlags = ['hideBehavior'] + addSolvers(simulation, iterative=False) + + nbSections = params.CableParameters.nbSections + length = 5 + dx = length / nbSections + positions = [[dx * i, 0, 0, 0, 0, 0, 1] for i in range(nbSections + 1)] + beam = BaseCosserat(modelling, simulation, params.CableParameters, positions, length) + beam.node.RigidBase.addObject('FixedConstraint', indices=0) diff --git a/src/Cosserat/Binding/testScene.py b/src/Cosserat/Binding/testScene.py index a2e6bea3..6ee60de1 100644 --- a/src/Cosserat/Binding/testScene.py +++ b/src/Cosserat/Binding/testScene.py @@ -17,7 +17,7 @@ def createScene(root): constraintPointsNode = needle.addChild("constraintPointsNode") slidingPoint = needle.addSlidingPoints() - pathToSlidingMo = slidingPoint.getLinkPath() + str("/slidingPointMO") + pathToSlidingMo = f"{slidingPoint.getLinkPath()}/slidingPointMO" print(f'------> The save path is : {pathToSlidingMo}') From 49bf1c5cde55cc4a1bd915c32f7865f12603a5eb Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 11 Sep 2024 23:59:36 +0200 Subject: [PATCH 13/22] code cleanning --- examples/python3/useful/geometry.py | 3 +- examples/python3/useful/header.py | 137 ++++++++++++++-------------- 2 files changed, 71 insertions(+), 69 deletions(-) diff --git a/examples/python3/useful/geometry.py b/examples/python3/useful/geometry.py index 8bd3ba6b..ca42aa22 100644 --- a/examples/python3/useful/geometry.py +++ b/examples/python3/useful/geometry.py @@ -1,5 +1,4 @@ # -from typing import List from useful.params import BeamGeometryParameters def calculate_beam_parameters(beamGeoParams): @@ -65,7 +64,7 @@ def calculate_frame_parameters(beamGeoParams): return frames_f, curv_abs_output_f, cable_position_f -def generate_edge_list(cable3DPos: List[List[float]]) -> list[list[int]]: +def generate_edge_list(cable3DPos: list[list[float]]) -> list[list[int]]: """ Generate an edge list required in the EdgeSetTopologyContainer component. diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index 24495214..4df2e621 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -15,29 +15,28 @@ from stlib3.physics.deformable import ElasticMaterialObject from stlib3.physics.constraints import FixedBox import os +from useful.params import ContactParameters as DefaultContactParams -def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=False, isContact=False, params=None): +def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=False, is_contact=False, contact_params=None): + """ - Adds to rootNode the default headers for a simulation with contact. Also adds and returns three nodes: - - Settings - - Modelling - - Simulation + Adds default headers for a simulation with contact to the parent node. + Also adds and returns three nodes: Settings, Modelling, Simulation. Args: - isContact: - inverse: - isConstrained: - parentNode: - multithreading: - - Usage: - addHeader(rootNode) + parent_node: The parent node to add the headers to. + multithreading: Enables multithreading (optional, default: False). + inverse: Enables inverse kinematics (optional, default: False). + is_constrained: Enables constraints (optional, default: False). + is_contact: Enables contact simulation (optional, default: False). + contact_params: Parameters for contact simulation (optional). Returns: - the three SOFA nodes {settings, modelling, simulation} - """ - settings = parentNode.addChild('Settings') + A tuple containing the settings, modelling, and simulation nodes. + """ + + settings = parent_node.addChild('Settings') settings.addObject('RequiredPlugin', pluginName=[ "Cosserat", "Sofa.Component.AnimationLoop", # Needed to use components FreeMotionAnimationLoop "Sofa.Component.Collision.Detection.Algorithm", @@ -64,40 +63,44 @@ def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=Fal ]) settings.addObject('BackgroundSetting', color=[1, 1, 1, 1]) - # settings.addObject('AttachBodyButtonSetting', stiffness=1e6) - parentNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels showCollisionModels ' + parent_node.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels showCollisionModels ' 'hideBoundingCollisionModels hideForceFields ' 'showInteractionForceFields hideWireframe showMechanicalMappings') - if isConstrained: - parentNode.addObject('FreeMotionAnimationLoop', parallelCollisionDetectionAndFreeMotion=multithreading, - parallelODESolving=multithreading) + + if is_constrained: + parent_node.addObject('FreeMotionAnimationLoop', parallelCollisionDetectionAndFreeMotion=multithreading, + parallelODESolving=multithreading) if inverse: settings.addObject('RequiredPlugin', name="SoftRobots.Inverse") - parentNode.addObject('QPInverseProblemSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, - multithreading=multithreading, epsilon=1) + parent_node.addObject('QPInverseProblemSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, + multithreading=multithreading, epsilon=1) else: - parentNode.addObject('GenericConstraintSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, - multithreading=multithreading, printLog=1) + parent_node.addObject('GenericConstraintSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, + multithreading=multithreading) - if isContact: - contactHeader(parentNode, _contact_params=params.contact_params) + if is_contact: + contactHeader(parent_node, contact_params=contact_params.contact_params) # components needed for contact modeling -def contactHeader(parentNode, _contact_params=None): - parentNode.addObject('CollisionPipeline') - parentNode.addObject("DefaultVisualManagerLoop") - parentNode.addObject('BruteForceBroadPhase') - parentNode.addObject('BVHNarrowPhase') - if not _contact_params == None: - parentNode.addObject('RuleBasedContactManager', responseParams=_contact_params.responseParams, - response='FrictionContactConstraint') - parentNode.addObject('LocalMinDistance', alarmDistance=_contact_params.alarmDistance, - contactDistance=_contact_params.contactDistance) - else : - parentNode.addObject('RuleBasedContactManager', responseParams='mu=0.1', response='FrictionContactConstraint') - parentNode.addObject('LocalMinDistance', alarmDistance=0.05, contactDistance=0.01) + +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) def addVisual(node): @@ -115,7 +118,7 @@ def addVisual(node): return node -def addSolverNode(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): """ @@ -124,7 +127,7 @@ def addSolverNode(node, name='solverNode', template='CompressedRowSparseMatrixd' Args: name: isConstrained: - node: + parent_node: template: for the LDLSolver rayleighMass: rayleighStiffness: @@ -134,41 +137,41 @@ def addSolverNode(node, name='solverNode', template='CompressedRowSparseMatrixd' Usage: addSolversNode(node) """ - solverNode = node.addChild(name) - solverNode.addObject('EulerImplicitSolver', firstOrder=firstOrder, rayleighStiffness=rayleighStiffness, + solver_node = parent_node.addChild(name) + solver_node.addObject('EulerImplicitSolver', firstOrder=firstOrder, rayleighStiffness=rayleighStiffness, rayleighMass=rayleighMass) if iterative: - solverNode.addObject('CGLinearSolver', name='Solver', template=template) + solver_node.addObject('CGLinearSolver', name='Solver', template=template) else: - solverNode.addObject('SparseLDLSolver', name='Solver', template=template, printLog=True) + solver_node.addObject('SparseLDLSolver', name='Solver', template=template, printLog=True) if isConstrained: - solverNode.addObject('GenericConstraintCorrection', linearSolver=solverNode.Solver.getLinkPath()) + solver_node.addObject('GenericConstraintCorrection', linearSolver=solver_node.Solver.getLinkPath()) - return solverNode + return solver_node -def addFEMObject(parentNode, path): - fingerSolver = addSolverNode(parentNode) +def addFEMObject(parent_node, path): + finger_solver = addSolverNode(parent_node) # Load a VTK tetrahedral mesh and expose the resulting topology in the scene . - loader = fingerSolver.addObject('MeshVTKLoader', name='loader', filename=f'{path}finger.vtk', + loader = finger_solver.addObject('MeshVTKLoader', name='loader', filename=f'{path}finger.vtk', translation="-17.5 -12.5 7.5", rotation="0 180 0") - fingerSolver.addObject('TetrahedronSetTopologyContainer', position=loader.position.getLinkPath(), + finger_solver.addObject('TetrahedronSetTopologyContainer', position=loader.position.getLinkPath(), tetras=loader.tetras.getLinkPath(), name='container') # Create a MechanicalObject component to stores the DoFs of the model - fingerSolver.addObject('MechanicalObject', template='Vec3', name='dofs') + finger_solver.addObject('MechanicalObject', template='Vec3', name='dofs') # Gives a mass to the model - fingerSolver.addObject('UniformMass', totalMass='0.075') + finger_solver.addObject('UniformMass', totalMass='0.075') # Add a TetrahedronFEMForceField component which implement an elastic material model # solved using the Finite Element Method on # tetrahedrons. - fingerSolver.addObject('TetrahedronFEMForceField', template='Vec3d', name='forceField', method='large', + finger_solver.addObject('TetrahedronFEMForceField', template='Vec3d', name='forceField', method='large', poissonRatio='0.45', youngModulus='500') - fingerSolver.addObject('BoxROI', name='ROI1', box='-18 -15 -8 2 -3 8', drawBoxes='true') - fingerSolver.addObject('RestShapeSpringsForceField', + finger_solver.addObject('BoxROI', name='ROI1', box='-18 -15 -8 2 -3 8', drawBoxes='true') + finger_solver.addObject('RestShapeSpringsForceField', points='@ROI1.indices', stiffness='1e12') ########################################## # Cable points # @@ -177,18 +180,18 @@ def addFEMObject(parentNode, path): # are constrained to slide on the cable. 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"] - femPoints = fingerSolver.addChild('femPoints') - femPoints.addObject('MechanicalObject', name="pointsInFEM", position=FEMpos, showObject="1", + fem_points = finger_solver.addChild('femPoints') + fem_points.addObject('MechanicalObject', name="pointsInFEM", position=FEMpos, showObject="1", showIndices="1") - femPoints.addObject('BarycentricMapping') + fem_points.addObject('BarycentricMapping') -def addMappedPoints(parentNode, name="pointsInFEM", position=None, showObject="1", +def addMappedPoints(parent_node, name="pointsInFEM", position=None, showObject="1", showIndices="1", 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"): if position is None: position = femPos - femPoints = parentNode.addChild(name) + femPoints = parent_node.addChild(name) femPoints.addObject( 'MechanicalObject', name=f'{name}Mo', @@ -201,7 +204,7 @@ def addMappedPoints(parentNode, name="pointsInFEM", position=None, showObject="1 return femPoints -def Finger(parentNode=None, name="Finger", rotation=None, translation=None, fixingBox=None, path=None, femPos=None): +def Finger(parent_node=None, name="Finger", rotation=None, translation=None, fixingBox=None, path=None, femPos=None): if fixingBox is None: fixingBox = [-18, -15, -8, 2, -3, 8] if rotation is None: @@ -212,7 +215,7 @@ def Finger(parentNode=None, name="Finger", rotation=None, translation=None, fixi path = f'{os.path.dirname(os.path.abspath(__file__))}/' # TODO : add physical properties as the finger input - finger = parentNode.addChild(name) + finger = parent_node.addChild(name) e_object = ElasticMaterialObject(finger, volumeMeshFileName=f'{path}finger.vtk', poissonRatio=0.48, @@ -236,6 +239,6 @@ def Finger(parentNode=None, name="Finger", rotation=None, translation=None, fixi # Test -def createScene(rootNode): - addHeader(rootNode) - addSolverNode(rootNode) +def createScene(root_node): + addHeader(root_node) + addSolverNode(root_node) From 1c7d59765c1eb26b6bfe510afbfd961af89d4b7c Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Mon, 7 Oct 2024 23:30:53 +0200 Subject: [PATCH 14/22] clean tuto scenes --- tutorial/tuto_scenes/tuto_1.py | 27 ++++++----- tutorial/tuto_scenes/tuto_2.py | 86 ++-------------------------------- 2 files changed, 17 insertions(+), 96 deletions(-) diff --git a/tutorial/tuto_scenes/tuto_1.py b/tutorial/tuto_scenes/tuto_1.py index 421a6b17..226b3e93 100644 --- a/tutorial/tuto_scenes/tuto_1.py +++ b/tutorial/tuto_scenes/tuto_1.py @@ -1,18 +1,19 @@ # -*- coding: utf-8 -*- -import Sofa -stiffness_param = 1.0e10 -beam_radius = 1.0 +stiffness_param: float = 1.0e10 +beam_radius: float = 1.0 -def _add_rigid_base(p_node): +def _add_rigid_base(p_node, positions=None): + if positions is None: + positions = [0, 0, 0, 0, 0, 0, 1] rigid_base_node = p_node.addChild("rigid_base") rigid_base_node.addObject( "MechanicalObject", template="Rigid3d", name="cosserat_base_mo", - position="0 0 0 0 0 0. 1", + position=positions, showObject=1, showObjectScale="0.1", ) @@ -49,13 +50,13 @@ def _add_cosserat_state(p_node, bending_states, list_sections_length, _radius=2. def _add_cosserat_frame( - p_node, - _bending_node, - framesF, - _section_curv_abs, - _frame_curv_abs, - _radius, - _beam_mass=0.0, + p_node, + _bending_node, + framesF, + _section_curv_abs, + _frame_curv_abs, + _radius, + _beam_mass=0.0, ): cosserat_in_Sofa_frame_node = p_node.addChild("cosserat_in_Sofa_frame_node") @@ -91,7 +92,7 @@ def createScene(root_node): root_node.addObject("RequiredPlugin", name='Sofa.Component.SolidMechanics.Spring') root_node.addObject("RequiredPlugin", name='Sofa.Component.StateContainer') root_node.addObject("RequiredPlugin", name='Sofa.Component.Visual') - + root_node.addObject( "VisualStyle", displayFlags="showBehaviorModels showCollisionModels showMechanicalMappings", diff --git a/tutorial/tuto_scenes/tuto_2.py b/tutorial/tuto_scenes/tuto_2.py index 8c0ca1b6..381c3c26 100644 --- a/tutorial/tuto_scenes/tuto_2.py +++ b/tutorial/tuto_scenes/tuto_2.py @@ -1,89 +1,9 @@ # -*- coding: utf-8 -*- -stiffness_param = 1.0e10 -beam_radius = 1.0 +from tuto_1 import _add_rigid_base, _add_cosserat_state, _add_cosserat_frame - -def _add_rigid_base(p_node): - rigid_base_node = p_node.addChild("rigid_base") - rigid_base_node.addObject( - "MechanicalObject", - template="Rigid3d", - name="cosserat_base_mo", - position="0 0 0 0 0 0. 1", - showObject=1, - showObjectScale="0.1", - ) - rigid_base_node.addObject( - "RestShapeSpringsForceField", - name="spring", - stiffness=stiffness_param, - angularStiffness=stiffness_param, - external_points="0", - mstate="@cosserat_base_mo", - points="0", - template="Rigid3d", - ) - return rigid_base_node - - -def _add_cosserat_state(p_node, bending_states, list_sections_length, _radius=2.0): - cosserat_coordinate_node = p_node.addChild("cosseratCoordinate") - print(f" ===> bendind state : {bending_states}") - cosserat_coordinate_node.addObject( - "MechanicalObject", - template="Vec3d", - name="cosserat_state", - position=bending_states, - ) - testNode = cosserat_coordinate_node.addObject( - "BeamHookeLawForceField", - crossSectionShape="circular", - length=list_sections_length, - radius=_radius, - youngModulus=1.0e4, - poissonRatio=0.4, - ) - print(f" the dire of node is : {dir(testNode)}") - return cosserat_coordinate_node - - -def _add_cosserat_frame( - p_node, - _bending_node, - framesF, - _section_curv_abs, - _frame_curv_abs, - _radius, - _beam_mass=0.0, -): - cosserat_in_Sofa_frame_node = p_node.addChild("cosserat_in_Sofa_frame_node") - - _bending_node.addChild(cosserat_in_Sofa_frame_node) - frames_mo = cosserat_in_Sofa_frame_node.addObject( - "MechanicalObject", - template="Rigid3d", - name="FramesMO", - position=framesF, - showIndices=0.0, - showObject=0, - showObjectScale=0.8, - ) - - cosserat_in_Sofa_frame_node.addObject("UniformMass", totalMass=_beam_mass) - - cosserat_in_Sofa_frame_node.addObject( - "DiscreteCosseratMapping", - curv_abs_input=_section_curv_abs, - curv_abs_output=_frame_curv_abs, - name="cosseratMapping", - input1=_bending_node.cosserat_state.getLinkPath(), - input2=p_node.cosserat_base_mo.getLinkPath(), - output=frames_mo.getLinkPath(), - debug=0, - radius=_radius, - ) - return cosserat_in_Sofa_frame_node +stiffness_param: float = 1.e10 +beam_radius: float = 1. def createScene(root_node): From 9332cde532eda04b65828ebd4c51f3aaa09e47ae Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Tue, 8 Oct 2024 10:56:25 +0200 Subject: [PATCH 15/22] update tuto3 scene regarding the modification of cosserat's prefab --- examples/python3/useful/geometry.py | 20 ++++++++++---------- examples/python3/useful/params.py | 21 +++++++++------------ tutorial/tuto_scenes/tuto_3.py | 25 +++++++++++++++++-------- 3 files changed, 36 insertions(+), 30 deletions(-) diff --git a/examples/python3/useful/geometry.py b/examples/python3/useful/geometry.py index ca42aa22..858a33dc 100644 --- a/examples/python3/useful/geometry.py +++ b/examples/python3/useful/geometry.py @@ -1,13 +1,14 @@ # from useful.params import BeamGeometryParameters + def calculate_beam_parameters(beamGeoParams): # Data validation checks for beamGeoParams attributes - if not all(hasattr(beamGeoParams, attr) for attr in ['beamLength', 'nbSection']): - raise ValueError("beamGeoParams must have, 'beamLength', and 'nbSection' attributes.") + if not all(hasattr(beamGeoParams, attr) for attr in ['beam_length', 'nb_section']): + raise ValueError("beamGeoParams must have, 'beam_length', and 'nb_section' attributes.") - total_length = beamGeoParams.beamLength - nb_sections = beamGeoParams.nbSection + total_length = beamGeoParams.beam_length + nb_sections = beamGeoParams.nb_section if not all(isinstance(val, (float)) for val in [total_length]): raise ValueError("init_pos and beamLength in beamGeoParams must be numeric values.") @@ -33,11 +34,11 @@ def calculate_beam_parameters(beamGeoParams): def calculate_frame_parameters(beamGeoParams): # Data validation checks for beamGeoParams attributes - if not all(hasattr(beamGeoParams, attr) for attr in ['beamLength', 'nbFrames']): + if not all(hasattr(beamGeoParams, attr) for attr in ['beam_length', 'nb_frames']): raise ValueError("beamGeoParams must have 'beamLength', and 'nbFrames' attributes.") - total_length = beamGeoParams.beamLength - nb_frames = beamGeoParams.nbFrames + total_length = beamGeoParams.beam_length + nb_frames = beamGeoParams.nb_frames if not all(isinstance(val, (int, float)) for val in [total_length]): raise ValueError("init_pos and beamLength in beamGeoParams must be numeric values.") @@ -57,7 +58,7 @@ def calculate_frame_parameters(beamGeoParams): cable_position_f.append([sol, 0, 0]) curv_abs_output_f.append(sol) - frames_f.append([total_length , 0, 0, 0, 0, 0, 1]) + frames_f.append([total_length, 0, 0, 0, 0, 0, 1]) cable_position_f.append([total_length, 0, 0]) curv_abs_output_f.append(total_length) @@ -77,11 +78,10 @@ def generate_edge_list(cable3DPos: list[list[float]]) -> list[list[int]]: number_of_points = len(cable3DPos) edges = [] for i in range(number_of_points - 1): - edges.append([i,i+1]) + edges.append([i, i + 1]) return edges - class CosseratGeometry: def __init__(self, beamGeoParams): # Data validation checks for beamGeoParams diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index d4785bcd..54f67136 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -9,18 +9,18 @@ class BeamGeometryParameters: """Cosserat Beam Geometry parameters""" - beamLength: float = 1.0 # beam length in m - nbSection: int = 5 # number of sections, sections along the beam length - nbFrames: int = 30 # number of frames along the beam - buildCollisionModel: int = 0 + beam_length: float = 1.0 # beam length in m + nb_section: int = 5 # number of sections, sections along the beam length + nb_frames: int = 30 # number of frames along the beam + build_collision_model: int = 0 def validate(self): - assert self.beamLength > 0, "Beam length must be positive" - assert self.nbSection > 0, "Number of sections must be positive" - assert self.nbFrames > 0, "Number of frames must be positive" - assert self.nbFrames >= self.nbSection, "Number of frames must be positive" - + assert self.beam_length > 0, "Beam length must be positive" + assert self.nb_section > 0, "Number of sections must be positive" + 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""" @@ -109,9 +109,6 @@ class ContactParameters: class Parameters: """Parameters for the Cosserat Beam""" - # beamPhysicsParams: BeamPhysicsParameters = field( - # default_factory=BeamPhysicsParameters - # ) beam_physics_params: BeamPhysicsBaseParameters = field(default_factory=BeamPhysicsParametersNoInertia) simu_params: SimulationParameters = field( default_factory=SimulationParameters diff --git a/tutorial/tuto_scenes/tuto_3.py b/tutorial/tuto_scenes/tuto_3.py index 9a2b2e73..118f3321 100644 --- a/tutorial/tuto_scenes/tuto_3.py +++ b/tutorial/tuto_scenes/tuto_3.py @@ -9,24 +9,33 @@ __copyright__ = "(c) 2021,Inria" __date__ = "October, 26 2021" -from useful.header import addHeader, addSolverNode, addVisual -from useful.params import BeamPhysicsParameters, BeamGeometryParameters, SimulationParameters +from useful.header import addHeader, addSolverNode from useful.params import Parameters +from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters from cosserat.CosseratBase import CosseratBase -geoParams = BeamGeometryParameters(init_pos=[0., 0., 0.], beamLength=30., showFramesObject=1, nbSection=2, nbFrames=12, buildCollisionModel=0) -physicsParams = BeamPhysicsParameters(beamMass=0.3, youngModulus=1.0e3, poissonRatio=0.38, beamRadius=1., beamLength=30) -simuParams = SimulationParameters() -Params = Parameters(beamGeoParams=geoParams, beamPhysicsParams=physicsParams, simuParams=simuParams) +geoParams = BeamGeometryParameters(beam_length=30., nb_section=2, nb_frames=12, build_collision_model=0) +physicsParams = BeamPhysicsParametersNoInertia(beam_mass=0.3, young_modulus=1.0e3, poisson_ratio=0.38, beam_radius=1., + beam_length=30) +Params = Parameters(beam_geo_params=geoParams, beam_physics_params=physicsParams) def createScene(root_node): addHeader(root_node) root_node.gravity = [0, -9.81, 0.] - solver_node = addSolverNode(root_node, name="solver_node") # create cosserat Beam - solver_node.addChild(CosseratBase(parent=solver_node, params=Params)) + beam = solver_node.addChild(CosseratBase(parent=solver_node, 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" + ) return root_node From 0752b10078f101afe5c950a8cb7f0b7f7345f33a Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 9 Oct 2024 17:54:26 +0200 Subject: [PATCH 16/22] 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 | 50 ++++++----- examples/python3/useful/header.py | 37 ++++---- examples/python3/useful/params.py | 3 +- examples/python3/useful/utils.py | 6 +- .../geo_cable_driven_cosserat_beam.py | 86 +++++++++++++++++++ 5 files changed, 135 insertions(+), 47 deletions(-) create mode 100644 tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index 5e057571..d2896566 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/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() diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index 4df2e621..776c6803 100644 --- a/examples/python3/useful/header.py +++ b/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,7 +85,7 @@ 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. @@ -93,14 +93,16 @@ def contactHeader(parent_node, contact_params = DefaultContactParams): 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') diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index 54f67136..51f72cac 100644 --- a/examples/python3/useful/params.py +++ b/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" diff --git a/examples/python3/useful/utils.py b/examples/python3/useful/utils.py index ca9e6651..98144a5f 100644 --- a/examples/python3/useful/utils.py +++ b/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", diff --git a/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py b/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py new file mode 100644 index 00000000..6ca71f6c --- /dev/null +++ b/tutorial/tuto_scenes/geo_cable_driven_cosserat_beam.py @@ -0,0 +1,86 @@ +# -*- 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 +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 From a5e9503d920d91f05d2ec505356fb86fb39d4b47 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 9 Oct 2024 23:05:17 +0200 Subject: [PATCH 17/22] 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 6ca71f6c..d4bebc90 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 00000000..a7e28c81 --- /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 00000000..edffbc07 --- /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 00000000..da655b1c --- /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 32a5617c..71fdcecc 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", From 21b4cdfb55033e66110a961719914d3bbb46312f Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Mon, 14 Oct 2024 18:31:10 +0200 Subject: [PATCH 18/22] 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 From 368f0d2b4a6c1c0d71d77825b5baec27b0684150 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Mon, 14 Oct 2024 18:32:13 +0200 Subject: [PATCH 19/22] Refactor CosseratBase and tutorial scenes to update parameter handling and fix typos --- examples/python3/cosserat/CosseratBase.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index d2896566..1b27c9b7 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -53,7 +53,7 @@ class CosseratBase(Sofa.Prefab): def __init__(self, *args, **kwargs): Sofa.Prefab.__init__(self, *args, **kwargs) - self.params = kwargs.get("beam_params") # Use the Parameters class with default values + self.params = kwargs.get("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'] From 07fa31e171d0d4515a0dbb7050508ff56e99cbad Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 16 Oct 2024 06:59:15 +0200 Subject: [PATCH 20/22] Enhance FEM object integration and update beam parameters Modify addFEMObject function to return solver and points nodes Update beam parameters in tutorial scenes (tuto_3, tuto_4, tuto_5) Refactor FEM object creation in tuto_5 scene Adjust cable constraint parameters and mapping in tuto_5 Minor code cleanup and parameter naming consistency improvements --- examples/python3/useful/header.py | 6 ++-- tutorial/tuto_scenes/tuto_3.py | 2 +- tutorial/tuto_scenes/tuto_4.py | 3 +- tutorial/tuto_scenes/tuto_5.py | 49 ++++++++++++++++--------------- 4 files changed, 32 insertions(+), 28 deletions(-) diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index 776c6803..f3b2da33 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -153,8 +153,8 @@ def addSolverNode(parent_node, name='solverNode', template='CompressedRowSparseM return solver_node -def addFEMObject(parent_node, path): - finger_solver = addSolverNode(parent_node) +def addFEMObject(parent_node, path, name='FEMFinger'): + finger_solver = addSolverNode(parent_node, name=name) # 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', @@ -188,6 +188,8 @@ def addFEMObject(parent_node, path): showIndices="1") fem_points.addObject('BarycentricMapping') + return finger_solver, fem_points + def addMappedPoints(parent_node, name="pointsInFEM", position=None, showObject="1", showIndices="1", 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"): diff --git a/tutorial/tuto_scenes/tuto_3.py b/tutorial/tuto_scenes/tuto_3.py index 118f3321..8ceb786b 100644 --- a/tutorial/tuto_scenes/tuto_3.py +++ b/tutorial/tuto_scenes/tuto_3.py @@ -14,7 +14,7 @@ from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters from cosserat.CosseratBase import CosseratBase -geoParams = BeamGeometryParameters(beam_length=30., nb_section=2, nb_frames=12, build_collision_model=0) +geoParams = BeamGeometryParameters(beam_length=30., nb_section=32, nb_frames=32, build_collision_model=0) physicsParams = BeamPhysicsParametersNoInertia(beam_mass=0.3, young_modulus=1.0e3, poisson_ratio=0.38, beam_radius=1., beam_length=30) Params = Parameters(beam_geo_params=geoParams, beam_physics_params=physicsParams) diff --git a/tutorial/tuto_scenes/tuto_4.py b/tutorial/tuto_scenes/tuto_4.py index 2ea454f1..4be71a69 100644 --- a/tutorial/tuto_scenes/tuto_4.py +++ b/tutorial/tuto_scenes/tuto_4.py @@ -25,7 +25,8 @@ 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) + beam_radius=_beam_radius, beam_length=_beam_length) + simuParams = SimulationParameters() Params = Parameters(beam_geo_params=geoParams, beam_physics_params=physicsParams, simu_params=simuParams) diff --git a/tutorial/tuto_scenes/tuto_5.py b/tutorial/tuto_scenes/tuto_5.py index c6867441..3aab1125 100644 --- a/tutorial/tuto_scenes/tuto_5.py +++ b/tutorial/tuto_scenes/tuto_5.py @@ -9,7 +9,7 @@ __copyright__ = "(c) 2021,Inria" __date__ = "October, 26 2021" -from useful.header import addHeader, addSolverNode, Finger +from useful.header import addHeader, addSolverNode, addFEMObject from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters, SimulationParameters from useful.params import Parameters from cosserat.CosseratBase import CosseratBase @@ -19,21 +19,21 @@ import os from math import pi from controller import FingerController -from numpy import array - +_beam_radius = 0.5 +_beam_length = 81. +_nb_section = 32 +force_null = [0., 0., 0., 0., 0., 0.] # N path = f'{os.path.dirname(os.path.abspath(__file__))}/../../examples/python3/actuators/mesh/' -geoParams = BeamGeometryParameters(init_pos=[0., 0., 0.], beamLength=81., showFramesObject=1, - nbSection=12, nbFrames=32, buildCollisionModel=0) -physicsParams = BeamPhysicsParametersNoInertia(beamMass=1., youngModulus=5.e6, poissonRatio=0.4, beamRadius=0.5, - beamLength=30) +geoParams = BeamGeometryParameters(beam_length=_beam_length, nb_section=_nb_section, nb_frames=_nb_section) +physicsParams = BeamPhysicsParametersNoInertia(beam_mass=1., young_modulus=5.e6, poisson_ratio=0.4, + beam_radius=_beam_radius, beam_length=_beam_length) 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 -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]] +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]] class ForceController(Sofa.Core.Controller): @@ -44,7 +44,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 @@ -69,7 +69,7 @@ def compute_force(self): for i, v in enumerate(vec): force[0][i] = v - def compute_orthogonal_force(self): + def compute_orthogonal_force(self) -> None: 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. @@ -80,13 +80,13 @@ def compute_orthogonal_force(self): for count in range(3): force[0][count] = vec[count] - def rotate_force(self): + def rotate_force(self) -> None: 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.rotateFromEuler([self.theta, 0., 0.]) # apply rotation around x-axis vec.normalize() for i, v in enumerate(vec): position[0][i + 3] = v @@ -100,10 +100,10 @@ def onKeypressedEvent(self, event): def createScene(root_node): - addHeader(root_node, isConstrained=True) + addHeader(root_node, is_constrained=True) root_node.gravity = [0, 0., 0.] - solver_node = addSolverNode(root_node, name="solver_node", isConstrained=True) + solver_node = addSolverNode(root_node, name="cable_node", isConstrained=True) # create cosserat Beam cosserat_beam = solver_node.addChild(CosseratBase(parent=solver_node, params=Params)) @@ -112,13 +112,15 @@ def createScene(root_node): # Finger node femFingerNode = root_node.addChild('femFingerNode') """ Add FEM finger to the scene""" - finger_node, fem_points_node = Finger(femFingerNode, name="Finger", rotation=array([0.0, 180.0, 0.0]), - translation=array([-17.5, -12.5, 7.5]), path=path) + # finger_node, fem_points_node = Finger(femFingerNode, name="Finger", rotation=array([0.0, 180.0, 0.0]), + # translation=array([-17.5, -12.5, 7.5]), path=path) + + finger_node, fem_points_node = addFEMObject(root_node, path=path, name="Finger") # 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 componante holding the degree of freedom of our + # 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", @@ -129,20 +131,19 @@ def createScene(root_node): distance_node = cable_state_node.addChild('distance_node') fem_points_node.addChild(distance_node) distance = distance_node.addObject('MechanicalObject', template='Vec3d', position=femPos, - name="distancePointsMO", showObject='1', showObjectScale='1') - + name="distancePointsMO", showObject='1', showObjectScale='1') """The controller of the cable is added to the scene""" cable_state_node.addObject(FingerController(cosserat_beam.rigidBaseNode.RigidBaseMO, - cosserat_beam.cosseratCoordinateNode.cosseratCoordinateMO)) + cosserat_beam.cosseratCoordinateNode.cosseratCoordinateMO)) inputCableMO = cable_state.getLinkPath() inputFEMCableMO = fem_points_node.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=inputFEMCableMO, indices="5", - input2=inputCableMO, output=outputPointMO, direction="@../../FramesMO.position") + distance_node.addObject('DifferenceMultiMapping', name="pointsMulti", input1=inputFEMCableMO, indices="6", + input2=inputCableMO, output=outputPointMO, direction="@../../FramesMO.position") return root_node # # this constance force is used only in the case we are doing force_type 1 or 2 From 8f54ed3934660e123ca2c9730fe8af08596532fe Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 30 Oct 2024 14:56:05 +0100 Subject: [PATCH 21/22] Fix logging in SparseLDLSolver and update gravity in tutorial - Disabled `printLog` in `SparseLDLSolver` within `header.py` for cleaner output. - Set `root_node.gravity` to `[0, -9.81, 0]` in `tuto_5.py` for realistic gravitational effect. - Added flexibility in constraint setup for `cable_node` by introducing `is_constrained` parameter. - Included missing `return root_node` statement to ensure complete scene creation in `createScene()`. --- examples/python3/useful/header.py | 2 +- tutorial/tuto_scenes/tuto_5.py | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index f3b2da33..a1068794 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -146,7 +146,7 @@ def addSolverNode(parent_node, name='solverNode', template='CompressedRowSparseM if iterative: solver_node.addObject('CGLinearSolver', name='Solver', template=template) else: - solver_node.addObject('SparseLDLSolver', name='Solver', template=template, printLog=True) + solver_node.addObject('SparseLDLSolver', name='Solver', template=template, printLog=False) if isConstrained: solver_node.addObject('GenericConstraintCorrection', linearSolver=solver_node.Solver.getLinkPath()) diff --git a/tutorial/tuto_scenes/tuto_5.py b/tutorial/tuto_scenes/tuto_5.py index 3aab1125..b37662fb 100644 --- a/tutorial/tuto_scenes/tuto_5.py +++ b/tutorial/tuto_scenes/tuto_5.py @@ -34,7 +34,7 @@ Params = Parameters(beam_geo_params=geoParams, beam_physics_params=physicsParams, simu_params=simuParams) 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]] - +is_constrained = False class ForceController(Sofa.Core.Controller): def __init__(self, *args, **kwargs): @@ -101,9 +101,9 @@ def onKeypressedEvent(self, event): def createScene(root_node): addHeader(root_node, is_constrained=True) - root_node.gravity = [0, 0., 0.] + root_node.gravity = [0, -9.81, 0.] - solver_node = addSolverNode(root_node, name="cable_node", isConstrained=True) + solver_node = addSolverNode(root_node, name="cable_node", isConstrained=is_constrained) # create cosserat Beam cosserat_beam = solver_node.addChild(CosseratBase(parent=solver_node, params=Params)) @@ -117,6 +117,8 @@ def createScene(root_node): finger_node, fem_points_node = addFEMObject(root_node, path=path, name="Finger") + return 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') From 04ec1c8180733447988227c6e29862acf8d4b791 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 15 Nov 2024 18:10:38 +0100 Subject: [PATCH 22/22] enhance and restructure FEM mesh handling and visualization Added new functions attach_mesh_with_springs, attach_3d_points_to_meca_with_barycentric_mapping, and add_finger_mesh_force_field_Object to modularize and simplify FEM mesh handling and visualization. Introduced show_mecha_visual for toggling visibility of mechanical objects and indices. Replaced addFEMObject with add_finger_mesh_force_field_Object for improved clarity and functionality. Updated tuto_5.py to utilize new FEM mesh functions, enabling better integration and enhanced visualization. Removed redundant code and refactored solver and geometry attachment logic for better maintainability. --- examples/python3/useful/header.py | 48 ++++++++++++++++--------------- tutorial/tuto_scenes/tuto_5.py | 18 +++++++++--- 2 files changed, 39 insertions(+), 27 deletions(-) diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index a1068794..d353195e 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -17,7 +17,9 @@ import os from useful.params import ContactParameters as DefaultContactParams - +def show_mecha_visual(node, show=True): + node.showObject = show + node.showIndices = show def addHeader(parent_node, multithreading=False, inverse=False, is_constrained=False, is_contact=False, contact_params=None): """ @@ -152,44 +154,44 @@ def addSolverNode(parent_node, name='solverNode', template='CompressedRowSparseM return solver_node +def attach_mesh_with_springs(mesh_node, _box='-18 -15 -8 2 -3 8'): + mesh_node.addObject('BoxROI', name='ROI1', box=_box, drawBoxes='true') + mesh_node.addObject('RestShapeSpringsForceField', + points='@ROI1.indices', stiffness='1e12') + +def attach_3d_points_to_meca_with_barycentric_mapping(parent_node, name='node_name', + list_of_points=[" 0.0 0 0 15 0 0 30 0 0 45 0 0 60 0 0 66 0 0 81 0.0 0.0"]): + points_node = parent_node.addChild(name) + points_node.addObject('MechanicalObject', name=name+"_mo", position=list_of_points) + points_node.addObject('BarycentricMapping') + return points_node -def addFEMObject(parent_node, path, name='FEMFinger'): - finger_solver = addSolverNode(parent_node, name=name) +def add_finger_mesh_force_field_Object(parent_node, path): + parent_node.addObject('VisualStyle', displayFlags='showForceFields') + #finger_solver = addSolverNode(parent_node, name=name) # 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', + loader = parent_node.addObject('MeshVTKLoader', name='loader', filename=f'{path}finger.vtk', translation="-17.5 -12.5 7.5", rotation="0 180 0") - finger_solver.addObject('TetrahedronSetTopologyContainer', position=loader.position.getLinkPath(), + parent_node.addObject('TetrahedronSetTopologyContainer', position=loader.position.getLinkPath(), 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') + parent_node.addObject('MechanicalObject', template='Vec3', name='dofs') # Gives a mass to the model - finger_solver.addObject('UniformMass', totalMass='0.075') + parent_node.addObject('UniformMass', totalMass='1.75') # Add a TetrahedronFEMForceField component which implement an elastic material model # solved using the Finite Element Method on # tetrahedrons. - finger_solver.addObject('TetrahedronFEMForceField', template='Vec3d', name='forceField', method='large', + parent_node.addObject('TetrahedronFEMForceField', template='Vec3d', name='forceField', method='large', poissonRatio='0.45', youngModulus='500') + attach_mesh_with_springs(parent_node) - finger_solver.addObject('BoxROI', name='ROI1', box='-18 -15 -8 2 -3 8', drawBoxes='true') - finger_solver.addObject('RestShapeSpringsForceField', - points='@ROI1.indices', stiffness='1e12') - ########################################## - # Cable points # - ########################################## # Mapped points inside the finger volume, these points attached to the FE model # are constrained to slide on the cable. - - 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") - fem_points.addObject('BarycentricMapping') - - return finger_solver, fem_points - + points_node = attach_3d_points_to_meca_with_barycentric_mapping(parent_node) + return points_node def addMappedPoints(parent_node, name="pointsInFEM", position=None, showObject="1", showIndices="1", 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"): diff --git a/tutorial/tuto_scenes/tuto_5.py b/tutorial/tuto_scenes/tuto_5.py index b37662fb..b34767c1 100644 --- a/tutorial/tuto_scenes/tuto_5.py +++ b/tutorial/tuto_scenes/tuto_5.py @@ -9,7 +9,7 @@ __copyright__ = "(c) 2021,Inria" __date__ = "October, 26 2021" -from useful.header import addHeader, addSolverNode, addFEMObject +from useful.header import addHeader, addSolverNode, add_finger_mesh_force_field_Object, show_mecha_visual from useful.params import BeamPhysicsParametersNoInertia, BeamGeometryParameters, SimulationParameters from useful.params import Parameters from cosserat.CosseratBase import CosseratBase @@ -19,6 +19,7 @@ import os from math import pi from controller import FingerController +from geo_cosserat_cable_driven_cosserat_beam import show_mecha_visual _beam_radius = 0.5 _beam_length = 81. @@ -100,9 +101,18 @@ def onKeypressedEvent(self, event): def createScene(root_node): - addHeader(root_node, is_constrained=True) + addHeader(root_node, is_constrained=False) root_node.gravity = [0, -9.81, 0.] + # Add FEM finger node + finger_node = addSolverNode(root_node, name="finger_node") + # this function attach the geometry and force field to predefine solver node + # It also fixe finger regarding predefine box + attached_3d_points_fem_node = add_finger_mesh_force_field_Object(finger_node, path=path) + show_mecha_visual(attached_3d_points_fem_node, show=True) + + return root_node + solver_node = addSolverNode(root_node, name="cable_node", isConstrained=is_constrained) # create cosserat Beam @@ -110,12 +120,12 @@ def createScene(root_node): cosserat_frames_node = cosserat_beam.cosseratFrame # Finger node - femFingerNode = root_node.addChild('femFingerNode') + #femFingerNode = root_node.addChild('femFingerNode') """ Add FEM finger to the scene""" # finger_node, fem_points_node = Finger(femFingerNode, name="Finger", rotation=array([0.0, 180.0, 0.0]), # translation=array([-17.5, -12.5, 7.5]), path=path) - finger_node, fem_points_node = addFEMObject(root_node, path=path, name="Finger") + return root_node