Lab 04-01: Reading IMU Data in MuJoCo

Objectives

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

Access accelerometer and gyroscope data from a simulated IMU sensor
Understand the relationship between sensor readings and robot motion
Visualize IMU data in real-time during simulation
Implement basic noise filtering on sensor readings

Prerequisites

Completed Module 01 labs (MuJoCo basics)
Understanding of coordinate frames and transformations
Basic 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	Data processing and visualization
simulation	humanoid-sensors.xml	required	Humanoid model with sensors

Background

Inertial Measurement Units (IMUs) are fundamental sensors in robotics, combining:

Accelerometers: Measure linear acceleration (including gravity)
Gyroscopes: Measure angular velocity
Magnetometers (optional): Measure magnetic field for heading

In MuJoCo, sensor data is accessed through the data.sensordata array, which contains readings from all defined sensors in the order they appear in the model XML.

Instructions

Step 1: Understanding the Sensor Model

First, examine the sensor definitions in the humanoid model:

import mujoco
import numpy as np

# Load the model
model = mujoco.MjModel.from_xml_path("textbook/assets/robot-models/mujoco/humanoid-sensors.xml")
data = mujoco.MjData(model)

# List all sensors
print("Available sensors:")
for i in range(model.nsensor):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i)
    sensor_type = model.sensor_type[i]
    dim = model.sensor_dim[i]
    print(f"  {name}: type={sensor_type}, dim={dim}")

Expected: You should see sensors including torso_accelerometer (dim=3) and torso_gyroscope (dim=3).

Step 2: Reading Sensor Data

Access the IMU readings during simulation:

def get_imu_readings(model, data, accel_sensor_id, gyro_sensor_id):
    """
    Extract accelerometer and gyroscope readings from sensor data.

    Args:
        model: MuJoCo model
        data: MuJoCo data
        accel_sensor_id: Sensor ID for accelerometer
        gyro_sensor_id: Sensor ID for gyroscope

    Returns:
        Tuple of (acceleration [3], angular_velocity [3])
    """
    # Get sensor data addresses
    accel_adr = model.sensor_adr[accel_sensor_id]
    gyro_adr = model.sensor_adr[gyro_sensor_id]

    # Extract readings (each is 3D)
    acceleration = data.sensordata[accel_adr:accel_adr+3].copy()
    angular_velocity = data.sensordata[gyro_adr:gyro_adr+3].copy()

    return acceleration, angular_velocity

# Get sensor IDs
accel_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "torso_accelerometer")
gyro_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "torso_gyroscope")

# Step simulation and read sensors
mujoco.mj_step(model, data)
accel, gyro = get_imu_readings(model, data, accel_id, gyro_id)
print(f"Acceleration: {accel}")
print(f"Angular velocity: {gyro}")

Expected: Standing still, accelerometer should read approximately [0, 0, -9.81] (gravity in z-down frame).

Step 3: Collecting Time-Series Data

Run a simulation while collecting sensor data:

import matplotlib.pyplot as plt

def simulate_and_collect(model, data, duration_seconds=5.0):
    """
    Run simulation and collect IMU data.

    Returns:
        Dictionary with time, acceleration, and angular velocity arrays
    """
    dt = model.opt.timestep
    n_steps = int(duration_seconds / dt)

    # Pre-allocate arrays
    times = np.zeros(n_steps)
    accelerations = np.zeros((n_steps, 3))
    angular_velocities = np.zeros((n_steps, 3))

    # Reset simulation
    mujoco.mj_resetData(model, data)

    # Apply initial perturbation (push the robot)
    data.qvel[0] = 0.5  # Forward velocity

    for i in range(n_steps):
        mujoco.mj_step(model, data)

        times[i] = data.time
        accel, gyro = get_imu_readings(model, data, accel_id, gyro_id)
        accelerations[i] = accel
        angular_velocities[i] = gyro

    return {
        'time': times,
        'acceleration': accelerations,
        'angular_velocity': angular_velocities
    }

# Collect data
imu_data = simulate_and_collect(model, data, duration_seconds=3.0)

Expected: Data collection completes without errors. Arrays should have consistent shapes.

Step 4: Visualizing Sensor Data

Create plots to understand the sensor behavior:

def plot_imu_data(imu_data):
    """Plot accelerometer and gyroscope data."""
    fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)

    # Accelerometer plot
    axes[0].plot(imu_data['time'], imu_data['acceleration'][:, 0], label='X')
    axes[0].plot(imu_data['time'], imu_data['acceleration'][:, 1], label='Y')
    axes[0].plot(imu_data['time'], imu_data['acceleration'][:, 2], label='Z')
    axes[0].set_ylabel('Acceleration (m/s²)')
    axes[0].set_title('Accelerometer Readings')
    axes[0].legend()
    axes[0].grid(True)
    axes[0].axhline(y=-9.81, color='k', linestyle='--', alpha=0.3, label='Gravity')

    # Gyroscope plot
    axes[1].plot(imu_data['time'], imu_data['angular_velocity'][:, 0], label='Roll rate')
    axes[1].plot(imu_data['time'], imu_data['angular_velocity'][:, 1], label='Pitch rate')
    axes[1].plot(imu_data['time'], imu_data['angular_velocity'][:, 2], label='Yaw rate')
    axes[1].set_xlabel('Time (s)')
    axes[1].set_ylabel('Angular Velocity (rad/s)')
    axes[1].set_title('Gyroscope Readings')
    axes[1].legend()
    axes[1].grid(True)

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

plot_imu_data(imu_data)

Expected: Two subplot figure showing acceleration and angular velocity over time. Z-acceleration should hover around -9.81 m/s² when upright.

Step 5: Adding Sensor Noise

Real IMU sensors have noise. Implement a simple noise model:

class NoisyIMU:
    """
    Wrapper that adds realistic noise to IMU readings.

    Typical noise characteristics:
    - Accelerometer: ~0.01-0.1 m/s² standard deviation
    - Gyroscope: ~0.001-0.01 rad/s standard deviation
    """

    def __init__(self, model, data, accel_id, gyro_id,
                 accel_noise_std=0.05, gyro_noise_std=0.005,
                 accel_bias=None, gyro_bias=None):
        self.model = model
        self.data = data
        self.accel_id = accel_id
        self.gyro_id = gyro_id
        self.accel_noise_std = accel_noise_std
        self.gyro_noise_std = gyro_noise_std

        # Random bias (constant offset)
        self.accel_bias = accel_bias if accel_bias is not None else np.random.normal(0, 0.1, 3)
        self.gyro_bias = gyro_bias if gyro_bias is not None else np.random.normal(0, 0.01, 3)

    def read(self):
        """Get noisy IMU readings."""
        accel, gyro = get_imu_readings(self.model, self.data, self.accel_id, self.gyro_id)

        # Add Gaussian noise
        accel_noisy = accel + np.random.normal(0, self.accel_noise_std, 3) + self.accel_bias
        gyro_noisy = gyro + np.random.normal(0, self.gyro_noise_std, 3) + self.gyro_bias

        return accel_noisy, gyro_noisy

# Create noisy IMU
noisy_imu = NoisyIMU(model, data, accel_id, gyro_id)

# Compare clean vs noisy readings
mujoco.mj_step(model, data)
clean_accel, clean_gyro = get_imu_readings(model, data, accel_id, gyro_id)
noisy_accel, noisy_gyro = noisy_imu.read()

print(f"Clean acceleration: {clean_accel}")
print(f"Noisy acceleration: {noisy_accel}")
print(f"Difference: {noisy_accel - clean_accel}")

Expected: Noisy readings should differ from clean readings by small amounts (typically < 0.2 m/s² for acceleration).

Step 6: Simple Low-Pass Filter

Implement basic noise filtering:

class LowPassFilter:
    """
    Simple exponential moving average filter.

    y[n] = alpha * x[n] + (1 - alpha) * y[n-1]
    """

    def __init__(self, alpha=0.1, initial_value=None):
        """
        Args:
            alpha: Filter coefficient (0-1). Lower = more smoothing.
            initial_value: Initial filter state
        """
        self.alpha = alpha
        self.value = initial_value

    def update(self, measurement):
        """Update filter with new measurement."""
        if self.value is None:
            self.value = measurement.copy()
        else:
            self.value = self.alpha * measurement + (1 - self.alpha) * self.value
        return self.value.copy()

# Test filtering
accel_filter = LowPassFilter(alpha=0.1)
gyro_filter = LowPassFilter(alpha=0.1)

# Collect filtered data
n_samples = 500
raw_data = np.zeros((n_samples, 3))
filtered_data = np.zeros((n_samples, 3))

mujoco.mj_resetData(model, data)
for i in range(n_samples):
    mujoco.mj_step(model, data)
    noisy_accel, _ = noisy_imu.read()
    raw_data[i] = noisy_accel
    filtered_data[i] = accel_filter.update(noisy_accel)

# Plot comparison
plt.figure(figsize=(12, 4))
plt.plot(raw_data[:, 2], alpha=0.5, label='Raw Z-acceleration')
plt.plot(filtered_data[:, 2], label='Filtered Z-acceleration')
plt.axhline(y=-9.81, color='k', linestyle='--', alpha=0.3, label='True gravity')
plt.xlabel('Sample')
plt.ylabel('Acceleration (m/s²)')
plt.title('Low-Pass Filtering of Noisy IMU Data')
plt.legend()
plt.grid(True)
plt.savefig('filtered_imu.png', dpi=150)
plt.show()

Expected: Filtered signal should be smoother than raw signal, tracking close to -9.81 m/s² for Z-axis.

Expected Outcomes

After completing this lab, you should have:

Code artifacts:
- imu_reader.py: Functions for reading IMU data from MuJoCo
- noisy_imu.py: Noise model wrapper class
- filters.py: Low-pass filter implementation
Visual outputs:
- imu_data.png: Time-series plot of accelerometer and gyroscope data
- filtered_imu.png: Comparison of raw vs filtered sensor data
Understanding:
- How MuJoCo organizes sensor data
- The structure of IMU measurements
- Basic noise modeling and filtering techniques

Rubric

Criterion	Points	Description
Sensor access	20	Correctly reads accelerometer and gyroscope data
Data collection	20	Properly collects time-series data during simulation
Visualization	20	Creates clear, labeled plots of sensor data
Noise model	20	Implements realistic noise with bias and variance
Filtering	20	Low-pass filter reduces noise while preserving signal
Total	100

Common Errors

Error: Sensor ID not found Solution: Check sensor names in your XML model file. Use mujoco.mj_id2name() to list available sensors.

Error: Array index out of bounds when reading sensor data Solution: Verify sensor_adr and sensor_dim match your sensor configuration.

Error: Accelerometer shows [0, 0, 0] when robot is stationary Solution: Ensure the model has gravity enabled and the sensor is properly attached to a body.

Extensions

For advanced students:

Complementary Filter: Combine accelerometer and gyroscope data to estimate orientation
Allan Variance: Characterize sensor noise using Allan variance analysis
Multiple IMUs: Read from multiple IMU sensors placed at different body locations
Real-time Visualization: Create a live-updating plot during simulation

Theory: Module 04 theory.md, Section 4.1 (Inertial Measurement Units)
Next Lab: Lab 04-02 (Camera Image Processing)
Case Study: See ANYmal inspection case study for real-world IMU usage

Objectives​

Prerequisites​

Materials​

Background​

Instructions​

Step 1: Understanding the Sensor Model​

Step 2: Reading Sensor Data​

Step 3: Collecting Time-Series Data​

Step 4: Visualizing Sensor Data​

Step 5: Adding Sensor Noise​

Step 6: Simple Low-Pass Filter​

Expected Outcomes​

Rubric​

Common Errors​

Extensions​

Related Content​