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