Lab 05-01: Implementing PID Control for a 2-DOF Arm

Objectives

By the end of this lab, you will be able to:

• Implement a PID (Proportional-Integral-Derivative) controller from scratch
• Tune PID gains using systematic methods
• Analyze step response characteristics (rise time, overshoot, settling time)
• Understand the effect of each gain component on system behavior

Prerequisites

• Completed Module 03 (Kinematics) - understanding of joint space
• Basic calculus (derivatives, integrals)
• Python programming with NumPy

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation
software	Python 3.10+	required	Programming environment
software	NumPy, Matplotlib	required	Computation and visualization
simulation	2dof-arm.xml	required	Two-link planar arm model

Background

PID Control

The PID controller computes a control signal based on the error between desired and actual states:

$u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt}$

Where:

• $K_p$: Proportional gain - responds to current error
• $K_i$: Integral gain - eliminates steady-state error
• $K_d$: Derivative gain - dampens oscillations

Discrete Implementation

For digital control at timestep $\Delta t$:

$u[k] = K_p e[k] + K_i \sum_{i=0}^{k} e[i] \Delta t + K_d \frac{e[k] - e[k-1]}{\Delta t}$

Instructions

Step 1: Set Up the Simulation Environment

Load the 2-DOF arm model and understand its structure:

import mujoco
import numpy as np
import matplotlib.pyplot as plt

# Load model
model = mujoco.MjModel.from_xml_path("textbook/assets/robot-models/mujoco/2dof-arm.xml")
data = mujoco.MjData(model)

# Print model information
print(f"Number of joints: {model.njnt}")
print(f"Number of actuators: {model.nu}")
print(f"Simulation timestep: {model.opt.timestep}")

# Get joint names and limits
for i in range(model.njnt):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i)
    jnt_range = model.jnt_range[i]
    print(f"Joint {i} ({name}): range [{np.degrees(jnt_range[0]):.1f}, {np.degrees(jnt_range[1]):.1f}] deg")

Expected: Model has 2 joints with reasonable range limits (e.g., ±180°).

Step 2: Implement the PID Controller Class

Create a reusable PID controller:

class PIDController:
    """
    Discrete PID controller with anti-windup.

    Implements the parallel form:
    u = Kp*e + Ki*integral(e) + Kd*derivative(e)
    """

    def __init__(self, kp, ki, kd, dt,
                 output_limits=(-np.inf, np.inf),
                 integral_limits=(-100, 100)):
        """
        Initialize PID controller.

        Args:
            kp: Proportional gain
            ki: Integral gain
            kd: Derivative gain
            dt: Sample time (seconds)
            output_limits: (min, max) for control output
            integral_limits: (min, max) for anti-windup
        """
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt
        self.output_limits = output_limits
        self.integral_limits = integral_limits

        # State variables
        self.integral = 0.0
        self.prev_error = 0.0
        self.first_call = True

    def reset(self):
        """Reset controller state."""
        self.integral = 0.0
        self.prev_error = 0.0
        self.first_call = True

    def compute(self, setpoint, measurement):
        """
        Compute control output.

        Args:
            setpoint: Desired value
            measurement: Current measured value

        Returns:
            Control output (torque command)
        """
        # Calculate error
        error = setpoint - measurement

        # Proportional term
        p_term = self.kp * error

        # Integral term with anti-windup
        self.integral += error * self.dt
        self.integral = np.clip(self.integral, *self.integral_limits)
        i_term = self.ki * self.integral

        # Derivative term (on error)
        if self.first_call:
            derivative = 0.0
            self.first_call = False
        else:
            derivative = (error - self.prev_error) / self.dt
        d_term = self.kd * derivative

        self.prev_error = error

        # Total output
        output = p_term + i_term + d_term

        # Apply output limits
        output = np.clip(output, *self.output_limits)

        return output, {'p': p_term, 'i': i_term, 'd': d_term, 'error': error}

# Create controller for one joint
dt = model.opt.timestep
pid_joint0 = PIDController(kp=100, ki=10, kd=20, dt=dt, output_limits=(-50, 50))

print("PID controller created")
print(f"Gains: Kp={pid_joint0.kp}, Ki={pid_joint0.ki}, Kd={pid_joint0.kd}")

Expected: PIDController class with proper initialization and methods.

Step 3: Control a Single Joint

Test the controller on joint 0:

def run_single_joint_control(model, data, pid, target_angle, duration=3.0):
    """
    Run PID control on joint 0.

    Args:
        model, data: MuJoCo model and data
        pid: PIDController instance
        target_angle: Desired joint angle (radians)
        duration: Simulation duration (seconds)

    Returns:
        Dictionary with time series data
    """
    dt = model.opt.timestep
    n_steps = int(duration / dt)

    # Storage
    times = np.zeros(n_steps)
    positions = np.zeros(n_steps)
    velocities = np.zeros(n_steps)
    torques = np.zeros(n_steps)
    errors = np.zeros(n_steps)

    # Reset
    mujoco.mj_resetData(model, data)
    pid.reset()

    for i in range(n_steps):
        # Get current state
        current_angle = data.qpos[0]
        current_vel = data.qvel[0]

        # Compute control
        torque, info = pid.compute(target_angle, current_angle)

        # Apply torque
        data.ctrl[0] = torque

        # Store data
        times[i] = data.time
        positions[i] = current_angle
        velocities[i] = current_vel
        torques[i] = torque
        errors[i] = info['error']

        # Step simulation
        mujoco.mj_step(model, data)

    return {
        'time': times,
        'position': positions,
        'velocity': velocities,
        'torque': torques,
        'error': errors,
        'target': target_angle
    }

# Run control to target of 45 degrees
target = np.radians(45)
result = run_single_joint_control(model, data, pid_joint0, target, duration=3.0)

# Check final error
final_error = np.degrees(result['error'][-1])
print(f"Target: 45 deg, Final error: {final_error:.3f} deg")

Expected: Final error should be small (< 1 degree) with proper PID gains.

Step 4: Analyze Step Response

Compute and visualize step response characteristics:

def analyze_step_response(time, response, target, settling_threshold=0.02):
    """
    Analyze step response characteristics.

    Args:
        time: Time array
        response: System response array
        target: Target/setpoint value
        settling_threshold: Percentage for settling time (default 2%)

    Returns:
        Dictionary with rise time, overshoot, settling time, steady-state error
    """
    # Normalize response
    initial = response[0]
    final = response[-1]

    # Rise time (10% to 90% of final value)
    response_10 = initial + 0.1 * (target - initial)
    response_90 = initial + 0.9 * (target - initial)

    idx_10 = np.argmax(response >= response_10)
    idx_90 = np.argmax(response >= response_90)

```python
```python
    rise_time = time[idx_90] - time[idx_10] if idx_90 > idx_10 else 0

    # Overshoot
    peak = np.max(response)
    overshoot = max(0, (peak - target) / (target - initial) * 100) if target != initial else 0

    # Settling time (within threshold of final value)
    settling_band = abs(target) * settling_threshold
```python
```python
    settled = np.abs(response - target) <= settling_band

    if np.any(settled):
        # Find first time it enters and stays in band
        for i in range(len(settled)):
            if np.all(settled[i:]):
                settling_time = time[i]
                break
        else:
            settling_time = time[-1]
    else:
        settling_time = time[-1]

    # Steady-state error
    steady_state_error = target - np.mean(response[-100:])

    return {
        'rise_time': rise_time,
        'overshoot_percent': overshoot,
        'settling_time': settling_time,
        'steady_state_error': steady_state_error
    }

# Analyze the response
metrics = analyze_step_response(
    result['time'],
    result['position'],
    target
)

print("\nStep Response Analysis:")
print(f"  Rise time: {metrics['rise_time']*1000:.1f} ms")
print(f"  Overshoot: {metrics['overshoot_percent']:.1f}%")
print(f"  Settling time: {metrics['settling_time']*1000:.1f} ms")
print(f"  Steady-state error: {np.degrees(metrics['steady_state_error']):.4f} deg")

Expected: Reasonable metrics - rise time < 500ms, overshoot < 30%, settling < 2s.

Step 5: Visualize the Response

Create publication-quality plots:

def plot_step_response(result, metrics):
    """Plot step response with annotations."""
    fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)

    time = result['time']
    target_deg = np.degrees(result['target'])

    # Position plot
    axes[0].plot(time, np.degrees(result['position']), 'b-', linewidth=2, label='Actual')
    axes[0].axhline(y=target_deg, color='r', linestyle='--', label='Target')
    axes[0].axhline(y=target_deg * 0.98, color='g', linestyle=':', alpha=0.5)
    axes[0].axhline(y=target_deg * 1.02, color='g', linestyle=':', alpha=0.5, label='±2% band')
    axes[0].set_ylabel('Position (deg)')
    axes[0].set_title(f'Step Response: Rise={metrics["rise_time"]*1000:.0f}ms, '
                      f'Overshoot={metrics["overshoot_percent"]:.1f}%, '
                      f'Settling={metrics["settling_time"]*1000:.0f}ms')
    axes[0].legend()
    axes[0].grid(True)

    # Velocity plot
    axes[1].plot(time, np.degrees(result['velocity']), 'b-', linewidth=1.5)
    axes[1].set_ylabel('Velocity (deg/s)')
    axes[1].grid(True)

    # Torque plot
    axes[2].plot(time, result['torque'], 'b-', linewidth=1.5)
    axes[2].set_xlabel('Time (s)')
    axes[2].set_ylabel('Torque (Nm)')
    axes[2].grid(True)

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

plot_step_response(result, metrics)

Expected: Three-panel plot showing position, velocity, and torque over time.

Step 6: Tune the Controller

Systematically vary gains to understand their effects:

def compare_gains(model, data, target, gain_sets, labels):
    """Compare different PID gain configurations."""
    fig, axes = plt.subplots(1, 2, figsize=(14, 5))

    for (kp, ki, kd), label in zip(gain_sets, labels):
        pid = PIDController(kp=kp, ki=ki, kd=kd, dt=model.opt.timestep, output_limits=(-50, 50))
        result = run_single_joint_control(model, data, pid, target, duration=2.0)

        axes[0].plot(result['time'], np.degrees(result['position']), label=label)
        axes[1].plot(result['time'], result['torque'], label=label)

    # Position plot
    axes[0].axhline(y=np.degrees(target), color='k', linestyle='--', alpha=0.5)
    axes[0].set_xlabel('Time (s)')
    axes[0].set_ylabel('Position (deg)')
    axes[0].set_title('Effect of PID Gains on Position Response')
    axes[0].legend()
    axes[0].grid(True)

    # Torque plot
    axes[1].set_xlabel('Time (s)')
    axes[1].set_ylabel('Torque (Nm)')
    axes[1].set_title('Effect of PID Gains on Control Effort')
    axes[1].legend()
    axes[1].grid(True)

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

# Compare different gain configurations
target = np.radians(45)
gain_sets = [
    (50, 0, 0),      # P only (underdamped)
    (100, 0, 0),     # Higher P (more oscillation)
    (100, 10, 0),    # P + I
    (100, 10, 20),   # P + I + D (well-tuned)
    (100, 10, 50),   # High D (overdamped)
]
labels = ['Kp=50', 'Kp=100', 'Kp=100 Ki=10', 'Kp=100 Ki=10 Kd=20', 'Kp=100 Ki=10 Kd=50']

compare_gains(model, data, target, gain_sets, labels)

Expected: Visual comparison showing how different gains affect response speed, overshoot, and damping.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • pid_controller.py: Reusable PID controller class
   • step_analysis.py: Functions for analyzing step response

2. Visual outputs:

   • pid_step_response.png: Annotated step response plot
   • pid_gain_comparison.png: Gain tuning comparison

3. Understanding:

   • How each PID term affects system behavior
   • Trade-offs between speed and stability
   • Systematic approaches to tuning

Rubric

Criterion	Points	Description
PID Implementation	25	Correct computation of P, I, D terms with anti-windup
Single Joint Control	20	Functional control loop achieving target position
Step Response Analysis	20	Correct computation of rise time, overshoot, settling time
Visualization	15	Clear, labeled plots with relevant information
Gain Tuning	20	Systematic comparison showing gain effects
Total	100

Common Errors

Error: System oscillates indefinitely Solution: Reduce Kp or increase Kd. High proportional gain without damping causes instability.

Error: Large steady-state error Solution: Add or increase Ki term. Pure P controllers have steady-state error under load.

Error: Slow response Solution: Increase Kp for faster response, but watch for overshoot.

Error: Actuator saturation (torque clipping) Solution: Reduce gains or increase output limits. Saturation causes integral windup.

Extensions

For advanced students:

1. Multi-joint Control: Extend to control both joints simultaneously
2. Trajectory Tracking: Follow a time-varying reference instead of step
3. Derivative on Measurement: Implement D term on measurement instead of error to avoid derivative kick
4. Auto-tuning: Implement Ziegler-Nichols or relay auto-tuning method

Related Content

• Theory: Module 05 theory.md, Section 5.2 (PID Control)
• Next Lab: Lab 05-02 (Computed Torque Control)
• Application: See Tesla Optimus case study for industrial motion control