fourBarMechanismIftomm.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  double four-bar mechanism according to IFToMM benchmarks; here, it is non-redundant
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2021-05-30
  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
 16
 17from math import sin, cos, pi
 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
 35g = [0,-9.81,0]     #gravity in m/s^2
 36
 37mass = 1 #kg
 38L = 1    #m
 39w = 0.1  #m, just for drawing
 40
 41inertiaBar = InertiaRodX(mass, L)
 42J = inertiaBar.inertiaTensor[2,2]
 43addSensors = True #turn off to obtain max. computational speed ...
 44
 45v0 = 1
 46listRef=[[0,0.5*L,0.5*pi],[0.5*L,L,0],
 47         [L,0.5*L,0.5*pi],[1.5*L,L,0],
 48         [2*L,0.5*L,0.5*pi]]   #reference positions and rotations of bars
 49listVel=[[0.5*v0,0,-v0/L],[v0,0,0],
 50         [0.5*v0,0,-v0/L],[v0,0,0],
 51         [0.5*v0,0,-v0/L]]   #reference positions and rotations of bars
 52
 53listNodes=[]
 54listBodies=[]
 55listMarkers0=[]
 56listMarkers1=[]
 57listSensors=[]
 58
 59for i in range(len(listRef)):
 60    graphicsBar = graphics.RigidLink(p0=[-0.5*L,0,0], p1=[0.5*L,0,0],
 61                                        axis0=[0,0,1],axis1=[0,0,1],
 62                                        radius=[0.5*w,0.5*w], thickness=w,
 63                                        width=[0.5*w*2,0.5*w*2],
 64                                        color=graphics.color.steelblue)
 65
 66    nRigid=mbs.AddNode(NodeRigidBody2D(referenceCoordinates=listRef[i],
 67                                       initialVelocities=listVel[i]))
 68    oRigid=mbs.AddObject(RigidBody2D(nodeNumber=nRigid, physicsMass=inertiaBar.mass,
 69                              physicsInertia=inertiaBar.inertiaTensor[2,2],
 70                              visualization=VRigidBody2D(graphicsData=[graphicsBar])))
 71
 72    mMass = mbs.AddMarker(MarkerBodyMass(bodyNumber=oRigid))
 73    lMass = mbs.AddLoad(LoadMassProportional(markerNumber=mMass, loadVector=g))
 74
 75    m0=mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[-0.5*L,0,0]))
 76    m1=mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[ 0.5*L,0,0]))
 77
 78    if addSensors: # and i==0:
 79        f=0 #set to 0 for energy computation, else 1
 80        sPos = mbs.AddSensor(SensorBody(bodyNumber=oRigid,
 81                                        localPosition=[f*0.5*L,0,0],storeInternal=True,
 82                                        outputVariableType=exu.OutputVariableType.Position))
 83        sVel = mbs.AddSensor(SensorBody(bodyNumber=oRigid,
 84                                        localPosition=[f*0.5*L,0,0],storeInternal=True,
 85                                        outputVariableType=exu.OutputVariableType.Velocity))
 86        sAng = mbs.AddSensor(SensorBody(bodyNumber=oRigid,
 87                                        localPosition=[f*0.5*L,0,0],storeInternal=True,
 88                                        outputVariableType=exu.OutputVariableType.AngularVelocity))
 89        listSensors+=[sPos,sVel,sAng]
 90
 91    listNodes+=[nRigid]
 92    listBodies+=[oRigid]
 93    listMarkers0+=[m0]
 94    listMarkers1+=[m1]
 95
 96#%%++++++++++++++++++++++++++++++++++++++++++++++++
 97#ground body and marker
 98gGround = graphics.CheckerBoard(point=[L,0,-w], size=4)
 99oGround = mbs.AddObject(ObjectGround())
100#oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
101markerGround0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
102markerGround1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[L,0,0]))
103markerGround2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[2*L,0,0]))
104
105mbs.AddObject(RevoluteJoint2D(markerNumbers=[markerGround0,listMarkers0[0]]))
106mbs.AddObject(RevoluteJoint2D(markerNumbers=[markerGround1,listMarkers0[2]]))
107mbs.AddObject(RevoluteJoint2D(markerNumbers=[markerGround2,listMarkers0[4]]))
108mbs.AddObject(RevoluteJoint2D(markerNumbers=[listMarkers1[0],listMarkers0[1]]))
109mbs.AddObject(RevoluteJoint2D(markerNumbers=[listMarkers1[1],listMarkers0[3]]))
110mbs.AddObject(RevoluteJoint2D(markerNumbers=[listMarkers1[2],listMarkers0[3]]))
111mbs.AddObject(RevoluteJoint2D(markerNumbers=[listMarkers1[3],listMarkers1[4]]))
112
113if addSensors:
114    def UFsensors(mbs, t, sensorNumbers, factors, configuration):
115        T=0
116        for i in range(int(len(sensorNumbers)/3)):
117            h = mbs.GetSensorValues(sensorNumbers[i*3+0])[1]
118            v = mbs.GetSensorValues(sensorNumbers[i*3+1])
119            omega = mbs.GetSensorValues(sensorNumbers[i*3+2])[2]
120
121            T += 0.5*NormL2(v)**2 * mass + 0.5*J*omega**2 + h*(-g[1])*mass
122        return [T - 3.5*mass*(-g[1]) - 1.5] #1.5 is initial kinetic energy
123
124    sUser = mbs.AddSensor(SensorUserFunction(sensorNumbers=listSensors,
125                                     sensorUserFunction=UFsensors,
126                                     factors=[0]*len(listSensors),storeInternal=True))
127                                     #fileName='solution/energyDoubleFourBar.txt'
128
129#%%++++++++++++++++++++++++++++++++++++++++++++++++
130#simulate:
131mbs.Assemble()
132
133simulationSettings = exu.SimulationSettings() #takes currently set values or default values
134
135tEnd = 4 #genAlpha=0.8
136h=0.01  #use small step size to detext contact switching
137#h=0.01: #rho=0.95 #CPU-time=0.0776 s on Intel i9
138#max energy error= 0.1140507007
139#h=0.001:
140#pos=10,0.3284112372,0.9445348375,0
141#vel=10,1.422958522,-0.4947565894,0
142#max energy error= 0.001143193966
143#h=0.0005:
144#pos=10,0.3284465114,0.9445225721,0
145#vel=10,1.423028497,-0.494841072,0
146#max energy error= 0.0002858041366
147#h=0.00025:
148#pos=10,0.3284552113,0.9445195467,0
149#vel=10,1.423045728,-0.4948619021,0
150#max energy error= 7.14659441e-05
151#h=0.000125:
152#pos=10,0.3284573768,0.9445187937,0
153#vel=10,1.423050003,-0.4948670827,0
154#max energy error= 1.79516107e-05
155
156simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
157simulationSettings.timeIntegration.endTime = tEnd
158simulationSettings.solutionSettings.writeSolutionToFile= False
159simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
160simulationSettings.timeIntegration.verboseMode = 1
161#simulationSettings.displayComputationTime=True
162
163if False:
164    simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
165    simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
166simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95
167simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
168simulationSettings.timeIntegration.newton.useModifiedNewton=True
169# simulationSettings.timeIntegration.simulateInRealtime= True
170# simulationSettings.timeIntegration.realtimeFactor=0.1
171
172#simulationSettings.linearSolverSettings.ignoreSingularJacobian = True #for redundant constraints
173
174SC.visualizationSettings.nodes.show = True
175SC.visualizationSettings.nodes.drawNodesAsPoint  = False
176SC.visualizationSettings.nodes.showBasis = True
177SC.visualizationSettings.nodes.basisSize = w*2
178
179# simulationSettings.timeIntegration.simulateInRealtime=True
180# simulationSettings.timeIntegration.realtimeFactor=0.1
181if True: #record animation frames:
182    SC.visualizationSettings.loads.drawSimplified=False
183    SC.visualizationSettings.general.graphicsUpdateInterval=0.01
184    SC.visualizationSettings.openGL.lineWidth=2
185    SC.visualizationSettings.exportImages.saveImageFileName = "animation/frame"
186    #SC.visualizationSettings.window.renderWindowSize=[1980,1080]
187    SC.visualizationSettings.window.renderWindowSize=[1280,720]
188    SC.visualizationSettings.openGL.multiSampling = 4
189    #simulationSettings.solutionSettings.recordImagesInterval = 0.01
190
191SC.visualizationSettings.general.autoFitScene = False #use loaded render state
192#useGraphics = True
193if useGraphics:
194    exu.StartRenderer()
195    if 'renderState' in exu.sys:
196        SC.SetRenderState(exu.sys[ 'renderState' ])
197    mbs.WaitForUserToContinue()
198
199mbs.SolveDynamic(simulationSettings)
200
201
202if useGraphics:
203    SC.WaitForRenderEngineStopFlag()
204    exu.StopRenderer() #safely close rendering window!
205
206    ##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
207    #plot results
208    if True:
209
210        mbs.PlotSensor(sensorNumbers=[listSensors[0],listSensors[1],],
211                   components=[0,0], closeAll=True)
212        mbs.PlotSensor(sensorNumbers=[sUser], title='Energy error')
213
214if addSensors:
215    #x=np.loadtxt('solution/energyDoubleFourBar.txt',comments='#', delimiter=',')
216    x=mbs.GetSensorStoredData(sUser)
217    maxEnergyError = max(abs(x[:,1]))
218    exu.Print("max energy error=",maxEnergyError)
219
220    p0=mbs.GetSensorStoredData(listSensors[0])[-1,1:] #of last bar
221    exu.Print("pos first bar=",list(p0)) #[0.05815092990659491, 0.49660695660597587, 0.0]
222    u = maxEnergyError + p0[0]
223
224    exu.Print('fourBarMechanismIftomm result:', u)
225    exudynTestGlobals.testError = u - (0.1721665271840173)
226    exudynTestGlobals.testResult = u