explicitLieGroupIntegratorTest.py
You can view and download this file on Github: explicitLieGroupIntegratorTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Lie group integration for multibody system using rotation vector formulation;
5# Uses internal C++ Lie group integrator
6#
7# Author: Johannes Gerstmayr
8# Date: 2020-01-26
9#
10# 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.
11#
12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
14import exudyn as exu
15from exudyn.utilities import *
16
17import numpy as np
18
19useGraphics = True #without test
20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
22try: #only if called from test suite
23 from modelUnitTests import exudynTestGlobals #for globally storing test results
24 useGraphics = exudynTestGlobals.useGraphics
25except:
26 class ExudynTestGlobals:
27 pass
28 exudynTestGlobals = ExudynTestGlobals()
29#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
31SC = exu.SystemContainer()
32mbs = SC.AddSystem()
33
34useConstraints = False
35drawResults = True
36
37color = [0.1,0.1,0.8,1]
38r = 0.5 #radius
39L = 1 #length
40
41
42background0 =[GraphicsDataCheckerBoard([0,0,-1], size=5),GraphicsDataBasis()]
43oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= background0)))
44
45#heavy top is fixed at [0,0,0] (COM of simulated body), but force is applied at [0,1,0] (COM of real top)
46m = 15
47Jxx=0.234375
48Jyy=0.46875
49Jzz=0.234375
50#yS = 1 #distance from
51
52#vector to COM, where force is applied
53rp = [0.,1.,0.]
54rpt = np.array(Skew(rp))
55Fg = np.array([0,0,-m*9.81])
56#inertia tensor w.r.t. fixed point
57JFP = np.diag([Jxx,Jyy,Jzz]) - m*np.dot(rpt,rpt)
58#exu.Print(JFP)
59
60omega0 = [0,150,-4.61538] #arbitrary initial angular velocity
61#omega0 = [10,20,30] #arbitrary initial angular velocity
62p0 = [0,0,0] #reference position
63v0 = [0.,0.,0.] #initial translational velocity
64
65#nodeType = exu.NodeType.RotationEulerParameters
66#nodeType = exu.NodeType.RotationRxyz
67nodeType = exu.NodeType.RotationRotationVector
68
69nRB = 0
70if nodeType == exu.NodeType.RotationEulerParameters:
71 ep0 = eulerParameters0 #no rotation
72 ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
73 #exu.Print(ep_t0)
74 nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, initialVelocities=v0+list(ep_t0)))
75elif nodeType == exu.NodeType.RotationRxyz:
76 rot0 = [0,0,0]
77 #omega0 = [10,0,0]
78 rot_t0 = AngularVelocity2RotXYZ_t(omega0, rot0)
79 #exu.Print('rot_t0=',rot_t0)
80 nRB = mbs.AddNode(NodeRigidBodyRxyz(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
81elif nodeType == exu.NodeType.RotationRotationVector:
82 rot0 = [0,0,0]
83 rot_t0 = omega0
84 #exu.Print('rot_t0=',rot_t0)
85 nRB = mbs.AddNode(NodeRigidBodyRotVecLG(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
86
87
88oGraphics = [GraphicsDataBasis(), GraphicsDataOrthoCube(-r/2,-L/2,-r/2, r/2,L/2,r/2, [0.1,0.1,0.8,0.3])]
89oRB = mbs.AddObject(ObjectRigidBody(physicsMass=m, physicsInertia=[JFP[0][0], JFP[1][1], JFP[2][2], JFP[1][2], JFP[0][2], JFP[0][1]],
90 nodeNumber=nRB, visualization=VObjectRigidBody(graphicsData=oGraphics)))
91
92mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[0,1,0])) #this is the real COM
93mbs.AddLoad(Force(markerNumber = mMassRB, loadVector=Fg))
94
95#mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[0,0,0])) #this is the real COM
96#mbs.AddLoad(Force(markerNumber = mMassRB, loadVector=-Fg))
97
98nPG=mbs.AddNode(PointGround(referenceCoordinates=[0,0,0])) #for coordinate constraint
99mCground = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0)) #coordinate number does not matter
100
101mC0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=0)) #ux
102mC1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=1)) #uy
103mC2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=2)) #uz
104if useConstraints:
105 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC0]))
106 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC1]))
107 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC2]))
108
109if True:
110 sRot = mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorRotation.txt',
111 writeToFile = drawResults,
112 outputVariableType=exu.OutputVariableType.Rotation))
113 sAngVelLoc = mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorAngVel.txt',
114 writeToFile = drawResults,
115 outputVariableType=exu.OutputVariableType.AngularVelocity))
116
117 sPos = mbs.AddSensor(SensorBody(bodyNumber=oRB, storeInternal=True,#fileName='solution/sensorPosition.txt',
118 writeToFile = drawResults,
119 localPosition=rp, outputVariableType=exu.OutputVariableType.Position))
120 sCoords = mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorCoordinates.txt',
121 writeToFile = drawResults,
122 outputVariableType=exu.OutputVariableType.Coordinates))
123
124#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
125mbs.Assemble()
126
127simulationSettings = exu.SimulationSettings() #takes currently set values or default values
128
129
130#SC.visualizationSettings.bodies.showNumbers = False
131SC.visualizationSettings.nodes.defaultSize = 0.025
132dSize=0.01
133SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
134
135if useGraphics: #only start graphics once, but after background is set
136 exu.StartRenderer()
137 mbs.WaitForUserToContinue()
138
139#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
140#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
141#comparison of Python and C++ solver:
142#STEP2000, t = 2 sec, timeToGo = 7.99602e-14 sec, Nit/step = 0
143#single body reference solution: omegay= -106.16651966441937 (python solver; 64bits)
144#single body reference solution: omegay= -106.16651966442174 (RK44, deltat=1e-3; 32bits)
145
146#reference solution and convergence results for tEnd=2:
147#single body reference solution: omegay= -106.16380619750855 (RK44, deltat=1e-4)
148#single body reference solution: omegay= -106.163804141195 (RK44, deltat=1e-5)
149#single body reference solution: omegay= -106.16380495843258 (RK67, deltat=1e-3)
150#single body reference solution: omegay= -106.1638041430917 (RK67, deltat=1e-4)
151#single body reference solution: omegay= -106.16380414217615 (RK67, deltat=5e-5)
152#single body reference solution: omegay= -106.16380414311477 (RK67, deltat=2e-5)
153#single body reference solution: omegay= -106.16380414096614 (RK67, deltat=1e-5)
154
155#single body reference solution: omegay= -106.1628865531392 (ODE23, tmax=0.01; standard settings (rtol=atol=1e-8))
156#single body reference solution: omegay= -106.16380001820764 (ODE23, tmax=0.01; atol = 1e-10, rtol=0)
157#single body reference solution: omegay= -106.16380410174922 (ODE23, tmax=0.01; atol = 1e-12, rtol=0)
158
159#single body reference solution: omegay= -106.16368051203796 (DOPRI5, tmax=0.01; 3258 steps, standard settings (rtol=atol=1e-8))
160#single body reference solution: omegay= -106.16380356935012 (DOPRI5, tmax=0.01; atol = 1e-10, rtol=0)
161#single body reference solution: omegay= -106.16380413741084 (DOPRI5, tmax=0.01; atol = 1e-12, rtol=0)
162#single body reference solution: omegay= -106.16380414306643 (DOPRI5, tmax=0.01; atol = 1e-14, rtol=0)
163
164#reference solution and convergence results for tEnd=0.01:
165#Lie group: omegay = 149.8473939540758 (converged to 14 digits), PYTHON implementation reference
166#Index2TR: omegay = 149.84738885863888 (Euler parameters)
167#Index2TR: omegay = 149.85635042397638 (Euler angles)
168#RK44,n=400: omegay = 149.8473836771092 (Euler angles)
169#RK44,n=4000: omegay = 149.84739395298053 (Euler angles)
170#RK67,n=4000: omegay = 149.84739395407394 (Euler angles)
171#RK44,n=4000: omegay = 149.89507599262697 (Rotation vector)
172#Lie group rotation vector:
173#RK44,n=400: omegay = 149.88651478249065 NodeType.RotationRotationVector
174
175if useGraphics:
176 simulationSettings.timeIntegration.verboseMode = 1
177
178simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/2000
179simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
180simulationSettings.solutionSettings.writeSolutionToFile = False
181simulationSettings.timeIntegration.explicitIntegration.useLieGroupIntegration = True
182
183methods=[
184#exu.DynamicSolverType.ExplicitEuler, #requires h=1e-4 for this example
185exu.DynamicSolverType.ExplicitMidpoint,
186exu.DynamicSolverType.RK44,
187exu.DynamicSolverType.RK67,
188exu.DynamicSolverType.DOPRI5,
189 ]
190err = 0
191for method in methods:
192 h = 1e-3
193 tEnd = 2
194 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
195 simulationSettings.timeIntegration.endTime = tEnd #0.01 for comparison
196 simulationSettings.timeIntegration.absoluteTolerance = 1e-10
197 simulationSettings.timeIntegration.relativeTolerance = 0
198
199 # print(exu.InfoStat())
200 solverType = method
201 mbs.SolveDynamic(solverType=solverType, simulationSettings=simulationSettings)
202 omega=mbs.GetSensorValues(sAngVelLoc)
203 pos=mbs.GetSensorValues(sPos)
204 coords=mbs.GetSensorValues(sCoords)
205
206 err += NormL2(coords)+NormL2(pos)
207 exu.Print(str(method)+",h=",h,":\n omega =", omega, "\n coords=", coords)
208 if method == exu.DynamicSolverType.DOPRI5:
209 nsteps = mbs.sys['dynamicSolver'].it.currentStepIndex-1 #total number of steps of automatic stepsize constrol
210 exu.Print("nSteps=",nsteps)
211 err+=nsteps/8517 #reference value
212 #exu.Print("omegay =", mbs.GetNodeOutput(nRB,exu.OutputVariableType.AngularVelocity)[1])
213 # print(exu.InfoStat())
214
215err *=1e-3 #avoid problems with 32/64 bits
216exu.Print("explicitLieGrouIntegratorTest result=",err)
217
218exudynTestGlobals.testError = err - (0.16164013319819076) #2021-01-26: 0.16164013319819076
219exudynTestGlobals.testResult = err
220exu.Print("explicitLieGrouIntegratorTest error=",exudynTestGlobals.testError)
221
222if useGraphics: #only start graphics once, but after background is set
223 #SC.WaitForRenderEngineStopFlag()
224 exu.StopRenderer() #safely close rendering window!
225
226 if drawResults:
227
228 mbs.PlotSensor(sensorNumbers=[sAngVelLoc], labels=['omega X'],subPlot=[1,3,1],
229 components=[0],yLabel='angular velocity (rad/s)', closeAll=True)
230 mbs.PlotSensor(sensorNumbers=[sAngVelLoc], labels=['omega Y'],subPlot=[1,3,2],newFigure=False,
231 components=[1],yLabel='angular velocity (rad/s)',colorCodeOffset=1)
232 mbs.PlotSensor(sensorNumbers=[sAngVelLoc], labels=['omega Z'],subPlot=[1,3,3],newFigure=False,
233 components=[2],yLabel='angular velocity (rad/s)',colorCodeOffset=1)