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 python36-robotpy-pathfinder
robotpy-installer install-opkg python36-robotpy-pathfinder
Installation (other)¶
Note that this requires a C++11 compiler to be present on your system, as I’m not currently publishing 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.