Lab 03-01: Forward Kinematics of a 2-DOF Arm

Objectives

• Implement forward kinematics using DH parameters
• Visualize the robot workspace
• Verify FK calculations against simulation
• Understand the relationship between joint angles and end-effector position

Materials

Type	Name	Version	Tier
software	Python	3.10+	required
software	MuJoCo	3.0+	required
software	NumPy	1.24+	required
software	Matplotlib	3.5+	required

Background

Forward kinematics (FK) computes the end-effector position and orientation given joint angles. For a serial manipulator, this involves multiplying transformation matrices along the kinematic chain.

Instructions

Step 1: Define the 2-DOF Planar Arm

import numpy as np
import matplotlib.pyplot as plt

# Link lengths
L1 = 0.5  # First link length (m)
L2 = 0.4  # Second link length (m)

def forward_kinematics_2dof(theta1, theta2, l1=L1, l2=L2):
    """
    Compute end-effector position for a 2-DOF planar arm.

    Args:
        theta1: First joint angle (radians)
        theta2: Second joint angle (radians)
        l1, l2: Link lengths

    Returns:
        (x, y): End-effector position
        (x1, y1): First joint position (elbow)
    """
    # First joint (shoulder) is at origin
    x0, y0 = 0, 0

    # 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 (x2, y2), (x1, y1)

# Test with specific angles
theta1 = np.radians(45)  # 45 degrees
theta2 = np.radians(30)  # 30 degrees

end_pos, elbow_pos = forward_kinematics_2dof(theta1, theta2)
print(f"Joint angles: θ1={np.degrees(theta1):.1f}°, θ2={np.degrees(theta2):.1f}°")
print(f"Elbow position: ({elbow_pos[0]:.3f}, {elbow_pos[1]:.3f})")
print(f"End-effector position: ({end_pos[0]:.3f}, {end_pos[1]:.3f})")

Verification: For θ1=45°, θ2=30°:

• Elbow should be at approximately (0.354, 0.354)
• End-effector should be at approximately (0.554, 0.700)

Step 2: Visualize the Arm Configuration

def plot_arm(theta1, theta2, l1=L1, l2=L2, ax=None):
    """Plot the 2-DOF arm configuration."""
    if ax is None:
        fig, ax = plt.subplots(figsize=(8, 8))

    end_pos, elbow_pos = forward_kinematics_2dof(theta1, theta2, l1, l2)

    # Plot links
    ax.plot([0, elbow_pos[0]], [0, elbow_pos[1]], 'b-', linewidth=3, label='Link 1')
    ax.plot([elbow_pos[0], end_pos[0]], [elbow_pos[1], end_pos[1]], 'r-', linewidth=3, label='Link 2')

    # Plot joints
    ax.plot(0, 0, 'ko', markersize=10)  # Base
    ax.plot(elbow_pos[0], elbow_pos[1], 'ko', markersize=8)  # Elbow
    ax.plot(end_pos[0], end_pos[1], 'g^', markersize=12)  # End-effector

    # Formatting
    ax.set_xlim(-1, 1)
    ax.set_ylim(-1, 1)
    ax.set_aspect('equal')
    ax.grid(True, alpha=0.3)
    ax.axhline(y=0, color='k', linewidth=0.5)
    ax.axvline(x=0, color='k', linewidth=0.5)

    return ax

# Plot multiple configurations
fig, axes = plt.subplots(2, 2, figsize=(12, 12))

configs = [
    (0, 0, "Stretched out"),
    (np.pi/4, np.pi/4, "Both 45°"),
    (np.pi/2, -np.pi/2, "Folded back"),
    (np.pi/3, np.pi/6, "Reaching up")
]

for ax, (t1, t2, title) in zip(axes.flat, configs):
    plot_arm(t1, t2, ax=ax)
    ax.set_title(f"{title}\nθ1={np.degrees(t1):.0f}°, θ2={np.degrees(t2):.0f}°")

plt.tight_layout()
plt.savefig('arm_configurations.png', dpi=150)
plt.show()

Expected: Four subplots showing different arm configurations.

Step 3: Compute the Workspace

def compute_workspace(l1=L1, l2=L2, resolution=100):
    """Compute reachable workspace by sweeping joint angles."""
    theta1_range = np.linspace(-np.pi, np.pi, resolution)
    theta2_range = np.linspace(-np.pi, np.pi, resolution)

    workspace_points = []

    for t1 in theta1_range:
        for t2 in theta2_range:
            pos, _ = forward_kinematics_2dof(t1, t2, l1, l2)
            workspace_points.append(pos)

    return np.array(workspace_points)

# Compute and plot workspace
workspace = compute_workspace()

plt.figure(figsize=(10, 10))
plt.scatter(workspace[:, 0], workspace[:, 1], s=1, alpha=0.3, c='blue')

# Draw workspace boundaries
theta = np.linspace(0, 2*np.pi, 100)
outer_circle = (L1 + L2) * np.array([np.cos(theta), np.sin(theta)])
inner_circle = abs(L1 - L2) * np.array([np.cos(theta), np.sin(theta)])

plt.plot(outer_circle[0], outer_circle[1], 'r-', linewidth=2, label=f'Outer: r={L1+L2}')
plt.plot(inner_circle[0], inner_circle[1], 'g-', linewidth=2, label=f'Inner: r={abs(L1-L2)}')

plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title('2-DOF Arm Workspace')
plt.legend()
plt.axis('equal')
plt.grid(True, alpha=0.3)
plt.savefig('workspace.png', dpi=150)
plt.show()

print(f"Workspace boundaries:")
print(f"  Maximum reach: {L1 + L2:.2f} m")
print(f"  Minimum reach: {abs(L1 - L2):.2f} m")

Analysis: The workspace should be an annular region (donut shape) with:

• Outer radius = L1 + L2 = 0.9 m
• Inner radius = |L1 - L2| = 0.1 m

Step 4: Implement DH Convention

def dh_matrix(theta, d, a, alpha):
    """
    Compute the Denavit-Hartenberg transformation matrix.

    Args:
        theta: Joint angle (rotation about z)
        d: Link offset (translation along z)
        a: Link length (translation along x)
        alpha: Link twist (rotation about x)

    Returns:
        4x4 homogeneous transformation matrix
    """
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)

    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d],
        [0,   0,      0,     1]
    ])

def forward_kinematics_dh(joint_angles, dh_params):
    """
    Compute FK using DH parameters.

    Args:
        joint_angles: List of joint angles
        dh_params: List of (d, a, alpha) for each joint

    Returns:
        4x4 transformation matrix from base to end-effector
    """
    T = np.eye(4)

    for theta, (d, a, alpha) in zip(joint_angles, dh_params):
        T = T @ dh_matrix(theta, d, a, alpha)

    return T

# DH parameters for 2-DOF planar arm: (d, a, alpha)
# Joint 1: d=0, a=L1, alpha=0
# Joint 2: d=0, a=L2, alpha=0
dh_params = [
    (0, L1, 0),  # Joint 1
    (0, L2, 0),  # Joint 2
]

# Test with same angles as before
joint_angles = [np.radians(45), np.radians(30)]
T = forward_kinematics_dh(joint_angles, dh_params)

print("Transformation matrix:")
print(T)
print(f"\nEnd-effector position from DH: ({T[0,3]:.3f}, {T[1,3]:.3f})")

# Verify against direct calculation
end_pos, _ = forward_kinematics_2dof(joint_angles[0], joint_angles[1])
print(f"End-effector position direct:  ({end_pos[0]:.3f}, {end_pos[1]:.3f})")

Verification: Both methods should give the same end-effector position.

Step 5: Verify with MuJoCo Simulation

import mujoco

# Create 2-DOF arm model
arm_xml = """
<mujoco>
  <option gravity="0 0 0"/>

  <worldbody>
    <light diffuse="1 1 1" pos="0 0 2"/>
    <geom type="plane" size="2 2 0.1" rgba="0.8 0.8 0.8 1"/>

    <body name="link1" pos="0 0 0.5">
      <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0"/>
      <geom type="capsule" fromto="0 0 0 0.5 0 0" size="0.02" rgba="0 0 1 1"/>

      <body name="link2" pos="0.5 0 0">
        <joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0"/>
        <geom type="capsule" fromto="0 0 0 0.4 0 0" size="0.02" rgba="1 0 0 1"/>

        <site name="end_effector" pos="0.4 0 0" size="0.03" rgba="0 1 0 1"/>
      </body>
    </body>
  </worldbody>

  <actuator>
    <position name="act1" joint="joint1" kp="100"/>
    <position name="act2" joint="joint2" kp="100"/>
  </actuator>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(arm_xml)
data = mujoco.MjData(model)

# Set joint angles
data.qpos[0] = np.radians(45)
data.qpos[1] = np.radians(30)

# Forward step to update positions
mujoco.mj_forward(model, data)

# Get end-effector position from simulation
ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "end_effector")
ee_pos_sim = data.site_xpos[ee_site_id]

print(f"MuJoCo end-effector position: ({ee_pos_sim[0]:.3f}, {ee_pos_sim[1]:.3f})")
print(f"Analytical FK position:       ({end_pos[0]:.3f}, {end_pos[1]:.3f})")
print(f"Error: {np.linalg.norm(ee_pos_sim[:2] - np.array(end_pos)):.6f} m")

Verification: MuJoCo position should match analytical FK within numerical precision.

Expected Outcomes

1. Working FK implementation for 2-DOF arm
2. Visualization of arm configurations
3. Workspace analysis showing annular reachable region
4. DH parameter implementation
5. Verification against MuJoCo simulation

Rubric

Criterion	Points	Description
FK Implementation	25	Correct forward kinematics calculation
Visualization	20	Clear arm configuration plots
Workspace Analysis	20	Correct workspace boundaries
DH Convention	20	Proper DH matrix implementation
Simulation Verification	15	Matches MuJoCo results

Total: 100 points