Pure Pursuit

Pure Pursuit is a path-following idea: instead of steering toward the next point right beside the robot, repeatedly choose a point farther ahead on the path and steer toward it. That moving target helps the robot follow a curve smoothly.

This page explains the geometry. It is not a complete drivetrain controller: encoder localization, motor control, path planning, and safety stops still need to be handled. Most FTC teams should learn the idea, then use a maintained library for the robot-specific implementation.

Path and pose

You need two things:

1. A path: a sequence of points describing where the robot should go on the field.
2. The robot's estimated pose: its field position and heading, usually supplied by odometry or a localization library.

Pure Pursuit uses the pose to choose a point on the path ahead of the robot. That point is the lookahead point. Its distance from the robot is the lookahead distance.

Lookahead point

At each update, find the part of the path near the robot and look for a point about one lookahead distance ahead. A line segment may cross a circle around the robot between two stored path points, so implementations often calculate the intersection instead of choosing the nearest stored point.

If there is no useful point ahead, the robot may be at the end of the path or its pose estimate may be wrong. Handle that case explicitly; do not steer toward an old point behind the robot.

Curvature

For the steering calculation, describe the lookahead point in robot-centered coordinates:

• x is forward from the robot;
• y is sideways from the robot (choose a sign convention and keep it consistent).

For a differential-drive robot, the point defines an arc. The curvature needed for that arc is:

Curvature is signed: with the convention above, a point to one side asks for a turn toward that side. For example, a target at (10, 5) gives κ = 10 / 125 = 0.08 per distance unit. This is a geometry example, not a motor-power setting.

The drivetrain controller turns curvature and a chosen speed into wheel commands. That last step depends on whether the robot is tank, mecanum, or another drive type; Pure Pursuit alone does not define it.

Lookahead distance

Lookahead distance changes how sharply the robot follows the path:

Begin slowly with one broad curve. Change lookahead a little at a time and watch the robot's actual track against the planned path. There is no value that works for every robot, speed, and field surface.

Path testing

1. Check the localization estimate while the robot is still. Make sure the field axes and heading direction match your path coordinates.
2. Lift the robot and verify that each motor command turns the wheels in the expected direction.
3. Run a short, slow path in a clear practice area with a stop condition in every loop.
4. Compare the planned path, estimated pose, and the robot's physical motion.
5. If the pose estimate is wrong, fix localization first. If the pose is right but the robot steers too sharply or cuts corners, tune lookahead and speed.

Never test a new path at full speed near people or field elements. Keep the robot easy to stop and have a mentor check the test area.

Geometry helpers

The following class contains only geometry. Save it as PurePursuitGeometry.java. It does not choose a route segment, update progress, or command motors. Coordinates use field +X forward at heading zero and +Y left; distances must share one unit.

circleIntersections() returns the segment parameters t where a segment crosses the lookahead circle. A result in [0, 1] lies on the segment. A tangent may return the same value twice. forwardCurvature() rejects targets at or behind the robot; a forward-only controller must stop or enter an explicit recovery state in that case.

public final class PurePursuitGeometry {
    private PurePursuitGeometry() {}

    public static double[] circleIntersections(
            double ax, double ay, double bx, double by,
            double robotX, double robotY, double radius) {
        if (!Double.isFinite(ax) || !Double.isFinite(ay)
                || !Double.isFinite(bx) || !Double.isFinite(by)
                || !Double.isFinite(robotX) || !Double.isFinite(robotY)
                || !Double.isFinite(radius) || radius <= 0.0) {
            throw new IllegalArgumentException("Finite coordinates and positive radius required");
        }
        double dx = bx - ax, dy = by - ay;
        double fx = ax - robotX, fy = ay - robotY;
        double a = dx * dx + dy * dy;
        if (a == 0.0) return new double[0];
        double b = 2.0 * (fx * dx + fy * dy);
        double c = fx * fx + fy * fy - radius * radius;
        double discriminant = b * b - 4.0 * a * c;
        if (discriminant < 0.0) return new double[0];
        double root = Math.sqrt(discriminant);
        double first = (-b - root) / (2.0 * a);
        double second = (-b + root) / (2.0 * a);
        boolean firstOnSegment = first >= 0.0 && first <= 1.0;
        boolean secondOnSegment = second >= 0.0 && second <= 1.0;
        if (firstOnSegment && secondOnSegment) return new double[] { first, second };
        if (firstOnSegment) return new double[] { first };
        if (secondOnSegment) return new double[] { second };
        return new double[0];
    }

    public static double forwardCurvature(
            double targetX, double targetY, double robotX, double robotY,
            double headingRadians) {
        if (!Double.isFinite(targetX) || !Double.isFinite(targetY)
                || !Double.isFinite(robotX) || !Double.isFinite(robotY)
                || !Double.isFinite(headingRadians)) {
            throw new IllegalArgumentException("Finite pose and target required");
        }
        double dx = targetX - robotX, dy = targetY - robotY;
        double localX = Math.cos(headingRadians) * dx + Math.sin(headingRadians) * dy;
        double localY = -Math.sin(headingRadians) * dx + Math.cos(headingRadians) * dy;
        double distanceSquared = localX * localX + localY * localY;
        if (localX <= 0.0 || distanceSquared < 1e-9) {
            throw new IllegalArgumentException("No forward target; stop or recover");
        }
        return 2.0 * localY / distanceSquared;
    }
}

For the segment (0, 0) to (10, 0), robot (5, 0), and radius 2, the intersections are t = 0.3 and 0.7: field points (3, 0) and (7, 0). If the robot faces +X, only (7, 0) is ahead. At robot (11, 0), the remaining circle intersection (9, 0) is behind; it must not produce a positive straight-drive command.

Path progress

A complete follower needs a progress policy in addition to those helpers:

1. Track the robot's projected progress on the current segment. Advance it monotonically within a bounded nearby part of the route.
2. Search the current and selected upcoming segments, discarding intersections earlier than that progress. Searching the entire path can jump to another branch at a crossing.
3. Choose a forward candidate in route order. Keep measured progress separate from the chosen lookahead: aiming farther ahead does not mean the robot has already travelled there.
4. Near the final segment, use the endpoint when it lies inside the lookahead circle. The curvature denominator must use the target's actual squared distance, as above, because that point is not necessarily on the circle.
5. Finish within measured position/heading tolerances. If the endpoint is behind after an overshoot, stop and use a defined recovery; do not keep driving forward or reuse an old target.

For differential drive, a speed v, curvature κ, and track width w give ideal wheel speeds vLeft = v × (1 - κw/2) and vRight = v × (1 + κw/2). Those are velocities, not motor powers. Use calibrated velocity control/feedforward and acceleration limits to turn them into hardware commands. Dividing by a guessed maximum speed is only an approximate power mapping.

A maintained follower handles progress, endpoints, and drivetrain commands. Use these geometry helpers to understand and test its behavior; they do not replace the full controller.

Troubleshooting

• The pose drifts: Pure Pursuit cannot fix bad odometry. Calibrate the localization system first.
• The robot turns the wrong way: Check the sign convention for heading, sideways position, and curvature.
• It cuts corners: Reduce speed or shorten lookahead; also check whether the path itself has sharp turns.
• It jitters: Increase lookahead modestly, reduce speed, and check for noisy pose measurements.
• It never finishes: Define what “at the end” means and stop the drivetrain when the endpoint condition is met.

Pathing libraries

The geometry is useful for understanding what a path follower is doing, but robust FTC code also needs localization, speed limits, endpoint handling, drivetrain-specific control, and safe stop behavior. See the Pedro Pathing guide for a maintained FTC pathing option. Check its official documentation for current installation and APIs; those change between major versions.

Continue with Odometry to understand pose estimates, Motion Planning to choose a route, or Sensor Fusion to combine measurements.