Heading interpolation strategies for Bézier segments

On a mecanum robot, heading (the angle the robot faces) is mechanically decoupled from the direction of travel. Odyssey takes advantage of this by controlling orientation independently from the translational path: each BezierCurve segment carries its own HeadingInterpolator instance, which maps the curve parameter t ∈ [0, 1] to a desired robot heading in radians. The Follower queries getHeadingAtT(t) on every control cycle and feeds the result to its heading PID, so you can mix and match orientation strategies across the segments of a single Path.

The HeadingInterpolator interface

All four strategies implement a single-method interface:

public interface HeadingInterpolator {
    double getHeading(double t, BezierCurve curve);
}

t is the arc-length–normalized curve parameter produced by getTAtDistance, and curve is the segment being followed — passed so that interpolators like TangentInterpolator can query the curve’s geometry directly.

The four strategies

ConstantInterpolator
LinearInterpolator
TangentInterpolator
FaceTargetInterpolator

ConstantInterpolator(double heading) — the robot holds a fixed heading for the entire segment, regardless of where it is on the curve. This is ideal for strafing manoeuvres where you want the robot to face a particular direction (toward a scoring target, for example) while sliding sideways.

// Hold 90° (facing the positive-Y axis) throughout the segment
new ConstantInterpolator(Math.PI / 2)

Internally, getHeading simply returns the stored value:

@Override
public double getHeading(double t, BezierCurve curve) {
    return heading; // constant — t is ignored
}

LinearInterpolator(double startHeading, double endHeading) — linearly interpolates from startHeading at t = 0 to endHeading at t = 1. The delta is computed via normalizeAngle so the rotation always takes the shortest angular path (never spinning more than 180°).

// Rotate from 0° to 90° as the robot traverses the segment
new LinearInterpolator(0, Math.PI / 2)

This is the most common interpolator for paths where the robot needs to change its orientation while moving. Pair it with matching angles at segment seams to avoid heading discontinuities (see Path seam continuity).

TangentInterpolator() — the robot always faces the direction of travel (the tangent to the curve at the current t). This gives natural, tank-drive-style motion where the front of the robot leads through every bend.

// Face the direction of travel at every point
new TangentInterpolator()

Under the hood it calls curve.getTangentAngle(t), which returns atan2 of the derivative vector B′(t):

@Override
public double getHeading(double t, BezierCurve curve) {
    return curve.getTangentAngle(t);
}

Because the heading is derived from curve geometry rather than a fixed angle, TangentInterpolator adapts automatically to any curve shape — no parameters needed.

FaceTargetInterpolator(Vector2d target) — the robot always faces a fixed point on the field, regardless of where it is on the curve. The heading is recomputed at every t as atan2(target.y − point.y, target.x − point.x).

// Always face the submersible at (1 800, 1 800)
new FaceTargetInterpolator(new Vector2d(1800, 1800))

Use this when you need to keep a sensor, intake, or scorer aimed at a landmark — a junction, a sample pile, or an AprilTag — throughout the entire segment.

Quick-reference examples

// Hold 90° throughout
new ConstantInterpolator(Math.PI / 2)

// Rotate from 0° to 90°
new LinearInterpolator(0, Math.PI / 2)

// Face direction of travel
new TangentInterpolator()

// Always face the submersible at (1800, 1800)
new FaceTargetInterpolator(new Vector2d(1800, 1800))

Choosing a strategy

ConstantInterpolator

Robot orientation is fixed. Best for strafing across the field or when the robot must face a fixed direction for the whole segment.

LinearInterpolator

Smooth turn from one angle to another. The go-to choice for most path segments that need an orientation change.

TangentInterpolator

Robot faces the direction of travel. Natural feel for high-speed sweeping curves; no tuning required.

FaceTargetInterpolator

Robot always points at a field landmark. Useful when a sensor or mechanism must track a fixed target while the robot moves.

All angles are in radians. Use Math.toRadians() to convert degree values from a driver-station dashboard or competition sheet into the radians that every HeadingInterpolator expects. For example, Math.toRadians(90) gives π/2 ≈ 1.5708.

Get Started

Core Concepts

Guides

Heading interpolation strategies for Bézier segments

The HeadingInterpolator interface

The four strategies

Quick-reference examples

Choosing a strategy

ConstantInterpolator

LinearInterpolator

TangentInterpolator

FaceTargetInterpolator

Build docs developers (and LLMs) love

Get Started

Core Concepts

Guides

Documentation Index

​The HeadingInterpolator interface

​The four strategies

​Quick-reference examples

​Choosing a strategy

ConstantInterpolator

LinearInterpolator

TangentInterpolator

FaceTargetInterpolator

Build docs developers (and LLMs) love

The HeadingInterpolator interface

The four strategies

Quick-reference examples

Choosing a strategy