Pathfinding

PathPlannerLib now includes a set of commands that will automatically plan a path between two points while avoiding obstacles on the field. They can also be used to generate a path to another path, allowing you to chain together a generated and pre-planned path for finer control.

A few important considerations to note before attempting to use these commands:

There must be a navgrid.json file present in your deploy/pathplanner directory in order for the system to know where obstacles are. This file will be created automatically when opening the project in pathplanner. You can edit the navgrid in the GUI, but you probably shouldn't have to.
You have no control of the robot's heading at the start and end points. In other words, you can't attempt to pathfind to a position to the left of the robot, but have it arrive at that point while moving to the right. The shortest path from A to B will be used.
Because of the above, this is more difficult to get great results with a differential drivetrain. It will still be possible, you just need to take more care with it. For example, doing a turn in place command if your robot is not facing the direction it will travel when pathfinding.
Even with a holonomic drive train, it's not that great at lining up with things (for example, a human player station) because of the heading restriction. This is why the ability to chain paths together exists. It is recommended to create a pre-planned path for doing the final line up with something, then pathfind to that path if precision is required.
The AD* algorithm used for pathfinding does not just produce one path, it produces a few as it further refines the path in the background. In some rare cases, the robot could start moving in one direction, then switch to the other direction when AD* figures out that direction is more optimal.

Compared to the normal path following commands, pathfinding commands have a couple of additional optional parameters:

Goal end velocity: The velocity the robot should be moving at when reaching the goal position. If pathfinding to a predefined path, this will be automatically set based on the max velocity of that path
Rotation delay distance: This parameter controls the distance that the robot should move before attempting to rotate to the goal rotation while pathfinding. This is useful in cases where you are pathfinding out of a cramped area, avoiding possible collisions from rotation

AdvantageKit Compatibility

public void robotInit() {
  // DO THIS FIRST
  Pathfinding.setPathfinder(new LocalADStarAK());

  // ... remaining robot initialization
}

Pathfind to Pose

Using AutoBuilder

The easiest way to create a pathfinding command is by using AutoBuilder. See Build an Auto to configure AutoBuilder.

// Since we are using a holonomic drivetrain, the rotation component of this pose
// represents the goal holonomic rotation
Pose2d targetPose = new Pose2d(10, 5, Rotation2d.fromDegrees(180));

// Create the constraints to use while pathfinding
PathConstraints constraints = new PathConstraints(
        3.0, 4.0,
        Units.degreesToRadians(540), Units.degreesToRadians(720));

// Since AutoBuilder is configured, we can use it to build pathfinding commands
Command pathfindingCommand = AutoBuilder.pathfindToPose(
        targetPose,
        constraints,
        0.0, // Goal end velocity in meters/sec
        0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
);

#include <pathplanner/lib/auto/AutoBuilder.h>

using namespace pathplanner;

// Since we are using a holonomic drivetrain, the rotation component of this pose
// represents the goal holonomic rotation
frc::Pose2d targetPose = frc::Pose2d(10_m, 5_m, frc::Rotation2d(180_deg));

// Create the constraints to use while pathfinding
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// Since AutoBuilder is configured, we can use it to build pathfinding commands
frc2::CommandPtr pathfindingCommand = AutoBuilder::pathfindToPose(
    targetPose,
    constraints,
    0.0_mps, // Goal end velocity in meters/sec
    0.0_m // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
);

from pathplannerlib.auto import AutoBuilder
from pathplannerlib.path import PathConstraints
from wpimath.geometry import Pose2d, Rotation2d
from wpimath.units import degreesToRadians

# Since we are using a holonomic drivetrain, the rotation component of this pose
# represents the goal holonomic rotation
targetPose = Pose2d(10, 5, Rotation2d.fromDegrees(180))

# Create the constraints to use while pathfinding
constraints = PathConstraints(
    3.0, 4.0, 
    degreesToRadians(540), degreesToRadians(720)
)

# Since AutoBuilder is configured, we can use it to build pathfinding commands
pathfindingCommand = AutoBuilder.pathfindToPose(
    targetPose,
    constraints,
    goal_end_vel=0.0, # Goal end velocity in meters/sec
    rotation_delay_distance=0.0 # Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
)

Manually

// Since we are using a holonomic drivetrain, the rotation component of this pose
// represents the goal holonomic rotation
Pose2d targetPose = new Pose2d(10, 5, Rotation2d.fromDegrees(180));

// Create the constraints to use while pathfinding
PathConstraints constraints = new PathConstraints(
        3.0, 4.0,
        Units.degreesToRadians(540), Units.degreesToRadians(720));

// See the "Follow a single path" example for more info on what gets passed here
Command pathfindingCommand = new PathfindHolonomic(
        targetPose,
        constraints,
        0.0, // Goal end velocity in m/s. Optional
        swerveSubsystem::getPose,
        swerveSubsystem::getRobotRelativeSpeeds,
        swerveSubsystem::driveRobotRelative,
        Constants.Swerve.pathFollowingConfig, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
        0.0, // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
        swerveSubsystem // Reference to drive subsystem to set requirements
);

#include <pathplanner/lib/commands/PathfindHolonomic.h>

using namespace pathplanner;

// Since we are using a holonomic drivetrain, the rotation component of this pose
// represents the goal holonomic rotation
frc::Pose2d targetPose = frc::Pose2d(10_m, 5_m, frc::Rotation2d(180_deg));

// Create the constraints to use while pathfinding
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// See the "Follow a single path" example for more info on what gets passed here
// Assuming this is somewhere in the drive subsystem, so using "this"
frc2::CommandPtr pathfindingCommand = PathfindHolonomic(
    targetPose,
    constraints,
    0.0_mps, // Goal end velocity in m/s. Optional
    [this](){ return getPose(); },
    [this](){ return getRobotRelativeSpeeds(); },
    [this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    Constants::Swerve::pathFollowingConfig, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    this, // Pointer to drive subsystem to set requirements
    0.0_m // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
);

from pathplannerlib.path import PathConstraints
from pathplannerlib.commands import PathfindHolonomic
from pathplannerlib.config import HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants
from wpimath.geometry import Pose2d, Rotation2d
from wpimath.units import degreesToRadians

# Since we are using a holonomic drivetrain, the rotation component of this pose
# represents the goal holonomic rotation
targetPose = Pose2d(10, 5, Rotation2d.fromDegrees(180))

# Create the constraints to use while pathfinding
constraints = PathConstraints(
    3.0, 4.0, 
    degreesToRadians(540), degreesToRadians(720)
)

# See the "Follow a single path" example for more info on what gets passed here
# Assuming this is somewhere in the drive subsystem, so using "self"
pathfindingCommand = PathfindHolonomic(
    constraints,
    self.getPose,
    self.getCurrentSpeeds,
    self.driveRobotRelative,
    HolonomicPathFollowerConfig( # HolonomicPathFollowerConfig, this should likely live in your Constants class
          PIDConstants(5.0, 0.0, 0.0), # Translation PID constants
          PIDConstants(5.0, 0.0, 0.0), # Rotation PID constants
          4.5, # Max module speed, in m/s
          0.4, # Drive base radius in meters. Distance from robot center to furthest module.
          ReplanningConfig() # Default path replanning config. See the API for the options here
    ),
    lambda: False, # Don't flip paths when pathfinding to a pose
    self, # Reference to this subsystem to set requirements
    rotation_delay_distance=0.0, # Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
    target_pose=targetPose,
    goal_end_vel=0.0 # Goal end velocity in meters/sec
)

Pathfind Then Follow Path

Using AutoBuilder

The easiest way to create a pathfinding command is by using AutoBuilder. See Build an Auto to configure AutoBuilder.

// Load the path we want to pathfind to and follow
PathPlannerPath path = PathPlannerPath.fromPathFile("Example Human Player Pickup");

// Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path.
PathConstraints constraints = new PathConstraints(
        3.0, 4.0,
        Units.degreesToRadians(540), Units.degreesToRadians(720));

// Since AutoBuilder is configured, we can use it to build pathfinding commands
Command pathfindingCommand = AutoBuilder.pathfindThenFollowPath(
        path,
        constraints,
        3.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
);

#include <pathplanner/lib/auto/AutoBuilder.h>

using namespace pathplanner;

// Load the path we want to pathfind to and follow
auto path = PathPlannerPath::fromPathFile("Example Human Player Pickup");

// Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path.
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// Since AutoBuilder is configured, we can use it to build pathfinding commands
frc2::CommandPtr pathfindingCommand = AutoBuilder::pathfindThenFollowPath(
    path,
    constraints,
    3.0_m // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
);

from pathplannerlib.auto import AutoBuilder
from pathplannerlib.path import PathPlannerPath PathConstraints
from wpimath.units import degreesToRadians

# Load the path we want to pathfind to and follow
path = PathPlannerPath.fromPathFile('Example Human Player Pickup');

# Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path.
constraints = PathConstraints(
    3.0, 4.0, 
    degreesToRadians(540), degreesToRadians(720)
)

# Since AutoBuilder is configured, we can use it to build pathfinding commands
pathfindingCommand = AutoBuilder.pathfindThenFollowPath(
    path,
    constraints,
    rotation_delay_distance=3.0 # Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
)

Manually

// Load the path we want to pathfind to and follow
PathPlannerPath path = PathPlannerPath.fromPathFile("Example Human Player Pickup");

// Create the constraints to use while pathfinding
PathConstraints constraints = new PathConstraints(
        3.0, 4.0,
        Units.degreesToRadians(540), Units.degreesToRadians(720));

// See the "Follow a single path" example for more info on what gets passed here
Command pathfindingCommand = new PathfindThenFollowPathHolonomic(
        path,
        constraints,
        swerveSubsystem::getPose,
        swerveSubsystem::getRobotRelativeSpeeds,
        swerveSubsystem::driveRobotRelative,
        Constants.Swerve.pathFollowingConfig, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
        3.0, // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
        () -> {
          // Boolean supplier that controls when the path will be mirrored for the red alliance
          // This will flip the path being followed to the red side of the field.
          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE

          var alliance = DriverStation.getAlliance();
          if (alliance.isPresent()) {
            return alliance.get() == DriverStation.Alliance.Red;
          }
          return false;
        },
        swerveSubsystem // Reference to drive subsystem to set requirements
);

#include <pathplanner/lib/commands/PathfindThenFollowPath.h>

using namespace pathplanner;

// Load the path we want to pathfind to and follow
auto path = PathPlannerPath::fromPathFile("Example Human Player Pickup");

// Create the constraints to use while pathfinding
PathConstraints constraints = PathConstraints(
    3.0_mps, 4.0_mps_sq, 
    540_deg_per_s, 720_deg_per_s);

// See the "Follow a single path" example for more info on what gets passed here
// Assuming this is somewhere in the drive subsystem, so using "this"
frc2::CommandPtr pathfindingCommand = PathfindThenFollowPathHolonomic(
    path,
    constraints,
    [this](){ return getPose(); },
    [this](){ return getRobotRelativeSpeeds(); },
    [this](frc::ChassisSpeeds speeds){ driveRobotRelative(speeds); }, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    Constants::Swerve::pathFollowingConfig, // HolonomicPathFollwerConfig, see the API or "Follow a single path" example for more info
    []() {
        // Boolean supplier that controls when the path will be mirrored for the red alliance
        // This will flip the path being followed to the red side of the field.
        // THE ORIGIN WILL REMAIN ON THE BLUE SIDE

        auto alliance = DriverStation::GetAlliance();
        if (alliance) {
            return alliance.value() == DriverStation::Alliance::kRed;
        }
        return false;
    },
    this, // Reference to drive subsystem to set requirements
    3.0_m, // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
);

from pathplannerlib.commands import PathfindThenFollowPathHolonomic
from pathplannerlib.config import HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants
from pathplannerlib.path import PathPlannerPath PathConstraints
from wpimath.units import degreesToRadians

# Load the path we want to pathfind to and follow
path = PathPlannerPath.fromPathFile('Example Human Player Pickup');

# Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path.
constraints = PathConstraints(
    3.0, 4.0, 
    degreesToRadians(540), degreesToRadians(720)
)

# See the "Follow a single path" example for more info on what gets passed here
# Assuming this is somewhere in the drive subsystem, so using "self"
pathfindingCommand = PathfindThenFollowPathHolonomic(
    path,
    constraints,
    self.getPose,
    self.getCurrentSpeeds,
    self.driveRobotRelative,
    HolonomicPathFollowerConfig( # HolonomicPathFollowerConfig, this should likely live in your Constants class
          PIDConstants(5.0, 0.0, 0.0), # Translation PID constants
          PIDConstants(5.0, 0.0, 0.0), # Rotation PID constants
          4.5, # Max module speed, in m/s
          0.4, # Drive base radius in meters. Distance from robot center to furthest module.
          ReplanningConfig() # Default path replanning config. See the API for the options here
    ),
    self.shouldFlipPath, # Supplier to control path flipping based on alliance color
    self, # Reference to this subsystem to set requirements
    rotation_delay_distance=3.0 # Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
)

def shouldFlipPath():
    # Boolean supplier that controls when the path will be mirrored for the red alliance
    # This will flip the path being followed to the red side of the field.
    # THE ORIGIN WILL REMAIN ON THE BLUE SIDE
    return DriverStation.getAlliance() == DriverStation.Alliance.kRed

Custom Pathfinders

PathPlannerLib supports the ability to use a custom pathfinder implementation for pathfinding commands. In order to do so, your pathfinder must implement the methods in the Pathfinder interface.

public class MyPathfinder implements Pathfinder {
  /**
   * Get if a new path has been calculated since the last time a path was retrieved
   *
   * @return True if a new path is available
   */
  @Override
  public boolean isNewPathAvailable() {
    // TODO: Implement your pathfinder
  }

  /**
   * Get the most recently calculated path
   *
   * @param constraints The path constraints to use when creating the path
   * @param goalEndState The goal end state to use when creating the path
   * @return The PathPlannerPath created from the points calculated by the pathfinder
   */
  @Override
  public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
    // TODO: Implement your pathfinder
  }

  /**
   * Set the start position to pathfind from
   *
   * @param startPosition Start position on the field. If this is within an obstacle it will be
   *     moved to the nearest non-obstacle node.
   */
  @Override
  public void setStartPosition(Translation2d startPosition) {
    // TODO: Implement your pathfinder
  }

  /**
   * Set the goal position to pathfind to
   *
   * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
   *     to the nearest non-obstacle node.
   */
  @Override
  public void setGoalPosition(Translation2d goalPosition) {
    // TODO: Implement your pathfinder
  }

  /**
   * Set the dynamic obstacles that should be avoided while pathfinding.
   *
   * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
   *     opposite corners of a bounding box.
   * @param currentRobotPos The current position of the robot. This is needed to change the start
   *     position of the path to properly avoid obstacles
   */
  @Override
  public void setDynamicObstacles(
          List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
    // TODO: Implement your pathfinder
  }
}

#include <pathplanner/lib/pathfinding/Pathfinder.h>

class MyPathfinder : public Pathfinder {
public:
    /**
     * Get if a new path has been calculated since the last time a path was retrieved
     *
     * @return True if a new path is available
     */
    bool isNewPathAvailable() override {
        // TODO: Implement your pathfinder
    }

    /**
     * Get the most recently calculated path
     *
     * @param constraints The path constraints to use when creating the path
     * @param goalEndState The goal end state to use when creating the path
     * @return The PathPlannerPath created from the points calculated by the pathfinder
     */
    PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) override {
        // TODO: Implement your pathfinder
    }

    /**
     * Set the start position to pathfind from
     *
     * @param startPosition Start position on the field. If this is within an obstacle it will be
     *     moved to the nearest non-obstacle node.
     */
    void setStartPosition(const Translation2d& startPosition) override {
        // TODO: Implement your pathfinder
    }

    /**
     * Set the goal position to pathfind to
     *
     * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
     *     to the nearest non-obstacle node.
     */
    void setGoalPosition(const Translation2d& goalPosition) override {
        // TODO: Implement your pathfinder
    }

    /**
     * Set the dynamic obstacles that should be avoided while pathfinding.
     *
     * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
     *     opposite corners of a bounding box.
     * @param currentRobotPos The current position of the robot. This is needed to change the start
     *     position of the path to properly avoid obstacles
     */
    void setDynamicObstacles(
            const std::vector<std::pair<Translation2d, Translation2d>>& obs, const Translation2d& currentRobotPos) override {
        // TODO: Implement your pathfinder
    }
}

from pathplannerlib.pathfinders import Pathfinder

class MyPathfinder(Pathfinder):
    def isNewPathAvailable(self) -> bool:
        """
        Get if a new path has been calculated since the last time a path was retrieved

        :return: True if a new path is available
        """
        raise NotImplementedError

    def getCurrentPath(self, constraints: PathConstraints, goal_end_state: GoalEndState) -> Union[
        PathPlannerPath, None]:
        """
        Get the most recently calculated path

        :param constraints: The path constraints to use when creating the path
        :param goal_end_state: The goal end state to use when creating the path
        :return: The PathPlannerPath created from the points calculated by the pathfinder
        """
        raise NotImplementedError

    def setStartPosition(self, start_position: Translation2d) -> None:
        """
        Set the start position to pathfind from

        :param start_position: Start position on the field. If this is within an obstacle it will be moved to the nearest non-obstacle node.
        """
        raise NotImplementedError

    def setGoalPosition(self, goal_position: Translation2d) -> None:
        """
        Set the goal position to pathfind to

        :param goal_position: Goal position on the field. f this is within an obstacle it will be moved to the nearest non-obstacle node.
        """
        raise NotImplementedError

    def setDynamicObstacles(self, obs: List[Tuple[Translation2d, Translation2d]],
                            current_robot_pos: Translation2d) -> None:
        """
        Set the dynamic obstacles that should be avoided while pathfinding.

        :param obs: A List of Translation2d pairs representing obstacles. Each Translation2d represents opposite corners of a bounding box.
        :param current_robot_pos: The current position of the robot. This is needed to change the start position of the path to properly avoid obstacles
        """
        raise NotImplementedError

After creating your pathfinder, you must tell PathPlannerLib to use it for pathfinding commands. This should be done at the beginning of your robotInit method, before any pathfinding commands are created.

public void robotInit() {
  // DO THIS FIRST
  Pathfinding.setPathfinder(new MyPathfinder());

  // ... remaining robot initialization
}

#include <pathplanner/lib/pathfinding/Pathfinding.h>

using namespace pathplanner;

Robot::robotInit() {
    // DO THIS FIRST
    Pathfinding::setPathfinder(MyPathfinder());

    // ... remaining robot initialization
}

from pathplannerlib.pathfinding import Pathfinding

def robotInit():
    // DO THIS FIRST
    Pathfinding.setPathfinder(MyPathfinder())

    // ... remaining robot initialization

Java Warmup

public void robotInit() {
  // ... all other robot initialization

  // DO THIS AFTER CONFIGURATION OF YOUR DESIRED PATHFINDER
  PathfindingCommand.warmupCommand().schedule();
}

Last modified: 16 May 2024