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
 16from exudyn.FEM import *
 17
 18import numpy as np
 19
 20testIsActive = exu.sys.get('testIsActive', False)
 21
 22SC = exu.SystemContainer()
 23mbs = SC.AddSystem()
 24
 25nBodies = 2
 26color = [0.1,0.1,0.8,1]
 27s = 0.1 #width of cube
 28sx = 3*s #length of cube/body
 29cPosZ = 0. #offset of constraint in z-direction
 30zz = sx * (nBodies+1)*2 #max size of background
 31
 32background0 = graphics.Lines([[-zz,-zz,0], [zz,-zz,0], [zz,2.5*sx,0], [-zz,2.5*sx,0], [-zz,-zz,0]], color=color)
 33oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], 
 34                                   visualization=VObjectGround(graphicsData= [background0])))
 35
 36m=25
 37inertia=np.array([[10,1,2],
 38                  [ 1,7,3],
 39                  [ 2,3,6]])
 40
 41nodeList=[]
 42objectList=[]
 43for case in range(2):
 44    nRB=-1
 45    if case == 0:
 46        com=[0,0,0]
 47    else:
 48        #com=[0.4,0.6,1.3]
 49        com=[0.4,0.22,-0.35]
 50    zOff = 0.5*case*0
 51
 52    RBinertia = RigidBodyInertia(mass=m, inertiaTensor=inertia)
 53    #exu.Print("RBinertia orig =", RBinertia)
 54    RBinertia = RBinertia.Translated(com) #this includes the correct terms in inertia
 55
 56    if np.linalg.norm(RBinertia.com) != 0 and i==1:
 57        exu.Print("AddRigidBody COM=", RBinertia.com)
 58        exu.Print("inertia6D=", RBinertia.GetInertia6D())
 59    #exu.Print("RBinertia trans=", RBinertia)
 60    #exu.Print("inertia6D=", RBinertia.GetInertia6D())
 61    #exu.Print("inertia.com=", RBinertia.com)
 62    oRBlast = oGround
 63
 64    #create a chain of bodies:
 65    for i in range(nBodies):
 66        omega0 = [0,0,0] #arbitrary initial angular velocity
 67        
 68        #Rotxyz:
 69        #ep0 = [0,0,0]
 70        #ep_t0 = [0,0,0]
 71
 72        p0 = (np.array([i*2*sx+sx,0.,zOff]) - com) #reference position
 73        v0 = [0.,0.,0.] #initial translational velocity
 74
 75        color=[0.8,0.1,0.1,1]
 76        if case==0:
 77            color=[0.1,0.1,0.8,1]
 78
 79        oGraphics = graphics.Brick(centerPoint=[com[0], com[1], com[2]], size=[2*sx, 2*s, 2*s], addFaces=False, addEdges=True, edgeColor=color)
 80        d=0.02
 81        oGraphicsCOM = graphics.Brick(centerPoint=[com[0], com[1], com[2]], size=[2*d, 2*d, 2*d], addFaces=False, addEdges=True, edgeColor=[0.1,0.8,0.1,1])
 82
 83        rDict = mbs.CreateRigidBody(inertia=RBinertia, 
 84                                  referencePosition=p0, 
 85                                  initialVelocity=v0,initialAngularVelocity=omega0, 
 86                                  gravity=[0.,-9.81,0.],
 87                                  graphicsDataList=[oGraphics,oGraphicsCOM],returnDict=True)
 88        oRB = rDict['bodyNumber']
 89        nRB = rDict['nodeNumber']
 90
 91        val=0
 92        if i==0: val=1
 93        mbs.CreateGenericJoint(itemNumbers=[oRB, oRBlast], position=(np.array([-sx,0.,0]) + com), 
 94                               constrainedAxes=[1,1,1, val,val,0], useGlobalFrame=False)
 95
 96        #for next chain body
 97        oRBlast = oRB
 98
 99    sCoords=mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName="solution/sensor"+str(case)+".txt", 
100                             outputVariableType=exu.OutputVariableType.Coordinates))
101    nodeList += [nRB]
102    objectList += [oRB]
103
104mbs.Assemble()
105#exu.Print(mbs)
106
107simulationSettings = exu.SimulationSettings() #takes currently set values or default values
108
109fact = 100
110simulationSettings.timeIntegration.numberOfSteps = 1*fact
111simulationSettings.timeIntegration.endTime = 0.01*fact
112simulationSettings.solution.file.writePeriod = simulationSettings.timeIntegration.endTime/1000
113simulationSettings.timeIntegration.verboseMode = 1
114simulationSettings.solution.file.write = False
115
116simulationSettings.timeIntegration.newton.useModifiedNewton = True
117simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
118simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
119simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well 
120
121simulationSettings.solution.file.information = "rigid body tests"
122SC.visualizationSettings.nodes.defaultSize = 0.025
123SC.visualizationSettings.nodes.drawNodesAsPoint = False
124SC.visualizationSettings.nodes.showBasis = True
125
126#simulationSettings.show.computationTime = True
127#simulationSettings.show.statistics = True
128
129
130if not testIsActive:
131    SC.renderer.Start()
132    SC.renderer.DoIdleTasks()
133
134mbs.SolveDynamic(simulationSettings)
135
136
137
138p0=mbs.GetObjectOutputBody(objectList[0], exu.OutputVariableType.Displacement, mbs.GetObject(objectList[0])['centerOfMass'])
139#exu.Print("p0=", p0)
140p1=mbs.GetObjectOutputBody(objectList[1], exu.OutputVariableType.Displacement, mbs.GetObject(objectList[1])['centerOfMass'])
141#exu.Print("p1=", p1)
142
143#exu.Print("p0-p1=", p0-p1)
144#convergence of two formulations (difference due to time integration):
145#h=0.001:  p0-p1= [ 2.89037808e-06 -4.38559926e-07  4.83240595e-07] #similar results for Rxyz parameterization
146#h=0.0001: p0-p1= [ 2.88781241e-08 -4.40013365e-09  5.24721844e-09]
147#h=0.00001:p0-p1= [ 2.64592348e-10 -5.90557048e-11  4.66975986e-10]
148
149#+++++++++++++++++++++++++++++++++++++++++++++
150u=np.linalg.norm(p0) + np.linalg.norm(p1)
151exu.Print('solution of rigidBodyCOMtest=',u)
152
153exu.sys['testResult'] = u
154
155
156if not testIsActive:
157    SC.renderer.DoIdleTasks()
158    SC.renderer.Stop() #safely close rendering window!