Model Predictive Path Integral Local Planner (ROS1, Eigen)

Get Start

Drop the source to a ros workspace as a package.
catkin_make --only-pkg-with-deps mppi_local_planner && source ./devel/setup.bash
Don't forget set move base

Overview

This is a predictive controller (local trajectory planner) that implements the Model Predictive Path Integral (MPPI) algorithm to track a path with adaptive collision avoidance. It contains plugin-based critic functions to impact the behavior of the algorithm. It was created by Aleksei Budyakov and adapted & developed for Nav2 by Steve Macenski. I just tried to migrate it to ROS1 and Eigen to enhance compatibility. The current version no longer depends on xtensor and supports dynamic reconfiguring.

This plugin implements the nav_core::BaseLocalPlanner interface allowing it to be used across the navigation stack as a local trajectory planner.

It works currently with Differential, Omnidirectional, and Ackermann robots.

MPPI Description

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.

Features

Predictive MPC trajectory planner
Utilizes plugin-based critics which can be swapped out, tuned, or replaced easily by the user
Highly optimized CPU-only performance using vectorization and tensor operations
Supports a number of common motion models, including Ackermann, Differential-Drive, and Omni-directional
Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors
High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide
Easily extensible to support modern research variants of MPPI
Comes pre-tuned for good out-of-the-box behavior

Configuration

Controller

Parameter	Type	Definition
motion_model	string	Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann].
critics	string	Default: None. Critics (plugins) names
iteration_count	int	Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches.
batch_size	int	Default 1000. Count of randomly sampled candidate trajectories
time_steps	int	Default 56. Number of time steps (points) in each sampled trajectory
model_dt	double	Default: 0.05. Time interval (s) between two sampled points in trajectories.
vx_std	double	Default 0.2. Sampling standard deviation for VX
vy_std	double	Default 0.2. Sampling standard deviation for VY
wz_std	double	Default 0.4. Sampling standard deviation for Wz
vx_max	double	Default 0.5. Max VX (m/s)
vy_max	double	Default 0.5. Max VY in either direction, if holonomic. (m/s)
vx_min	double	Default -0.35. Min VX (m/s)
wz_max	double	Default 1.9. Max WZ (rad/s)
temperature	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	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 of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information.
visualize	bool	Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging.
retry_attempt_limit	int	Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.
regenerate_noises	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.

Trajectory Visualizer

Parameter	Type	Definition
trajectory_step	int	Default: 5. The step between trajectories to visualize to downsample candidate trajectory pool.
time_step	int	Default: 3. The step between points on trajectories to visualize to downsample trajectory density.

Path Handler

Parameter	Type	Definition
max_robot_pose_search_dist	double	Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping.
prune_distance	double	Default: 1.5. Distance ahead of nearest point on path to robot to prune path to.
transform_tolerance	double	Default: 0.1. Time tolerance for data transformations with TF.
enforce_path_inversion	double	Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested.
inversion_xy_tolerance	double	Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion.
inversion_yaw_tolerance	double	Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg.

Ackermann Motion Model

Parameter	Type	Definition
min_turning_r	double	minimum turning radius for ackermann motion model

Constraint Critic

Parameter	Type	Definition
cost_weight	double	Default 4.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.

Goal Angle Critic

Parameter	Type	Definition
cost_weight	double	Default 3.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
threshold_to_consider	double	Default 0.5. Minimal distance between robot and goal above which angle goal cost considered.

Goal Critic

Parameter	Type	Definition
cost_weight	double	Default 5.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
threshold_to_consider	double	Default 1.4. Distance between robot and goal above which goal cost starts being considered

Obstacles Critic

Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles

Parameter	Type	Definition
consider_footprint	bool	Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.
critical_weight	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	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	int	Default 1. Power order to apply to term.
collision_cost	double	Default 100000.0. Cost to apply to a true collision in a trajectory.
collision_margin_distance	double	Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable.
near_goal_distance	double	Default 0.5. Distance 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	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

Uses inflated costmap cost directly to avoid obstacles

Parameter	Type	Definition
consider_footprint	bool	Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.
cost_weight	double	Default 3.81. Wight to apply to critic to avoid obstacles.
cost_power	int	Default 1. Power order to apply to term.
collision_cost	double	Default 1000000.0. Cost to apply to a true collision in a trajectory.
critical_cost	double	Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles.
near_goal_distance	double	Default 0.5. Distance 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	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.

Path Align Critic

Parameter	Type	Definition
cost_weight	double	Default 10.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
threshold_to_consider	double	Default 0.5. Distance between robot and goal above which path align cost stops being considered
offset_from_furthest	double	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.
trajectory_point_step	double	Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.
max_path_occupancy_ratio	double	Default 0.07 (7%). 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.
use_path_orientations	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.

Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023.

Path Angle Critic

Parameter	Type	Definition
cost_weight	double	Default 2.2. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
threshold_to_consider	double	Default 0.5. Distance between robot and goal above which path angle cost stops being considered
offset_from_furthest	int	Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to.
max_angle_to_furthest	double	Default 0.785398. Angular distance between robot and goal above which path angle cost starts being considered
mode	int	Default 0 (Forward Preference). 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

Parameter	Type	Definition
cost_weight	double	Default 5.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
offset_from_furthest	int	Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to.
threshold_to_consider	float	Default 1.4. Distance between robot and goal above which path follow cost stops being considered

Prefer Forward Critic

Parameter	Type	Definition
cost_weight	double	Default 5.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
threshold_to_consider	double	Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered

Twirling Critic

Parameter	Type	Definition
cost_weight	double	Default 10.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.

Velocity Critic

Parameter	Type	Definition
cost_weight	double	Default 10.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.

Topics

Topic	Type	Description
`trajectories`	`visualization_msgs/MarkerArray`	Randomly generated trajectories, including resulting control sequence
`transformed_global_plan`	`nav_msgs/Path`	Part of global plan considered by local planner

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 proportional 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.

If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics.

By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See PathAlignCritic::score() for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state.

If you want to slow further on approach to goal, consider increasing the threshold_to_consider parameters to give a hand off from the path tracking critics to the goal approach critics sooner - then tune those critics for your profile of interest.

Prediction Horizon, Costmap Sizing, and Offsets

As this is a predictive planner, there is some relationship between maximum speed, prediction times, 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 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.

Forrest-Z / mppi_local_planner