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