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