1 files changed, 60 insertions, 0 deletions
diff --git a/tutorials/module_3/fourbar.py b/tutorials/module_3/fourbar.py
new file mode 100644
index 0000000..516fdce
--- /dev/null
+++ b/tutorials/module_3/fourbar.py
@@ -0,0 +1,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()
+\ No newline at end of file