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