3.4 Nav2: Path Planning for Bipedal Humanoid Constraints

Introduction

The Navigation Challenge: You have a map from VSLAM. You know where the robot is. Now: how does it get from point A to point B without hitting walls, falling over, or violating physical constraints?

The Nav2 Solution: The ROS 2 Navigation Stack (Nav2) provides a complete autonomous navigation framework: global path planning, local obstacle avoidance, recovery behaviors, and velocity control — all integrated with your VSLAM localization.

The Humanoid Twist: Unlike wheeled robots that can spin in place and accelerate instantly, bipedal humanoids have strict constraints:

Step width: 0.2-0.4m (too wide = splits, too narrow = unstable)
Balance margins: Cannot accelerate >0.5 m/s² without tipping
Turning radius: Must walk in arcs, cannot pivot on one foot
Step frequency: 1-2 Hz (limited by gait dynamics)

This section teaches you to:

Configure Nav2 for humanoid-specific constraints
Design costmaps accounting for bipedal footprints and balance
Create behavior trees for navigation tasks and recovery
Tune planners for smooth, feasible paths
Handle failures (no valid path, stuck robot, dynamic obstacles)

Real-World Impact: Boston Dynamics' Atlas and Agility Robotics' Digit use similar constrained planning for bipedal navigation.

Nav2 Architecture Overview

┌──────────────────────────────────────────────────────────────────┐
│                        Nav2 Navigation Stack                     │
├──────────────────────────────────────────────────────────────────┤
│                                                                  │
│  ┌────────────┐         ┌────────────┐         ┌─────────────┐  │
│  │   Global   │ ──────> │   Local    │ ──────> │  Controller │  │
│  │  Planner   │  Path   │  Planner   │  Path   │   (DWB)     │  │
│  └────────────┘         └────────────┘         └─────────────┘  │
│       │                      │                        │          │
│       │ Uses Global          │ Uses Local             │ Output   │
│       ▼ Costmap              ▼ Costmap                ▼          │
│  ┌────────────┐         ┌────────────┐         ┌─────────────┐  │
│  │ Global Map │         │ Local Map  │         │   cmd_vel   │  │
│  │ (static)   │         │ (dynamic)  │         │  (motion)   │  │
│  └────────────┘         └────────────┘         └─────────────┘  │
│                                                                  │
│  ┌───────────────────────────────────────────────────────────┐  │
│  │               Behavior Tree Coordinator                    │  │
│  │   (NavigateToPose, FollowPath, Spin, Wait, Backup...)     │  │
│  └───────────────────────────────────────────────────────────┘  │
│                                                                  │
└──────────────────────────────────────────────────────────────────┘

Key Components:

Global Planner: Computes optimal path from start to goal using the full map (A*, Dijkstra, Smac)
Local Planner: Adjusts path in real-time for dynamic obstacles and kinematic constraints
Controller: Tracks the local path and generates velocity commands
Costmaps: Represent obstacles and traversability (higher cost = less desirable)
Behavior Trees: High-level task coordination (retry logic, recovery behaviors)
Recovery Behaviors: Actions when stuck (spin, backup, clear costmap)

Challenge 1: Bipedal Footprint

Wheeled Robot: Circular or rectangular footprint, can rotate in place

Humanoid Robot: Two feet with specific placement constraints

Footprint Representation:

     Front
       ↑
   ┌──────┐
   │ Left │  ← 0.1m × 0.25m per foot
   └──────┘
      ↕ 0.15m (step width)
   ┌──────┐
   │ Right│
   └──────┘

Costmap Configuration: Use two-circle approximation or polygon footprint:

Circle 1: Left foot (radius 0.12m, offset +0.075m left)
Circle 2: Right foot (radius 0.12m, offset -0.075m right)
Combined footprint: 0.25m wide × 0.25m long

Consequence: Paths must ensure both feet can fit through doorways and corridors.

Challenge 2: Balance Constraints

Wheeled Robot: Can stop instantly, accelerate at 2 m/s²

Humanoid Robot: Must maintain center of mass (COM) over support polygon

Physics:

Maximum acceleration: ~0.3 m/s² (before tipping)
Maximum deceleration: ~0.5 m/s² (can lean backward)
Maximum lateral velocity: ~0.2 m/s (sideways walking is slow)

Costmap Strategy: Inflation radius creates soft boundaries:

Inner radius (0.25m): High cost (robot footprint)
Inflation radius (0.5m): Gradient cost (balance margin)
Paths prefer staying >0.5m from walls to allow deceleration space

Example: Robot approaches a wall at 0.5 m/s:

Without inflation: Plans path 0.26m from wall → Needs 0.5m to stop → Collision!
With 0.5m inflation: Plans path 0.8m from wall → Has 0.8m to stop → Safe

Challenge 3: Turning Radius

Wheeled Robot: Differential drive can spin in place (zero turning radius)

Humanoid Robot: Must walk in arcs

Kinematic Constraint: Minimum turning radius ≈ 0.5m (depends on step width and gait)

Planner Requirement: Use Ackermann-style constraints (like cars) or State Lattice planners

Example Path Comparison:

Wheeled (spin in place):
Start → ↻ (rotate 90°) → Forward → Goal

Humanoid (arc turns):
Start → ⤴ (curved path) → Goal

Configuration: Set min_turning_radius: 0.5 in planner params.

Challenge 4: Step Frequency Limits

Wheeled Robot: Can change velocity every control cycle (50 Hz)

Humanoid Robot: Gait changes are discrete (one step = 0.5-1 second)

Implication: Velocity commands must be smooth to avoid unstable gait transitions

Velocity Smoother: Filters cmd_vel to respect:

max_acceleration: 0.3 m/s²
max_jerk: 2.0 m/s³ (rate of acceleration change)
smoothing_frequency: 20 Hz

Without Smoother: Controller sends cmd_vel: [0.0, 0.5, 0.0, 0.5, ...] → Robot staggers With Smoother: Smoothed cmd_vel: [0.0, 0.1, 0.2, 0.3, 0.4, 0.5] → Smooth walking

Costmaps: Representing Traversability

What is a Costmap?

A costmap is a 2D grid where each cell has a value (0-255) representing traversability:

0 (free): No obstacles, safe to traverse
1-127 (low cost): Inflation zone, prefer avoiding
128-252 (high cost): Near obstacles, dangerous
253 (inscribed): Robot footprint touches obstacle
254 (lethal): Obstacle occupies cell, impassable
255 (unknown): Unexplored area

Example Costmap (5×5 grid, obstacle in center):

 0   0   0   0     ← Free space
50 128 50   0     ← Inflation gradient
128 254 128  0     ← Lethal obstacle at center
50 128 50   0
 0   0   0   0

Planner Behavior: Prefers paths through cells with lower cost (avoids obstacles and inflation zones).

Global Costmap vs. Local Costmap

Feature	Global Costmap	Local Costmap
Purpose	Strategic planning (start to goal)	Reactive obstacle avoidance
Size	Entire map (100m × 100m)	Small window (5m × 5m around robot)
Update Rate	Slow (1 Hz)	Fast (5-10 Hz)
Source	VSLAM static map	Real-time sensor data (depth, LiDAR)
Layers	Static obstacles only	Static + dynamic obstacles
Use	Global planner (Dijkstra, A*)	Local planner (DWB, TEB)

Workflow:

Global planner uses global costmap → Computes long path avoiding walls
Local planner uses local costmap → Adjusts path for people, chairs, unexpected obstacles
If local planner can't find valid path → Request global replan

Costmap Layers

Nav2 costmaps are composed of layers (like Photoshop):

1. Static Layer

Source: VSLAM map or pre-built occupancy grid Purpose: Permanent obstacles (walls, furniture bolted to floor) Configuration:

static_layer:
  plugin: "nav2_costmap_2d::StaticLayer"
  map_topic: "/map"  # From map server or VSLAM
  subscribe_to_updates: true

2. Obstacle Layer

Source: Real-time sensor data (depth cameras, LiDAR) Purpose: Dynamic obstacles (people, movable objects) Configuration:

obstacle_layer:
  plugin: "nav2_costmap_2d::ObstacleLayer"
  observation_sources: "depth_camera"
  depth_camera:
    topic: "/camera/depth/points"
    sensor_frame: "camera_depth_optical_frame"
    marking: true  # Add obstacles to costmap
    clearing: true  # Remove obstacles when no longer visible
    max_obstacle_height: 2.0  # Ignore ceiling
    min_obstacle_height: 0.1  # Ignore floor noise

How It Works:

Depth camera publishes point cloud
Obstacle layer projects points into 2D grid
Cells with points → marked as obstacles (cost = 254)
Cells without points → cleared (cost = 0)

3. Inflation Layer

Purpose: Create safety margin around obstacles Configuration:

inflation_layer:
  plugin: "nav2_costmap_2d::InflationLayer"
  inflation_radius: 0.5  # Humanoid balance margin
  cost_scaling_factor: 5.0  # Steepness of cost gradient

Cost Calculation:

cost(d) = 253 * exp(-cost_scaling_factor * (d - robot_radius))

Where d = distance to obstacle

Example:

At obstacle edge (d=0): cost = 253 (inscribed)
At d=0.25m: cost = 128 (high)
At d=0.5m: cost = 30 (low)
At d>0.5m: cost = 0 (free)

Effect: Paths naturally stay >0.5m from walls.

Humanoid Costmap Configuration

Create costmap_params.yaml:

global_costmap:
  global_costmap:
    ros__parameters:
      # Frame configuration
      global_frame: "map"
      robot_base_frame: "base_link"

      # Costmap dimensions
      width: 100  # meters
      height: 100
      resolution: 0.05  # 5cm per cell

      # Update rates
      update_frequency: 1.0  # Hz
      publish_frequency: 0.5

      # Humanoid footprint (two-circle approximation)
      footprint: "[
        [-0.125, 0.075], [0.125, 0.075],   # Left foot
        [0.125, -0.075], [-0.125, -0.075]  # Right foot
      ]"

      # Plugins (layers)
      plugins: ["static_layer", "inflation_layer"]

      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_topic: "/map"

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.5  # Balance margin
        cost_scaling_factor: 5.0

local_costmap:
  local_costmap:
    ros__parameters:
      # Frame configuration
      global_frame: "odom"
      robot_base_frame: "base_link"

      # Costmap dimensions (rolling window)
      width: 5  # meters (5m × 5m around robot)
      height: 5
      resolution: 0.05
      rolling_window: true  # Moves with robot

      # Update rates
      update_frequency: 5.0  # Hz (faster for obstacles)
      publish_frequency: 2.0

      # Humanoid footprint
      footprint: "[
        [-0.125, 0.075], [0.125, 0.075],
        [0.125, -0.075], [-0.125, -0.075]
      ]"

      # Plugins
      plugins: ["obstacle_layer", "inflation_layer"]

      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: "depth_camera"
        depth_camera:
          topic: "/camera/depth/points"
          sensor_frame: "camera_depth_optical_frame"
          marking: true
          clearing: true
          max_obstacle_height: 2.0
          min_obstacle_height: 0.1

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.4  # Smaller for reactive avoidance
        cost_scaling_factor: 5.0

Path Planners: Finding the Route

Global Planners

1. NavFn (Dijkstra's Algorithm)

How It Works: Flood-fill from goal, computing cost to every cell

Advantages:

Always finds optimal path if one exists
Smooth paths (follows cost gradients)

Disadvantages:

Slow for large maps (100m × 100m = 4 million cells)
Does not consider robot kinematics (may plan infeasible turns)

Configuration:

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5  # Goal tolerance (meters)
      use_astar: false  # Use Dijkstra (true = A* heuristic)
      allow_unknown: false  # Avoid unexplored areas

Use Case: Default choice for most indoor navigation (fast enough, reliable).

2. Smac Planner (State Lattice)

How It Works: Searches over discrete states (x, y, theta) considering kinematic constraints

Advantages:

Respects turning radius and minimum step width
Generates kinematically feasible paths (humanoid can actually follow them)

Disadvantages:

Slower than NavFn (10x more states to search)
Requires careful tuning of motion primitives

Configuration:

SmacPlanner:
  plugin: "nav2_smac_planner/SmacPlannerLattice"
  tolerance: 0.5
  downsample_costmap: false
  downsampling_factor: 1
  allow_unknown: false
  max_iterations: 1000000

  # Humanoid motion primitives
  minimum_turning_radius: 0.5  # meters
  cache_obstacle_heuristic: true
  reverse_penalty: 2.0  # Humanoids don't walk backward well

  # Lattice parameters
  lattice_filepath: "humanoid_lattice.json"

Lattice File (humanoid_lattice.json): Defines allowed motions (forward steps, turning arcs, side steps):

{
  "primitives": [
    {"dx": 0.3, "dy": 0.0, "dtheta": 0.0},      // Forward step
    {"dx": 0.25, "dy": 0.0, "dtheta": 0.1},     // Forward + turn left
    {"dx": 0.25, "dy": 0.0, "dtheta": -0.1},    // Forward + turn right
    {"dx": 0.0, "dy": 0.1, "dtheta": 0.0}       // Side step left
  ]
}

Use Case: Recommended for humanoids when kinematic feasibility is critical.

Local Planners (Controllers)

DWB (Dynamic Window Approach)

How It Works:

Sample velocity trajectories (v_x, v_y, v_theta) within dynamic window
Simulate robot motion for each trajectory (2 seconds forward)
Score trajectories using critics (obstacle distance, goal alignment, path following)
Select highest-scoring feasible trajectory
Execute first velocity command, repeat next cycle

Dynamic Window: Set of velocities reachable within one control cycle given acceleration limits

Example (current velocity = 0.3 m/s):

Max acceleration: 0.3 m/s²
Control period: 0.1s
Dynamic window: [0.27, 0.33] m/s (±0.03 m/s)

Configuration:

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"

      # Velocity limits (humanoid constraints)
      min_vel_x: 0.0
      max_vel_x: 0.5  # Conservative walking speed
      min_vel_y: 0.0  # No strafing for bipeds
      max_vel_y: 0.0
      max_vel_theta: 0.8  # Turning rate
      min_vel_theta: -0.8

      # Acceleration limits (balance constraints)
      acc_lim_x: 0.3  # m/s² (prevent tipping)
      acc_lim_y: 0.0
      acc_lim_theta: 1.0  # rad/s²
      decel_lim_x: -0.5  # Can decelerate faster
      decel_lim_theta: -1.5

      # Trajectory sampling
      vx_samples: 20  # Test 20 forward velocities
      vy_samples: 1   # No lateral motion
      vtheta_samples: 20  # Test 20 turning rates
      sim_time: 2.0  # Predict 2 seconds ahead

      # Trajectory critics (scoring functions)
      critics: [
        "RotateToGoal",     # Align to goal orientation
        "Oscillation",      # Penalize back-and-forth motion
        "BaseObstacle",     # Avoid obstacles
        "GoalAlign",        # Face goal direction
        "PathAlign",        # Stay on global path
        "PathDist",         # Minimize distance to path
        "GoalDist"          # Minimize distance to goal
      ]

      # Critic weights
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      GoalAlign.scale: 24.0
      PathDist.scale: 32.0
      GoalDist.scale: 24.0

Critic Explanations:

RotateToGoal: When near goal, prioritize aligning orientation
Oscillation: Prevent "wiggling" (changing direction frequently)
BaseObstacle: Score trajectories by clearance to obstacles
PathAlign: Prefer trajectories parallel to global path
PathDist: Stay close to global path
GoalDist: Make progress toward goal

Use Case: Default for most robots, works well for humanoids with proper velocity limits.

What is a Behavior Tree?

A behavior tree is a hierarchical structure for decision-making:

Sequence: Execute children left-to-right until one fails
Fallback: Execute children until one succeeds (try alternatives)
Action: Leaf node that performs a task (FollowPath, Spin, Wait)

Example Tree (Navigate to Goal):

Fallback (NavigateWithRecovery)
├─ Sequence (NormalNavigation)
│  ├─ ComputePathToPose
│  └─ FollowPath
└─ Sequence (RecoveryActions)
   ├─ ClearCostmap
   ├─ Spin (360°)
   ├─ Wait (3s)
   └─ BackUp (0.3m)

Execution:

Try NormalNavigation sequence
If ComputePathToPose or FollowPath fails → Sequence fails
Fall back to RecoveryActions
Execute recoveries in order (clear costmap, spin, wait, backup)
After recoveries, retry NormalNavigation

Humanoid Behavior Tree

Create behavior_tree.xml:

<root main_tree_to_execute="HumanoidNavigate">
  <BehaviorTree ID="HumanoidNavigate">
    <RecoveryNode number_of_retries="3" name="NavigateRecovery">

      <!-- Main navigation sequence -->
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>

        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>

      <!-- Recovery behaviors (fallback) -->
      <SequenceStar name="RecoveryActions">
        <ClearEntireCostmap name="ClearAllCostmaps" service_name="local_costmap/clear_entirely_local_costmap"/>

        <!-- Gentle spin (humanoid-safe) -->
        <Spin spin_dist="1.57" name="SpinRecovery"/>  <!-- 90° rotation -->

        <!-- Stabilization pause -->
        <Wait wait_duration="2.0" name="WaitForStabilization"/>

        <!-- Conservative backup -->
        <BackUp backup_dist="0.3" backup_speed="0.1" name="BackupRecovery"/>
      </SequenceStar>

    </RecoveryNode>
  </BehaviorTree>
</root>

Key Humanoid Adaptations:

Spin: 90° instead of 360° (faster, less destabilizing)
Wait: 2s after spin (allow balance to settle)
Backup: 0.3m at 0.1 m/s (conservative, prevents falling backward)
Retry limit: 3 attempts (avoid infinite recovery loops)

Velocity Smoothing: Preventing Gait Instability

The Problem

DWB controller outputs cmd_vel at 20 Hz. Velocity can change abruptly:

t=0.0s:  v=0.5 m/s
t=0.05s: v=0.2 m/s  (obstacle detected)
t=0.10s: v=0.5 m/s  (obstacle cleared)

Humanoid Gait: Cannot change velocity mid-step → Jerky motion, potential fall

The Solution: Velocity Smoother

Purpose: Filter cmd_vel to respect acceleration and jerk limits

Configuration (velocity_smoother_params.yaml):

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0  # Hz
    scale_velocities: false
    feedback: "OPEN_LOOP"

    # Velocity limits
    max_velocity: [0.5, 0.0, 0.8]  # [v_x, v_y, v_theta]
    min_velocity: [-0.1, 0.0, -0.8]  # Allow slow backward

    # Acceleration limits (balance constraints)
    max_accel: [0.3, 0.0, 1.0]  # m/s², rad/s²
    max_decel: [-0.5, 0.0, -1.5]

    # Jerk limits (smoothness)
    max_jerk: [2.0, 0.0, 5.0]  # m/s³, rad/s³

    # Deadband (ignore tiny velocities)
    deadband_velocity: [0.01, 0.0, 0.01]
    velocity_timeout: 1.0  # Stop if no cmd_vel for 1s

    odom_topic: "/odom"
    odom_duration: 0.1

Effect:

Raw cmd_vel:      [0.0, 0.5, 0.5, 0.0, 0.5, ...]  (jerky)
Smoothed cmd_vel: [0.0, 0.1, 0.2, 0.3, 0.4, 0.5]  (gradual)

Hands-On: Launching Nav2 for Humanoids

Step 1: Verify VSLAM is Running

Nav2 requires localization (robot pose in map):

# Check VSLAM pose output
ros2 topic hz /vslam/pose

# Expected: ~30 Hz

# Check map availability
ros2 topic echo /map --once

# Expected: OccupancyGrid message with width, height, data

Step 2: Launch Nav2 Stack

Create nav2_humanoid.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    config_dir = os.path.join(os.getcwd(), 'config')

    return LaunchDescription([
        # Nav2 lifecycle manager
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_navigation',
            output='screen',
            parameters=[{
                'use_sim_time': True,
                'autostart': True,
                'node_names': [
                    'controller_server',
                    'planner_server',
                    'recoveries_server',
                    'bt_navigator',
                    'velocity_smoother'
                ]
            }]
        ),

        # Controller server
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[os.path.join(config_dir, 'planner_params.yaml')],
            remappings=[('/cmd_vel', '/cmd_vel_raw')]  # Before smoothing
        ),

        # Planner server
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[os.path.join(config_dir, 'planner_params.yaml')]
        ),

        # Recoveries server
        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            output='screen',
            parameters=[os.path.join(config_dir, 'recovery_params.yaml')]
        ),

        # Behavior tree navigator
        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[{
                'use_sim_time': True,
                'default_bt_xml_filename': os.path.join(config_dir, 'behavior_tree.xml')
            }]
        ),

        # Velocity smoother
        Node(
            package='nav2_velocity_smoother',
            executable='velocity_smoother',
            name='velocity_smoother',
            output='screen',
            parameters=[os.path.join(config_dir, 'velocity_smoother_params.yaml')],
            remappings=[
                ('/cmd_vel_smoothed', '/cmd_vel'),  # Output to robot
                ('/cmd_vel', '/cmd_vel_raw')  # Input from controller
            ]
        ),
    ])

Launch it:

ros2 launch nav2-humanoid nav2_humanoid.launch.py

# Expected output:
[lifecycle_manager]: Starting managed nodes...
[controller_server]: Configured and activated
[planner_server]: Configured and activated
[recoveries_server]: Configured and activated
[bt_navigator]: Configured and activated
[velocity_smoother]: Configured and activated
[lifecycle_manager]: All nodes started successfully

Option 1: RViz 2D Nav Goal

# Launch RViz
ros2 run rviz2 rviz2

# In RViz:
Set Fixed Frame to "map"
Add → By topic → /map → Map
Add → By topic → /global_costmap/costmap → Costmap
Add → By topic → /local_costmap/costmap → Costmap
Add → By topic → /plan → Path
Toolbar → "2D Nav Goal" button
Click and drag to set goal pose (position + orientation)

Option 2: Command Line

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{
  pose: {
    header: {
      frame_id: 'map'
    },
    pose: {
      position: {x: 5.0, y: 2.0, z: 0.0},
      orientation: {x: 0.0, y: 0.0, z: 0.707, w: 0.707}
    }
  }
}" --feedback

# Navigate to (5.0, 2.0) facing 90° left

Expected Output:

Feedback:
  current_pose: {x: 0.5, y: 0.1, ...}
  distance_remaining: 5.2
  navigation_time: 3.5
  estimated_time_remaining: 7.2

Check velocity commands:

ros2 topic echo /cmd_vel

# Expected output (while moving):
linear:
  x: 0.35  # Forward velocity
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.15  # Turning rate

Verify velocity limits respected:

|linear.x| ≤ 0.5 m/s
|angular.z| ≤ 0.8 rad/s

Check costmaps:

ros2 topic echo /global_costmap/costmap --once

# Verify resolution: 0.05m
# Verify footprint applied

Objective

Configure Nav2 for a humanoid robot and successfully navigate a cluttered environment.

Scenario

10m × 10m room with obstacles (tables, chairs)
Narrow doorway (0.8m wide)
Dynamic obstacle (moving person)
Goal: Navigate from start to opposite corner

Procedure

Configure costmaps with humanoid footprint and inflation
Set DWB velocity limits (max 0.5 m/s linear, 0.8 rad/s angular)
Enable velocity smoother (max accel 0.3 m/s²)
Launch Nav2 with custom configs
Send goal through narrow doorway
Observe behavior:
- Does path fit through doorway?
- Does robot slow down near obstacles?
- Does velocity smoother prevent jerky motion?
Introduce dynamic obstacle (person walks across path)
- Does local planner replan around person?
- Does robot trigger recovery if blocked?

Success Criteria

✅ Global path computed in <5 seconds ✅ Path avoids static obstacles with >0.5m margin ✅ Path fits through 0.8m doorway (humanoid footprint <0.25m wide) ✅ Velocity commands respect limits (≤0.5 m/s, ≤0.8 rad/s) ✅ Smooth acceleration (no sudden changes >0.3 m/s²) ✅ Local planner avoids dynamic obstacle in real-time ✅ Robot reaches goal within 10% of optimal time ✅ Recovery behaviors trigger if stuck (spin, backup)

Failure 1: No Valid Path (Infeasible Goal)

Symptom: Planner reports "FAILURE: No valid path found"

Causes:

Goal is inside an obstacle (costmap shows lethal cost at goal)
Goal requires turns tighter than minimum turning radius
Doorway too narrow for humanoid footprint

Diagnostics:

# Check goal cost
ros2 topic echo /global_costmap/costmap --once

# Look up goal coordinates in costmap data
# If cost > 253 → Goal is in obstacle

Mitigation:

Increase goal tolerance: tolerance: 1.0 (accept "close enough")
Reduce footprint temporarily (risky)
Choose different goal

Failure 2: Stuck Robot (Local Planner Blocked)

Symptom: Robot stops, oscillates back and forth, recovery behaviors triggered

Causes:

Local costmap shows obstacles on all sides
Global path leads through untraversable area
Robot is too close to obstacle (within inscribed radius)

Diagnostics:

# Visualize local costmap in RViz
# Check if any free space exists in 5m window

Recovery Sequence (from behavior tree):

Clear local costmap (maybe sensor glitch)
Spin 90° (try different heading)
Wait 2s (allow environment to change, e.g., person moves)
Backup 0.3m (create space)
Request global replan

If recovery fails 3 times → Report navigation failure to user

Failure 3: Oscillation (Wiggling)

Symptom: Robot wiggles left-right while moving forward, unstable gait

Causes:

DWB controller changing heading frequently
Oscillation critic weight too low
Path alignment critic too low

Fix:

# Increase oscillation penalty
Oscillation.scale: 5.0  # (was 1.0)

# Increase path alignment
PathAlign.scale: 64.0  # (was 32.0)

Effect: Prefer trajectories that maintain current heading

Failure 4: Collisions Despite Costmap

Symptom: Robot hits obstacles even though costmap shows them

Causes:

Footprint configuration wrong (smaller than actual robot)
Sensor blind spot (obstacle not in camera FOV)
Sensor latency (obstacle detected too late)

Diagnostics:

# Verify footprint matches robot dimensions
ros2 param get /local_costmap/local_costmap footprint

# Check sensor update rate
ros2 topic hz /camera/depth/points

# Expected: ≥10 Hz

Fix:

Correct footprint polygon
Add additional sensors (more cameras, LiDAR)
Increase inflation radius (more margin)

Check Your Understanding

What is the difference between global and local costmaps?
- A) Global is faster
- B) Global for strategic planning (full map), local for reactive avoidance (small window)
- C) Global uses cameras, local uses LiDAR
- Answer: B
Why do humanoids need larger inflation radius than wheeled robots?
- A) Humanoids are larger
- B) Humanoids need deceleration distance (cannot stop instantly)
- C) Humanoids are slower
- Answer: B
What does the velocity smoother prevent?
- A) Exceeding maximum velocity
- B) Jerky acceleration that destabilizes gait
- C) Collisions
- Answer: B
What is a behavior tree used for?
- A) Planning paths
- B) Coordinating navigation tasks and recovery behaviors
- C) Controlling motors
- Answer: B
What happens when DWB local planner cannot find a valid trajectory?
- A) Robot stops immediately
- B) Triggers recovery behaviors (clear costmap, spin, backup)
- C) Switches to manual control
- Answer: B

Key Takeaways

✅ Nav2 provides complete navigation stack: Global planning, local avoidance, recovery behaviors

✅ Humanoids require special constraints: Step width, balance margins, turning radius, smooth acceleration

✅ Costmaps represent traversability: Inflation layers create safety margins around obstacles

✅ Behavior trees coordinate tasks: Try navigation, fall back to recoveries on failure

✅ Velocity smoothing prevents instability: Filter cmd_vel to respect acceleration and jerk limits

✅ Navigation can fail: Infeasible paths, stuck robot, oscillation — require robust diagnostics and recovery

Next: 3.5 Complete AI Perception-Navigation Pipeline

Introduction​

Nav2 Architecture Overview​

The Navigation Stack​

Humanoid-Specific Navigation Challenges​

Challenge 1: Bipedal Footprint​

Challenge 2: Balance Constraints​

Challenge 3: Turning Radius​

Challenge 4: Step Frequency Limits​

Costmaps: Representing Traversability​

What is a Costmap?​

Global Costmap vs. Local Costmap​

Costmap Layers​

1. Static Layer​

2. Obstacle Layer​

3. Inflation Layer​

Humanoid Costmap Configuration​

Path Planners: Finding the Route​

Global Planners​

1. NavFn (Dijkstra's Algorithm)​

2. Smac Planner (State Lattice)​

Local Planners (Controllers)​

DWB (Dynamic Window Approach)​

Behavior Trees: Coordinating Navigation Tasks​

What is a Behavior Tree?​

Humanoid Behavior Tree​

Velocity Smoothing: Preventing Gait Instability​

The Problem​

The Solution: Velocity Smoother​

Hands-On: Launching Nav2 for Humanoids​

Step 1: Verify VSLAM is Running​

Step 2: Launch Nav2 Stack​

Step 3: Send Navigation Goal​

Option 1: RViz 2D Nav Goal​

Option 2: Command Line​

Step 4: Monitor Navigation​

Practice Exercise: Humanoid Navigation Challenge​

Objective​

Scenario​

Procedure​

Success Criteria​

Reality Check: When Navigation Fails​

Failure 1: No Valid Path (Infeasible Goal)​

Failure 2: Stuck Robot (Local Planner Blocked)​

Failure 3: Oscillation (Wiggling)​

Failure 4: Collisions Despite Costmap​

Check Your Understanding​

Key Takeaways​

Introduction

Nav2 Architecture Overview

The Navigation Stack

Humanoid-Specific Navigation Challenges

Challenge 1: Bipedal Footprint

Challenge 2: Balance Constraints

Challenge 3: Turning Radius

Challenge 4: Step Frequency Limits

Costmaps: Representing Traversability

What is a Costmap?

Global Costmap vs. Local Costmap

Costmap Layers

1. Static Layer

2. Obstacle Layer

3. Inflation Layer

Humanoid Costmap Configuration

Path Planners: Finding the Route

Global Planners

1. NavFn (Dijkstra's Algorithm)

2. Smac Planner (State Lattice)

Local Planners (Controllers)

DWB (Dynamic Window Approach)

Behavior Trees: Coordinating Navigation Tasks

What is a Behavior Tree?

Humanoid Behavior Tree

Velocity Smoothing: Preventing Gait Instability

The Problem

The Solution: Velocity Smoother

Hands-On: Launching Nav2 for Humanoids

Step 1: Verify VSLAM is Running

Step 2: Launch Nav2 Stack

Step 3: Send Navigation Goal

Option 1: RViz 2D Nav Goal

Option 2: Command Line

Step 4: Monitor Navigation

Practice Exercise: Humanoid Navigation Challenge

Objective

Scenario

Procedure

Success Criteria

Reality Check: When Navigation Fails

Failure 1: No Valid Path (Infeasible Goal)

Failure 2: Stuck Robot (Local Planner Blocked)

Failure 3: Oscillation (Wiggling)

Failure 4: Collisions Despite Costmap

Check Your Understanding

Key Takeaways