1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
|
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from scipy.optimize import fsolve
# Link lengths (arbitrary units)
L1 = 4.0 # Ground link
L2 = 2.0 # Input crank
L3 = 3.0 # Coupler
L4 = 3.5 # Output rocker
# Fixed pivots
A = np.array([0, 0])
D = np.array([L1, 0])
# Input crank angle range
theta2_vals = np.linspace(0, 2 * np.pi, 360)
# Store previous angles to ensure continuity
previous_guess = [np.pi / 2, np.pi / 2]
# Solve joint positions using loop closure equations
def solve_positions(theta2):
global previous_guess
B = A + L2 * np.array([np.cos(theta2), np.sin(theta2)])
def loop_closure(vars):
theta3, theta4 = vars
eq1 = L2 * np.cos(theta2) + L3 * np.cos(theta3) - L4 * np.cos(theta4) - L1
eq2 = L2 * np.sin(theta2) + L3 * np.sin(theta3) - L4 * np.sin(theta4)
return [eq1, eq2]
theta3, theta4 = fsolve(loop_closure, previous_guess)
previous_guess = [theta3, theta4] # update guess for continuity
C = B + L3 * np.array([np.cos(theta3), np.sin(theta3)])
return A, B, C, D
# Set up plot
fig, ax = plt.subplots()
ax.set_xlim(-5, 8)
ax.set_ylim(-5, 5)
ax.set_aspect('equal')
line, = ax.plot([], [], 'o-', lw=2)
def init():
line.set_data([], [])
return line,
def update(frame):
A, B, C, D = solve_positions(theta2_vals[frame])
x_vals = [A[0], B[0], C[0], D[0]]
y_vals = [A[1], B[1], C[1], D[1]]
line.set_data(x_vals, y_vals)
return line,
ani = animation.FuncAnimation(fig, update, frames=len(theta2_vals), init_func=init,
blit=True, interval=20, repeat=True)
plt.title("Four-Bar Linkage Simulation")
plt.show()
|
