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