rigidBodyCOMtest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test rigid body formulation for different center of mass (COM)
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2020-04-22
  8#
  9# 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.
 10#
 11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 12
 13import exudyn as exu
 14from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics #only import if it does not conflict
 16from exudyn.FEM 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()
 33mbs = SC.AddSystem()
 34
 35nBodies = 2
 36color = [0.1,0.1,0.8,1]
 37s = 0.1 #width of cube
 38sx = 3*s #length of cube/body
 39cPosZ = 0. #offset of constraint in z-direction
 40zz = sx * (nBodies+1)*2 #max size of background
 41
 42background0 = GraphicsDataRectangle(-zz,-zz,zz,2.5*sx,color)
 43oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
 44                                   visualization=VObjectGround(graphicsData= [background0])))
 45
 46m=25
 47inertia=np.array([[10,1,2],
 48                  [ 1,7,3],
 49                  [ 2,3,6]])
 50
 51nodeList=[]
 52objectList=[]
 53for case in range(2):
 54    nRB=-1
 55    if case == 0:
 56        com=[0,0,0]
 57    else:
 58        #com=[0.4,0.6,1.3]
 59        com=[0.4,0.22,-0.35]
 60    zOff = 0.5*case*0
 61
 62    RBinertia = RigidBodyInertia(mass=m, inertiaTensor=inertia)
 63    #exu.Print("RBinertia orig =", RBinertia)
 64    RBinertia = RBinertia.Translated(com) #this includes the correct terms in inertia
 65
 66    if NormL2(RBinertia.com) != 0 and i==1:
 67        exu.Print("AddRigidBody COM=", RBinertia.com)
 68        exu.Print("inertia6D=", RBinertia.GetInertia6D())
 69    #exu.Print("RBinertia trans=", RBinertia)
 70    #exu.Print("inertia6D=", RBinertia.GetInertia6D())
 71    #exu.Print("inertia.com=", RBinertia.com)
 72    oRBlast = oGround
 73
 74    #create a chain of bodies:
 75    for i in range(nBodies):
 76        omega0 = [0,0,0] #arbitrary initial angular velocity
 77
 78        #Rotxyz:
 79        #ep0 = [0,0,0]
 80        #ep_t0 = [0,0,0]
 81
 82        p0 = VSub([i*2*sx+sx,0.,zOff],com) #reference position
 83        v0 = [0.,0.,0.] #initial translational velocity
 84
 85        color=[0.8,0.1,0.1,1]
 86        if case==0:
 87            color=[0.1,0.1,0.8,1]
 88
 89        oGraphics = GraphicsDataOrthoCubeLines(-sx+com[0],-s+com[1],-s+com[2], sx+com[0],s+com[1],s+com[2], color)
 90        d=0.02
 91        oGraphicsCOM = GraphicsDataOrthoCubeLines(-d+com[0],-d+com[1],-d+com[2], d+com[0],d+com[1],d+com[2], [0.1,0.8,0.1,1])
 92
 93        rDict = mbs.CreateRigidBody(inertia=RBinertia,
 94                                  referencePosition=p0,
 95                                  initialVelocity=v0,initialAngularVelocity=omega0,
 96                                  gravity=[0.,-9.81,0.],
 97                                  graphicsDataList=[oGraphics,oGraphicsCOM],returnDict=True)
 98        oRB = rDict['bodyNumber']
 99        nRB = rDict['nodeNumber']
100
101        val=0
102        if i==0: val=1
103        mbs.CreateGenericJoint(bodyNumbers=[oRB, oRBlast], position=VAdd([-sx,0.,0],com),
104                               constrainedAxes=[1,1,1, val,val,0], useGlobalFrame=False)
105
106        #for next chain body
107        oRBlast = oRB
108
109    sCoords=mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName="solution/sensor"+str(case)+".txt",
110                             outputVariableType=exu.OutputVariableType.Coordinates))
111    nodeList += [nRB]
112    objectList += [oRB]
113
114mbs.Assemble()
115#exu.Print(mbs)
116
117simulationSettings = exu.SimulationSettings() #takes currently set values or default values
118
119fact = 100
120simulationSettings.timeIntegration.numberOfSteps = 1*fact
121simulationSettings.timeIntegration.endTime = 0.01*fact
122simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/1000
123simulationSettings.timeIntegration.verboseMode = 1
124simulationSettings.solutionSettings.writeSolutionToFile = False
125
126simulationSettings.timeIntegration.newton.useModifiedNewton = True
127simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
128simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
129simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well
130
131simulationSettings.solutionSettings.solutionInformation = "rigid body tests"
132SC.visualizationSettings.nodes.defaultSize = 0.025
133SC.visualizationSettings.nodes.drawNodesAsPoint = False
134SC.visualizationSettings.nodes.showBasis = True
135
136#simulationSettings.displayComputationTime = True
137#simulationSettings.displayStatistics = True
138
139
140if useGraphics:
141    exu.StartRenderer()
142    mbs.WaitForUserToContinue()
143
144mbs.SolveDynamic(simulationSettings)
145
146
147
148p0=mbs.GetObjectOutputBody(objectList[0], exu.OutputVariableType.Displacement, mbs.GetObject(objectList[0])['physicsCenterOfMass'])
149#exu.Print("p0=", p0)
150p1=mbs.GetObjectOutputBody(objectList[1], exu.OutputVariableType.Displacement, mbs.GetObject(objectList[1])['physicsCenterOfMass'])
151#exu.Print("p1=", p1)
152
153#exu.Print("p0-p1=", p0-p1)
154#convergence of two formulations (difference due to time integration):
155#h=0.001:  p0-p1= [ 2.89037808e-06 -4.38559926e-07  4.83240595e-07] #similar results for Rxyz parameterization
156#h=0.0001: p0-p1= [ 2.88781241e-08 -4.40013365e-09  5.24721844e-09]
157#h=0.00001:p0-p1= [ 2.64592348e-10 -5.90557048e-11  4.66975986e-10]
158
159#+++++++++++++++++++++++++++++++++++++++++++++
160u=NormL2(p0) + NormL2(p1)
161exu.Print('solution of rigidBodyCOMtest=',u)
162
163exudynTestGlobals.testError = u - (3.409431467726293) #2020-04-22: 3.409431467726293
164exudynTestGlobals.testResult = u
165
166
167if useGraphics:
168    SC.WaitForRenderEngineStopFlag()
169    exu.StopRenderer() #safely close rendering window!