Stanley Controller

Standalone Stanley + PID trajectory-tracking controller for unicycle / differential-drive robots. Feed it a planned trajectory, call it every control step, and it returns (v, w) — linear and angular velocity commands.

Single file, no framework dependencies. Requires only numpy.

Quick start

import numpy as np
from stanley_controller import StanleyController

ctrl = StanleyController(time_step=0.25)   # match your control period
ctrl.set_trajectory(traj)                  # [T, 4] = (x, y, cos_h, sin_h), world frame

while not ctrl.is_done(px, py):
    v, w = ctrl.compute(px, py, theta)     # current robot pose, world frame
    # send (v, w) to your robot, advance one control step

Run the demo (S-curve tracking):

python example.py

It prints tracking stats and writes stanley_scurve.mp4 — an animation of the robot following the S-curve with live speed / mode / cross-track error readouts (needs matplotlib + ffmpeg). Without ffmpeg it saves a static stanley_scurve.png instead; without matplotlib it prints stats only.

Trajectory format

• [T, 4]: rows of (x, y, cos_heading, sin_heading) in the world frame. A time column t = index * time_step is appended automatically.
• [T, 5]: same with an explicit trailing time column.
• Waypoint spacing encodes speed. The controller derives its target speed from the spacing between consecutive waypoints (spacing / time_step), capped by the curvature-based limit. Space waypoints at intended_speed * time_step.
• Replanning: just call set_trajectory() again — progress tracking and the PID integrator reset automatically.

Control law

• Lateral (forward mode): w = v * kappa + k_steer * (heading_err + arctan(k_e * cross_track / (|v| + k_soft))) — curvature feedforward plus Stanley feedback. cross_track > 0 means the robot is to the right of the path.
• Longitudinal: PID toward a target speed built from waypoint spacing, curvature slowdown v_pref / (1 + curvature_speed_gain * |kappa|), and an approach ramp within approach_dist of the goal.
• Modes (ctrl.last_mode): forward / reverse (lookahead point behind the robot; pure-pursuit style steering) / rotate (in-place turn when heading error exceeds rotate_to_heading_min_angle) / stop.
• Handles self-crossing trajectories: closest-point search never jumps backward past already-reached progress.

Parameters (defaults)

Group	Param	Default	Meaning
Speed	v_pref / max_speed	1.0 / 1.0	preferred / hard max linear speed (m/s)
Speed	max_angular_velocity	1.5	hard max |w| (rad/s)
Timing	time_step	0.25	control period (s) — must match your loop
Stanley	k_e	2.5	cross-track error gain
Stanley	k_soft	1.0	low-speed softening constant
Stanley	k_steer	1.0	overall steering gain
PID	k_p / k_i / k_d	1.0 / 0.2 / 0.0	longitudinal speed PID
Profile	curvature_speed_gain	1.5	curve slowdown strength
Profile	min_speed / min_approach_speed	0.25 / 0.05	speed floors (m/s)
Profile	approach_dist	0.6	goal approach ramp distance (m)
Mode	rotate_to_heading_min_angle	0.785	heading error that triggers in-place rotation (rad)
Mode	rotate_angular_vel	1.2	in-place rotation speed (rad/s)
Mode	reverse_speed_ratio	0.5	reverse speed = ratio * forward target
Mode	goal_tolerance	0.1	is_done position tolerance (m)
Accel	max_angular_accel	3.2	used by the rotate-mode decel profile
Accel	max_linear_accel / max_linear_decel	2.5 / 2.5	used only by the optional _apply_accel_limits helper

Notes

• Output is clipped to max_speed / max_angular_velocity but not acceleration-limited; smooth it downstream if your platform needs it.
• ctrl._debug (dict) exposes the last step's heading error, cross-track error and closest waypoint index for logging.