Model Predictive Path Integral Controller

Source code on Github.

The MPPI Controller implements a Model Predictive Path Integral Controller. The new Nav2 MPPI Controller is a predictive controller - a successor to TEB and pure path tracking MPC controllers. It uses a sampling based approach to select optimal trajectories, optimizing between successive iterations. It contains plugin-based objective functions for customization and extension for various behaviors and behavioral attributes.

It works currently with Differential, Omnidirectional, and Ackermann robots. This controller is measured to run at 100+ Hz on a modest Intel processor (4th gen i5).

The MPPI algorithm is an MPC variant that finds a control velocity for the robot using an iterative approach. Using the previous time step's best control solution and the robot's current state, a set of randomly sampled perturbations from a Gaussian distribution are applied. These noised controls are forward simulated to generate a set of trajectories within the robot's motion model. Next, these trajectories are scored using a set of plugin-based critic functions to find the best trajectory in the batch. The output scores are used to set the best control with a soft max function. This process is then repeated a number of times and returns a converged solution. This solution is then used as the basis of the next time step's initial control.

A powerful result of this work is the ability to utilize objective functions which are not require to be convex nor differentiable, providing greater designer flexibility in behavior.

See the package's README for more complete information.

MPPI Parameters

`motion_model`

Type: string Default: "diff_drive": The desired motion model plugin to use for trajectory planning. The plugin type is required to be specified in the corresponding namespace.

`<motion_model>.plugin`

Type: string Default: N/A: The plugin to use for the motion model constraints of the MPPI planner. Supported motion model plugins include "mppi::DiffDriveMotionModel", "mppi::OmniMotionModel", and "mppi::AckermannMotionModel" for differential drive, omnidirectional, and Ackermann robots respectively.

`critics`

Type: vector<string> Default: N/A: A vector of critic plugin functions to use, without mppi::critic:: namespace which will be automatically added on loading.

`iteration_count`

Type: int Default: 1: Iteration count in the MPPI algorithm. Recommended to remain as 1 and instead prefer larger batch sizes.

`batch_size`

Type: int Default: 1000: Count of randomly sampled candidate trajectories from current optimal control sequence in a given iteration. 1000 @ 50 Hz or 2000 @ 30 Hz seems to produce good results.

`time_steps`

Type: int Default: 56: Number of time steps (points) in candidate trajectories

`model_dt`

Type: double Default: 0.05: Length of each time step's dt timestep, in seconds. time_steps * model_dt is the prediction horizon.

`model_delay_vx`

Type: double Default: 0.0: Actuator delay in seconds. The rollout shifts the control sequence by round(model_delay_vx / model_dt) steps and replays recently published commands during the delay window.

`model_delay_vy`

Type: double Default: 0.0: Same as model_delay_vx for the vy axis (holonomic only)

`model_delay_wx`

Type: double Default: 0.0: Same as model_delay_vx for the wz axis

`vx_std`

Type: double Default: 0.2: Sampling standard deviation for Vx

`vy_std`

Type: double Default: 0.2: Sampling standard deviation for Vy

`wz_std`

Type: double Default: 0.2: Sampling standard deviation for Wz (angular velocity)

`vx_max`

Type: double Default: 0.5: Target maximum forward velocity (m/s).

`vy_max`

Type: double Default: 0.5: Target maximum lateral velocity, if using "Omni" motion model (m/s).

`vx_min`

Type: double Default: -0.35: Maximum reverse velocity (m/s).

`wz_max`

Type: double Default: 1.9: Maximum rotational velocity (rad/s).

`ax_max`

Type: double Default: 3.0: Maximum forward acceleration (m/s^2).

`ay_min`

Type: double Default: -3.0: Minimum lateral acceleration in either direction, if using "Omni" motion model (m/s^2).

`ay_max`

Type: double Default: 3.0: Maximum lateral acceleration in either direction, if using "Omni" motion model (m/s^2).

`ax_min`

Type: double Default: -3.0: Maximum deceleration along the X-axis (m/s^2).

`az_max`

Type: double Default: 3.5: Maximum angular acceleration (rad/s^2).

`temperature`

Type: double Default: 0.3: Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration.

`gamma`

Type: double Default: 0.015: A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information.

`clamp_raw_controls`

Type: bool Default: false: Whether to apply acceleration limits on the raw controls. Set to true when the command output from MPPI is too noisy, especially with a high time_steps value. May cause issues if ax_max && ax_min are very asymmetric.

`visualize`

Type: bool Default: false: Whether to publish debugging trajectories for visualization and critic statistics. When enabled, candidate trajectories are colored by cost (green to red gradient, magenta for collisions) and a nav2_msgs::msg::CriticsStats message is published on the ~/critics_stats topic. This can slow down the controller substantially (e.g. 1000 batches of 56 size every 30hz is a lot of data).

`critic_index_to_visualize`

Type: int Default: 0: Selects which critic to visualize the color-scheme of when visualize is true publishing Marker messages for visualization in rviz. 0 shows the total cost across all critics, 1..N selects an individual critic by index (in the order listed in the critics parameter).

`publish_optimal_trajectory`

Type: bool Default: false: Whether to publish the optimal trajectory (pose, velocity, timestamps of via points) computed by MPC for visualization, debugging, or injection by lower-level control systems and/or collision avoidance systems that need awarenes of future velocity commands and/or poses.

`retry_attempt_limit`

Type: int Default: 1: Number of attempts to find feasible trajectory on failure for soft-resets before reporting total failure.

`regenerate_noises`

Type: bool Default: false: Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution.

`sgf_order`

Type: int Default: 2: The order of the Savitzky-Golay filter (SGF) used for smoothing the optimal control sequence. Must be either 1 (1st order) or 2 (2nd order). Recommend using second order, as first order can oversmooth making it difficult to fit through tight situations, but comes with the benefit of being smoother.

`publish_critics_stats`

Type: bool Default: false: Whether to publish statistics about each critic's performance. When enabled, publishes a nav2_msgs::msg::CriticsStats message containing critic names, whether they changed costs, and the sum of costs added by each critic for all trajectory samples. Useful for debugging and tuning critic behavior but should not be enabled for generic runtime use.

`open_loop`

Type: bool Default: false: Whether to use last command velocity or use odometry for MPPI initial state estimation. When enable, use last command velocity for MPPI initial state estimation.

`TrajectoryValidator.plugin`

Type: string Default: "mppi::DefaultOptimalTrajectoryValidator": The plugin to use for validating final optimal trajectories.

Trajectory Visualization

`trajectory_step`

Type: int Default: 5: The step between trajectories to visualize to downsample candidate trajectory pool.

`time_step`

Type: int Default: 3: The step between points on trajectories to visualize to downsample trajectory density.

`allow_parameter_qos_overrides`

Type: bool Default: true: Whether to allow QoS profiles to be overwritten with parameterized values.

AckermannMotionModel

`<motion_model>.min_turning_r`

Type: double Default: 0.2: The minimum turning radius possible for the vehicle platform (m). This is only used if <motion_model>.plugin is set to "mppi::AckermannMotionModel".

Default Optimal Trajectory Validator

This validator checks the final optimal trajectory for validity and does not collide with obstacles. Additional validator plugins can be created to support different features like constraints on the maximum deviation from a path, collision margin, progress being made, etc. Dynamic and kinematic constraints are not required to be checked as trajectories are guaranteed to be within the constraints of the motion model.

`collision_lookahead_time`

Type: double Default: 2.0: The time in seconds to look ahead for potential collisions when validating the trajectory.

`consider_footprint`

Type: bool Default: false: Whether to consider the robot's footprint when validating the trajectory. Else, will use the center point cost of a circular robot

Constraint Critic

This critic penalizes trajectories that have components outside of the set dynamic or kinematic constraints

`cost_weight`

Type: double Default: 4.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

Goal Angle Critic

This critic incentivizes navigating to achieve the angle of the goal posewhen in reasonable proximity to goal

`cost_weight`

Type: double Default: 3.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`threshold_to_consider`

Type: double Default: 0.5: Minimal distance (m) between robot and goal above which angle goal cost considered.

`symmetric_yaw_tolerance`

Type: bool Default: false: Enable symmetric goal orientation acceptance. When enabled, the critic prefers trajectories that approach the goal at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in (i.e. MPPI decides). When enabled, the critic uses the minimum distance to either goal orientation, reducing the cost penalty for approaching from the backward direction. See Tuning Guide for detailed information.

Goal Critic

This critic incentivizes navigating spatially towards the goal when in reasonable proximity to goal

`cost_weight`

Type: double Default: 5.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`threshold_to_consider`

Type: double Default: 1.4: Minimal distance (m) between robot and goal above which goal distance cost considered. It is wise to start with this as being the same as your prediction horizon to have a clean hand-off with the path follower critic.

Obstacles Critic

This critic incentivizes navigating away from obstacles and critical collisions using either a circular robot point-check or full SE2 footprint check using distances from obstacles.

`critical_weight`

Type: double Default: 20.0: Weight to apply to critic for near collisions closer than collision_margin_distance to prevent near collisions only as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions.

`repulsion_weight`

Type: double Default: 1.5: Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the inflation_radius distance from obstacles.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`consider_footprint`

Type: bool Default: false: Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.

`collision_cost`

Type: double Default: 100000.0: Cost to apply to a true collision in a trajectory.

`collision_margin_distance`

Type: double Default: 0.1: Margin distance (m) from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. Note that it will highly influence the controller not to enter spaces more confined than this, so ensure this parameter is set lower than the narrowest you expect the robot to need to traverse through.

`near_goal_distance`

Type: double Default: 0.5: Distance (m) near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.

`inflation_layer_name`

Type: string Default: "": Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use.

Cost Critic

This critic incentivizes navigating away from obstacles and critical collisions using either a circular robot point-check or full SE2 footprint check using the costmap values.

`cost_weight`

Type: double Default: 3.81: Weight to apply to critic.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`consider_footprint`

Type: bool Default: false: Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.

`collision_cost`

Type: double Default: 1000000.0: Cost to apply to a true collision in a trajectory.

`near_collision_cost`

Type: int Default: 253: Costmap cost value to set a maximum proximity for avoidance.

`critical_cost`

Type: double Default: 300.0: Cost to apply to a pose with a cost higher than the near_collision_cost.

`near_goal_distance`

Type: double Default: 0.5: Distance (m) near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.

`inflation_layer_name`

Type: string Default: "": Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use.

`trajectory_point_step`

Type: int Default: 2: The step to take in trajectories for evaluating them in the critic. Since trajectories are extremely dense, its unnecessary to evaluate each point and computationally expensive.

Path Align Critic

This critic incentivizes aligning with the global path, if relevant. It does not implement path following behavior.

`cost_weight`

Type: double Default: 10.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`threshold_to_consider`

Type: double Default: 0.5: Distance (m) between robot and goal to stop considering path alignment and allow goal critics to take over.

`offset_from_furthest`

Type: int Default: 20: Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading.

`max_path_occupancy_ratio`

Type: double Default: 0.07: Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. Between 0-1 for 0-100%.

`use_path_orientations`

Type: bool Default: false: Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false.

`trajectory_point_step`

Type: int Default: 4: The step to take in trajectories for evaluating them in the critic. Since trajectories are extremely dense, its unnecessary to evaluate each point and computationally expensive.

Path Angle Critic

This critic penalizes trajectories at a high relative angle to the path. This helps the robot make sharp turns when necessary due to large accumulated angular errors.

`cost_weight`

Type: double Default: 2.2: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`threshold_to_consider`

Type: double Default: 0.5: Distance (m) between robot and goal to stop considering path angles and allow goal critics to take over.

`offset_from_furthest`

Type: int Default: 20: Number of path points after furthest one any trajectory achieves to compute path angle relative to.

`max_angle_to_furthest`

Type: double Default: 0.785398: Angular distance (rad) between robot and goal above which path angle cost starts being considered

`mode`

Type: int Default: 0

Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires.

0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path.
1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable.
2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction.

Path Follow Critic

This critic incentivizes making progress along the path. This is what drives the robot forward along the path.

`cost_weight`

Type: double Default: 5.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`threshold_to_consider`

Type: double Default: 1.4: Distance (m) between robot and goal to stop considering path following and allow goal critics to take over. It is wise to start with this as being the same as your prediction horizon to have a clean hand-off with the goal critic.

`offset_from_furthest`

Type: int Default: 6: Number of path points after furthest one any trajectory achieves to drive path tracking relative to.

Prefer Forward Critic

This critic incentivizes moving in the forward direction, rather than reversing.

`cost_weight`

Type: double Default: 5.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`threshold_to_consider`

Type: double Default: 0.5: Distance (m) between robot and goal to stop considering preferring forward and allow goal critics to take over.

Twirling Critic

This critic penalizes unnecessary 'twisting' with holonomic vehicles. It adds a constraint on the rotation angle to keep it consistent.

`cost_weight`

Type: double Default: 10.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

Velocity Deadband Critic

This critic penalizes velocities that fall below the deadband threshold, helping to mitigate hardware limitations on certain platforms.

`cost_weight`

Type: double Default: 35.0: Weight to apply to critic term.

`cost_power`

Type: int Default: 1: Power order to apply to term.

`deadband_velocities`

Type: vector<double> Default: [0.05, 0.05, 0.05]: The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action.

Example

controller_server:
  ros__parameters:
    controller_frequency: 30.0
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.5
      vx_min: -0.35
      vy_max: 0.5
      wz_max: 1.9
      ax_max: 3.0
      ax_min: -3.0
      ay_min: -3.0
      ay_max: 3.0
      az_max: 3.5
      iteration_count: 1
      temperature: 0.3
      gamma: 0.015
      motion_model: "diff_drive"
      diff_drive:
        plugin: "mppi::DiffDriveMotionModel"
      visualize: false
      critic_index_to_visualize: 0
      regenerate_noises: false
      sgf_order: 2
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      TrajectoryValidator:
        plugin: "mppi::DefaultOptimalTrajectoryValidator"
        collision_lookahead_time: 2.0
        consider_footprint: false
      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      # ObstaclesCritic:
      #   enabled: true
      #   cost_power: 1
      #   repulsion_weight: 1.5
      #   critical_weight: 20.0
      #   consider_footprint: false
      #   collision_cost: 10000.0
      #   collision_margin_distance: 0.1
      #   near_goal_distance: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      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
        mode: 0
      # VelocityDeadbandCritic:
      #   enabled: true
      #   cost_power: 1
      #   cost_weight: 35.0
      #   deadband_velocities: [0.05, 0.05, 0.05]
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0

Notes to Users

General Words of Wisdom

The model_dt parameter generally should be set to the duration of your control frequency. So if your control frequency is 20hz, this should be 0.05. However, you may also set it lower but not larger.

Visualization of the trajectories using visualize uses compute resources to back out trajectories for visualization and therefore slows compute time. It is not suggested that this parameter is set to true during a deployed use, but is a useful debug instrument while tuning the system, but use sparingly. Visualizing 2000 batches @ 56 points at 30 hz is a lot.

The most common parameters you might want to start off changing are the velocity profiles (vx_max, vx_min, wz_max, and vy_max if holonomic) and the motion_model to correspond to your vehicle. Its wise to consider the prune_distance of the path plan in proportion to your maximum velocity and prediction horizon. The only deeper parameter that will likely need to be adjusted for your particular settings is the Obstacle critics' repulsion_weight since the tuning of this is proprtional to your inflation layer's radius. Higher radii should correspond to reduced repulsion_weight due to the penalty formation (e.g. inflation_radius - min_dist_to_obstacle). If this penalty is too high, the robot will slow significantly when entering cost-space from non-cost space or jitter in narrow corridors. It is noteworthy, but likely not necessary to be changed, that the Obstacle critic may use the full footprint information if consider_footprint = true, though comes at an increased compute cost.

Tune std's carefully for low acceleration. If you're seeing a lot of chatter in the angular velocity, reduce its std. If you're not seeing your robot get to full speed, increase your std to explore more of the space. Also take care that your odometry publishes at least as fast as your control frequency (ideally much faster) when using low accelerations to make the fullest use of the acceleration limits.

Otherwise, the parameters have been closely pre-tuned by your friendly neighborhood navigator to give you a decent starting point that hopefully you only need to retune for your specific desired behavior lightly (if at all). Varying costmap parameters or maximum speeds are the actions which require the most attention, as described below:

Prediction Horizon, Costmap Sizing, and Offsets

As this is a predictive planner, there is some relationship between maximum speed, prediction times, std sampling, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the minimum required costmap radius is 1.5m - or 3m total width. The faster the robot is set to go, the higher the velocity sampling standard deviations should be in order to effectively explore the velocity space.

The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. Setting the threshold for consideration in the path follower + goal critics as the same as your prediction horizon can make sure you have clean hand-offs between them, as the path follower will otherwise attempt to slow slightly once it reaches the final goal pose as its marker.

The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic offset_from_furthest represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, prediction_horizon_s * max_speed / path_resolution / 3.0 is a good baseline.

Obstacle, Inflation Layer, and Path Following

There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task.

Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered.

As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the FollowPath critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly.

Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it can be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off.