explicitLieGroupMBSTest.py

You can view and download this file on Github: explicitLieGroupMBSTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Lie group integration with RK1 and RK4 for multibody system using rotation vector formulation
  5#
  6# Author:   Johannes Gerstmayr, Stefan Holzinger
  7# Date:     2020-01-08
  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.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics
 16from exudyn.lieGroupIntegration import *
 17
 18import numpy as np
 19
 20testIsActive = exu.sys.get('testIsActive', False)
 21
 22SC = exu.SystemContainer()
 23mbs = SC.AddSystem()
 24
 25color = [0.1,0.1,0.8,1]
 26r = 0.2 #radius
 27L = 1   #length
 28
 29
 30background0 = graphics.Lines([[-L,-L,0], [L,-L,0], [L,L,0], [-L,L,0], [-L,-L,0]], color=color)
 31oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background0])))
 32
 33#heavy top is fixed at [0,0,0] (COM of simulated body), but force is applied at [0,1,0] (COM of real top)
 34m = 15
 35Jxx=0.234375
 36Jyy=0.46875
 37Jzz=0.234375
 38#yS = 1 #distance from 
 39
 40#vector to COM, where force is applied
 41rp = [0.,1.,0.]
 42rpt = Skew(rp)
 43Fg = [0,0,-m*9.81]
 44#inertia tensor w.r.t. fixed point
 45JFP = np.diag([Jxx,Jyy,Jzz]) - m*np.dot(rpt,rpt)
 46#exu.Print(JFP)
 47
 48#omega0 = [0,150,-4.61538] #arbitrary initial angular velocity
 49omega0 = [3*0,2*0,1*0] #arbitrary initial angular velocity
 50p0 = [0,0,0] #reference position
 51p1 = [0,L,0] #reference position
 52v0 = [0.,0.,0.] #initial translational velocity
 53#v0 = [1.,0.,1.] #for linear motion; initial translational velocity
 54
 55#nodeType = exu.NodeType.RotationEulerParameters
 56#nodeType = exu.NodeType.RotationRxyz
 57nodeType = exu.NodeType.RotationRotationVector
 58
 59useExplicitIntegrator = True
 60
 61useBody2 = True
 62
 63nRB = 0
 64nRB2 = 0
 65if nodeType == exu.NodeType.RotationEulerParameters:
 66    ep0 = eulerParameters0 #no rotation
 67    ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
 68    nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, initialVelocities=v0+list(ep_t0)))
 69    if useBody2:
 70        nRB2 = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p1+ep0, initialVelocities=v0+list(ep_t0)))
 71elif nodeType == exu.NodeType.RotationRxyz:
 72    rot0 = [0,1*0,0]
 73    #omega0 = [10,0,0]
 74    rot_t0 = AngularVelocity2RotXYZ_t(omega0, rot0)
 75    rotMat = RotXYZ2RotationMatrix(rot0)
 76#    exu.Print('rotMat=',rotMat)
 77    #exu.Print('rot_t0=',rot_t0)
 78    nRB = mbs.AddNode(NodeRigidBodyRxyz(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
 79    if useBody2:
 80        nRB2 = mbs.AddNode(NodeRigidBodyRxyz(referenceCoordinates=p1+rot0, initialVelocities=v0+list(rot_t0)))
 81elif nodeType == exu.NodeType.RotationRotationVector:
 82    rot0 = [0,1*0,0]
 83    rotMat = RotationVector2RotationMatrix(rot0)
 84#    exu.Print('rotMat=',rotMat)
 85#    exu.Print('rot0b=',RotationMatrix2RotationVector(rotMat))
 86
 87
 88    rot_t0 = np.dot(rotMat.transpose(),omega0)
 89    exu.Print('rot_t0=',rot_t0)
 90    nRB = mbs.AddNode(NodeRigidBodyRotVecLG(referenceCoordinates=p0+[0,0,0], initialCoordinates=[0,0,0]+rot0, initialVelocities=v0+list(rot_t0)))
 91    if useBody2:
 92        nRB2 = mbs.AddNode(NodeRigidBodyRotVecLG(referenceCoordinates=p1+[0,0,0], initialCoordinates=[0,0,0]+rot0, initialVelocities=v0+list(rot_t0)))
 93
 94
 95oGraphics = graphics.Brick(centerPoint=[0, 0, 0], size=[r, L, r], color=[0.1,0.1,0.8,0.5])
 96oRB = mbs.AddObject(ObjectRigidBody(mass=m, inertia=[JFP[0][0], JFP[1][1], JFP[2][2], JFP[1][2], JFP[0][2], JFP[0][1]], 
 97                                    nodeNumber=nRB, visualization=VObjectRigidBody(graphicsData=[oGraphics])))
 98
 99mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[r/2,L/2,0])) #this is the real COM
100#mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[0,0,0])) #for linear motion
101mbs.AddLoad(Force(markerNumber = mMassRB, loadVector=Fg)) 
102
103nPG=mbs.AddNode(PointGround(referenceCoordinates=[0,0,0])) #for coordinate constraint
104mCground = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0)) #coordinate number does not matter
105
106mC0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=0)) #ux
107mC1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=1)) #uy
108mC2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=2)) #uz
109mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC0]))
110mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC1]))
111mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC2]))
112
113#++++++++++++++++++++++++++++++++++++
114
115if useBody2:
116    oGraphics2 = graphics.Brick(centerPoint=[0, 0, 0], size=[r, L, r], color=[0.8,0.1,0.1,0.5])
117    oRB2 = mbs.AddObject(ObjectRigidBody(mass=m, inertia=[JFP[0][0], JFP[1][1], JFP[2][2], JFP[1][2], JFP[0][2], JFP[0][1]], 
118                                        nodeNumber=nRB2, visualization=VObjectRigidBody(graphicsData=[oGraphics2])))
119    
120    mMassRB2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB2, localPosition=[r/2,-L/2,0])) #this is the real COM
121    mMassRB2F = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB2, localPosition=[0,L,0])) #this is the real COM
122    mbs.AddLoad(Force(markerNumber = mMassRB2F, loadVector=Fg)) 
123    
124    k=1e4
125    mbs.AddObject(CartesianSpringDamper(markerNumbers=[mMassRB, mMassRB2], stiffness=[k,k,k]))
126
127
128if not testIsActive:
129    #mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorRotation.txt', outputVariableType=exu.OutputVariableType.Rotation))
130    mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorAngVelLocal.txt', outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
131    #mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorAngVel.txt', outputVariableType=exu.OutputVariableType.AngularVelocity))
132    
133    mbs.AddSensor(SensorBody(bodyNumber=oRB, fileName='solution/sensorPosition.txt', localPosition=rp, outputVariableType=exu.OutputVariableType.Position))
134    mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorCoordinates.txt', outputVariableType=exu.OutputVariableType.Coordinates))
135
136#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
137mbs.Assemble()
138
139simulationSettings = exu.SimulationSettings() #takes currently set values or default values
140
141
142#SC.visualizationSettings.bodies.showNumbers = False
143SC.visualizationSettings.nodes.defaultSize = 0.025
144dSize=0.01
145SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
146
147
148if not testIsActive: #only start graphics once, but after background is set
149    SC.renderer.Start()
150    #SC.renderer.DoIdleTasks()
151
152#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
153#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
154#compute data needed for Lie group integrator:
155if useExplicitIntegrator:#nodeType == exu.NodeType.RotationRotationVector:
156    LieGroupExplicitRKInitialize(mbs) #initialize Lie group nodes and coordinate constraints for explicit integration
157    
158    exu.Print("constrained to ground coords=",mbs.sys['constrainedToGroundCoordinatesList'] )
159    exu.Print("lieGroupODE2indices=",mbs.sys['lieGroupODE2indices'] )
160    
161
162#STEP2000, t = 2 sec, timeToGo = 7.99602e-14 sec, Nit/step = 0
163#solver finished after 1.46113 seconds.
164#omegay= -106.16651966441937
165#single body reference solution: omegay= -106.16651966441937
166
167#two-body, 0.5sec:
168#index3, a0=0
169#20000: omegay= 3.4938334310636976
170#40000: omegay= 3.4936345621186176
171#80000: omegay= 3.4935351274251025
172#index3:
173#20000: omegay= 3.493390210631171
174#40000: omegay= 3.493412945808683
175#80000: omegay= 3.493424317747051
176#160000:omegay= 3.4934300048200115
177#index2:
178#10000: omegay= 3.4934359503518673
179#20000: omegay= 3.4934357159579665
180#40000: omegay= 3.493435698477724
181#80000: omegay= 3.49343569408307
182
183#Rxyz:
184#1000:  omegay= 3.49438947822092
185#2000:  omegay= 3.493676264726853
186#4000:  omegay= 3.4934947182154583
187#8000:  omegay= 3.4934437605569335
188#16000: omegay= 3.49343936041691
189#32000: omegay= 3.49343670618681
190#64000: omegay= 3.49343610818212
191
192#Rotvec:
193#6:     omegay= 2.702587408085803
194#12:    omegay= 3.500994026725583
195#25:    omegay= 3.494254676339284
196#50:    omegay= 3.4934962239098266
197#100:   omegay= 3.4934396559233964
198#200:   omegay= 3.493435944205257
199#400:   omegay= 3.4934357084409986
200#1000:  omegay= 3.4934356930303623
201#2000:  omegay= 3.4934356926497165
202#4000:  omegay= 3.493435692625912
203#8000:  omegay= 3.4934357574192836
204#16000: omegay= 3.4934358339144076
205    
206ts=[6,12,25,50,100,200,400,1000,2000,8000,16000]
207val=np.array([2.702587408085803,3.500994026725583,3.494254676339284,
2083.4934962239098266,
2093.4934396559233964,
2103.493435944205257 ,
2113.4934357084409986,
2123.4934356930303623,
2133.4934356926497165,3.4934357574192836,3.4934358339144076
214]) - 3.493435692625912
215val=abs(val)
216exu.Print(val)
217
218
219#linear motion:
220#pos= -0.72625
221
222
223dynamicSolver = exu.MainSolverImplicitSecondOrder()
224
225#shortened from 1s/16000 steps to 0.1s/1600 steps when the
226#model was added to the test suite. The STEP SIZE is unchanged, so the integrator is exercised
227#exactly as before - only less simulated time. Reference value taken from the shortened model.
228fact = 1600
229simulationSettings.timeIntegration.numberOfSteps = fact
230simulationSettings.timeIntegration.endTime = 0.1
231
232simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
233simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
234simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
235#simulationSettings.show.computationTime = True
236simulationSettings.timeIntegration.verboseMode = 1
237simulationSettings.solution.sensors.writePeriod = simulationSettings.timeIntegration.endTime/2000
238if nodeType != exu.NodeType.RotationRotationVector:
239    simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = True
240else:
241    simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
242    
243if useExplicitIntegrator:#nodeType == exu.NodeType.RotationRotationVector:
244    dynamicSolver.SetUserFunctionNewton(mbs, UserFunctionNewtonLieGroupRK4)
245
246dynamicSolver.SolveSystem(mbs, simulationSettings)
247#mbs.SolveDynamic(simulationSettings)
248
249omegay=mbs.GetNodeOutput(nRB,exu.OutputVariableType.AngularVelocity)[1] #y-component of angular vel
250exu.Print("omegay=", omegay)
251#pos=mbs.GetNodeOutput(nRB,exu.OutputVariableType.Position)[2] #z-component of pos
252#exu.Print("pos=", pos)
253exu.sys['testResult'] = omegay
254
255if not testIsActive: #only start graphics once, but after background is set
256    #SC.renderer.DoIdleTasks()
257    SC.renderer.Stop() #safely close rendering window!
258
259if not testIsActive:
260    import matplotlib.pyplot as plt
261    import matplotlib.ticker as ticker
262    plt.close("all")
263    
264#    [fig1, ax1] = plt.subplots()
265    [fig2, ax2] = plt.subplots()
266    [fig3, ax3] = plt.subplots()
267#    data1 = np.loadtxt('solution/sensorCoordinates.txt', comments='#', delimiter=',')
268#    ax1.plot(data1[:,0], data1[:,1+3], 'r-', label='coordinate 0')  #1, because coordinates to not include ref. values
269#    ax1.plot(data1[:,0], data1[:,2+3], 'g-', label='coordinate 1') 
270#    ax1.plot(data1[:,0], data1[:,3+3], 'b-', label='coordinate 2') 
271    
272    
273    data2 = np.loadtxt('solution/sensorAngVelLocal.txt', comments='#', delimiter=',')
274    ax2.plot(data2[:,0], data2[:,1], 'r-', label='omega X') 
275    ax2.plot(data2[:,0], data2[:,2], 'g-', label='omega Y') 
276    ax2.plot(data2[:,0], data2[:,3], 'b-', label='omega Z') 
277
278#    data2 = np.loadtxt('solution/sensorAngVelLocalRef.txt', comments='#', delimiter=',')
279#    ax2.plot(data2[:,0], data2[:,1], 'r:', label='omega X') 
280#    ax2.plot(data2[:,0], data2[:,2], 'g:', label='omega Y') 
281#    ax2.plot(data2[:,0], data2[:,3], 'b:', label='omega Z') 
282
283#    data1 = np.loadtxt('../../docs/verification/HeavyTopSolution/HeavyTop_TimeBodyAngularVelocity_RK4.txt', comments='#', delimiter=',')
284#    ax2.plot(data1[:,0], data1[:,1], 'r:', label='omega 0 ref')  #1, because coordinates to not include ref. values
285#    ax2.plot(data1[:,0], data1[:,2], 'g:', label='omega 1 ref') 
286#    ax2.plot(data1[:,0], data1[:,3], 'b:', label='omega 2 ref') 
287    
288#    data3 = np.loadtxt('solution/sensorPosition.txt', comments='#', delimiter=',')
289#    ax3.plot(data3[:,0], data3[:,1], 'r-', label='position X') 
290#    ax3.plot(data3[:,0], data3[:,2], 'g-', label='position Y') 
291#    ax3.plot(data3[:,0], data3[:,3], 'b-', label='position Z') 
292    
293    #ax3.loglog(ts, val, 'b-', label='conv') #    
294#    axList=[ax1,ax2,ax3]
295#    figList=[fig1, fig2, fig3]
296    axList=[ax2]
297    figList=[fig2]
298    
299    for ax in axList:
300        ax.grid(True, 'major', 'both')
301#        ax.xaxis.set_major_locator(ticker.MaxNLocator(10)) 
302#        ax.yaxis.set_major_locator(ticker.MaxNLocator(10)) 
303        ax.set_xlabel("time (s)")
304        ax.legend()
305        
306    ax2.set_ylabel("angular velocity (rad/s)")
307    #ax3.set_ylabel("coordinate (m)")
308    
309    for f in figList:
310        f.tight_layout()
311        f.show() #bring to front