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