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