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()