From 37b54a7e8aeaaa51e3bc0d9d69caad2d10c40356 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Fri, 30 Aug 2024 17:41:06 +0200 Subject: [PATCH 1/3] update python prefabs --- examples/python3/cosserat/CosseratBase.py | 8 +++++--- examples/python3/useful/geometry.py | 8 ++++++-- examples/python3/useful/header.py | 24 ++++++++++++++--------- examples/python3/useful/params.py | 5 +++-- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/examples/python3/cosserat/CosseratBase.py b/examples/python3/cosserat/CosseratBase.py index c4e29950..d0948d82 100644 --- a/examples/python3/cosserat/CosseratBase.py +++ b/examples/python3/cosserat/CosseratBase.py @@ -146,7 +146,9 @@ def _addRigidBaseNode(self): template="Rigid3d", name="RigidBaseMO", showObjectScale=0.2, - position=positions + position=positions, + translation=self.translation, + rotation=self.rotation ) rigidBaseNodeMo.showObject.setParent(self.showObject) @@ -226,8 +228,8 @@ def _addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): template="Rigid3d", name="FramesMO", position=framesF, - showIndices=self.params.beamGeoParams.showFramesObject, - showObject=self.params.beamGeoParams.showFramesObject, + showIndices=self.params.beamGeoParams.show_frames_indices, + showObject=self.params.beamGeoParams.show_frames_object, showObjectScale=1.8, # Todo: remove this hard code ) diff --git a/examples/python3/useful/geometry.py b/examples/python3/useful/geometry.py index bb8d86f7..4bec898b 100644 --- a/examples/python3/useful/geometry.py +++ b/examples/python3/useful/geometry.py @@ -67,7 +67,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[int]: +def generate_edge_list(cable3DPos: List[List[float]]) -> list[list[int]]: """ Generate an edge list required in the EdgeSetTopologyContainer component. @@ -78,7 +78,11 @@ def generate_edge_list(cable3DPos: List[List[float]]) -> List[int]: List[int]: A list of indices forming edges in the EdgeSetTopologyContainer. """ number_of_points = len(cable3DPos) - return [i for i in range(number_of_points - 1) for _ in range(2)] + edges = [] + for i in range(number_of_points - 1): + edges.append([i,i+1]) + return edges + class CosseratGeometry: diff --git a/examples/python3/useful/header.py b/examples/python3/useful/header.py index ea77ced6..aceac414 100644 --- a/examples/python3/useful/header.py +++ b/examples/python3/useful/header.py @@ -17,7 +17,7 @@ import os -def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=False, isContact=False): +def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=False, isContact=False, params=None): """ Adds to rootNode the default headers for a simulation with contact. Also adds and returns three nodes: - Settings @@ -68,7 +68,7 @@ def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=Fal parentNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels showCollisionModels ' 'hideBoundingCollisionModels hideForceFields ' - 'hideInteractionForceFields hideWireframe showMechanicalMappings') + 'showInteractionForceFields hideWireframe showMechanicalMappings') if isConstrained: parentNode.addObject('FreeMotionAnimationLoop', parallelCollisionDetectionAndFreeMotion=multithreading, parallelODESolving=multithreading) @@ -78,20 +78,26 @@ def addHeader(parentNode, multithreading=False, inverse=False, isConstrained=Fal multithreading=multithreading, epsilon=1) else: parentNode.addObject('GenericConstraintSolver', name='ConstraintSolver', tolerance=1e-8, maxIterations=100, - multithreading=multithreading) + multithreading=multithreading, printLog=1) if isContact: - contactHeader(parentNode) + contactHeader(parentNode, _contact_params=params.contactParams) # components needed for contact modeling -def contactHeader(parentNode): +def contactHeader(parentNode, _contact_params=None): parentNode.addObject('CollisionPipeline') parentNode.addObject("DefaultVisualManagerLoop") - parentNode.addObject('RuleBasedContactManager', responseParams='mu=0.8', response='FrictionContactConstraint') parentNode.addObject('BruteForceBroadPhase') parentNode.addObject('BVHNarrowPhase') - parentNode.addObject('LocalMinDistance', alarmDistance=0.05, contactDistance=0.01) + 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 addVisual(node): @@ -105,7 +111,7 @@ def addVisual(node): """ node.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' 'hideBoundingCollisionModels hideForceFields ' - 'hideInteractionForceFields hideWireframe showMechanicalMappings') + 'showInteractionForceFields hideWireframe showMechanicalMappings') return node @@ -134,7 +140,7 @@ def addSolverNode(node, name='solverNode', template='CompressedRowSparseMatrixd' if iterative: solverNode.addObject('CGLinearSolver', name='Solver', template=template) else: - solverNode.addObject('SparseLDLSolver', name='Solver', template=template) + solverNode.addObject('SparseLDLSolver', name='Solver', template=template, printLog=True) if isConstrained: solverNode.addObject('GenericConstraintCorrection', linearSolver=solverNode.Solver.getLinkPath()) diff --git a/examples/python3/useful/params.py b/examples/python3/useful/params.py index 2f553ed4..25e4805b 100644 --- a/examples/python3/useful/params.py +++ b/examples/python3/useful/params.py @@ -18,7 +18,8 @@ class BeamGeometryParameters: ) # The beam rigid base position as a list [x, y, z] """Parameters for the visualisation of the beam""" - showFramesObject: int = 1 + show_frames_object: bool = True + show_frames_indices: bool = False showRigidBaseObject: int = 1 @@ -71,7 +72,7 @@ class VisualParameters: class ContactParameters: """Contact parameters""" - responseParams: str = "mu=0.8" + responseParams: str = "mu=0.0" response: str = "FrictionContactConstraint" alarmDistance: float = 0.05 contactDistance: float = 0.01 From ecbd8b0de4aaa39ecf6bb2cc8a3c55d8840fb3e8 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Tue, 3 Dec 2024 20:31:09 +0100 Subject: [PATCH 2/3] Refactor PCS_Example3.py: Remove unused code, fix variable names, and rename file - Renamed `wip/PCS_Example3.py` to `PCS_Example3.py` for clarity and organization. - Removed commented-out and unused code for better readability. - Fixed variable name `force` to `forces` and corrected index handling in `ForceController`. - Adjusted references from `nonLinearCosserat` to `PCS_Cosserat`. --- examples/python3/{wip => }/PCS_Example3.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) rename examples/python3/{wip => }/PCS_Example3.py (90%) diff --git a/examples/python3/wip/PCS_Example3.py b/examples/python3/PCS_Example3.py similarity index 90% rename from examples/python3/wip/PCS_Example3.py rename to examples/python3/PCS_Example3.py index 06534de6..8d68d703 100644 --- a/examples/python3/wip/PCS_Example3.py +++ b/examples/python3/PCS_Example3.py @@ -11,7 +11,6 @@ import Sofa from cosserat.cosseratObject import Cosserat -# from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry from math import sqrt @@ -19,7 +18,6 @@ from splib3.numerics import Quat LegendrePolyOrder = 5 -# initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0]] initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0]] YM = 4.015e8 @@ -52,11 +50,10 @@ def onAnimateEndEvent(self, event): orientation = Quat( position[3], position[4], position[5], position[6]) # Get the force direction in order to remain orthogonal to the last section of beam - with self.forceNode.force.writeable() as force: + with self.forceNode.forces.writeable() as force: vec = orientation.rotate([0., self.forceCoeff, 0.]) - # print(f' The new vec is : {vec}') for count in range(3): - force[count] = vec[count] + force[0][count] = vec[count] def onKeypressedEvent(self, event): key = event['key'] @@ -91,12 +88,12 @@ def createScene(rootNode): Cosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, inertialParams=inertialParams, name="cosserat", radius=Rb, youngModulus=YM)) - beamFrame = nonLinearCosserat.cosseratFrame + beamFrame = PCS_Cosserat.cosseratFrame constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=0.02, - indices=nonLinearConfig['nbFramesF'], force=F1) + indices=nonLinearConfig['nbFramesF'], forces=F1) - nonLinearCosserat = solverNode.addObject( + solverNode.addObject( ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) # ---------------- From fe0b069643cd0c4b1dc27726c92e36f799ea014d Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Tue, 3 Dec 2024 22:19:24 +0100 Subject: [PATCH 3/3] Refactor PCS_Example3.py: Remove unused code, fix variable names, and rename file --- examples/python3/PCS_Example3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python3/PCS_Example3.py b/examples/python3/PCS_Example3.py index 8d68d703..af926d62 100644 --- a/examples/python3/PCS_Example3.py +++ b/examples/python3/PCS_Example3.py @@ -69,7 +69,7 @@ def createScene(rootNode): rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList]) rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' 'hideBoundingCollisionModels hideForceFields ' - 'hideInteractionForceFields hideWireframe') + 'hideInteractionForceFields hideWireframe showMechanicalMappings') rootNode.dt.value = 0.02 rootNode.gravity.value = [0., 0., 0.] rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765')