Using Pathfinder

Note

This documentation is a Python translation of Jaci’s Java notes that can be found at https://github.com/JacisNonsense/Pathfinder/wiki/Pathfinder-for-FRC—Java

Installation (RobotPy on a RoboRIO)

Use robotpy-installer to install the precompiled package.

robotpy-installer download-opkg python37-robotpy-pathfinder
robotpy-installer install-opkg python37-robotpy-pathfinder

Installation (Windows)

We now publish Windows binaries for pathfinder, you should be able to install them via pip on supported versions of Python 3:

py -m pip install robotpy-pathfinder

Installation (other)

Note that this requires a C++11 compiler to be present on your system, as I’m not currently publishing non-Windows wheels of this library.

pip3 install 'pybind11>=2.2'
pip3 install robotpy-pathfinder

Generating a Trajectory

Whenever you want to generate a trajectory, you can do so by defining a set of waypoints and calling pathfinder.generate():

import pathfinder as pf

points = [
    pf.Waypoint(-4, -1, math.radians(-45.0)),   # Waypoint @ x=-4, y=-1, exit angle=-45 degrees
    pf.Waypoint(-2, -2, 0),                     # Waypoint @ x=-2, y=-2, exit angle=0 radians
    pf.Waypoint(0, 0, 0),                       # Waypoint @ x=0, y=0,   exit angle=0 radians
]

info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
                               dt=0.05, # 50ms
                               max_velocity=1.7,
                               max_acceleration=2.0,
                               max_jerk=60.0)

You can also modify the trajectory for either Swerve or Tank drive:

modifier = pf.modifiers.TankModifier(trajectory).modify(0.5)
# OR
modifier = pf.modifiers.SwerveModifier(trajectory).modify(0.5, 0.6)

Note

It can take a really long time to generate a trajectory on a RoboRIO, but very little time on a modern computer. You can take advantage of this by pre-generating the trajectory before deployment. When using with RobotPy, you can use the following pattern to pregenerate them automatically before you deploy code to the robot:

import os.path
import pickle

# because of a quirk in pyfrc, this must be in a subdirectory
# or the file won't get copied over to the robot
pickle_file = os.path.join(os.path.dirname(__file__), 'trajectory.pickle')

if wpilib.RobotBase.isSimulation():
    # generate the trajectory here

    # and then write it out
    with open(pickle_file, 'wb') as fp:
        pickle.dump(trajectory, fp)
else:
    with open('fname', 'rb') as fp:
        trajectory = pickle.load(fp)

This works because whenever you run robot.py deploy, your robot code modules are imported and executed.

Following a Trajectory

To get your robot to follow a trajectory, you can use the EncoderFollower object. As the name suggests, this will use encoders as feedback to guide your robot along the trajectory. It is important that your time step passed into your generate call is the same as the time difference between control loop iterations, otherwise you may find your path tracking inaccurately.

Tank Drive

Create two EncoderFollower objects, one for the left and one for the right:

from pathfinder.followers import EncoderFollower

left = EncoderFollower(modifier.getLeftTrajectory())
right = EncoderFollower(modifier.getRightTrajectory())

When you’re ready to start following:

Setup your encoder details:

# Encoder Position is the current, cumulative position of your encoder. If
# you're using an SRX, this will be the 'getEncPosition' function.
# 1000 is the amount of encoder ticks per full revolution
# Wheel Diameter is the diameter of your wheels (or pulley for a track system) in meters
left.configureEncoder(encoder_position, 1000, wheel_diameter)

Set your PID/VA variables:

# The first argument is the proportional gain. Usually this will be quite high
# The second argument is the integral gain. This is unused for motion profiling
# The third argument is the derivative gain. Tweak this if you are unhappy with the tracking of the trajectory
# The fourth argument is the velocity ratio. This is 1 over the maximum velocity you provided in the
#      trajectory configuration (it translates m/s to a -1 to 1 scale that your motors can read)
# The fifth argument is your acceleration gain. Tweak this if you want to get to a higher or lower speed quicker
left.configurePIDVA(1.0, 0.0, 0.0, 1 / max_velocity, 0)

Inside your control loop, you can add the following code to calculate the desired output of your motors:

output = left.calculate(encoder_position);

Now, keep in mind this doesn’t account for heading of your robot, meaning it won’t track a curved path. To adjust for this, you can use your Gyroscope and the desired heading of the robot to create a simple, proportional gain that will turn your tracks. A full example, including the calculations for each side of the drive train is given below.

l = left.calculate(encoder_position_left)
r = right.calculate(encoder_position_right)

gyro_heading = ... your gyro code here ...    # Assuming the gyro is giving a value in degrees
desired_heading = pf.r2d(left.getHeading())   # Should also be in degrees

angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
turn = 0.8 * (-1.0/80.0) * angleDifference

setLeftMotors(l + turn)
setRightMotors(r - turn)

The boundHalfDegrees() function simply binds a degrees angle to -180..180, making sure we don’t end up with an absurdly large turn value.

Note that for the desired heading of the robot, we’re only using the left follower as a comparison. This is because both the left and right sides of a tank drive are parallel, and therefore always face in the same direction.

Swerve Drive

Swerve Drive following is very similar to Tank Drive, except each wheel can have a different trajectory and heading. To make things simple, I will be showing how to do it for a single wheel. For all 4 wheels, just do the exact same thing 4 times.

Create an EncoderFollower object for your wheel:

from pathfinder.followers import EncoderFollower

flFollower = EncoderFollower(modifier.getFrontLeftTrajectory())   # Front Left wheel

When you’re ready to start following:

Setup your encoder details:

# Encoder Position is the current, cumulative position of your encoder. If
# you're using an SRX, this will be the 'getEncPosition' function.
# 1000 is the amount of encoder ticks per full revolution
# Wheel Diameter is the diameter of your wheel in meters
flFollower.configureEncoder(fl_encoder_position, 1000, wheel_diameter)

Set your PID/VA variables:

# The first argument is the proportional gain. Usually this will be quite high
# The second argument is the integral gain. This is unused for motion profiling
# The third argument is the derivative gain. Tweak this if you are unhappy with the tracking of the trajectory
# The fourth argument is the velocity ratio. This is 1 over the maximum velocity you provided in the
#      trajectory configuration (it translates m/s to a -1 to 1 scale that your motors can read)
# The fifth argument is your acceleration gain. Tweak this if you want to get to a higher or lower speed quicker
flFollower.configurePIDVA(1.0, 0.0, 0.0, 1 / max_velocity, 0)

Inside your control loop, you can add the following code to calculate the desired output of your motor:

output = flFollower.calculate(fl_encoder_position)

The above EncoderFollower.calculate call won’t account for the heading of your wheel. If you run this as is, you will be permanently going in a straight line. To fix this, we need to know the heading of your swerve wheel. For most teams, this will be done with an encoder. Some example code for dealing with heading is given below:

output = flFollower.calculate(fl_encoder_position)
desiredHeading = pf.boundHalfDegrees(pf.r2d(flFollower.getHeading()))    # Bound to -180..180 degrees

frontLeftWheel.setDirection(desiredHeading)
frontLeftWheel.setSpeed(output)

The setDirection implementation is up to you. Usually, for a swerve drive, this will be some kind of PID control loop.

Example code

The RobotPy examples repository has a pathfinder example program in it, which also contains a working physics module so you can experiment with pathfinder using the pyfrc simulator.