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="> " 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="> " 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
108[n0,b0]=AddRigidBody(mainSys = mbs, inertia=inertiaAB, nodeType=str(nodeType),
109 position=pA, angularVelocity=omega0, gravity=g,
110 graphicsDataList=[graphicsAB])
111
112################ Body1: CONROD
113graphicsBC = graphics.RigidLink(p0=[-0.5*lBC,0,0],p1=[0.5*lBC,0,0], axis1=[0,0,0],
114 radius=[0.01,0.01], thickness = 0.01,
115 width = [0.02,0.02], color=graphics.color.lightred)
116pBC = ScalarMult(0.5,VAdd(pB,pC))
117[n1,b1]=AddRigidBody(mainSys = mbs, inertia=inertiaBC, nodeType=str(nodeType),
118 position=pBC, velocity=v1Init, angularVelocity=omega1Init,
119 rotationMatrix=rotMatBC, gravity=g, graphicsDataList=[graphicsBC])
120
121################ Body2: SLIDER
122d = 0.03
123graphicsSlider = graphics.BrickXYZ(-d/2,-d/2,-d/2, d/2,d/2, d/2, [0.5,0.5,0.5,0.5])
124[n2,b2]=AddRigidBody(mainSys = mbs, inertia=inertiaSlider, nodeType=str(nodeType),
125 position=pC, velocity=v2Init, angularVelocity=[0,0,0],
126 graphicsDataList=[graphicsSlider])
127
128
129oGround = mbs.AddObject(ObjectGround())
130markerGroundA = mbs.AddMarker(MarkerBodyRigid(name='markerGroundA', bodyNumber=oGround, localPosition=pA))
131markerGroundD = mbs.AddMarker(MarkerBodyRigid(name='markerGroundD', bodyNumber=oGround, localPosition=[0,0,0]))
132
133markerCrankA = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0))
134markerCrankB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,lAB]))
135
136markerConrodB = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[-0.5*lBC,0,0]))
137markerConrodC = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[ 0.5*lBC,0,0]))
138
139markerSlider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b2))
140
141mbs.AddObject(GenericJoint(markerNumbers=[markerGroundA, markerCrankA], constrainedAxes=[1,1,1,0,1,1],
142 visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
143
144mbs.AddObject(GenericJoint(markerNumbers=[markerGroundD, markerSlider], constrainedAxes=[0,1,1,1,1,1],
145 visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
146
147mbs.AddObject(GenericJoint(markerNumbers=[markerCrankB, markerConrodB], constrainedAxes=[1,1,1,0,0,0],
148 visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
149
150#classical cardan, x=locked
151#mbs.AddObject(GenericJoint(markerNumbers=[markerConrodC, markerSlider], constrainedAxes=[1,1,1,1,0,0],
152# visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
153
154mbs.AddObject(GenericJoint(markerNumbers=[markerSlider, markerConrodC], constrainedAxes=[1,1,1,0,0,1], # xAxisMarker0=free, yAxisMarker1=free
155 visualization=VObjectJointGeneric(axesRadius=0.005, axesLength=0.02)))
156
157if useGraphics:
158 sCrankAngle=mbs.AddSensor(SensorNode(nodeNumber = n0, storeInternal=True,#fileName='solution/crankAngle.txt',
159 outputVariableType=exu.OutputVariableType.Rotation))
160 sCrankAngVel=mbs.AddSensor(SensorNode(nodeNumber = n0, storeInternal=True,#fileName='solution/crankAngularVelocity.txt',
161 outputVariableType=exu.OutputVariableType.AngularVelocity))
162 sSliderPos=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,#fileName='solution/sliderPosition.txt',
163 outputVariableType=exu.OutputVariableType.Position))
164 sSliderVel=mbs.AddSensor(SensorNode(nodeNumber = n2, storeInternal=True,#fileName='solution/sliderVelocity.txt',
165 outputVariableType=exu.OutputVariableType.Velocity))
166
167if fixedVelocity:
168 groundNode = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #add a coordinate fixed to ground
169 markerGroundCoordinate = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=groundNode, coordinate=0))
170 markerRotX = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=n0, coordinate=3)) #Euler angle x
171
172 mbs.AddObject(CoordinateConstraint(markerNumbers=[markerGroundCoordinate, markerRotX],
173 offset = 6, velocityLevel=True))
174
175#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
176mbs.Assemble()
177#mbs.systemData.Info()
178
179simulationSettings = exu.SimulationSettings() #takes currently set values or default values
180
181fact = 1000 #1000 for testing
182outputFact = 1000
183simulationSettings.timeIntegration.numberOfSteps = 1*fact
184simulationSettings.timeIntegration.endTime = 0.2 #0.2 for testing
185simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/outputFact
186simulationSettings.solutionSettings.sensorsWritePeriod = simulationSettings.timeIntegration.endTime/outputFact
187simulationSettings.solutionSettings.writeSolutionToFile = useGraphics
188simulationSettings.timeIntegration.verboseMode = 1
189
190simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
191simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
192simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 #0.6 works well
193
194simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
195
196SC.visualizationSettings.connectors.showJointAxes = True
197SC.visualizationSettings.connectors.jointAxesLength = 0.02
198SC.visualizationSettings.connectors.jointAxesRadius = 0.002
199
200if useGraphics:
201# simulationSettings.timeIntegration.numberOfSteps = 4*5000
202# simulationSettings.timeIntegration.endTime = 5 #0.2 for testing
203
204 exu.StartRenderer()
205 mbs.WaitForUserToContinue()
206
207mbs.SolveDynamic(simulationSettings)
208
209
210#compute initial velocities:
211#if fixedVelocity:
212# v0 = mbs.GetNodeOutput(n0,exu.OutputVariableType.Coordinates_t)
213# exu.Print('v0=',v0)
214#
215# v1 = mbs.GetNodeOutput(n1,exu.OutputVariableType.Coordinates_t)
216# exu.Print('v1=',v1[0:3])
217# omega1 = mbs.GetNodeOutput(n1,exu.OutputVariableType.AngularVelocity)
218# exu.Print('omega1=',omega1[0],omega1[1],omega1[2])
219#
220# v2 = mbs.GetNodeOutput(n2,exu.OutputVariableType.Coordinates_t)
221# exu.Print('v2=',v2[0:3])
222
223
224#+++++++++++++++++++++++++++++++++++++++++++++
225#compute TestModel error for EulerParameters and index2 solver
226sol = mbs.systemData.GetODE2Coordinates();
227solref = mbs.systemData.GetODE2Coordinates(configuration=exu.ConfigurationType.Reference);
228#exu.Print('sol=',sol)
229u = 0
230for i in range(14): #take coordinates of first two bodies
231 u += abs(sol[i]+solref[i])
232
233exu.Print('solution of 3D slidercrank iftomm benchmark=',u)
234
235exudynTestGlobals.testError = u - (3.36427617809219) #2020-04-22(corrected GenericJoint): 3.36427617809219;2020-02-19: 3.3642838177004832
236exudynTestGlobals.testResult = u
237
238
239if useGraphics:
240 #SC.WaitForRenderEngineStopFlag()
241 exu.StopRenderer() #safely close rendering window!
242
243if useGraphics:
244 import matplotlib.pyplot as plt
245 import matplotlib.ticker as ticker
246 plt.close("all")
247
248 [fig1, ax1] = plt.subplots()
249 [fig2, ax2] = plt.subplots()
250 # data1 = np.loadtxt('solution/crankAngularVelocity.txt', comments='#', delimiter=',')
251 data1 = mbs.GetSensorStoredData(sCrankAngVel)
252 ax1.plot(data1[:,0], data1[:,1], 'r-', label='crank angular velocity')
253 # data1 = np.loadtxt('solution/crankAngle.txt', comments='#', delimiter=',')
254 data1 = mbs.GetSensorStoredData(sCrankAngle)
255 ax1.plot(data1[:,0], data1[:,1], 'b-', label='crank angle')
256 if False: #only if available ...
257 data1 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masarati.txt', comments='#', delimiter=',')
258 ax1.plot(data1[:,0], data1[:,2], 'r:', label='Ref Masarati: crank angle')
259 data1 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masoudi.txt', comments='#', delimiter='\t')
260 ax1.plot(data1[:,0], data1[:,2], 'k:', label='Ref Masoudi: crank angle')
261 data1 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Chaojie.txt', comments='#', delimiter=',')
262 ax1.plot(data1[:,0], data1[:,2], 'g:', label='Ref Chaojie: crank angle')
263
264
265 # data2 = np.loadtxt('solution/sliderPosition.txt', comments='#', delimiter=',')
266 data2 = mbs.GetSensorStoredData(sSliderPos)
267 ax2.plot(data2[:,0], data2[:,1], 'b-', label='slider position')
268 #data2 = np.loadtxt('solution/sliderPosition_1e-4.txt', comments='#', delimiter=',')
269 #ax2.plot(data2[:,0], data2[:,1], 'r-', label='slider position, dt=1e-4')
270# data2 = np.loadtxt('solution/sliderVelocity.txt', comments='#', delimiter=',')
271# ax2.plot(data2[:,0], data2[:,1], 'r-', label='slider velocity')
272
273 if False: #only if available ...
274 data2 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masarati.txt', comments='#', delimiter=',')
275 ax2.plot(data2[:,0], data2[:,1], 'r:', label='Ref Masarati: slider position')
276 data2 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Masoudi.txt', comments='#', delimiter='\t')
277 ax2.plot(data2[:,0], data2[:,1], 'k:', label='Ref Masoudi: slider position')
278 data2 = np.loadtxt('../../../docs/verification/Slidercrank3DiftommBenchmark/Spatial_rigid_slider-crank_mechanism_Chaojie.txt', comments='#', delimiter=',')
279 ax2.plot(data2[:,0], data2[:,1], 'g:', label='Ref Chaojie: slider position')
280
281
282 axList=[ax1,ax2]
283 figList=[fig1, fig2]
284
285 for ax in axList:
286 ax.grid(True, 'major', 'both')
287 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
288 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
289 ax.set_xlabel("time (s)")
290 ax.legend()
291
292 ax1.set_ylabel("crank angle / angular velocity")
293 ax2.set_ylabel("slider position (m)")
294
295 for f in figList:
296 f.tight_layout()
297 f.show() #bring to front