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