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, its 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.
• 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.
• You are able to customize the replanning config for the path you follow after pathfinding, but you should at least have initial replanning enabled for this path. It is essentially required for it to work properly.

The following examples will only show the holonomic command variants. But, the Ramsete and LTV variants are very similar and will have similar config to their normal path following command versions.

AdvantageKit Compatibility

IMPORTANT:

If using AdvantageKit, you must use a different pathfinder implementation in order for pathfinding to be compatible with log replay. To do so, add this class to your robot code project, then make the following code change in Robot.java:

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

    // ... remaining robot initialization
}

Pathfind to Pose

AutoBuilder

The easiest way to create a pathfinding command is by using AutoBuilder. See Java Example: 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.
);

Manual

// 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
);

Pathfind Then Follow Path

AutoBuilder

The easiest way to create a pathfinding command is by using AutoBuilder. See Java Example: 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.
);

Manual

// 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
    swerveSubsystem // Reference to drive subsystem to set requirements
);