
## Autonomous Navigation with Nav2

This is the final part of the Project Ogre series. We've built the [robot](/blog/project-ogre-part1-building-robot), created a [digital twin](/blog/project-ogre-part2-isaac-sim), built [maps with SLAM](/blog/project-ogre-part3-slam-mapping), and trained an [RL policy](/blog/project-ogre-part4-isaac-lab-training). Now we bring it all together with Nav2 for autonomous waypoint navigation.

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>Project Goal:</strong> Navigate autonomously to user-specified waypoints while avoiding obstacles, using the saved map and trained policy.
</div>

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>📦 Full Source Code:</strong> Nav2 configuration and launch files are available at <a href="https://github.com/protomota/ogre-slam" style={{ color: '#ff8c29', textDecoration: 'underline' }}>github.com/protomota/ogre-slam</a>
</div>

The video below shows Nav2 autonomous navigation in action. On the left is RViz displaying the map, costmaps, and planned path. On the right is Isaac Sim where the robot navigates autonomously through the maze using the trained RL policy to execute velocity commands.

<div style={{ position: 'relative', paddingBottom: '56.25%', height: 0, overflow: 'hidden', maxWidth: '100%', marginTop: '1.5rem' }}>
  <iframe
    style={{ position: 'absolute', top: 0, left: 0, width: '100%', height: '100%' }}
    src="https://www.youtube.com/embed/IfBRDBTknWg"
    title="Nav2 Autonomous Navigation Demonstration"
    frameBorder="0"
    allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture"
    allowFullScreen
  />
</div>

## The Nav2 Stack

Nav2 (Navigation 2) is ROS2's navigation framework. It handles:

1. **Localization**: Where is the robot on the map?
2. **Global Planning**: What's the best path to the goal?
3. **Local Planning**: How do we follow that path while avoiding new obstacles?
4. **Recovery**: What to do when stuck?

For Project Ogre, the architecture looks like:

```
Saved Map → AMCL (localization)
                    ↓
User Goal → Global Planner → Local Planner → /cmd_vel
                                                  ↓
                                          Policy Controller
                                                  ↓
                                           Wheel Commands
```

The RL policy sits between Nav2's velocity commands and the actual wheel control, translating ideal commands into optimal wheel velocities.

## Understanding Costmaps

Nav2 uses **costmaps** to represent navigable space. Each cell in the grid has a cost (0-254) indicating traversal difficulty:

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Cost</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Name</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Meaning</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>0</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>FREE_SPACE</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Safe to traverse</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>1-252</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>INFLATED</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Near obstacles (increasingly expensive)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>253</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>INSCRIBED</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Inside robot's radius (collision likely)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>254</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>LETHAL</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Obstacle (certain collision)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}>255</td>
      <td style={{ padding: '12px 16px' }}>NO_INFORMATION</td>
      <td style={{ padding: '12px 16px' }}>Unknown space</td>
    </tr>
  </tbody>
</table>

### Two Costmaps

**Global Costmap** (`/global_costmap/costmap`)
- Covers the entire map
- Used for path planning
- Updates from static map + sensors

**Local Costmap** (`/local_costmap/costmap`)
- Rolling 3m × 3m window around robot
- Used for reactive obstacle avoidance
- Updates continuously from sensors

### Inflation

Obstacles are "inflated" by the robot's radius to prevent collision. The key parameters:

```yaml
robot_radius: 0.20        # Inscribed radius
inflation_radius: 0.35    # How far to spread costs
cost_scaling_factor: 3.0  # How quickly costs drop off
```

If the robot clips walls, increase `inflation_radius`. If it can't navigate narrow passages, decrease it.

## Nav2 Configuration

The full configuration lives in `config/nav2_params.yaml`. Here are the key sections:

### AMCL (Localization)

AMCL (Adaptive Monte Carlo Localization) tracks the robot's position on the map using laser scan matching:

```yaml
amcl:
  ros__parameters:
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    scan_topic: /scan
    min_particles: 500
    max_particles: 2000
    update_min_d: 0.1  # Update after 10cm movement
    update_min_a: 0.2  # Update after ~11° rotation
```

### Global Planner

We use Smac 2D for global path planning - it handles tight spaces better than the default NavFn:

```yaml
planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.50  # Search 50cm around goal
      max_planning_time: 2.0
```

The `tolerance` parameter is crucial - it defines how close the robot needs to get to the exact goal point. Higher tolerance helps when goals are near obstacles.

### Local Controller

The local controller follows the global path while avoiding obstacles. For mecanum robots, we use a velocity-based approach:

```yaml
controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      max_vel_x: 0.3
      max_vel_y: 0.3     # Strafing enabled for mecanum
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      min_speed_theta: 0.0
```

Note `max_vel_y` - this enables strafing, which is what makes mecanum navigation special.

### Behavior Server (Recovery)

When the robot gets stuck, recovery behaviors kick in:

```yaml
behavior_server:
  ros__parameters:
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
```

The robot will try spinning, backing up, or waiting before giving up on a goal.

## Running Navigation

### In Isaac Sim

**Terminal 1: Start Isaac Sim**
- Load `usds/ogre.usd`
- Verify ROS2 Context has a matching Domain ID
- Press Play

**Terminal 2: Launch Navigation**
```bash
cd ~/ros2_ws && source install/setup.bash

ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/isaac_sim_map.yaml \
  use_sim_time:=true
```

The `use_sim_time:=true` flag is critical - without it, TF transforms will time out because the simulation clock doesn't match wall clock.

**Terminal 3: Launch Policy Controller**
```bash
conda deactivate
source ~/ros2_ws/install/setup.bash

ros2 launch ogre_policy_controller policy_controller.launch.py use_sim_time:=true
```

**Terminal 4: Launch RViz (optional, for remote visualization)**
```bash
./scripts/launch_isaac_sim_rviz.sh
```

### On the Real Robot

```bash
cd ~/ros2_ws && source install/setup.bash
ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/my_map.yaml
```

This launches:
- AMCL localization with the saved map
- RPLIDAR + RealSense D435 sensors
- Full Nav2 stack
- Web teleop for manual override

## Navigating in RViz

With navigation running, RViz becomes your command interface:

### Step 1: Set Initial Pose

Click **2D Pose Estimate** in the toolbar, then click and drag on the map to indicate:
- Where the robot is
- Which direction it's facing

AMCL needs this initial hint to start tracking correctly. As the robot moves, localization will refine automatically.

### Step 2: Send Navigation Goals

Click **Nav2 Goal** (or **2D Goal Pose**), then click on the map where you want the robot to go. Drag to set the final orientation.

The robot will:
1. Plan a global path (green line in RViz)
2. Follow the path using local planning
3. Avoid obstacles detected in real-time
4. Execute recovery behaviors if stuck

### Manual Override

On the real robot, the web teleop interface at `http://10.21.21.45:8080` provides manual control. Any teleop commands override Nav2's autonomous control.

## The Complete Data Flow

Let's trace a navigation command through the entire system:

```
1. User clicks goal in RViz
         ↓
2. Global Planner computes path using costmap
         ↓
3. Local Controller generates /cmd_vel following path
         ↓
4. Policy Controller receives velocity command
         ↓
5. Neural network computes optimal wheel velocities
         ↓
6. /joint_command sent to robot/sim
         ↓
7. Robot moves, sensors update, localization refines
         ↓
   (loop continues until goal reached)
```

The trained RL policy adds value at step 5 - it learns compensation for wheel slip, motor delays, and mechanical imperfections that pure kinematics can't handle.

## 3D Obstacle Avoidance

The RealSense D435 provides depth data that the RPLIDAR can't see - obstacles below the 2D scan plane like:
- Table legs
- Stairs
- Low furniture
- Pets

The depth camera publishes a PointCloud2 to `/camera_points`, which feeds into the local costmap's voxel layer:

```yaml
local_costmap:
  voxel_layer:
    plugin: "nav2_costmap_2d::VoxelLayer"
    observation_sources: scan camera
    camera:
      topic: /camera_points
      sensor_frame: camera_link
      max_obstacle_height: 0.5
      min_obstacle_height: 0.0
```

Now the robot can detect and avoid 3D obstacles in real-time.

## Troubleshooting Navigation

### "Legal potential but no path"

**Cause**: Goal is in an inflated zone near obstacles

**Solutions**:
1. Click goals further from walls
2. Increase `tolerance` in planner config
3. Switch to SmacPlanner2D (handles this better than NavFn)

### Robot Clips Walls

**Cause**: Inflation radius too small

**Solution**: Increase `inflation_radius` in both costmaps (try 0.40-0.50m)

### Robot Can't Enter Narrow Spaces

**Cause**: Inflation radius too large

**Solution**: Decrease `inflation_radius` (try 0.30m)

### Localization Drifts

**Cause**: AMCL losing track, often from rapid motion or featureless areas

**Solutions**:
1. Move slower during navigation
2. Increase `max_particles`
3. Ensure good initial pose estimate

### TF_OLD_DATA Errors

**Cause**: Clock synchronization issues

**Solutions**:
- In simulation: Ensure `use_sim_time:=true` on ALL nodes
- On real robot: Check that odometry and SLAM are publishing current timestamps

### Robot Spins Forever

**Cause**: Recovery behavior stuck in loop

**Solution**: Cancel the goal and try a different destination. Check if there's an obstacle blocking all paths.

## Performance on Jetson

Running the full Nav2 stack on Jetson Orin Nano requires some optimization:

### Memory Management

```yaml
global_costmap:
  resolution: 0.10  # Coarser than SLAM (0.05) to save memory
local_costmap:
  resolution: 0.05  # Finer for local avoidance
```

### CPU Optimization

```bash
# Enable max performance mode
sudo /usr/bin/jetson_clocks

# Monitor resources
tegrastats
```

### Disable Unnecessary Visualization

Don't run RViz on the Jetson - use remote visualization instead. Save those CPU cycles for navigation.

## What We Built

Over these five posts, we created a complete autonomous mobile robot:

1. **Hardware**: Mecanum drive robot with Jetson Orin Nano, RPLIDAR, and RealSense
2. **Odometry**: Calibrated encoders with EKF sensor fusion
3. **Mapping**: SLAM-generated occupancy grids
4. **Learning**: RL-trained velocity controller
5. **Navigation**: Full Nav2 stack with 3D obstacle avoidance

The robot can now navigate autonomously to any point on the map, avoiding both static obstacles (walls from the map) and dynamic obstacles (detected in real-time by sensors).

## Future Improvements

The system works, but there's always room to improve:

- **IMU Integration**: Adding an IMU would dramatically improve odometry, especially during rotation
- **Dynamic Obstacle Tracking**: Currently obstacles are binary - tracking moving obstacles would enable smarter planning
- **Sim-to-Real Transfer**: Fine-tuning the RL policy on real-world data

---

**Project Ogre Summary:**

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Component</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Technology</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Computing</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Jetson Orin Nano</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Drive</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Mecanum wheels (omnidirectional)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>2D Sensing</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RPLIDAR A1</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>3D Sensing</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RealSense D435</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>SLAM</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>slam_toolbox</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Localization</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>AMCL</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Planning</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Nav2 (Smac2D + DWB)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Control</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RL-trained policy</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}>Simulation</td>
      <td style={{ padding: '12px 16px' }}>NVIDIA Isaac Sim</td>
    </tr>
  </tbody>
</table>

**Key Launch Commands:**

```bash
# Mapping (simulation)
ros2 launch ogre_slam mapping.launch.py use_sim_time:=true

# Mapping (real robot)
./scripts/launch_mapping_session.sh

# Navigation (simulation)
ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/isaac_sim_map.yaml \
  use_sim_time:=true

# Navigation (real robot)
ros2 launch ogre_slam navigation.launch.py \
  map:=~/ros2_ws/src/ogre-slam/maps/my_map.yaml

# Policy controller
ros2 launch ogre_policy_controller policy_controller.launch.py
```

Thanks for following along with Project Ogre. If you build something similar or have questions, let me know in the comments.

## Credits

- **slam_toolbox**: Steve Macenski
- **robot_localization**: Tom Moore
- **RPLIDAR ROS**: SLAMTEC
- **Isaac Lab**: NVIDIA
- **Nav2**: Open Robotics & Nav2 maintainers


Bringing it all together - autonomous waypoint navigation using ROS2 Nav2 with SLAM maps and RL-trained velocity control.

Project Ogre Part 5: Autonomous Navigation with Nav2


<img src="/images/ogre-lab.png" alt="Isaac Lab Training Environment" style={{ width: '70%', height: 'auto', display: 'block' }} />

## Training in Isaac Lab

This is Part 4 of the Project Ogre series. In [Part 3](/blog/project-ogre-part3-slam-mapping), we built maps using SLAM. Now we'll train a neural network to optimally control the mecanum wheels using reinforcement learning in NVIDIA Isaac Lab.

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>Project Goal:</strong> Train an RL policy that converts velocity commands (vx, vy, vtheta) into optimal wheel velocities, learning the robot's real dynamics.
</div>

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>📦 Full Source Code:</strong> Training environment and scripts are available at <a href="https://github.com/protomota/ogre-lab" style={{ color: '#ff8c29', textDecoration: 'underline' }}>github.com/protomota/ogre-lab</a>
</div>

The video below shows Isaac Lab training 4096 parallel instances of the robot simultaneously. Each instance learns from its own experiences, accelerating the training process dramatically compared to single-environment training.

<div style={{ position: 'relative', paddingBottom: '56.25%', height: 0, overflow: 'hidden', maxWidth: '100%', marginTop: '1.5rem' }}>
  <iframe
    style={{ position: 'absolute', top: 0, left: 0, width: '100%', height: '100%' }}
    src="https://www.youtube.com/embed/_a5k1wXDSU8"
    title="Isaac Lab Training Demonstration"
    frameBorder="0"
    allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture"
    allowFullScreen
  />
</div>

## Why Use Reinforcement Learning?

Mecanum wheel kinematics have a clean theoretical model: given a desired body velocity (vx, vy, angular), you can compute the required wheel velocities using inverse kinematics. So why train a neural network?

**The theory doesn't match reality.**

Real robots have:
- Wheel slip on different surfaces
- Motor response delays and non-linearities
- Weight distribution asymmetries
- Mechanical imperfections in wheel alignment

An RL policy learns to compensate for these factors by experiencing thousands of trials in simulation. It discovers that "to go straight forward at 0.3 m/s, the right wheels need slightly higher velocity to compensate for weight imbalance."

The trained policy sits between Nav2's velocity commands and the actual wheel controllers, translating ideal commands into commands that achieve the desired behavior on the real robot.

## Isaac Lab Architecture

Isaac Lab is NVIDIA's GPU-accelerated robot learning framework built on Isaac Sim. Key features:

- **Parallel environments**: Train on 1024+ robots simultaneously
- **GPU physics**: PhysX runs entirely on GPU for massive speedup
- **RSL-RL integration**: Works with the RSL-RL library for PPO training

The training loop runs at ~30Hz control rate with 120Hz physics, training policies in minutes rather than hours.

## The Training Environment

The environment is defined in `ogre_navigation_env.py`. Let's break down the key components:

### Observation Space (10 dimensions)

The policy sees:

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Index</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Description</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>0-2</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Target velocity command (vx, vy, vtheta)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>3-5</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Current base velocity (vx, vy, vtheta)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}>6-9</td>
      <td style={{ padding: '12px 16px' }}>Current wheel velocities (fl, fr, rl, rr)</td>
    </tr>
  </tbody>
</table>

This gives the policy enough information to understand both what it should achieve and what the robot is currently doing.

### Action Space (4 dimensions)

The policy outputs wheel velocity targets for all four wheels:
- Front-left (FL)
- Front-right (FR)
- Rear-left (RL)
- Rear-right (RR)

Raw policy outputs are in [-1, 1] and scaled by `action_scale` (8.0 rad/s in the working config) to get actual wheel velocities.

### Reward Function

The policy learns through reward signals:

```python
# Main velocity tracking reward
vel_error = target_vel - current_vel
rew_vel_tracking = 2.0 * exp(-vel_error_norm / 0.25)

# Uprightness reward (+1 upright, -1 flipped)
rew_uprightness = 1.0 * up_z

# Wheel symmetry penalty (prevents veering during straight motion)
rew_symmetry = -1.0 * (left_avg - right_avg)^2
```

The velocity tracking reward uses exponential decay - perfect tracking gives maximum reward, with diminishing returns as error increases. The symmetry penalty discourages the policy from commanding asymmetric wheel velocities when the target is straight-line motion.

### Episode Structure

- **Duration**: 10 seconds per episode
- **Termination**: Episode ends early if robot flips (base height < 0.02m)
- **Reset**: Random target velocities sampled each episode

Random targets force the policy to generalize across the full velocity space rather than memorizing a single trajectory.

## Robot Configuration

The USD robot model must match the real hardware:

```python
OGRE_MECANUM_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path="~/ros2_ws/src/ogre-slam/usds/ogre_robot.usd",
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.04),  # Wheel axle height
    ),
    actuators={
        "wheels": ImplicitActuatorCfg(
            joint_names_expr=["fl_joint", "fr_joint", "rl_joint", "rr_joint"],
            effort_limit=10.0,
            stiffness=0.0,
            damping=10.0,
        ),
    },
)
```

Key parameters:
- `wheel_radius`: 0.040m (40mm)
- `wheelbase`: 0.095m (95mm)
- `track_width`: 0.205m (205mm)

## The Wheel Sign Challenge

One non-obvious issue: the USD model has the right wheels (FR, RR) with opposite joint axis orientation from the left wheels. Sending `[+10, +10, +10, +10]` to all wheels makes the robot spin instead of moving forward.

The solution is sign correction applied in both training and deployment:

**Training (_apply_action):**
```python
corrected_actions = self.actions.clone()
corrected_actions[:, 0] *= -1  # FR (right wheel)
corrected_actions[:, 1] *= -1  # RR (right wheel)
self.robot.set_joint_velocity_target(corrected_actions)
```

**Deployment (Isaac Sim action graph):**
The same correction must be applied when running the trained policy - multiply FR and RR velocities by -1.

This creates a normalized action space where `[+,+,+,+]` always means "all wheels forward" regardless of joint axis conventions.

## Running Training

### Prerequisites

- Isaac Sim 5.0+
- Isaac Lab installed at `~/isaac-lab/IsaacLab`
- Conda environment `env_isaaclab`

### Step 1: Sync the Environment

The training environment must be copied to Isaac Lab's task directory:

```bash
cd ~/ogre-lab
./scripts/sync_env.sh
```

This copies `ogre_navigation/` to Isaac Lab's source tree where it can be discovered and imported.

### Step 2: Run Training

```bash
conda activate env_isaaclab
cd ~/ogre-lab
./scripts/train_ogre_navigation.sh
```

Default settings:
- 4096 parallel environments
- 1000 iterations
- Headless mode (no visualization)

For debugging, run with fewer environments and visualization:

```bash
./scripts/train_ogre_navigation.sh 64 100
```

### Step 3: Monitor with TensorBoard

TensorBoard provides real-time visualization of training progress. In a separate terminal:

```bash
conda activate env_isaaclab
tensorboard --logdir ~/isaac-lab/IsaacLab/logs/rsl_rl/ogre_navigation/
```

Open http://localhost:6006 in your browser:

<img src="/images/tensorboard.png" alt="TensorBoard Training Metrics" style={{ width: '100%', height: 'auto', display: 'block', margin: '1.5rem 0' }} />

**Key metrics to watch:**

- **Train/mean_reward**: The average reward across all parallel environments. Should steadily increase as the policy learns. A healthy training run shows rewards climbing from near-zero to positive values.

- **Train/mean_episode_length**: How long episodes last before termination. Should approach the maximum (300 steps = 10 seconds at 30Hz). If episodes are short, robots are flipping or hitting termination conditions.

- **Loss/value_function**: The critic's prediction error. Should decrease over time as the value network learns to estimate expected returns. Spikes are normal early in training.

- **Loss/surrogate**: The PPO policy loss. Fluctuates during training but should generally trend downward.

TensorBoard is invaluable for diagnosing training issues. If rewards plateau, you might need to adjust reward weights. If episode lengths stay low, the action scale might be too aggressive.

### What to Expect

```
Learning iteration 50/100
  Mean reward: 1.25        # Positive and increasing = good
  Mean episode length: 299  # Near 300 = robots staying upright
```

If rewards are very negative (-20000), the velocity targets are too aggressive - reduce `max_lin_vel` and `max_ang_vel`. If episode lengths are short, robots are flipping - reduce `action_scale`.

## Exporting the Trained Policy

Once training completes, export the model for deployment:

```bash
cd ~/ogre-lab
./scripts/export_policy.sh
```

This runs Isaac Lab's `play.py` to verify the policy works, then exports:
- `policy.onnx` - For ONNX runtime inference
- `policy.pt` - PyTorch JIT format

The exported models go to the training run's `exported/` directory.

## Deploying to ROS2

Copy the trained model to the ROS2 workspace:

```bash
./scripts/deploy_model.sh --rebuild
```

This copies the latest exported model to `ogre-slam/ogre_policy_controller/models/` and rebuilds the ROS2 package.

### Testing in Isaac Sim

**Terminal 1: Isaac Sim**
- Load `ogre.usd`
- Press Play

**Terminal 2: Policy Controller**
```bash
conda deactivate  # Exit conda - ROS2 uses system Python
source ~/ros2_ws/install/setup.bash
ros2 launch ogre_policy_controller policy_controller.launch.py
```

**Terminal 3: Test Commands**
```bash
# Forward motion (0.3 m/s)
ros2 topic pub /cmd_vel_smoothed geometry_msgs/msg/Twist \
  "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" -r 10

# Strafe left
ros2 topic pub /cmd_vel_smoothed geometry_msgs/msg/Twist \
  "{linear: {x: 0.0, y: 0.3, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" -r 10

# Rotate
ros2 topic pub /cmd_vel_smoothed geometry_msgs/msg/Twist \
  "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}" -r 10
```

Watch the robot in Isaac Sim. It should move smoothly in the commanded direction.

## The Data Flow

```
Nav2 Planner → /cmd_vel → Policy Controller → /joint_command → Isaac Sim/Robot
                               ↓
                         policy.onnx
                               ↓
                    [FL, FR, RL, RR] wheel velocities
```

The policy controller subscribes to `/cmd_vel_smoothed`, runs the velocity command through the neural network, and publishes individual wheel velocities to `/joint_command`.

## Troubleshooting Training

### Robots Exploding/Flying

**Cause**: Physics instability, usually from collision issues or excessive forces

**Solutions**:
- Check USD model has proper collision meshes
- Reduce `action_scale` (try 6.0 instead of 8.0)
- Increase physics solver iterations

### Very Negative Rewards

**Cause**: Velocity targets exceed what the robot can achieve

**Solutions**:
- Reduce `max_lin_vel` and `max_ang_vel`
- Check `action_scale` allows sufficient wheel velocity

### Short Episode Lengths

**Cause**: Robots flipping over

**Solutions**:
- Reduce `action_scale`
- Add stronger uprightness reward
- Check wheel friction parameters

### Policy Doesn't Generalize

**Cause**: Training converged to local minimum

**Solutions**:
- Increase training iterations
- Add curriculum learning (start easy, increase difficulty)
- Tune reward scales

---

## Quick Reference

### Key Files

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>File</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Location</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Purpose</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>ogre_navigation_env.py</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>ogre-lab/isaaclab_env/</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Training environment</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>rsl_rl_ppo_cfg.py</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>ogre-lab/isaaclab_env/agents/</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>PPO hyperparameters</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>policy.onnx</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>ogre-slam/ogre_policy_controller/models/</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Deployed model</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}><code>policy_controller_node.py</code></td>
      <td style={{ padding: '12px 16px' }}>ogre-slam/ogre_policy_controller/</td>
      <td style={{ padding: '12px 16px' }}>ROS2 inference node</td>
    </tr>
  </tbody>
</table>

### Commands

```bash
# Train policy
./scripts/train_ogre_navigation.sh

# Export trained model
./scripts/export_policy.sh

# Deploy to ROS2
./scripts/deploy_model.sh --rebuild

# Run policy controller
ros2 launch ogre_policy_controller policy_controller.launch.py
```

## What's Next

With a trained policy deployed, [Part 5](/blog/project-ogre-part5-nav2-navigation) brings everything together with Nav2 navigation. The policy controller integrates seamlessly - Nav2 plans paths and generates velocity commands, the RL policy executes them optimally, and the robot navigates autonomously through the mapped environment.


Using reinforcement learning in NVIDIA Isaac Lab to train a neural network policy for optimal mecanum wheel velocity control.

Project Ogre Part 4: Training a Mecanum Controller in Isaac Lab


<img src="/images/slam-map.png" alt="SLAM Map" style={{ width: '70%', height: 'auto', display: 'block' }} />

## Building Maps with SLAM

This is Part 3 of the Project Ogre series. In [Part 1](/blog/project-ogre-part1-building-robot), we built the mecanum drive robot, and in [Part 2](/blog/project-ogre-part2-isaac-sim), we created a digital twin in Isaac Sim. Now we'll use both to build maps using SLAM (Simultaneous Localization and Mapping).

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>Project Goal:</strong> Create accurate occupancy grid maps using slam_toolbox that can be used for autonomous navigation with Nav2.
</div>

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>📦 Full Source Code:</strong> All ROS2 packages and configuration files are available at <a href="https://github.com/protomota/ogre-slam" style={{ color: '#ff8c29', textDecoration: 'underline' }}>github.com/protomota/ogre-slam</a>
</div>

The video below shows SLAM mapping in action. On the left is RViz displaying the occupancy grid map as it's being built in real-time. On the right is Isaac Sim where I'm teleoperating the simulated robot around the maze. As the robot explores, the LIDAR scans are processed by slam_toolbox to incrementally construct the map.

<div style={{ position: 'relative', paddingBottom: '56.25%', height: 0, overflow: 'hidden', maxWidth: '100%', marginTop: '1.5rem' }}>
  <iframe
    style={{ position: 'absolute', top: 0, left: 0, width: '100%', height: '100%' }}
    src="https://www.youtube.com/embed/-61Uhd4AEUM"
    title="SLAM Mapping Demonstration"
    frameBorder="0"
    allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture"
    allowFullScreen
  />
</div>

## What is SLAM?

SLAM (Simultaneous Localization and Mapping) solves a chicken-and-egg problem: to know where you are, you need a map. To build a map, you need to know where you are. SLAM algorithms solve both simultaneously by matching sensor data against an incrementally-built map.

For Project Ogre, we use **slam_toolbox** - a robust 2D SLAM implementation that works well with LIDAR sensors. It takes laser scan data from the RPLIDAR and odometry estimates from our encoders, then produces:

1. A **map** (occupancy grid showing obstacles and free space)
2. A **localization estimate** (where the robot is on that map)

## The Two-Computer Setup

Project Ogre uses a split architecture:

**Host Computer (Development)**
- Runs NVIDIA Isaac Sim for simulation
- Runs RViz for visualization
- Handles resource-intensive debugging

**Jetson Orin Nano (Robot)**
- Runs on the physical robot
- Handles real sensor data
- Executes navigation commands

Both computers use the same ROS_DOMAIN_ID to communicate over the network. This lets me visualize the robot's SLAM progress in RViz on my development machine while the actual processing happens on the Jetson.

## Configuring slam_toolbox

The SLAM configuration lives in `config/slam_toolbox_params.yaml`. Here are the key parameters tuned for Project Ogre's hardware:

### Frame Configuration

```yaml
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping
```

These define the TF frames SLAM uses and where it gets laser scan data.

### Scan Processing

```yaml
transform_publish_period: 0.02    # 50 Hz TF publishing
map_update_interval: 2.0          # Update map every 2 seconds
resolution: 0.05                  # 5cm grid cells
max_laser_range: 12.0             # RPLIDAR A1 max range
minimum_time_interval: 0.2        # Minimum time between scans
```

The `map_update_interval` and `resolution` are tuned for the Jetson's memory constraints. Publishing the map every 2 seconds instead of continuously reduces CPU load significantly.

### Critical: Minimum Travel Thresholds

```yaml
minimum_travel_distance: 0.0
minimum_travel_heading: 0.0
```

These are set to zero on purpose. By default, slam_toolbox only processes new scans when the robot has moved a minimum distance or rotated a minimum angle. With our noisy 2 PPR encoders, this caused issues - the map would stop updating because odometry drift made SLAM think the robot hadn't moved.

Setting both to 0.0 forces SLAM to process scans based on time only (`minimum_time_interval`), not odometry.

### Scan Matching Parameters

```yaml
correlation_search_space_dimension: 2.0
correlation_search_space_resolution: 0.01
distance_variance_penalty: 0.3
angle_variance_penalty: 0.5
```

These control how SLAM matches new scans against the existing map. Lower variance penalties mean "trust scan matching more than odometry." Given our encoder limitations, we want SLAM to rely more heavily on the LIDAR data.

### Loop Closure

```yaml
do_loop_closing: true
loop_search_maximum_distance: 6.0
loop_match_minimum_response_coarse: 0.45
loop_match_minimum_response_fine: 0.55
```

Loop closure happens when the robot revisits a previously-seen location. SLAM recognizes the overlap and corrects accumulated drift. These parameters balance between catching real loop closures and avoiding false positives.

## Mapping in Isaac Sim

Before mapping with the real robot, I develop and test in Isaac Sim. This catches configuration issues without burning through battery life or risking hardware damage.

**Step 1: Start Isaac Sim**

Load the `usds/ogre.usd` scene and verify the ROS2 Context node has a matching Domain ID. Press Play to start the simulation.

**Step 2: Launch SLAM**

```bash
cd ~/ros2_ws
source install/setup.bash

ros2 launch ogre_slam mapping.launch.py \
  use_rviz:=true \
  use_teleop:=false \
  use_odometry:=false \
  use_ekf:=false \
  use_sim_time:=true
```

Note the flags:
- `use_sim_time:=true` - Sync with Isaac Sim's clock
- `use_odometry:=false` - Isaac Sim provides odometry directly
- `use_ekf:=false` - Simulated odometry is clean, no filtering needed

**Step 3: Launch the Policy Controller**

The policy controller converts Twist commands to individual wheel velocities:

```bash
ros2 launch ogre_policy_controller policy_controller.launch.py
```

**Step 4: Drive and Map**

```bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard \
  --ros-args -r /cmd_vel:=/cmd_vel_smoothed
```

Drive slowly around the environment. Watch RViz as the map builds in real-time. Make sure to create loop closures by revisiting your starting position.

**Step 5: Save the Map**

```bash
ros2 run nav2_map_server map_saver_cli \
  -f ~/ros2_ws/src/ogre-slam/maps/isaac_sim_map
```

This creates two files:
- `isaac_sim_map.yaml` - Metadata (resolution, origin, etc.)
- `isaac_sim_map.pgm` - The actual map image

## Mapping on the Real Robot

Once SLAM works in simulation, deploying to the Jetson is straightforward:

**Step 1: Launch the Mapping Session**

SSH into the robot and run:

```bash
cd ~/ros2_ws/src/ogre-slam
./scripts/launch_mapping_session.sh
```

This unified script launches:
- RPLIDAR driver
- Encoder-based odometry node
- EKF sensor fusion
- slam_toolbox (mapping mode)
- Web teleop interface

**Step 2: Drive via Web Interface**

Open a browser on any device on the network:

```
http://10.21.21.45:8080
```

The web interface provides joystick-style control. Drive slowly and smoothly - jerky movements confuse the encoder-based odometry.

**Tips for Good Maps:**
- Move at consistent, slow speeds
- Avoid wheel slippage (especially on smooth floors)
- Create multiple loop closures
- Drive the same area from different angles

**Step 3: Save the Map**

```bash
ros2 run nav2_map_server map_saver_cli \
  -f ~/ros2_ws/src/ogre-slam/maps/my_map
```

## Remote Visualization

Running RViz on the Jetson consumes resources needed for SLAM. Instead, run RViz remotely:

**On the robot:**
```bash
ros2 launch ogre_slam mapping.launch.py use_rviz:=false
```

**On your development machine:**
```bash
cd ~/ros2_ws/src/ogre-slam
./scripts/remote_launch_slam_rviz.sh
```

The script sets up the ROS_DOMAIN_ID and launches RViz with pre-configured displays for:
- LaserScan (`/scan`)
- Map (`/map`)
- TF frames
- Robot footprint

## Troubleshooting

### Map Not Updating

**Symptom:** RViz shows an empty or frozen map

**Cause:** Usually odometry issues. slam_toolbox needs meaningful position changes to trigger scan processing.

**Solution:**
1. Verify `/odom` shows changing positions when driving:
   ```bash
   ros2 topic echo /odom --once | grep -A 3 "position"
   ```
2. Check gear_ratio calibration (Part 1)
3. Ensure `minimum_travel_distance: 0.0` in config

### SLAM Losing Track

**Symptom:** Map suddenly rotates or shifts incorrectly

**Cause:** Scan matching failed, possibly from driving too fast or odometry drift

**Solution:**
1. Drive slower
2. Increase `scan_buffer_size` for more reference scans
3. Lower `link_match_minimum_response_fine` to accept weaker matches

### High CPU Usage

**Symptom:** Jetson running hot, sluggish response

**Solution:**
1. Increase `map_update_interval` (try 3.0 or 5.0 seconds)
2. Increase `resolution` (0.10 instead of 0.05)
3. Disable RViz on the Jetson

### No Loop Closure

**Symptom:** Drift accumulates, map doesn't correct when revisiting areas

**Solution:**
1. Ensure `do_loop_closing: true`
2. Lower `loop_match_minimum_response_coarse` to detect more candidates
3. Increase `loop_search_maximum_distance` for wider search

## Understanding the Map Output

This is the 2D SLAM map from the maze I created for my robot within Isaac Sim:

<img src="/images/slam-map.png" alt="SLAM Occupancy Grid Map" style={{ width: '70%', height: 'auto', display: 'block', margin: '1.5rem 0' }} />

The saved map consists of:

**isaac_sim_map.yaml:**
```yaml
image: isaac_sim_map.pgm
mode: trinary
resolution: 0.05
origin: [-10.0, -10.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
```

**isaac_sim_map.pgm:**
A grayscale image where:
- **White (255)**: Free space - safe to traverse
- **Black (0)**: Occupied - obstacles detected
- **Gray (205)**: Unknown - not yet observed

The `resolution` (0.05) means each pixel represents a 5cm × 5cm area. The `origin` defines where the map's (0,0) pixel is in world coordinates.

---

## Quick Reference

### ROS2 Topics

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Topic</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Purpose</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>/scan</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RPLIDAR laser scans (input to SLAM)</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>/odom</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Raw wheel odometry</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>/odometry/filtered</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>EKF-smoothed odometry</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}><code>/map</code></td>
      <td style={{ padding: '12px 16px' }}>Output occupancy grid</td>
    </tr>
  </tbody>
</table>

### Commands

```bash
# Launch mapping (simulation)
ros2 launch ogre_slam mapping.launch.py use_sim_time:=true

# Launch mapping (real robot)
./scripts/launch_mapping_session.sh

# Save map
ros2 run nav2_map_server map_saver_cli -f ~/ros2_ws/src/ogre-slam/maps/my_map

# Check map publishing rate
ros2 topic hz /map
```

## What's Next

With a map saved, the next step is Nav2 integration - but there's a problem. Nav2 sends velocity commands assuming the robot will execute them perfectly. Mecanum wheels are difficult to control precisely. The angled rollers that enable omnidirectional movement also introduce wheel slip, and each motor responds slightly differently. Simple kinematic equations don't account for these real-world factors. None of this crossed my mind when I picked up what looked like a cool robot kit. It is cool - just significantly more complicated than expected.

[Part 4](/blog/project-ogre-part4-isaac-lab-training) covers training a reinforcement learning policy in NVIDIA Isaac Lab. The neural network learns the robot's actual dynamics through millions of simulated trials - compensating for wheel slip, uneven floors, and motor response differences. Setting up the Isaac Lab environment, defining reward functions, tuning hyperparameters, and iterating through training runs was a significant undertaking.

The trained policy sits between Nav2's velocity commands and the wheel motors, translating "go forward at 0.5 m/s" into the precise PWM signals each wheel needs to achieve that motion. Without it, the robot drifts, overshoots, and struggles to follow planned paths accurately.


Using slam_toolbox to build occupancy grid maps for autonomous navigation on a mecanum drive robot.

Project Ogre Part 3: Building Maps with SLAM


<img src="/images/ogre-sim.png" alt="Project Ogre in Isaac Sim" style={{ width: '70%', height: 'auto', display: 'block' }} />

## Building the Digital Twin

This is Part 2 of the Project Ogre series. In [Part 1](/blog/project-ogre-part1-building-robot), we built the physical mecanum drive robot. Now we'll create an accurate digital twin in NVIDIA Isaac Sim that lets us develop and test without touching the real hardware.

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>Project Goal:</strong> Build an accurate simulation of Project Ogre in Isaac Sim that uses the same ROS2 interface as the real robot, enabling simulation-first development.
</div>

<div style={{ padding: '1rem 1.5rem', backgroundColor: '#2a2a2a', borderLeft: '4px solid #ff8c29', borderRadius: '0.375rem', margin: '1.5rem 0', color: '#e0e0e0' }}>
  <strong style={{ color: '#ff8c29' }}>📦 Full Source Code:</strong> The USD robot model and Isaac Sim configuration are available at <a href="https://github.com/protomota/ogre-slam" style={{ color: '#ff8c29', textDecoration: 'underline' }}>github.com/protomota/ogre-slam</a>
</div>

## Why Simulation First?

In an ideal workflow, you'd build the simulation before touching hardware. I was too eager to get a working robot, so I built the physical platform first. I made progress on the real robot, but eventually hit enough issues that I realized constructing a digital twin would make debugging much easier.

The benefits of simulation-first development:

- **No hardware damage risk** - Test aggressive maneuvers without breaking motors or sensors
- **Faster iteration** - Reset the simulation in milliseconds vs. repositioning a physical robot
- **Reproducible experiments** - Same initial conditions every time
- **Parallel development** - Work on navigation code while the hardware is being built

Isaac Sim is NVIDIA's robotics simulation platform built on Omniverse. It provides:

- GPU-accelerated physics (PhysX 5)
- Photorealistic rendering with ray tracing
- Native ROS2 integration
- Sensor simulation (LIDAR, cameras, IMU)

## The USD Robot Model

The robot is defined in a Universal Scene Description (USD) file: `usds/ogre_robot.usd`. This format captures geometry, physics properties, and joint definitions in a single file.

### Creating the Model

I built the USD model by:

1. **Importing the chassis** - A simple box representing the steel frame
2. **Adding wheel assemblies** - Four mecanum wheels with proper roller geometry
3. **Defining joints** - Revolute joints connecting each wheel to the chassis
4. **Setting up collision meshes** - Simplified geometry for physics calculations
5. **Adding sensors** - LIDAR and camera mounts

### Physical Dimensions

The USD model matches the real robot's measurements exactly:

```
Body: 200mm (L) x 160mm (W) x 175mm (H)
Wheel Radius: 40mm
Wheelbase: 95mm (front-to-rear axle distance)
Track Width: 205mm (left-to-right wheel centers)
```

Getting these dimensions right is critical - SLAM and navigation algorithms depend on accurate odometry, which depends on knowing exactly how far the wheels travel per rotation.

### Wheel Configuration

Mecanum wheels require careful attention to roller angles:

```
M4 (FL) --- M1 (FR)
   |    X    |
M3 (RL) --- M2 (RR)
```

- Front-left and rear-right wheels have rollers angled one way
- Front-right and rear-left wheels have rollers angled the opposite way

This arrangement creates the characteristic X-pattern when viewed from above. If the roller angles are wrong, the robot won't strafe properly.

## Joint Configuration

Each wheel joint needs proper physics parameters for stable simulation:

### Joint Properties

```python
# Wheel joint configuration
joint_type: "revolute"
axis: [1, 0, 0]  # Rotate around X-axis
damping: 10.0
friction: 0.1
```

### Critical: Maximum Joint Velocity

Isaac Sim's default maximum joint velocity is extremely high (1,000,000 rad/s). This causes mecanum wheels to spin out of control during physics steps.

**Solution:** Set maximum velocity to 10,000 rad/s or lower:

```python
drive:
  maxVelocity: 10000.0  # NOT the default 1000000!
```

This single parameter took hours to debug. The robot would work fine at low speeds but become unstable at higher velocities.

### Wheel Sign Correction

The USD model has the right wheels (FR, RR) with opposite joint axis orientation from the left wheels. Sending `[+10, +10, +10, +10]` to all wheels makes the robot spin instead of moving forward.

The solution is sign correction in the action graph:

```
Left wheels (FL, RL): velocity as-is
Right wheels (FR, RR): velocity × -1
```

This normalization ensures `[+,+,+,+]` always means "all wheels forward" regardless of joint axis conventions.

## Physics Settings

Stable mecanum simulation requires tuned physics parameters:

### Simulation Timestep

```yaml
physics_dt: 1/120  # 120 Hz physics
rendering_dt: 1/60  # 60 Hz rendering
```

The physics timestep must be faster than your control rate. With 30 Hz control, 120 Hz physics gives four physics steps per control step.

### Wheel Friction

Mecanum wheel friction is counterintuitive. The angled rollers mean friction behaves differently along different axes:

```python
# Ground material
static_friction: 0.8
dynamic_friction: 0.6

# Wheel material
static_friction: 0.5
dynamic_friction: 0.4
```

Too much friction and the wheels won't slide sideways. Too little and the robot drifts uncontrollably.

### Joint Damping

```python
damping: 1.0 to 10.0  # Range for stability
```

Damping prevents joint oscillation. Higher values make the wheels feel "heavier" but more stable. Start with 10.0 and reduce if response feels sluggish.

## ROS2 Integration

Isaac Sim connects to ROS2 through Action Graphs - visual programming nodes that define data flow.

### Setting Up the ROS2 Context

Every Isaac Sim scene with ROS2 needs a context node:

1. Create an Action Graph
2. Add "ROS2 Context" node
3. Set `domain_id` to match your terminals (default: 0)

The domain ID must match between Isaac Sim and your ROS2 terminals, or topics won't be visible.

### Publishing Joint States

To publish wheel positions and velocities:

1. Add "Articulation State" node pointing to the robot
2. Connect to "ROS2 Publish Joint State" node
3. Set topic name: `/joint_states`

### Subscribing to Commands

To receive wheel velocity commands:

1. Add "ROS2 Subscribe Joint State" node
2. Set topic: `/joint_command`
3. Connect to "Articulation Controller" node
4. Set control type: "velocity"

## The Three Action Graphs

Rather than cramming everything into one monolithic action graph, I split the ROS2 integration into three focused graphs. Each handles a specific subsystem, making debugging easier and keeping the visual programming manageable.

### Mecanum Drive Controller

<img src="/images/actiongraph_ros2_mecanum_drive_sm.png" alt="Mecanum Drive Action Graph" style={{ width: '100%', height: 'auto', display: 'block', margin: '1rem 0' }} />

This is the core locomotion graph that handles bidirectional communication between ROS2 and the robot's wheels.

**What it does:**
- Subscribes to `/joint_command` for wheel velocity targets
- Applies wheel sign correction (FR, RR × -1) to normalize joint axis conventions
- Drives the Articulation Controller with velocity commands
- Publishes current joint states to `/joint_states` for odometry feedback

**Key nodes:**
- `ROS2 Context` - Establishes the ROS2 bridge with matching domain ID
- `ROS2 Subscribe Joint State` - Receives velocity commands from the policy controller
- `Articulation Controller` - Applies velocities to the simulated joints
- `Articulation State` - Reads current joint positions and velocities
- `ROS2 Publish Joint State` - Broadcasts state for external nodes

The sign correction happens between the subscriber and the articulation controller. Without it, sending identical velocities to all wheels makes the robot spin instead of driving straight.

### LIDAR Sensor Stream

<img src="/images/actiongraph_ros2_lidar_stream_sm.png" alt="LIDAR Stream Action Graph" style={{ width: '100%', height: 'auto', display: 'block', margin: '1rem 0' }} />

This graph handles the simulated RPLIDAR A1 sensor, streaming 2D laser scan data to SLAM and navigation nodes.

**What it does:**
- Reads from the RTX Lidar sensor attached to the robot
- Converts the point cloud to a 2D LaserScan message
- Publishes to `/scan` at the configured rate (~8 Hz to match the real A1)

**Key nodes:**
- `Isaac Read Lidar Point Cloud` - Pulls data from the RTX Lidar sensor
- `Isaac Create Render Product` - Sets up the rendering pipeline for the sensor
- `ROS2 Publish LaserScan` - Outputs sensor_msgs/LaserScan to `/scan`

**Configuration:**
```yaml
horizontal_fov: 360.0      # Full rotation
horizontal_resolution: 1.0  # 1 degree per sample
max_range: 12.0            # Matches RPLIDAR A1 spec
min_range: 0.15            # Minimum detection distance
```

The simulated LIDAR matches the real sensor's characteristics closely enough that slam_toolbox configuration works identically in both environments.

### RealSense Depth Camera

<img src="/images/actiongraph_ros2_real_sense_camera_sm.png" alt="RealSense Camera Action Graph" style={{ width: '100%', height: 'auto', display: 'block', margin: '1rem 0' }} />

This graph simulates the Intel RealSense D435 depth camera, providing both RGB and depth streams.

**What it does:**
- Captures RGB frames from the simulated camera
- Generates depth images from the camera's depth sensor
- Publishes synchronized image streams to standard RealSense topics

**Published topics:**
- `/camera/color/image_raw` - RGB image (sensor_msgs/Image)
- `/camera/depth/image_rect_raw` - Depth image (sensor_msgs/Image)
- `/camera/camera_info` - Camera intrinsics for 3D reconstruction

**Key nodes:**
- `Isaac Create Render Product` - Sets up the camera rendering pipeline
- `ROS2 Camera Helper` - Handles image encoding and publishing
- `Isaac Read Camera Info` - Extracts camera parameters

**Configuration:**
```yaml
resolution: [640, 480]
frame_rate: 30
depth_range: [0.2, 10.0]  # Matches D435 spec
```

The depth stream feeds into Nav2's voxel layer for 3D obstacle detection - critical for spotting obstacles below the LIDAR's scan plane like table legs or stairs.

### Why Three Separate Graphs?

Splitting into three graphs provides several advantages:

1. **Isolation** - A bug in the camera graph doesn't affect locomotion
2. **Performance** - Each graph can run at its optimal rate (wheels at 50Hz, LIDAR at 8Hz, camera at 30Hz)
3. **Debugging** - Easy to disable one sensor while testing another
4. **Reusability** - The mecanum drive graph works for any 4-wheeled robot

You could combine them into one graph, but the visual complexity becomes overwhelming quickly. Three focused graphs are much easier to maintain.

## Testing the Digital Twin

Once the model is configured, test it incrementally:

### Step 1: Basic Motion

```bash
# Terminal 1: Launch Isaac Sim, load ogre.usd, press Play

# Terminal 2: Send a velocity command
ros2 topic pub /joint_command sensor_msgs/msg/JointState \
  "{velocity: [1.0, 1.0, 1.0, 1.0]}" -r 10
```

All wheels should spin. If the robot spins in place instead of moving forward, check the wheel sign correction.

### Step 2: Omnidirectional Motion

```bash
# Forward
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.3}}" -r 10

# Strafe left
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {y: 0.3}}" -r 10

# Rotate
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  "{angular: {z: 0.5}}" -r 10
```

Test all motion primitives. The strafe test is the true mecanum validation - if it moves diagonally instead of purely sideways, check the wheel arrangement.

### Step 3: Sensor Verification

```bash
# Check LIDAR
ros2 topic echo /scan --once

# Visualize in RViz
rviz2 -d ~/ros2_ws/src/ogre-slam/rviz/isaac_sim.rviz
```

The LIDAR should show walls and obstacles in the simulation environment.

## Simulation vs. Real Robot

The key advantage of this setup: **the same ROS2 code runs in both environments**.

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Component</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Simulation</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Real Robot</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>LIDAR topic</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>/scan</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>/scan</code></td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Odometry</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Isaac Sim physics</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Encoder-based</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Camera</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Simulated RGB-D</td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RealSense D435</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}>Joint commands</td>
      <td style={{ padding: '12px 16px' }}><code>/joint_command</code></td>
      <td style={{ padding: '12px 16px' }}><code>/joint_command</code></td>
    </tr>
  </tbody>
</table>

The only difference is launch parameters. The same SLAM, navigation, and control code works in both places.

### Simulation-Only Flags

```bash
# Simulation
ros2 launch ogre_slam mapping.launch.py use_sim_time:=true

# Real robot
ros2 launch ogre_slam mapping.launch.py use_sim_time:=false
```

The `use_sim_time` flag tells ROS2 to use Isaac Sim's clock instead of wall clock. Without this, TF transforms time out because simulation time doesn't match real time.

## Common Issues

### Robot Falls Through Floor

**Cause:** Collision meshes not enabled or ground plane missing

**Solution:**
1. Ensure robot has collision enabled on all rigid bodies
2. Add a ground plane with collision

### Wheels Spin But Robot Doesn't Move

**Cause:** Wheel friction too low or joint damping too high

**Solution:**
1. Increase ground friction
2. Reduce joint damping
3. Check wheel contact with ground

### Robot Vibrates or Explodes

**Cause:** Physics instability from timestep or mass issues

**Solution:**
1. Reduce physics timestep (increase Hz)
2. Check mass distribution is reasonable
3. Reduce joint stiffness

### Topics Not Visible

**Cause:** ROS_DOMAIN_ID mismatch

**Solution:**
1. Check Isaac Sim ROS2 Context domain_id
2. Match in terminal: `export ROS_DOMAIN_ID=0`

---

## Quick Reference

### Key Files

<table style={{ width: '100%', borderCollapse: 'separate', borderSpacing: '0' }}>
  <thead>
    <tr>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>File</th>
      <th style={{ textAlign: 'left', padding: '12px 16px', borderBottom: '2px solid #444' }}>Purpose</th>
    </tr>
  </thead>
  <tbody>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>usds/ogre_robot.usd</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Robot model with physics</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>usds/ogre.usd</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Complete scene with environment</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>actiongraph_ros2_mecanum_drive_sm</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>Wheel control and joint state publishing</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>actiongraph_ros2_lidar_stream_sm</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>LIDAR sensor simulation</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}><code>actiongraph_ros2_real_sense_camera_sm</code></td>
      <td style={{ padding: '12px 16px', borderBottom: '1px solid #333' }}>RGB-D camera simulation</td>
    </tr>
    <tr>
      <td style={{ padding: '12px 16px' }}><code>rviz/isaac_sim.rviz</code></td>
      <td style={{ padding: '12px 16px' }}>RViz config for simulation visualization</td>
    </tr>
  </tbody>
</table>

### Isaac Sim Settings

```yaml
Physics:
  timestep: 1/120 (120 Hz)
  solver_iterations: 4

Wheel Joints:
  max_velocity: 10000 rad/s
  damping: 10.0

Ground:
  static_friction: 0.8
  dynamic_friction: 0.6
```

## What's Next

With the digital twin working, [Part 3](/blog/project-ogre-part3-slam-mapping) covers using slam_toolbox to build maps. We'll run SLAM in both simulation and on the real robot, comparing results and tuning parameters for accurate mapping.

The ability to test SLAM in simulation before deploying to hardware saves significant time - you can iterate on configuration without draining the robot's battery or worrying about collisions.


Creating an accurate digital twin of the mecanum drive robot in NVIDIA Isaac Sim for simulation-first development and testing.

Project Ogre Part 2: Building the Digital Twin in Isaac Sim

Building a 4-wheel mecanum drive robot from scratch with NVIDIA Jetson Orin Nano, RPLIDAR, and RealSense depth camera for autonomous navigation.

Posts tagged "mecanum"

Project Ogre Part 5: Autonomous Navigation with Nav2

Project Ogre Part 4: Training a Mecanum Controller in Isaac Lab

Project Ogre Part 3: Building Maps with SLAM

Project Ogre Part 2: Building the Digital Twin in Isaac Sim

Project Ogre Part 1: Building a Mecanum Drive Robot with Jetson Orin Nano