Skip to content

Python Example: Follow a Single Path

Michael Jansen edited this page Dec 7, 2023 · 1 revision

Using AutoBuilder

The easiest way to create a command to follow a single path is by using AutoBuilder. You must have previously configured AutoBuilder in order to use this option.

See Python Example: Build an Auto

from pathplannerlib.path import PathPlannerPath
from pathplannerlib.auto import AutoBuilder

def getAutonomousCommand():
    # Load the path you want to follow using its name in the GUI
    path = PathPlannerPath.fromPathFile('Example Path')

    # Create a path following command using AutoBuilder. This will also trigger event markers.
    return AutoBuilder.followPathWithEvents(path);

Manually Create Path Following Commands

Holonomic

from pathplannerlib.path import PathPlannerPath
from pathplannerlib.commands import FollowPathWithEvents, FollowPathHolonomic
from pathplannerlib.config import HolonomicPathFollowerConfig, ReplanningConfig, PIDConstants

# Assuming this is a method in your drive subsystem
def followPathCommand(pathName: str):
    path = PathPlannerPath.fromPathFile(pathName)

    # You must wrap the path following command in a FollowPathWithEvents command in order for event markers to work
    return FollowPathWithEvents(
        FollowPathHolonomic(
            path,
            self.getPose, # Robot pose supplier
            self.getRobotRelativeSpeeds, # ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
            self.driveRobotRelative, # Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
            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 # Reference to this subsystem to set requirements
        ),
        path, # FollowPathWithEvents also requires the path
        self.getPose # FollowPathWithEvents also requires the robot pose supplier
    )

Ramsete

from pathplannerlib.path import PathPlannerPath
from pathplannerlib.commands import FollowPathWithEvents, FollowPathRamsete
from pathplannerlib.config import ReplanningConfig, PIDConstants

# Assuming this is a method in your drive subsystem
def followPathCommand(pathName: str){
    path = PathPlannerPath.fromPathFile(pathName)

    # You must wrap the path following command in a FollowPathWithEvents command in order for event markers to work
    return FollowPathWithEvents(
        FollowPathRamsete(
            path,
            self.getPose, # Robot pose supplier
            self.getCurrentSpeeds, # Current ChassisSpeeds supplier
            self.drive, # Method that will drive the robot given ChassisSpeeds
            ReplanningConfig(), # Default path replanning config. See the API for the options here
            self # Reference to this subsystem to set requirements
        ),
        path, # FollowPathWithEvents also requires the path
        self.getPose # FollowPathWithEvents also requires the robot pose supplier
    )

LTV

from pathplannerlib.path import PathPlannerPath
from pathplannerlib.commands import FollowPathWithEvents, FollowPathLTV
from pathplannerlib.config import ReplanningConfig, PIDConstants

# Assuming this is a method in your drive subsystem
def followPathCommand(pathName: str){
    path = PathPlannerPath.fromPathFile(pathName)

    # You must wrap the path following command in a FollowPathWithEvents command in order for event markers to work
    return FollowPathWithEvents(
        FollowPathLTV(
            path,
            self.getPose, # Robot pose supplier
            self.getCurrentSpeeds, # Current ChassisSpeeds supplier
            self.drive, # Method that will drive the robot given ChassisSpeeds
            (0.0625, 0.125, 2.0), # qelems/error tolerances
            (1.0, 2.0), # relems/control effort
            0.02, # Robot control loop period in seconds. Default is 0.02
            ReplanningConfig(), # Default path replanning config. See the API for the options here
            self # Reference to this subsystem to set requirements
        ),
        path, # FollowPathWithEvents also requires the path
        self.getPose # FollowPathWithEvents also requires the robot pose supplier
    )
Clone this wiki locally