sliderCrank3Dtest.py

You can view and download this file on Github: sliderCrank3Dtest.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 #only import if it does not conflict
 17from exudyn.lieGroupIntegration import *
 18
 19import numpy as np
 20from numpy import linalg as LA
 21
 22useGraphics = True #without test
 23#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 24#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 25try: #only if called from test suite
 26    from modelUnitTests import exudynTestGlobals #for globally storing test results
 27    useGraphics = exudynTestGlobals.useGraphics
 28except:
 29    class ExudynTestGlobals:
 30        pass
 31    exudynTestGlobals = ExudynTestGlobals()
 32#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 33
 34SC = exu.SystemContainer()
 35mbs = SC.AddSystem()
 36
 37
 38###############################################################################
 39# given parameters:
 40
 41g = [0,0,-9.81]
 42lAB = 0.08 #crank length
 43lBC = 0.3  #conrod
 44theta0 = 0 #initial crank angle
 45omega0 = [6,0,0] #initial crank angular velocity
 46
 47#initial values for bodies 1 and 2 computed from initial value problem with constant angular velocity
 48v1Init = [1.2e-01, -2.400e-01, 0]
 49#omega1Init = [7.29730172e-01, -1.39497250e+00, 4.79376439e-01]
 50omega1Init = [1.6941176470530785, -0.8470588235366621, 0.705882352947701] #appr. 10 digits accuracy
 51#initial values: 0.12,-0.24,0,-0.9343942376,-2.73093948,-0.6316790456
 52v2Init = [0.24,0,0]
 53omega2Init = [0,0,0]
 54
 55zA = 0.12       #z-position of crank CoR
 56yA = 0.1        #y-position of crank CoR
 57
 58#initial x-position of slider:
 59xD  = np.sqrt(lBC**2 - yA**2 - (zA + lAB)**2);
 60exu.Print('slider initial position =', xD)
 61
 62#initial positions of points A-C
 63pA = [0, yA, zA]
 64pB = VAdd([0, yA, zA], [0,0,lAB])
 65pC = [xD, 0, 0]
 66
 67vCB = np.array(pC) - np.array(pB)
 68exu.Print('vCB len=', LA.norm(vCB))
 69#xAxis1 = (1/lBC)*vCB #local x-axis of conrod
 70[xAxis1,zAxis1, vDummy] = GramSchmidt(vCB, pA) # compute projected pA to xAxis1 ==> gives z axis
 71yAxis1 = -np.cross(xAxis1, zAxis1)
 72
 73rotMatBC=np.array([xAxis1, yAxis1, zAxis1]).transpose()
 74
 75#mass and inertia
 76mAB = 0.12
 77mBC = 0.5
 78mSlider = 2
 79
 80iAB = np.diag([0.0001,0.00001,0.0001]) #crank inertia
 81#iBC = np.diag([0.004,0.0004,0.004])    #conrod inertia; iftomm: x=axis of conrod; McPhee: y=axis of conrod
 82iSlider = np.diag([0.0001,0.0001,0.0001]) #slider inertia; McPhee: no inertia of slider / does not rotate
 83
 84#Maple  / McPhee ?
 85#<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>
 86#<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>
 87
 88#we choose x-axis as conrod axis!
 89iBC = np.diag([0.0004,0.004,0.004])    #conrod inertia; iftomm: x=axis of conrod; McPhee: y=axis of conrod
 90
 91
 92inertiaAB = RigidBodyInertia(mass=mAB, inertiaTensor=iAB)
 93inertiaBC = RigidBodyInertia(mass=mBC, inertiaTensor=iBC)
 94inertiaSlider = RigidBodyInertia(mass=mSlider, inertiaTensor=iSlider)
 95
 96fixedVelocity = False #constrain angular velocity of crank
 97
 98nodeType=exu.NodeType.RotationEulerParameters
 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
108dict0 = mbs.CreateRigidBody(referencePosition=pA,
109                            initialAngularVelocity=omega0,
110                            inertia=inertiaAB,
111                            gravity=g,
112                            nodeType=nodeType,
113                            graphicsDataList=[graphicsAB],
114                            returnDict=True)
115[n0, b0] = [dict0['nodeNumber'], dict0['bodyNumber']]
116
117
118################ Body1: CONROD
119graphicsBC = graphics.RigidLink(p0=[-0.5*lBC,0,0],p1=[0.5*lBC,0,0], axis1=[0,0,0],
120                                   radius=[0.01,0.01], thickness = 0.01,
121                                   width = [0.02,0.02], color=graphics.color.lightred)
122pBC = ScalarMult(0.5,VAdd(pB,pC))
123dict1 = mbs.CreateRigidBody(referencePosition=pBC,
124                            referenceRotationMatrix=rotMatBC,
125                            initialVelocity=v1Init,
126                            initialAngularVelocity=omega1Init,
127                            inertia=inertiaBC,
128                            gravity=g,
129                            nodeType=nodeType,
130                            graphicsDataList=[graphicsBC],
131                            returnDict=True)
132[n1, b1] = [dict1['nodeNumber'], dict1['bodyNumber']]
133
134################ Body2: SLIDER
135d = 0.03
136graphicsSlider = graphics.BrickXYZ(-d/2,-d/2,-d/2, d/2,d/2, d/2, [0.5,0.5,0.5,0.5])
137dict2 = mbs.CreateRigidBody(referencePosition=pC,
138                            initialVelocity=v2Init,
139                            inertia=inertiaSlider,
140                            nodeType=nodeType,
141                            graphicsDataList=[graphicsSlider],
142                            returnDict=True)
143[n2,b2] = [dict2['nodeNumber'], dict2['bodyNumber']]
144
145
146oGround = mbs.CreateGround()
147markerGroundA = mbs.AddMarker(MarkerBodyRigid(name='markerGroundA', bodyNumber=oGround, localPosition=pA))
148markerGroundD = mbs.AddMarker(MarkerBodyRigid(name='markerGroundD', bodyNumber=oGround, localPosition=[0,0,0]))
149
150markerCrankA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0))
151markerCrankB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,lAB]))
152
153markerConrodB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[-0.5*lBC,0,0]))
154markerConrodC = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[ 0.5*lBC,0,0]))
155
156markerSlider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b2))
157
158mbs.AddObject(GenericJoint(markerNumbers=[markerGroundA, markerCrankA], constrainedAxes=[1,1,1,0,1,1],
159                           visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
160
161mbs.AddObject(GenericJoint(markerNumbers=[markerGroundD, markerSlider], constrainedAxes=[0,1,1,1,1,1],
162                            visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
163
164mbs.AddObject(GenericJoint(markerNumbers=[markerCrankB, markerConrodB], constrainedAxes=[1,1,1,0,0,0],
165                            visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
166
167#classical cardan, x=locked
168#mbs.AddObject(GenericJoint(markerNumbers=[markerConrodC, markerSlider], constrainedAxes=[1,1,1,1,0,0],
169#                            visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
170
171mbs.AddObject(GenericJoint(markerNumbers=[markerSlider, markerConrodC], constrainedAxes=[1,1,1,0,0,1], # xAxisMarker0=free, yAxisMarker1=free
172                            visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
173
174if useGraphics:
175    sCrankAngle=mbs.AddSensor(SensorNode(nodeNumber = n0, storeInternal=True,#fileName='solution/crankAngle.txt',
176                             outputVariableType=exu.OutputVariableType.Rotation))
177    sCrankAngVel=mbs.AddSensor(SensorNode(nodeNumber = n0, storeInternal=True,#fileName='solution/crankAngularVelocity.txt',
178                             outputVariableType=exu.OutputVariableType.AngularVelocity))
179    sSliderPos=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,#fileName='solution/sliderPosition.txt',
180                             outputVariableType=exu.OutputVariableType.Position))
181    sSliderVel=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,#fileName='solution/sliderVelocity.txt',
182                             outputVariableType=exu.OutputVariableType.Velocity))
183
184if fixedVelocity:
185    groundNode = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #add a coordinate fixed to ground
186    markerGroundCoordinate = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=groundNode, coordinate=0))
187    markerRotX = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=n0, coordinate=3)) #Euler angle x
188
189    mbs.AddObject(CoordinateConstraint(markerNumbers=[markerGroundCoordinate, markerRotX],
190                                       offset = 6, velocityLevel=True))
191
192#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
193mbs.Assemble()
194#mbs.systemData.Info()
195
196simulationSettings = exu.SimulationSettings() #takes currently set values or default values
197
198fact = 1000 #1000 for testing
199outputFact = 1000
200simulationSettings.timeIntegration.numberOfSteps = 1000
201simulationSettings.timeIntegration.endTime = 0.2 #0.2 for testing
202simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/outputFact
203simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/outputFact
204simulationSettings.solutionSettings.writeSolutionToFile = useGraphics
205simulationSettings.timeIntegration.verboseMode = 1
206
207simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
208simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
209simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 #0.6 works well
210
211simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
212
213SC.visualizationSettings.connectors.showJointAxes = True
214SC.visualizationSettings.connectors.jointAxesLength = 0.02
215SC.visualizationSettings.connectors.jointAxesRadius = 0.002
216
217if useGraphics:
218    simulationSettings.timeIntegration.numberOfSteps = 20000
219    simulationSettings.timeIntegration.endTime = 5 #0.2 for testing
220
221    exu.StartRenderer()
222    mbs.WaitForUserToContinue()
223
224mbs.SolveDynamic(simulationSettings)
225
226
227#compute initial velocities:
228#if fixedVelocity:
229#    v0 = mbs.GetNodeOutput(n0,exu.OutputVariableType.Coordinates_t)
230#    exu.Print('v0=',v0)
231#
232#    v1 = mbs.GetNodeOutput(n1,exu.OutputVariableType.Coordinates_t)
233#    exu.Print('v1=',v1[0:3])
234#    omega1 = mbs.GetNodeOutput(n1,exu.OutputVariableType.AngularVelocity)
235#    exu.Print('omega1=',omega1[0],omega1[1],omega1[2])
236#
237#    v2 = mbs.GetNodeOutput(n2,exu.OutputVariableType.Coordinates_t)
238#    exu.Print('v2=',v2[0:3])
239
240
241#+++++++++++++++++++++++++++++++++++++++++++++
242#compute TestModel error for EulerParameters and index2 solver
243sol = mbs.systemData.GetODE2Coordinates();
244solref = mbs.systemData.GetODE2Coordinates(configuration=exu.ConfigurationType.Reference);
245#exu.Print('sol=',sol)
246u = 0
247for i in range(14): #take coordinates of first two bodies
248    u += abs(sol[i]+solref[i])
249
250exu.Print('solution of 3D slidercrank iftomm benchmark=',u)
251
252exudynTestGlobals.testError = u - (3.36427617809219) #2020-04-22(corrected GenericJoint): 3.36427617809219;2020-02-19: 3.3642838177004832
253exudynTestGlobals.testResult = u
254
255
256if useGraphics:
257    #SC.WaitForRenderEngineStopFlag()
258    exu.StopRenderer() #safely close rendering window!
259
260if useGraphics:
261    import matplotlib.pyplot as plt
262    import matplotlib.ticker as ticker
263    plt.close("all")
264
265    [fig1, ax1] = plt.subplots()
266    [fig2, ax2] = plt.subplots()
267    # data1 = np.loadtxt('solution/crankAngularVelocity.txt', comments='#', delimiter=',')
268    data1 = mbs.GetSensorStoredData(sCrankAngVel)
269    ax1.plot(data1[:,0], data1[:,1], 'r-', label='crank angular velocity')
270    # data1 = np.loadtxt('solution/crankAngle.txt', comments='#', delimiter=',')
271    data1 = mbs.GetSensorStoredData(sCrankAngle)
272    ax1.plot(data1[:,0], data1[:,1], 'b-', label='crank angle')
273    if False: #only if available ...
274        data1 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masarati.txt', comments='#', delimiter=',')
275        ax1.plot(data1[:,0], data1[:,2], 'r:', label='Ref Masarati: crank angle')
276        data1 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masoudi.txt', comments='#', delimiter='\t')
277        ax1.plot(data1[:,0], data1[:,2], 'k:', label='Ref Masoudi: crank angle')
278        data1 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Chaojie.txt', comments='#', delimiter=',')
279        ax1.plot(data1[:,0], data1[:,2], 'g:', label='Ref Chaojie: crank angle')
280
281
282    # data2 = np.loadtxt('solution/sliderPosition.txt', comments='#', delimiter=',')
283    data2 = mbs.GetSensorStoredData(sSliderPos)
284    ax2.plot(data2[:,0], data2[:,1], 'b-', label='slider position')
285    #data2 = np.loadtxt('solution/sliderPosition_1e-4.txt', comments='#', delimiter=',')
286    #ax2.plot(data2[:,0], data2[:,1], 'r-', label='slider position, dt=1e-4')
287#    data2 = np.loadtxt('solution/sliderVelocity.txt', comments='#', delimiter=',')
288#    ax2.plot(data2[:,0], data2[:,1], 'r-', label='slider velocity')
289
290    if False: #only if available ...
291        data2 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masarati.txt', comments='#', delimiter=',')
292        ax2.plot(data2[:,0], data2[:,1], 'r:', label='Ref Masarati: slider position')
293        data2 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masoudi.txt', comments='#', delimiter='\t')
294        ax2.plot(data2[:,0], data2[:,1], 'k:', label='Ref Masoudi: slider position')
295        data2 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Chaojie.txt', comments='#', delimiter=',')
296        ax2.plot(data2[:,0], data2[:,1], 'g:', label='Ref Chaojie: slider position')
297
298
299    axList=[ax1,ax2]
300    figList=[fig1, fig2]
301
302    for ax in axList:
303        ax.grid(True, 'major', 'both')
304        ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
305        ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
306        ax.set_xlabel("time (s)")
307        ax.legend()
308
309    ax1.set_ylabel("crank angle / angular velocity")
310    ax2.set_ylabel("slider position (m)")
311
312    for f in figList:
313        f.tight_layout()
314        f.show() #bring to front