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