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