Python Example: Automatic 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, 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.

Pathfind to Pose

AutoBuilder

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

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

Pathfind Then Follow Path

AutoBuilder

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

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