reevingSystemSpringsTest.py

You can view and download this file on Github: reevingSystemSpringsTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  A simple 3D reeving system;
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2022-06-16
  8#
  9# Copyright:This file is part of Exudyn. Exudyn is free software. You can redistribute it and/or modify it under the terms of the Exudyn license. See 'LICENSE.txt' for more details.
 10#
 11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 12
 13import exudyn as exu
 14from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics #only import if it does not conflict
 16
 17useGraphics = True #without test
 18#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 19#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 20try: #only if called from test suite
 21    from modelUnitTests import exudynTestGlobals #for globally storing test results
 22    useGraphics = exudynTestGlobals.useGraphics
 23except:
 24    class ExudynTestGlobals:
 25        pass
 26    exudynTestGlobals = ExudynTestGlobals()
 27#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 28
 29import numpy as np
 30from math import sin, cos, sqrt,pi
 31
 32SC = exu.SystemContainer()
 33mbs = SC.AddSystem()
 34
 35gGround = graphics.CheckerBoard(point=[8,-8,-2], size=32, nTiles=10)
 36oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
 37
 38t = 0.1
 39r0 = 0.5
 40g = [0,-9.81,0]
 41
 42posList = [[0,0,0],
 43           [7,-20,1],
 44           [12,0,2],
 45           ]
 46rList = [r0*1.,4*r0,r0,5.9*r0,r0,r0,r0]
 47
 48# posList = [[0,0,0],
 49#            [5,-20,1],
 50#            [8,0,2],
 51#            [12,-10,0],
 52#            [16,0,0]
 53#            ]
 54# rList = [r0*0.,6*r0,r0,5.9*r0,r0,r0,r0]
 55dirList = [-1,1,-1,1,-1,1,1]
 56
 57
 58n = len(posList)
 59nodeList = []
 60bodyList = []
 61markerList = []
 62sheavesRadii = []
 63sheavesAxes = exu.Vector3DList()
 64Lref = 0
 65pLast = [0,0,0]
 66
 67for i, pos in enumerate(posList):
 68    r = rList[i]
 69
 70    graphicsRoll = graphics.Cylinder(pAxis=[0,0,-0.5*t], vAxis=[0,0,t], nTiles=64, radius=r, color=graphics.color.dodgerblue, alternatingColor=graphics.color.darkgrey)
 71
 72    inertiaRoll = InertiaCylinder(density=1000,length=t,outerRadius=r, axis=2)
 73    #if i==1 or i==3: inertiaRoll.mass *= 2
 74    inertiaRoll = inertiaRoll.Translated([0,-r,0])
 75
 76    [nR,bR]=AddRigidBody(mainSys = mbs,
 77                         inertia = inertiaRoll,
 78                         nodeType = exu.NodeType.RotationEulerParameters,
 79                         position = pos,
 80                         gravity = g,
 81                         graphicsDataList = [graphicsRoll, graphics.Basis(inertiaRoll.COM(), length=0.5) ])
 82    mR = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nR))
 83    nodeList += [nR]
 84    bodyList += [bR]
 85    markerList += [mR]
 86
 87    sheavesAxes.Append([0,0,dirList[i]*1])
 88    sheavesRadii += [r]
 89
 90    if i != 0:
 91        Lref += NormL2(np.array(pos)-pLast)
 92    if i > 0 and i < len(posList)-1:
 93        #note that in this test example, Lref is slightly too long, leading to negative spring forces (compression) if not treated nonlinearly with default settings in ReevingSystemSprings
 94        Lref += r*pi #0.8*r*pi would always lead to tension
 95
 96    #mR = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bR, localPosition=[0,-r,0]))
 97    #mbs.AddLoad(Force(markerNumber=mR, loadVector=[0,-inertiaRoll.mass*9.81,0]))
 98
 99    pLast = pos
100
101
102markerGround0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=posList[0]))
103markerGroundMid = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=posList[2]))
104markerGroundLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=posList[-1]))
105
106#sMarkerR = mbs.AddSensor(SensorMarker(markerNumber=markerR, outputVariableType=exu.OutputVariableType.Position))
107
108#%%++++++++++++++++++++++++++++++++++++++++++++++++
109#add joints:
110oJoint0 = mbs.AddObject(GenericJoint(markerNumbers=[markerGround0, markerList[0]],
111                                    constrainedAxes=[1,1,1,1,1,1],
112                                    visualization=VGenericJoint(axesRadius=0.5*t, axesLength=1.5*t)))
113oJointLast = mbs.AddObject(GenericJoint(markerNumbers=[markerGroundLast, markerList[-1]],
114                                    constrainedAxes=[1,1,1,1,1,1],
115                                    visualization=VGenericJoint(axesRadius=0.5*t, axesLength=1.5*t)))
116
117if len(posList) > 3:
118    mbs.AddObject(GenericJoint(markerNumbers=[markerGroundMid, markerList[2]],
119                                        constrainedAxes=[1,1,1,1,1,1],
120                                        visualization=VGenericJoint(axesRadius=0.5*t, axesLength=1.5*t)))
121
122#%%++++++++++++++++++++++++++++++++++++++++++++++++
123#add reeving system spring
124stiffness = 1e5 #stiffness per length
125damping = 0.5*stiffness #dampiung per length
126oRS=mbs.AddObject(ReevingSystemSprings(markerNumbers=markerList, hasCoordinateMarkers=False,
127                                   stiffnessPerLength=stiffness, dampingPerLength=damping,
128                                   referenceLength = Lref,
129                                   dampingTorsional = 0.01*damping, dampingShear = 0.1*damping,
130                                   sheavesAxes=sheavesAxes, sheavesRadii=sheavesRadii,
131                                   #regularizationForce = -1, #purely linear system
132                                   visualization=VReevingSystemSprings(ropeRadius=0.05, color=graphics.color.dodgerblue)))
133
134#%%++++++++++++++++++++++++++++++++++++++++++++++++
135#add sensors
136if True:
137    sPos1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
138                                          outputVariableType=exu.OutputVariableType.Position))
139    sOmega1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
140                                          outputVariableType=exu.OutputVariableType.AngularVelocity))
141    sLength= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
142                                          outputVariableType=exu.OutputVariableType.Distance))
143    sLength_t= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
144                                          outputVariableType=exu.OutputVariableType.VelocityLocal))
145    sForce= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
146                                          outputVariableType=exu.OutputVariableType.ForceLocal))
147
148#%%++++++++++++++++++++++++++++++++++++++++++++++++
149#simulate:
150mbs.Assemble()
151
152simulationSettings = exu.SimulationSettings() #takes currently set values or default values
153
154tEnd = 2
155if useGraphics:
156    tEnd = 2 #200
157h=0.01  #use small step size to detext contact switching
158
159simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
160simulationSettings.timeIntegration.endTime = tEnd
161#simulationSettings.solutionSettings.writeSolutionToFile= True #set False for CPU performance measurement
162simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
163
164simulationSettings.timeIntegration.verboseMode = 1
165
166simulationSettings.timeIntegration.newton.useModifiedNewton = True
167
168SC.visualizationSettings.nodes.show = True
169SC.visualizationSettings.nodes.drawNodesAsPoint  = False
170SC.visualizationSettings.nodes.showBasis = True
171SC.visualizationSettings.nodes.basisSize = 0.2
172
173SC.visualizationSettings.openGL.multiSampling = 4
174
175#SC.visualizationSettings.general.autoFitScene = False #use loaded render state
176# useGraphics = True
177if useGraphics:
178    exu.StartRenderer()
179    if 'renderState' in exu.sys:
180        SC.SetRenderState(exu.sys[ 'renderState' ])
181    mbs.WaitForUserToContinue()
182
183
184mbs.SolveDynamic(simulationSettings,
185                 #solverType=exu.DynamicSolverType.TrapezoidalIndex2 #in this case, drift shows up significantly!
186                 )
187
188if useGraphics:
189    SC.WaitForRenderEngineStopFlag()
190    exu.StopRenderer() #safely close rendering window!
191
192if True:
193
194
195    mbs.PlotSensor(sPos1, components=[0,1,2], labels=['pos X','pos Y','pos Z'], closeAll=True)
196    mbs.PlotSensor(sOmega1, components=[0,1,2], labels=['omega X','omega Y','omega Z'])
197    mbs.PlotSensor(sLength, components=[0], labels=['length'])
198    mbs.PlotSensor(sLength_t, components=[0], labels=['vel'])
199    mbs.PlotSensor(sForce, components=[0], labels=['force'])
200
201
202
203
204#compute error for test suite:
205sol2 = mbs.systemData.GetODE2Coordinates();
206u = np.linalg.norm(sol2);
207exu.Print('solution of ReevingSystemSprings=',u)
208
209exudynTestGlobals.testResult = u