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)