sliderCrank3Dbenchmark.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Slidercrank 3D  (iftomm benchmark problem)
  5#           Ref.: https://www.iftomm-multibody.org/benchmark/problem/... spatial rigid slider-crank mechanism
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2020-02-16
  9#
 10# 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.
 11#
 12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13
 14import exudyn as exu
 15from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 16import exudyn.graphics as graphics
 17from exudyn.lieGroupIntegration import *
 18
 19import numpy as np
 20from numpy import linalg as LA
 21
 22from math import pi, sin, cos
 23
 24testIsActive = exu.sys.get('testIsActive', False)
 25
 26SC = exu.SystemContainer()
 27mbs = SC.AddSystem()
 28
 29
 30###############################################################################
 31# given parameters:
 32
 33g = [0,0,-9.81]
 34lAB = 0.08 #crank length
 35lBC = 0.3  #conrod
 36theta0 = 0 #initial crank angle
 37omega0 = [6,0,0] #initial crank angular velocity
 38
 39#initial values for bodies 1 and 2 computed from initial value problem with constant angular velocity
 40v1Init = [1.2e-01, -2.400e-01, 0]
 41#omega1Init = [0,0,0]
 42#omega1Init = [1.9199960831808043, -0.9600095615843229, 0.48000362878317626]
 43omega1Init = [1.92, -0.96, 0.48] #approx. retrieved from fixedVelocity=True, looks like this is the exact values
 44
 45#initial values: 0.12,-0.24,0,-0.9343942376,-2.73093948,-0.6316790456
 46v2Init = [0.24,0,0]
 47omega2Init = [0,0,0]
 48
 49zA = 0.12       #z-position of crank CoR
 50yA = 0.1        #y-position of crank CoR
 51
 52#initial x-position of slider:
 53xD  = np.sqrt(lBC**2 - yA**2 - (zA + lAB)**2);
 54exu.Print('slider initial position =', xD)
 55
 56#initial positions of points A-C
 57pA = [0, yA, zA]
 58pB = np.array([0, yA, zA]) + [0,0,lAB]
 59pC = [xD, 0, 0]
 60
 61vCB = np.array(pC) - np.array(pB)
 62exu.Print('vCB len=', LA.norm(vCB))
 63#xAxis1 = (1/lBC)*vCB #local x-axis of conrod
 64[xAxis1,zAxis1, vDummy] = GramSchmidt(vCB, [0, yA, zA+lAB]) # compute projected pA to xAxis1 ==> gives z axis
 65yAxis1 = -np.cross(xAxis1, zAxis1)
 66
 67rotMatBC=np.array([xAxis1, yAxis1, zAxis1]).T #xAxis, etc. become row vectors ==> transpose needed!
 68# rotMatBC=np.array([xAxis1, -vDummy, zAxis1]).T #xAxis, etc. become row vectors ==> transpose needed!
 69
 70#mass and inertia
 71mAB = 0.12
 72mBC = 0.5
 73mSlider = 2
 74
 75iAB = np.diag([0.0001,0.0001,0.00001]) #crank inertia
 76#iBC = np.diag([0.004,0.0004,0.004])    #conrod inertia; iftomm: x=axis of conrod; McPhee: y=axis of conrod
 77iSlider = np.diag([0.0001,0.0001,0.0001]) #slider inertia; McPhee: no inertia of slider / does not rotate
 78
 79#Maple  / McPhee ?
 80#<Text-field prompt="&gt; " style="Maple Input" layout="Normal">m1:= 0.12; m2:= 0.5; m3:= 2; L1:= 0.08; L2:= 0.3; Ay:= 0.1; Az:= 0.12;</Text-field>
 81#<Text-field prompt="&gt; " style="Maple Input" layout="Normal">Ixx1:= 0.0001; Ixx2:= 0.0004; Iyy2:= 0.004; Izz2:= 0.004; G:= 9.81;</Text-field>
 82
 83#we choose x-axis as conrod axis!
 84#adjusted to benchmark example
 85iBC = np.diag([0.0004,0.004,0.004])    #conrod inertia; iftomm: x=axis of conrod; McPhee: y=axis of conrod
 86#original:
 87# iBC = np.diag([0.004,0.0004,0.004])    #conrod inertia; iftomm: x=axis of conrod; McPhee: y=axis of conrod
 88
 89inertiaAB = RigidBodyInertia(mass=mAB, inertiaTensor=iAB)
 90inertiaAB = inertiaAB.Translated([0,0,0.5*lAB])
 91#inertiaAB.com = [0,0,0.5*lAB]
 92inertiaBC = RigidBodyInertia(mass=mBC, inertiaTensor=iBC)
 93inertiaSlider = RigidBodyInertia(mass=mSlider, inertiaTensor=iSlider)
 94
 95fixedVelocity = False #constrain angular velocity of crank
 96
 97nodeType=exu.NodeType.RotationEulerParameters
 98# nodeType=exu.NodeType.RotationRotationVector #Lie group integrator
 99# nodeType=exu.NodeType.RotationRxyz
100
101
102################ Body0: CRANK
103#graphicsAB = graphics.BrickXYZ(-d/2,-d/2,0, d/2,d/2, lAB, [0.1,0.1,0.8,1])
104graphicsAB = graphics.RigidLink(p0=[0,0,0],p1=[0,0,lAB], axis0=[1,0,0], 
105                                   radius=[0.01,0.01], thickness = 0.01, 
106                                   width = [0.02,0.02], color=graphics.color.steelblue)
107
108b0 = mbs.CreateRigidBody(inertia=inertiaAB, 
109                         nodeType=nodeType,
110                         referencePosition=pA, 
111                         initialAngularVelocity=omega0, 
112                         gravity=g, 
113                         graphicsDataList=[graphicsAB])
114
115n0 = mbs.GetObject(b0)['nodeNumber']
116
117################ Body1: CONROD
118graphicsBC = graphics.RigidLink(p0=[-0.5*lBC,0,0],p1=[0.5*lBC,0,0], axis1=[0,0,0], 
119                                   radius=[0.01,0.01], thickness = 0.01, 
120                                   width = [0.02,0.02], color=graphics.color.lightred)
121pBC = 0.5*(np.array(pB) + pC)
122b1 = mbs.CreateRigidBody(inertia=inertiaBC, 
123                         nodeType=nodeType,
124                         referencePosition=pBC, 
125                         initialVelocity=v1Init, 
126                         initialAngularVelocity=omega1Init, 
127                         referenceRotationMatrix=rotMatBC, 
128                         gravity=g, graphicsDataList=[graphicsBC])
129n1 = mbs.GetObject(b1)['nodeNumber']
130
131################ Body2: SLIDER
132dSlider = 0.02
133graphicsSlider = graphics.Brick(size=[dSlider*2,dSlider,dSlider], color=graphics.color.dodgerblue[0:3]+[0.5])
134b2 = mbs.CreateRigidBody(inertia=inertiaSlider, 
135                         nodeType=nodeType,
136                         referencePosition=pC, 
137                         initialVelocity=v2Init, 
138                         initialAngularVelocity=[0,0,0],
139                         gravity=g,
140                         graphicsDataList=[graphicsSlider])
141n2 = mbs.GetObject(b2)['nodeNumber']
142
143dimT = 0.02
144dimZ = 0.2
145dimX = 0.3
146dimY = 0.24
147gGround = [graphics.Brick([-0.5*dimT,0,0.5*dimZ-dimT],[dimT, dimY, dimZ], color=graphics.color.grey) ]
148gGround+= [graphics.Brick([0.5*dimX-dimT,0,-dimT*0.5-dSlider*0.5],[dimX, dimY, dimT], color=graphics.color.darkgrey) ]
149gGround+= [graphics.Brick([0.5*dimX-dimT,-dSlider*0.25-dimY*0.25,-dimT*0.5],[dimX, dimY*0.5-dSlider*0.5, dimT], color=graphics.color.grey) ]
150gGround+= [graphics.Brick([0.5*dimX-dimT,+dSlider*0.25+dimY*0.25,-dimT*0.5],[dimX, dimY*0.5-dSlider*0.5, dimT], color=graphics.color.grey) ]
151
152#simple function to add ground:
153oGround = mbs.CreateGround(graphicsDataList = gGround)
154
155#conveniently add generic joints:
156mbs.CreateGenericJoint(itemNumbers=[oGround, b0],
157                       position=pA,
158                       constrainedAxes=[1,1,1,0,1,1],
159                       useGlobalFrame=False,
160                       axesRadius=0.005, axesLength=0.02)
161
162mbs.CreateGenericJoint(itemNumbers=[oGround, b2],
163                       position=[0,0,0],
164                       constrainedAxes=[0,1,1,1,1,1],
165                       useGlobalFrame=False,
166                       axesRadius=0.005, axesLength=0.02)
167
168mbs.CreateGenericJoint(itemNumbers=[b0, b1],
169                       position=[0,0,lAB],
170                       constrainedAxes=[1,1,1,0,0,0],
171                       useGlobalFrame=False,
172                       axesRadius=0.005, axesLength=0.02)
173
174# the case of two constrained axes, without joint rotations, constrains x-axis of first body (conrod) 
175#    to be perpendicular to y-axis of second body (slider), both defined in initial configuration
176mbs.CreateGenericJoint(itemNumbers=[b1, b2],
177                       position=pC, #global reference position of joint
178                       constrainedAxes=[1,1,1,0,0,1], #constraints
179                       useGlobalFrame=True, #frame for definition of position and initial axes
180                       axesRadius=0.005, axesLength=0.02)
181
182markerGroundA = mbs.AddMarker(MarkerBodyRigid(name='markerGroundA', bodyNumber=oGround, localPosition=pA))
183markerCrankA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0))
184
185
186if not testIsActive:
187    sCrankAngVel=mbs.AddSensor(SensorNode(nodeNumber = n0, storeInternal=True,fileName='solution/crankAngularVelocity.txt',
188                             outputVariableType=exu.OutputVariableType.AngularVelocity))
189    sSliderPos=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,fileName='solution/sliderPosition.txt',
190                             outputVariableType=exu.OutputVariableType.Position))
191    sSliderVel=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,fileName='solution/sliderVelocity.txt',
192                             outputVariableType=exu.OutputVariableType.Velocity))
193    sSliderAngVel=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,fileName='solution/sliderAngularVelocity.txt',
194                             outputVariableType=exu.OutputVariableType.AngularVelocity))
195
196    #add TSD to measure full revolutions; measures around local Z-axis
197    nGeneric=mbs.AddNode(NodeGenericData(initialCoordinates=[0], numberOfDataCoordinates=1)) #for infinite rotations
198    HTcrank = exu.HT().SetRotationY(0.5*pi) #turned, so that z is the crank axis
199    oTSD = mbs.AddObject(TorsionalSpringDamper(markerNumbers=[mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localHT=exu.HT(rotation=RotationMatrixY(0.5*pi), translation=pA))),
200                                                              mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localHT=HTcrank))],
201                                               nodeNumber=nGeneric,
202                                               visualization=VTorsionalSpringDamper(show=False)))
203    sCrankAngle=mbs.AddSensor(SensorObject(objectNumber = oTSD, storeInternal=True,fileName='solution/crankAngle.txt',
204                             outputVariableType=exu.OutputVariableType.Rotation))
205
206if fixedVelocity:
207    groundNode = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #add a coordinate fixed to ground
208    markerGroundCoordinate = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=groundNode, coordinate=0))
209    #markerRotX = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=n0, coordinate=3)) #Euler angle x
210    markerRotX = mbs.AddMarker(MarkerNodeRotationCoordinate(nodeNumber=n0, rotationCoordinate=0)) 
211    
212    mbs.AddObject(CoordinateConstraint(markerNumbers=[markerGroundCoordinate, markerRotX], 
213                                       offset = 6, velocityLevel=True))
214
215
216#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
217mbs.Assemble()
218#mbs.systemData.Info()
219
220simulationSettings = exu.SimulationSettings() #takes currently set values or default values
221
222stepSize = 1e-3*0.1 #slider position with 6 digits converged after 0.5 seconds
223writeStepSize = stepSize
224tEnd = 0.5 #was 0.5*10, shortened for the test suite; the comment above records that the slider
225           #position is converged to 6 digits after 0.5s, so the extra 4.5s only cost runtime
226# if fixedVelocity: #to check initial velocity
227#     stepSize = 1e-8
228#     tEnd = stepSize
229simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
230simulationSettings.timeIntegration.endTime = tEnd #0.2*5*4 #0.2 for testing
231simulationSettings.solution.file.writePeriod = writeStepSize*10
232simulationSettings.solution.sensors.writePeriod = 0.001
233simulationSettings.solution.file.write = (not testIsActive)
234simulationSettings.timeIntegration.verboseMode = 1
235simulationSettings.show.computationTime = True
236simulationSettings.show.statistics = True
237
238simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.9 #0.6 works well 
239simulationSettings.timeIntegration.newton.useModifiedNewton = True
240# simulationSettings.timeIntegration.newton.modifiedNewtonJacUpdatePerStep = True
241simulationSettings.linearSolver.solverType=exu.LinearSolverType.EigenSparse
242
243simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
244simulationSettings.timeIntegration.generalizedAlpha.lieGroupAddTangentOperator = False
245
246SC.visualizationSettings.connectors.showJointAxes = True
247SC.visualizationSettings.connectors.jointAxesLength = 0.02
248SC.visualizationSettings.connectors.jointAxesRadius = 0.002
249
250SC.visualizationSettings.nodes.drawNodesAsPoint = False
251SC.visualizationSettings.nodes.showBasis = True
252SC.visualizationSettings.nodes.basisSize = 0.05
253
254if not testIsActive:
255    SC.visualizationSettings.general.autoFitScene = False #prevent from autozoom
256    SC.renderer.Start()
257    SC.renderer.RestoreSavedState()
258    SC.renderer.DoIdleTasks()
259
260
261mbs.SolveDynamic(simulationSettings,
262                 # solverType=exu.DynamicSolverType.TrapezoidalIndex2
263                 )
264
265
266#compute initial velocities:
267if fixedVelocity:
268    v0 = mbs.GetNodeOutput(n0,exu.OutputVariableType.Coordinates_t)
269    exu.Print('v0=',list(v0))
270    
271    v1 = mbs.GetNodeOutput(n1,exu.OutputVariableType.Coordinates_t)
272    exu.Print('v1=',list(v1[0:3]))
273    omega1 = mbs.GetNodeOutput(n1,exu.OutputVariableType.AngularVelocity)
274    exu.Print('omega1=',omega1[0],omega1[1],omega1[2])
275    
276    v2 = mbs.GetNodeOutput(n2,exu.OutputVariableType.Coordinates_t)
277    exu.Print('v2=',list(v2[0:3]))
278    omega2 = mbs.GetNodeOutput(n2,exu.OutputVariableType.AngularVelocity)
279    exu.Print('omega2=',omega2[0],omega2[1],omega2[2])
280
281
282#+++++++++++++++++++++++++++++++++++++++++++++
283#compute TestModel error for EulerParameters and index2 solver
284sol = mbs.systemData.GetODE2Coordinates(); 
285sol_t = mbs.systemData.GetODE2Coordinates_t(); 
286solref = mbs.systemData.GetODE2Coordinates(configuration=exu.ConfigurationType.Reference); 
287#exu.Print('sol=',sol)
288u = 0
289for i in range(14): #take coordinates of first two bodies
290    u += abs(sol[i]+solref[i])
291
292p1 = mbs.GetNodeOutput(n1, exu.OutputVariableType.Position)
293r1 = mbs.GetNodeOutput(n1, exu.OutputVariableType.Rotation)
294v1 = mbs.GetNodeOutput(n1, exu.OutputVariableType.Velocity)
295w1 = mbs.GetNodeOutput(n1, exu.OutputVariableType.AngularVelocity)
296
297exu.Print('solution of 3D slidercrank iftomm benchmark=',p1, r1,v1,v1,w1)
298exu.Print('slider pos =', mbs.GetNodeOutput(n2, exu.OutputVariableType.Position))
299
300u = np.linalg.norm(p1) + np.linalg.norm(r1) + np.linalg.norm(v1) + np.linalg.norm(w1)
301exu.Print('error norm=', u)
302exu.sys['testResult'] = u
303
304
305if not testIsActive:
306    #SC.renderer.DoIdleTasks()
307    SC.renderer.Stop() #safely close rendering window!
308
309if False:
310    #%%+++++++++++++++++++++++
311    
312    mbs.SolutionViewer()
313    
314
315
316
317#%%+++++++++++++++++++++++++++++++++
318if not testIsActive:
319    import matplotlib.pyplot as plt
320    import matplotlib.ticker as ticker
321    plt.close("all")
322    
323    [fig1, ax1] = plt.subplots()
324    [fig2, ax2] = plt.subplots()
325    [fig3, ax3] = plt.subplots()
326    data1 = mbs.GetSensorStoredData(sCrankAngVel)
327    ax1.plot(data1[:,0], data1[:,1], 'r-', label='crank angular velocity')  
328
329
330    data1 = mbs.GetSensorStoredData(sCrankAngle)
331    ax1.plot(data1[:,0], data1[:,1], 'b-', label='crank angle')  
332    if True: #only if available ...
333        directory = '../../docs/verification/'
334        data1 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masarati.txt', comments='#', delimiter=',')
335        ax1.plot(data1[:,0], data1[:,2], 'r:', label='Ref Masarati: crank angle')  
336        data1 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masoudi.txt', comments='#', delimiter='\t')
337        ax1.plot(data1[:,0], data1[:,2], 'k:', label='Ref Masoudi: crank angle')  
338        data1 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Chaojie.txt', comments='#', delimiter=',')
339        ax1.plot(data1[:,0], data1[:,2], 'g:', label='Ref Chaojie: crank angle')  
340        data1 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial rigid slider-crank_mechanism_KarthikBushan.txt', comments='#', delimiter=',')
341        ax1.plot(data1[:,0], data1[:,2], 'c:', label='Ref Bushan: crank angle')  
342        data1 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial rigid slider-crank_mechanism_PingZhou.txt', comments='#', delimiter=',')
343        ax1.plot(data1[:,0], data1[:,3], 'm:', label='Ref Zhou: crank angle')  
344    
345    # data2 = np.loadtxt('solution/sliderPosition.txt', comments='#', delimiter=',')
346    data2 = mbs.GetSensorStoredData(sSliderPos)
347    ax2.plot(data2[:,0], data2[:,1], 'b-', label='slider position')  
348    #data2 = np.loadtxt('solution/sliderPosition_1e-4.txt', comments='#', delimiter=',')
349    #ax2.plot(data2[:,0], data2[:,1], 'r-', label='slider position, dt=1e-4')  
350#    data2 = np.loadtxt('solution/sliderVelocity.txt', comments='#', delimiter=',')
351
352    # data2 = mbs.GetSensorStoredData(sSliderVel)
353    # ax3.plot(data2[:,0], data2[:,1], 'r-', label='slider velocity')  
354    # data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial rigid slider-crank_mechanism_PingZhou.txt', comments='#', delimiter=',')
355    # ax3.plot(data2[:,0], data2[:,2], 'm:', label='Ref Zhou: slider velocity')  
356    
357    if True: #only if available ...
358        data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masarati.txt', comments='#', delimiter=',')
359        ax2.plot(data2[:,0], data2[:,1], 'r:', label='Ref Masarati: slider position')  
360        data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masoudi.txt', comments='#', delimiter='\t')
361        ax2.plot(data2[:,0], data2[:,1], 'k:', label='Ref Masoudi: slider position')  
362        # data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Chaojie.txt', comments='#', delimiter=',')
363        # ax2.plot(data2[:,0], data2[:,1], 'g:', label='Ref Chaojie: slider position')  
364        # data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial rigid slider-crank_mechanism_KarthikBushan.txt', comments='#', delimiter=',')
365        # ax2.plot(data2[:,0], data2[:,1], 'c:', label='Ref Bushan: slider position')  
366        # data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial rigid slider-crank_mechanism_PingZhou.txt', comments='#', delimiter=',')
367        # ax2.plot(data2[:,0], data2[:,1], 'm:', label='Ref Zhou: slider position')  
368
369        data2 = np.loadtxt(directory+'Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Gonzalez.txt', comments='#', delimiter=',')
370        ax2.plot(data2[:,0], data2[:,1], 'm:', label='Ref Fran: slider position')  
371    
372    axList=[ax1,ax2]#,ax3]
373    figList=[fig1, fig2]#, fig3]
374        
375    for ax in axList:
376        ax.grid(True, 'major', 'both')
377        ax.xaxis.set_major_locator(ticker.MaxNLocator(10)) 
378        ax.yaxis.set_major_locator(ticker.MaxNLocator(10)) 
379        ax.set_xlabel("time (s)")
380        ax.legend()
381        
382    ax1.set_ylabel("crank angle / angular velocity")
383    ax2.set_ylabel("slider position (m)")
384    
385    for f in figList:
386        f.tight_layout()
387        f.show() #bring to front