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