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