rotatingTableTest.py
You can view and download this file on Github: rotatingTableTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: RollindDisc test model with moving ground
5#
6# Author: Johannes Gerstmayr
7# Date: 2023-01-02
8#
9# 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.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13
14#%%
15import exudyn as exu
16from exudyn.itemInterface import *
17from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
18import exudyn.graphics as graphics #only import if it does not conflict
19from exudyn.graphicsDataUtilities import *
20
21import numpy as np
22from math import pi, sin, cos, exp, sqrt
23
24useGraphics = True #without test
25#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
27try: #only if called from test suite
28 from modelUnitTests import exudynTestGlobals #for globally storing test results
29 useGraphics = exudynTestGlobals.useGraphics
30except:
31 class ExudynTestGlobals:
32 pass
33 exudynTestGlobals = ExudynTestGlobals()
34#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
35useGraphics = False #without test
36
37SC = exu.SystemContainer()
38mbs = SC.AddSystem()
39uTest = 0 #reference solution
40
41for mode in range(2):
42 mbs.Reset()
43 #Dimensions
44 mass = 30 #wheel mass in kg
45 r = 0.5 #wheel diameter in m
46 d = 3 #frame length in m
47 g = 9.81 #gravity constant (in )
48
49 #drawing parameters:
50 w=0.1 #width for drawing
51
52
53 #%%++++++++++++++++++++++++++++++++++
54 gravity = [0,-g,0] #gravity
55 vDisc = np.array([0,0,1])
56 p0Wheel = [0,0,d] #initial position of wheel
57
58 p0Plane = [0,-r,0]
59 n0Plane = np.array([0,1,0])
60
61 iWheel = InertiaCylinder(1000, w, r, axis=2)
62
63 graphicsBodyWheel = [graphics.Brick(size=[1.2*r,1.2*r, w*3], color=graphics.color.lightred, addEdges=True)]
64 graphicsBodyWheel+= [graphics.Cylinder(pAxis=[0,0,-0.5*w], vAxis=[0,0,w], radius=r, color=graphics.color.dodgerblue, nTiles=32, addEdges=True)]
65 graphicsBodyWheel+= [graphics.Cylinder(pAxis=[0,0,-d], vAxis=[0,0,d], radius=0.5*w,
66 color=graphics.color.orange, addEdges=True)]
67 bWheel = mbs.CreateRigidBody(inertia = iWheel,
68 referencePosition = p0Wheel,
69 gravity = gravity,
70 graphicsDataList = graphicsBodyWheel)
71
72 #ground body and marker
73 p0Ground= p0Plane
74 iPlane = RigidBodyInertia(mass=100, inertiaTensor=np.eye(3)*1000)
75 graphicsPlane = [graphics.CheckerBoard(point=[0,0,0], normal=n0Plane, size=6*d)]
76 oGround = mbs.AddObject(ObjectGround(referencePosition=p0Plane))
77
78 #ground is also a movable rigid body
79 bPlane = mbs.CreateRigidBody(inertia = iPlane,
80 referencePosition = p0Plane,
81 gravity = gravity,
82 graphicsDataList = graphicsPlane)
83
84 #++++++++++++++++++++
85 #joint for plane
86 markerSupportGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
87 markerSupportPlane = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bPlane, localPosition=[0,0,0]))
88 #marker at wheel fixed to frame:
89
90 mbs.AddObject(GenericJoint(markerNumbers=[markerSupportGround,markerSupportPlane],
91 constrainedAxes=[1,1,1,1,0,1],
92 visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.12)))
93
94 oTSD = mbs.AddObject(TorsionalSpringDamper(markerNumbers=[markerSupportGround,markerSupportPlane],
95 stiffness=0, damping=0,
96 rotationMarker0=RotationMatrixX(0.5*pi), #rotation marker is around z-axis=>change to y-axis
97 rotationMarker1=RotationMatrixX(0.5*pi),
98 ))
99 #++++++++++++++++++++
100 #joint between wheel/frame and ground:
101 #marker for fixing frame
102 markerSupportGroundWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,r,0]))
103 #marker at wheel fixed to frame:
104 markerWheelFrame = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bWheel, localPosition=[0,0,-d]))
105
106 kSD = 1e6
107 dSD = 1e4
108 mbs.AddObject(RigidBodySpringDamper(markerNumbers=[markerWheelFrame,markerSupportGroundWheel],
109 stiffness=kSD*np.diag([1,1,1,0,0,0]),
110 damping=dSD*np.diag([1,1,1,0,0,0]) ))
111
112 #generic joint shows joint frames, are helpful to understand ...
113 # mbs.AddObject(GenericJoint(markerNumbers=[markerWheelFrame,markerSupportGroundWheel],
114 # constrainedAxes=[1,1,1,0,0,0],
115 # visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.12)))
116
117 mbs.AddLoad(Torque(markerNumber=markerWheelFrame,
118 loadVector=[0,0,20], bodyFixed=True))
119
120 #++++++++++++++++++++
121 #rolling disc joint:
122 markerWheelCenter = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bWheel, localPosition=[0,0,0]))
123 markerRollingPlane = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bPlane, localPosition=[0,0,0]))
124
125 if True:
126 if mode==0:
127 oRolling=mbs.AddObject(ObjectJointRollingDisc(markerNumbers=[markerRollingPlane,markerWheelCenter],
128 constrainedAxes=[1,1,1],
129 discRadius=r,
130 discAxis=vDisc,
131 planeNormal = n0Plane,
132 visualization=VObjectJointRollingDisc(show=False,discWidth=w,color=graphics.color.blue)))
133 else:
134 nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
135 oRolling=mbs.AddObject(RollingDiscPenalty(markerNumbers=[markerRollingPlane,markerWheelCenter],
136 nodeNumber=nGeneric,
137 discRadius=r,
138 discAxis=vDisc,
139 planeNormal = n0Plane, contactStiffness=kSD, contactDamping=dSD,
140 dryFriction=[0.5,0.5], dryFrictionProportionalZone=0.01,
141 useLinearProportionalZone=True,
142 visualization=VObjectJointRollingDisc(show=False,discWidth=w,color=graphics.color.blue)))
143
144 sForce = mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,
145 outputVariableType = exu.OutputVariableType.ForceLocal))
146
147 sTrailVel = mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,
148 outputVariableType = exu.OutputVariableType.Velocity))
149
150
151 # nGeneric0 = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
152 # oRolling=mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerRollingPlane,markerWheelCenter],
153 # nodeNumber=nGeneric0,
154 # contactStiffness=1e5, contactDamping=1e3,
155 # discRadius=r,
156 # discAxis=AA@vDisc,
157 # planeNormal = AA@n0Plane,
158 # visualization=VObjectJointRollingDisc(discWidth=w,color=graphics.color.blue)))
159
160 sAngVel = mbs.AddSensor(SensorBody(bodyNumber=bWheel, storeInternal=True,
161 outputVariableType = exu.OutputVariableType.AngularVelocity))
162 sAngVelLocal = mbs.AddSensor(SensorBody(bodyNumber=bWheel, storeInternal=True,
163 outputVariableType = exu.OutputVariableType.AngularVelocityLocal))
164 sAngAcc = mbs.AddSensor(SensorBody(bodyNumber=bWheel, storeInternal=True,
165 outputVariableType = exu.OutputVariableType.AngularAcceleration))
166
167
168 def PreStepUserFunction(mbs, t):
169 if abs(t-2.5) < 1e-4:
170 mbs.SetObjectParameter(oTSD, 'damping', 5000)
171 return True
172
173 mbs.SetPreStepUserFunction(PreStepUserFunction)
174
175 mbs.Assemble()
176
177
178 simulationSettings = exu.SimulationSettings() #takes currently set values or default values
179
180 tEnd = 5
181 h = 0.005
182 simulationSettings.timeIntegration.endTime = tEnd #0.2 for testing
183 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
184 #simulationSettings.solutionSettings.solutionWritePeriod = 0.01
185 #simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
186 simulationSettings.timeIntegration.verboseMode = 1
187 simulationSettings.displayStatistics = True
188
189 # simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
190 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
191 simulationSettings.timeIntegration.newton.useModifiedNewton = True
192
193 simulationSettings.timeIntegration.simulateInRealtime = useGraphics
194
195 SC.visualizationSettings.connectors.showJointAxes = True
196 SC.visualizationSettings.connectors.jointAxesLength = 0.3
197 SC.visualizationSettings.connectors.jointAxesRadius = 0.08
198 SC.visualizationSettings.openGL.lineWidth=2 #maximum
199 SC.visualizationSettings.openGL.lineSmooth=True
200 SC.visualizationSettings.openGL.shadow=0.15
201 SC.visualizationSettings.openGL.multiSampling = 4
202 SC.visualizationSettings.openGL.light0position = [8,8,10,0]
203 simulationSettings.solutionSettings.solutionInformation = "Example Kollermill"
204 SC.visualizationSettings.general.graphicsUpdateInterval = 0.02
205
206 if useGraphics:
207 exu.StartRenderer()
208 if 'renderState' in exu.sys:
209 SC.SetRenderState(exu.sys['renderState'])
210 mbs.WaitForUserToContinue()
211
212 mbs.SolveDynamic(simulationSettings,
213 solverType=exu.DynamicSolverType.TrapezoidalIndex2,
214 #showHints=True
215 )
216
217 if useGraphics:
218 SC.WaitForRenderEngineStopFlag()
219 exu.StopRenderer() #safely close rendering window!
220
221 sol2 = mbs.systemData.GetODE2Coordinates();
222 u = np.linalg.norm(sol2);
223 exu.Print("rotatingTableTest mode",mode, "=", u)
224 uTest += u
225
226exu.Print("rotatingTableTest=", uTest)
227
228exudynTestGlobals.testError = (uTest - 7.838680371309492)
229exudynTestGlobals.testResult = uTest
230
231#%%+++++++++++++++++++++++
232if useGraphics:
233
234 mbs.PlotSensor(closeAll=True)
235 mbs.PlotSensor(sensorNumbers=[sForce], components=[0,1,2])
236 mbs.PlotSensor(sensorNumbers=sAngVel, components=[0,1,2])
237 mbs.PlotSensor(sensorNumbers=sAngVelLocal, components=[0,1,2])
238 mbs.PlotSensor(sensorNumbers=sAngAcc, components=[0,1,2])
239
240 mbs.PlotSensor(sensorNumbers=sTrailVel, components=[0,1,2])
241 mbs.PlotSensor(sensorNumbers=sTrailVel, components=exu.plot.componentNorm,
242 newFigure=False, colorCodeOffset=3, labels=['trail velocity norm'])
243
244 forceEnd=mbs.GetSensorValues(sensorNumber=sForce)
245 print('sForce: ',forceEnd)
246
247 angAcc0=mbs.GetSensorStoredData(sensorNumber=sAngAcc)[0,1:]
248 print('angAcc: ',angAcc0)