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