Simulate a 2-Link Robot Arm
Goal: Build a 2-link planar robot arm simulator in Python. Implement forward kinematics, inverse kinematics, and PID joint control. Animate the arm reaching for target positions.
Prerequisites: Kinematics, PID Controller, State Space Representation, Motors and Actuators
What We’re Building
A planar arm with two revolute joints. Joint 1 at the base, joint 2 at the elbow. We’ll compute where the end-effector is (FK), solve for joint angles to reach a target (IK), and simulate PID-controlled joint motion.
θ₁ θ₂
●────────l₁────────●────────l₂────────→ (x, y)
base elbow end effector
Step 1: Forward Kinematics
Given joint angles → compute end-effector position:
import numpy as np
import matplotlib.pyplot as plt
def forward_kinematics(theta1, theta2, l1=1.0, l2=1.0):
"""Compute joint and end-effector positions."""
# Elbow position
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
# End-effector position
x2 = x1 + l2 * np.cos(theta1 + theta2)
y2 = y1 + l2 * np.sin(theta1 + theta2)
return (x1, y1), (x2, y2)
# Test: both joints at 45°
elbow, tip = forward_kinematics(np.pi/4, np.pi/4)
print(f"Elbow: ({elbow[0]:.3f}, {elbow[1]:.3f})")
print(f"Tip: ({tip[0]:.3f}, {tip[1]:.3f})")Step 2: Visualize the Arm
def draw_arm(theta1, theta2, l1=1.0, l2=1.0, target=None, ax=None):
"""Draw the arm configuration."""
if ax is None:
fig, ax = plt.subplots(1, 1, figsize=(6, 6))
elbow, tip = forward_kinematics(theta1, theta2, l1, l2)
# Draw links
ax.plot([0, elbow[0]], [0, elbow[1]], 'b-o', linewidth=3, markersize=8)
ax.plot([elbow[0], tip[0]], [elbow[1], tip[1]], 'r-o', linewidth=3, markersize=8)
if target:
ax.plot(*target, 'gx', markersize=15, markeredgewidth=3)
ax.set_xlim(-2.5, 2.5); ax.set_ylim(-2.5, 2.5)
ax.set_aspect('equal'); ax.grid(True)
ax.set_title(f'θ₁={np.degrees(theta1):.0f}°, θ₂={np.degrees(theta2):.0f}°')
# Draw a few configurations
fig, axes = plt.subplots(1, 3, figsize=(15, 5))
for ax, (t1, t2) in zip(axes, [(0.5, 0.5), (1.0, -0.5), (0.2, 1.5)]):
draw_arm(t1, t2, ax=ax)
plt.tight_layout(); plt.show()Step 3: Inverse Kinematics
Given a target (x, y) → compute joint angles. Uses the law of cosines:
def inverse_kinematics(x, y, l1=1.0, l2=1.0, elbow_up=True):
"""Compute joint angles for target (x, y). Returns None if unreachable."""
d = np.sqrt(x**2 + y**2)
# Check reachability
if d > l1 + l2 or d < abs(l1 - l2):
return None
# Law of cosines for θ₂
cos_theta2 = (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)
cos_theta2 = np.clip(cos_theta2, -1, 1)
if elbow_up:
theta2 = np.arccos(cos_theta2)
else:
theta2 = -np.arccos(cos_theta2)
# θ₁ from geometry
theta1 = np.arctan2(y, x) - np.arctan2(
l2 * np.sin(theta2), l1 + l2 * np.cos(theta2))
return theta1, theta2
# Test: reach for (1.0, 1.0)
result = inverse_kinematics(1.0, 1.0)
if result:
theta1, theta2 = result
print(f"θ₁={np.degrees(theta1):.1f}°, θ₂={np.degrees(theta2):.1f}°")
# Verify with FK
_, tip = forward_kinematics(theta1, theta2)
print(f"FK check: ({tip[0]:.4f}, {tip[1]:.4f}) — should be (1.0, 1.0)")Two Solutions
Most positions have two IK solutions (elbow-up and elbow-down):
fig, axes = plt.subplots(1, 2, figsize=(12, 5))
target = (1.0, 0.8)
for ax, elbow_up, label in zip(axes, [True, False], ['Elbow Up', 'Elbow Down']):
result = inverse_kinematics(*target, elbow_up=elbow_up)
if result:
draw_arm(*result, target=target, ax=ax)
ax.set_title(f'{label}: θ₁={np.degrees(result[0]):.0f}°, θ₂={np.degrees(result[1]):.0f}°')
plt.tight_layout(); plt.show()Step 4: Workspace Visualization
Show all reachable positions:
fig, ax = plt.subplots(figsize=(6, 6))
thetas = np.linspace(-np.pi, np.pi, 200)
points_x, points_y = [], []
for t1 in thetas:
for t2 in thetas:
_, (x, y) = forward_kinematics(t1, t2)
points_x.append(x)
points_y.append(y)
ax.scatter(points_x, points_y, s=0.1, alpha=0.3, c='blue')
ax.set_xlim(-2.5, 2.5); ax.set_ylim(-2.5, 2.5)
ax.set_aspect('equal'); ax.grid(True)
ax.set_title('Workspace (all reachable positions)')
plt.show()The workspace is an annular ring: outer radius = l1+l2, inner radius = |l1-l2|.
Step 5: PID Joint Control
Instead of teleporting to the IK solution, simulate motors driving the joints with PID controllers:
class JointPID:
def __init__(self, Kp=20, Ki=0.5, Kd=5, dt=0.01):
self.Kp, self.Ki, self.Kd = Kp, Ki, Kd
self.dt = dt
self.integral = 0.0
self.prev_error = 0.0
def update(self, target, current):
error = target - current
# Wrap angle to [-π, π]
error = (error + np.pi) % (2 * np.pi) - np.pi
self.integral += error * self.dt
derivative = (error - self.prev_error) / self.dt
self.prev_error = error
torque = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
return np.clip(torque, -50, 50) # motor torque limit
def simulate_arm(target_pos, duration=3.0, dt=0.01):
"""Simulate PID-controlled arm reaching for target."""
result = inverse_kinematics(*target_pos)
if result is None:
print("Target unreachable!")
return
target_t1, target_t2 = result
# Initial joint angles (arm hanging down)
t1, t2 = 0.0, 0.0
v1, v2 = 0.0, 0.0 # angular velocities
inertia = 0.5 # simplified inertia
pid1 = JointPID(dt=dt)
pid2 = JointPID(dt=dt)
steps = int(duration / dt)
history_t1, history_t2 = [], []
history_x, history_y = [], []
for _ in range(steps):
# PID computes torques
torque1 = pid1.update(target_t1, t1)
torque2 = pid2.update(target_t2, t2)
# Simple dynamics: torque = I * alpha
a1 = torque1 / inertia
a2 = torque2 / inertia
v1 += a1 * dt
v2 += a2 * dt
v1 *= 0.95 # damping (friction)
v2 *= 0.95
t1 += v1 * dt
t2 += v2 * dt
_, (x, y) = forward_kinematics(t1, t2)
history_t1.append(t1)
history_t2.append(t2)
history_x.append(x)
history_y.append(y)
return history_t1, history_t2, history_x, history_y
# Run simulation
target = (1.2, 0.8)
h_t1, h_t2, h_x, h_y = simulate_arm(target)
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 5))
# Joint angles over time
t = np.arange(len(h_t1)) * 0.01
result = inverse_kinematics(*target)
ax1.plot(t, np.degrees(h_t1), label='θ₁')
ax1.plot(t, np.degrees(h_t2), label='θ₂')
ax1.axhline(y=np.degrees(result[0]), color='b', linestyle='--', alpha=0.5)
ax1.axhline(y=np.degrees(result[1]), color='orange', linestyle='--', alpha=0.5)
ax1.set_xlabel('Time (s)'); ax1.set_ylabel('Angle (deg)'); ax1.legend(); ax1.grid()
ax1.set_title('Joint angles converging to IK solution')
# End-effector path
ax2.plot(h_x, h_y, 'b-', alpha=0.5, label='End-effector path')
ax2.plot(*target, 'rx', markersize=15, markeredgewidth=3, label='Target')
ax2.plot(h_x[-1], h_y[-1], 'go', markersize=10, label='Final position')
draw_arm(h_t1[-1], h_t2[-1], target=target, ax=ax2)
ax2.legend(); ax2.set_title('End-effector trajectory')
plt.tight_layout(); plt.show()Exercises
-
Path following: Make the end-effector trace a circle. Compute IK targets along the circle and update the PID targets at each step. Animate with
matplotlib.animation. -
Obstacle avoidance: Add a circular obstacle. If the IK solution collides with the obstacle, try the other elbow configuration. Visualize both solutions.
-
Jacobian control: Instead of computing IK analytically, use the Jacobian
Jto computeΔθ = J⁻¹ · Δxiteratively. Compare convergence speed with analytical IK. -
Dynamics: Replace the simplified dynamics with the actual 2-link arm equations of motion (mass matrix M, Coriolis C, gravity G):
M(θ)·θ̈ + C(θ,θ̇)·θ̇ + G(θ) = τ. This is the real State Space Representation for a robot arm.
This is the final tutorial in the current set. Return to Tutorials Roadmap for the full learning path.