explicitLieGroupIntegratorPythonTest.py

You can view and download this file on Github: explicitLieGroupIntegratorPythonTest.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#           Test uses solver implemented in Python interface
  6#
  7# Author:   Johannes Gerstmayr, Stefan Holzinger
  8# Date:     2020-01-08
  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 *
 16from exudyn.lieGroupIntegration import *
 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()
 33#mbs = exu.MainSystem()
 34mbs = SC.AddSystem()
 35
 36color = [0.1,0.1,0.8,1]
 37r = 0.5 #radius
 38L = 1   #length
 39
 40
 41background0 = GraphicsDataRectangle(-L,-L,L,L,color)
 42oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background0])))
 43
 44#heavy top is fixed at [0,0,0] (COM of simulated body), but force is applied at [0,1,0] (COM of real top)
 45m = 15
 46Jxx=0.234375
 47Jyy=0.46875
 48Jzz=0.234375
 49#yS = 1 #distance from
 50
 51#vector to COM, where force is applied
 52rp = [0.,1.,0.]
 53rpt = np.array(Skew(rp))
 54Fg = [0,0,-m*9.81]
 55#inertia tensor w.r.t. fixed point
 56JFP = np.diag([Jxx,Jyy,Jzz]) - m*np.dot(rpt,rpt)
 57#exu.Print(JFP)
 58
 59omega0 = [0,150,-4.61538] #arbitrary initial angular velocity
 60p0 = [0,0,0] #reference position
 61v0 = [0.,0.,0.] #initial translational velocity
 62
 63#nodeType = exu.NodeType.RotationEulerParameters
 64#nodeType = exu.NodeType.RotationRxyz
 65nodeType = exu.NodeType.RotationRotationVector
 66
 67nRB = 0
 68if nodeType == exu.NodeType.RotationEulerParameters:
 69    ep0 = eulerParameters0 #no rotation
 70    ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
 71    #exu.Print(ep_t0)
 72
 73    nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, initialVelocities=v0+list(ep_t0)))
 74elif nodeType == exu.NodeType.RotationRxyz:
 75    rot0 = [0,0,0]
 76    #omega0 = [10,0,0]
 77    rot_t0 = AngularVelocity2RotXYZ_t(omega0, rot0)
 78    #exu.Print('rot_t0=',rot_t0)
 79    nRB = mbs.AddNode(NodeRigidBodyRxyz(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
 80elif nodeType == exu.NodeType.RotationRotationVector:
 81    rot0 = [0,0,0]
 82    rot_t0 = omega0
 83    #exu.Print('rot_t0=',rot_t0)
 84    nRB = mbs.AddNode(NodeRigidBodyRotVecLG(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
 85
 86
 87oGraphics = GraphicsDataOrthoCube(-r/2,-L/2,-r/2, r/2,L/2,r/2, [0.1,0.1,0.8,1])
 88oRB = mbs.AddObject(ObjectRigidBody(physicsMass=m, physicsInertia=[JFP[0][0], JFP[1][1], JFP[2][2], JFP[1][2], JFP[0][2], JFP[0][1]],
 89                                    nodeNumber=nRB, visualization=VObjectRigidBody(graphicsData=[oGraphics])))
 90
 91mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[0,1,0])) #this is the real COM
 92mbs.AddLoad(Force(markerNumber = mMassRB, loadVector=Fg))
 93
 94nPG=mbs.AddNode(PointGround(referenceCoordinates=[0,0,0])) #for coordinate constraint
 95mCground = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0)) #coordinate number does not matter
 96
 97mC0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=0)) #ux
 98mC1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=1)) #uy
 99mC2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=2)) #uz
100mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC0]))
101mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC1]))
102mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC2]))
103
104if useGraphics:
105    #mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorRotation.txt', outputVariableType=exu.OutputVariableType.Rotation))
106    sAngVelLoc=mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True))#fileName='solution/sensorAngVelLocal.txt', outputVariableType=exu.OutputVariableType.AngularVelocityLocal
107    #mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorAngVel.txt', outputVariableType=exu.OutputVariableType.AngularVelocity))
108
109    sPos=mbs.AddSensor(SensorBody(bodyNumber=oRB,
110                                    storeInternal=True,#fileName='solution/sensorPosition.txt',
111                                    localPosition=rp, outputVariableType=exu.OutputVariableType.Position))
112    sCoords=mbs.AddSensor(SensorNode(nodeNumber=nRB,
113                                    storeInternal=True,#fileName='solution/sensorCoordinates.txt',
114                                    outputVariableType=exu.OutputVariableType.Coordinates))
115
116#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
117mbs.Assemble()
118
119simulationSettings = exu.SimulationSettings() #takes currently set values or default values
120
121
122#SC.visualizationSettings.bodies.showNumbers = False
123SC.visualizationSettings.nodes.defaultSize = 0.025
124dSize=0.01
125SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
126
127
128if useGraphics: #only start graphics once, but after background is set
129    exu.StartRenderer()
130    #mbs.WaitForUserToContinue()
131
132#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
133#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
134#compute data needed for Lie group integrator:
135if nodeType == exu.NodeType.RotationRotationVector:
136    LieGroupExplicitRKInitialize(mbs)
137    #exu.Print("constrained coords=",mbs.sys['constrainedToGroundCoordinatesList'] )
138
139#STEP2000, t = 2 sec, timeToGo = 7.99602e-14 sec, Nit/step = 0
140#solver finished after 1.46113 seconds.
141#omegay= -106.16651966441937
142#single body reference solution: omegay= -106.16651966441937
143
144#convergence: (RotVecLieGroup)
145#1000: omegay= -105.89196163228372
146#2000: omegay= -106.16651966442134
147#4000: omegay= -106.16459373617013
148#8000: omegay= -106.16387282138162
149#16000:omegay= -106.16380903826868
150#32000:omegay= -106.163804467377
151#ts=[2000,4000,8000,16000,32000]
152#val=np.array([-106.16651966442134,
153#-106.16459373617013,
154#-106.16387282138162,
155#-106.16380903826868,
156#-106.163804467377]) +106.1638041640045
157#val *= -1
158#exu.Print(val)
159
160dynamicSolver = exu.MainSolverImplicitSecondOrder()
161#dynamicSolver.useOldAccBasedSolver = True
162fact = 400 #400 steps for test suite/error
163simulationSettings.timeIntegration.numberOfSteps = fact
164simulationSettings.timeIntegration.endTime = 0.01              #0.01s for test suite
165simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
166#simulationSettings.displayComputationTime = True
167simulationSettings.timeIntegration.verboseMode = 1
168simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/2000
169simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
170
171dynamicSolver.SetUserFunctionNewton(mbs, UserFunctionNewtonLieGroupRK4)
172
173dynamicSolver.SolveSystem(mbs, simulationSettings)
174#mbs.SolveDynamic(simulationSettings)
175
176omegay=mbs.GetNodeOutput(nRB,exu.OutputVariableType.AngularVelocity)[1] #y-component of angular vel
177exu.Print("explicitLieGroupIntegratorPythonTest=", omegay)
178#400 steps, tEnd=0.01, rotationVector, RK4 LieGroup integrator
179#solution is converged for 14 digits (compared to 800 steps)
180exudynTestGlobals.testError = omegay - (149.8473939540758) #2020-02-11: 149.8473939540758
181exudynTestGlobals.testResult = omegay
182
183
184if useGraphics: #only start graphics once, but after background is set
185    #SC.WaitForRenderEngineStopFlag()
186    exu.StopRenderer() #safely close rendering window!
187
188if useGraphics:
189
190
191
192    fileVerif = '../../../docs/verification/HeavyTopSolution/HeavyTop_TimeBodyAngularVelocity_RK4.txt'
193
194    mbs.PlotSensor(sensorNumbers=[sAngVelLoc]*3, labels=['omega X','omega Y','omega Z'],
195               components=[0,1,2],yLabel='angular velocity (rad/s)', closeAll=True)
196    mbs.PlotSensor(sensorNumbers=[fileVerif]*3,newFigure=False, colorCodeOffset=3+7,
197               labels=['omega ref X','omega ref Y','omega ref Z'], #markerStyles=['^ ','x ','o '],
198               components=[0,1,2],yLabel='angular velocity (rad/s)')