Nav2 MPPI not following the SMAC path

[BLUEEAGLE271]

Hi, I've been trying to implement nav2 MPPI controller for my project. However, the MPPI is failing to stay on the global path from SMAC.

I am using a Jetson nano orin to run the Nav2 stack, and I'm using the Humble branch.

I have adjusted many parameters, including the costs for critic. I have also changed the linear, angular velocities and turning radius, giving it a higher leeway to turn, or adjust itself. However none of them work.

It seems that the MPPI is somehow only choosing the middle trajectory, (the trajectory halfway between all sampled trajectories).

I recognise that my batch sizes might be much lower to hit the desired controller frequency, but I can see that the sampled trajectories are overlapping the global path, but the controller doesn't pick those trajectories instead. The goal poses are all at least 1 meter away.

Here are some images of what it generated

Any diagnostic steps would be hugely appreciated! I have already tried changing weights of most critics from 5 - 15

Here are my MPPI params

controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]

progress_checker:
  plugin: "nav2_controller::SimpleProgressChecker"
  required_movement_radius: 0.05
  movement_time_allowance: 10.0

general_goal_checker:
  stateful: True
  plugin: "nav2_controller::SimpleGoalChecker"
  xy_goal_tolerance: 0.08
  yaw_goal_tolerance: 0.3

FollowPath:
  plugin: "nav2_mppi_controller::MPPIController"
  time_steps: 50
  trajectory_point_step: 1
  model_dt: 0.05
  publish_optimal_trajectory: true
  visualize: true
  batch_size: 600
  use_sim_time: False
  gamma: 0.015
  iteration_count: 1
  prune_distance: 0.6
  use_unordered_multithreading: true
  regenerate_noises: false      # Biggest single CPU saving
  # ... (keep your time_steps, batch_size, etc.) ...
  
  # 1. CHANGE THE MOTION MODEL
  motion_model: "Ackermann"  # <--- This is the key switch
  temperature: 0.3
  # 2. DEFINE THE GEOMETRY (The "Cheat" Prevention)
  # This is the strict physical constraint you asked for.
  AckermannConstraints:
    min_turning_r: 0.3       # The robot CANNOT turn tighter than this (0.5m radius)
  
  # 3. SPEEDS (Mouse Mode)
  vx_max: 0.3
  vx_min: 0.0    # You NEED reverse to park with Ackermann (like a car parallel parking)
  wz_max: 2.0     # Max rotation speed

  vx_std: 0.1
  vy_std: 0.0  # Must be 0 for DiffDrive
  wz_std: 0.8
  
  # 4. CRITICS (Tuned for Arcs)
  # We remove 'PathAngleCritic' because it often tempts the robot to spin in place to fix heading.
  critics: ["ConstraintCritic","CostCritic","PathAngleCritic","PathAlignCritic","PathFollowCritic","ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PreferForwardCritic"]

  # ... (ObstaclesCritic, GoalCritic etc. remain same as before) ...
  ConstraintCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0

  CostCritic:
    enabled: true
    cost_power: 1
    cost_weight: 3.81
    critical_cost: 300.0
    consider_footprint: true
    collision_cost: 1000000.0
    near_goal_distance: 0.0
  # CRITICAL: This critic forces the robot to align with the path using curves, not spins.
  PathAlignCritic:
    enabled: true
    cost_power: 1
    cost_weight: 14.0
    max_path_occupancy_ratio: 0.05
    trajectory_point_step: 3
    threshold_to_consider: 0.5
    offset_from_furthest: 15
    use_path_orientation: true
  
  PathAngleCritic:
    enabled: true
    cost_power: 1
    cost_weight: 2.0
    offset_from_furthest: 4
    threshold_to_consider: 0.5
    max_angle_to_furthest: 1.0
    forward_preference: true
    use_path_orientations: true


  GoalAngleCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0   # How strongly it cares about the final angle
    threshold_to_consider: 0.4 # Only start worrying about the angle when within 0.4m of the goal

  GoalCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0
    threshold_to_consider: 0.4

  ObstaclesCritic:
    enabled: true
    cost_power: 1
    repulsion_weight: 0.5  # LOW weight = Not afraid of walls (Mouse mode)
    critical_weight: 5.0  # HIGH weight = But never hit them
    near_goal_distance: 0.0
    consider_footprint: true

  PathFollowCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0
    offset_from_furthest: 3
    threshold_to_consider: 0.4

  PreferForwardCritic:
    enabled: false
    cost_power: 1
    cost_weight: 0.1
    threshold_to_consider: 0.5

[mohsinarf]

Try tuning critic weights especially the PathFollowCritic and PathAlignCritic so they heavily reward trajectories closer to the global path.

[kehan-zhou]

Maybe you can try:

1. Increasing the Batch Size to allow for more sampled trajectories;
2. Increasing the Controller Frequency, which might improve responsiveness and path-following performance;
3. Adjust the weights of PathAlignCritic and PathFollowCritic to prioritize path-following;
4. Check if the Ackermann motion model constraints are too strict.

For a more detailed discussion, please consider posting on Robotics Stack Exchange with your configuration files, images, and more details.