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