heavyTop.py

You can view and download this file on Github: heavyTop.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Heavy top example
  5#           Refs.:  Terze, Z., Müller, A., Zlatar, D.: Singularity-free time integration of rotational quaternions using non-redundant ordinary differential equations. Multibody System Dynamics 38(3),201–225 (2016)
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2020-02-02
  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
 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()
 33mbs = SC.AddSystem()
 34
 35#exu.Print('EXUDYN version='+exu.config.Version())
 36
 37#background
 38#rect = [-0.1,-0.1,0.1,0.1] #xmin,ymin,xmax,ymax
 39#background0 = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':[rect[0],rect[1],0, rect[2],rect[1],0, rect[2],rect[3],0, rect[0],rect[3],0, rect[0],rect[1],0]} #background
 40color = [0.1,0.1,0.8,1]
 41r = 0.5 #radius
 42L = 1   #length
 43
 44
 45background0 = GraphicsDataRectangle(-L,-L,L,L,color)
 46oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background0])))
 47
 48#heavy top is fixed at [0,0,0] (COM of simulated body), but force is applied at [0,1,0] (COM of real top)
 49m = 15
 50Jxx=0.234375
 51Jyy=0.46875
 52Jzz=0.234375
 53#yS = 1 #distance from
 54
 55#vector to COM, where force is applied
 56rp = [0.,1.,0.]
 57#rpt = np.array(Skew(rp))
 58rpt = Skew(rp)
 59Fg = [0,0,-m*9.81]
 60#inertia tensor w.r.t. fixed point
 61JFP = np.diag([Jxx,Jyy,Jzz]) - m*np.dot(rpt,rpt)
 62#exu.Print(JFP)
 63
 64omega0 = [0,150,-4.61538] #arbitrary initial angular velocity
 65p0 = [0,0,0] #reference position
 66v0 = [0.,0.,0.] #initial translational velocity
 67
 68nodeTypeList = [exu.NodeType.RotationEulerParameters, exu.NodeType.RotationRxyz]
 69
 70sAngVel=[]
 71sPos=[]
 72sCoords=[]
 73for nodeType in nodeTypeList:
 74
 75    nRB = 0
 76    if nodeType == exu.NodeType.RotationEulerParameters:
 77        ep0 = eulerParameters0 #no rotation
 78        ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
 79        #exu.Print(ep_t0)
 80
 81        nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, initialVelocities=v0+list(ep_t0)))
 82    else: #Rxyz
 83        rot0 = [0,0,0] #no rotation
 84        #omega0 = [10,0,0]
 85        rot_t0 = AngularVelocity2RotXYZ_t(omega0, rot0)
 86        #exu.Print('rot_t0=',rot_t0)
 87
 88        nRB = mbs.AddNode(NodeRigidBodyRxyz(referenceCoordinates=p0+rot0, initialVelocities=v0+list(rot_t0)))
 89
 90    oGraphics = graphics.BrickXYZ(-r/2,-L/2,-r/2, r/2,L/2,r/2, [0.1,0.1,0.8,1])
 91    oRB = mbs.AddObject(ObjectRigidBody(physicsMass=m, physicsInertia=[JFP[0][0], JFP[1][1], JFP[2][2], JFP[1][2], JFP[0][2], JFP[0][1]],
 92                                        nodeNumber=nRB, visualization=VObjectRigidBody(graphicsData=[oGraphics])))
 93
 94    mMassRB = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition=[0,1,0])) #this is the real COM
 95    mbs.AddLoad(Force(markerNumber = mMassRB, loadVector=Fg))
 96
 97    nPG=mbs.AddNode(PointGround(referenceCoordinates=[0,0,0])) #for coordinate constraint
 98    mCground = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0)) #coordinate number does not matter
 99
100    mC0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=0)) #ux
101    mC1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=1)) #uy
102    mC2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRB, coordinate=2)) #uz
103    mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC0]))
104    mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC1]))
105    mbs.AddObject(CoordinateConstraint(markerNumbers=[mCground, mC2]))
106
107    if useGraphics:
108        sAdd = ''
109        if nodeType == exu.NodeType.RotationRxyz:
110            sAdd = 'Rxyz' #avoid that both sensor file names are identical
111        #mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName='solution/sensorRotation'+sAdd+'.txt', outputVariableType=exu.OutputVariableType.Rotation))
112        sAngVel+=[mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True, #fileName='solution/sensorAngVelLocal'+sAdd+'.txt',
113                                           outputVariableType=exu.OutputVariableType.AngularVelocityLocal))]
114        #mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName='solution/sensorAngVel'+sAdd+'.txt', outputVariableType=exu.OutputVariableType.AngularVelocity))
115
116        sPos+=[mbs.AddSensor(SensorBody(bodyNumber=oRB, storeInternal=True, #fileName='solution/sensorPosition'+sAdd+'.txt',
117                                        localPosition=rp, outputVariableType=exu.OutputVariableType.Position))]
118        sCoords+=[mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True, #fileName='solution/sensorCoordinates'+sAdd+'.txt',
119                                           outputVariableType=exu.OutputVariableType.Coordinates))]
120
121#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
122mbs.Assemble()
123#exu.Print(mbs)
124
125simulationSettings = exu.SimulationSettings() #takes currently set values or default values
126
127fact = 2000
128simulationSettings.timeIntegration.numberOfSteps = 1*fact
129simulationSettings.timeIntegration.endTime = 0.0001*fact
130#simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/fact
131simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/fact
132
133simulationSettings.timeIntegration.verboseMode = 1
134
135simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
136simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
137#simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well
138
139if useGraphics:
140    SC.renderer.Start()
141    SC.renderer.DoIdleTasks()
142
143mbs.SolveDynamic(simulationSettings)
144
145if useGraphics:
146    SC.renderer.DoIdleTasks()
147    SC.renderer.Stop() #safely close rendering window!
148
149solTotal = mbs.systemData.GetODE2CoordinatesTotal()
150u = 0
151for i in range(4):
152    u += abs(solTotal[3+i]); #Euler parameters
153for i in range(3):
154    u += abs(solTotal[7+3+i]); #Euler angles Rxyz
155
156exu.Print('sol=',solTotal)
157
158exu.Print('solution of heavy top =',u)
159# EP ref solution MATLAB: at t=0.2
160#  gen alpha (sigma=0.98, h=1e-4): -0.70813,0.43881,0.54593,0.089251 ==> abs sum=1.782121
161#  RK4:                            -0.70828,0.43878,0.54573,0.0894   ==> abs sum=1.78219
162#Exudyn: (index2)                  -1.70824157  0.43878143  0.54578152   0.08937154
163
164#RotXYZ solution EXUDYN:           29.86975964,-0.7683481513,-1.002841906
165
166exudynTestGlobals.testError = u - (33.42312575174431) #2020-02-04 added RigidRxyz: (33.423125751773306) 2020-02-03: (1.7821760506326125)
167exudynTestGlobals.testResult = u
168
169
170
171#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
172#compute exact solution:
173
174if useGraphics:
175
176    fileRef = '../../../docs/verification/HeavyTopSolution/HeavyTop_TimeEulerParameter_RK4.txt'
177    mbs.PlotSensor(sCoords[0], components=[3,4,5,6], labels=['theta 0','theta 1','theta 2','theta 3'],
178               closeAll=True, offsets=[1.,0,0,0], yLabel='Euler parameters') #offsets for reference coords
179    mbs.PlotSensor(fileRef, components=[0,1,2,3], labels=['theta 0 ref','theta 1 ref','theta 2 ref','theta 3 ref'],
180               colorCodeOffset=7, newFigure=False)
181
182    mbs.PlotSensor(sAngVel[0], components=[0,1,2], labels=['omega X','omega Y','omega Z'])
183    mbs.PlotSensor(sPos[0], components=[0,1,2])