Chapter 7: Gazebo Physics Simulation

Introduction

Gazebo is a leading open-source 3D simulation environment for robotics, providing realistic physics simulation, sensor modeling, and rendering capabilities. It serves as a critical tool in the robotics development pipeline, enabling algorithm testing, system integration, and safe exploration of robotic behaviors before deployment in the real world. This chapter explores the fundamentals of Gazebo physics simulation, from basic physics concepts to advanced plugin development.

Gazebo bridges the gap between algorithm development and real-world deployment by providing a realistic physics environment where robots can be tested, trained, and validated without risk to hardware or personnel.

7.1 Gazebo Architecture Overview

7.1.1 System Architecture

Gazebo uses a modular architecture that separates physics, rendering, sensors, and GUI components:

Diagram: Gazebo Architecture

┌─────────────────────────────────────────────────────────────┐
│                     Gazebo Client                           │
│  ┌─────────┐  ┌─────────┐  ┌─────────┐  ┌─────────┐        │
│  │   GUI   │  │   API   │  │   CLI   │  │Plugins  │        │
│  │         │  │         │  │         │  │         │        │
│  └─────────┘  └─────────┘  └─────────┘  └─────────┘        │
└─────────────────────────────────────────────────────────────┘
                              ↕
┌─────────────────────────────────────────────────────────────┐
│                     Gazebo Server                           │
│  ┌─────────────────────────────────────────────────────────┐ │
│  │                  Physics Engine                          │ │
│  │  ┌─────────┐  ┌─────────┐  ┌─────────┐                 │ │
│  │  │   ODE   │  │  Bullet │  │  DART   │                 │ │
│  │  └─────────┘  └─────────┘  └─────────┘                 │ │
│  └─────────────────────────────────────────────────────────┘ │
│  ┌─────────────────────────────────────────────────────────┐ │
│  │                 Rendering Engine                         │ │
│  │  ┌─────────┐  ┌─────────┐  ┌─────────┐                 │ │
│  │  │  Ogre   │  │  OpenGL │  │   RTX   │                 │ │
│  │  └─────────┘  └─────────┘  └─────────┘                 │ │
│  └─────────────────────────────────────────────────────────┘ │
│  ┌─────────────────────────────────────────────────────────┐ │
│  │                Sensor Systems                           │ │
│  │  ┌─────────┐  ┌─────────┐  ┌─────────┐                 │ │
│  │  │ Camera  │  │  LiDAR  │   │   IMU   │                 │ │
│  │  └─────────┘  └─────────┘  └─────────┘                 │ │
│  └─────────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────┘

7.1.2 Physics Engine Options

Gazebo supports multiple physics engines, each with different characteristics:

ODE (Open Dynamics Engine)

• Most mature and stable
• Good for general-purpose simulation
• Limited support for complex contacts

Bullet Physics

• Modern features and optimizations
• Better soft body support
• GPU acceleration available

DART (Dynamic Animation and Robotics Toolkit)

• Designed for robotics simulation
• Excellent for articulated bodies
• Advanced collision detection

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="physics_test">

    <!-- Physics engine configuration -->
    <physics name="1ms" type="ode">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>

      <!-- ODE specific settings -->
      <ode>
        <solver>
          <type>quick</type>
          <iters>10</iters>
          <sor>1.3</sor>
          <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
        </solver>
        <constraints>
          <cfm>0</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>100</contact_max_correcting_vel>
          <contact_surface_layer>0.001</contact_surface_layer>
        </constraints>
      </ode>

      <!-- Gravity configuration -->
      <gravity>0 0 -9.8066</gravity>
      <magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
    </physics>

  </world>
</sdf>

7.2 Physics Engine Fundamentals

7.2.1 Rigid Body Dynamics

The physics engine implements Newton's laws for rigid bodies:

$$\mathbf{F} = m\mathbf{a}$$ $$\mathbf{\tau} = I\mathbf{\alpha}$$

Where:

• $\mathbf{F}$ - Force vector
• $m$ - Mass
• $\mathbf{a}$ - Linear acceleration
• $\mathbf{\tau}$ - Torque vector
• $I$ - Inertia tensor
• $\mathbf{\alpha}$ - Angular acceleration

7.2.2 Collision Detection and Response

Gazebo implements collision detection in multiple phases:

Diagram: Collision Detection Pipeline

Broad Phase Collision Detection
┌─────────────────────────────────┐
│ Spatial Partitioning             │
│ - AABB trees                    │
│ - Octree/BVH                    │
│ - Sweep and Prune               │
└─────────────────────────────────┘
              ↓
Narrow Phase Collision Detection
┌─────────────────────────────────┐
│ Precise Collision Testing       │
│ - GJK (Gilbert-Johnson-Keerthi) │
│ - EPA (Expanding Polytope)      │
│ - SAT (Separating Axis Theorem) │
└─────────────────────────────────┘
              ↓
Collision Response
┌─────────────────────────────────┐
│ Physics Resolution              │
│ - Impulse-based response       │
│ - Contact force calculation     │
│ - Friction modeling             │
└─────────────────────────────────┘

7.2.3 Contact and Friction Models

Contact Forces

$$\mathbf{F}_{contact} = \mathbf{F}_{normal} + \mathbf{F}_{friction}$$

Normal Force

$$\mathbf{F}_{normal} = k_{penetration} \cdot \delta + d_{penetration} \cdot \dot{\delta}$$

Friction Force

$$\mathbf{F}_{friction} = \mu \cdot |\mathbf{F}_{normal}| \cdot \hat{\mathbf{v}}_{tangential}$$

Example: Custom Contact Plugin

#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>

namespace gazebo {
class CustomContactPlugin : public ModelPlugin {
public:
    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override {
        this->model = _parent;
        this->world = _parent->GetWorld();

        // Connect to contact event
        this->contactManager = this->world->Physics()->GetContactManager();
        this->connection = this->contactManager->ConnectContactResponse(
            std::bind(&CustomContactPlugin::OnContact, this,
            std::placeholders::_1));

        ROS_INFO("Custom contact plugin loaded");
    }

private:
    void OnContact(const msgs::Contact &_msg) {
        // Process contact information
        for (int i = 0; i < _msg.contact_size(); ++i) {
            const msgs::Contact &contact = _msg.contact(i);

            // Extract collision names
            std::string collision1 = contact.collision1();
            std::string collision2 = contact.collision2();

            // Calculate contact force
            ignition::math::Vector3d force(
                contact.wrench(0).force().x(),
                contact.wrench(0).force().y(),
                contact.wrench(0).force().z()
            );

            // Custom contact handling
            ProcessContact(collision1, collision2, force);
        }
    }

    void ProcessContact(const std::string &col1, const std::string &col2,
                       const ignition::math::Vector3d &force) {
        // Custom contact processing logic
        double force_magnitude = force.Length();

        if (force_magnitude > this->contact_threshold) {
            ROS_INFO("High force contact: %s - %s, Force: %.2f N",
                     col1.c_str(), col2.c_str(), force_magnitude);
        }
    }

    physics::ModelPtr model;
    physics::WorldPtr world;
    physics::ContactManagerPtr contactManager;
    event::ConnectionPtr connection;
    double contact_threshold = 10.0; // Newtons
};

GZ_REGISTER_MODEL_PLUGIN(CustomContactPlugin)
}

7.3 Robot Modeling in Gazebo

7.3.1 URDF to SDF Conversion

Gazebo uses Simulation Description Format (SDF) while ROS typically uses URDF. The conversion process adds simulation-specific properties:

Diagram: URDF to SDF Enhancement

URDF (Robot Description)
├── Links (geometry, visual)
├── Joints (kinematics)
└── Materials (appearance)
              ↓
Conversion Process
              ↓
SDF (Simulation Description)
├── URDF properties (preserved)
├── Collision geometry (added)
├── Physical properties (mass, inertia)
├── Sensor definitions (added)
├── Plugin configurations (added)
├── Material properties (physics)
└── World positioning (added)

7.3.2 Complete Robot Model Example

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="mobile_robot">

    <!-- Base link -->
    <link name="base_link">
      <inertial>
        <mass>20.0</mass>
        <inertia>
          <ixx>1.0</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>1.0</iyy>
          <iyz>0.0</iyz>
          <izz>1.0</izz>
        </inertia>
      </inertial>

      <visual name="base_visual">
        <geometry>
          <box>
            <size>0.5 0.4 0.2</size>
          </box>
        </geometry>
        <material>
          <ambient>0.8 0.8 0.8 1</ambient>
          <diffuse>0.8 0.8 0.8 1</diffuse>
        </material>
      </visual>

      <collision name="base_collision">
        <geometry>
          <box>
            <size>0.5 0.4 0.2</size>
          </box>
        </geometry>
        <surface>
          <friction>
            <ode>
              <mu>0.9</mu>
              <mu2>0.9</mu2>
            </ode>
          </friction>
          <contact>
            <ode>
              <max_vel>100</max_vel>
              <min_depth>0.01</min_depth>
            </ode>
          </contact>
        </surface>
      </collision>
    </link>

    <!-- Left wheel -->
    <link name="left_wheel">
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.1</ixx>
          <iyy>0.1</iyy>
          <izz>0.1</izz>
        </inertia>
        <pose>0 0 0 0 0 0</pose>
      </inertial>

      <visual name="left_wheel_visual">
        <geometry>
          <cylinder>
            <radius>0.1</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <pose>0.15 -0.25 0 0 1.5708 0</pose>
        <material>
          <ambient>0.2 0.2 0.2 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
        </material>
      </visual>

      <collision name="left_wheel_collision">
        <geometry>
          <cylinder>
            <radius>0.1</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <pose>0.15 -0.25 0 0 1.5708 0</pose>
        <surface>
          <friction>
            <ode>
              <mu>1.0</mu>
              <mu2>1.0</mu2>
            </ode>
          </friction>
        </surface>
      </collision>
    </link>

    <!-- Left wheel joint -->
    <joint name="left_wheel_joint" type="revolute">
      <parent>base_link</parent>
      <child>left_wheel</child>
      <pose>0 0 0 0 0 0</pose>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>

    <!-- Right wheel (similar structure) -->
    <link name="right_wheel">
      <!-- Similar to left wheel -->
    </link>

    <joint name="right_wheel_joint" type="revolute">
      <parent>base_link</parent>
      <child>right_wheel</child>
      <pose>0 0 0 0 0 0</pose>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>

    <!-- Caster wheel -->
    <link name="caster_wheel">
      <inertial>
        <mass>0.5</mass>
        <inertia>
          <ixx>0.01</ixx>
          <iyy>0.01</iyy>
          <izz>0.01</izz>
        </inertia>
      </inertial>

      <visual name="caster_visual">
        <geometry>
          <sphere>
            <radius>0.05</radius>
          </sphere>
        </geometry>
        <pose>-0.2 0 -0.05 0 0 0</pose>
        <material>
          <ambient>0.5 0.5 0.5 1</ambient>
          <diffuse>0.5 0.5 0.5 1</diffuse>
        </material>
      </visual>

      <collision name="caster_collision">
        <geometry>
          <sphere>
            <radius>0.05</radius>
          </sphere>
        </geometry>
        <pose>-0.2 0 -0.05 0 0 0</pose>
      </collision>
    </link>

    <joint name="caster_joint" type="sphere">
      <parent>base_link</parent>
      <child>caster_wheel</child>
    </joint>

    <!-- Camera sensor -->
    <link name="camera_link">
      <inertial>
        <mass>0.1</mass>
      </inertial>

      <sensor name="camera" type="camera">
        <pose>0 0 0 0 0 0</pose>
        <camera>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>640</width>
            <height>480</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>
        <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
          <ros>
            <namespace>/camera</namespace>
            <image_topic_name>/image_raw</image_topic_name>
            <camera_info_topic_name>/camera_info</camera_info_topic_name>
            <frame_name>camera_link</frame_name>
          </ros>
        </plugin>
      </sensor>
    </link>

    <joint name="camera_joint" type="fixed">
      <parent>base_link</parent>
      <child>camera_link</child>
      <pose>0.25 0 0.1 0 0 0</pose>
    </joint>

    <!-- LiDAR sensor -->
    <link name="laser_link">
      <inertial>
        <mass>0.2</mass>
      </inertial>

      <sensor name="laser" type="ray">
        <pose>0 0 0 0 0 0</pose>
        <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>
        </ray>
        <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
          <ros>
            <namespace>/laser</namespace>
            <topic_name>/scan</topic_name>
            <frame_name>laser_link</frame_name>
          </ros>
        </plugin>
      </sensor>
    </link>

    <joint name="laser_joint" type="fixed">
      <parent>base_link</parent>
      <child>laser_link</child>
      <pose>0 0 0.3 0 0 0</pose>
    </joint>

    <!-- Differential drive plugin -->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <ros>
        <namespace>/</namespace>
        <remapping>cmd_vel:=cmd_vel</remapping>
        <remapping>odom:=odom</remapping>
      </ros>

      <!-- wheels -->
      <left_joint>left_wheel_joint</left_joint>
      <right_joint>right_wheel_joint</right_joint>

      <!-- kinematics -->
      <wheel_separation>0.5</wheel_separation>
      <wheel_diameter>0.2</wheel_diameter>
      <max_wheel_torque>20</max_wheel_torque>
      <max_wheel_acceleration>1.0</max_wheel_acceleration>

      <!-- output -->
      <publish_odom>true</publish_odom>
      <publish_odom_tf>true</publish_odom_tf>
      <publish_wheel_tf>true</publish_wheel_tf>

      <odometry_source>world</odometry_source>
    </plugin>

  </model>
</sdf>

7.4 Sensor Simulation

7.4.1 Camera Sensor Modeling

Gazebo simulates camera sensors with realistic optics and image processing:

Example: Custom Camera Plugin

#include <gazebo/sensors/SensorTypes.hh>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

namespace gazebo {
class CustomCameraPlugin : public CameraPlugin {
public:
    CustomCameraPlugin() : CameraPlugin() {}

    void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) override {
        // Load CameraPlugin parent
        CameraPlugin::Load(_sensor, _sdf);

        // Get ROS node
        this->node = transport::NodePtr(new transport::Node());
        this->node->Init(this->world->Name());

        // ROS initialization
        if (!ros::isInitialized()) {
            int argc = 0;
            char **argv = NULL;
            ros::init(argc, argv, this->world->Name() + "_camera",
                      ros::init_options::NoSigintHandler);
        }

        this->rosNode.reset(new ros::NodeHandle(this->world->Name()));

        // Get camera sensor
        this->parentSensor = std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);

        if (!this->parentSensor) {
            gzerr << "CustomCameraPlugin requires a CameraSensor.\n";
            return;
        }

        // Get camera name
        if (_sdf->HasElement("cameraName")) {
            this->cameraName = _sdf->Get<std::string>("cameraName");
        } else {
            this->cameraName = this->parentSensor->Name();
        }

        // Create ROS publishers
        this->imagePub = this->rosNode->advertise<sensor_msgs::Image>(
            this->cameraName + "/image_raw", 1);
        this->infoPub = this->rosNode->advertise<sensor_msgs::CameraInfo>(
            this->cameraName + "/camera_info", 1);

        // Connect to camera update
        this->parentSensor->ConnectUpdated(
            std::bind(&CustomCameraPlugin::OnNewFrame, this));

        this->parentSensor->SetActive(true);
    }

private:
    void OnNewFrame() {
        // Get image data
        unsigned char* data = this->image.data;
        uint32_t width = this->image.width;
        uint32_t height = this->image.height;

        // Create ROS image message
        sensor_msgs::Image image_msg;
        image_msg.header.stamp = ros::Time(this->parentSensor->LastMeasurementTime());
        image_msg.header.frame_id = this->cameraName + "_optical_frame";
        image_msg.height = height;
        image_msg.width = width;
        image_msg.encoding = "rgb8";
        image_msg.step = width * 3;
        image_msg.data.resize(width * height * 3);

        // Copy image data
        memcpy(&image_msg.data[0], data, width * height * 3);

        // Create camera info message
        sensor_msgs::CameraInfo info_msg;
        info_msg.header = image_msg.header;
        info_msg.height = height;
        info_msg.width = width;

        // Camera intrinsics
        double fx = this->camera->HFOV().Radian() / (2.0 * tan(this->camera->HFOV().Radian() / 2.0));
        double fy = fx;
        double cx = width / 2.0;
        double cy = height / 2.0;

        info_msg.K[0] = fx;
        info_msg.K[2] = cx;
        info_msg.K[4] = fy;
        info_msg.K[5] = cy;
        info_msg.K[8] = 1.0;

        // Publish messages
        this->imagePub.publish(image_msg);
        this->infoPub.publish(info_msg);
    }

    ros::NodeHandlePtr rosNode;
    ros::Publisher imagePub;
    ros::Publisher infoPub;
    std::string cameraName;
};

GZ_REGISTER_SENSOR_PLUGIN(CustomCameraPlugin)
}

7.4.2 LiDAR Sensor Simulation

Example: LiDAR Data Processing

import matplotlib.pyplot as plt
from sensor_msgs.msg import LaserScan

class LidarProcessor:
    def __init__(self):
        self.scan_data = None
        self.min_range = 0.1
        self.max_range = 10.0

    def process_scan(self, scan_msg):
        """Process LiDAR scan message"""
        self.scan_data = scan_msg

        # Convert to numpy array
        ranges = np.array(scan_msg.ranges)
        angles = np.linspace(
            scan_msg.angle_min,
            scan_msg.angle_max,
            len(ranges)
        )

        # Filter invalid readings
        valid_ranges = ranges[
            (ranges >= self.min_range) &
            (ranges <= self.max_range)
        ]
        valid_angles = angles[
            (ranges >= self.min_range) &
            (ranges <= self.max_range)
        ]

        return valid_ranges, valid_angles

    def detect_obstacles(self, ranges, angles, threshold=1.0):
        """Detect obstacles in LiDAR data"""
        obstacles = []

        for i, (r, theta) in enumerate(zip(ranges, angles)):
            if r < threshold:
                # Convert polar to Cartesian
                x = r * np.cos(theta)
                y = r * np.sin(theta)
                obstacles.append([x, y])

        return np.array(obstacles)

    def segment_clusters(self, obstacles, distance_threshold=0.5):
        """Cluster obstacle points"""
        if len(obstacles) == 0:
            return []

        clusters = []
        visited = np.zeros(len(obstacles), dtype=bool)

        for i in range(len(obstacles)):
            if visited[i]:
                continue

            cluster = [obstacles[i]]
            visited[i] = True

            # Find nearby points
            for j in range(i+1, len(obstacles)):
                if not visited[j]:
                    dist = np.linalg.norm(obstacles[i] - obstacles[j])
                    if dist < distance_threshold:
                        cluster.append(obstacles[j])
                        visited[j] = True

            clusters.append(np.array(cluster))

        return clusters

    def visualize_scan(self, ranges, angles, obstacles=None):
        """Visualize LiDAR scan"""
        plt.figure(figsize=(10, 10))

        # Plot LiDAR scan
        x = ranges * np.cos(angles)
        y = ranges * np.sin(angles)

        plt.subplot(111, projection='polar')
        plt.plot(angles, ranges, 'b.', markersize=2, label='Laser scan')

        if obstacles is not None:
            # Plot detected obstacles
            for obs in obstacles:
                obs_angles = np.arctan2(obs[:, 1], obs[:, 0])
                obs_ranges = np.linalg.norm(obs, axis=1)
                plt.plot(obs_angles, obs_ranges, 'ro', markersize=4, label='Obstacle')

        plt.title('LiDAR Scan Visualization')
        plt.grid(True)
        plt.legend()
        plt.show()

# Example usage
processor = LidarProcessor()

# Simulate LiDAR scan processing
def simulate_lidar_processing():
    # Create sample scan data
    scan = LaserScan()
    scan.header.stamp = rospy.Time.now()
    scan.header.frame_id = "laser"
    scan.angle_min = -np.pi
    scan.angle_max = np.pi
    scan.angle_increment = np.pi / 180
    scan.range_min = 0.1
    scan.range_max = 10.0

    # Generate sample ranges (simulating obstacles)
    angles = np.linspace(scan.angle_min, scan.angle_max, 360)
    ranges = np.ones(360) * 8.0  # Default range

    # Add some obstacles
    ranges[90:110] = 2.0  # Front obstacle
    ranges[180:200] = 3.0  # Left obstacle

    scan.ranges = ranges.tolist()

    # Process scan
    valid_ranges, valid_angles = processor.process_scan(scan)
    obstacles = processor.detect_obstacles(valid_ranges, valid_angles, threshold=4.0)

    if len(obstacles) > 0:
        clusters = processor.segment_clusters(obstacles)
        print(f"Detected {len(clusters)} obstacle clusters")

    # Visualize
    processor.visualize_scan(valid_ranges, valid_angles, obstacles)

# Uncomment to run simulation
# simulate_lidar_processing()

7.5 Advanced Gazebo Features

7.5.1 Plugin Development

Gazebo plugins extend simulation capabilities:

Example: Custom Physics Plugin

#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace gazebo {

class CustomPhysicsPlugin : public WorldPlugin {
public:
    CustomPhysicsPlugin() : WorldPlugin() {}

    void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf) override {
        this->world = _world;

        // ROS initialization
        if (!ros::isInitialized()) {
            int argc = 0;
            char **argv = NULL;
            ros::init(argc, argv, "custom_physics_plugin");
        }

        this->rosNode.reset(new ros::NodeHandle());

        // ROS subscribers
        this->gravitySub = this->rosNode->subscribe(
            "/gravity_multiplier", 1,
            &CustomPhysicsPlugin::OnGravityMsg, this);

        // Connect to physics update
        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
            std::bind(&CustomPhysicsPlugin::OnUpdate, this));

        // Initialize gravity
        this->gravityMultiplier = 1.0;

        ROS_INFO("Custom physics plugin loaded");
    }

private:
    void OnUpdate() {
        // Apply custom physics modifications
        physics::PhysicsEnginePtr physics = this->world->Physics();

        // Modify gravity
        ignition::math::Vector3d gravity = physics->Gravity();
        gravity.Z(9.8066 * this->gravityMultiplier);
        physics->SetGravity(gravity);

        // Apply custom forces to models
        for (auto model : this->world->Models()) {
            ApplyCustomForces(model);
        }
    }

    void ApplyCustomForces(physics::ModelPtr model) {
        // Example: Apply drag force
        ignition::math::Vector3d velocity = model->WorldLinearVel();
        ignition::math::Vector3d drag_force = -0.1 * velocity;

        // Apply force to main link
        auto link = model->GetLink();
        if (link) {
            link->AddForce(drag_force);
        }
    }

    void OnGravityMsg(const std_msgs::Float64::ConstPtr& msg) {
        this->gravityMultiplier = msg->data;
        ROS_INFO("Gravity multiplier set to: %.2f", this->gravityMultiplier);
    }

    physics::WorldPtr world;
    event::ConnectionPtr updateConnection;
    ros::NodeHandlePtr rosNode;
    ros::Subscriber gravitySub;
    double gravityMultiplier;
};

GZ_REGISTER_WORLD_PLUGIN(CustomPhysicsPlugin)
}

7.5.2 Custom Materials

Example: Material Definitions

<sdf version="1.6">
  <world name="material_test">

    <!-- Define custom materials -->
    <include>
      <uri>model://custom_materials/materials</uri>
    </include>

    <!-- Ground plane with custom material -->
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <!-- Test objects with different materials -->
    <model name="test_objects">

      <!-- Rubber ball -->
        <link name="rubber_ball">
          <visual name="visual">
            <geometry>
              <sphere>
                <radius>0.1</radius>
              </sphere>
            </geometry>
            <material>
              <script>
                <uri>model://custom_materials/materials/scripts</uri>
                <uri>model://custom_materials/materials/textures</uri>
                <name>Rubber/Red</name>
              </script>
            </material>
          </visual>

          <collision name="collision">
            <geometry>
              <sphere>
                <radius>0.1</radius>
              </sphere>
            </geometry>
            <surface>
              <friction>
                <ode>
                  <mu>1.2</mu>
                  <mu2>1.2</mu2>
                </ode>
              </friction>
              <restitution>
                <ode>
                  <restitution_coefficient>0.8</restitution_coefficient>
                </ode>
              </restitution>
            </surface>
          </collision>

          <inertial>
            <mass>0.5</mass>
            <inertia>
              <ixx>0.002</ixx>
              <iyy>0.002</iyy>
              <izz>0.002</izz>
            </inertia>
          </inertial>
        </link>
      </model>

    </model>

  </world>
</sdf>

7.6 Performance Optimization

7.6.1 Simulation Tuning

Optimize Gazebo performance by adjusting physics parameters, reducing model complexity, and using appropriate time steps for your use case.

Example: Performance-Optimized Physics

  <!-- Larger time step for faster simulation -->
  <max_step_size>0.004</max_step_size>
  <real_time_factor>2.0</real_time_factor>
  <real_time_update_rate>250</real_time_update_rate>

  <ode>
    <solver>
      <!-- Use faster solver with fewer iterations -->
      <type>quick</type>
      <iters>5</iters>
      <sor>1.3</sor>
    </solver>

    <constraints>
      <!-- Reduce constraint accuracy for speed -->
      <cfm>0.00001</cfm>
      <erp>0.2</erp>
      <contact_max_correcting_vel>10</contact_max_correcting_vel>
      <contact_surface_layer>0.001</contact_surface_layer>
    </constraints>
  </ode>
</physics>

7.6.2 Memory Management

Example: Memory-Efficient Simulation Control

import gazebo_msgs.srv
from gazebo_msgs.msg import ModelStates
from std_srvs.srv import Empty

class SimulationManager:
    def __init__(self):
        rospy.init_node('simulation_manager')

        # Wait for services
        rospy.wait_for_service('/gazebo/unpause_physics')
        rospy.wait_for_service('/gazebo/pause_physics')
        rospy.wait_for_service('/gazebo/reset_world')

        # Service proxies
        self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
        self.reset = rospy.ServiceProxy('/gazebo/reset_world', Empty)

        # Model state subscriber
        self.model_sub = rospy.Subscriber(
            '/gazebo/model_states', ModelStates, self.model_callback)

        self.models = {}
        self.simulation_time = 0.0

    def model_callback(self, msg):
        """Cache model information for efficiency"""
        self.models = dict(zip(msg.name, msg.pose))

    def reset_simulation(self):
        """Reset and pause simulation for setup"""
        self.reset()
        self.pause()
        rospy.sleep(1.0)  # Allow reset to complete

    def run_simulation(self, duration):
        """Run simulation for specified duration"""
        self.unpause()

        start_time = rospy.Time.now()
        while (rospy.Time.now() - start_time).to_sec() < duration:
            # Process simulation data
            self.process_simulation_step()
            rospy.sleep(0.01)  # 100 Hz control loop

        self.pause()

    def process_simulation_step(self):
        """Process one step of simulation"""
        # Efficient processing without excessive data collection
        current_time = rospy.Time.now().to_sec()

        # Process only relevant models
        for model_name in ['mobile_robot', 'obstacle_1', 'obstacle_2']:
            if model_name in self.models:
                pose = self.models[model_name]
                # Process pose data
                self.process_model_pose(model_name, pose, current_time)

        self.simulation_time = current_time

    def process_model_pose(self, model_name, pose, time):
        """Process individual model pose"""
        # Extract position and orientation
        x = pose.position.x
        y = pose.position.y
        z = pose.position.z

        # Extract orientation
        quat = pose.orientation
        yaw = self.quaternion_to_yaw(quat)

        # Process based on model type
        if model_name == 'mobile_robot':
            self.process_robot_pose(x, y, yaw, time)
        elif 'obstacle' in model_name:
            self.process_obstacle_pose(x, y, time)

    def quaternion_to_yaw(self, quat):
        """Convert quaternion to yaw angle"""
        import math
        siny_cosp = 2 * (quat.w * quat.z + quat.x * quat.y)
        cosy_cosp = 1 - 2 * (quat.y * quat.y + quat.z * quat.z)
        return math.atan2(siny_cosp, cosy_cosp)

    def process_robot_pose(self, x, y, yaw, time):
        """Process robot pose for navigation"""
        # Implement navigation logic
        pass

    def process_obstacle_pose(self, x, y, time):
        """Process obstacle pose for collision detection"""
        # Implement obstacle tracking
        pass

# Usage example
if __name__ == '__main__':
    manager = SimulationManager()

    # Reset simulation
    manager.reset_simulation()

    # Run multiple episodes
    for episode in range(10):
        print(f"Running episode {episode + 1}")

        # Run simulation for 30 seconds
        manager.run_simulation(30.0)

        # Process episode results
        # ... analysis and learning ...

        # Reset for next episode
        manager.reset_simulation()

7.7 Integration with ROS 2

7.7.1 Gazebo-ROS Bridge

Example: Gazebo ROS Interface

from rclpy.node import Node
from gazebo_msgs.msg import ModelState, ModelStates
from gazebo_msgs.srv import SetModelState, GetModelState
from geometry_msgs.msg import Twist

class GazeboInterface(Node):
    def __init__(self):
        super().__init__('gazebo_interface')

        # Create service clients
        self.set_state_client = self.create_client(
            SetModelState, '/gazebo/set_model_state')
        self.get_state_client = self.create_client(
            GetModelState, '/gazebo/get_model_state')

        # Create publishers and subscribers
        self.cmd_vel_pub = self.create_publisher(
            Twist, '/cmd_vel', 10)

        self.model_sub = self.create_subscription(
            ModelStates, '/gazebo/model_states',
            self.model_callback, 10)

        # Wait for services
        while not self.set_state_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Waiting for set_model_state service...')

        self.current_models = {}

    def model_callback(self, msg):
        """Track model states"""
        self.current_models = dict(zip(msg.name, msg.pose))

    def set_model_pose(self, model_name, x, y, z, roll=0, pitch=0, yaw=0):
        """Set model pose in simulation"""
        request = SetModelState.Request()

        # Model state
        request.model_state.model_name = model_name
        request.model_state.reference_frame = "world"

        # Position
        request.model_state.pose.position.x = x
        request.model_state.pose.position.y = y
        request.model_state.pose.position.z = z

        # Orientation
        import tf_transformations
        quat = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
        request.model_state.pose.orientation.x = quat[0]
        request.model_state.pose.orientation.y = quat[1]
        request.model_state.pose.orientation.z = quat[2]
        request.model_state.pose.orientation.w = quat[3]

        # Send request
        future = self.set_state_client.call_async(request)
        return future

    def get_model_pose(self, model_name):
        """Get current model pose"""
        request = GetModelState.Request()
        request.model_name = model_name
        request.reference_frame = "world"

        future = self.get_state_client.call_async(request)
        return future

    def publish_velocity(self, linear_x, linear_y, angular_z):
        """Publish velocity command"""
        msg = Twist()
        msg.linear.x = linear_x
        msg.linear.y = linear_y
        msg.angular.z = angular_z
        self.cmd_vel_pub.publish(msg)

    def reset_simulation(self):
        """Reset simulation to initial state"""
        # Reset robot position
        self.set_model_pose('mobile_robot', 0, 0, 0.1)

        # Reset obstacles to random positions
        import random
        for i in range(3):
            x = random.uniform(-5, 5)
            y = random.uniform(-5, 5)
            self.set_model_pose(f'obstacle_{i}', x, y, 0.5)

# Usage example
def main():
    rclpy.init()

    gazebo_interface = GazeboInterface()

    # Reset simulation
    gazebo_interface.reset_simulation()

    # Simple movement test
    for i in range(100):
        gazebo_interface.publish_velocity(0.5, 0.0, 0.1)
        rclpy.spin_once(gazebo_interface, timeout_sec=0.1)

    rclpy.shutdown()

if __name__ == '__main__':
    main()

Summary

Gazebo provides a comprehensive physics simulation environment essential for robotics development and testing. This chapter covered the fundamental concepts of physics simulation, robot modeling, sensor simulation, and advanced plugin development.

Key takeaways:

• Gazebo uses multiple physics engines with different characteristics
• Accurate robot modeling requires proper URDF to SDF conversion
• Sensor simulation enables realistic perception system testing
• Custom plugins extend simulation capabilities
• Performance optimization is crucial for complex simulations
• ROS 2 integration enables seamless simulation-robot workflows

Exercises

Exercise 7.1: Physics Engine Comparison

Create a simple simulation and test it with different physics engines:

• Implement a falling object scenario
• Compare ODE, Bullet, and DART physics
• Measure computational performance
• Analyze accuracy of physical behavior

Exercise 7.2: Custom Robot Model

Design and simulate a custom robot:

• Create URDF model with multiple joints
• Convert to SDF with simulation properties
• Add sensors and actuators
• Test in Gazebo simulation

Exercise 7.3: Sensor Plugin Development

Develop a custom sensor plugin:

• Choose a sensor type (e.g., temperature, pressure)
• Implement physics-based sensing model
• Add ROS 2 interface
• Validate sensor behavior

Exercise 7.4: Performance Optimization

Optimize a complex simulation:

• Identify performance bottlenecks
• Adjust physics parameters
• Optimize model complexity
• Measure performance improvements

Exercise 7.5: Integration Project

Create a complete simulation-robot system:

• Design robot and environment
• Implement perception and control
• Test in simulation
• Deploy to real robot and compare performance

Glossary Terms

• SDF (Simulation Description Format): XML format for describing Gazebo worlds and models
• ODE (Open Dynamics Engine): Physics engine for rigid body simulation
• URDF (Unified Robot Description Format): XML format for robot model description
• Plugin System: Gazebo architecture for extending functionality
• Sensor Simulation: Modeling of real sensor behavior in simulation
• Contact Mechanics: Physics of contact and collision between objects
• Real-time Factor: Ratio of simulation time to real time
• Gazebo-ROS Bridge: Interface between Gazebo simulation and ROS 2
• Physics Time Step: Simulation integration time step
• Broad/Narrow Phase: Collision detection optimization techniques