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