stiffFlyballGovernor.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Stiff flyball governor (iftomm benchmark problem)
  5#           Ref.: https://www.iftomm-multibody.org/benchmark/problem/Stiff_flyball_governor/
  6#
  7# Author:   Johannes Gerstmayr, Stefan Holzinger
  8# Date:     2020-02-13
  9#
 10# 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.
 11#
 12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13
 14import exudyn as exu
 15from exudyn.utilities import *
 16from exudyn.lieGroupBasics import *
 17from exudyn.lieGroupIntegration import *
 18
 19import numpy as np
 20from numpy import linalg as LA
 21
 22useGraphics = True #without test
 23#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 24#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 25try: #only if called from test suite
 26    from modelUnitTests import exudynTestGlobals #for globally storing test results
 27    useGraphics = exudynTestGlobals.useGraphics
 28except:
 29    class ExudynTestGlobals:
 30        pass
 31    exudynTestGlobals = ExudynTestGlobals()
 32#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 33
 34SC = exu.SystemContainer()
 35mbs = SC.AddSystem()
 36
 37color = [0.1,0.1,0.8,1]
 38r = 0.2 #radius
 39L = 1   #length
 40
 41
 42background0 = GraphicsDataRectangle(-L,-L,L,L,color)
 43oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background0])))
 44
 45
 46
 47###############################################################################
 48## body dimensions according to reference in m
 49
 50# shaft
 51lengthShaft = 1     #z
 52widthShaft  = 0.01  #=height
 53
 54# rod
 55lengthRod = 1
 56widthRod  = 0.01    #=height
 57
 58# slider
 59dimSlider = 0.1 #x=y=z
 60sSlider = 0.5
 61
 62# scalar distance between point A and B
 63xAB = 0.1
 64beta0 = np.deg2rad(30)
 65initAngleRod = np.deg2rad(60)
 66
 67# initial angular velocity of shaft and slider
 68omega0 = [0., 0., 0.16*2*np.pi]
 69
 70
 71
 72###############################################################################
 73## body masses according to reference in kg
 74
 75density = 3000
 76
 77mShaft        = 0.3
 78mRod          = 0.3
 79mSlider       = 3
 80mMassPoint    = 5
 81mRodMassPoint = mRod + mMassPoint
 82
 83
 84###############################################################################
 85# gravity
 86g = [0,0,-9.81]
 87
 88#setup rod along x-direction
 89iRod = InertiaCuboid(density=density, sideLengths=[lengthRod,widthRod,0.01])
 90iMass = InertiaMassPoint(mass=mMassPoint).Translated([lengthRod/2,0,0])
 91iRodSum = iRod+iMass
 92
 93#compute reference point of rod (midpoint)
 94refRod = -iRodSum.com
 95iRodSum = iRodSum.Translated(refRod)
 96
 97#exu.Print("refRod=", refRod)
 98#exu.Print("iRodSum=", iRodSum)
 99
100#nodeType = exu.NodeType.RotationEulerParameters
101#nodeType = exu.NodeType.RotationRxyz
102nodeType = exu.NodeType.RotationRotationVector
103
104
105nRigidBodyNodes = 4
106#nRB=[-1]*nRigidBodyNodes #final node numbers
107
108inertiaList=[InertiaCuboid(density=density, sideLengths=[widthShaft,widthShaft,lengthShaft]),
109             InertiaCuboid(density=density, sideLengths=[dimSlider,dimSlider,dimSlider]),
110             iRodSum, iRodSum]
111
112refPosList=[[0,0,lengthShaft/2], # shaft
113            [0,0,sSlider], # slider
114            [ xAB/2 + (lengthRod/2-refRod[0])*np.cos(beta0), 0, lengthShaft - (lengthRod/2-refRod[0])*np.sin(beta0)], # rodAC
115            [-xAB/2 - (lengthRod/2-refRod[0])*np.cos(beta0), 0, lengthShaft - (lengthRod/2-refRod[0])*np.sin(beta0)]] # rodBD
116
117refVelList = [[0., 0., 0.], # shaft
118              [0., 0., 0.], # slider
119#              np.dot(Skew(omega0), np.array([lengthRod/2-refRod[0], 0, 0])), # rodAC
120#              np.dot(Skew(omega0), np.array([-(lengthRod/2-refRod[0]), 0, 0]))] # rodBD
121              [0,omega0[2]*refPosList[2][0],0], # rodAC
122              [0,omega0[2]*refPosList[3][0],0]] # rodBD
123
124#global initial angular velocities
125refAngularVelList = [omega0,     # shaft
126                     omega0,     # slider
127                     omega0,    # rodAC
128                     omega0]    # rodBD
129
130#GraphicsDataOrthoCube(xMin, yMin, zMin, xMax, yMax, zMax, color=[0.,0.,0.,1.]):
131#graphicsRod    = GraphicsDataOrthoCube(-lengthRod/2,-widthRod/2,-widthRod/2, lengthRod/2,widthRod/2,widthRod/2, [0.1,0.1,0.8,1])
132graphicsRodAC  = GraphicsDataOrthoCube(-(lengthRod/2-refRod[0]),-widthRod/2,-widthRod/2, lengthRod/2+refRod[0],widthRod/2,widthRod/2, [0.1,0.1,0.8,1])
133graphicsRodBD  = GraphicsDataOrthoCube(-lengthRod/2-refRod[0],-widthRod/2,-widthRod/2, lengthRod/2-refRod[0],widthRod/2,widthRod/2, [0.1,0.1,0.8,1])
134graphicsSlider = GraphicsDataOrthoCube(-dimSlider/2,-dimSlider/2,-dimSlider/2, dimSlider/2,dimSlider/2,dimSlider/2, [0.1,0.1,0.8,1])
135graphicsShaft  = GraphicsDataOrthoCube(-widthShaft/2,-widthShaft/2,-lengthShaft/2, widthShaft/2,widthShaft/2,lengthShaft/2, [0.1,0.1,0.8,1])
136
137#lists for 4 nodes/bodies: [shaft, slider, rodAC, rodBD]
138graphicsList=[graphicsShaft, graphicsSlider, graphicsRodAC, graphicsRodBD]
139
140#eulerParameters0 = [1, 0, 0, 0]
141rotParList = []
142if nodeType == exu.NodeType.RotationEulerParameters:
143    refRotParList = [eulerParameters0,             # shaft
144                     eulerParameters0,             # slider
145                     RotationMatrix2EulerParameters(RotationMatrixY(beta0)),   # rodAC
146                     RotationMatrix2EulerParameters(RotationMatrixY(-beta0))]  # rodBD
147    refRotMatList = [EulerParameters2RotationMatrix(refRotParList[0]),
148                     EulerParameters2RotationMatrix(refRotParList[1]),
149                     EulerParameters2RotationMatrix(refRotParList[2]),
150                     EulerParameters2RotationMatrix(refRotParList[3])]
151
152elif nodeType == exu.NodeType.RotationRxyz:
153    refRotParList = [[0,0,0],       # shaft
154                     [0,0,0],       # slider
155                     [0,beta0,0],   # rodAC
156                     [0,-beta0,0]]  # rodBD
157    refRotMatList = [RotXYZ2RotationMatrix(refRotParList[0]),
158                     RotXYZ2RotationMatrix(refRotParList[1]),
159                     RotXYZ2RotationMatrix(refRotParList[2]),
160                     RotXYZ2RotationMatrix(refRotParList[3])]
161
162elif nodeType == exu.NodeType.RotationRotationVector:
163    refRotParList = [[0,0,0],       # shaft
164                     [0,0,0],       # slider
165                     [0,beta0,0],   # rodAC
166                     [0,-beta0,0]]  # rodBD
167    refRotMatList = [RotationVector2RotationMatrix(refRotParList[0]),
168                     RotationVector2RotationMatrix(refRotParList[1]),
169                     RotationVector2RotationMatrix(refRotParList[2]),
170                     RotationVector2RotationMatrix(refRotParList[3])]
171
172# add rigid bodies to mbs
173nodeNumberList = [-1]*nRigidBodyNodes
174bodyNumberList = [-1]*nRigidBodyNodes
175for i in range(nRigidBodyNodes):
176    [n0,b0]=AddRigidBody(mainSys = mbs,
177                         inertia = inertiaList[i],
178                         nodeType = str(nodeType),
179                         position = refPosList[i],
180                         velocity = refVelList[i],
181                         rotationMatrix = [],#refRotMatList[i],
182                         rotationParameters = refRotParList[i],
183                         angularVelocity = refAngularVelList[i],
184                         gravity = g,
185                         graphicsDataList = [graphicsList[i]])
186    nodeNumberList[i] = n0
187    bodyNumberList[i] = b0
188
189
190
191
192###############################################################################
193## spring-damper parameters for connecting the rods with the slider
194
195# spring
196k  = 8.e5*0.005 # spring stiffness in N/m
197l0 = 0.5  # relaxed spring length in m
198
199# damper
200c = 4.e4*0.005
201
202## connecting points
203# slider
204pointEslider = [dimSlider/2, 0., 0.]
205pointFslider = [-dimSlider/2, 0., 0.]
206
207# connectin points for connecting rods with slider
208connectingPointRodACWithSlider = [refRod[0], 0, 0]
209connectingPointRodBDWithSlider = [-refRod[0], 0, 0]
210
211# connecting points for connecting rods with shaft
212pointA = [xAB/2, 0, lengthShaft/2]
213pointB = [-xAB/2, 0, lengthShaft/2]
214pointARodAC = [-(lengthRod/2-refRod[0]), 0, 0]
215pointARodBD = [(lengthRod/2-refRod[0]), 0, 0]
216
217# connecting point of shaft with ground
218connectingPointShaftWithGround = [0, 0, -lengthShaft/2]
219
220# markers
221markerShaftCOM     = mbs.AddMarker(MarkerBodyRigid(name='markerShaftCOM', bodyNumber=bodyNumberList[0], localPosition=[0,0,0]))
222markerShaftGround  = mbs.AddMarker(MarkerBodyRigid(name='markerShaftGround', bodyNumber=bodyNumberList[0], localPosition=connectingPointShaftWithGround))
223markerShaftPointA  = mbs.AddMarker(MarkerBodyRigid(name='markerShaftPointA', bodyNumber=bodyNumberList[0], localPosition=pointA))
224markerShaftPointB  = mbs.AddMarker(MarkerBodyRigid(name='markerShaftPointB', bodyNumber=bodyNumberList[0], localPosition=pointB))
225
226markerSliderCOM    = mbs.AddMarker(MarkerBodyRigid(name='markerSliderCOM', bodyNumber=bodyNumberList[1], localPosition=[0,0,0]))
227markerSliderPointE = mbs.AddMarker(MarkerBodyRigid(name='markerSliderPointE', bodyNumber=bodyNumberList[1], localPosition=pointEslider))
228markerSliderPointF = mbs.AddMarker(MarkerBodyRigid(name='markerSliderPointF', bodyNumber=bodyNumberList[1], localPosition=pointFslider))
229
230markerRodACShaft   = mbs.AddMarker(MarkerBodyRigid(name='markerRodACShaft', bodyNumber=bodyNumberList[2], localPosition=pointARodAC))
231markerRodACSlider  = mbs.AddMarker(MarkerBodyRigid(name='markerRodACSlider', bodyNumber=bodyNumberList[2], localPosition=connectingPointRodACWithSlider))
232
233markerRodBDShaft   = mbs.AddMarker(MarkerBodyRigid(name='markerRodBDShaft', bodyNumber=bodyNumberList[3], localPosition=pointARodBD))
234markerRodBDSlider  = mbs.AddMarker(MarkerBodyRigid(name='markerRodBDSlider', bodyNumber=bodyNumberList[3], localPosition=connectingPointRodBDWithSlider))
235
236
237
238oGround = mbs.AddObject(ObjectGround())
239markerGround = mbs.AddMarker(MarkerBodyRigid(name='markerGround', bodyNumber=oGround, localPosition=[0,0,0]))
240
241nj2=-1
242
243if False:
244
245    mbs.AddObject(GenericJoint(markerNumbers=[markerGround, markerShaftGround], constrainedAxes=[1,1,1,1,1,0],
246                                visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
247
248    mbs.AddObject(GenericJoint(markerNumbers=[markerShaftCOM, markerSliderCOM], constrainedAxes=[1*0,1*0,0,1,1,1],
249                                visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
250
251    mbs.AddObject(GenericJoint(markerNumbers=[markerShaftPointA, markerRodACShaft], constrainedAxes=[1,1,1,1,0,1],
252                                visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
253
254    mbs.AddObject(GenericJoint(markerNumbers=[markerShaftPointB, markerRodBDShaft], constrainedAxes=[1,1,1,1,0,1],
255                                visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
256
257else:
258    kj=1e5*0.2
259    dj = kj*0.05
260
261    kj2 = kj*0.05 #rotatory springs can be softer!
262    dj2 = kj2*0.05
263
264    mbs.AddObject(RigidBodySpringDamper(markerNumbers=[markerGround, markerShaftGround],
265                                        stiffness=np.diag([kj,kj,kj,kj2,kj2,0]), damping=np.diag([dj,dj,dj,dj2,dj2,0])))
266
267    mbs.AddObject(RigidBodySpringDamper(markerNumbers=[markerShaftCOM, markerSliderCOM],
268                                        stiffness=np.diag([kj,kj,0,kj2,kj2,kj2]), damping=0*np.diag([dj,dj,0,0,0,0])))
269
270    nj2 = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[markerShaftPointA, markerRodACShaft],
271                                        stiffness=np.diag([kj,kj,kj,kj2,0,kj2]), damping=0.*np.diag([dj,dj,dj,0,0,0])))
272
273    mbs.AddObject(RigidBodySpringDamper(markerNumbers=[markerShaftPointB, markerRodBDShaft],
274                                        stiffness=np.diag([kj,kj,kj,kj2,0,kj2]), damping=0.*np.diag([dj,dj,dj,0,0,0])))
275
276
277
278
279# spring-damper elements
280mbs.AddObject(SpringDamper(markerNumbers=[markerSliderPointE, markerRodACSlider], stiffness=k, damping=c, referenceLength=l0))
281mbs.AddObject(SpringDamper(markerNumbers=[markerSliderPointF, markerRodBDSlider], stiffness=k, damping=c, referenceLength=l0))
282
283sPos=mbs.AddSensor(SensorNode(nodeNumber = nodeNumberList[1],
284                         storeInternal=True,#fileName='solution/flyballSliderPosition.txt',
285                         outputVariableType=exu.OutputVariableType.Position))
286sRot=mbs.AddSensor(SensorNode(nodeNumber = nodeNumberList[2],
287                         storeInternal=True,#fileName='solution/flyballSliderRotation.txt',
288                         outputVariableType=exu.OutputVariableType.Rotation)) #Tait Bryan rotations
289sAngVel=mbs.AddSensor(SensorNode(nodeNumber = nodeNumberList[0],
290                         storeInternal=True,#fileName='solution/flyballShaftAngularVelocity.txt',
291                         outputVariableType=exu.OutputVariableType.AngularVelocity))
292
293
294#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
295mbs.Assemble()
296
297simulationSettings = exu.SimulationSettings() #takes currently set values or default values
298
299
300if useGraphics: #only start graphics once, but after background is set
301    exu.StartRenderer()
302    #mbs.WaitForUserToContinue()
303
304dynamicSolver = exu.MainSolverImplicitSecondOrder()
305
306fact = 20 #200000
307if useGraphics: #only start graphics once, but after background is set
308    fact = 20
309
310simulationSettings.timeIntegration.numberOfSteps = fact #1000 steps for test suite/error
311simulationSettings.timeIntegration.endTime = 5e-5*fact              #1s for test suite / error
312
313
314SC.visualizationSettings.markers.show = True
315#SC.visualizationSettings.markers.showNumbers = True
316
317simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
318simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
319simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
320#simulationSettings.displayComputationTime = True
321simulationSettings.timeIntegration.verboseMode = 1
322
323simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/2000
324simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/2000
325
326if nodeType != exu.NodeType.RotationRotationVector:
327    simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = True
328else:
329    simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
330
331
332if True:
333#if nodeType == exu.NodeType.RotationRotationVector:
334    LieGroupExplicitRKInitialize(mbs)
335    dynamicSolver.SetUserFunctionNewton(mbs, UserFunctionNewtonLieGroupRK4)
336
337dynamicSolver.SolveSystem(mbs, simulationSettings)
338#mbs.SolveDynamic(simulationSettings)
339
340if useGraphics: #only start graphics once, but after background is set
341    #SC.WaitForRenderEngineStopFlag()
342    exu.StopRenderer() #safely close rendering window!
343
344
345for i in range(4):
346    om=mbs.GetNodeOutput(i,exu.OutputVariableType.AngularVelocity)
347    # exu.Print("om",i,"=",om)
348
349for i in range(4):
350    vel=mbs.GetNodeOutput(i,exu.OutputVariableType.Velocity)
351    # exu.Print("v",i,"=",vel)
352
353for i in range(2):
354    rot=mbs.GetNodeOutput(i+2,exu.OutputVariableType.RotationMatrix)
355    # exu.Print("Rot",i+2,"=",rot)
356
357result = mbs.GetNodeOutput(2,exu.OutputVariableType.Velocity)[1] #y-velocity of bar
358exu.Print('solution of stiffFlyballGovernor=',result)
359
360exudynTestGlobals.testError = result - (0.8962488779114738) #2021-01-04: 0.015213599619996604 (Python3.7)
361exudynTestGlobals.testResult = result
362
363
364plist=[]
365plist += [mbs.GetObjectOutputBody(objectNumber = bodyNumberList[2], variableType = exu.OutputVariableType.Velocity, localPosition = list(pointARodAC), configuration =
366exu.ConfigurationType.Current)]
367plist += [mbs.GetObjectOutputBody(objectNumber = bodyNumberList[2], variableType = exu.OutputVariableType.Velocity, localPosition = connectingPointRodACWithSlider, configuration =
368exu.ConfigurationType.Current)]
369plist += [mbs.GetObjectOutputBody(objectNumber = bodyNumberList[3], variableType = exu.OutputVariableType.Velocity, localPosition = pointARodBD, configuration =
370exu.ConfigurationType.Current)]
371# for i in range(3):
372#     exu.Print("vX",i,"=",plist[i])
373
374#locU = mbs.GetObjectOutput(objectNumber = nj2, variableType =exu.OutputVariableType.DisplacementLocal)
375#exu.Print('locU=', locU)
376#locR = mbs.GetObjectOutput(objectNumber = nj2, variableType =exu.OutputVariableType.Rotation)
377#exu.Print('locR=', locR)
378
379
380#Rxyz initial velocities:
381#om 0 = [0.         0.         6.28318531]
382#om 1 = [0.         0.         6.28318531]
383#om 2 = [ 0.00000000e+00 -8.54693196e-10  6.28318531e+00]
384#om 3 = [0.00000000e+00 8.54693196e-10 6.28318531e+00]
385#v 0 = [ 0.00000000e+00  0.00000000e+00 -4.90499796e-10]
386#v 1 = [ 0.00000000e+00  0.00000000e+00 -4.90499608e-10]
387#v 2 = [-1.91975841e-16  5.60155553e+00 -4.90500111e-10]
388#v 3 = [ 1.91975841e-16 -5.60155553e+00 -4.90500111e-10]
389
390if useGraphics:
391
392
393    mbs.PlotSensor(sPos, components=[0,1,2], closeAll=True)
394    mbs.PlotSensor(sRot, components=[0,1,2])
395
396    if False:
397        import matplotlib.pyplot as plt
398        import matplotlib.ticker as ticker
399        #data = np.loadtxt('solution/flyballSliderPositionRxyz.txt', comments='#', delimiter=',')    #rigid joints?
400        data = np.loadtxt('solution/flyballSliderPositionRK4Rxyz.txt', comments='#', delimiter=',') #compliant joints
401        #plt.plot(data[:,0], data[:,3], 'r:') #z coordinate of slider
402        plt.plot(data[:,0], data[:,1], 'b:') #z coordinate of slider
403        plt.plot(data[:,0], data[:,2], 'g:') #z coordinate of slider
404        plt.plot(data[:,0], data[:,3], 'k:') #z coordinate of slider
405
406
407        ax=plt.gca() # get current axes
408        ax.grid(True, 'major', 'both')
409        ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
410        ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
411        plt.tight_layout()
412        plt.show()