heavyTop.py
You can view and download this file on Github: heavyTop.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Heavy top example
5# Refs.: Terze, Z., Müller, A., Zlatar, D.: Singularity-free time integration of rotational quaternions using non-redundant ordinary differential equations. Multibody System Dynamics 38(3),201–225 (2016)
6#
7# Author: Johannes Gerstmayr
8# Date: 2020-02-02
9#
10# 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.
11#
12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
14import exudyn as exu
15from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
16import exudyn.graphics as graphics #only import if it does not conflict
17
18import numpy as np
19
20useGraphics = True #without test
21#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
23try: #only if called from test suite
24 from modelUnitTests import exudynTestGlobals #for globally storing test results
25 useGraphics = exudynTestGlobals.useGraphics
26except:
27 class ExudynTestGlobals:
28 pass
29 exudynTestGlobals = ExudynTestGlobals()
30#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
32SC = exu.SystemContainer()
33mbs = SC.AddSystem()
34
35#exu.Print('EXUDYN version='+exu.GetVersionString())
36
37#background
38#rect = [-0.1,-0.1,0.1,0.1] #xmin,ymin,xmax,ymax
39#background0 = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':[rect[0],rect[1],0, rect[2],rect[1],0, rect[2],rect[3],0, rect[0],rect[3],0, rect[0],rect[1],0]} #background
40color = [0.1,0.1,0.8,1]
41r = 0.5 #radius
42L = 1 #length
43
44
45background0 = GraphicsDataRectangle(-L,-L,L,L,color)
46oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background0])))
47
48#heavy top is fixed at [0,0,0] (COM of simulated body), but force is applied at [0,1,0] (COM of real top)
49m = 15
50Jxx=0.234375
51Jyy=0.46875
52Jzz=0.234375
53#yS = 1 #distance from
54
55#vector to COM, where force is applied
56rp = [0.,1.,0.]
57#rpt = np.array(Skew(rp))
58rpt = Skew(rp)
59Fg = [0,0,-m*9.81]
60#inertia tensor w.r.t. fixed point
61JFP = np.diag([Jxx,Jyy,Jzz]) - m*np.dot(rpt,rpt)
62#exu.Print(JFP)
63
64omega0 = [0,150,-4.61538] #arbitrary initial angular velocity
65p0 = [0,0,0] #reference position
66v0 = [0.,0.,0.] #initial translational velocity
67
68nodeTypeList = [exu.NodeType.RotationEulerParameters, exu.NodeType.RotationRxyz]
69
70sAngVel=[]
71sPos=[]
72sCoords=[]
73for nodeType in nodeTypeList:
74
75 nRB = 0
76 if nodeType == exu.NodeType.RotationEulerParameters:
77 ep0 = eulerParameters0 #no rotation
78 ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
79 #exu.Print(ep_t0)
80
81 nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, initialVelocities=v0+list(ep_t0)))
82 else: #Rxyz
83 rot0 = [0,0,0] #no rotation
84 #omega0 = [10,0,0]
85 rot_t0 = AngularVelocity2RotXYZ_t(omega0, rot0)
86 #exu.Print('rot_t0=',rot_t0)
87
88 nRB = mbs.AddNode(NodeRigidBodyRxyz(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
89
90 oGraphics = graphics.BrickXYZ(-r/2,-L/2,-r/2, r/2,L/2,r/2, [0.1,0.1,0.8,1])
91 oRB = mbs.AddObject(ObjectRigidBody(physicsMass=m, physicsInertia=[JFP[0][0], JFP[1][1], JFP[2][2], JFP[1][2], JFP[0][2], JFP[0][1]],
92 nodeNumber=nRB, visualization=VObjectRigidBody(graphicsData=[oGraphics])))
93
94 mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[0,1,0])) #this is the real COM
95 mbs.AddLoad(Force(markerNumber = mMassRB, loadVector=Fg))
96
97 nPG=mbs.AddNode(PointGround(referenceCoordinates=[0,0,0])) #for coordinate constraint
98 mCground = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0)) #coordinate number does not matter
99
100 mC0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=0)) #ux
101 mC1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=1)) #uy
102 mC2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=2)) #uz
103 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC0]))
104 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC1]))
105 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC2]))
106
107 if useGraphics:
108 sAdd = ''
109 if nodeType == exu.NodeType.RotationRxyz:
110 sAdd = 'Rxyz' #avoid that both sensor file names are identical
111 #mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorRotation'+sAdd+'.txt', outputVariableType=exu.OutputVariableType.Rotation))
112 sAngVel+=[mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True, #fileName='solution/sensorAngVelLocal'+sAdd+'.txt',
113 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))]
114 #mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorAngVel'+sAdd+'.txt', outputVariableType=exu.OutputVariableType.AngularVelocity))
115
116 sPos+=[mbs.AddSensor(SensorBody(bodyNumber=oRB, storeInternal=True, #fileName='solution/sensorPosition'+sAdd+'.txt',
117 localPosition=rp, outputVariableType=exu.OutputVariableType.Position))]
118 sCoords+=[mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True, #fileName='solution/sensorCoordinates'+sAdd+'.txt',
119 outputVariableType=exu.OutputVariableType.Coordinates))]
120
121#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
122mbs.Assemble()
123#exu.Print(mbs)
124
125simulationSettings = exu.SimulationSettings() #takes currently set values or default values
126
127fact = 2000
128simulationSettings.timeIntegration.numberOfSteps = 1*fact
129simulationSettings.timeIntegration.endTime = 0.0001*fact
130#simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/fact
131simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/fact
132
133simulationSettings.timeIntegration.verboseMode = 1
134
135simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
136simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
137#simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well
138
139if useGraphics:
140 exu.StartRenderer()
141 mbs.WaitForUserToContinue()
142
143mbs.SolveDynamic(simulationSettings)
144
145if useGraphics:
146 SC.WaitForRenderEngineStopFlag()
147 exu.StopRenderer() #safely close rendering window!
148
149sol = mbs.systemData.GetODE2Coordinates();
150solref = mbs.systemData.GetODE2Coordinates(configuration=exu.ConfigurationType.Reference);
151#exu.Print('sol=',sol)
152u = 0
153for i in range(4):
154 u += abs(sol[3+i]+solref[3+i]); #Euler parameters
155
156for i in range(3):
157 u += abs(sol[7+3+i]+solref[7+3+i]); #Euler angles Rxyz
158
159exu.Print('solution of heavy top =',u)
160# EP ref solution MATLAB: at t=0.2
161# gen alpha (sigma=0.98, h=1e-4): -0.70813,0.43881,0.54593,0.089251 ==> abs sum=1.782121
162# RK4: -0.70828,0.43878,0.54573,0.0894 ==> abs sum=1.78219
163#Exudyn: (index2) -1.70824157 0.43878143 0.54578152 0.08937154
164
165#RotXYZ solution EXUDYN: 29.86975964,-0.7683481513,-1.002841906
166
167exudynTestGlobals.testError = u - (33.423125751773306) #2020-02-04 added RigidRxyz: (33.423125751773306) 2020-02-03: (1.7821760506326125)
168exudynTestGlobals.testResult = u
169
170
171
172#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173#compute exact solution:
174
175if useGraphics:
176
177 fileRef = '../../../docs/verification/HeavyTopSolution/HeavyTop_TimeEulerParameter_RK4.txt'
178 mbs.PlotSensor(sCoords[0], components=[3,4,5,6], labels=['theta 0','theta 1','theta 2','theta 3'],
179 closeAll=True, offsets=[1.,0,0,0], yLabel='Euler parameters') #offsets for reference coords
180 mbs.PlotSensor(fileRef, components=[0,1,2,3], labels=['theta 0 ref','theta 1 ref','theta 2 ref','theta 3 ref'],
181 colorCodeOffset=7, newFigure=False)
182
183 mbs.PlotSensor(sAngVel[0], components=[0,1,2], labels=['omega X','omega Y','omega Z'])
184 mbs.PlotSensor(sPos[0], components=[0,1,2])
185
186 # if False:
187 # import matplotlib.pyplot as plt
188 # import matplotlib.ticker as ticker
189 # plt.close("all")
190
191 # [fig1, ax1] = plt.subplots()
192 # [fig2, ax2] = plt.subplots()
193 # [fig3, ax3] = plt.subplots()
194 # data1 = np.loadtxt('solution/sensorCoordinates.txt', comments='#', delimiter=',')
195 # ax1.plot(data1[:,0], data1[:,1+3]+1, 'r-', label='theta 0') #1, because coordinates to not include ref. values
196 # ax1.plot(data1[:,0], data1[:,2+3], 'g-', label='theta 1')
197 # ax1.plot(data1[:,0], data1[:,3+3], 'b-', label='theta 2')
198 # ax1.plot(data1[:,0], data1[:,4+3], 'k-', label='theta 3')
199
200 # data1 = np.loadtxt('../../../docs/verification/HeavyTopSolution/HeavyTop_TimeEulerParameter_RK4.txt', comments='#', delimiter=',')
201 # ax1.plot(data1[:,0], data1[:,1], 'r:', label='theta 0 ref') #1, because coordinates to not include ref. values
202 # ax1.plot(data1[:,0], data1[:,2], 'g:', label='theta 1 ref')
203 # ax1.plot(data1[:,0], data1[:,3], 'b:', label='theta 2 ref')
204 # ax1.plot(data1[:,0], data1[:,4], 'k:', label='theta 3 ref')
205 # ax1.set_ylabel("Euler parameter")
206
207 # data2 = np.loadtxt('solution/sensorAngVel.txt', comments='#', delimiter=',')
208 # ax2.plot(data2[:,0], data2[:,1], 'r-', label='omega X')
209 # ax2.plot(data2[:,0], data2[:,2], 'g-', label='omega Y')
210 # ax2.plot(data2[:,0], data2[:,3], 'b-', label='omega Z')
211
212 # data3 = np.loadtxt('solution/sensorPosition.txt', comments='#', delimiter=',')
213 # ax3.plot(data3[:,0], data3[:,1], 'r-', label='position X')
214 # ax3.plot(data3[:,0], data3[:,2], 'g-', label='position Y')
215 # ax3.plot(data3[:,0], data3[:,3], 'b-', label='position Z')
216
217 # axList=[ax1,ax2,ax3]
218 # figList=[fig1, fig2, fig3]
219
220 # for ax in axList:
221 # ax.grid(True, 'major', 'both')
222 # ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
223 # ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
224 # ax.set_xlabel("time (s)")
225 # ax.legend()
226
227 # ax2.set_ylabel("angular velocity (rad/s)")
228 # ax3.set_ylabel("coordinate (m)")
229
230 # for f in figList:
231 # f.tight_layout()
232 # f.show() #bring to front