rigidBody2Dtest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test for RigidBody2D, using nonzero center of mass
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2015-02-05
  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
 16import numpy as np
 17
 18useGraphics = True #without test
 19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 20#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 21try: #only if called from test suite
 22    from modelUnitTests import exudynTestGlobals #for globally storing test results
 23    useGraphics = exudynTestGlobals.useGraphics
 24except:
 25    class ExudynTestGlobals:
 26        pass
 27    exudynTestGlobals = ExudynTestGlobals()
 28#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29
 30SC = exu.SystemContainer()
 31mbs = SC.AddSystem()
 32
 33
 34#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 35#rigid pendulum:
 36L = 1    #x-dim of pendulum
 37b = 0.1  #y-dim of pendulum
 38background = graphics.CheckerBoard(point=[-1,0.5,-b],size=2)
 39oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background])))
 40massRigid = 12
 41inertiaRigidCOM = massRigid/12*(L)**2
 42inertiaRigidRef = massRigid/3*(L)**2
 43g = 9.81    # gravity
 44fact = 100
 45k = 5000*fact    # stiffness of spring-damper
 46d = 50*fact      # damping of spring-damper
 47
 48#%%+++++++++++++++++++++++++++++++++++++++++++++++
 49#body with COM=0
 50com = np.array([0.5*L,0.])
 51initAngVel = 2*0 #if not 0, it does not represent same initial conditions for both cases
 52graphicsCube = graphics.Brick(size= [L,b,b], color= graphics.color.dodgerblue, addEdges=True)
 53graphicsJoint = graphics.Cylinder(pAxis=[-0.5*L,0,-0.6*b], vAxis= [0,0,1.2*b], radius = 0.55*b, color=graphics.color.darkgrey, addEdges=True)
 54
 55nRigid = mbs.AddNode(Rigid2D(referenceCoordinates=[-com[0],L+com[1],0], initialVelocities=[0,0,initAngVel]));
 56oRigid = mbs.AddObject(RigidBody2D(physicsMass=massRigid,
 57                                   physicsInertia=inertiaRigidCOM,
 58                                   nodeNumber=nRigid,
 59                                   visualization=VObjectRigidBody2D(graphicsData= [graphicsCube, graphicsJoint, graphics.Basis()])))
 60
 61mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=list(-com)+[0])) #support point
 62mRcom = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[ 0.,0.,0.])) #COM point
 63
 64mG0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[-L,L,0.]))
 65mbs.AddObject(CartesianSpringDamper(markerNumbers=[mG0,mR1],
 66                                    stiffness=[k,k,0], damping=[d,d,0]))
 67
 68mbs.AddLoad(Force(markerNumber = mRcom, loadVector = [0, -massRigid*g, 0]))
 69
 70#%%+++++++++++++++++++++++++++++++++++++++++++++++
 71#body with COM!=0
 72if True:
 73    graphicsCube2 = graphics.Brick(centerPoint=[0.5*L,0,0.2*b], size= [L,b,b], color= graphics.color.red, addEdges=True)
 74    graphicsJoint2 = graphics.Cylinder(pAxis=[-0.*L,0,-0.6*b], vAxis= [0,0,1.2*b], radius = 0.55*b, color=graphics.color.darkgrey, addEdges=True)
 75    nRigid2 = mbs.AddNode(Rigid2D(referenceCoordinates=[-L,L,0], initialVelocities=[0,0,initAngVel]));
 76    oRigid2 = mbs.AddObject(RigidBody2D(physicsMass=massRigid,
 77                                       physicsInertia=inertiaRigidRef,
 78                                       physicsCenterOfMass=com,
 79                                       nodeNumber=nRigid2,
 80                                       visualization=VObjectRigidBody2D(graphicsData= [graphicsCube2, graphicsJoint2, graphics.Basis(origin=[com[0],com[1],0])])))
 81
 82    mR12 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[ 0.,0.,0.])) #support point
 83    mRcom2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=list(com)+[0])) #COM point
 84
 85    mG02 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[-L,L,0.]))
 86    mbs.AddObject(CartesianSpringDamper(markerNumbers=[mG02,mR12],
 87                                        stiffness=[k,k,0], damping=[d,d,0]))
 88
 89    #mbs.AddLoad(Force(markerNumber = mRcom2, loadVector = [0, -massRigid*g, 0]))
 90    mMass2 = mbs.AddMarker(MarkerBodyMass(bodyNumber=oRigid2))
 91    mbs.AddLoad(LoadMassProportional(markerNumber = mMass2, loadVector = [0, -g, 0]))
 92
 93
 94#%%+++++++++++++++++++++++++++++++++++++++++++++++
 95#body with COM!=0
 96if True:
 97    graphicsCube3 = graphics.Brick(centerPoint=[0.5*L,0,0.2*b], size= [L,b,b], color= graphics.color.green, addEdges=True)
 98    graphicsJoint3 = graphics.Cylinder(pAxis=[-0.*L,0,-0.6*b], vAxis= [0,0,1.2*b], radius = 0.55*b, color=graphics.color.darkgrey, addEdges=True)
 99    oRigid3 = mbs.CreateRigidBody(referencePosition=[-L,L,0],
100                                  nodeType=exu.NodeType.RotationRxyz,
101                                  inertia=RigidBodyInertia(massRigid, np.diag([inertiaRigidRef]*3), com=list(com)+[0]),
102                                  gravity = [0,-g,0],
103                                  graphicsDataList=[graphicsCube3, graphicsJoint3, graphics.Basis(origin=[com[0],com[1],0])],
104                                  )
105    nRigid3 = mbs.GetObject(oRigid3)['nodeNumber']
106
107    mR13 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid3, localPosition=[ 0.,0.,0.])) #support point
108    mRcom3 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid3, localPosition=list(com)+[0])) #COM point
109
110    mG03 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[-L,L,0.]))
111    mbs.AddObject(CartesianSpringDamper(markerNumbers=[mG03,mR13],
112                                        stiffness=[k,k,0], damping=[d,d,0]))
113
114#+++++++++++++++++++++++++++++++++++++++
115mbs.Assemble()
116
117
118simulationSettings = exu.SimulationSettings() #takes currently set values or default values
119
120tEnd = 0.4
121stepSize = 1e-3
122
123simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
124simulationSettings.timeIntegration.endTime = tEnd
125simulationSettings.timeIntegration.startTime = 0
126simulationSettings.timeIntegration.verboseMode = 1
127
128simulationSettings.timeIntegration.newton.useModifiedNewton = True
129simulationSettings.displayStatistics = True
130
131#SC.visualizationSettings.nodes.defaultSize = 0.05
132
133SC.visualizationSettings.openGL.multiSampling = 4
134SC.visualizationSettings.openGL.lineWidth = 2
135
136if useGraphics:
137    exu.StartRenderer()
138    mbs.WaitForUserToContinue()
139
140mbs.SolveDynamic(simulationSettings)
141
142if useGraphics:
143    SC.WaitForRenderEngineStopFlag()
144    exu.StopRenderer() #safely close rendering window!
145
146phi = mbs.GetNodeOutput(nRigid, variableType=exu.OutputVariableType.Coordinates)[2]
147phi2 = mbs.GetNodeOutput(nRigid2, variableType=exu.OutputVariableType.Coordinates)[2]
148phi3 = mbs.GetNodeOutput(nRigid3, variableType=exu.OutputVariableType.Coordinates)[5]
149
150comOut = mbs.GetObjectOutputBody(oRigid, variableType = exu.OutputVariableType.Position)#, localPosition=[0,0,0])
151comOut2 = mbs.GetObjectOutputBody(oRigid2, variableType = exu.OutputVariableType.Position, localPosition=[com[0], com[1], 0])
152comOut3 = mbs.GetObjectOutputBody(oRigid3, variableType = exu.OutputVariableType.Position, localPosition=[com[0], com[1], 0])
153
154exu.Print('phis=',phi, phi2, phi3, ', err=', abs(phi2-phi3) )
155exu.Print('coms=',comOut, comOut2, comOut3, ', err=', comOut3-comOut2)
156
157u = (phi+phi2+phi3+NormL2(comOut)+NormL2(comOut2)+NormL2(comOut3))
158
159exu.Print('solution of rigidBody2Dtest =', u)
160exudynTestGlobals.testResult = u