Local Planning with MPPI

In this tutorial, we will be learning how to configure a second local planner, MPPI. It assumes that you have already perused the previous DWB local planning tutorial, as many of the concepts overlap.

DWB Limitations

The local planner figures out what velocities to send to the base to get to the goal. DWB does that by examining a range of velocities around the current velocity, determining what trajectories those velocities would result in, and then ranking them with a scoring algorithm. Then, it repeats the process anew on the next iteration.

This approach is conceptually straight forward, and thus is an excellent entry point into the concepts of local planning, and in many scenarios, is sufficient for effective behavior. However, it has two key shortcomings.

• All the information about potential trajectories and their value is generated on each iteration, with virtually no holdover between calculations.
• Trajectories are calculated using a single velocity. The trajectory generated is the result of starting at the current velocity, accelerating/decellerating to the given velocity, and then continuing to drive at the velocity for the duration of the simulated time. The result is that you cannot represent more complex trajectories like "drive straight, then turn at the corner." Instead, you get will more frequently drive straight, slow down for a couple iterations, and then turn.

The MPPI local planner overcomes these limitations.

MPPI Overview

MPPI stands for Model Predictive Path Integral. It was introduced in 2016 by this paper. It was implemented for the ROS Nav2 framework by Steve Macenski, and you can hear him talk about the algorithm in his 2023 ROSCon Talk.

There is a lot of theory that goes into how the algorithm was developed and how it optimizes for efficient path planning, but we're going to take a slightly more practical look at the resulting algorithm.

The first important difference between DWB and MPPI is that MPPI is stochastic, i.e. it has a random element to it. We'll start with a batch of trajectories (the exact number determined by the batch_size parameter). Initially, each of these trajectories contains random motion, based on the kinematic parameters of the robot. Each trajectory is split up into a some number of time steps (determined by the time_steps parameter) with an interval of model_dt seconds between each. So far, these trajectories are not dissimilar from those in DWB. However, instead of just one velocity determining the entire trajectory, these trajectories have distinct velocity commands for each time step. This allows for more complex trajectories, smoother transitions between different behaviors and potentially more "aggressive" trajectories (which was the goal of the original paper.)

Just like DWB, all of the trajectories are then evaluated by a set of critics, and the best one is sent to the Stretch base.

However, here's the key difference between DWB and MPPI. In DWB, we would toss all of the trajectories and then start again. In MPPI, the first time step of each trajectory is discarded, and a new time step is added to the end. This means that MPPI starts each iteration with a significant amount of "prep work" already done, which jump starts the optimization.

Furthermore, MPPI uses the critic evaluations from the previous iteration to guide the optimization. Badly scoring trajectories are discarded, and high scoring trajectories are propogated and expanded upon. In this way, MPPI is much closer to a genetic algorithm than DWB, leveraging the population of randomly generated solutions that evolve iteratively to get to an ideal solution.

MPPI Kinematic Parameters

MPPI also allows for different motion models than DWB, including "Omni" which is what we'll use to take advantage of Stretch 4's omnidirectional base. This is set using motion_model: Omni.

Frustruatingly, all of the kinematic parameters have different names in DWB and MPPI. This table shows the equivalent names.

DWB Parameter	MPPI Parameter
min_vel_x	vx_min
max_vel_x	vx_max
min_vel_y	N/A
max_vel_y	vy_max
max_vel_theta	wz_max
acc_lim_x	ax_max
decel_lim_x	ax_min
acc_lim_y	ay_max
decel_lim_y	ay_min
acc_lim_theta	az_max
decel_lim_theta	N/A

These parameters can be tuned in much the same way as we did with DWB to achieve different behaviors, i.e. setting vx_min to 0.0 instead of a negative value will limit Stretch to forward motion.

Whereas in DWB, there were a set number of velocities explored via the vx_samples parameter (and similar), in MPPI, the exploration of velocities is done probabilistically, using sampling. The standard deviation of the sampling in each dimension is set by the vx_std, vy_std and wz_std parameters for each the linear x, linear y and turning velocties respectively.

MPPI Critics

Each trajectory is evaluated/scored by the critics. Although they use separate implementations, many of the critics have a corresponding critic(s) in DWB.

DWB Critic	MPPI Critic	Description
PathDist	PathAlignCritic PathFollowCritic	Used for staying close to the global path
GoalDist	GoalCritic	Prioritizes moving toward the goal
GoalAlign	GoalAngleCritic	Alignment with goal orientation
PathAlign	PathAngleCritic	Alignment with global path orientation
BaseObstacle	CostCritic ObstaclesCritic	Avoiding obstacles/costs on costmap
PreferForward	PreferForwardCritic	Prefer forward motion
Oscillation	TwirlingCritic	Penalizes wiggling/twirling
	ConstraintCritic	Used to ensure the sampled trajectories follow all kinematic constraints