MPPI Cost Critic Ignoring Near Collisions In Navigation2

by Admin 57 views
MPPI Cost Critic Ignoring Near Collisions in Navigation2

Introduction

Hey guys! Today, we're diving into a fascinating problem encountered while using the Model Predictive Path Integral (MPPI) controller with the Cost Critic in ROS2 Navigation2. The issue? The Cost Critic doesn't seem to be doing its job when it comes to penalizing trajectories that bring our robot dangerously close to obstacles, especially in those tricky, narrow spots. This is a big deal because we want our robots to be smart and avoid bumping into things, right? So, let's break down the problem, explore the configuration, and figure out where we can start digging into the code to find a solution.

Understanding the Problem

Imagine you're trying to parallel park, but instead of carefully maneuvering, your car just insists on getting as close as humanly possible to the other vehicles. That's kind of what's happening here. The robot, instead of choosing a safer path that might involve backing up or moving sideways, decides to rotate in a way that brings the rear part of its footprint uncomfortably close to obstacles. And the Cost Critic? It's just sitting there, seemingly unconcerned.

The goal here is to get the robot to respect its physical dimensions and make decisions that prioritize safety. We want it to be able to navigate close to obstacles, sure, but not at the expense of potentially crashing. The desired behavior is that when the robot has the option to maneuver more safely, it should take it, even if it means a slightly less optimal path in the short term.

Configuration Details

Let's take a look at the configuration settings that are supposed to make the Cost Critic work its magic:

time_steps: 35
model_dt: 0.05
batch_size: 2000
ax_max: 3.0
ax_min: -3.0
ay_max: 3.0
ay_min: -3.0
az_max: 3.5
vx_std: 0.2
vy_std: 0.2
wz_std: 0.9
vx_max: 0.3
vx_min: -0.1
vy_max: 0.5
wz_max: 0.3
iteration_count: 1
prune_distance: 0.8
enforce_path_inversion: True
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
#...
CostCritic:
  enabled: true
  cost_power: 1
  cost_weight: 2.5
  near_collision_cost: 253
  critical_cost: 300.0
  consider_footprint: true
  collision_cost: 1000000.0
  near_goal_distance: 0.0
  trajectory_point_step: 2

Key parameters to note:

  • near_collision_cost: This is set to 253, which should be a significant penalty for getting too close to obstacles.
  • consider_footprint: This is set to true, meaning the Cost Critic should be taking the robot's entire footprint into account when evaluating trajectories.
  • collision_cost: This is a very high value, indicating that actual collisions should be heavily penalized. This works as expected, however its the "near" misses that are the real problem for path planning.

Despite these settings, the robot seems to be ignoring the near_collision_cost and proceeding with trajectories that bring it too close to obstacles. This suggests something might be wrong with how the Cost Critic is calculating or applying this cost.

The Hunt for the Cause

So, where do we start looking for the root cause of this issue? Here are a few areas to investigate:

  1. Footprint Transformation: Double-check that the footprint specified is correctly transformed and applied to the costmap. Make sure that the local and global costmaps are in sync and using the correct footprint representation (footprint: "[ [0.28638, 0.274], [0.28638, -0.274], [-0.39762, -0.274], [-0.39762, 0.274] ]"). A mismatch here could lead to the Cost Critic misinterpreting the robot's proximity to obstacles. This is a really important place to start looking!
  2. Cost Calculation: Dive into the code where the near_collision_cost is actually calculated. Look for any potential bugs or logical errors in how the distance to obstacles is being computed. Is it possible that the code is only considering a single point on the footprint, rather than the entire shape? Or perhaps there's an issue with the coordinate frames being used in the calculation.
  3. Cost Application: Examine how the calculated cost is being applied to the trajectories. Is the cost_weight parameter actually having the intended effect? Is there any normalization or scaling happening that might be diminishing the impact of the near_collision_cost? This is another critical area to investigate.
  4. Trajectory Point Step: Check trajectory_point_step parameter. The current value is 2, meaning the collision checker skips points along the trajectory. If the trajectory is too coarse, near collisions can be missed.
  5. Costmap Updates: Ensure that the costmap is being updated frequently enough to accurately reflect the environment. If the costmap is stale, the Cost Critic might be making decisions based on outdated information. Pay special attention to the update frequency and resolution of the costmap.

Digging into the Code

To really get to the bottom of this, you'll need to get your hands dirty with the Navigation2 codebase. Here are some specific files and functions that might be relevant:

  • nav2_mppi_controller: This package contains the core MPPI controller implementation, including the Cost Critic.
  • cost_critic.hpp and cost_critic.cpp: These files likely contain the implementation of the Cost Critic itself, including the logic for calculating and applying the collision cost.
  • costmap_2d: This package provides the costmap data structure and related functions for querying obstacle information.

Use a debugger and step through the code to see exactly how the near_collision_cost is being calculated and applied. Pay close attention to the values of variables like the distance to obstacles, the footprint coordinates, and the resulting cost. This will help you pinpoint where the problem is occurring.

Additional Tips

  • Simplify the Scenario: Start with a very simple environment and a small number of trajectories to make debugging easier. Once you've identified the problem, you can gradually increase the complexity.
  • Visualize the Data: Use the ROS2 visualization tools (like RViz) to visualize the robot's footprint, the costmap, and the generated trajectories. This can help you get a better understanding of what's happening.
  • Experiment with Parameters: Try different values for the near_collision_cost, cost_weight, and other relevant parameters to see if you can influence the behavior of the Cost Critic.

Bug Report Details

Here's a summary of the original bug report:

Operating System: Ubuntu 24.04 Computer: Ryzen 9 7940 ROS2 Version: Jazzy Version or commit hash: 97de6f21c067f6d514c3940b6f5d44afb2ceaf34 DDS implementation: Cyclone

Steps to Reproduce the Issue

The user was experimenting with the prediction horizon by changing it from 1.75s to 3.75s for a robot with a maximum speed of 0.3 m/s and 0.3 rad/s. Despite these changes, the Cost Critic failed to penalize near-collision trajectories in a narrow spot.

Expected Behavior

The Cost Critic should encourage the robot to choose safer motions, such as reversing or moving parallel to the obstacle, instead of forcing a collision.

Actual Behavior

The Cost Critic doesn't penalize when the rear part of the footprint gets closer to the obstacle during rotation.

Conclusion

Alright, guys, that's a wrap! We've identified a tricky issue with the MPPI Cost Critic in Navigation2. By carefully examining the configuration, diving into the code, and using the debugging tips outlined above, you should be well-equipped to track down the cause of this problem and get your robot navigating safely and smartly. Good luck, and happy debugging!

Remember, the key is to understand how the Cost Critic is calculating and applying the collision cost, and to ensure that the robot's footprint is being accurately represented in the costmap. With a little bit of detective work, you'll have your robot avoiding those near-collision scenarios in no time.