stiffFlyballGovernorKT.py
You can view and download this file on Github: stiffFlyballGovernorKT.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Stiff flyball governor built with kinematic tree (IFToMM benchmark problem);
5# Ref.: https://www.iftomm-multibody.org/benchmark/problem/Stiff_flyball_governor/
6#
7# Model: Flyball governor with kinematic tree
8#
9# Author: Johannes Gerstmayr
10# Date: 2022-8-22
11#
12# 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.
13#
14# *clean example*
15#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
17## import libaries
18import sys
19sys.exudynFast=True
20
21import exudyn as exu
22from exudyn.itemInterface import *
23from exudyn.utilities import *
24from exudyn.graphicsDataUtilities import *
25
26import numpy as np
27from numpy import linalg as LA
28
29## set up MainSystem mbs
30SC = exu.SystemContainer()
31mbs = SC.AddSystem()
32
33useGraphics=True
34
35color = [0.1,0.1,0.8,1]
36r = 0.2 #radius
37L = 1 #length
38
39
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## 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., 2*np.pi]
68
69
70
71#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
72## body masses according to reference in kg
73
74density = 3000
75
76mShaft = 0.3
77mRod = 0.3
78mSlider = 3
79mMassPoint = 5
80mRodMassPoint = mRod + mMassPoint
81
82
83#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
84## define gravity vector
85g = [0,0,-9.81]
86
87## setup rod inertia along x-direction
88iRod = InertiaCuboid(density=density, sideLengths=[lengthRod,widthRod,0.01]).Translated([lengthRod/2,0,0])
89iMass = InertiaMassPoint(mass=mMassPoint).Translated([lengthRod,0,0])
90iRodSum = iRod+iMass
91
92# #compute reference point of rod (midpoint)
93# refRod = -iRodSum.com
94# iRodSum = iRodSum.Translated(refRod)
95# exu.Print("iRodSum=", iRodSum)
96
97nRigidBodyNodes = 4
98
99## set ub shaft and slider inertias w.r.t. center of mass
100inertiaList=[InertiaCuboid(density=density, sideLengths=[widthShaft,widthShaft,lengthShaft]),
101 InertiaCuboid(density=density, sideLengths=[dimSlider,dimSlider,dimSlider]),
102 iRodSum, iRodSum]
103
104## set up graphics objects (blocks) for 4 bodies
105graphicsShaft = GraphicsDataOrthoCube(-widthShaft/2,-widthShaft/2,-lengthShaft/2, widthShaft/2,widthShaft/2,lengthShaft/2, [0.1,0.1,0.8,1])
106graphicsSlider = GraphicsDataOrthoCube(-dimSlider/2,-dimSlider/2,-dimSlider/2, dimSlider/2,dimSlider/2,dimSlider/2, [0.1,0.1,0.8,1])
107graphicsRodAC = GraphicsDataOrthoCubePoint([0.5*lengthRod, 0, 0], [lengthRod,widthRod,widthRod], color4red)
108graphicsRodBD = GraphicsDataOrthoCubePoint([0.5*lengthRod, 0, 0], [lengthRod,widthRod,widthRod], color4dodgerblue)
109
110## lists for 4 bodies: [shaft, slider, rodAC, rodBD]
111graphicsList=[[graphicsShaft], [graphicsSlider], [graphicsRodAC], [graphicsRodBD]]
112
113
114
115## create kinematic tree for 4 links [shaft, slider, rodAC, rodBD]
116### create generic node for unknowns of KinematicTree
117nGeneric = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0.]*nRigidBodyNodes,
118 initialCoordinates=[0.]*nRigidBodyNodes,
119 initialCoordinates_t=[omega0[2],0,0,0], #initial angular velocity
120 numberOfODE2Coordinates=nRigidBodyNodes))
121
122### create position vectors for links in kinematic tree
123refPosList=[[0,0,lengthShaft*0.5], # shaft
124 [0,0,sSlider-lengthShaft*0.5], # slider
125 [ xAB/2, 0, lengthShaft*0.5], # rodAC
126 [-xAB/2, 0, lengthShaft*0.5]] # rodBD
127
128### set up list of joint types, masses, COMs, inertias, and transformations for kinematic tree
129jointTypes = [exu.JointType.RevoluteZ, exu.JointType.PrismaticZ, exu.JointType.RevoluteY, exu.JointType.RevoluteY]
130linkMasses = []
131linkCOMs = exu.Vector3DList()
132linkInertiasCOM=exu.Matrix3DList()
133
134jointTransformations=exu.Matrix3DList()
135jointOffsets = exu.Vector3DList()
136
137### transform quantities for kinematic tree
138for i in range(nRigidBodyNodes):
139 inertia = inertiaList[i]
140 linkMasses += [inertia.Mass()]
141 linkCOMs.Append(inertia.COM())
142 linkInertiasCOM.Append(inertia.InertiaCOM())
143
144 A = np.eye(3)
145 if i == 2:
146 A = RotationMatrixY(beta0)
147 if i == 3:
148 A = RotationMatrixY((pi-beta0))
149
150
151 jointTransformations.Append(A)
152 jointOffsets.Append(refPosList[i])
153
154
155## create kinematic tree object 'KinematicTree' with links [shaft, slider, rodAC, rodBD]
156oKT=mbs.AddObject(ObjectKinematicTree(nodeNumber=nGeneric, jointTypes=jointTypes,
157 linkParents=[-1,0,0,0],
158 jointTransformations=jointTransformations, jointOffsets=jointOffsets,
159 linkInertiasCOM=linkInertiasCOM, linkCOMs=linkCOMs, linkMasses=linkMasses,
160 baseOffset = [0.,0.,0.], gravity=g,
161 visualization=VObjectKinematicTree(graphicsDataList = graphicsList)
162 ))
163
164
165#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166## add spring-damper parameters for connecting the rods with the slider
167
168# spring
169k = 8.e5 # spring stiffness in N/m
170l0 = 0.5 # relaxed spring length in m
171c = 4.e4 # damping coefficient Ns/m
172
173## add markers for joints
174markerRodACSlider = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=2,
175 localPosition=[lengthRod/2,0,0]))
176markerSliderPointE = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=1,
177 localPosition=[dimSlider/2,0,0]))
178
179markerRodBDSlider = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=3,
180 localPosition=[lengthRod/2,0,0]))
181markerSliderPointF = mbs.AddMarker(MarkerKinematicTreeRigid(objectNumber=oKT, linkNumber=1,
182 localPosition=[-dimSlider/2,0,0]))
183
184## add spring-dampers for compliant mechanism
185mbs.AddObject(SpringDamper(markerNumbers=[markerSliderPointE, markerRodACSlider], stiffness=k, damping=c, referenceLength=l0))
186mbs.AddObject(SpringDamper(markerNumbers=[markerSliderPointF, markerRodBDSlider], stiffness=k, damping=c, referenceLength=l0))
187
188## add sensor to measure slider position
189sPos = mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=1,
190 localPosition=[0,0,0],storeInternal=True,
191 outputVariableType=exu.OutputVariableType.Position))
192
193
194## assemble system
195mbs.Assemble()
196
197if useGraphics: #only start graphics once, but after background is set
198 ## start renderer
199 exu.StartRenderer()
200 mbs.WaitForUserToContinue()
201
202tEnd = 10
203# h = 2e-5 #RK44
204h = 5e-4*1
205
206simulationSettings = exu.SimulationSettings() #takes currently set values or default values
207simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
208simulationSettings.timeIntegration.endTime = tEnd
209simulationSettings.displayComputationTime = False
210simulationSettings.timeIntegration.verboseMode = 1
211
212## use optimized simulation settings for performance
213simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/100
214simulationSettings.solutionSettings.writeSolutionToFile = False
215
216simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = True
217
218simulationSettings.timeIntegration.newton.useModifiedNewton = True
219simulationSettings.timeIntegration.newton.maxModifiedNewtonIterations = 2
220simulationSettings.timeIntegration.newton.numericalDifferentiation.jacobianConnectorDerivative = False
221simulationSettings.timeIntegration.newton.relativeTolerance = 1e-6
222
223simulationSettings.timeIntegration.verboseMode = 1
224# simulationSettings.displayComputationTime = True
225simulationSettings.displayStatistics = True
226
227simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.7
228
229simulationSettings.timeIntegration.absoluteTolerance = 1e-6
230simulationSettings.timeIntegration.relativeTolerance = simulationSettings.timeIntegration.absoluteTolerance
231
232SC.visualizationSettings.markers.show = True
233
234# dynamicSolver.SolveSystem(mbs, simulationSettings)
235solverType = exu.DynamicSolverType.TrapezoidalIndex2 #same as generalized alpha
236# solverType = exu.DynamicSolverType.GeneralizedAlpha
237# solverType = exu.DynamicSolverType.ODE23 #0.8 seconds for h=0.05 and aTol=rTol=1e-5
238
239#tests:
240#Python 3.7, fast, TrapezoidalIndex2, numDiff systemWide, maxModNewtonIts=2: 0.6701 seconds
241#Python 3.8 Linux, fast, TrapezoidalIndex2, numDiff systemWide, maxModNewtonIts=2: 0.5259 seconds
242
243## start solver
244mbs.SolveDynamic(simulationSettings,
245 solverType=solverType,
246 )
247
248
249if useGraphics: #only start graphics once, but after background is set
250 ## wait for user to quit, then stop visualization
251 SC.WaitForRenderEngineStopFlag()
252 exu.StopRenderer() #safely close rendering window!
253
254## print relevant results
255# result = mbs.GetNodeOutput(2,exu.OutputVariableType.Velocity)[1] #y-velocity of bar
256# exu.Print('solution of stiffFlyballGovernor=',result)
257resultSlider = mbs.GetNodeOutput(nGeneric,exu.OutputVariableType.Coordinates_t)[1] #z-velocity of slider
258exu.Print('velocity of slider=',resultSlider)
259
260posSlider = mbs.GetNodeOutput(nGeneric,exu.OutputVariableType.Coordinates)[1]+0.5 #z-velocity of slider
261exu.Print('position of slider=', posSlider)
262
263if useGraphics:
264 ## plot results
265 mbs.PlotSensor(sPos, components=[2], closeAll=True)