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 * #includes itemInterface and rigidBodyUtilities
16import exudyn.graphics as graphics #only import if it does not conflict
17from exudyn.lieGroupIntegration import *
18
19import numpy as np
20
21useGraphics = True #without test
22#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
24try: #only if called from test suite
25 from modelUnitTests import exudynTestGlobals #for globally storing test results
26 useGraphics = exudynTestGlobals.useGraphics
27except:
28 class ExudynTestGlobals:
29 pass
30 exudynTestGlobals = ExudynTestGlobals()
31#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32
33SC = exu.SystemContainer()
34#mbs = exu.MainSystem()
35mbs = SC.AddSystem()
36
37color = [0.1,0.1,0.8,1]
38r = 0.5 #radius
39L = 1 #length
40
41
42background0 = GraphicsDataRectangle(-L,-L,L,L,color)
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 = [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
61p0 = [0,0,0] #reference position
62v0 = [0.,0.,0.] #initial translational velocity
63
64#nodeType = exu.NodeType.RotationEulerParameters
65#nodeType = exu.NodeType.RotationRxyz
66nodeType = exu.NodeType.RotationRotationVector
67
68nRB = 0
69if nodeType == exu.NodeType.RotationEulerParameters:
70 ep0 = eulerParameters0 #no rotation
71 ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
72 #exu.Print(ep_t0)
73
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 = graphics.BrickXYZ(-r/2,-L/2,-r/2, r/2,L/2,r/2, [0.1,0.1,0.8,1])
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
95nPG=mbs.AddNode(PointGround(referenceCoordinates=[0,0,0])) #for coordinate constraint
96mCground = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0)) #coordinate number does not matter
97
98mC0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=0)) #ux
99mC1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=1)) #uy
100mC2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=2)) #uz
101mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC0]))
102mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC1]))
103mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC2]))
104
105if useGraphics:
106 #mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorRotation.txt', outputVariableType=exu.OutputVariableType.Rotation))
107 sAngVelLoc=mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True))#fileName='solution/sensorAngVelLocal.txt', outputVariableType=exu.OutputVariableType.AngularVelocityLocal
108 #mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorAngVel.txt', outputVariableType=exu.OutputVariableType.AngularVelocity))
109
110 sPos=mbs.AddSensor(SensorBody(bodyNumber=oRB,
111 storeInternal=True,#fileName='solution/sensorPosition.txt',
112 localPosition=rp, outputVariableType=exu.OutputVariableType.Position))
113 sCoords=mbs.AddSensor(SensorNode(nodeNumber=nRB,
114 storeInternal=True,#fileName='solution/sensorCoordinates.txt',
115 outputVariableType=exu.OutputVariableType.Coordinates))
116
117#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
118mbs.Assemble()
119
120simulationSettings = exu.SimulationSettings() #takes currently set values or default values
121
122
123#SC.visualizationSettings.bodies.showNumbers = False
124SC.visualizationSettings.nodes.defaultSize = 0.025
125dSize=0.01
126SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
127
128
129if useGraphics: #only start graphics once, but after background is set
130 exu.StartRenderer()
131 #mbs.WaitForUserToContinue()
132
133#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
134#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
135#compute data needed for Lie group integrator:
136if nodeType == exu.NodeType.RotationRotationVector:
137 LieGroupExplicitRKInitialize(mbs)
138 #exu.Print("constrained coords=",mbs.sys['constrainedToGroundCoordinatesList'] )
139
140#STEP2000, t = 2 sec, timeToGo = 7.99602e-14 sec, Nit/step = 0
141#solver finished after 1.46113 seconds.
142#omegay= -106.16651966441937
143#single body reference solution: omegay= -106.16651966441937
144
145#convergence: (RotVecLieGroup)
146#1000: omegay= -105.89196163228372
147#2000: omegay= -106.16651966442134
148#4000: omegay= -106.16459373617013
149#8000: omegay= -106.16387282138162
150#16000:omegay= -106.16380903826868
151#32000:omegay= -106.163804467377
152#ts=[2000,4000,8000,16000,32000]
153#val=np.array([-106.16651966442134,
154#-106.16459373617013,
155#-106.16387282138162,
156#-106.16380903826868,
157#-106.163804467377]) +106.1638041640045
158#val *= -1
159#exu.Print(val)
160
161dynamicSolver = exu.MainSolverImplicitSecondOrder()
162#dynamicSolver.useOldAccBasedSolver = True
163fact = 400 #400 steps for test suite/error
164simulationSettings.timeIntegration.numberOfSteps = fact
165simulationSettings.timeIntegration.endTime = 0.01 #0.01s for test suite
166simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
167#simulationSettings.displayComputationTime = True
168simulationSettings.timeIntegration.verboseMode = 1
169simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/2000
170simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
171
172dynamicSolver.SetUserFunctionNewton(mbs, UserFunctionNewtonLieGroupRK4)
173
174dynamicSolver.SolveSystem(mbs, simulationSettings)
175#mbs.SolveDynamic(simulationSettings)
176
177omegay=mbs.GetNodeOutput(nRB,exu.OutputVariableType.AngularVelocity)[1] #y-component of angular vel
178exu.Print("explicitLieGroupIntegratorPythonTest=", omegay)
179#400 steps, tEnd=0.01, rotationVector, RK4 LieGroup integrator
180#solution is converged for 14 digits (compared to 800 steps)
181exudynTestGlobals.testError = omegay - (149.8473939540758) #2020-02-11: 149.8473939540758
182exudynTestGlobals.testResult = omegay
183
184
185if useGraphics: #only start graphics once, but after background is set
186 #SC.WaitForRenderEngineStopFlag()
187 exu.StopRenderer() #safely close rendering window!
188
189if useGraphics:
190
191
192
193 fileVerif = '../../../docs/verification/HeavyTopSolution/HeavyTop_TimeBodyAngularVelocity_RK4.txt'
194
195 mbs.PlotSensor(sensorNumbers=[sAngVelLoc]*3, labels=['omega X','omega Y','omega Z'],
196 components=[0,1,2],yLabel='angular velocity (rad/s)', closeAll=True)
197 mbs.PlotSensor(sensorNumbers=[fileVerif]*3,newFigure=False, colorCodeOffset=3+7,
198 labels=['omega ref X','omega ref Y','omega ref Z'], #markerStyles=['^ ','x ','o '],
199 components=[0,1,2],yLabel='angular velocity (rad/s)')