sliderCrank3DwithANCFbeltDrive2.py
You can view and download this file on Github: sliderCrank3DwithANCFbeltDrive2.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Create slider-crank mechanism with belt drive modeled with ANCF cable elements
5#
6# Authors: Martin Knapp and Lukas March; edited by Johannes Gerstmayr, in particular adapted to functionality of beams module and fixed initialization problems (2024-10-15)
7# Date: Created on Thu May 19 12:22:52 2020
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# Notes: PROJECT Exercise: Drive System + Crank System; VU Industrielle Mechatronik 2 - Robotics and Simulation
12# The performance could be significantly improved, if GeneralContact would be used instead of ObjectContactFrictionCircleCable2D
13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
15#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16#
17#
18#AUTHORS: Martin Knapp & Lukas March
19#
20#
21#Copyright: This file is part of Exudyn. Exudyn is free software.
22#You can redistribute it and/or modify it under the terms of the Exudyn license.
23#See 'LICENSE.txt' for more details.
24#"""
25
26#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28#Tested with EXUDYN Version 0.1.342.
29
30import sys
31import os
32
33
34#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
35#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
36import sys
37import os
38import numpy as np
39import matplotlib.pyplot as plt
40import matplotlib.ticker as ticker
41
42
43import exudyn as exu
44#a local run must not open a plot window when windows are suppressed (#2477)
45if exu.special.userInterface.suppressPlots: import matplotlib; matplotlib.use('Agg')
46import numpy as np
47from exudyn.itemInterface import (MarkerNodeRotationCoordinate, ObjectConnectorCartesianSpringDamper,
48 LoadTorqueVector, VObjectJointPrismatic2D, ObjectJointPrismatic2D, Torque,
49 MassPoint2D, RigidBody2D, NodePoint2D, RevoluteJoint2D, CoordinateConstraint,
50 ObjectGround, ObjectContactFrictionCircleCable2D, NodeGenericData,
51 MarkerBodyCable2DShape, NodePoint2DSlope1, ObjectANCFCable2D, VObjectANCFCable2D, MarkerBodyPosition,
52 VObjectJointRevolute2D, VObjectRigidBody2D, NodePointGround, MarkerNodePosition,
53 MarkerNodeCoordinate, Force, SensorBody, NodeRigidBody2D, ObjectRigidBody2D,
54 MarkerBodyRigid, ObjectJointRevolute2D, SensorLoad)
55from exudyn.utilities import (AddRigidBody, RigidBodyInertia, ObjectConnectorCoordinate, InertiaCuboid,
56 OutputFilePath)
57import exudyn.graphics as graphics
58from exudyn.beams import *
59
60#import visHelper
61#visHelper.init()
62
63#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
64
65PLOTS_PATH = OutputFilePath("solution/plots/")
66fontSize = 20
67
68#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
69# Change plot axis
70def set_axis(plt, equal = False):
71 ax=plt.gca() #get current axes
72 ax.grid(True , 'major', 'both')
73 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
74 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
75 if equal:
76 ax.set_aspect('equal')
77 plt.legend() #show labels as legend
78
79def plotOmegaDisk0():
80 ang_vel = np.loadtxt(OutputFilePath("solution/angular_velocity_disk0.txt"), comments='#', delimiter=',')
81
82 fig = plt.figure(figsize=[13,5])
83 plt.plot(ang_vel[:,0], ang_vel[:,3], 'r-', label='$\\omega_{disk0}$')
84 set_axis(plt)
85 ax=plt.gca()
86 ax.set_xlabel('$t [s]$', fontsize=fontSize)
87 ax.set_ylabel('$\\omega_{disk0} [rad/s]$', fontsize=fontSize)
88 ax.grid(True, 'major', 'both')
89 plt.legend()
90 plt.tight_layout()
91 plt.show()
92 fig.savefig(PLOTS_PATH + 'angular_velocity_disk0.pdf', format='pdf')
93
94def plotOmegaDisk1():
95 ang_vel = np.loadtxt(OutputFilePath("solution/angular_velocity_disk1.txt"), comments='#', delimiter=',')
96
97 fig = plt.figure(figsize=[13,5])
98 plt.plot(ang_vel[:,0], ang_vel[:,3], 'r-', label='$\\omega_{disk1}$')
99 set_axis(plt)
100 ax=plt.gca()
101 ax.set_xlabel('$t [s]$', fontsize=fontSize)
102 ax.set_ylabel('$\\omega_{disk1} [rad/s]$', fontsize=fontSize)
103 ax.grid(True, 'major', 'both')
104 plt.legend()
105 plt.tight_layout()
106 plt.show()
107 fig.savefig(PLOTS_PATH + 'angular_velocity_disk1.pdf', format='pdf')
108
109def plotTorque():
110 ang_vel = np.loadtxt(OutputFilePath("solution/torque.txt"), comments='#', delimiter=',')
111
112 fig = plt.figure(figsize=[13,5])
113 plt.plot(ang_vel[:,0], ang_vel[:,3], 'r-', label='$\\tau$')
114 set_axis(plt)
115 ax=plt.gca()
116 ax.set_xlabel('$t [s]$', fontsize=fontSize)
117 ax.set_ylabel('$\\tau [Nm]$', fontsize=fontSize)
118 ax.grid(True, 'major', 'both')
119 plt.legend()
120 plt.tight_layout()
121 plt.show()
122 fig.savefig(PLOTS_PATH + 'torque.pdf', format='pdf')
123
124def plotCrankPos():
125 ang_vel = np.loadtxt(OutputFilePath("solution/crank_pos.txt"), comments='#', delimiter=',')
126
127 fig = plt.figure(figsize=[13,5])
128 plt.plot(ang_vel[:,0], ang_vel[:,1], 'r-', label='$x_{Pos}$')
129 plt.plot(ang_vel[:,0], ang_vel[:,2], 'b-', label='$y_{Pos}$')
130 set_axis(plt)
131 ax=plt.gca()
132 ax.set_xlabel('$t [s]$', fontsize=fontSize)
133 ax.set_ylabel('$x,y [m]$', fontsize=fontSize)
134 ax.grid(True, 'major', 'both')
135 plt.legend()
136 plt.tight_layout()
137 plt.show()
138 fig.savefig(PLOTS_PATH + 'crank_pos.pdf', format='pdf')
139
140
141def vishelperInit():
142 plt.close('all')
143 os.makedirs(PLOTS_PATH, exist_ok=True) #solution/ may not exist yet (#2475)
144
145def visHelperPlot_all():
146 plotOmegaDisk0()
147 plotOmegaDisk1()
148 plotTorque()
149 plotCrankPos()
150
151#if __name__ is "__main__":
152# init()
153# plot_all()
154
155
156vishelperInit()
157
158
159
160print("Exudyn used:", exu.config.Version())
161
162#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
163#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
164#SYSTEM SETTINGS
165
166# 0 - belt-drive-system
167# 1 - crank system 2D
168# 2 - crank system 3D
169# 3 - belt-drive-system + crank system 2D
170# 4 - belt-drive-system + crank system 3D
171
172sys_set = 4
173
174useGraphics = True
175export_images = useGraphics
176PLOTS_PATH = OutputFilePath("solution/plots/")
177
178if export_images:
179 os.makedirs(PLOTS_PATH, exist_ok=True) #solution/ may not exist yet (#2475)
180
181# belt-drive-system
182enable_force = True # Enable Preload Force in the belt
183enable_disk_friction = True # When True the disks will have contact to the belt else the belt is free floating
184enable_controller = True # If True the disk0 will get a torque regulated by a P-controller else a fixed torque
185nElements = 1*50 # Belt element count
186
187#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
188#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
189
190#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
191#geometry parameters
192L_0 = 0.5 #distance between the disks [m]
193L_A = 0.15 #length of crank [m]
194h_A = 0.025 #cross-section height and width of crank [m]
195L_B = 0.3 #length of connecting rod [m]
196h_B = 0.025 #cross-section height and width of connecting rod [m]
197r_0 = 0.05 #radius of disk0 [m]
198r_1 = 0.1 #radius of disk1 [m]
199b_a0 = 0.1 #section-length of crank [m]
200b_a1 = 0.05 #section-length of crank [m]
201b_a2 = 0.05 #section-length of crank [m]
202h_S = 0.1 #cross-section height and width of slider [m]
203#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
204#belt parameters
205d_belt = 0.01 #cross-section height and width [m]
206csa_belt = d_belt**2 #cross-section area [m^2]
207E_belt = 2e9 #E modulus [N/m^2]
208rho_belt = 1e3 #density [kg/m^3]
209F_p = 1e4 #preload force[N]
210
211contactStiffness=1e5*10 #contactStiffness between the belt and the disks ->
212 #holds the belt around disks
213contactDamping=200 #damping between the belt and the disks
214mu = 0.9 #friction coefficent between the belt and the disks
215velPenalty = 5e2 #frictionVelocityPenalty between tangential velocities
216 #of the belt against the disks
217controller_set_vel = 100 # Desired angular velocity in rad/s of the disk0: 100 rad/s = ~ 955 U/min
218controller_p = 30.0 # P-Factor of controller
219
220MassPerLength_belt = rho_belt * csa_belt
221bendStiffness_belt = E_belt * (d_belt**4 / 12)
222#Hook: F = E * A * epsilon
223axialStiffness_belt = E_belt * csa_belt
224
225#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
226#mass parameters
227m_disk0 = 0.5 #mass of disk0 [kg]
228m_disk1 = 1 #mass of disk1 [kg]
229m_crank = 0.5 #mass of crank [kg]
230density_conrod = 1000 #Density of the conrod [kg/m^3]
231m_slider = 0.2 #mass of slider [kg]
232
233#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
234#inertia parameters
235#inertia disks
236J_disk0 = 0.5 * m_disk0 * r_0**2 #inertia disk0 [Nm^2]
237J_disk1 = 0.5 * m_disk1 * r_1**2 #inertia disk1 [Nm^2]
238
239#inertia crank
240J_xx_crank = 5e-3 #inertia xx [Nm^2]
241J_yy_crank = 7e-3 #inertia yy [Nm^2]
242J_zz_crank = 2.5e-3 #inertia zz [Nm^2]
243inertia_crank = RigidBodyInertia(mass=m_crank,
244 inertiaTensor=np.diag([J_xx_crank,
245 J_yy_crank,
246 J_zz_crank]))
247#inertia conrod
248inertia_conrod = InertiaCuboid(density=density_conrod,
249 sideLengths = [L_B,h_B,h_B])
250
251#inertia slider
252#Dummy inertia because MassPoint will result in System Jacobian not invertible!
253inertia_slider = InertiaCuboid(density=(m_slider/h_S**3),
254 sideLengths = [h_S]*3)
255
256#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
257#bearing parameters
258kBearing = 50e3 #stiffness of bearing [N/m]
259dBearing = 1e3 #damping of bearing [N/ms]
260#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
261#load parameters
262M = 0.1 #torque [Nm]
263g = [0,-9.81,0] #acceleration of earth [m/s^2]
264load_crank = [0,-9.81*m_crank,0] #gravity [N]
265load_conrod = [0,-9.81*inertia_conrod.mass,0] #gravity [N]
266load_slider = [0,-9.81*m_slider,0] #gravity [N]
267
268#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
269#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
270
271SC = exu.SystemContainer()
272mbs = SC.AddSystem()
273
274#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
275#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
276#BELT DRIVE SYSTEM
277if sys_set == 0 or sys_set > 2:
278
279 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
280 #grounds
281
282 #ground-node and ground-marker in center of disk0
283 nG_disk0 = mbs.AddNode(NodePointGround(referenceCoordinates = [L_0,0,0]))
284 mG_disk0 = mbs.AddMarker(MarkerNodePosition(nodeNumber = nG_disk0))
285
286 #ground-node and ground-marker in center of disk1
287 nG_disk1 = mbs.AddNode(NodePointGround(referenceCoordinates = [0,0,0]))
288 mG_disk1 = mbs.AddMarker(MarkerNodePosition(nodeNumber = nG_disk1))
289
290 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
291 #disk0
292
293 #initial coordinates disk0
294 u0_disk0 = [L_0,0,0]
295 #NodeRigidBody2D on center of gravity disk0
296 nRB2D_disk0 = mbs.AddNode(NodeRigidBody2D(referenceCoordinates = [0,0,0],
297 initialCoordinates = u0_disk0,
298 initialVelocities = [0,0,0]))
299 #visualization of disk0
300 bodyVis_disk0 = VObjectRigidBody2D(graphicsData=[{'type':'Circle',
301 'color':[0,0,0,1],
302 'radius':r_0,
303 'position':[0,0,0],
304 'normal':[0,0,1]},
305 {'type':'Line',
306 'color':[1,0,0,1],
307 'data':[0,0,0,r_0,0,0]}])
308 #ObjectRigidBody2D disk0
309 oRB2D_disk0 = mbs.AddObject(ObjectRigidBody2D(nodeNumber = nRB2D_disk0,
310 mass=m_disk0,
311 inertia=J_disk0,
312 visualization = bodyVis_disk0))
313 #MarkerBodyRigid on center of disk0
314 mNP_disk0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB2D_disk0,
315 localPosition=[0,0,0]))
316
317 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
318 #disk1
319
320 #initial coordinates disk1
321 u0_disk1 = [0,0,0]
322 #NodeRigidBody2D on center of disk1
323 nRB2D_disk1 = mbs.AddNode(NodeRigidBody2D(referenceCoordinates = [0,0,0],
324 initialCoordinates = u0_disk1,
325 initialVelocities = [0,0,0]))
326 #visualization of disk1
327 bodyVis_disk1 = VObjectRigidBody2D(graphicsData=[{'type':'Circle',
328 'color':[0,0,0,1],
329 'radius':r_1,
330 'position':[0,0,0],
331 'normal':[0,0,1]},
332 {'type':'Line',
333 'color':[1,0,0,1],
334 'data':[0,0,0,r_1,0,0]}])
335 #ObjectRigidBody2D disk1
336 oRB2D_disk1 = mbs.AddObject(ObjectRigidBody2D(nodeNumber = nRB2D_disk1,
337 mass=m_disk1,
338 inertia=J_disk1,
339 visualization = bodyVis_disk1))
340 #MarkerBodyRigid on center of disk1
341 mNP_disk1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB2D_disk1,
342 localPosition=[0,0,0]))
343
344 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
345 #joints
346
347 #visualization of joint0 in the center of disk0
348 jointVis_disk0 = VObjectJointRevolute2D(show=True, drawSize=0.02,
349 color=[0,0,0,1])
350 #ObjectJointRevolute2D between ground and disk0
351 oJR2D_disk0 = mbs.AddObject(ObjectJointRevolute2D(markerNumbers = [mG_disk0,mNP_disk0],
352 visualization = jointVis_disk0))
353 #visualization of joint1 in the center of disk1
354 jointVis_disk1 = VObjectJointRevolute2D(show=True, drawSize=0.02, color=[0,0,0,1])
355 #ObjectJointRevolute2D between ground and disk1
356 oJR2D_disk1 = mbs.AddObject(ObjectJointRevolute2D(markerNumbers = [mG_disk1,mNP_disk1],
357 visualization = jointVis_disk1))
358
359 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 #function to get length of the belt
361 def belt_get_lengths(L_0, r_l, r_r):
362 alpha = np.arcsin((r_l-r_r)/L_0) #angle of the arc length
363 b_belt = L_0*np.cos(alpha) #branch between the disks
364 al_dr_belt = r_r*(np.pi-2*alpha) #arc length disk0
365 al_dl_belt = r_l*(np.pi+2*alpha) #arc length disk1
366 len_belt = 2*b_belt+al_dr_belt+al_dl_belt #belt length
367 return [alpha, b_belt, al_dl_belt, al_dr_belt, len_belt]
368
369
370 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
371 #belt cable elements
372 [alpha, b_belt, al_dl_belt, al_dr_belt, len_belt] = belt_get_lengths(L_0, r_1, r_0)
373 #calculate belt length with preload force:
374 epsilon_belt = -F_p / axialStiffness_belt
375 len_belt_p = len_belt * (1.0 + epsilon_belt)
376
377 print("Belt length: ", len_belt, " and after preload force: ", len_belt_p)
378
379
380 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
381 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
382 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
383 #new functionality with beams module:
384 circleList = [[[L_0,0],r_0,'R'],
385 [[0,0],r_1,'R'],
386 [[L_0,0],r_0,'R'],
387 [[0,0],r_1,'R'],
388 ]
389
390 reevingDict = CreateReevingCurve(circleList, drawingLinesPerCircle = 64,
391 removeLastLine=True,
392 numberOfANCFnodes=nElements, graphicsNodeSize= 0.01)
393
394 del circleList[-1] #remove circles not needed for contact/visualization
395 del circleList[-1] #remove circles not needed for contact/visualization
396
397 gList=[]
398 if False: #visualize reeving curve, without simulation
399 gList = reevingDict['graphicsDataLines'] + reevingDict['graphicsDataCircles']
400
401 #%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
402 #create ANCF elements:
403
404 # dimZ = b #z.dimension
405 hDraw = 0.002
406 cableTemplate = ObjectANCFCable2D(
407 bendingStiffness=bendStiffness_belt*0.1,
408 massPerLength=MassPerLength_belt,
409 axialStiffness=axialStiffness_belt,
410 useReducedOrderIntegration=2,
411 referenceAxialStrain=epsilon_belt,
412 bendingDamping = 0.005*bendStiffness_belt,
413 axialDamping = 0.02*axialStiffness_belt,
414 visualization=VObjectANCFCable2D(drawHeight=hDraw),
415 )
416
417 ancf = PointsAndSlopes2ANCFCable2D(mbs, reevingDict['ancfPointsSlopes'], reevingDict['elementLengths'],
418 cableTemplate,
419 #massProportionalLoad=gVec,
420 firstNodeIsLastNode=True, graphicsSizeConstraints=0.01)
421
422 nNP2DS = ancf[0]
423 oANCFC2D_b = ancf[1]
424
425
426 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
427 #contact friction belt
428
429 if enable_disk_friction:
430
431 nCableGenData_disk0 = []
432 contact_disk0 = []
433 nCableGenData_disk1 = []
434 contact_disk1 = []
435 mCableShape = []
436
437 for i in range(0,len(oANCFC2D_b)):
438 nCS = 8
439 initGap = 0.001 #positive gap gives fast initial settling of contact
440 #NodeGenericData disk0
441 nCableGenData_disk0.append(mbs.AddNode(NodeGenericData(initialCoordinates = [initGap]*nCS+[-2]*nCS+[0]*nCS,
442 numberOfDataCoordinates=3*nCS)))
443 #NodeGenericData disk1
444 nCableGenData_disk1.append(mbs.AddNode(NodeGenericData(initialCoordinates = [initGap]*nCS+[-2]*nCS+[0]*nCS,
445 numberOfDataCoordinates=3*nCS)))
446 #MarkerBodyCable2DShape on cable
447 mCableShape.append(mbs.AddMarker(MarkerBodyCable2DShape(bodyNumber=oANCFC2D_b[i],
448 numberOfSegments=nCS)))
449 #contact friction to disk0
450 contact_disk0.append(mbs.AddObject(ObjectContactFrictionCircleCable2D(markerNumbers=[mNP_disk0,mCableShape[-1]],
451 nodeNumber=nCableGenData_disk0[-1],
452 circleRadius=r_0,
453 contactStiffness=contactStiffness,
454 contactDamping=contactDamping,
455 numberOfContactSegments=nCS,
456 frictionCoefficient=mu,
457 frictionVelocityPenalty=velPenalty)))
458 #contact friction to disk1
459 contact_disk1.append(mbs.AddObject(ObjectContactFrictionCircleCable2D(markerNumbers=[mNP_disk1, mCableShape[-1]],
460 nodeNumber=nCableGenData_disk1[-1],
461 circleRadius=r_1,
462 contactStiffness=contactStiffness,
463 contactDamping=contactDamping,
464 numberOfContactSegments=nCS,
465 frictionCoefficient=mu,
466 frictionVelocityPenalty=velPenalty)))
467
468 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
469 # Velocity controller
470
471 s_disk0 = mbs.AddSensor(SensorBody(bodyNumber=oRB2D_disk0, writeToFile=True,
472 fileName="solution/angular_velocity_disk0.txt",
473 outputVariableType=exu.OutputVariableType.AngularVelocity))
474
475 def p_controller(mbs, t, loadVector):
476 vel = mbs.GetSensorValues(s_disk0)[2]
477 torque = controller_p * (controller_set_vel - vel)
478 return [0,0,torque]
479
480 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
481 #torque on disk0
482 if enable_controller:
483 l_Torquedisk0 = mbs.AddLoad(LoadTorqueVector(markerNumber=mNP_disk0,
484 loadVectorUserFunction=p_controller))
485
486 else:
487 l_Torquedisk0 = mbs.AddLoad(LoadTorqueVector(markerNumber=mNP_disk0,loadVector=[0,0,M]))
488
489 s_disk1 = mbs.AddSensor(SensorBody(bodyNumber=oRB2D_disk1, writeToFile=True,
490 fileName="solution/angular_velocity_disk1.txt",
491 outputVariableType=exu.OutputVariableType.AngularVelocity))
492 s_load = mbs.AddSensor(SensorLoad(loadNumber=l_Torquedisk0, writeToFile=True,
493 fileName="solution/torque.txt"))
494
495
496#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
497#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
498#CRANK DRIVE SYSTEM 2D
499if sys_set == 1 or sys_set == 3:
500
501 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
502 #ground
503 nPG = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
504 oG = mbs.AddObject(ObjectGround(referencePosition=[0,0,0]))
505 mBPG = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oG, localPosition=[0,0,0]))
506 mNCG = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0))
507
508 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
509 #crank
510
511 #visualization of crank
512 graphics_crank = graphics.RigidLink(p0=[-0.5*L_A,0,-h_A/2],
513 p1=[0.5*L_A ,0,-h_A/2],
514 axis0=[0,0,1], axis1=[0,0,1],
515 radius=[0.5*h_A,0.5*h_A],
516 thickness=h_A, width=[h_A,h_A],
517 color=[0.8,0.8,0.8,1.],nTiles=16)
518 #node on center of gravity of crank
519 nRB2D_crank = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[-L_A*0.5,0,0],
520 initialVelocities=[0,0,0]))
521 #RigidBody2D crank
522 oRB2D_crank = mbs.AddObject(RigidBody2D(mass=m_crank,
523 inertia=J_zz_crank,
524 nodeNumber=nRB2D_crank,
525 visualization=VObjectRigidBody2D(graphicsData=[graphics_crank])))
526
527 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
528 #connecting rod
529
530 #visualization of connecting rod
531 graphics_conrod = graphics.RigidLink(p0=[-0.5*L_B,0,h_B/2],
532 p1=[0.5*L_B,0,h_B/2],
533 axis0=[0,0,1], axis1=[0,0,1],
534 radius=[0.5*h_B,0.5*h_B],
535 thickness=h_B, width=[h_B,h_B],
536 color=[0.7,0.7,0.7,1],nTiles=36)
537 #node on center of gravity of connecting rod
538 nRB2D_conrod = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[-(L_A+L_B*0.5),0,0],
539 initialVelocities=[0,0,0]))
540 #RigidBody2D connecting rod
541 oRB2D_conrod = mbs.AddObject(RigidBody2D(mass=inertia_conrod.mass,
542 inertia=inertia_conrod.inertiaTensor[1,1],
543 nodeNumber=nRB2D_conrod,
544 visualization=VObjectRigidBody2D(graphicsData= [graphics_conrod])))
545
546 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
547 #slider
548
549 #visualization of slider
550 c=0.025 #dimension of slider
551 graphics_slider = graphics.Brick(centerPoint=[0, 0, -c], size=[2*c, 2*c, 2*c], color=[0.2,0.2,0.2,0.9])
552 #node on center of gravity of slider
553 nP2D_slider = mbs.AddNode(NodePoint2D(referenceCoordinates=[-(L_A+L_B),0]))
554 #MassPoint2D slider
555 oMP2D_slider = mbs.AddObject(MassPoint2D(mass=m_slider,
556 nodeNumber=nP2D_slider,
557 visualization=VObjectRigidBody2D(graphicsData= [graphics_slider])))
558
559 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
560 #markers for joints
561 mBPLeft_crank = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_crank,
562 localPosition=[-L_A*0.5,0.,0.]))
563 mBPRight_crank = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_crank,
564 localPosition=[L_A*0.5,0.,0.]))
565 mBPLeft_conrod = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_conrod,
566 localPosition=[-L_B*0.5,0.,0.]))
567 mBPRight_conrod = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_conrod,
568 localPosition=[L_B*0.5,0.,0.]))
569 mBP_slider = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oMP2D_slider,
570 localPosition=[ 0.,0.,0.]))
571
572 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
573 #joints
574 oRJ2D_ground_crank = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBPG,mBPRight_crank]))
575 oRJ2D_crank_conrod = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBPLeft_crank,mBPRight_conrod]))
576 oRJ2D_slider = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBP_slider,mBPLeft_conrod]))
577
578 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
579 #markers for node constraints
580 mNC_Y_slider = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nP2D_slider, coordinate=1)) #y-coordinate is constrained
581
582 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
583 #coordinate constraints for slider (free motion in x-direction)
584 oCC_ground_slider = mbs.AddObject(CoordinateConstraint(markerNumbers=[mNCG, mNC_Y_slider]))
585
586 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
587 #markers for load
588 mBR_crank_torque = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB2D_crank, localPosition=[L_A/2,0.,0.]))
589 mBR_crank_gravity = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB2D_crank, localPosition=[0.,0.,0.]))
590 mBR_conrod_gravity = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB2D_conrod, localPosition=[0.,0.,0.]))
591
592 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
593 #loads and driving forces
594
595 #gravity of crank
596 lC_crank_gravity = mbs.AddLoad(Force(markerNumber = mBR_crank_gravity,
597 loadVector = load_crank))
598 #gravity of conrod
599 lC_conrod_gravity = mbs.AddLoad(Force(markerNumber = mBR_conrod_gravity,
600 loadVector = load_conrod))
601 #gravity of slider
602 lC_slider_gravity = mbs.AddLoad(Force(markerNumber = mBP_slider,
603 loadVector = load_slider))
604
605 if sys_set == 1:
606 #torque at crank
607 lC_crank_torque = mbs.AddLoad(Torque(markerNumber = mBR_crank_torque,
608 loadVector = [0, 0, M]))
609
610 if sys_set == 3:
611 #ConnectorCoordinate - crank gets torque of disk1
612 mNC_disk1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRB2D_disk1,coordinate=2))
613 mNC_crank = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRB2D_crank,coordinate=2))
614 oCC_disk1_crank = mbs.AddObject(ObjectConnectorCoordinate(markerNumbers=[mNC_disk1,mNC_crank],
615 velocityLevel=True))
616
617#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
618#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
619#CRANK DRIVE SYSTEM 3D
620if sys_set == 2 or sys_set == 4:
621
622 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
623 #ground
624 oG_Left = mbs.AddObject(ObjectGround())
625 oG_Right = mbs.AddObject(ObjectGround())
626 oG_slider = mbs.AddObject(ObjectGround())
627
628 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
629 #crank
630 graphics_crank_1 = graphics.RigidLink(p0=[0,0,0],p1=[0,0,-b_a0], axis1=[0,0,1], radius=[h_A/2,h_A/1.3],
631 thickness = h_A, width=[0,h_A/2], color=[0.8,0.8,0.8,1])
632 graphics_crank_2 = graphics.RigidLink(p0=[0,0,0],p1=[-L_A,0,0], radius=[h_A/2,h_A/2],
633 thickness = h_A, color=[0.8,0.8,0.8,1])
634 graphics_crank_3 = graphics.RigidLink(p0=[-L_A,0,0],p1=[-L_A,0,b_a1], radius=[h_A/2,h_A/2],
635 thickness = h_A, color=[0.8,0.8,0.8,1])
636 graphics_crank_4 = graphics.RigidLink(p0=[-L_A,0,b_a1],p1=[0,0,b_a1], radius=[h_A/2,h_A/2],
637 thickness = h_A, color=[0.8,0.8,0.8,1])
638 graphics_crank_5 = graphics.RigidLink(p0=[0,0,b_a1],p1=[0,0,b_a1+b_a2],axis1=[0,0,1], radius=[h_A/2,h_A/1.3],
639 thickness = h_A, width=[0,h_A/2], color=[0.8,0.8,0.8,1])
640
641 dictCrank = mbs.CreateRigidBody(
642 inertia=inertia_crank,
643 referencePosition=[0, 0, b_a0],
644 gravity=g,
645 graphicsDataList=[graphics_crank_1, graphics_crank_2, graphics_crank_3, graphics_crank_4, graphics_crank_5],
646 returnDict=True)
647 [nRG_crank, oRB_crank] = [dictCrank['nodeNumber'], dictCrank['bodyNumber']]
648
649 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
650 #connecting rod
651 graphics_conrod = graphics.RigidLink(p0=[L_B/2, 0, 0], p1=[-L_B/2, 0, 0], axis0=[0, 0, 1], axis1=[0, 0, 1],
652 radius=[h_B/1.5, h_B/2], thickness=h_B, width=[h_B, h_B], color=[0.5, 0.5, 0.5, 1])
653 dictConrod = mbs.CreateRigidBody(
654 inertia=inertia_conrod,
655 referencePosition=[-L_A - L_B/2, 0, b_a0 + b_a1/2],
656 gravity=g,
657 graphicsDataList=[graphics_conrod],
658 returnDict=True)
659 [nRG_conrod, oRB_conrod] = [dictConrod['nodeNumber'], dictConrod['bodyNumber']]
660
661 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
662 #slider
663 d = 0.07
664 graphics_slider = graphics.Brick(centerPoint=[0, 0, -0.5*(d + h_B)], size=[d, d, d], color=[0.2, 0.2, 0.2, 0.9])
665 dictSlider = mbs.CreateRigidBody(
666 inertia=inertia_slider,
667 initialAngularVelocity=[0, 0, 0],
668 referencePosition=[-(L_A + L_B), 0, b_a0 + b_a1/2],
669 gravity=g,
670 graphicsDataList=[graphics_slider],
671 returnDict=True)
672 [nRB_slider, oRB_slider] = [dictSlider['nodeNumber'], dictSlider['bodyNumber']]
673
674
675 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
676 #marker for joints
677 mG_Left = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oG_Left,
678 localPosition=[0,0,0]))
679 mG_Right = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oG_Right,
680 localPosition=[0,0,b_a0+b_a1+b_a2]))
681 mG_slider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oG_slider,
682 localPosition=[-(L_A+L_B),0,b_a0+b_a1/2]))
683 mBR_crank_Left = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
684 localPosition=[0,0,-b_a0]))
685 mBR_crank_Right = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
686 localPosition=[0,0,b_a1+b_a2]))
687 mBR_crank_conrod = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
688 localPosition=[-L_A,0,b_a1/2]))
689 mBR_conrod_crank = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_conrod,
690 localPosition=[L_B/2,0,0]))
691 mBR_conrod_slider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_conrod,
692 localPosition=[-L_B/2,0,0]))
693 mBR_slider_conrod = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_slider,
694 localPosition=[0,0,0]))
695 mBR_slider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_slider,
696 localPosition=[0,0,0]))
697
698 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++q
699 #joints
700 oGJ_crank_Left = mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mG_Left, mBR_crank_Left],
701 stiffness=[kBearing]*3, damping=[dBearing]*3))
702 oGJ_crank_Right = mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mG_Right, mBR_crank_Right],
703 stiffness=[kBearing]*3, damping=[dBearing]*3))
704 oRJ2D_crank_conrod = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBR_crank_conrod,mBR_conrod_crank],
705 visualization=VObjectJointRevolute2D(drawSize=0.05)))
706 oRJ2D_conrod_slider = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBR_conrod_slider,mBR_slider_conrod],
707 visualization=VObjectJointRevolute2D(drawSize=0.05)))
708 oJP2D_slider = mbs.AddObject(ObjectJointPrismatic2D(markerNumbers=[mG_slider,mBR_slider],
709 axisMarker0 = [1.,0.,0.],
710 normalMarker1 = [0.,1.,0.],
711 visualization=VObjectJointPrismatic2D(drawSize=0.01)))
712 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
713 #load and driving forces
714
715 if sys_set == 2:
716 #markers for load
717 mBR_crank_torque = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
718 localPosition=[0.,0.,b_a1+b_a2]))
719 #driving forces
720 lC_crank_torque = mbs.AddLoad(Torque(markerNumber = mBR_crank_torque,
721 loadVector = [0, 0, M/2]))
722
723 if sys_set == 4:
724 #ConnectorCoordinate - crank gets torque of disk1
725 #markers for Connector
726 mNC_disk1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRB2D_disk1,coordinate=2))
727 # disk1 is a NodeRigidBody2D -> coordinates = [q_0,q_1,psi] -> Rotation psi
728 mNC_crank = mbs.AddMarker(MarkerNodeRotationCoordinate(nodeNumber=nRG_crank,rotationCoordinate=2))
729 # crank is a 3D Node -> Rotation around z-axis
730 #Connector Coordinate
731 oCC_disk1_crank = mbs.AddObject(ObjectConnectorCoordinate(markerNumbers=[mNC_disk1,mNC_crank],
732 velocityLevel=True))
733
734 s_crank = mbs.AddSensor(SensorBody(bodyNumber=oRB_crank, writeToFile=True,
735 fileName="solution/crank_pos.txt", localPosition=[0,0,-b_a0],
736 outputVariableType=exu.OutputVariableType.Position))
737
738
739#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
740#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
741
742mbs.Assemble()
743print(mbs)
744
745#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
746#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
747#simulation
748
749tEnd = 1 #end time of the simulation
750stepSize = 2.5e-5
751
752sims = exu.SimulationSettings()
753sims.timeIntegration.generalizedAlpha.spectralRadius=0.8
754sims.pauseAfterEachStep = False
755sims.show.statistics = True
756sims.show.computationTime = True
757sims.timeIntegration.numberOfSteps = int(tEnd/stepSize)
758sims.timeIntegration.endTime = tEnd
759sims.timeIntegration.newton.absoluteTolerance = 1e-7
760sims.timeIntegration.newton.relativeTolerance = 1e-7
761sims.timeIntegration.newton.useModifiedNewton = True
762sims.timeIntegration.discontinuous.iterationTolerance = 0.1
763sims.timeIntegration.discontinuous.maxIterations = 3
764sims.solution.sensors.writePeriod = 0.001
765sims.solution.file.writePeriod = stepSize*4*5
766sims.parallel.numberOfThreads = 1
767sims.timeIntegration.verboseMode = 1
768
769#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
770#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
771#visualizaion
772SC.visualizationSettings.general.circleTiling=128
773dSize = 0.02
774SC.visualizationSettings.nodes.defaultSize = dSize*0.25
775SC.visualizationSettings.markers.defaultSize = dSize*0.25
776SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
777SC.visualizationSettings.connectors.defaultSize = dSize
778SC.visualizationSettings.nodes.show=True
779SC.visualizationSettings.loads.show=False
780SC.visualizationSettings.loads.drawSimplified = False
781SC.visualizationSettings.loads.defaultSize = 0.1
782SC.visualizationSettings.markers.show=True
783SC.visualizationSettings.sensors.show=True
784SC.visualizationSettings.general.autoFitScene=False
785SC.visualizationSettings.openGL.advanced.initialCenterPoint = [0,0,0]
786SC.visualizationSettings.openGL.advanced.initialZoom = 0.5
787SC.visualizationSettings.contour.outputVariable = exu.OutputVariableType.ForceLocal
788
789if sys_set == 0:
790 rot_alpha=0
791 rot_beta=0
792 rot_gamma=0
793elif sys_set > 0:
794 rot_alpha=-0.6
795 rot_beta=0.6
796 rot_gamma=0.37
797
798R_x = np.array([[ 1, 0, 0],
799 [ 0, np.cos(rot_alpha), -np.sin(rot_alpha)],
800 [ 0, np.sin(rot_alpha), np.cos(rot_alpha)]])
801R_y = np.array([[ np.cos(rot_beta), 0, np.sin(rot_beta)],
802 [ 0, 1, 0],
803 [ -np.sin(rot_beta), 0, np.cos(rot_beta)]])
804R_z = np.array([[ np.cos(rot_gamma), -np.sin(rot_gamma), 0],
805 [ np.sin(rot_gamma), np.cos(rot_gamma), 0],
806 [ 0, 0, 1]])
807IMR = np.dot(R_x,R_y)
808IMR = np.dot(IMR,R_z)
809SC.visualizationSettings.openGL.advanced.initialModelRotation = [[IMR[0,0],IMR[0,1],IMR[0,2]],
810 [IMR[1,0],IMR[1,1],IMR[1,2]],
811 [IMR[2,0],IMR[2,1],IMR[2,2]]]
812
813#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
814#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
815#Rendering
816SC.renderer.Start() #start graphics visualization
817SC.renderer.DoIdleTasks() #wait for pressing SPACE bar to continue
818# mbs.SolveStatic(sims)
819mbs.SolveDynamic(sims)
820SC.renderer.DoIdleTasks() #wait for pressing 'Q' to quit
821SC.renderer.Stop() #safely close rendering window!
822
823#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
824#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
825
826if export_images:
827 visHelperPlot_all()