rigidBodyIMUtest.py
You can view and download this file on Github: rigidBodyIMUtest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Rigid body example, which generates test data for IMU (local angular velocity and accelerations)
5#
6# Author: Johannes Gerstmayr
7# Date: 2020-11-13
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
13import exudyn as exu
14from exudyn.itemInterface import *
15from exudyn.utilities import *
16#
17from math import sin, cos, pi
18
19SC = exu.SystemContainer()
20mbs = SC.AddSystem()
21
22print('EXUDYN version='+exu.GetVersionString())
23
24modes = ['100Hz132Xrot', '100Hz132XrotFine', '10Hz132Xrot', '100HzGeneralRotFine']
25iMode = 0
26mStr = modes[iMode]
27
28tEnd = 4
29h = 1e-5
30if mStr.find('Fine') != -1:
31 h = 1e-6 #very fine integration
32
33hSensor = 0.01 #100Hz
34if mStr.find('10Hz') != -1:
35 hSensor = 0.1
36
37localPosition = [0.05,0.05,0.05] #position for measurement of acceleration
38#localPosition = [0.0,0.0,0.0] #position for measurement of acceleration
39
40#background
41#rect = [-0.1,-0.1,0.1,0.1] #xmin,ymin,xmax,ymax
42#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
43color = [0.1,0.1,0.8,1]
44s = 0.25 #size of cube
45sx = s #x-size
46zz = 1.25*s #max size
47cPosZ = 0.1 #offset of constraint in z-direction
48
49# background0 = GraphicsDataRectangle(-zz,-zz,zz,zz,color)
50oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0]))#, visualization=VObjectGround(graphicsData= [background0])))
51# mPosLast = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oGround,
52# localPosition=[-2*sx,0,cPosZ]))
53
54
55
56omega0 = [0,0,0] #arbitrary initial angular velocity
57ep0 = eulerParameters0 #no rotation
58
59ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
60
61p0 = [0,0,0] #reference position
62v0 = [0,0,0] #initial translational velocity
63
64nGyro = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0,
65 initialVelocities=v0+list(ep_t0)))
66mass = 2
67Ixx = 0.2
68Iyy = 0.2
69Izz = 0.2
70if mStr == '100HzGeneralRot':
71 Ixx = 0.1
72 Iyy = 0.4
73
74oGraphics = GraphicsDataOrthoCube(-sx,-s,-s, sx,s,s, [0.8,0.1,0.1,1])
75oGyro = mbs.AddObject(ObjectRigidBody(physicsMass=mass,
76 physicsInertia=[Ixx,Iyy,Izz,0,0,0],
77 nodeNumber=nGyro,
78 visualization=VObjectRigidBody(graphicsData=[oGraphics])))
79
80# mMassRB = mbs.AddMarker(MarkerBodyMass(bodyNumber = oRB))
81# mbs.AddLoad(Gravity(markerNumber = mMassRB, loadVector=[0.,-9.81,0.])) #gravity in negative z-direction
82
83#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
84#add torque to rotate body around one axis; M=Ixx*omega_t
85angleXX = 0.5*2*pi #amount of rotation caused by torque
86angleYY = 0.25*2*pi #amount of rotation caused by torque
87angleZZ = 0.25*2*pi #amount of rotation caused by torque
88def UFtorque1(mbs, t, load):
89 fx = 0
90 fy = 0
91 fz = 0
92 if t <= 0.5:
93 fx = Ixx*angleXX*4 #factor 4 because of integration of accelerations
94 elif t <= 1:
95 fx = -Ixx*angleXX*4
96 elif t <= 1.5:
97 fz = Izz*angleZZ*4 #factor 4 because of integration of accelerations
98 elif t <= 2:
99 fz = -Izz*angleZZ*4
100 elif t <= 2.5:
101 fy = Iyy*angleYY*4 #factor 4 because of integration of accelerations
102 elif t <= 3:
103 fy = -Iyy*angleYY*4
104 elif t <= 2.5:
105 fy = Iyy*angleYY*4 #factor 4 because of integration of accelerations
106 fx = Ixx*angleXX*4*0.5 #factor 4 because of integration of accelerations
107 elif t <= 3:
108 fy = -Iyy*angleYY*4
109 fx = -Ixx*angleXX*4*0.5
110 return [fx,fy,fz]
111
112def UFtorque2(mbs, t, load):
113 fx = 0
114 fy = 0
115 fz = 0
116 Mloc= Ixx*angleXX*4
117 if t <= 1:
118 fx = Mloc #factor 4 because of integration of accelerations
119 fz = Mloc #factor 4 because of integration of accelerations
120 elif t <= 2:
121 fx = -Mloc
122 fz = -Mloc
123 return [fx,fy,fz]
124
125forceFact = 0
126UFtorque = UFtorque1
127if mStr.find('GeneralRot') != -1:
128 UFtorque = UFtorque2
129 forceFact = 1
130 tEnd = 3
131
132#apply COM force
133aX = 10*forceFact
134aY = 2*forceFact
135def UFforce(mbs, t, load):
136 fx = 0
137 fy = 0
138 fz = 0
139 if t <= 1:
140 fx = mass * aX
141 fy = mass * aY * t
142 elif t <= 2:
143 fx = -mass * aX
144 fy = -mass * aY * (2-t)
145 return [fx,fy,fz]
146
147
148
149mCenterRB = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGyro, localPosition = [0.,0.,0.]))
150mbs.AddLoad(Torque(markerNumber = mCenterRB,
151 loadVector=[0,0,0],
152 bodyFixed=True, #use local coordinates for torque
153 loadVectorUserFunction = UFtorque)) #gravity in negative z-direction
154mbs.AddLoad(Force(markerNumber = mCenterRB,
155 loadVector=[0,0,0],
156 bodyFixed=False, #use global coordinates for force
157 loadVectorUserFunction = UFforce)) #gravity in negative z-direction
158
159#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
160#sensors
161#all sensors placed at localPosition=[0,0,0]
162sOmegaLocal = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/angularVelocityLocal.txt',
163 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
164sRotation = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/rotation.txt',
165 outputVariableType=exu.OutputVariableType.Rotation))
166sOmega = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/angularVelocityGlobal.txt',
167 outputVariableType=exu.OutputVariableType.AngularVelocity))
168
169sPos = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/displacementGlobal.txt',
170 localPosition = localPosition,
171 outputVariableType=exu.OutputVariableType.Displacement))
172sVel = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/velocityGlobal.txt',
173 localPosition = localPosition,
174 outputVariableType=exu.OutputVariableType.Velocity))
175sAcc = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/accelerationGlobal.txt',
176 localPosition = localPosition,
177 outputVariableType=exu.OutputVariableType.Acceleration))
178sRot = mbs.AddSensor(SensorBody(bodyNumber=oGyro, fileName='solutionIMU'+mStr+'/rotationMatrix.txt',
179 outputVariableType=exu.OutputVariableType.RotationMatrix))
180
181mbs.Assemble()
182#print(mbs)
183
184simulationSettings = exu.SimulationSettings() #takes currently set values or default values
185
186
187simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
188simulationSettings.timeIntegration.endTime = tEnd
189simulationSettings.solutionSettings.solutionWritePeriod = 2e-3
190simulationSettings.solutionSettings.writeSolutionToFile = False
191simulationSettings.solutionSettings.sensorsWritePeriod = hSensor
192simulationSettings.timeIntegration.verboseMode = 1
193
194simulationSettings.timeIntegration.newton.useModifiedNewton = True
195
196#use Newmark index2, no damping
197simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
198simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
199
200simulationSettings.solutionSettings.solutionInformation = "rigid body tests"
201SC.visualizationSettings.loads.show = False
202SC.visualizationSettings.nodes.showBasis=True
203SC.visualizationSettings.nodes.basisSize=1.25
204SC.visualizationSettings.nodes.show=True
205
206SC.visualizationSettings.general.autoFitScene = 0
207exu.StartRenderer()
208SC.SetRenderState({'centerPoint': [0.0, 0.0, 0.0],
209 'maxSceneSize': 1.0,
210 'zoom': 2.0,
211 'currentWindowSize': [1024, 768],
212 'modelRotation': np.array([[0,0,1],
213 [1,0,0],
214 [0,1,0]])}) #load last model view
215
216#mbs.WaitForUserToContinue()
217mbs.SolveDynamic(simulationSettings)
218
219#SC.WaitForRenderEngineStopFlag()
220exu.StopRenderer() #safely close rendering window!
221
222if False:
223 import matplotlib.pyplot as plt
224 plt.close("all")
225 mbs.PlotSensor([sOmegaLocal]*3, components = [0,1,2])
226 plt.figure()
227 mbs.PlotSensor([sRotation]*3, components = [0,1,2])
228
229#+++++++++++++++++++++++++++++++++++++++++++++++++++++
230
231if True:
232 import matplotlib.pyplot as plt
233 import matplotlib.ticker as ticker
234 plt.close("all")
235 ax=plt.gca() # get current axes
236
237 dataRot = np.loadtxt('solutionIMU'+mStr+'/rotationMatrix.txt', comments='#', delimiter=',')
238 dataAcc = np.loadtxt('solutionIMU'+mStr+'/accelerationGlobal.txt', comments='#', delimiter=',')
239 dataVel = np.loadtxt('solutionIMU'+mStr+'/velocityGlobal.txt', comments='#', delimiter=',')
240 dataPos = np.loadtxt('solutionIMU'+mStr+'/displacementGlobal.txt', comments='#', delimiter=',')
241
242 n = len(dataAcc)
243 accLocal = np.zeros((n,4))
244 accLocal[:,0] = dataAcc[:,0]
245 rotMat = np.zeros((n,3,3))
246 for i in range(n):
247 rotMat[i,:,:] = dataRot[i,1:10].reshape(3,3)
248 accLocal[i,1:4] = rotMat[i,:,:].T @ dataAcc[i,1:4]
249
250 #plot 3 components of global accelerations
251 plt.plot(accLocal[:,0], accLocal[:,1], 'r-', label='acc0 local')
252 plt.plot(accLocal[:,0], accLocal[:,2], 'g-', label='acc1 local')
253 plt.plot(accLocal[:,0], accLocal[:,3], 'b-', label='acc2 local')
254 ax.grid(True, 'major', 'both')
255 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
256 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
257 plt.tight_layout()
258 plt.legend()
259
260 plt.figure()
261 #plot 3 components of local accelerations
262 plt.plot(dataAcc[:,0], dataAcc[:,1], 'r-', label='acc0 global')
263 plt.plot(dataAcc[:,0], dataAcc[:,2], 'g-', label='acc1 global')
264 plt.plot(dataAcc[:,0], dataAcc[:,3], 'b-', label='acc2 global')
265 ax.grid(True, 'major', 'both')
266 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
267 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
268 plt.tight_layout()
269 plt.legend()
270
271 plt.figure()
272 mbs.PlotSensor([sRotation]*3, components = [0,1,2])
273
274 plt.figure()
275 mbs.PlotSensor([sPos]*3, components = [0,1,2])
276
277 plt.figure()
278 mbs.PlotSensor([sOmega]*3, components = [0,1,2])
279
280 plt.figure()
281 mbs.PlotSensor([sVel]*3, components = [0,1,2])
282
283 ax.grid(True, 'major', 'both')
284 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
285 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
286 plt.tight_layout()
287 plt.show()