<!-- GENERATED by tools/generators/examplesDocsEmitter.py from python/ - do not edit -->
(testmodels-rigidbodycomtest)=
# rigidBodyCOMtest.py

You can view and download this file on Github: [rigidBodyCOMtest.py](https://github.com/jgerstmayr/EXUDYN/tree/master/python/TestModels/rigidBodyCOMtest.py)

```{code-block} python
:linenos:

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  Test rigid body formulation for different center of mass (COM)
#
# Author:   Johannes Gerstmayr
# Date:     2020-04-22
#
# 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.
#
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

import exudyn as exu
from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
import exudyn.graphics as graphics
from exudyn.FEM import *

import numpy as np

testIsActive = exu.sys.get('testIsActive', False)

SC = exu.SystemContainer()
mbs = SC.AddSystem()

nBodies = 2
color = [0.1,0.1,0.8,1]
s = 0.1 #width of cube
sx = 3*s #length of cube/body
cPosZ = 0. #offset of constraint in z-direction
zz = sx * (nBodies+1)*2 #max size of background

background0 = graphics.Lines([[-zz,-zz,0], [zz,-zz,0], [zz,2.5*sx,0], [-zz,2.5*sx,0], [-zz,-zz,0]], color=color)
oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], 
                                   visualization=VObjectGround(graphicsData= [background0])))

m=25
inertia=np.array([[10,1,2],
                  [ 1,7,3],
                  [ 2,3,6]])

nodeList=[]
objectList=[]
for case in range(2):
    nRB=-1
    if case == 0:
        com=[0,0,0]
    else:
        #com=[0.4,0.6,1.3]
        com=[0.4,0.22,-0.35]
    zOff = 0.5*case*0

    RBinertia = RigidBodyInertia(mass=m, inertiaTensor=inertia)
    #exu.Print("RBinertia orig =", RBinertia)
    RBinertia = RBinertia.Translated(com) #this includes the correct terms in inertia

    if np.linalg.norm(RBinertia.com) != 0 and i==1:
        exu.Print("AddRigidBody COM=", RBinertia.com)
        exu.Print("inertia6D=", RBinertia.GetInertia6D())
    #exu.Print("RBinertia trans=", RBinertia)
    #exu.Print("inertia6D=", RBinertia.GetInertia6D())
    #exu.Print("inertia.com=", RBinertia.com)
    oRBlast = oGround

    #create a chain of bodies:
    for i in range(nBodies):
        omega0 = [0,0,0] #arbitrary initial angular velocity
        
        #Rotxyz:
        #ep0 = [0,0,0]
        #ep_t0 = [0,0,0]

        p0 = (np.array([i*2*sx+sx,0.,zOff]) - com) #reference position
        v0 = [0.,0.,0.] #initial translational velocity

        color=[0.8,0.1,0.1,1]
        if case==0:
            color=[0.1,0.1,0.8,1]

        oGraphics = graphics.Brick(centerPoint=[com[0], com[1], com[2]], size=[2*sx, 2*s, 2*s], addFaces=False, addEdges=True, edgeColor=color)
        d=0.02
        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])

        rDict = mbs.CreateRigidBody(inertia=RBinertia, 
                                  referencePosition=p0, 
                                  initialVelocity=v0,initialAngularVelocity=omega0, 
                                  gravity=[0.,-9.81,0.],
                                  graphicsDataList=[oGraphics,oGraphicsCOM],returnDict=True)
        oRB = rDict['bodyNumber']
        nRB = rDict['nodeNumber']

        val=0
        if i==0: val=1
        mbs.CreateGenericJoint(itemNumbers=[oRB, oRBlast], position=(np.array([-sx,0.,0]) + com), 
                               constrainedAxes=[1,1,1, val,val,0], useGlobalFrame=False)

        #for next chain body
        oRBlast = oRB

    sCoords=mbs.AddSensor(SensorNode(nodeNumber=nRB, storeInternal=True,#fileName="solution/sensor"+str(case)+".txt", 
                             outputVariableType=exu.OutputVariableType.Coordinates))
    nodeList += [nRB]
    objectList += [oRB]

mbs.Assemble()
#exu.Print(mbs)

simulationSettings = exu.SimulationSettings() #takes currently set values or default values

fact = 100
simulationSettings.timeIntegration.numberOfSteps = 1*fact
simulationSettings.timeIntegration.endTime = 0.01*fact
simulationSettings.solution.file.writePeriod = simulationSettings.timeIntegration.endTime/1000
simulationSettings.timeIntegration.verboseMode = 1
simulationSettings.solution.file.write = False

simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well 

simulationSettings.solution.file.information = "rigid body tests"
SC.visualizationSettings.nodes.defaultSize = 0.025
SC.visualizationSettings.nodes.drawNodesAsPoint = False
SC.visualizationSettings.nodes.showBasis = True

#simulationSettings.show.computationTime = True
#simulationSettings.show.statistics = True


if not testIsActive:
    SC.renderer.Start()
    SC.renderer.DoIdleTasks()

mbs.SolveDynamic(simulationSettings)


p0=mbs.GetObjectOutputBody(objectList[0], exu.OutputVariableType.Displacement, mbs.GetObject(objectList[0])['centerOfMass'])
#exu.Print("p0=", p0)
p1=mbs.GetObjectOutputBody(objectList[1], exu.OutputVariableType.Displacement, mbs.GetObject(objectList[1])['centerOfMass'])
#exu.Print("p1=", p1)

#exu.Print("p0-p1=", p0-p1)
#convergence of two formulations (difference due to time integration):
#h=0.001:  p0-p1= [ 2.89037808e-06 -4.38559926e-07  4.83240595e-07] #similar results for Rxyz parameterization
#h=0.0001: p0-p1= [ 2.88781241e-08 -4.40013365e-09  5.24721844e-09]
#h=0.00001:p0-p1= [ 2.64592348e-10 -5.90557048e-11  4.66975986e-10]

#+++++++++++++++++++++++++++++++++++++++++++++
u=np.linalg.norm(p0) + np.linalg.norm(p1)
exu.Print('solution of rigidBodyCOMtest=',u)

exu.sys['testResult'] = u


if not testIsActive:
    SC.renderer.DoIdleTasks()
    SC.renderer.Stop() #safely close rendering window!
```