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