Lab 05-02: Computed Torque Control

Objectives

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

• Understand the robot dynamics equation and its components
• Implement computed torque (inverse dynamics) control
• Compare computed torque to PID control performance
• Handle model uncertainty and its effects on control

Prerequisites

• Completed Lab 05-01 (PID Control)
• Understanding of rigid body dynamics (Module 02)
• Matrix algebra 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

Robot Dynamics Equation

The manipulator dynamics are described by:

$M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau$

Where:

• $M(q)$: Mass/inertia matrix (n×n)
• $C(q, \dot{q})$: Coriolis and centrifugal matrix
• $g(q)$: Gravity vector
• $\tau$: Joint torques

Computed Torque Control

The idea is to use a model of the robot to compute the torque that would achieve desired accelerations:

$\tau = M(q)(\ddot{q}_d + K_d \dot{e} + K_p e) + C(q, \dot{q})\dot{q} + g(q)$

This linearizes the system, allowing simple PD control to achieve desired dynamics.

Instructions

Step 1: Access MuJoCo Dynamics Functions

MuJoCo provides the dynamics matrices we need:

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)

def get_dynamics_matrices(model, data):
    """
    Extract dynamics matrices from MuJoCo.

    Returns:
        M: Mass matrix (nv x nv)
        bias: Coriolis + gravity forces (nv,)
    """
    nv = model.nv  # Number of velocity DOFs

    # Mass matrix
    M = np.zeros((nv, nv))
    mujoco.mj_fullM(model, M, data.qM)

    # Bias forces (Coriolis + centrifugal + gravity)
    # data.qfrc_bias contains these after mj_forward()
    mujoco.mj_forward(model, data)
    bias = data.qfrc_bias.copy()

    return M, bias

# Test
mujoco.mj_resetData(model, data)
data.qpos[:] = [0.5, 0.3]  # Set some joint positions
data.qvel[:] = [0.1, -0.2]  # Set some velocities

M, bias = get_dynamics_matrices(model, data)
print(f"Mass matrix M:\n{M}")
print(f"Bias forces (C*qdot + g): {bias}")

Expected: Mass matrix should be symmetric positive definite. Bias forces should be non-zero due to gravity.

Step 2: Implement Computed Torque Controller

Create the controller using inverse dynamics:

class ComputedTorqueController:
    """
    Computed torque (inverse dynamics) controller.

    Computes: τ = M(q)(q̈_d + Kd*ė + Kp*e) + C(q,q̇)q̇ + g(q)
    """

    def __init__(self, model, kp, kd):
        """
        Initialize controller.

        Args:
            model: MuJoCo model
            kp: Position gain (scalar or array)
            kd: Velocity gain (scalar or array)
        """
        self.model = model
        self.nv = model.nv

        # Convert gains to arrays
        self.kp = np.atleast_1d(kp) * np.ones(self.nv)
        self.kd = np.atleast_1d(kd) * np.ones(self.nv)

    def compute(self, data, q_desired, qd_desired, qdd_desired=None):
        """
        Compute control torque.

        Args:
            data: MuJoCo data (contains current state)
            q_desired: Desired positions
            qd_desired: Desired velocities
            qdd_desired: Desired accelerations (default zeros)

        Returns:
            tau: Joint torques
            info: Dictionary with debug information
        """
        if qdd_desired is None:
            qdd_desired = np.zeros(self.nv)

        # Current state
        q = data.qpos[:self.nv].copy()
        qd = data.qvel[:self.nv].copy()

        # Errors
        e = q_desired - q
        ed = qd_desired - qd

        # Desired acceleration with feedback
        qdd_command = qdd_desired + self.kd * ed + self.kp * e

        # Get dynamics matrices
        M, bias = get_dynamics_matrices(self.model, data)

        # Computed torque: τ = M*q̈_command + bias
        tau = M @ qdd_command + bias

        info = {
            'error': e,
            'error_dot': ed,
            'qdd_command': qdd_command,
            'M': M,
            'bias': bias
        }

        return tau, info

# Create controller
kp = 100  # Position gain
kd = 20   # Velocity gain (typically 2*sqrt(kp) for critical damping)
ctc = ComputedTorqueController(model, kp, kd)

print(f"Computed Torque Controller created")
print(f"Gains: Kp={ctc.kp}, Kd={ctc.kd}")

Expected: Controller created with proper gain arrays.

Step 3: Run Position Control

Test the controller on a setpoint regulation task:

def run_computed_torque_control(model, data, controller, q_target, duration=3.0):
    """
    Run computed torque control to reach a target position.

    Args:
        model, data: MuJoCo model and data
        controller: ComputedTorqueController instance
        q_target: Target joint positions
        duration: Simulation duration

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

    # Storage
    results = {
        'time': np.zeros(n_steps),
        'position': np.zeros((n_steps, nv)),
        'velocity': np.zeros((n_steps, nv)),
        'torque': np.zeros((n_steps, nv)),
        'error': np.zeros((n_steps, nv))
    }

    # Reset simulation
    mujoco.mj_resetData(model, data)

    # Desired state (stationary at target)
    q_desired = np.array(q_target)
    qd_desired = np.zeros(nv)

    for i in range(n_steps):
        # Compute control
        tau, info = controller.compute(data, q_desired, qd_desired)

        # Apply torque
        data.ctrl[:nv] = tau

        # Store data
        results['time'][i] = data.time
        results['position'][i] = data.qpos[:nv].copy()
        results['velocity'][i] = data.qvel[:nv].copy()
        results['torque'][i] = tau.copy()
        results['error'][i] = info['error'].copy()

        # Step simulation
        mujoco.mj_step(model, data)

    return results

# Run to target position
target = np.array([np.radians(45), np.radians(-30)])
results_ctc = run_computed_torque_control(model, data, ctc, target)

# Check performance
final_error = np.degrees(results_ctc['error'][-1])
print(f"Target: {np.degrees(target)} deg")
print(f"Final error: {final_error} deg")

Expected: Very small final errors (< 0.1 degree) due to model-based compensation.

Step 4: Compare with PID Control

Run the same task with PID for comparison:

# Import PID from Lab 05-01
class PIDController:
    def __init__(self, kp, ki, kd, dt, output_limits=(-np.inf, np.inf)):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.output_limits = output_limits
        self.integral = 0.0
        self.prev_error = 0.0
        self.first_call = True

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0
        self.first_call = True

    def compute(self, setpoint, measurement):
        error = setpoint - measurement
        p_term = self.kp * error
        self.integral += error * self.dt
        i_term = self.ki * self.integral
        d_term = 0 if self.first_call else self.kd * (error - self.prev_error) / self.dt
        self.first_call = False
        self.prev_error = error
        output = np.clip(p_term + i_term + d_term, *self.output_limits)
        return output

def run_pid_control_2dof(model, data, pid_list, q_target, duration=3.0):
    """Run independent PID control on each joint."""
    dt = model.opt.timestep
    n_steps = int(duration / dt)
    nv = model.nv

    results = {
        'time': np.zeros(n_steps),
        'position': np.zeros((n_steps, nv)),
        'velocity': np.zeros((n_steps, nv)),
        'torque': np.zeros((n_steps, nv)),
        'error': np.zeros((n_steps, nv))
    }

    mujoco.mj_resetData(model, data)
    for pid in pid_list:
        pid.reset()

    for i in range(n_steps):
        tau = np.zeros(nv)
        for j in range(nv):
            tau[j] = pid_list[j].compute(q_target[j], data.qpos[j])

        data.ctrl[:nv] = tau

        results['time'][i] = data.time
        results['position'][i] = data.qpos[:nv].copy()
        results['velocity'][i] = data.qvel[:nv].copy()
        results['torque'][i] = tau.copy()
        results['error'][i] = q_target - data.qpos[:nv]

        mujoco.mj_step(model, data)

    return results

# Create PID controllers (tuned for similar response)
pid_list = [
    PIDController(kp=100, ki=10, kd=20, dt=model.opt.timestep, output_limits=(-50, 50)),
    PIDController(kp=80, ki=8, kd=16, dt=model.opt.timestep, output_limits=(-30, 30))
]

results_pid = run_pid_control_2dof(model, data, pid_list, target)

# Compare
print("\nPerformance Comparison:")
print(f"CTC final error: {np.degrees(results_ctc['error'][-1])} deg")
print(f"PID final error: {np.degrees(results_pid['error'][-1])} deg")

Expected: CTC should achieve better performance, especially in coupled dynamics.

Step 5: Visualize Comparison

Create comparison plots:

def plot_comparison(results_ctc, results_pid, target):
    """Plot CTC vs PID comparison."""
    fig, axes = plt.subplots(2, 2, figsize=(14, 10))

    # Joint 0 position
    axes[0, 0].plot(results_ctc['time'], np.degrees(results_ctc['position'][:, 0]),
                    'b-', linewidth=2, label='CTC')
    axes[0, 0].plot(results_pid['time'], np.degrees(results_pid['position'][:, 0]),
                    'r--', linewidth=2, label='PID')
    axes[0, 0].axhline(y=np.degrees(target[0]), color='k', linestyle=':', alpha=0.5)
    axes[0, 0].set_ylabel('Position (deg)')
    axes[0, 0].set_title('Joint 0 Position')
    axes[0, 0].legend()
    axes[0, 0].grid(True)

    # Joint 1 position
    axes[0, 1].plot(results_ctc['time'], np.degrees(results_ctc['position'][:, 1]),
                    'b-', linewidth=2, label='CTC')
    axes[0, 1].plot(results_pid['time'], np.degrees(results_pid['position'][:, 1]),
                    'r--', linewidth=2, label='PID')
    axes[0, 1].axhline(y=np.degrees(target[1]), color='k', linestyle=':', alpha=0.5)
    axes[0, 1].set_ylabel('Position (deg)')
    axes[0, 1].set_title('Joint 1 Position')
    axes[0, 1].legend()
    axes[0, 1].grid(True)

    # Joint 0 torque
    axes[1, 0].plot(results_ctc['time'], results_ctc['torque'][:, 0],
                    'b-', linewidth=1.5, label='CTC')
    axes[1, 0].plot(results_pid['time'], results_pid['torque'][:, 0],
                    'r--', linewidth=1.5, label='PID')
    axes[1, 0].set_xlabel('Time (s)')
    axes[1, 0].set_ylabel('Torque (Nm)')
    axes[1, 0].set_title('Joint 0 Torque')
    axes[1, 0].legend()
    axes[1, 0].grid(True)

    # Joint 1 torque
    axes[1, 1].plot(results_ctc['time'], results_ctc['torque'][:, 1],
                    'b-', linewidth=1.5, label='CTC')
    axes[1, 1].plot(results_pid['time'], results_pid['torque'][:, 1],
                    'r--', linewidth=1.5, label='PID')
    axes[1, 1].set_xlabel('Time (s)')
    axes[1, 1].set_ylabel('Torque (Nm)')
    axes[1, 1].set_title('Joint 1 Torque')
    axes[1, 1].legend()
    axes[1, 1].grid(True)

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

plot_comparison(results_ctc, results_pid, target)

Expected: Plots showing CTC with faster settling and better decoupling between joints.

Step 6: Test Trajectory Tracking

Evaluate tracking performance on a sinusoidal trajectory:

def generate_trajectory(t, amplitude=0.5, frequency=0.5):
    """Generate sinusoidal joint trajectory."""
    q = amplitude * np.sin(2 * np.pi * frequency * t)
    qd = amplitude * 2 * np.pi * frequency * np.cos(2 * np.pi * frequency * t)
    qdd = -amplitude * (2 * np.pi * frequency)**2 * np.sin(2 * np.pi * frequency * t)
    return q, qd, qdd

def run_trajectory_tracking(model, data, controller, duration=5.0):
    """Run trajectory tracking with computed torque."""
    dt = model.opt.timestep
    n_steps = int(duration / dt)
    nv = model.nv

    results = {
        'time': np.zeros(n_steps),
        'position': np.zeros((n_steps, nv)),
        'desired': np.zeros((n_steps, nv)),
        'error': np.zeros((n_steps, nv)),
        'torque': np.zeros((n_steps, nv))
    }

    mujoco.mj_resetData(model, data)

    for i in range(n_steps):
        t = i * dt

        # Generate trajectory for both joints (with phase offset)
        q1, qd1, qdd1 = generate_trajectory(t, amplitude=0.6, frequency=0.3)
        q2, qd2, qdd2 = generate_trajectory(t + 0.5, amplitude=0.4, frequency=0.5)

        q_desired = np.array([q1, q2])
        qd_desired = np.array([qd1, qd2])
        qdd_desired = np.array([qdd1, qdd2])

        # Compute control with feedforward acceleration
        tau, info = controller.compute(data, q_desired, qd_desired, qdd_desired)
        data.ctrl[:nv] = tau

        # Store
        results['time'][i] = t
        results['position'][i] = data.qpos[:nv].copy()
        results['desired'][i] = q_desired.copy()
        results['error'][i] = info['error'].copy()
        results['torque'][i] = tau.copy()

        mujoco.mj_step(model, data)

    return results

# Run trajectory tracking
results_traj = run_trajectory_tracking(model, data, ctc, duration=5.0)

# Compute tracking error statistics
rms_error = np.sqrt(np.mean(results_traj['error']**2, axis=0))
max_error = np.max(np.abs(results_traj['error']), axis=0)

print(f"\nTrajectory Tracking Performance:")
print(f"RMS error: {np.degrees(rms_error)} deg")
print(f"Max error: {np.degrees(max_error)} deg")

# Plot
fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)

for j in range(2):
    axes[j].plot(results_traj['time'], np.degrees(results_traj['desired'][:, j]),
                 'r--', linewidth=2, label='Desired')
    axes[j].plot(results_traj['time'], np.degrees(results_traj['position'][:, j]),
                 'b-', linewidth=1.5, label='Actual')
    axes[j].set_ylabel(f'Joint {j} (deg)')
    axes[j].legend()
    axes[j].grid(True)

axes[0].set_title('Trajectory Tracking with Computed Torque Control')
axes[1].set_xlabel('Time (s)')

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

Expected: Actual trajectory should closely follow desired, with RMS error < 2 degrees.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • computed_torque.py: CTC controller implementation
   • trajectory_generator.py: Smooth trajectory generation

2. Visual outputs:

   • ctc_vs_pid.png: Comparison of CTC and PID performance
   • trajectory_tracking.png: Trajectory following results

3. Understanding:

   • How model-based control uses dynamics
   • Trade-offs between model-based and model-free control
   • Effect of feedforward terms on tracking

Rubric

Criterion	Points	Description
Dynamics Extraction	15	Correctly obtains M and bias from MuJoCo
CTC Implementation	30	Proper inverse dynamics computation
Setpoint Control	15	Achieves target with small error
Comparison	20	Fair comparison with PID control
Trajectory Tracking	20	Demonstrates feedforward benefit
Total	100

Common Errors

Error: Large torque spikes or instability Solution: Check that dynamics matrices are computed after mj_forward(). Ensure consistent state.

Error: Poor tracking despite correct implementation Solution: Increase gains Kp/Kd. CTC doesn't remove the need for feedback.

Error: CTC performs worse than PID Solution: Verify model parameters match simulation. Model mismatch degrades CTC performance.

Extensions

For advanced students:

1. Model Uncertainty: Add mass perturbations and observe robustness
2. Adaptive Control: Estimate parameters online to handle uncertainty
3. Operational Space Control: Implement task-space computed torque
4. Force Control: Combine with contact force regulation

Related Content

• Theory: Module 05 theory.md, Section 5.4 (Model-Based Control)
• Previous Lab: Lab 05-01 (PID Control)
• Next Lab: Lab 05-03 (Adaptive Control)