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()