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
 16import numpy as np
 17
 18testIsActive = exu.sys.get('testIsActive', False)
 19
 20SC = exu.SystemContainer()
 21mbs = SC.AddSystem()
 22
 23
 24#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 25#rigid pendulum:
 26L = 1    #x-dim of pendulum
 27b = 0.1  #y-dim of pendulum
 28background = graphics.CheckerBoard(point=[-1,0.5,-b],size=2)
 29oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background])))
 30massRigid = 12
 31inertiaRigidCOM = massRigid/12*(L)**2
 32inertiaRigidRef = massRigid/3*(L)**2
 33g = 9.81    # gravity
 34fact = 100
 35k = 5000*fact    # stiffness of spring-damper
 36d = 50*fact      # damping of spring-damper
 37
 38#%%+++++++++++++++++++++++++++++++++++++++++++++++
 39#body with COM=0
 40com = np.array([0.5*L,0.])
 41initAngVel = 2*0 #if not 0, it does not represent same initial conditions for both cases
 42graphicsCube = graphics.Brick(size= [L,b,b], color= graphics.color.dodgerblue, addEdges=True)
 43graphicsJoint = 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)
 44
 45nRigid = mbs.AddNode(Rigid2D(referenceCoordinates=[-com[0],L+com[1],0], initialVelocities=[0,0,initAngVel]));
 46oRigid = mbs.AddObject(RigidBody2D(mass=massRigid, 
 47                                   inertia=inertiaRigidCOM,
 48                                   nodeNumber=nRigid,
 49                                   visualization=VObjectRigidBody2D(graphicsData= [graphicsCube, graphicsJoint, graphics.Basis()])))
 50
 51mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=list(-com)+[0])) #support point
 52mRcom = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[ 0.,0.,0.])) #COM point
 53
 54mG0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[-L,L,0.]))
 55mbs.AddObject(CartesianSpringDamper(markerNumbers=[mG0,mR1],
 56                                    stiffness=[k,k,0], damping=[d,d,0]))
 57
 58mbs.AddLoad(Force(markerNumber = mRcom, loadVector = [0, -massRigid*g, 0]))
 59
 60#%%+++++++++++++++++++++++++++++++++++++++++++++++
 61#body with COM!=0
 62if True:
 63    graphicsCube2 = graphics.Brick(centerPoint=[0.5*L,0,0.2*b], size= [L,b,b], color= graphics.color.red, addEdges=True)
 64    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)
 65    nRigid2 = mbs.AddNode(Rigid2D(referenceCoordinates=[-L,L,0], initialVelocities=[0,0,initAngVel]));
 66    oRigid2 = mbs.AddObject(RigidBody2D(mass=massRigid, 
 67                                       inertia=inertiaRigidRef,
 68                                       centerOfMass=com,
 69                                       nodeNumber=nRigid2,
 70                                       visualization=VObjectRigidBody2D(graphicsData= [graphicsCube2, graphicsJoint2, graphics.Basis(origin=[com[0],com[1],0])])))
 71
 72    mR12 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[ 0.,0.,0.])) #support point
 73    mRcom2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=list(com)+[0])) #COM point
 74
 75    mG02 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[-L,L,0.]))
 76    mbs.AddObject(CartesianSpringDamper(markerNumbers=[mG02,mR12],
 77                                        stiffness=[k,k,0], damping=[d,d,0]))
 78
 79    #mbs.AddLoad(Force(markerNumber = mRcom2, loadVector = [0, -massRigid*g, 0]))
 80    mMass2 = mbs.AddMarker(MarkerBodyMass(bodyNumber=oRigid2))
 81    mbs.AddLoad(LoadMassProportional(markerNumber = mMass2, loadVector = [0, -g, 0]))
 82
 83
 84#%%+++++++++++++++++++++++++++++++++++++++++++++++
 85#body with COM!=0
 86if True:
 87    graphicsCube3 = graphics.Brick(centerPoint=[0.5*L,0,0.2*b], size= [L,b,b], color= graphics.color.green, addEdges=True)
 88    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)
 89    oRigid3 = mbs.CreateRigidBody(referencePosition=[-L,L,0], 
 90                                  nodeType=exu.NodeType.RotationRxyz,
 91                                  inertia=RigidBodyInertia(massRigid, np.diag([inertiaRigidRef]*3), com=list(com)+[0]),
 92                                  gravity = [0,-g,0],
 93                                  graphicsDataList=[graphicsCube3, graphicsJoint3, graphics.Basis(origin=[com[0],com[1],0])],
 94                                  )
 95    nRigid3 = mbs.GetObject(oRigid3)['nodeNumber']
 96
 97    mR13 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid3, localPosition=[ 0.,0.,0.])) #support point
 98    mRcom3 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid3, localPosition=list(com)+[0])) #COM point
 99
100    mG03 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[-L,L,0.]))
101    mbs.AddObject(CartesianSpringDamper(markerNumbers=[mG03,mR13],
102                                        stiffness=[k,k,0], damping=[d,d,0]))
103
104#+++++++++++++++++++++++++++++++++++++++
105mbs.Assemble()
106
107
108simulationSettings = exu.SimulationSettings() #takes currently set values or default values
109
110tEnd = 0.4
111stepSize = 1e-3
112
113simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
114simulationSettings.timeIntegration.endTime = tEnd
115simulationSettings.timeIntegration.startTime = 0
116simulationSettings.timeIntegration.verboseMode = 1
117
118simulationSettings.timeIntegration.newton.useModifiedNewton = True
119simulationSettings.show.statistics = True
120
121#SC.visualizationSettings.nodes.defaultSize = 0.05
122
123SC.visualizationSettings.openGL.multiSampling = 4
124SC.visualizationSettings.openGL.lineWidth = 2
125
126if not testIsActive:
127    SC.renderer.Start()
128    SC.renderer.DoIdleTasks()
129
130mbs.SolveDynamic(simulationSettings)
131
132if not testIsActive:
133    SC.renderer.DoIdleTasks()
134    SC.renderer.Stop() #safely close rendering window!
135
136phi = mbs.GetNodeOutput(nRigid, variableType=exu.OutputVariableType.Coordinates)[2]
137phi2 = mbs.GetNodeOutput(nRigid2, variableType=exu.OutputVariableType.Coordinates)[2]
138phi3 = mbs.GetNodeOutput(nRigid3, variableType=exu.OutputVariableType.Coordinates)[5]
139
140comOut = mbs.GetObjectOutputBody(oRigid, variableType = exu.OutputVariableType.Position)#, localPosition=[0,0,0])
141comOut2 = mbs.GetObjectOutputBody(oRigid2, variableType = exu.OutputVariableType.Position, localPosition=[com[0], com[1], 0])
142comOut3 = mbs.GetObjectOutputBody(oRigid3, variableType = exu.OutputVariableType.Position, localPosition=[com[0], com[1], 0])
143
144exu.Print('phis=',phi, phi2, phi3, ', err=', abs(phi2-phi3) )
145exu.Print('coms=',comOut, comOut2, comOut3, ', err=', comOut3-comOut2)
146
147u = (phi+phi2+phi3+np.linalg.norm(comOut)+np.linalg.norm(comOut2)+np.linalg.norm(comOut3))
148
149exu.Print('solution of rigidBody2Dtest =', u)
150exu.sys['testResult'] = u