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