You can view and download this file on Github: sliderCrank3DwithANCFbeltDrive2.py
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details: Create slider-crank mechanism with belt drive modeled with ANCF cable elements
#
# Authors: Martin Knapp and Lukas March
# Date: Created on Thu May 19 12:22:52 2020
#
# 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.
#
# Notes: PROJECT Exercise: Drive System + Crank System; VU Industrielle Mechatronik 2 - Robotics and Simulation
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#
#
#AUTHORS: Martin Knapp & Lukas March
#
#
#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.
#"""
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#Tested with EXUDYN Version 0.1.342.
import sys
import os
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
import sys
import os
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
import exudyn as exu
import numpy as np
from exudyn.itemInterface import (MarkerNodeRotationCoordinate, ObjectConnectorCartesianSpringDamper,
LoadTorqueVector, VObjectJointPrismatic2D, ObjectJointPrismatic2D, Torque,
MassPoint2D, RigidBody2D, NodePoint2D, RevoluteJoint2D, CoordinateConstraint,
ObjectGround, ObjectContactFrictionCircleCable2D, NodeGenericData,
MarkerBodyCable2DShape, NodePoint2DSlope1, ObjectANCFCable2D, MarkerBodyPosition,
VObjectJointRevolute2D, VObjectRigidBody2D, NodePointGround, MarkerNodePosition,
MarkerNodeCoordinate, Force, SensorBody, NodeRigidBody2D, ObjectRigidBody2D,
MarkerBodyRigid, ObjectJointRevolute2D, SensorLoad)
from exudyn.utilities import AddRigidBody, RigidBodyInertia, ObjectConnectorCoordinate, InertiaCuboid
import exudyn.graphics as graphics #only import if it does not conflict
#import visHelper
#visHelper.init()
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
PLOTS_PATH = "plots/"
fontSize = 20
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# Change plot axis
def set_axis(plt, equal = False):
ax=plt.gca() #get current axes
ax.grid(True , 'major', 'both')
ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
if equal:
ax.set_aspect('equal')
plt.legend() #show labels as legend
def plotOmegaDisk0():
ang_vel = np.loadtxt("angular_velocity_disk0.txt", comments='#', delimiter=',')
fig = plt.figure(figsize=[13,5])
plt.plot(ang_vel[:,0], ang_vel[:,3], 'r-', label='$\omega_{disk0}$')
set_axis(plt)
ax=plt.gca()
ax.set_xlabel('$t [s]$', fontsize=fontSize)
ax.set_ylabel('$\omega_{disk0} [rad/s]$', fontsize=fontSize)
ax.grid(True, 'major', 'both')
plt.legend()
plt.tight_layout()
plt.show()
fig.savefig(PLOTS_PATH + 'angular_velocity_disk0.pdf', format='pdf')
def plotOmegaDisk1():
ang_vel = np.loadtxt("angular_velocity_disk1.txt", comments='#', delimiter=',')
fig = plt.figure(figsize=[13,5])
plt.plot(ang_vel[:,0], ang_vel[:,3], 'r-', label='$\omega_{disk1}$')
set_axis(plt)
ax=plt.gca()
ax.set_xlabel('$t [s]$', fontsize=fontSize)
ax.set_ylabel('$\omega_{disk1} [rad/s]$', fontsize=fontSize)
ax.grid(True, 'major', 'both')
plt.legend()
plt.tight_layout()
plt.show()
fig.savefig(PLOTS_PATH + 'angular_velocity_disk1.pdf', format='pdf')
def plotTorque():
ang_vel = np.loadtxt("torque.txt", comments='#', delimiter=',')
fig = plt.figure(figsize=[13,5])
plt.plot(ang_vel[:,0], ang_vel[:,3], 'r-', label='$\\tau$')
set_axis(plt)
ax=plt.gca()
ax.set_xlabel('$t [s]$', fontsize=fontSize)
ax.set_ylabel('$\\tau [Nm]$', fontsize=fontSize)
ax.grid(True, 'major', 'both')
plt.legend()
plt.tight_layout()
plt.show()
fig.savefig(PLOTS_PATH + 'torque.pdf', format='pdf')
def plotCrankPos():
ang_vel = np.loadtxt("crank_pos.txt", comments='#', delimiter=',')
fig = plt.figure(figsize=[13,5])
plt.plot(ang_vel[:,0], ang_vel[:,1], 'r-', label='$x_{Pos}$')
plt.plot(ang_vel[:,0], ang_vel[:,2], 'b-', label='$y_{Pos}$')
set_axis(plt)
ax=plt.gca()
ax.set_xlabel('$t [s]$', fontsize=fontSize)
ax.set_ylabel('$x,y [m]$', fontsize=fontSize)
ax.grid(True, 'major', 'both')
plt.legend()
plt.tight_layout()
plt.show()
fig.savefig(PLOTS_PATH + 'crank_pos.pdf', format='pdf')
def plotBelt():
belt = np.loadtxt("belt.txt", comments='#', delimiter=',')
belt_slope = np.loadtxt("belt_slope.txt", comments='#', delimiter=',')
fig = plt.figure(figsize=[13,5])
plt.plot(belt[:,0], belt[:,1], 'r-', label='Belt')
scale = 100 # scale arrow length
for i in range(belt_slope.shape[0]-1):
plt.arrow(belt_slope[i,0], belt_slope[i,2],
belt_slope[i,1] / scale, belt_slope[i,3] / scale, zorder=10)
set_axis(plt, True)
ax=plt.gca()
ax.set_xlabel('$x [m]$', fontsize=fontSize)
ax.set_ylabel('$y [m]$', fontsize=fontSize)
ax.grid(True, 'major', 'both')
plt.legend()
plt.tight_layout()
plt.show()
fig.savefig(PLOTS_PATH + 'belt.pdf', format='pdf')
def vishelperInit():
plt.close('all')
if not os.path.isdir(PLOTS_PATH):
os.mkdir(PLOTS_PATH)
def visHelperPlot_all():
plotBelt()
plotOmegaDisk0()
plotOmegaDisk1()
plotTorque()
plotCrankPos()
#if __name__ is "__main__":
# init()
# plot_all()
vishelperInit()
print("Exudyn used:", exu.GetVersionString())
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#SYSTEM SETTINGS
# 0 - belt-drive-system
# 1 - crank system 2D
# 2 - crank system 3D
# 3 - belt-drive-system + crank system 2D
# 4 - belt-drive-system + crank system 3D
sys_set = 4
useGraphics = True
export_images = useGraphics
PLOTS_PATH = "plots/"
if export_images:
if not os.path.isdir(PLOTS_PATH):
os.mkdir(PLOTS_PATH)
# belt-drive-system
test_belt_function = True # Draws the belt curve and the tangential vectors
enable_force = True # Enable Preload Force in the belt
enable_disk_friction = True # When True the disks will have contact to the belt else the belt is free floating
enable_controller = True # If True the disk0 will get a torque regulated by a P-controller else a fixed torque
n = 50 # Belt element count
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#geometry parameters
L_0 = 0.5 #distance between the disks [m]
L_A = 0.15 #length of crank [m]
h_A = 0.025 #cross-section height and width of crank [m]
L_B = 0.3 #length of connecting rod [m]
h_B = 0.025 #cross-section height and width of connecting rod [m]
r_0 = 0.05 #radius of disk0 [m]
r_1 = 0.1 #radius of disk1 [m]
b_a0 = 0.1 #section-length of crank [m]
b_a1 = 0.05 #section-length of crank [m]
b_a2 = 0.05 #section-length of crank [m]
h_S = 0.1 #cross-section height and width of slider [m]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#belt parameters
d_belt = 0.01 #cross-section height and width [m]
csa_belt = d_belt**2 #cross-section area [m^2]
E_belt = 2e9 #E modulus [N/m^2]
rho_belt = 1e3 #density [kg/m^3]
F_p = 1e4 #preload force[N]
contactStiffness=1e5 #contactStiffness between the belt and the disks ->
#holds the belt around disks
contactDamping=200 #damping between the belt and the disks
mu = 0.9 #friction coefficent between the belt and the disks
velPenalty = 5e2 #frictionVelocityPenalty between tangential velocities
#of the belt against the disks
controller_set_vel = 100 # Desired angular velocity in rad/s of the disk0: 100 rad/s = ~ 955 U/min
controller_p = 30.0 # P-Factor of controller
MassPerLength_belt = rho_belt * csa_belt
bendStiffness_belt = E_belt * (d_belt**4 / 12)
#Hook: F = E * A * epsilon
axialStiffness_belt = E_belt * csa_belt
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#mass parameters
m_disk0 = 0.5 #mass of disk0 [kg]
m_disk1 = 1 #mass of disk1 [kg]
m_crank = 0.5 #mass of crank [kg]
density_conrod = 1000 #Density of the conrod [kg/m^3]
m_slider = 0.2 #mass of slider [kg]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#inertia parameters
#inertia disks
J_disk0 = 0.5 * m_disk0 * r_0**2 #inertia disk0 [Nm^2]
J_disk1 = 0.5 * m_disk1 * r_1**2 #inertia disk1 [Nm^2]
#inertia crank
J_xx_crank = 5e-3 #inertia xx [Nm^2]
J_yy_crank = 7e-3 #inertia yy [Nm^2]
J_zz_crank = 2.5e-3 #inertia zz [Nm^2]
inertia_crank = RigidBodyInertia(mass=m_crank,
inertiaTensor=np.diag([J_xx_crank,
J_yy_crank,
J_zz_crank]))
#inertia conrod
inertia_conrod = InertiaCuboid(density=density_conrod,
sideLengths = [L_B,h_B,h_B])
#inertia slider
#Dummy inertia because MassPoint will result in System Jacobian not invertible!
inertia_slider = InertiaCuboid(density=(m_slider/h_S**3),
sideLengths = [h_S]*3)
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#bearing parameters
k = 4e4 #stiffness of bearing [N/m]
d = 8e2 #damping of bearing [N/ms]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#load parameters
M = 0.1 #torque [Nm]
g = [0,-9.81,0] #acceleration of earth [m/s^2]
load_crank = [0,-9.81*m_crank,0] #gravity [N]
load_conrod = [0,-9.81*inertia_conrod.mass,0] #gravity [N]
load_slider = [0,-9.81*m_slider,0] #gravity [N]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SC = exu.SystemContainer()
mbs = SC.AddSystem()
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#BELT DRIVE SYSTEM
if sys_set == 0 or sys_set > 2:
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#grounds
#ground-node and ground-marker in center of disk0
nG_disk0 = mbs.AddNode(NodePointGround(referenceCoordinates = [L_0,0,0]))
mG_disk0 = mbs.AddMarker(MarkerNodePosition(nodeNumber = nG_disk0))
#ground-node and ground-marker in center of disk1
nG_disk1 = mbs.AddNode(NodePointGround(referenceCoordinates = [0,0,0]))
mG_disk1 = mbs.AddMarker(MarkerNodePosition(nodeNumber = nG_disk1))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#disk0
#initial coordinates disk0
u0_disk0 = [L_0,0,0]
#NodeRigidBody2D on center of gravity disk0
nRB2D_disk0 = mbs.AddNode(NodeRigidBody2D(referenceCoordinates = [0,0,0],
initialCoordinates = u0_disk0,
initialVelocities = [0,0,0]))
#visualization of disk0
bodyVis_disk0 = VObjectRigidBody2D(graphicsData=[{'type':'Circle',
'color':[0,0,0,1],
'radius':r_0,
'position':[0,0,0],
'normal':[0,0,1]},
{'type':'Line',
'color':[1,0,0,1],
'data':[0,0,0,r_0,0,0]}])
#ObjectRigidBody2D disk0
oRB2D_disk0 = mbs.AddObject(ObjectRigidBody2D(nodeNumber = nRB2D_disk0,
physicsMass=m_disk0,
physicsInertia=J_disk0,
visualization = bodyVis_disk0))
#MarkerBodyRigid on center of disk0
mNP_disk0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB2D_disk0,
localPosition=[0,0,0]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#disk1
#initial coordinates disk1
u0_disk1 = [0,0,0]
#NodeRigidBody2D on center of disk1
nRB2D_disk1 = mbs.AddNode(NodeRigidBody2D(referenceCoordinates = [0,0,0],
initialCoordinates = u0_disk1,
initialVelocities = [0,0,0]))
#visualization of disk1
bodyVis_disk1 = VObjectRigidBody2D(graphicsData=[{'type':'Circle',
'color':[0,0,0,1],
'radius':r_1,
'position':[0,0,0],
'normal':[0,0,1]},
{'type':'Line',
'color':[1,0,0,1],
'data':[0,0,0,r_1,0,0]}])
#ObjectRigidBody2D disk1
oRB2D_disk1 = mbs.AddObject(ObjectRigidBody2D(nodeNumber = nRB2D_disk1,
physicsMass=m_disk1,
physicsInertia=J_disk1,
visualization = bodyVis_disk1))
#MarkerBodyRigid on center of disk1
mNP_disk1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB2D_disk1,
localPosition=[0,0,0]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#joints
#visualization of joint0 in the center of disk0
jointVis_disk0 = VObjectJointRevolute2D(show=True, drawSize=0.02,
color=[0,0,0,1])
#ObjectJointRevolute2D between ground and disk0
oJR2D_disk0 = mbs.AddObject(ObjectJointRevolute2D(markerNumbers = [mG_disk0,mNP_disk0],
visualization = jointVis_disk0))
#visualization of joint1 in the center of disk1
jointVis_disk1 = VObjectJointRevolute2D(show=True, drawSize=0.02, color=[0,0,0,1])
#ObjectJointRevolute2D between ground and disk1
oJR2D_disk1 = mbs.AddObject(ObjectJointRevolute2D(markerNumbers = [mG_disk1,mNP_disk1],
visualization = jointVis_disk1))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#function to get length of the belt
def belt_get_lengths(L_0, r_l, r_r):
alpha = np.arcsin((r_l-r_r)/L_0) #angle of the arc length
b_belt = L_0*np.cos(alpha) #branch between the disks
al_dr_belt = r_r*(np.pi-2*alpha) #arc length disk0
al_dl_belt = r_l*(np.pi+2*alpha) #arc length disk1
len_belt = 2*b_belt+al_dr_belt+al_dl_belt #belt length
return [alpha, b_belt, al_dl_belt, al_dr_belt, len_belt]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#curve parameterization of belt curve
def belt_function(p, L_0, r_l, r_r):
"""
Calcultes the x,y-Position and the tangential Vectors of the belt
curve at the length parameter p [0-1] given.
p = 0 is on up on the left disk where the belt leaves the disk.
"""
[alpha, b_belt, al_dl_belt, al_dr_belt, len_belt] = belt_get_lengths(L_0, r_l, r_r)
# scale [0-1] to [0 - len_belt]
p = p * len_belt
if p < 0.0 or p > len_belt:
return False;
elif p < b_belt:
element_type = "s_u" #straight up
# straight branch up
p_element = p
# calculate start point (p = 0):
x_offset = r_l*np.sin(alpha)
y_offset = r_l*np.cos(alpha)
x_slopex = np.cos(alpha)
y_slopex = -np.sin(alpha)
x_pos = x_offset + p_element * x_slopex
y_pos = y_offset + p_element * y_slopex
elif p < (b_belt + al_dr_belt):
element_type = "d_r" #disk right
# arc at right disk:
p_element = p - b_belt
alpha_element = p_element / r_r
# calculate start point at arc:
alpha_offset = alpha
alpha_i = alpha_offset + alpha_element
x_pos = L_0 + r_r*np.sin(alpha_i)
y_pos = r_r*np.cos(alpha_i)
x_slopex = np.cos(alpha_i)
y_slopex = -np.sin(alpha_i)
elif p <= (2 * b_belt + al_dr_belt):
element_type = "s_d" #straight down
# straight branch down
p_element = p - (b_belt + al_dr_belt)
# calculate start point (p = 0):
x_offset = L_0 + r_r*np.sin(alpha)
y_offset = -r_r*np.cos(alpha)
x_slopex = -np.cos(alpha)
y_slopex = -np.sin(alpha)
x_pos = x_offset + p_element * x_slopex
y_pos = y_offset + p_element * y_slopex
elif p <= len_belt:
element_type = "d_l" #disk left
# arc at left disk:
p_element = p - (2 * b_belt + al_dr_belt)
alpha_element = p_element / r_l
# calculate start point at arc:q
alpha_offset = 2*np.pi - alpha
alpha_i = alpha_offset + alpha_element
x_pos = -r_l*np.sin(alpha_i)
y_pos = -r_l*np.cos(alpha_i)
x_slopex = -np.cos(alpha_i)
y_slopex = np.sin(alpha_i)
return [x_pos, y_pos, x_slopex, y_slopex, element_type]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#Test belt_function
if test_belt_function:
x = []
y = []
for pi in range(0,1001):
# calculate position
p = pi / 1000.0
[xi, yi, sxi, syi, el_typei] = belt_function(p, L_0, r_1, r_0)
x.append(xi)
y.append(yi)
np.savetxt("belt.txt", np.array([x,y]).T, delimiter=',')
slope_x = []
slope_y = []
for pi in range(n):
# calculate position
p = pi / n
[xi, yi, sxi, syi, el_typei] = belt_function(p, L_0, r_1, r_0)
slope_x.append([xi, sxi])
slope_y.append([yi, syi])
np.savetxt("belt_slope.txt", np.hstack((slope_x,slope_y)),delimiter=',')
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#belt nodes
nNP2DS = []
for i in range(n):
p = i / n
[xi, yi, sxi, syi, el_typei] = belt_function(p, L_0, r_1, r_0)
nNP2DS.append(mbs.AddNode(NodePoint2DSlope1(name=el_typei+"_"+str(i),
referenceCoordinates = [xi,yi,sxi,syi])))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#belt cable elements
[alpha, b_belt, al_dl_belt, al_dr_belt, len_belt] = belt_get_lengths(L_0, r_1, r_0)
#calculate belt length with preload force:
epsilon_belt = -F_p / axialStiffness_belt
len_belt_p = len_belt * (1.0 + epsilon_belt)
print("Belt length: ", len_belt, " and after preload force: ", len_belt_p)
el_len = len_belt / len(nNP2DS)
oANCFC2D_b = []
if not enable_force:
epsilon_belt = 0.0 # Without preload force
print("epsilon: ", epsilon_belt)
#ANCF cable objects
b_len = 0.0
for i in range(len(nNP2DS)):
b_len += el_len # Sum all elements -> after loop len_belt must be equal to this sum
oANCFC2D_b.append(mbs.AddObject(ObjectANCFCable2D(nodeNumbers=[nNP2DS[i], nNP2DS[(i+1)%len(nNP2DS)]],
physicsLength=el_len,
physicsBendingStiffness=bendStiffness_belt,
physicsMassPerLength=MassPerLength_belt,
physicsAxialStiffness=axialStiffness_belt,
physicsReferenceAxialStrain=epsilon_belt)))
print("Belt length from element length sum: ", b_len)
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#contact friction belt
if enable_disk_friction:
nCableGenData_disk0 = []
contact_disk0 = []
nCableGenData_disk1 = []
contact_disk1 = []
mCableShape = []
for i in range(0,len(oANCFC2D_b)):
nCS = 8
#NodeGenericData disk0
nCableGenData_disk0.append(mbs.AddNode(NodeGenericData(initialCoordinates = [0,0,0]*nCS,
numberOfDataCoordinates=3*nCS)))
#NodeGenericData disk1
nCableGenData_disk1.append(mbs.AddNode(NodeGenericData(initialCoordinates = [0,0,0]*nCS,
numberOfDataCoordinates=3*nCS)))
#MarkerBodyCable2DShape on cable
mCableShape.append(mbs.AddMarker(MarkerBodyCable2DShape(bodyNumber=oANCFC2D_b[i],
numberOfSegments=nCS)))
#contact friction to disk0
contact_disk0.append(mbs.AddObject(ObjectContactFrictionCircleCable2D(markerNumbers=[mNP_disk0,mCableShape[-1]],
nodeNumber=nCableGenData_disk0[-1],
circleRadius=r_0,
contactStiffness=contactStiffness,
contactDamping=contactDamping,
numberOfContactSegments=nCS,
frictionCoefficient=mu,
frictionVelocityPenalty=velPenalty)))
#contact friction to disk1
contact_disk1.append(mbs.AddObject(ObjectContactFrictionCircleCable2D(markerNumbers=[mNP_disk1, mCableShape[-1]],
nodeNumber=nCableGenData_disk1[-1],
circleRadius=r_1,
contactStiffness=contactStiffness,
contactDamping=contactDamping,
numberOfContactSegments=nCS,
frictionCoefficient=mu,
frictionVelocityPenalty=velPenalty)))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# Velocity controller
s_disk0 = mbs.AddSensor(SensorBody(bodyNumber=oRB2D_disk0, writeToFile=True,
fileName="angular_velocity_disk0.txt",
outputVariableType=exu.OutputVariableType.AngularVelocity))
def p_controller(mbs, t, loadVector):
vel = mbs.GetSensorValues(s_disk0)[2]
torque = controller_p * (controller_set_vel - vel)
return [0,0,torque]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#torque on disk0
if enable_controller:
l_Torquedisk0 = mbs.AddLoad(LoadTorqueVector(markerNumber=mNP_disk0,
loadVectorUserFunction=p_controller))
else:
l_Torquedisk0 = mbs.AddLoad(LoadTorqueVector(markerNumber=mNP_disk0,loadVector=[0,0,M]))
s_disk1 = mbs.AddSensor(SensorBody(bodyNumber=oRB2D_disk1, writeToFile=True,
fileName="angular_velocity_disk1.txt",
outputVariableType=exu.OutputVariableType.AngularVelocity))
s_load = mbs.AddSensor(SensorLoad(loadNumber=l_Torquedisk0, writeToFile=True,
fileName="torque.txt"))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#CRANK DRIVE SYSTEM 2D
if sys_set == 1 or sys_set == 3:
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#ground
nPG = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
oG = mbs.AddObject(ObjectGround(referencePosition=[0,0,0]))
mBPG = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oG, localPosition=[0,0,0]))
mNCG = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nPG, coordinate=0))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#crank
#visualization of crank
graphics_crank = graphics.RigidLink(p0=[-0.5*L_A,0,-h_A/2],
p1=[0.5*L_A ,0,-h_A/2],
axis0=[0,0,1], axis1=[0,0,1],
radius=[0.5*h_A,0.5*h_A],
thickness=h_A, width=[h_A,h_A],
color=[0.8,0.8,0.8,1.],nTiles=16)
#node on center of gravity of crank
nRB2D_crank = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[-L_A*0.5,0,0],
initialVelocities=[0,0,0]))
#RigidBody2D crank
oRB2D_crank = mbs.AddObject(RigidBody2D(physicsMass=m_crank,
physicsInertia=J_zz_crank,
nodeNumber=nRB2D_crank,
visualization=VObjectRigidBody2D(graphicsData=[graphics_crank])))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#connecting rod
#visualization of connecting rod
graphics_conrod = graphics.RigidLink(p0=[-0.5*L_B,0,h_B/2],
p1=[0.5*L_B,0,h_B/2],
axis0=[0,0,1], axis1=[0,0,1],
radius=[0.5*h_B,0.5*h_B],
thickness=h_B, width=[h_B,h_B],
color=[0.7,0.7,0.7,1],nTiles=36)
#node on center of gravity of connecting rod
nRB2D_conrod = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[-(L_A+L_B*0.5),0,0],
initialVelocities=[0,0,0]))
#RigidBody2D connecting rod
oRB2D_conrod = mbs.AddObject(RigidBody2D(physicsMass=inertia_conrod.mass,
physicsInertia=inertia_conrod.inertiaTensor[1,1],
nodeNumber=nRB2D_conrod,
visualization=VObjectRigidBody2D(graphicsData= [graphics_conrod])))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#slider
#visualization of slider
c=0.025 #dimension of slider
graphics_slider = graphics.BrickXYZ(-c,-c,-c*2,c,c,0,
color=[0.2,0.2,0.2,0.9])
#node on center of gravity of slider
nP2D_slider = mbs.AddNode(NodePoint2D(referenceCoordinates=[-(L_A+L_B),0]))
#MassPoint2D slider
oMP2D_slider = mbs.AddObject(MassPoint2D(physicsMass=m_slider,
nodeNumber=nP2D_slider,
visualization=VObjectRigidBody2D(graphicsData= [graphics_slider])))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#markers for joints
mBPLeft_crank = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_crank,
localPosition=[-L_A*0.5,0.,0.]))
mBPRight_crank = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_crank,
localPosition=[L_A*0.5,0.,0.]))
mBPLeft_conrod = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_conrod,
localPosition=[-L_B*0.5,0.,0.]))
mBPRight_conrod = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRB2D_conrod,
localPosition=[L_B*0.5,0.,0.]))
mBP_slider = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oMP2D_slider,
localPosition=[ 0.,0.,0.]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#joints
oRJ2D_ground_crank = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBPG,mBPRight_crank]))
oRJ2D_crank_conrod = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBPLeft_crank,mBPRight_conrod]))
oRJ2D_slider = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBP_slider,mBPLeft_conrod]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#markers for node constraints
mNC_Y_slider = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nP2D_slider, coordinate=1)) #y-coordinate is constrained
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#coordinate constraints for slider (free motion in x-direction)
oCC_ground_slider = mbs.AddObject(CoordinateConstraint(markerNumbers=[mNCG, mNC_Y_slider]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#markers for load
mBR_crank_torque = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB2D_crank, localPosition=[L_A/2,0.,0.]))
mBR_crank_gravity = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB2D_crank, localPosition=[0.,0.,0.]))
mBR_conrod_gravity = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB2D_conrod, localPosition=[0.,0.,0.]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#loads and driving forces
#gravity of crank
lC_crank_gravity = mbs.AddLoad(Force(markerNumber = mBR_crank_gravity,
loadVector = load_crank))
#gravity of conrod
lC_conrod_gravity = mbs.AddLoad(Force(markerNumber = mBR_conrod_gravity,
loadVector = load_conrod))
#gravity of slider
lC_slider_gravity = mbs.AddLoad(Force(markerNumber = mBP_slider,
loadVector = load_slider))
if sys_set == 1:
#torque at crank
lC_crank_torque = mbs.AddLoad(Torque(markerNumber = mBR_crank_torque,
loadVector = [0, 0, M]))
if sys_set == 3:
#ConnectorCoordinate - crank gets torque of disk1
mNC_disk1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRB2D_disk1,coordinate=2))
mNC_crank = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRB2D_crank,coordinate=2))
oCC_disk1_crank = mbs.AddObject(ObjectConnectorCoordinate(markerNumbers=[mNC_disk1,mNC_crank],
velocityLevel=True))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#CRANK DRIVE SYSTEM 3D
if sys_set == 2 or sys_set == 4:
nodeType=exu.NodeType.RotationEulerParameters
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#ground
oG_Left = mbs.AddObject(ObjectGround())
oG_Right = mbs.AddObject(ObjectGround())
oG_slider = mbs.AddObject(ObjectGround())
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#crank
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],
thickness = h_A, width=[0,h_A/2], color=[0.8,0.8,0.8,1])
graphics_crank_2 = graphics.RigidLink(p0=[0,0,0],p1=[-L_A,0,0], radius=[h_A/2,h_A/2],
thickness = h_A, color=[0.8,0.8,0.8,1])
graphics_crank_3 = graphics.RigidLink(p0=[-L_A,0,0],p1=[-L_A,0,b_a1], radius=[h_A/2,h_A/2],
thickness = h_A, color=[0.8,0.8,0.8,1])
graphics_crank_4 = graphics.RigidLink(p0=[-L_A,0,b_a1],p1=[0,0,b_a1], radius=[h_A/2,h_A/2],
thickness = h_A, color=[0.8,0.8,0.8,1])
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],
thickness = h_A, width=[0,h_A/2], color=[0.8,0.8,0.8,1])
[nRG_crank,oRB_crank]=AddRigidBody(mainSys = mbs, inertia=inertia_crank, nodeType=str(nodeType),
position=[0,0,b_a0], angularVelocity=[0,0,0],
gravity=g, graphicsDataList=[graphics_crank_1,
graphics_crank_2,graphics_crank_3, graphics_crank_4,graphics_crank_5])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#connecting rod
graphics_conrod = graphics.RigidLink(p0=[L_B/2,0,0],p1=[-L_B/2,0,0], axis0=[0,0,1], axis1=[0,0,1], radius=[h_B/1.5,h_B/2],
thickness = h_B, width=[h_B,h_B], color=[0.5,0.5,0.5,1])
[nRG_conrod,oRB_conrod]=AddRigidBody(mainSys = mbs, inertia=inertia_conrod, nodeType=str(nodeType), angularVelocity=[0,0,0],
position=[-L_A-L_B/2,0,b_a0+b_a1/2], gravity=g, graphicsDataList=[graphics_conrod])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#slider
d=0.07
graphics_slider = graphics.BrickXYZ(-d/2,-d/2,-d-h_B/2,d/2,d/2,-h_B/2,
color=[0.2,0.2,0.2,0.9])
[nRB_slider,oRB_slider]=AddRigidBody(mainSys = mbs,
inertia=inertia_slider,
nodeType=str(nodeType),
angularVelocity=[0,0,0],
position=[-(L_A+L_B),0,b_a0+b_a1/2],
gravity=g,
graphicsDataList=[graphics_slider])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#marker for joints
mG_Left = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oG_Left,
localPosition=[0,0,0]))
mG_Right = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oG_Right,
localPosition=[0,0,b_a0+b_a1+b_a2]))
mG_slider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oG_slider,
localPosition=[-(L_A+L_B),0,b_a0+b_a1/2]))
mBR_crank_Left = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
localPosition=[0,0,-b_a0]))
mBR_crank_Right = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
localPosition=[0,0,b_a1+b_a2]))
mBR_crank_conrod = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
localPosition=[-L_A,0,b_a1/2]))
mBR_conrod_crank = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_conrod,
localPosition=[L_B/2,0,0]))
mBR_conrod_slider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_conrod,
localPosition=[-L_B/2,0,0]))
mBR_slider_conrod = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_slider,
localPosition=[0,0,0]))
mBR_slider = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_slider,
localPosition=[0,0,0]))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++q
#joints
oGJ_crank_Left = mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mG_Left, mBR_crank_Left],
stiffness=[k]*3, damping=[d]*3))
oGJ_crank_Right = mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mG_Right, mBR_crank_Right],
stiffness=[k]*3, damping=[d]*3))
oRJ2D_crank_conrod = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBR_crank_conrod,mBR_conrod_crank],
visualization=VObjectJointRevolute2D(drawSize=0.05)))
oRJ2D_conrod_slider = mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBR_conrod_slider,mBR_slider_conrod],
visualization=VObjectJointRevolute2D(drawSize=0.05)))
oJP2D_slider = mbs.AddObject(ObjectJointPrismatic2D(markerNumbers=[mG_slider,mBR_slider],
axisMarker0 = [1.,0.,0.],
normalMarker1 = [0.,1.,0.],
visualization=VObjectJointPrismatic2D(drawSize=0.01)))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#load and driving forces
if sys_set == 2:
#markers for load
mBR_crank_torque = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRB_crank,
localPosition=[0.,0.,b_a1+b_a2]))
#driving forces
lC_crank_torque = mbs.AddLoad(Torque(markerNumber = mBR_crank_torque,
loadVector = [0, 0, M/2]))
if sys_set == 4:
#ConnectorCoordinate - crank gets torque of disk1
#markers for Connector
mNC_disk1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRB2D_disk1,coordinate=2))
# disk1 is a NodeRigidBody2D -> coordinates = [q_0,q_1,psi] -> Rotation psi
mNC_crank = mbs.AddMarker(MarkerNodeRotationCoordinate(nodeNumber=nRG_crank,rotationCoordinate=2))
# crank is a 3D Node -> Rotation around z-axis
#Connector Coordinate
oCC_disk1_crank = mbs.AddObject(ObjectConnectorCoordinate(markerNumbers=[mNC_disk1,mNC_crank],
velocityLevel=True))
s_crank = mbs.AddSensor(SensorBody(bodyNumber=oRB_crank, writeToFile=True,
fileName="crank_pos.txt", localPosition=[0,0,-b_a0],
outputVariableType=exu.OutputVariableType.Position))
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
mbs.Assemble()
print(mbs)
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#simulation
if sys_set == 0:
tEnd = 1 #end time of the simulation
steps = 1000 #number of steps
elif sys_set > 0:
tEnd = 2 #end time of the simulation
steps = 1000 #number of steps
sims = exu.SimulationSettings()
sims.timeIntegration.generalizedAlpha.spectralRadius=1
if export_images:
sims.solutionSettings.recordImagesInterval = 0.001
if not os.path.isdir("images"):
os.mkdir("images")
else:
sims.solutionSettings.recordImagesInterval = -1
sims.pauseAfterEachStep = False
sims.displayStatistics = True
sims.displayComputationTime = True
sims.timeIntegration.numberOfSteps = steps
sims.timeIntegration.endTime = tEnd
sims.solutionSettings.sensorsWritePeriod = 0.001
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#visualizaion
SC.visualizationSettings.general.circleTiling=128
dSize = 0.02
SC.visualizationSettings.nodes.defaultSize = dSize
SC.visualizationSettings.markers.defaultSize = dSize
SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
SC.visualizationSettings.connectors.defaultSize = dSize
SC.visualizationSettings.nodes.show=True
SC.visualizationSettings.loads.show=True
SC.visualizationSettings.markers.show=True
SC.visualizationSettings.sensors.show=True
SC.visualizationSettings.general.autoFitScene=False
if sys_set == 0:
SC.visualizationSettings.openGL.initialCenterPoint = [L_0/2,0,0]
SC.visualizationSettings.openGL.initialZoom = 0.5
elif sys_set == 1:
SC.visualizationSettings.openGL.initialCenterPoint = [-L_A,0,0]
SC.visualizationSettings.openGL.initialZoom = 0.4
elif sys_set == 2:
SC.visualizationSettings.openGL.initialCenterPoint = [-0.1,0,0]
SC.visualizationSettings.openGL.initialZoom = 0.3
elif sys_set == 3:
SC.visualizationSettings.openGL.initialCenterPoint = [0,0,0]
SC.visualizationSettings.openGL.initialZoom = 0.5
elif sys_set == 4:
SC.visualizationSettings.openGL.initialCenterPoint = [0,0,0]
SC.visualizationSettings.openGL.initialZoom = 0.5
SC.visualizationSettings.contour.outputVariable = exu.OutputVariableType.Force
if sys_set == 0:
rot_alpha=0
rot_beta=0
rot_gamma=0
elif sys_set > 0:
rot_alpha=-0.6
rot_beta=0.6
rot_gamma=0.37
R_x = np.array([[ 1, 0, 0],
[ 0, np.cos(rot_alpha), -np.sin(rot_alpha)],
[ 0, np.sin(rot_alpha), np.cos(rot_alpha)]])
R_y = np.array([[ np.cos(rot_beta), 0, np.sin(rot_beta)],
[ 0, 1, 0],
[ -np.sin(rot_beta), 0, np.cos(rot_beta)]])
R_z = np.array([[ np.cos(rot_gamma), -np.sin(rot_gamma), 0],
[ np.sin(rot_gamma), np.cos(rot_gamma), 0],
[ 0, 0, 1]])
IMR = np.dot(R_x,R_y)
IMR = np.dot(IMR,R_z)
SC.visualizationSettings.openGL.initialModelRotation = [[IMR[0,0],IMR[0,1],IMR[0,2]],
[IMR[1,0],IMR[1,1],IMR[1,2]],
[IMR[2,0],IMR[2,1],IMR[2,2]]]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#Rendering
exu.StartRenderer() #start graphics visualization
mbs.WaitForUserToContinue() #wait for pressing SPACE bar to continue
mbs.SolveDynamic(sims)
SC.WaitForRenderEngineStopFlag() #wait for pressing 'Q' to quit
exu.StopRenderer() #safely close rendering window!
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if export_images:
visHelperPlot_all()