From 352c6ef412d894d76fdd9b3157297c343caebf27 Mon Sep 17 00:00:00 2001 From: eulalie Date: Mon, 20 Nov 2023 15:11:14 +0100 Subject: [PATCH] [tutorials] cleaning PneunetGripper --- .../details/fingerController.py | 4 +- .../tutorials/PneunetGripper/details/param.py | 64 ++++---- .../details/step1-meshLoading.py | 8 +- .../details/step2-forceFieldAndMass.py | 14 +- .../details/step3-stiffLayer.py | 24 ++- .../details/step4-boundaryConditions.py | 28 ++-- .../step5-timeIntegrationAndMatrixSolver.py | 33 ++-- ...umaticActuatorAndPythonScriptController.py | 44 ++++-- .../details/step7-grabTheCube.py | 149 ++++++++++-------- .../PneunetGripper/details/step7-withSTLIB.py | 31 +++- .../details/wholeGripperController.py | 40 ++--- 11 files changed, 268 insertions(+), 171 deletions(-) diff --git a/examples/tutorials/PneunetGripper/details/fingerController.py b/examples/tutorials/PneunetGripper/details/fingerController.py index c7e150e2..5dfebd31 100644 --- a/examples/tutorials/PneunetGripper/details/fingerController.py +++ b/examples/tutorials/PneunetGripper/details/fingerController.py @@ -9,8 +9,8 @@ class FingerController(Sofa.Core.Controller): def __init__(self, *args, **kwargs): Sofa.Core.Controller.__init__(self, args, kwargs) self.node = args[0] - self.fingerNode = self.node.getChild('finger') - self.pressureConstraint = self.fingerNode.cavity.getObject('SurfacePressureConstraint') + self.fingerNode = self.node.getChild('Finger') + self.pressureConstraint = self.fingerNode.Cavity.getObject('SurfacePressureConstraint') def onKeypressedEvent(self, e): pressureValue = self.pressureConstraint.value.value[0] diff --git a/examples/tutorials/PneunetGripper/details/param.py b/examples/tutorials/PneunetGripper/details/param.py index 2f9072a6..25719cc4 100644 --- a/examples/tutorials/PneunetGripper/details/param.py +++ b/examples/tutorials/PneunetGripper/details/param.py @@ -1,18 +1,16 @@ # -*- coding: utf-8 -*- import math -#################### USER PARAM ########################## - -cubeParam = { 'name' : "Cube", - 'totalMass' : 0.0008, - 'translation' : [15.0,20.0,0.0], - 'uniformScale' : 21} - -floorParam = { 'name' : "Plane", - 'color' : [1.0, 0.0, 1.0], - 'isAStaticObject' : True, - 'uniformScale' : 10} +# User parameters +cubeParam = {'name': "Cube", + 'totalMass': 0.01, + 'translation': [15.0, 20.0, 0.0], + 'uniformScale': 21} +floorParam = {'name': "Plane", + 'color': [1.0, 0.0, 1.0], + 'isAStaticObject': True, + 'uniformScale': 3} # Fingers Mesh & Shared Parameters fingersVolumeMesh = 'data/mesh/pneunetCutCoarse.vtk' @@ -25,8 +23,8 @@ # Fingers Position heightInitial = 150 radius = 70 -angle1 = 120*math.pi/180 # Angle between 1st and 2nd finger in radian -angle2 = 240*math.pi/180 # Angle between 1st and 3rd finger in radian +angle1 = 120 * math.pi / 180 # Angle between 1st and 2nd finger in radian +angle2 = 240 * math.pi / 180 # Angle between 1st and 3rd finger in radian youngModulusFingers = 500 youngModulusStiffLayerFingers = 1500 @@ -34,24 +32,24 @@ fingersMass = 0.04 cavitiesInitialValue = 0.0001 -# Paramters for each Fingers +# Parameters for each Fingers fingersParameters = [ - { - 'name' : 'finger1', - 'rotation' : [0.0, 0.0, -270.0], - 'translation' : [100.0, heightInitial , 0.0], - 'ROIBox' : [100, heightInitial-10, -20, 70, heightInitial, 20], - }, - { - 'name' : 'finger2', - 'rotation' : [360 - angle1*180/math.pi, 0.0, 90.0], - 'translation' : [0, heightInitial, radius*math.cos(angle1-math.pi/2)], - 'ROIBox' : [100, heightInitial-10, -20, 70, heightInitial, 20], - }, - { - 'name' : 'finger3', - 'rotation' : [360 - angle2*180/math.pi, 0.0, 90.0], - 'translation' : [0.0, heightInitial, radius*math.cos(angle2-math.pi/2)], - 'ROIBox' : [100, heightInitial-10, -20, 70, heightInitial, 20], - } - ] + { + 'name': 'Finger1', + 'rotation': [0.0, 0.0, -270.0], + 'translation': [100.0, heightInitial, 0.0], + 'ROIBox': [100, heightInitial - 10, -20, 70, heightInitial, 20], + }, + { + 'name': 'Finger2', + 'rotation': [360 - angle1 * 180 / math.pi, 0.0, 90.0], + 'translation': [0, heightInitial, radius * math.cos(angle1 - math.pi / 2)], + 'ROIBox': [100, heightInitial - 10, -20, 70, heightInitial, 20], + }, + { + 'name': 'Finger3', + 'rotation': [360 - angle2 * 180 / math.pi, 0.0, 90.0], + 'translation': [0.0, heightInitial, radius * math.cos(angle2 - math.pi / 2)], + 'ROIBox': [100, heightInitial - 10, -20, 70, heightInitial, 20], + } +] diff --git a/examples/tutorials/PneunetGripper/details/step1-meshLoading.py b/examples/tutorials/PneunetGripper/details/step1-meshLoading.py index b3d39b97..45985fff 100644 --- a/examples/tutorials/PneunetGripper/details/step1-meshLoading.py +++ b/examples/tutorials/PneunetGripper/details/step1-meshLoading.py @@ -3,7 +3,13 @@ def createScene(rootNode): rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') - finger = rootNode.addChild('finger') + rootNode.addObject("DefaultAnimationLoop") + + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + + finger = rootNode.addChild('Finger') finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk') finger.addObject('MeshTopology', src='@loader', name='container') finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1) diff --git a/examples/tutorials/PneunetGripper/details/step2-forceFieldAndMass.py b/examples/tutorials/PneunetGripper/details/step2-forceFieldAndMass.py index 70bfaf6a..a66de495 100644 --- a/examples/tutorials/PneunetGripper/details/step2-forceFieldAndMass.py +++ b/examples/tutorials/PneunetGripper/details/step2-forceFieldAndMass.py @@ -4,12 +4,20 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showForceFields') rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') - rootNode.findData('gravity').value = [-9810, 0, 0] + rootNode.gravity.value = [-9810, 0, 0] + rootNode.addObject("DefaultAnimationLoop") - finger = rootNode.addChild('finger') + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + + finger = rootNode.addChild('Finger') finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk') finger.addObject('MeshTopology', src='@loader', name='container') finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1) finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=500) - finger.addObject('UniformMass', totalMass=0.0008) + finger.addObject('UniformMass', totalMass=0.04) diff --git a/examples/tutorials/PneunetGripper/details/step3-stiffLayer.py b/examples/tutorials/PneunetGripper/details/step3-stiffLayer.py index 6039a0b8..d4cb8cab 100644 --- a/examples/tutorials/PneunetGripper/details/step3-stiffLayer.py +++ b/examples/tutorials/PneunetGripper/details/step3-stiffLayer.py @@ -2,21 +2,31 @@ def createScene(rootNode): - rootNode.addObject('VisualStyle', displayFlags='showForceFields') + rootNode.addObject('VisualStyle', displayFlags='showForceFields showWireframe') rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') - rootNode.findData('gravity').value = [-9810, 0, 0]; + rootNode.gravity.value = [-9810, 0, 0] + rootNode.addObject("DefaultAnimationLoop") - finger = rootNode.addChild('finger') + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') # Needed to use components [BoxROI] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + + finger = rootNode.addChild('Finger') finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk') finger.addObject('MeshTopology', src='@loader', name='container') finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1) finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=500) - finger.addObject('UniformMass', totalMass=0.0008) - finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], drawBoxes=True, strict=False) + finger.addObject('UniformMass', totalMass=0.04) + boxROISubTopo = finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], + drawBoxes=True, strict=False) - modelSubTopo = finger.addChild('modelSubTopo') - modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra='@boxROISubTopo.tetrahedraInROI', + modelSubTopo = finger.addChild('SubTopology') + modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra=boxROISubTopo.tetrahedraInROI.linkpath, name='container') modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=1500) diff --git a/examples/tutorials/PneunetGripper/details/step4-boundaryConditions.py b/examples/tutorials/PneunetGripper/details/step4-boundaryConditions.py index c70aab60..bbe188b2 100644 --- a/examples/tutorials/PneunetGripper/details/step4-boundaryConditions.py +++ b/examples/tutorials/PneunetGripper/details/step4-boundaryConditions.py @@ -5,21 +5,31 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showForceFields showBehavior') rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') - rootNode.findData('gravity').value = [-9810, 0, 0] + rootNode.gravity.value = [-9810, 0, 0] + rootNode.addObject("DefaultAnimationLoop") - finger = rootNode.addChild('finger') + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') # Needed to use components [BoxROI] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring') # Needed to use components [RestShapeSpringsForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + + finger = rootNode.addChild('Finger') finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk') finger.addObject('MeshTopology', src='@loader', name='container') finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1) finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=500) - finger.addObject('UniformMass', totalMass=0.0008) - finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) - finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True) - finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness=1e12, angularStiffness=1e12) + finger.addObject('UniformMass', totalMass=0.04) + boxROISubTopo = finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) + boxROI = finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True) + finger.addObject('RestShapeSpringsForceField', points=boxROI.indices.linkpath, stiffness=1e12, angularStiffness=1e12) - modelSubTopo = finger.addChild('modelSubTopo') - modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra='@boxROISubTopo.tetrahedraInROI', - name='container') + modelSubTopo = finger.addChild('SubTopology') + modelSubTopo.addObject('MeshTopology', position='@loader.position', name='container', + tetrahedra=boxROISubTopo.tetrahedraInROI.linkpath) modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=1500) diff --git a/examples/tutorials/PneunetGripper/details/step5-timeIntegrationAndMatrixSolver.py b/examples/tutorials/PneunetGripper/details/step5-timeIntegrationAndMatrixSolver.py index fdd9524e..089b2a22 100644 --- a/examples/tutorials/PneunetGripper/details/step5-timeIntegrationAndMatrixSolver.py +++ b/examples/tutorials/PneunetGripper/details/step5-timeIntegrationAndMatrixSolver.py @@ -5,24 +5,37 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showForceFields') rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') - rootNode.findData('gravity').value = [-9810, 0, 0] + rootNode.gravity.value = [-9810, 0, 0] rootNode.addObject('AttachBodyButtonSetting', stiffness=10) + rootNode.addObject("DefaultAnimationLoop") - finger = rootNode.addChild('finger') - finger.addObject('EulerImplicitSolver', name='odesolver') - finger.addObject('SparseLDLSolver', name='directSolver') + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') # Needed to use components [BoxROI] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') # Needed to use components [SparseLDLSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') # Needed to use components [EulerImplicitSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring') # Needed to use components [RestShapeSpringsForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + rootNode.addObject('RequiredPlugin', name='Sofa.GUI.Component') # Needed to use components [AttachBodyButtonSetting] + + finger = rootNode.addChild('Finger') + finger.addObject('EulerImplicitSolver') + finger.addObject('SparseLDLSolver', template='CompressedRowSparseMatrixd') finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk') finger.addObject('MeshTopology', src='@loader', name='container') finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1) finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=500) - finger.addObject('UniformMass', totalMass=0.0008) - finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) - finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True) - finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness=1e12, angularStiffness=1e12) + finger.addObject('UniformMass', totalMass=0.04) + boxROISubTopo = finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) + boxROI = finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True) + finger.addObject('RestShapeSpringsForceField', points=boxROI.indices.linkpath, stiffness=1e12, angularStiffness=1e12) - modelSubTopo = finger.addChild('modelSubTopo') - modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra='@boxROISubTopo.tetrahedraInROI', + modelSubTopo = finger.addChild('SubTopology') + modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra=boxROISubTopo.tetrahedraInROI.linkpath, name='container') modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=1500) diff --git a/examples/tutorials/PneunetGripper/details/step6-pneumaticActuatorAndPythonScriptController.py b/examples/tutorials/PneunetGripper/details/step6-pneumaticActuatorAndPythonScriptController.py index 511296e0..f8ae1416 100644 --- a/examples/tutorials/PneunetGripper/details/step6-pneumaticActuatorAndPythonScriptController.py +++ b/examples/tutorials/PneunetGripper/details/step6-pneumaticActuatorAndPythonScriptController.py @@ -4,34 +4,50 @@ def createScene(rootNode): rootNode.addObject('VisualStyle', displayFlags='showForceFields showBehaviorModels') - rootNode.addObject('RequiredPlugin', - pluginName='SoftRobots SofaPython3') + rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') + + rootNode.addObject('RequiredPlugin', name='Sofa.Component.AnimationLoop') # Needed to use components [FreeMotionAnimationLoop] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Correction') # Needed to use components [LinearSolverConstraintCorrection] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Solver') # Needed to use components [GenericConstraintSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') # Needed to use components [BoxROI] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshSTLLoader,MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') # Needed to use components [SparseLDLSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.Linear') # Needed to use components [BarycentricMapping] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') # Needed to use components [EulerImplicitSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring') # Needed to use components [RestShapeSpringsForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + rootNode.addObject('RequiredPlugin', name='Sofa.GUI.Component') # Needed to use components [AttachBodyButtonSetting] + rootNode.gravity.value = [-9810, 0, 0] rootNode.addObject('AttachBodyButtonSetting', stiffness=10) rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance=1e-12, maxIterations=10000) + rootNode.addObject('GenericConstraintSolver', tolerance=1e-7, maxIterations=1000) - finger = rootNode.addChild('finger') - finger.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.1, rayleighMass=0.1) - finger.addObject('SparseLDLSolver', name='directSolver') + finger = rootNode.addChild('Finger') + finger.addObject('EulerImplicitSolver', rayleighStiffness=0.1, rayleighMass=0.1) + finger.addObject('SparseLDLSolver', template='CompressedRowSparseMatrixd') finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk') finger.addObject('MeshTopology', src='@loader', name='container') finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1) finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=500) - finger.addObject('UniformMass', totalMass=0.0008) - finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) - finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True) - finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness=1e12, angularStiffness=1e12) - finger.addObject('LinearSolverConstraintCorrection') + finger.addObject('UniformMass', totalMass=0.04) + boxROISubTopo = finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) + boxROI = finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True) + finger.addObject('RestShapeSpringsForceField', points=boxROI.indices.linkpath, stiffness=1e12, angularStiffness=1e12) + finger.addObject('GenericConstraintCorrection') - modelSubTopo = finger.addChild('modelSubTopo') - modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra='@boxROISubTopo.tetrahedraInROI', + modelSubTopo = finger.addChild('SubTopology') + modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra=boxROISubTopo.tetrahedraInROI.linkpath, name='container') modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=1500) - cavity = finger.addChild('cavity') + cavity = finger.addChild('Cavity') cavity.addObject('MeshSTLLoader', name='cavityLoader', filename='data/mesh/pneunetCavityCut.stl') cavity.addObject('MeshTopology', src='@cavityLoader', name='cavityMesh') cavity.addObject('MechanicalObject', name='cavity') diff --git a/examples/tutorials/PneunetGripper/details/step7-grabTheCube.py b/examples/tutorials/PneunetGripper/details/step7-grabTheCube.py index 49d1ae3f..953bde96 100644 --- a/examples/tutorials/PneunetGripper/details/step7-grabTheCube.py +++ b/examples/tutorials/PneunetGripper/details/step7-grabTheCube.py @@ -1,4 +1,3 @@ -import Sofa import math from wholeGripperController import WholeGripperController @@ -19,42 +18,72 @@ def createScene(rootNode): rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3') + rootNode.addObject('RequiredPlugin', name='Sofa.Component.AnimationLoop') # Needed to use components [FreeMotionAnimationLoop] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Detection.Algorithm') # Needed to use components [BVHNarrowPhase,BruteForceBroadPhase,CollisionPipeline] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Detection.Intersection') # Needed to use components [LocalMinDistance] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Geometry') # Needed to use components [LineCollisionModel,PointCollisionModel,TriangleCollisionModel] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Response.Contact') # Needed to use components [CollisionResponse] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Correction') # Needed to use components [GenericConstraintCorrection,UncoupledConstraintCorrection] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Solver') # Needed to use components [GenericConstraintSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') # Needed to use components [BoxROI] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh') # Needed to use components [MeshOBJLoader,MeshSTLLoader,MeshVTKLoader] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') # Needed to use components [SparseLDLSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Iterative') # Needed to use components [CGLinearSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.Linear') # Needed to use components [BarycentricMapping] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.NonLinear') # Needed to use components [RigidMapping] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') # Needed to use components [EulerImplicitSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Setting') # Needed to use components [BackgroundSetting] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring') # Needed to use components [RestShapeSpringsForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Dynamic') # Needed to use components [TetrahedronSetTopologyContainer] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + rootNode.addObject('RequiredPlugin', name='Sofa.GL.Component.Rendering3D') # Needed to use components [OglModel,OglSceneFrame] rootNode.addObject('VisualStyle', - displayFlags='showVisualModels hideBehaviorModels hideCollisionModels hideBoundingCollisionModels hideForceFields showInteractionForceFields hideWireframe') + displayFlags='showVisualModels hideBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hideForceFields ' + 'showInteractionForceFields hideWireframe') rootNode.gravity.value = [-9810, 0, 0] rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance=1e-12, maxIterations=10000) - rootNode.addObject('DefaultPipeline') + rootNode.addObject('GenericConstraintSolver', tolerance=1e-7, maxIterations=1000) + rootNode.addObject('CollisionPipeline') rootNode.addObject('BruteForceBroadPhase') rootNode.addObject('BVHNarrowPhase') - rootNode.addObject('DefaultContactManager', response='FrictionContactConstraint', responseParams='mu=0.6') - rootNode.addObject('LocalMinDistance', name='Proximity', alarmDistance=5, contactDistance=1, angleCone=0.0) + rootNode.addObject('CollisionResponse', response='FrictionContactConstraint', responseParams='mu=0.6') + rootNode.addObject('LocalMinDistance', name='Proximity', alarmDistance=5, contactDistance=1) rootNode.addObject('BackgroundSetting', color=[0, 0.168627, 0.211765, 1.]) rootNode.addObject('OglSceneFrame', style='Arrows', alignment='TopRight') - planeNode = rootNode.addChild('Plane') - planeNode.addObject('MeshOBJLoader', name='loader', filename='data/mesh/floorFlat.obj', triangulate=True, - rotation=[0, 0, 270], scale=10, translation=[-122, 0, 0]) - planeNode.addObject('MeshTopology', src='@loader') - planeNode.addObject('MechanicalObject', src='@loader') - planeNode.addObject('TriangleCollisionModel', simulated=False, moving=False) - planeNode.addObject('LineCollisionModel', simulated=False, moving=False) - planeNode.addObject('PointCollisionModel', simulated=False, moving=False) - planeNode.addObject('OglModel', name='Visual', src='@loader', color=[1, 0, 0, 1]) - - cube = rootNode.addChild('cube') - cube.addObject('EulerImplicitSolver', name='odesolver') - cube.addObject('SparseLDLSolver', name='linearSolver') + ########################################## + # Plane + ########################################## + plane = rootNode.addChild('Plane') + plane.addObject('MeshOBJLoader', name='loader', filename='data/mesh/floorFlat.obj', + rotation=[0, 0, 270], scale=10, translation=[-122, 0, 0]) + plane.addObject('MeshTopology', src='@loader') + plane.addObject('MechanicalObject', src='@loader') + plane.addObject('TriangleCollisionModel') + plane.addObject('LineCollisionModel') + plane.addObject('PointCollisionModel') + plane.addObject('OglModel', name='Visual', src='@loader', color=[1, 0, 0, 1]) + + ########################################## + # Cube + ########################################## + cube = rootNode.addChild('Cube') + cube.addObject('EulerImplicitSolver') + cube.addObject('CGLinearSolver', threshold=1e-5, tolerance=1e-5, iterations=50) cube.addObject('MechanicalObject', template='Rigid3', position=[-100, 70, 0, 0, 0, 0, 1]) - cube.addObject('UniformMass', totalMass=0.001) - cube.addObject('UncoupledConstraintCorrection', defaultCompliance=1e-5) + cube.addObject('UniformMass', totalMass=0.01) + cube.addObject('UncoupledConstraintCorrection') # collision - cubeCollis = cube.addChild('cubeCollis') - cubeCollis.addObject('MeshOBJLoader', name='loader', filename='data/mesh/smCube27.obj', triangulate=True, - scale=6) + cubeCollis = cube.addChild('Collision') + cubeCollis.addObject('MeshOBJLoader', name='loader', filename='data/mesh/smCube27.obj', scale=6) cubeCollis.addObject('MeshTopology', src='@loader') cubeCollis.addObject('MechanicalObject') cubeCollis.addObject('TriangleCollisionModel') @@ -63,82 +92,64 @@ def createScene(rootNode): cubeCollis.addObject('RigidMapping') # visualization - cubeVisu = cube.addChild('cubeVisu') + cubeVisu = cube.addChild('Visu') cubeVisu.addObject('MeshOBJLoader', name='loader', filename='data/mesh/smCube27.obj') cubeVisu.addObject('OglModel', name='Visual', src='@loader', color=[0.0, 0.1, 0.5], scale=6.2) cubeVisu.addObject('RigidMapping') for i in range(3): ########################################## - # Finger Model # + # Finger Model ########################################## - finger = rootNode.addChild('finger' + str(i + 1)) + finger = rootNode.addChild('Finger' + str(i + 1)) finger.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.1, rayleighMass=0.1) - finger.addObject('SparseLDLSolver', name='preconditioner') - + finger.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixd") finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk', rotation=[360 - angles[i] * 180 / math.pi, 0, 0], translation=translations[i]) finger.addObject('MeshTopology', src='@loader', name='container') - finger.addObject('MechanicalObject', name='tetras', template='Vec3', showIndices=False, showIndicesScale=4e-5) finger.addObject('UniformMass', totalMass=0.04) finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=youngModulusFingers) - - finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], doUpdate=False) - finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) - if i == 0: - finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness=1e12, - angularStiffness=1e12) - else: - finger.addObject('RestShapeSpringsForceField', points='@../finger1/boxROI.indices', stiffness=1e12, - angularStiffness=1e12) - - finger.addObject('LinearSolverConstraintCorrection') - - ########################################## - # Sub topology # - ########################################## - modelSubTopo = finger.addChild('modelSubTopo') if i == 0: - modelSubTopo.addObject('TetrahedronSetTopologyContainer', position='@loader.position', - tetrahedra='@boxROISubTopo.tetrahedraInROI', name='container') - else: - modelSubTopo.addObject('TetrahedronSetTopologyContainer', position='@loader.position', - tetrahedra='@../../finger1/boxROISubTopo.tetrahedraInROI', name='container') + boxROI = finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], doUpdate=False) + boxROISubTopo = finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False) + finger.addObject('RestShapeSpringsForceField', + points=boxROI.indices.linkpath, + stiffness=1e12, angularStiffness=1e12) + finger.addObject('GenericConstraintCorrection') + + # Sub topology + modelSubTopo = finger.addChild('SubTopology') + modelSubTopo.addObject('TetrahedronSetTopologyContainer', position='@../loader.position', + tetrahedra=boxROISubTopo.tetrahedraInROI.linkpath, name='container') modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3, youngModulus=youngModulusStiffLayerFingers - youngModulusFingers) - ########################################## - # Constraint # - ########################################## - cavity = finger.addChild('cavity') + # Constraint + cavity = finger.addChild('Cavity') cavity.addObject('MeshSTLLoader', name='loader', filename='data/mesh/pneunetCavityCut.stl', translation=translations[i], rotation=[360 - angles[i] * 180 / math.pi, 0, 0]) cavity.addObject('MeshTopology', src='@loader', name='topo') cavity.addObject('MechanicalObject', name='cavity') - cavity.addObject('SurfacePressureConstraint', name='SurfacePressureConstraint', template='Vec3', value=0.0001, + cavity.addObject('SurfacePressureConstraint', name='SurfacePressureConstraint', template='Vec3', + value=0.0001, triangles='@topo.triangles', valueType='pressure') cavity.addObject('BarycentricMapping', name='mapping', mapForces=False, mapMasses=False) - ########################################## - # Collision # - ########################################## - - collisionFinger = finger.addChild('collisionFinger') + # Collision + collisionFinger = finger.addChild('Collision') collisionFinger.addObject('MeshSTLLoader', name='loader', filename='data/mesh/pneunetCut.stl', translation=translations[i], rotation=[360 - angles[i] * 180 / math.pi, 0, 0]) collisionFinger.addObject('MeshTopology', src='@loader', name='topo') - collisionFinger.addObject('MechanicalObject', name='collisMech') - collisionFinger.addObject('TriangleCollisionModel', selfCollision=False) - collisionFinger.addObject('LineCollisionModel', selfCollision=False) - collisionFinger.addObject('PointCollisionModel', selfCollision=False) + collisionFinger.addObject('MechanicalObject') + collisionFinger.addObject('TriangleCollisionModel') + collisionFinger.addObject('LineCollisionModel') + collisionFinger.addObject('PointCollisionModel') collisionFinger.addObject('BarycentricMapping') - ########################################## - # Visualization # - ########################################## - modelVisu = finger.addChild('visu') + # Visualization + modelVisu = finger.addChild('Visu') modelVisu.addObject('MeshSTLLoader', name='loader', filename='data/mesh/pneunetCut.stl') modelVisu.addObject('OglModel', src='@loader', color=[0.7, 0.7, 0.7, 0.6], translation=translations[i], rotation=[360 - angles[i] * 180 / math.pi, 0, 0]) diff --git a/examples/tutorials/PneunetGripper/details/step7-withSTLIB.py b/examples/tutorials/PneunetGripper/details/step7-withSTLIB.py index a6971753..86f15deb 100644 --- a/examples/tutorials/PneunetGripper/details/step7-withSTLIB.py +++ b/examples/tutorials/PneunetGripper/details/step7-withSTLIB.py @@ -17,6 +17,27 @@ def createScene(rootNode): + rootNode.addObject('RequiredPlugin', name='Sofa.Component.AnimationLoop') # Needed to use components [FreeMotionAnimationLoop] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Detection.Algorithm') # Needed to use components [BVHNarrowPhase,BruteForceBroadPhase,CollisionPipeline] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Detection.Intersection') # Needed to use components [LocalMinDistance] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Geometry') # Needed to use components [LineCollisionModel,PointCollisionModel,TriangleCollisionModel] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Collision.Response.Contact') # Needed to use components [RuleBasedContactManager] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Correction') # Needed to use components [LinearSolverConstraintCorrection,UncoupledConstraintCorrection] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Solver') # Needed to use components [GenericConstraintSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Select') # Needed to use components [BoxROI] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct') # Needed to use components [SparseLDLSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Iterative') # Needed to use components [CGLinearSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.Linear') # Needed to use components [BarycentricMapping] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.NonLinear') # Needed to use components [RigidMapping] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward') # Needed to use components [EulerImplicitSolver] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic') # Needed to use components [TetrahedronFEMForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring') # Needed to use components [RestShapeSpringsForceField] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer') # Needed to use components [MechanicalObject] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Dynamic') # Needed to use components [TetrahedronSetTopologyContainer,TriangleSetTopologyContainer] + rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual') # Needed to use components [VisualStyle] + MainHeader(rootNode, plugins=['SofaPython3', 'SoftRobots'], gravity=[0.0, -9810, 0.0]) @@ -24,7 +45,7 @@ def createScene(rootNode): ContactHeader(rootNode, alarmDistance=5, contactDistance=1, - frictionCoef=0.7) + frictionCoef=0.6) rootNode.VisualStyle.displayFlags = "showBehaviors showCollisionModels" @@ -47,19 +68,19 @@ def createScene(rootNode): youngModulus=youngModulusFingers, totalMass=fingersMass) - finger.dofs.name ='tetras' + finger.dofs.name = 'tetras' rootNode.addChild(finger) finger.integration.rayleighStiffness = 0.1 finger.integration.rayleighMass = 0.1 finger.addObject('BoxROI', name='boxROI', box=fingersParameters[i]['ROIBox'], drawBoxes=True, doUpdate=False) - finger.addObject('RestShapeSpringsForceField', points='@../finger1/boxROI.indices', stiffness=1e12 - , angularStiffness=1e12) + finger.addObject('RestShapeSpringsForceField', points='@../Finger1/boxROI.indices', stiffness=1e12, + angularStiffness=1e12) PneumaticCavity(surfaceMeshFileName=fingersCavitySurfaceMesh, attachedAsAChildOf=finger, - name='cavity', + name='Cavity', rotation=fingersParameters[i]['rotation'], translation=fingersParameters[i]['translation'], initialValue=cavitiesInitialValue, diff --git a/examples/tutorials/PneunetGripper/details/wholeGripperController.py b/examples/tutorials/PneunetGripper/details/wholeGripperController.py index 6e6d6c47..02773f9a 100644 --- a/examples/tutorials/PneunetGripper/details/wholeGripperController.py +++ b/examples/tutorials/PneunetGripper/details/wholeGripperController.py @@ -7,16 +7,18 @@ def moveRestPos(rest_pos, dx, dy, dz): out = [] - for i in range(0,len(rest_pos)) : - out += [[rest_pos[i][0]+dx, rest_pos[i][1]+dy, rest_pos[i][2]+dz]] + for i in range(0, len(rest_pos)): + out += [[rest_pos[i][0] + dx, rest_pos[i][1] + dy, rest_pos[i][2] + dz]] return out -def rotateRestPos(rest_pos,rx,centerPosY,centerPosZ): +def rotateRestPos(rest_pos, rx, centerPosY, centerPosZ): out = [] - for i in range(0,len(rest_pos)) : - newRestPosY = (rest_pos[i][1] - centerPosY)*math.cos(rx) - (rest_pos[i][2] - centerPosZ)*math.sin(rx) + centerPosY - newRestPosZ = (rest_pos[i][1] - centerPosY)*math.sin(rx) + (rest_pos[i][2] - centerPosZ)*math.cos(rx) + centerPosZ + for i in range(0, len(rest_pos)): + newRestPosY = (rest_pos[i][1] - centerPosY) * math.cos(rx) - (rest_pos[i][2] - centerPosZ) * math.sin( + rx) + centerPosY + newRestPosZ = (rest_pos[i][1] - centerPosY) * math.sin(rx) + (rest_pos[i][2] - centerPosZ) * math.cos( + rx) + centerPosZ out += [[rest_pos[i][0], newRestPosY, newRestPosZ]] return out @@ -29,9 +31,9 @@ def __init__(self, *a, **kw): self.node = kw["node"] self.constraints = [] self.dofs = [] - for i in range(1,4): - self.dofs.append(self.node.getChild('finger' + str(i)).tetras) - self.constraints.append(self.node.getChild('finger'+str(i)).cavity.SurfacePressureConstraint) + for i in range(1, 4): + self.dofs.append(self.node.getChild('Finger' + str(i)).getMechanicalState()) + self.constraints.append(self.node.getChild('Finger' + str(i)).Cavity.SurfacePressureConstraint) self.centerPosY = 70 self.centerPosZ = 0 @@ -39,7 +41,7 @@ def __init__(self, *a, **kw): return - def onKeypressedEvent(self,e): + def onKeypressedEvent(self, e): increment = 0.01 @@ -68,8 +70,8 @@ def onKeypressedEvent(self,e): self.dofs[i].rest_position.value = results elif e["key"] == Sofa.constants.Key.leftarrow: - dy = 3.0*math.cos(self.rotAngle) - dz = 3.0*math.sin(self.rotAngle) + dy = 3.0 * math.cos(self.rotAngle) + dz = 3.0 * math.sin(self.rotAngle) for i in range(3): results = moveRestPos(self.dofs[i].rest_position.value, 0.0, dy, dz) self.dofs[i].rest_position.value = results @@ -77,8 +79,8 @@ def onKeypressedEvent(self,e): self.centerPosZ = self.centerPosZ + dz elif e["key"] == Sofa.constants.Key.rightarrow: - dy = -3.0*math.cos(self.rotAngle) - dz = -3.0*math.sin(self.rotAngle) + dy = -3.0 * math.cos(self.rotAngle) + dz = -3.0 * math.sin(self.rotAngle) for i in range(3): results = moveRestPos(self.dofs[i].rest_position.value, 0.0, dy, dz) self.dofs[i].rest_position.value = results @@ -88,13 +90,15 @@ def onKeypressedEvent(self,e): # Direct rotation elif e["key"] == "A": for i in range(3): - results = rotateRestPos(self.dofs[i].rest_position.value, math.pi / 16, self.centerPosY, self.centerPosZ) + results = rotateRestPos(self.dofs[i].rest_position.value, math.pi / 16, self.centerPosY, + self.centerPosZ) self.dofs[i].rest_position.value = results - self.rotAngle = self.rotAngle + math.pi/16 + self.rotAngle = self.rotAngle + math.pi / 16 # Indirect rotation elif e["key"] == "Q": for i in range(3): - results = rotateRestPos(self.dofs[i].rest_position.value, -math.pi / 16, self.centerPosY, self.centerPosZ) + results = rotateRestPos(self.dofs[i].rest_position.value, -math.pi / 16, self.centerPosY, + self.centerPosZ) self.dofs[i].rest_position.value = results - self.rotAngle = self.rotAngle - math.pi/16 + self.rotAngle = self.rotAngle - math.pi / 16