Physics Simulation and Sensor Simulation

Learning Objectives

By the end of this chapter, readers will be able to:

• Configure realistic physics parameters for humanoid robot simulation
• Implement LiDAR, depth camera, and IMU sensor simulations with appropriate noise models
• Apply sensor fusion techniques for enhanced perception
• Validate sensor outputs against expected real-world behavior
• Calibrate sensors and tune noise parameters for realistic simulation

Prerequisites

• Completion of Module 1: Foundations of Physical AI & Humanoid Robotics
• Completion of Module 2: Robotic Nervous System
• Chapters 1-2: Gazebo setup and robot modeling
• Basic understanding of probability and statistics
• Knowledge of sensor principles and characteristics

Introduction

Realistic physics simulation and sensor modeling are crucial for effective robot development. In the context of humanoid robotics, the interplay between accurate physics simulation and realistic sensor data determines the success of sim-to-real transfer. This chapter explores the principles and implementation of physics simulation and sensor simulation in Gazebo, focusing on creating environments that closely mirror real-world conditions.

The chapter covers both the theoretical foundations of physics simulation and practical implementation of various sensor types, including their noise characteristics and calibration techniques. Understanding these concepts is essential for creating trustworthy simulation environments that enable effective robot training and testing.

1. Physics Simulation Principles

1.1 Understanding the Physics Pipeline

Physics simulation in Gazebo operates through a multi-stage pipeline that transforms applied forces into motion:

1. Force Application: External forces, torques, and constraints are applied to bodies
2. Integration: Numerical integration methods compute velocities and positions
3. Collision Detection: Spatial algorithms detect intersecting objects
4. Contact Resolution: Constraint solvers compute contact forces
5. State Update: New positions and velocities are computed for the next timestep

1.2 Key Physics Parameters

Time Step Management:

<physics type='ode'>
  <max_step_size>0.001</max_step_size>  <!-- Simulation time per update -->
  <real_time_factor>1.0</real_time_factor>  <!-- Speed relative to real time -->
  <real_time_update_rate>1000.0</real_time_update_rate>  <!-- Updates per second -->
</physics>

Gravitational Field:

<gravity>0 0 -9.8</gravity>  <!-- Earth gravity: 9.8 m/s² downward -->

Solver Configuration:

<ode>
  <solver>
    <type>quick</type>  <!-- Type of constraint solver -->
    <iters>10</iters>    <!-- Iterations per timestep -->
    <sor>1.3</sor>      <!-- Successive Over Relaxation parameter -->
  </solver>
  <constraints>
    <cfm>0.0</cfm>      <!-- Constraint Force Mixing -->
    <erp>0.2</erp>      <!-- Error Reduction Parameter -->
    <contact_max_correcting_vel>100.0</contact_max_correcting_vel>
    <contact_surface_layer>0.001</contact_surface_layer>
  </constraints>
</ode>

1.3 Collision Detection Strategies

Gazebo offers multiple collision detection algorithms optimized for different scenarios:

Hash Space (Default): Efficient for medium-sized environments with moderate object density Quadtree/BSP: Better for sparse environments with large objects Octree: Optimal for 3D environments with uniform object distribution

Configuration example:

<physics type='ode'>
  <collision_detector>ode</collision_detector>  <!-- Use ODE collision detection -->
</physics>

2. Friction and Contact Modeling

2.1 Friction Coefficients

Friction is modeled using the Coulomb friction model with static and dynamic coefficients:

<surface>
  <friction>
    <ode>
      <mu>0.5</mu>      <!-- Primary friction coefficient -->
      <mu2>0.5</mu2>    <!-- Secondary friction coefficient -->
      <fdir1>0 0 0</fdir1>  <!-- Direction of mu -->
    </ode>
    <torsional>
      <coefficient>0.1</coefficient>
      <use_patch_radius>true</use_patch_radius>
      <surface_radius>0.01</surface_radius>
    </torsional>
  </friction>
  <contact>
    <ode>
      <soft_cfm>0.0</soft_cfm>
      <soft_erp>0.2</soft_erp>
      <kp>1e+6</kp>     <!-- Contact stiffness -->
      <kd>1e+3</kd>     <!-- Contact damping -->
      <max_vel>100.0</max_vel>
      <min_depth>0.001</min_depth>
    </ode>
  </contact>
</surface>

2.2 Realistic Surface Properties

Different materials require different friction properties:

Surface Type	μ (static)	μ₂ (dynamic)	Comments
Rubber on concrete	1.0	0.8	High traction for walking
Metal on metal	0.6	0.4	Moderate friction
Ice on ice	0.1	0.03	Low friction, challenging for locomotion
Wood on wood	0.4	0.3	Moderate friction

3. LiDAR Sensor Simulation

3.1 LiDAR Physics and Modeling

LiDAR (Light Detection and Ranging) sensors emit laser pulses and measure the time of flight to determine distances. In simulation, we model:

• Ray casting: Virtual laser rays are cast into the environment
• Reflection modeling: Surfaces reflect rays based on material properties
• Noise simulation: Realistic measurement errors are added
• Occlusion handling: Objects block laser beams appropriately

3.2 LiDAR Configuration in Gazebo

Basic LiDAR Sensor:

<sensor name="lidar_2d" type="ray">
  <ray>
    <scan>
      <horizontal>
        <samples>360</samples>           <!-- Number of rays -->
        <resolution>1</resolution>       <!-- Angular resolution -->
        <min_angle>-3.14159</min_angle>  <!-- Start angle (-π) -->
        <max_angle>3.14159</max_angle>   <!-- End angle (π) -->
      </horizontal>
    </scan>
    <range>
      <min>0.1</min>                   <!-- Minimum detectable range -->
      <max>10.0</max>                  <!-- Maximum detectable range -->
      <resolution>0.01</resolution>     <!-- Range resolution -->
    </range>
  </ray>
  <always_on>true</always_on>
  <update_rate>10</update_rate>         <!-- Hz -->
  <visualize>true</visualize>           <!-- Show ray visualization -->
</sensor>

3D LiDAR Configuration:

<sensor name="lidar_3d" type="ray">
  <ray>
    <scan>
      <horizontal>
        <samples>640</samples>
        <resolution>1</resolution>
        <min_angle>-3.14159</min_angle>
        <max_angle>3.14159</max_angle>
      </horizontal>
      <vertical>
        <samples>64</samples>
        <resolution>1</resolution>
        <min_angle>-0.5236</min_angle>  <!-- -30 degrees -->
        <max_angle>0.3491</max_angle>    <!-- +20 degrees -->
      </vertical>
    </scan>
    <range>
      <min>0.1</min>
      <max>50.0</max>
      <resolution>0.01</resolution>
    </range>
  </ray>
  <always_on>true</always_on>
  <update_rate>20</update_rate>
  <visualize>false</visualize>
</sensor>

3.3 LiDAR Noise Modeling

Realistic LiDAR noise includes several components:

<sensor name="lidar_noisy" type="ray">
  <ray>
    <scan>
      <horizontal>
        <samples>360</samples>
        <resolution>1</resolution>
        <min_angle>-3.14159</min_angle>
        <max_angle>3.14159</max_angle>
      </horizontal>
    </scan>
    <range>
      <min>0.1</min>
      <max>10.0</max>
      <resolution>0.01</resolution>
    </range>
    <!-- Noise modeling -->
    <noise>
      <type>gaussian</type>
      <mean>0.0</mean>
      <stddev>0.02</stddev>  <!-- 2cm standard deviation -->
    </noise>
  </ray>
  <always_on>true</always_on>
  <update_rate>10</update_rate>
</sensor>

4. Depth Camera Simulation

4.1 Depth Camera Principles

Depth cameras provide 3D point cloud data by measuring distances to objects in the scene. Common technologies include:

• Stereo vision: Uses triangulation between two cameras
• Structured light: Projects known patterns and analyzes deformation
• Time-of-flight: Measures light travel time to determine distance

4.2 Depth Camera Configuration

Basic Depth Camera:

<sensor name="depth_camera" type="depth">
  <camera>
    <horizontal_fov>1.047</horizontal_fov>  <!-- 60 degrees -->
    <image>
      <width>640</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.1</near>    <!-- Near clipping plane -->
      <far>10.0</far>     <!-- Far clipping plane -->
    </clip>
  </camera>
  <always_on>true</always_on>
  <update_rate>30</update_rate>
  <visualize>true</visualize>
</sensor>

Advanced Depth Camera with Noise:

<sensor name="noisy_depth_camera" type="depth">
  <camera>
    <horizontal_fov>1.047</horizontal_fov>
    <image>
      <width>640</width>
      <height>480</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.1</near>
      <far>10.0</far>
    </clip>
    <noise>
      <type>gaussian</type>
      <mean>0.0</mean>
      <stddev>0.007</stddev>  <!-- Depth noise: 7mm -->
    </noise>
  </camera>
  <always_on>true</always_on>
  <update_rate>30</update_rate>
  <visualize>true</visualize>
</sensor>

4.3 Point Cloud Generation

Depth cameras can also output point clouds:

<sensor name="point_cloud_camera" type="depth">
  <camera>
    <horizontal_fov>1.047</horizontal_fov>
    <image>
      <width>320</width>
      <height>240</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.1</near>
      <far>5.0</far>
    </clip>
  </camera>
  <always_on>true</always_on>
  <update_rate>15</update_rate>
  <point_cloud>
    <output>sensor</output>
    <point_cloud_min_dist>0.1</point_cloud_min_dist>
    <point_cloud_max_dist>5.0</point_cloud_max_dist>
  </point_cloud>
</sensor>

5. IMU Sensor Simulation

5.1 IMU Physics and Modeling

Inertial Measurement Units (IMUs) measure linear acceleration and angular velocity using microelectromechanical systems (MEMS). In simulation, we model:

• Accelerometer: Measures proper acceleration (including gravity)
• Gyroscope: Measures angular velocity around three axes
• Magnetometer: Measures magnetic field strength (often simulated separately)

5.2 IMU Configuration

Basic IMU Sensor:

<sensor name="imu_sensor" type="imu">
  <always_on>true</always_on>
  <update_rate>100</update_rate>
  <imu>
    <angular_velocity>
      <x>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>  <!-- rad/s -->
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.001</bias_stddev>
        </noise>
      </x>
      <y>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.001</bias_stddev>
        </noise>
      </y>
      <z>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.001</bias_stddev>
        </noise>
      </z>
    </angular_velocity>
    <linear_acceleration>
      <x>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>1.7e-2</stddev>  <!-- m/s² -->
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.1</bias_stddev>
        </noise>
      </x>
      <y>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>1.7e-2</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.1</bias_stddev>
        </noise>
      </y>
      <z>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>1.7e-2</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.1</bias_stddev>
        </noise>
      </z>
    </linear_acceleration>
  </imu>
</sensor>

5.3 Advanced IMU Features

IMU with Correlated Noise and Drift:

<sensor name="advanced_imu" type="imu">
  <always_on>true</always_on>
  <update_rate>200</update_rate>
  <imu>
    <angular_velocity>
      <x>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.0017</stddev>  <!-- Very low noise -->
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.001</bias_stddev>
          <dynamic_bias_correlation_time>100</dynamic_bias_correlation_time>
          <dynamic_bias_stddev>0.001</dynamic_bias_stddev>
        </noise>
      </x>
      <y>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.0017</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.001</bias_stddev>
          <dynamic_bias_correlation_time>100</dynamic_bias_correlation_time>
          <dynamic_bias_stddev>0.001</dynamic_bias_stddev>
        </noise>
      </y>
      <z>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.0017</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.001</bias_stddev>
          <dynamic_bias_correlation_time>100</dynamic_bias_correlation_time>
          <dynamic_bias_stddev>0.001</dynamic_bias_stddev>
        </noise>
      </z>
    </angular_velocity>
    <linear_acceleration>
      <x>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.017</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.01</bias_stddev>
          <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
          <dynamic_bias_stddev>0.01</dynamic_bias_stddev>
        </noise>
      </x>
      <y>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.017</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.01</bias_stddev>
          <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
          <dynamic_bias_stddev>0.01</dynamic_bias_stddev>
        </noise>
      </y>
      <z>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.017</stddev>
          <bias_mean>0.0</bias_mean>
          <bias_stddev>0.01</bias_stddev>
          <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
          <dynamic_bias_stddev>0.01</dynamic_bias_stddev>
        </noise>
      </z>
    </linear_acceleration>
  </imu>
</sensor>

6. Realistic Noise Models and Sensor Calibration

6.1 Noise Characterization

Real sensors exhibit various types of noise:

White Gaussian Noise: Random, zero-mean noise added to measurements Bias: Systematic offset that remains constant over time Drift: Slow variation in bias over extended periods Quantization: Discrete representation errors in digital sensors

6.2 Noise Parameter Selection

For realistic simulation, noise parameters should match real sensor specifications:

LiDAR Noise: Typically 1-3 cm standard deviation for indoor sensors Camera Noise: Varies with distance, often 1-5 mm at 1 meter IMU Noise: Depends on grade - consumer (higher) vs. tactical (lower)

6.3 Calibration Procedures

LiDAR Calibration:

# Collect overlapping scans from different poses
ros2 run scan_matching_calibration calibrate_lidar --input scans.bag

# Estimate extrinsic parameters
ros2 run lidar_extrinsic_calibration estimate_transform \
  --lidar_points lidar_scan \
  --reference_points checkerboard_points

Camera-LiDAR Fusion Calibration:

# Use calibration targets visible to both sensors
ros2 run camera_lidar_calibration calibrate \
  --image_topic /camera/image_raw \
  --lidar_topic /lidar/points \
  --calibration_board apriltag_board

7. Sensor Fusion Techniques

7.1 Kalman Filtering

Kalman filters optimally combine measurements from multiple sensors:

import numpy as np
from scipy.linalg import inv

class ExtendedKalmanFilter:
    def __init__(self, dim_x, dim_z):
        self.x = np.zeros(dim_x)  # State vector
        self.P = np.eye(dim_x)    # Covariance matrix
        self.Q = np.eye(dim_x)    # Process noise
        self.R = np.eye(dim_z)    # Measurement noise
        self.H = np.zeros((dim_z, dim_x))  # Observation matrix

    def predict(self, F, Q=None):
        """Predict next state"""
        if Q is not None:
            self.Q = Q
        self.x = F @ self.x
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z, R=None):
        """Update state with measurement"""
        if R is not None:
            self.R = R

        # Innovation
        y = z - self.H @ self.x
        # Innovation covariance
        S = self.H @ self.P @ self.H.T + self.R
        # Kalman gain
        K = self.P @ self.H.T @ inv(S)

        # Update state and covariance
        self.x = self.x + K @ y
        I_KH = np.eye(len(self.x)) - K @ self.H
        self.P = I_KH @ self.P

7.2 IMU-Accelerometer Fusion

Combining IMU and accelerometer data for attitude estimation:

class ComplementaryFilter:
    def __init__(self, alpha=0.98):
        self.alpha = alpha
        self.pitch = 0.0
        self.roll = 0.0
        self.dt = 0.01  # Time step

    def update(self, gyro_x, gyro_y, acc_x, acc_y, acc_z):
        # Integrate gyroscope readings
        delta_pitch = gyro_x * self.dt
        delta_roll = gyro_y * self.dt

        # Calculate angles from accelerometer
        acc_pitch = np.arctan2(acc_y, acc_z)
        acc_roll = np.arctan2(-acc_x, np.sqrt(acc_y**2 + acc_z**2))

        # Complementary filter
        self.pitch = self.alpha * (self.pitch + delta_pitch) + (1 - self.alpha) * acc_pitch
        self.roll = self.alpha * (self.roll + delta_roll) + (1 - self.alpha) * acc_roll

        return self.pitch, self.roll

8. Sensor Validation and Testing

8.1 Validation Approaches

Ground Truth Comparison: Compare sensor outputs with known ground truth Cross-validation: Compare different sensors measuring same phenomena Consistency Checking: Verify temporal and spatial consistency Statistical Analysis: Analyze noise characteristics and distributions

8.2 Validation Metrics

Precision and Accuracy:

• RMSE (Root Mean Square Error): Overall measurement error
• MAE (Mean Absolute Error): Average magnitude of errors
• Standard Deviation: Consistency of measurements
• Bias: Systematic offset in measurements

Temporal Characteristics:

• Latency: Time delay between event and measurement
• Jitter: Variation in measurement timing
• Bandwidth: Frequency response of sensor

8.3 Validation Tools and Techniques

Simulation-based Validation:

def validate_lidar_performance(true_distances, measured_distances):
    """Validate LiDAR performance against ground truth"""
    rmse = np.sqrt(np.mean((true_distances - measured_distances)**2))
    mae = np.mean(np.abs(true_distances - measured_distances))
    std_dev = np.std(measured_distances - true_distances)

    print(f"LiDAR Validation Results:")
    print(f"  RMSE: {rmse:.3f} m")
    print(f"  MAE: {mae:.3f} m")
    print(f"  Std Dev: {std_dev:.3f} m")
    print(f"  Bias: {np.mean(measured_distances - true_distances):.3f} m")

    return rmse, mae, std_dev

Statistical Analysis:

def analyze_sensor_noise(measurements, expected_value=None):
    """Analyze sensor noise characteristics"""
    mean_val = np.mean(measurements)
    std_val = np.std(measurements)
    variance = np.var(measurements)

    # Shapiro-Wilk test for normality
    from scipy.stats import shapiro
    stat, p_value = shapiro(measurements[:5000])  # Max 5000 samples

    print(f"Sensor Noise Analysis:")
    print(f"  Mean: {mean_val:.6f}")
    print(f"  Std: {std_val:.6f}")
    print(f"  Variance: {variance:.6f}")
    print(f"  Normal Distribution (p-value): {p_value:.4f}")

    return mean_val, std_val, variance, p_value

9. Performance Optimization

9.1 Sensor Update Rate Optimization

Balance between responsiveness and computational load:

• High-rate sensors (IMU: 100-1000Hz): Critical for control
• Medium-rate sensors (Camera: 15-60Hz): Suitable for perception
• Low-rate sensors (GPS: 1-10Hz): Adequate for positioning

9.2 Computational Efficiency

Point Cloud Downsampling:

def downsample_point_cloud(points, voxel_size=0.01):
    """Downsample point cloud using voxel grid filter"""
    if len(points) == 0:
        return points

    # Create voxel grid
    voxels = {}
    for point in points:
        voxel_coords = tuple(np.floor(point / voxel_size).astype(int))
        if voxel_coords not in voxels:
            voxels[voxel_coords] = point

    return np.array(list(voxels.values()))

Multi-threaded Sensor Processing:

import threading
import queue

class MultiThreadedSensorProcessor:
    def __init__(self):
        self.sensor_queue = queue.Queue()
        self.result_queue = queue.Queue()
        self.running = True

    def process_sensor_data(self):
        """Process sensor data in separate thread"""
        while self.running:
            if not self.sensor_queue.empty():
                sensor_data = self.sensor_queue.get()
                processed_data = self._process_single_sensor(sensor_data)
                self.result_queue.put(processed_data)

    def start_processing(self):
        self.process_thread = threading.Thread(target=self.process_sensor_data)
        self.process_thread.start()

10. Troubleshooting Common Issues

10.1 Sensor Performance Issues

High Latency:

• Reduce sensor update rate
• Optimize rendering settings
• Use simpler collision geometries

Inconsistent Measurements:

• Check for simulation instability
• Verify physics parameters
• Adjust solver settings

Excessive Noise:

• Review noise parameters
• Check for numerical instabilities
• Verify contact properties

10.2 Physics Simulation Issues

Object Penetration:

<!-- Increase constraint force mixing -->
<physics type='ode'>
  <constraints>
    <cfm>1e-5</cfm>  <!-- Small but not zero -->
    <erp>0.8</erp>   <!-- Higher error reduction -->
  </constraints>
</physics>

Instability:

<!-- Reduce time step and increase iterations -->
<physics type='ode'>
  <max_step_size>0.0005</max_step_size>  <!-- Smaller step -->
  <ode>
    <solver>
      <iters>100</iters>  <!-- More iterations -->
    </solver>
  </ode>
</physics>

11. Best Practices for Realistic Simulation

11.1 Sensor Placement

• Position sensors to match real robot configurations
• Consider field of view and occlusion
• Account for mounting offsets and orientations

11.2 Environmental Factors

• Model lighting conditions realistically
• Include dynamic elements (moving objects, changing illumination)
• Consider environmental disturbances (wind, vibrations)

11.3 Validation Against Reality

• Compare simulation results with real robot data
• Use identical control algorithms in sim and reality
• Maintain consistent coordinate frames and units

12. Summary

This chapter covered the fundamentals of physics simulation and sensor modeling:

• Physics simulation principles including time stepping, collision detection, and contact resolution
• Realistic friction and contact modeling for humanoid robot applications
• Comprehensive coverage of LiDAR, depth camera, and IMU simulation
• Noise modeling and sensor calibration techniques
• Sensor fusion approaches for enhanced perception
• Validation methodologies and performance optimization
• Troubleshooting common simulation issues

With realistic physics and sensor simulation, you can now create comprehensive simulation environments that closely mirror real-world conditions, enabling effective robot development and testing.

13. Exercises and Practice

Complete the following exercises to reinforce your understanding of physics simulation and sensor modeling:

1. Chapter 3 Exercises - Practice problems covering sensor simulation, noise modeling, and fusion techniques
2. Chapter 3 Solutions - Complete implementations and solution guides