1.2 Kinematic Analysis Code
Kinematic Analysis Code for Wiper:
import numpy as np
import matplotlib.pyplot as plt
def slidercrankpos(l, th_2, delta):
if delta == 1:
# Open configuration: theta3 in [-pi/2, pi/2]
th_3 = np.arcsin((l[0]*np.sin(th_2) - l[2]) / l[1])
else:
# Crossed configuration: theta3 in [pi/2, 3*pi/2]
th_3 = np.arcsin(-(l[0]*np.sin(th_2) - l[2]) / l[1]) + np.pi
l1 = l[1]*np.cos(th_3) - l[0]*np.cos(th_2)
return l1, th_3
def slidercrankvel(l_vec, th_2, th_3, w_2):
w_3 = (l_vec[0] / l_vec[1]) * (np.cos(th_2) / np.cos(th_3)) * w_2
l1_dot = l_vec[1]*w_3*np.sin(th_3) - l_vec[0]*np.sin(th_2)*w_2
return l1_dot, w_3
def slidercrankacc(l_vec, th_2, th_3, w_2, w_3, a_2):
alpha_3 = (
l_vec[0]*a_2*np.cos(th_2)
- l_vec[0]*(w_2**2)*np.sin(th_2)
+ l_vec[1]*(w_3**2)*np.sin(th_3)
) / (l_vec[1]*np.cos(th_3))
l1_db_dot = (l_vec[0]*a_2*np.sin(th_2) + l_vec[0]*(w_2**2)*np.cos(th_2) -
l_vec[1]*alpha_3*np.sin(th_3) - l_vec[1]*(w_3**2)*np.cos(th_3))
return l1_db_dot, alpha_3
# Link lengths: [crank, coupler, offset]
l_vec = np.array([0.0285, 0.130, 0.0283])
th_2 = np.linspace(np.deg2rad(0), np.deg2rad(360), 1000)
# Omega = Power / Torque = VI / torque
# The torque is 800 g/cm but now in NM
# For our motor, it is (3V * 0.25A)/(0.0784532 Nm) = 9.5598395 # rad/s
n_motor = 30 # teeth of motor gear
n_driver = 100 # teeth of driver gear
w_2 = n_motor / n_driver
a_2 = 0 # rad/s^2
delta = 1 # open configuration
l1_vec, th_3 = [], []
for item in th_2:
l1, theta_3 = slidercrankpos(l_vec, item, delta)
th_3.append(theta_3)
l1_vec.append(l1)
l1_dot_vec, w_3_vec, l1_db_dot_vec, a_3_vec = [], [], [], []
for t2, t3 in zip(th_2, th_3):
l1_dot, w_3 = slidercrankvel(l_vec, t2, t3, w_2)
l1_dot_vec.append(l1_dot)
w_3_vec.append(w_3)
l1_db_dot, a_3 = slidercrankacc(l_vec, t2, t3, w_2, w_3, a_2)
l1_db_dot_vec.append(l1_db_dot)
a_3_vec.append(a_3)
fig, ax = plt.subplots(3, figsize=(10, 12))
plt.subplots_adjust(hspace=0.6)
ax[0].plot(th_2 * 180/np.pi, l1_db_dot_vec)
ax[0].set_title("Slider Acceleration vs. Input Angle")
ax[0].set_xlabel(r"$\theta_2$ (deg)")
ax[0].set_ylabel("Acceleration (m/s²)")
ax[0].grid()
ax[1].plot(th_2 * 180/np.pi, l1_dot_vec)
ax[1].set_title("Slider Velocity vs. Input Angle")
ax[1].set_xlabel(r"$\theta_2$ (deg)")
ax[1].set_ylabel("Velocity (m/s)")
ax[1].grid()
ax[2].plot(th_2 * 180/np.pi, l1_vec)
ax[2].set_title("Slider Position vs. Input Angle")
ax[2].set_xlabel(r"$\theta_2$ (deg)")
ax[2].set_ylabel("Position (m)")
ax[2].grid()
plt.show()Kinematic Analysis Code for Pulleys:
import numpy as np
import matplotlib.pyplot as plt
"""
Source: https://www.engineersedge.com/mechanics_machines/geneva_mechanism_design_equations__14919.htm
Written with the assistance of llms
"""
def geneva_kinematics(theta_driver, n_slots, m_geneva, omega_2):
step = 2 * np.pi / n_slots
engage_limit = np.pi / n_slots
# Wrap to current position in cycle
alpha_mod = theta_driver % (2 * np.pi)
alpha_eng = np.where(alpha_mod <= np.pi,
alpha_mod,
alpha_mod - 2 * np.pi)
engaged = np.abs(alpha_eng) <= engage_limit
denom = 1 + m_geneva**2 - 2 * m_geneva * np.cos(alpha_eng)
beta_local_engaged = -np.arctan2(
np.sin(alpha_eng),
m_geneva - np.cos(alpha_eng)
)
omega_g_engaged = -omega_2 * (
(m_geneva * np.cos(alpha_eng) - 1) / denom
)
alpha_g_engaged = -(omega_2**2) * (
m_geneva * np.sin(alpha_eng) * (1 - m_geneva**2)
) / (denom**2)
beta_local_dwell = -np.arctan2(
np.sin(engage_limit),
m_geneva - np.cos(engage_limit)
)
beta_local = np.where(engaged, beta_local_engaged, beta_local_dwell)
omega_g = np.where(engaged, omega_g_engaged, 0.0)
alpha_g = np.where(engaged, alpha_g_engaged, 0.0)
# Calculate which slot based on continuous angle
# Each engagement ends at theta_driver = engage_limit, 2π + engage_limit, 4π + engage_limit, etc.
slot_index = np.floor((theta_driver + engage_limit) / (2 * np.pi)).astype(int)
beta_total = -slot_index * step + beta_local
return beta_total, omega_g, alpha_g
w_motor = 9.5598395 #rad/sec
n_motor = 30
n_driver = 100
omega_2 = (30/100) * w_motor
pulley_radius = 0.025 # m
center_distance = 0.057 # m
pin_orbit_radius = 0.041 # m
pin_radius = 0.0035 # m
follower_radius = 0.038 # m
n_slots = 4
m_geneva = center_distance / pin_orbit_radius
th2_vec = np.linspace(0, 4 * 2 * np.pi, 4000)
beta, omega_g, alpha_g = geneva_kinematics(th2_vec, n_slots, m_geneva, omega_2)
l_pully_angular_pos = -beta + np.pi
r_pully_angular_pos = beta
l_vx = pulley_radius * omega_g * np.sin(-beta + np.pi)
l_vy = -pulley_radius * omega_g * np.cos(-beta + np.pi)
r_vx = -pulley_radius * omega_g * np.sin(beta)
r_vy = pulley_radius * omega_g * np.cos(beta)
l_ax = -pulley_radius * (omega_g**2) * np.cos(-beta + np.pi) + pulley_radius * alpha_g * np.sin(-beta + np.pi)
l_ay = -pulley_radius * (omega_g**2) * np.sin(-beta + np.pi) - pulley_radius * alpha_g * np.cos(-beta + np.pi)
r_ax = -pulley_radius * (omega_g**2) * np.cos(beta) - pulley_radius * alpha_g * np.sin(beta)
r_ay = -pulley_radius * (omega_g**2) * np.sin(beta) + pulley_radius * alpha_g * np.cos(beta)
l_vmag_vec = np.sqrt(l_vx**2 + l_vy**2)
r_vmag_vec = np.sqrt(r_vx**2 + r_vy**2)
l_amag_vec = np.sqrt(l_ax**2 + l_ay**2)
r_amag_vec = np.sqrt(r_ax**2 + r_ay**2)
fig, ax = plt.subplots(5, figsize=(10, 32))
plt.subplots_adjust(hspace=0.6)
# 1. Angular Position
ax[0].plot(th2_vec * 180/np.pi, l_pully_angular_pos * 180/np.pi, label="Left Pully")
ax[0].plot(th2_vec * 180/np.pi, r_pully_angular_pos * 180/np.pi, label="Right Pully")
ax[0].set_title("Pullys Angular Position vs. Input Angle")
ax[0].set_xlabel(r"$\theta_2$ (deg)")
ax[0].set_ylabel("Angular Position (deg)")
ax[0].grid()
ax[0].legend()
# 2. Angular Velocity
ax[1].plot(th2_vec * 180/np.pi, -omega_g, label="Left Pully")
ax[1].plot(th2_vec * 180/np.pi, omega_g, label="Right Pully")
ax[1].set_title("Pullys Angular Velocity vs. Input Angle")
ax[1].set_xlabel(r"$\theta_2$ (deg)")
ax[1].set_ylabel("Angular Velocity (rad/s)")
ax[1].grid()
ax[1].legend()
# 3. Linear Velocity Magnitude
ax[2].plot(th2_vec * 180/np.pi, l_vmag_vec, label="Left Pully")
ax[2].plot(th2_vec * 180/np.pi, r_vmag_vec, label="Right Pully")
ax[2].set_title("Pullys Linear Velocity Magnitude vs. Input Angle")
ax[2].set_xlabel(r"$\theta_2$ (deg)")
ax[2].set_ylabel("Linear Velocity Magnitude (m/s)")
ax[2].grid()
ax[2].legend()
# 4. Angular Acceleration
ax[3].plot(th2_vec * 180/np.pi, -alpha_g, label="Left Pully")
ax[3].plot(th2_vec * 180/np.pi, alpha_g, label="Right Pully")
ax[3].set_title("Pullys Angular Acceleration vs. Input Angle")
ax[3].set_xlabel(r"$\theta_2$ (deg)")
ax[3].set_ylabel("Angular Acceleration (rad/s²)")
ax[3].grid()
ax[3].legend()
# 5. Linear Acceleration Magnitude
ax[4].plot(th2_vec * 180/np.pi, l_amag_vec, label="Left Pully")
ax[4].plot(th2_vec * 180/np.pi, r_amag_vec, label="Right Pully")
ax[4].set_title("Pullys Linear Acceleration Magnitude vs. Input Angle")
ax[4].set_xlabel(r"$\theta_2$ (deg)")
ax[4].set_ylabel("Linear Acceleration Magnitude (m/s²)")
ax[4].grid()
ax[4].legend()
plt.show()Animation Code:
"""
WindowWasherVisualize.py
"""
# Imports
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Circle
class WindowWasherVisualize():
# Constructor, Will center plot around the driver
def __init__(self):
self.w_motor = 9.5598395 #rad/sec
self.n_motor = 30
self.n_driver = 100
self.omega_2 = (30/100) * self.w_motor
self.l2 = 0.0285 # m
self.l3 = 0.130 # m
self.l4 = 0.0283 # m
self.driver_radius = 0.041
self.sc_global_offset = np.deg2rad(270)
self.pulley_radius = 0.025 # m
self.l_pully_x = -0.067
self.r_pully_x = 0.124
self.pully_y = 0.040
# Geneva
self.center_distance = 0.057
self.pin_orbit_radius = 0.041
self.pin_radius = 0.0035
self.follower_radius = 0.038
self.n_slots = 4
self.m_geneva = self.center_distance / self.pin_orbit_radius
self.follower_center_x = self.center_distance
self.follower_center_y = 0.0
self.fig, self.ax = plt.subplots(1,1,figsize=(6,6)) # New figure
self.ax.set_aspect('equal')
self.fig.set_tight_layout(True)
self.ax.set_xlim(-0.30, 0.30)
self.ax.set_ylim(-0.30, 0.30)
# Centers the axes
self.ax.spines['left'].set_position('center')
self.ax.spines['bottom'].set_position('center')
self.ax.spines['right'].set_color('none')
self.ax.spines['top'].set_color('none')
def window_washer_plot(self,th_2_in,showVelArrows=True, showAccArrows=True):
self.ax.clear()
self.ax.set_aspect('equal')
self.fig.set_tight_layout(True)
self.ax.set_xlim(-0.30, 0.30)
self.ax.set_ylim(-0.30, 0.30)
# Centers the axes
self.ax.spines['left'].set_position('center')
self.ax.spines['bottom'].set_position('center')
self.ax.spines['right'].set_color('none')
self.ax.spines['top'].set_color('none')
th_2_rad = np.deg2rad(th_2_in)
"""
Shared
"""
geneva_circle = Circle((0, 0), self.driver_radius, fill=False, color='black', linewidth=2)
self.ax.add_patch(geneva_circle)
self.ax.scatter([0],[0], s=100, marker='o', facecolors='none', edgecolors='black') # Joint O_2
"""
Slider Crank:
"""
th_2_local = th_2_rad - self.sc_global_offset
th_3_local, l1_local = self.slider_crank_pos(self.l2, self.l3, th_2_local, crossed=False, offset=self.l4)
th_3_global = th_3_local + self.sc_global_offset
w_3, l1_vel = self.slider_crank_vel(self.l2, self.l3, th_2_local, th_3_local, self.omega_2)
a_3, l1_acc = self.slider_crank_acc(self.l2, self.l3, th_2_local, th_3_local, self.omega_2, w_3, 0)
R_AO2X = self.l2*np.cos(th_2_rad)
R_AO2Y = self.l2*np.sin(th_2_rad)
# Get position of B with respect to A
R_BAX = self.l3*np.cos(th_3_global)
R_BAY = self.l3*np.sin(th_3_global)
# Get position of B in the GCS
R_BX = R_AO2X - R_BAX
R_BY = R_AO2Y - R_BAY
vAx = -self.l2 * self.omega_2 * np.sin(th_2_rad)
vAy = self.l2 * self.omega_2 * np.cos(th_2_rad)
vBAx = -self.l3 * w_3 * np.sin(th_3_global)
vBAy = self.l3 * w_3 * np.cos(th_3_global)
vBx = l1_vel * np.cos(self.sc_global_offset)
vBy = l1_vel * np.sin(self.sc_global_offset)
# Acceleration of A (alpha_2 = 0)
aAx = -self.l2 * (self.omega_2**2) * np.cos(th_2_rad)
aAy = -self.l2 * (self.omega_2**2) * np.sin(th_2_rad)
# Acceleration of B relative to A
aBAx = -self.l3 * a_3 * np.sin(th_3_global) - self.l3 * (w_3**2) * np.cos(th_3_global)
aBAy = self.l3 * a_3 * np.cos(th_3_global) - self.l3 * (w_3**2) * np.sin(th_3_global)
# Acceleration of B (slider direction)
aBx = l1_acc * np.cos(self.sc_global_offset)
aBy = l1_acc * np.sin(self.sc_global_offset)
self.ax.plot([0,R_AO2X],[0,R_AO2Y],color='red', linestyle='solid') # Link 2
self.ax.scatter([R_AO2X],[R_AO2Y], s=20, marker='o', facecolors='none', edgecolors='r') # Joint A
self.ax.plot([R_AO2X,R_BX],[R_AO2Y,R_BY],color='blue', linestyle='solid') # Link 3
self.ax.scatter([R_BX],[R_BY], s=20, marker='o', facecolors='none', edgecolors='b') # Joint B
if showVelArrows:
self.ax.arrow(R_AO2X,R_AO2Y, vAx, vAy,head_width=0.005,width=0.0005,length_includes_head=True,linestyle='dashed',color='r',overhang=1.0)
self.ax.arrow(R_AO2X,R_AO2Y,vBAx ,vBAy ,head_width=0.005,width=0.0005,length_includes_head=True,linestyle='dashed',color='b',overhang=1.0)
self.ax.arrow(R_BX,R_BY,vBx,vBy,head_width=0.005,width=0.0005,length_includes_head=True,linestyle='dashed',color='darkblue',overhang=1.0)
if showAccArrows:
self.ax.arrow(R_AO2X, R_AO2Y, aAx, aAy, head_width=0.005, width=0.0005, length_includes_head=True, linestyle='dashed', color='r', overhang=1.0)
self.ax.arrow(R_AO2X, R_AO2Y, aBAx, aBAy, head_width=0.005, width=0.0005, length_includes_head=True, linestyle='dashed', color='b', overhang=1.0)
self.ax.arrow(R_BX, R_BY, aBx, aBy, head_width=0.005, width=0.0005, length_includes_head=True, linestyle='dashed', color='darkblue', overhang=1.0)
"""
Geneva Mechanism:
"""
beta, omega_g, alpha_g = self.geneva_kinematics(th_2_rad)
# driver pin
pin_x = self.pin_orbit_radius * np.cos(th_2_rad)
pin_y = self.pin_orbit_radius * np.sin(th_2_rad)
self.ax.add_patch(Circle((pin_x, pin_y), self.pin_radius,
fill=False, color='darkred', linewidth=1.5))
# follower
self.ax.add_patch(Circle((self.follower_center_x, self.follower_center_y),
self.follower_radius,
fill=False, color='royalblue', linewidth=2))
self.ax.scatter([self.follower_center_x], [self.follower_center_y],
s=100, marker='o', facecolors='none', edgecolors='royalblue')
# 4 dots on follower
indent_angles = np.array([0, np.pi/2, np.pi, 3*np.pi/2])
indent_x = self.follower_center_x + self.follower_radius * np.cos(indent_angles + beta)
indent_y = self.follower_center_y + self.follower_radius * np.sin(indent_angles + beta)
self.ax.scatter(indent_x, indent_y, color='slateblue', s=25)
# marker radius on follower
marker_r = self.follower_radius * 0.8
marker_x = self.follower_center_x + marker_r * np.cos(beta)
marker_y = self.follower_center_y + marker_r * np.sin(beta)
self.ax.plot([self.follower_center_x, marker_x],
[self.follower_center_y, marker_y],
color='purple', linewidth=2)
self.ax.scatter([marker_x], [marker_y], color='purple', s=20)
left_pully = Circle((self.l_pully_x, self.pully_y), self.pulley_radius, fill=False, color='green', linewidth=2)
right_pully = Circle((self.r_pully_x, self.pully_y), self.pulley_radius, fill=False, color='green', linewidth=2)
self.ax.add_patch(left_pully)
self.ax.add_patch(right_pully)
l_pully_x = self.l_pully_x + self.pulley_radius * np.cos(-beta + np.pi)
l_pully_y = self.pully_y + self.pulley_radius * np.sin(-beta + np.pi)
r_pully_x = self.r_pully_x + self.pulley_radius * np.cos(beta)
r_pully_y = self.pully_y + self.pulley_radius * np.sin(beta)
self.ax.scatter([l_pully_x], [l_pully_y], color='green', s=20)
self.ax.scatter([r_pully_x], [r_pully_y], color='green', s=20)
# velocity
l_vx = self.pulley_radius * omega_g * np.sin(-beta + np.pi)
l_vy = -self.pulley_radius * omega_g * np.cos(-beta + np.pi)
r_vx = -self.pulley_radius * omega_g * np.sin(beta)
r_vy = self.pulley_radius * omega_g * np.cos(beta)
# acceleration
l_ax = -self.pulley_radius * (omega_g**2) * np.cos(-beta + np.pi) + self.pulley_radius * alpha_g * np.sin(-beta + np.pi)
l_ay = -self.pulley_radius * (omega_g**2) * np.sin(-beta + np.pi) - self.pulley_radius * alpha_g * np.cos(-beta + np.pi)
r_ax = -self.pulley_radius * (omega_g**2) * np.cos(beta) - self.pulley_radius * alpha_g * np.sin(beta)
r_ay = -self.pulley_radius * (omega_g**2) * np.sin(beta) + self.pulley_radius * alpha_g * np.cos(beta)
if showVelArrows:
self.ax.arrow(l_pully_x, l_pully_y, l_vx, l_vy,
head_width=0.005, width=0.0005,
length_includes_head=True,
linestyle='dashed', color='green', overhang=1.0)
self.ax.arrow(r_pully_x, r_pully_y, r_vx, r_vy,
head_width=0.005, width=0.0005,
length_includes_head=True,
linestyle='dashed', color='green', overhang=1.0)
if showAccArrows:
self.ax.arrow(l_pully_x, l_pully_y, l_ax, l_ay,
head_width=0.005, width=0.0005,
length_includes_head=True,
linestyle='dashed', color='green', overhang=1.0)
self.ax.arrow(r_pully_x, r_pully_y, r_ax, r_ay,
head_width=0.005, width=0.0005,
length_includes_head=True,
linestyle='dashed', color='green', overhang=1.0)
def slider_crank_pos(self, l2, l3, th_2, crossed: bool = False, offset=0):
if crossed:
theta_3_crossed = np.arcsin((l2 * np.sin(th_2) - offset) / (l3))
l1_pos = l2 * np.cos(th_2) - l3 * np.cos(theta_3_crossed)
return (theta_3_crossed, l1_pos)
else:
theta_3_open = np.arcsin(-(l2 * np.sin(th_2) - offset) / l3) + np.pi
l1_pos = l2 * np.cos(th_2) - l3 * np.cos(theta_3_open)
return (theta_3_open, l1_pos)
def slider_crank_vel(self, l2, l3, th2, th3, omega_2):
omega_3 = (l2 * omega_2 * np.cos(th2)) / (l3 * np.cos(th3))
l1_vel = l3 * omega_3 * np.sin(th3) - l2 * omega_2 * np.sin(th2)
return (omega_3, l1_vel)
def slider_crank_acc(self, l2, l3, th2, th3, omega_2, omega_3, alpha_2):
alpha_3 = (
(l2 * alpha_2 * np.cos(th2))
- (l2 * (omega_2**2) * np.sin(th2))
+ (l3 * (omega_3**2) * np.sin(th3))
) / (l3 * np.cos(th3))
l1_acc = (
(-l2 * alpha_2 * np.sin(th2))
- (l2 * (omega_2**2) * np.cos(th2))
+ (l3 * alpha_3 * np.sin(th3))
+ (l3 * (omega_3**2) * np.cos(th3))
)
return (alpha_3, l1_acc)
"""
Source: https://www.engineersedge.com/mechanics_machines/geneva_mechanism_design_equations__14919.htm
Geneva Mechanism portion written with the assistance of LLMs
"""
def geneva_kinematics(self, theta_driver):
step = 2 * np.pi / self.n_slots
engage_limit = np.pi / self.n_slots
# Wrap to current position in cycle
alpha_mod = theta_driver % (2 * np.pi)
alpha_eng = alpha_mod if alpha_mod <= np.pi else alpha_mod - 2 * np.pi
engaged = abs(alpha_eng) <= engage_limit
if engaged:
denom = 1 + self.m_geneva**2 - 2 * self.m_geneva * np.cos(alpha_eng)
beta_local = -np.arctan2(
np.sin(alpha_eng),
self.m_geneva - np.cos(alpha_eng)
)
omega_g = -self.omega_2 * (
(self.m_geneva * np.cos(alpha_eng) - 1) / denom
)
alpha_g = -(self.omega_2**2) * (
self.m_geneva * np.sin(alpha_eng) * (1 - self.m_geneva**2)
) / (denom**2)
else:
beta_local = -np.arctan2(
np.sin(engage_limit),
self.m_geneva - np.cos(engage_limit)
)
omega_g = 0.0
alpha_g = 0.0
# Calculate which slot based on continuous angle
# Each engagement ends at theta_driver = engage_limit, 2π + engage_limit, 4π + engage_limit, etc.
slot_index = int(np.floor((theta_driver + engage_limit) / (2 * np.pi)))
beta_total = -slot_index * step + beta_local
return beta_total, omega_g, alpha_g
# Function to animate the crank
def animate(self):
# Runs on the first animation frame
def init():
self.ax.clear()
self.ax.set_aspect('equal')
self.fig.set_tight_layout(True)
# Adds the four bar plotting function to the animation
def getFrames(frames):
self.window_washer_plot(frames, showVelArrows=False, showAccArrows=True)
# Animation function
delay_interval = round(1/abs(self.omega_2*180/np.pi)*2*1000)
theta_frames = np.linspace(0, 4*360, 4*360 + 1)
anim = FuncAnimation(self.fig, getFrames, init_func=init, frames=theta_frames, interval=delay_interval,repeat_delay=100)
# Save as GIF
anim.save('window_washer_acc.gif', dpi=200, writer='pillow')
if __name__ == '__main__':
mechanism = WindowWasherVisualize()
mechanism.animate() # Animates the mechanism and produces a GIF