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