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)