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