In the navigation stack (Chapter 14), the global planner outputs a reference path (often a polyline of waypoints), and the local motion generator must produce feasible commands that track that path while respecting the mobile base. Pure pursuit is a geometric tracking method: it “chases” a point ahead on the path and computes a curvature command that would bring the robot to that point along a circular arc.

We assume planar motion with pose \( (x, y, \theta) \), where \(\theta\) is the heading (yaw). The controller uses: \( \mathcal{P} = \{\mathbf{p}(s)\} \) (a path parameterized by arclength \(s\)), a lookahead distance \( L_d \), and a selected lookahead point \( \mathbf{p}_L = \mathbf{p}(s_L) \) with \( s_L = s_c + L_d \), where \(s_c\) corresponds to the closest point on the path to the current robot position.

flowchart TD
  IN["Inputs: pose (x,y,theta), path waypoints, speed v"] --> CP["Find closest point on path (progress s_c)"]
  CP --> LA["Select lookahead point p_L at s_L = s_c + Ld"]
  LA --> TF["Transform p_L into robot frame: (x_r, y_r)"]
  TF --> CURV["Compute curvature kappa = 2*y_r / (x_r^2 + y_r^2)"]
  CURV --> LIM["Apply limits: |kappa|<=kappa_max, |delta|<=delta_max, |omega|<=omega_max"]
  LIM --> OUT["Output command: (v, omega) or (v, delta)"]
  OUT --> UPD["Robot executes; repeat at control rate"]
        

Two common command interfaces: (i) \(v\) and \(\omega\) for a unicycle/differential-drive interface; (ii) \(v\) and steering angle \(\delta\) for an Ackermann/bicycle interface. Pure pursuit is often used as the lateral controller; longitudinal speed control is typically handled separately (e.g., speed schedule vs curvature, or a PID on speed).

Let the robot pose in the world frame be \( (x, y, \theta) \). Define the rotation matrix \( \mathbf{R}(\theta) \in \mathbb{R}^{2\times 2} \) as

\[ \mathbf{R}(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta \end{bmatrix}. \]

For any point \(\mathbf{p}\) in world coordinates, its coordinates in the robot body frame (x forward, y left) are \(\mathbf{p}^b = \mathbf{R}(\theta)^\top (\mathbf{p} - \mathbf{t})\), where \(\mathbf{t} = [x, y]^\top\). For the lookahead point \(\mathbf{p}_L\), write \(\mathbf{p}_L^b = [x_r, y_r]^\top\).

Pure pursuit constructs the unique circle that: (i) passes through the robot origin in body frame; (ii) passes through the lookahead point \( (x_r, y_r) \); and (iii) has tangent aligned with the robot heading at the origin. The circle’s curvature \( \kappa \) determines the commanded yaw rate for a unicycle model (Section 4).

In the robot body frame, place the robot at the origin with heading along the +x axis. Consider a circle of radius \(R \) whose center lies on the y-axis at \((0, R)\). This choice enforces that the tangent at the origin is horizontal (aligned with +x).

The circle equation is \( x^2 + (y - R)^2 = R^2 \). Since \((x_r, y_r)\) lies on the circle, we have:

\[ x_r^2 + (y_r - R)^2 = R^2 \;\Rightarrow\; x_r^2 + y_r^2 - 2 R y_r = 0 \;\Rightarrow\; R = \frac{x_r^2 + y_r^2}{2 y_r}. \]

The curvature is \( \kappa = 1/R \). Therefore the pure pursuit curvature command is:

\[ \boxed{\;\kappa = \frac{2 y_r}{x_r^2 + y_r^2}\;} \quad \text{(pure pursuit curvature)}. \]

If you define \(L = \sqrt{x_r^2 + y_r^2}\), then \( \kappa = 2 y_r / L^2 \). Note the essential qualitative properties:

• If the lookahead is directly ahead ( \(y_r = 0\)), then \(\kappa = 0\) and the controller commands straight motion.
• If the lookahead is left ( \(y_r > 0\)), then \(\kappa > 0\) and the robot turns left; similarly for right turns.
• Larger lookahead magnitude \(L\) reduces \(|\kappa|\), producing smoother but less aggressive tracking.

Implementation note. The derivation requires \(y_r \neq 0\) to compute \(R\). Numerically, you handle \(|y_r|\) small by computing \(\kappa\) via the formula above and clamping to a maximum curvature \(\kappa_{\max}\).

Pure pursuit outputs a curvature command \(\kappa\). To drive the base, map curvature into the command interface used by the robot.

Unicycle / differential-drive interface. The kinematic model is:

\[ \dot x = v\cos\theta,\quad \dot y = v\sin\theta,\quad \dot\theta = \omega. \]

Curvature is \(\kappa = \omega / v\) (for \(v \neq 0\)), so: \( \omega = v\kappa \). Saturate \(\omega\) to match actuator limits.

Ackermann / bicycle interface. For the kinematic bicycle model:

\[ \dot\theta = \frac{v}{L}\tan\delta, \quad \text{so}\quad \kappa = \frac{\tan\delta}{L}, \quad \delta = \arctan(L\kappa), \]

where \(L\) is the wheelbase. Again, saturate \(\delta\) to steering limits.

Speed scheduling. A practical coupling is to increase lookahead with speed: \( L_d(v) = \operatorname{clip}(L_{d,\min} + k_v |v|,\; L_{d,\min},\; L_{d,\max}) \). This reduces oscillations at high speed and improves comfort.

Real navigation stacks represent the path as waypoints \(\{\mathbf{w}_0,\dots,\mathbf{w}_{N-1}\}\). Pure pursuit requires a lookahead point on the continuous polyline. Two standard strategies:

1. Arclength lookahead: Find the closest point on the polyline, compute its arclength coordinate \(s_c\), and take \(s_L = s_c + L_d\) to interpolate on the polyline.
2. Circle intersection: Intersect the circle of radius \(L_d\) around the robot with path segments and pick the intersection that lies “ahead” along the path.

The arclength method is robust and easy when you can compute segment lengths and interpolate. The circle method matches the original “pursuit” intuition and can behave better when the path is sparse, but requires careful disambiguation when multiple intersections exist.

In the provided implementations, the main code uses the arclength method; an exercise file implements the circle intersection method.

Pure pursuit is geometric, but we can still analyze local stability for a straight reference path. Consider a straight path along the world x-axis, and define: \(e_y\) = lateral (cross-track) error and \(e_\theta\) = heading error relative to the path tangent. For a unicycle with forward speed \(v > 0\):

\[ \dot e_y = v\sin(e_\theta),\quad \dot e_\theta = -\omega. \]

For small errors, \(\sin(e_\theta) \approx e_\theta\). The pure pursuit lookahead point in body coordinates satisfies, to first order, \(y_r \approx e_y + L_d e_\theta\) (lateral error plus the lateral offset caused by heading error over the lookahead). Substituting \(\omega = v\kappa\) and \(\kappa \approx 2y_r/L_d^2\) yields:

\[ \dot e_y \approx v e_\theta,\quad \dot e_\theta \approx -\frac{2v}{L_d^2}\,(e_y + L_d e_\theta). \]

Differentiate the first equation and eliminate \(e_\theta\) to obtain a second-order linear ODE:

\[ \ddot e_y + \frac{2v}{L_d}\dot e_y + \frac{2v^2}{L_d^2} e_y = 0. \]

The characteristic polynomial is \( \lambda^2 + (2v/L_d)\lambda + 2(v^2/L_d^2) \), whose coefficients are positive for \(v > 0\), \(L_d > 0\). Hence both eigenvalues have negative real parts, so the origin \((e_y,e_\theta)=(0,0)\) is exponentially stable in the linearized model. This explains why too-small \(L_d\) can create oscillatory behavior (larger natural frequency), while larger \(L_d\) slows convergence but improves smoothness.

In mobile systems, actuator limits and discretization matter. Common guardrails:

• Curvature saturation: \( \kappa \leftarrow \operatorname{clip}(\kappa, -\kappa_{\max}, \kappa_{\max}) \).
• Steering/yaw-rate saturation: map to \(\delta\) or \(\omega\) and clip.
• Minimum speed handling: if \(|v|\) is near zero, reduce gains or hold steering to avoid numerical issues.
• Waypoint sparsity: if the path has large gaps, increase \(L_d\) or interpolate a denser polyline.
• Goal handling: when \(s_L\) goes beyond the end of the path, use the final waypoint and reduce speed.

A speed-dependent lookahead is widely used:

\[ L_d(v) = \operatorname{clip}\big(L_{d,\min} + k_v |v|,\; L_{d,\min},\; L_{d,\max}\big). \]

For tighter curves (high path curvature), additional heuristics reduce \(L_d\) to increase responsiveness. In later lessons (DWA/TEB), the local planner may output short-horizon trajectories; pure pursuit can then track the trajectory’s centerline similarly.

This implementation computes the closest point on a polyline, selects a lookahead point by arclength interpolation, computes curvature \(\kappa\), and simulates a kinematic bicycle model. It is intentionally minimal so you can port the geometry to your own stack.

Code file: Chapter15_Lesson1.py

"""
Chapter15_Lesson1.py
Pure Pursuit and Geometric Path Tracking — minimal, from-scratch reference implementation.

Dependencies:
  numpy
  matplotlib (optional for plotting)

This file focuses on geometry + kinematic simulation (bicycle model).
For a ROS2 integration sketch, see Chapter15_Lesson1.cpp (USE_ROS2 section).
"""

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import Tuple

import numpy as np


@dataclass
class Pose2D:
    x: float
    y: float
    theta: float  # heading [rad]


def wrap_angle(a: float) -> float:
    """Map angle to (-pi, pi]."""
    return (a + math.pi) % (2.0 * math.pi) - math.pi


def rot2(theta: float) -> np.ndarray:
    c, s = math.cos(theta), math.sin(theta)
    return np.array([[c, -s], [s, c]], dtype=float)


def world_to_body(pose: Pose2D, p_world: np.ndarray) -> np.ndarray:
    """
    Transform a world point to robot body frame:
      x forward, y left.
    """
    R = rot2(pose.theta).T
    t = np.array([pose.x, pose.y], dtype=float)
    return R @ (p_world - t)


def polyline_arclength(path: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
    """
    Compute cumulative arclength s[k] and segment lengths seg[k] for polyline points.
    path: (N,2)
    """
    dif = np.diff(path, axis=0)
    seg = np.linalg.norm(dif, axis=1)
    s = np.concatenate([[0.0], np.cumsum(seg)])
    return s, seg


def closest_point_on_segment(p: np.ndarray, a: np.ndarray, b: np.ndarray) -> Tuple[np.ndarray, float]:
    ab = b - a
    denom = float(np.dot(ab, ab))
    if denom <= 1e-12:
        return a.copy(), 0.0
    t = float(np.dot(p - a, ab) / denom)
    t_clamped = max(0.0, min(1.0, t))
    q = a + t_clamped * ab
    return q, t_clamped


def closest_point_on_polyline(p: np.ndarray, path: np.ndarray) -> Tuple[np.ndarray, int, float, float]:
    """
    Return:
      q: closest point in R^2
      i: segment index (between i and i+1)
      t: segment parameter in [0,1]
      d: distance ||p-q||
    """
    best_d = float("inf")
    best_q, best_i, best_t = path[0].copy(), 0, 0.0
    for i in range(len(path) - 1):
        q, t = closest_point_on_segment(p, path[i], path[i + 1])
        d = float(np.linalg.norm(p - q))
        if d < best_d:
            best_d = d
            best_q, best_i, best_t = q, i, t
    return best_q, best_i, best_t, best_d


def point_at_arclength(path: np.ndarray, s: np.ndarray, seg: np.ndarray, s_query: float) -> np.ndarray:
    """Return point at arclength s_query along polyline (clamped)."""
    if s_query <= 0.0:
        return path[0].copy()
    if s_query >= s[-1]:
        return path[-1].copy()

    j = int(np.searchsorted(s, s_query, side="right") - 1)  # j in [0, N-2]
    ds = s_query - s[j]
    t = ds / max(seg[j], 1e-12)
    return (1.0 - t) * path[j] + t * path[j + 1]


def pure_pursuit_curvature(pose: Pose2D, path: np.ndarray, Ld: float) -> Tuple[float, np.ndarray, np.ndarray]:
    """
    Classic pure pursuit curvature:
      - Find closest point on path (for progress)
      - Choose lookahead point Ld ahead in arclength
      - Transform lookahead to body frame (x_r, y_r)
      - Command curvature kappa = 2*y_r / (x_r^2 + y_r^2)

    Returns:
      kappa: curvature command [1/m]
      p_closest: closest point (world)
      p_look: lookahead point (world)
    """
    Ld = max(float(Ld), 1e-3)

    p = np.array([pose.x, pose.y], dtype=float)
    s, seg = polyline_arclength(path)
    q, i, t, _ = closest_point_on_polyline(p, path)

    s_closest = float(s[i] + t * seg[i])
    p_look = point_at_arclength(path, s, seg, s_closest + Ld)

    look_b = world_to_body(pose, p_look)
    x_r, y_r = float(look_b[0]), float(look_b[1])

    L = math.hypot(x_r, y_r)
    if L <= 1e-9:
        return 0.0, q, p_look

    kappa = 2.0 * y_r / (L * L)
    return float(kappa), q, p_look


def bicycle_step(pose: Pose2D, v: float, delta: float, wheelbase: float, dt: float) -> Pose2D:
    """Kinematic bicycle (no slip)."""
    x = pose.x + v * math.cos(pose.theta) * dt
    y = pose.y + v * math.sin(pose.theta) * dt
    theta = wrap_angle(pose.theta + (v / wheelbase) * math.tan(delta) * dt)
    return Pose2D(x, y, theta)


def speed_dependent_lookahead(v: float, Ld_min: float, Ld_max: float, k_v: float) -> float:
    """
    Simple heuristic: Ld = clamp(Ld_min + k_v * |v|, Ld_min, Ld_max).
    """
    return float(min(Ld_max, max(Ld_min, Ld_min + k_v * abs(v))))


def demo():
    # Path: gentle sinusoid
    xs = np.linspace(0.0, 25.0, 500)
    ys = 1.8 * np.sin(0.22 * xs)
    path = np.column_stack([xs, ys])

    pose = Pose2D(x=-2.0, y=-2.0, theta=0.2)
    dt = 0.02
    wheelbase = 0.33  # [m] small AGV-like

    v = 1.5  # [m/s]
    Ld_min, Ld_max, k_v = 0.8, 3.5, 0.8
    kappa_max = 1.6  # [1/m]
    delta_max = math.radians(32.0)

    traj = []
    look_pts = []

    for _ in range(int(25.0 / dt)):
        Ld = speed_dependent_lookahead(v, Ld_min, Ld_max, k_v)
        kappa, _, pL = pure_pursuit_curvature(pose, path, Ld)
        kappa = float(np.clip(kappa, -kappa_max, kappa_max))

        delta = math.atan(wheelbase * kappa)
        delta = float(np.clip(delta, -delta_max, delta_max))

        pose = bicycle_step(pose, v, delta, wheelbase, dt)
        traj.append((pose.x, pose.y))
        look_pts.append((float(pL[0]), float(pL[1])))

    traj = np.array(traj)
    look_pts = np.array(look_pts)

    try:
        import matplotlib.pyplot as plt

        plt.figure()
        plt.plot(path[:, 0], path[:, 1], label="path")
        plt.plot(traj[:, 0], traj[:, 1], label="robot")
        plt.plot(look_pts[::60, 0], look_pts[::60, 1], ".", label="lookahead (every 60)")
        plt.axis("equal")
        plt.xlabel("x [m]")
        plt.ylabel("y [m]")
        plt.title("Pure Pursuit (kinematic bicycle)")
        plt.legend()
        plt.show()
    except Exception as e:
        print("Plot skipped:", e)


if __name__ == "__main__":
    demo()
      

The exercise implementation below uses the “circle intersection” lookahead selection:

Code file: Chapter15_Lesson1_Ex1.py

"""
Chapter15_Lesson1_Ex1.py
Exercise: Compute the lookahead point by intersecting a circle (radius Ld around robot)
with each path segment and selecting the "forward-most" intersection along the path.

This mirrors a common "circle intersection" implementation of pure pursuit.

Dependencies: numpy
"""

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import List, Optional, Tuple

import numpy as np


@dataclass
class Pose2D:
    x: float
    y: float
    theta: float


def circle_segment_intersections(c: np.ndarray, r: float, a: np.ndarray, b: np.ndarray) -> List[np.ndarray]:
    """
    Intersections of circle (center c, radius r) with segment [a,b].
    Return list of points (0,1,2 solutions) that lie on the segment.
    """
    d = b - a
    f = a - c
    A = float(np.dot(d, d))
    B = 2.0 * float(np.dot(f, d))
    C = float(np.dot(f, f) - r * r)

    disc = B * B - 4.0 * A * C
    if disc < 0.0 or A < 1e-12:
        return []
    disc_sqrt = math.sqrt(max(0.0, disc))

    t1 = (-B - disc_sqrt) / (2.0 * A)
    t2 = (-B + disc_sqrt) / (2.0 * A)

    pts = []
    for t in (t1, t2):
        if 0.0 <= t <= 1.0:
            pts.append(a + t * d)
    return pts


def lookahead_by_circle(path: np.ndarray, progress_idx: int, pose: Pose2D, Ld: float) -> Optional[Tuple[np.ndarray, int]]:
    """
    Starting from progress_idx, find the first segment intersection with the lookahead circle.
    Return (p_look, seg_index) or None if no intersection (e.g., near end).
    """
    c = np.array([pose.x, pose.y], dtype=float)
    for i in range(progress_idx, len(path) - 1):
        pts = circle_segment_intersections(c, Ld, path[i], path[i + 1])
        if pts:
            # choose the intersection closer to the end of the segment (more "forward")
            if len(pts) == 1:
                return pts[0], i
            # pick point with larger projection along segment direction
            d = path[i + 1] - path[i]
            scores = [float(np.dot(p - path[i], d)) for p in pts]
            return pts[int(np.argmax(scores))], i
    return None


def demo():
    # Simple path: straight then turn
    path = np.array([[0.0, 0.0],
                     [5.0, 0.0],
                     [8.0, 2.0],
                     [10.0, 5.0]], dtype=float)

    pose = Pose2D(x=1.0, y=-1.0, theta=0.0)
    Ld = 2.5

    out = lookahead_by_circle(path, progress_idx=0, pose=pose, Ld=Ld)
    print("Lookahead:", out)


if __name__ == "__main__":
    demo()
      

The C++ file contains the same geometry (closest point, arclength lookahead, curvature), a simple simulation demo, and an optional ROS2 skeleton showing how you would connect to nav_msgs/Path and publish cmd_vel.

Code file: Chapter15_Lesson1.cpp

// Chapter15_Lesson1.cpp
// Pure Pursuit and Geometric Path Tracking (C++17)
// - Core geometry is library-free.
// - Optional ROS2 integration sketch is provided under USE_ROS2.
//
// Build (core demo only):
//   g++ -std=c++17 -O2 Chapter15_Lesson1.cpp -o pp_demo
//
// If you want ROS2:
//   - Define USE_ROS2 and link against rclcpp, nav_msgs, geometry_msgs, etc.
//   - The ROS2 block is a minimal sketch (not a full package).

#include <cmath>
#include <cstddef>
#include <iostream>
#include <limits>
#include <string>
#include <vector>

struct Vec2 {
  double x{0.0}, y{0.0};
};

struct Pose2D {
  double x{0.0}, y{0.0}, theta{0.0};
};

static inline double wrapAngle(double a) {
  while (a <= -M_PI) a += 2.0 * M_PI;
  while (a >   M_PI) a -= 2.0 * M_PI;
  return a;
}

static inline Vec2 operator+(const Vec2& a, const Vec2& b) { return {a.x + b.x, a.y + b.y}; }
static inline Vec2 operator-(const Vec2& a, const Vec2& b) { return {a.x - b.x, a.y - b.y}; }
static inline Vec2 operator*(double s, const Vec2& a) { return {s * a.x, s * a.y}; }

static inline double dot(const Vec2& a, const Vec2& b) { return a.x * b.x + a.y * b.y; }
static inline double norm(const Vec2& a) { return std::sqrt(dot(a, a)); }

static inline Vec2 worldToBody(const Pose2D& pose, const Vec2& p_world) {
  // body frame: x forward, y left
  const double c = std::cos(pose.theta);
  const double s = std::sin(pose.theta);
  const Vec2 d{p_world.x - pose.x, p_world.y - pose.y};
  // R^T * d
  return { c * d.x + s * d.y, -s * d.x + c * d.y };
}

static inline Vec2 closestPointOnSegment(const Vec2& p, const Vec2& a, const Vec2& b, double& t_out) {
  const Vec2 ab = b - a;
  const double denom = dot(ab, ab);
  if (denom <= 1e-12) { t_out = 0.0; return a; }
  const double t = dot(p - a, ab) / denom;
  const double tc = std::max(0.0, std::min(1.0, t));
  t_out = tc;
  return a + tc * ab;
}

static inline void polylineArcLength(const std::vector<Vec2>& path, std::vector<double>& s, std::vector<double>& seg) {
  const std::size_t N = path.size();
  s.assign(N, 0.0);
  seg.assign((N >= 2) ? (N - 1) : 0, 0.0);
  for (std::size_t i = 0; i + 1 < N; ++i) {
    seg[i] = norm(path[i + 1] - path[i]);
    s[i + 1] = s[i] + seg[i];
  }
}

static inline Vec2 pointAtArcLength(const std::vector<Vec2>& path,
                                    const std::vector<double>& s,
                                    const std::vector<double>& seg,
                                    double s_query) {
  if (path.empty()) return {0.0, 0.0};
  if (s_query <= 0.0) return path.front();
  if (s_query >= s.back()) return path.back();

  // find j s.t. s[j] <= s_query < s[j+1]
  std::size_t j = 0;
  while (j + 1 < s.size() && s[j + 1] <= s_query) ++j;
  if (j + 1 >= path.size()) return path.back();

  const double ds = s_query - s[j];
  const double t = ds / std::max(seg[j], 1e-12);
  return (1.0 - t) * path[j] + t * path[j + 1];
}

static inline void closestPointOnPolyline(const Vec2& p, const std::vector<Vec2>& path,
                                          Vec2& q_out, std::size_t& i_out, double& t_out, double& d_out) {
  double best_d = std::numeric_limits<double>::infinity();
  Vec2 best_q = path.front();
  std::size_t best_i = 0;
  double best_t = 0.0;

  for (std::size_t i = 0; i + 1 < path.size(); ++i) {
    double t = 0.0;
    Vec2 q = closestPointOnSegment(p, path[i], path[i + 1], t);
    const double d = norm(p - q);
    if (d < best_d) {
      best_d = d; best_q = q; best_i = i; best_t = t;
    }
  }
  q_out = best_q; i_out = best_i; t_out = best_t; d_out = best_d;
}

static inline double purePursuitCurvature(const Pose2D& pose, const std::vector<Vec2>& path,
                                         double Ld, Vec2& closest_world, Vec2& look_world) {
  Ld = std::max(Ld, 1e-3);
  const Vec2 p{pose.x, pose.y};

  std::vector<double> s, seg;
  polylineArcLength(path, s, seg);

  std::size_t i = 0;
  double t = 0.0, d = 0.0;
  Vec2 q;
  closestPointOnPolyline(p, path, q, i, t, d);
  closest_world = q;

  const double s_closest = s[i] + t * seg[i];
  look_world = pointAtArcLength(path, s, seg, s_closest + Ld);

  const Vec2 look_b = worldToBody(pose, look_world);
  const double L = std::sqrt(look_b.x * look_b.x + look_b.y * look_b.y);
  if (L <= 1e-9) return 0.0;

  // kappa = 2*y_r / L^2
  return 2.0 * look_b.y / (L * L);
}

static inline Pose2D bicycleStep(const Pose2D& pose, double v, double delta, double wheelbase, double dt) {
  Pose2D nxt = pose;
  nxt.x += v * std::cos(pose.theta) * dt;
  nxt.y += v * std::sin(pose.theta) * dt;
  nxt.theta = wrapAngle(pose.theta + (v / wheelbase) * std::tan(delta) * dt);
  return nxt;
}

int main() {
  // Demo path: simple sinusoid
  std::vector<Vec2> path;
  path.reserve(500);
  for (int i = 0; i < 500; ++i) {
    const double x = 0.05 * i * 25.0; // 0..25
    const double y = 1.8 * std::sin(0.22 * x);
    path.push_back({x, y});
  }

  Pose2D pose{-2.0, -2.0, 0.2};
  const double dt = 0.02;
  const double wheelbase = 0.33;
  const double v = 1.5;

  const double Ld_min = 0.8, Ld_max = 3.5, k_v = 0.8;
  const double kappa_max = 1.6;
  const double delta_max = 32.0 * M_PI / 180.0;

  for (int k = 0; k < static_cast<int>(25.0 / dt); ++k) {
    const double Ld = std::min(Ld_max, std::max(Ld_min, Ld_min + k_v * std::fabs(v)));

    Vec2 q, pL;
    double kappa = purePursuitCurvature(pose, path, Ld, q, pL);
    if (kappa >  kappa_max) kappa =  kappa_max;
    if (kappa < -kappa_max) kappa = -kappa_max;

    double delta = std::atan(wheelbase * kappa);
    if (delta >  delta_max) delta =  delta_max;
    if (delta < -delta_max) delta = -delta_max;

    pose = bicycleStep(pose, v, delta, wheelbase, dt);

    if (k % 50 == 0) {
      std::cout << "k=" << k
                << " pose=(" << pose.x << "," << pose.y << "," << pose.theta << ")"
                << " look=(" << pL.x << "," << pL.y << ")"
                << " kappa=" << kappa << " delta=" << delta
                << "\n";
    }
  }

  std::cout << "Done.\n";
  return 0;
}


/*
==================== OPTIONAL ROS2 SKETCH (NOT BUILT BY DEFAULT) ====================

#define USE_ROS2

#ifdef USE_ROS2
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

class PurePursuitNode : public rclcpp::Node {
public:
  PurePursuitNode() : Node("pure_pursuit_node") {
    path_sub_ = create_subscription<nav_msgs::msg::Path>(
      "/plan", 10, [this](const nav_msgs::msg::Path::SharedPtr msg) {
        path_.clear();
        for (const auto& ps : msg->poses) path_.push_back({ps.pose.position.x, ps.pose.position.y});
      });

    cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    timer_ = create_wall_timer(std::chrono::milliseconds(20), [this]() { onTimer(); });
  }

  void setPose(const Pose2D& pose) { pose_ = pose; } // supply from TF/odometry in real system

private:
  void onTimer() {
    if (path_.size() < 2) return;
    Vec2 q, pL;
    const double Ld = 1.5;
    double kappa = purePursuitCurvature(pose_, path_, Ld, q, pL);

    geometry_msgs::msg::Twist cmd;
    cmd.linear.x = 0.7;
    cmd.angular.z = cmd.linear.x * kappa; // unicycle/diff-drive mapping
    cmd_pub_->publish(cmd);
  }

  Pose2D pose_;
  std::vector<Vec2> path_;
  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
};
#endif

=====================================================================================
*/
      

Java is common in some robotics stacks (education, Android-based robots, or FRC-style tooling). The code below is library-free and can be integrated with ROS (rosjava) or any messaging system by wiring pose and path inputs and publishing \(\omega = v\kappa\).

Code file: Chapter15_Lesson1.java

// Chapter15_Lesson1.java
// Pure Pursuit and Geometric Path Tracking (Java 17+)
// Core geometry is library-free; integrate with your robotics stack by wiring pose + path IO.
// (If you use ROS via rosjava, publish Twist commands computed as omega = v * kappa.)

import java.util.ArrayList;
import java.util.List;

final class Vec2 {
    public final double x, y;
    public Vec2(double x, double y) { this.x = x; this.y = y; }
    public Vec2 add(Vec2 b) { return new Vec2(this.x + b.x, this.y + b.y); }
    public Vec2 sub(Vec2 b) { return new Vec2(this.x - b.x, this.y - b.y); }
    public Vec2 mul(double s) { return new Vec2(s * this.x, s * this.y); }
    public double dot(Vec2 b) { return this.x * b.x + this.y * b.y; }
    public double norm() { return Math.hypot(this.x, this.y); }
}

final class Pose2D {
    public final double x, y, theta;
    public Pose2D(double x, double y, double theta) { this.x = x; this.y = y; this.theta = theta; }
}

final class PurePursuit {

    public static double wrapAngle(double a) {
        while (a <= -Math.PI) a += 2.0 * Math.PI;
        while (a >   Math.PI) a -= 2.0 * Math.PI;
        return a;
    }

    public static Vec2 worldToBody(Pose2D pose, Vec2 pWorld) {
        // body frame: x forward, y left
        double c = Math.cos(pose.theta);
        double s = Math.sin(pose.theta);
        Vec2 d = new Vec2(pWorld.x - pose.x, pWorld.y - pose.y);
        return new Vec2(c * d.x + s * d.y, -s * d.x + c * d.y);
    }

    public static double[] polylineArcLength(List<Vec2> path, double[] segOut) {
        double[] s = new double[path.size()];
        s[0] = 0.0;
        for (int i = 0; i < path.size() - 1; i++) {
            Vec2 d = path.get(i + 1).sub(path.get(i));
            segOut[i] = d.norm();
            s[i + 1] = s[i] + segOut[i];
        }
        return s;
    }

    public static Vec2 closestPointOnSegment(Vec2 p, Vec2 a, Vec2 b, double[] tOut) {
        Vec2 ab = b.sub(a);
        double denom = ab.dot(ab);
        if (denom <= 1e-12) { tOut[0] = 0.0; return a; }
        double t = (p.sub(a)).dot(ab) / denom;
        double tc = Math.max(0.0, Math.min(1.0, t));
        tOut[0] = tc;
        return a.add(ab.mul(tc));
    }

    public static class ClosestResult {
        public final Vec2 q; public final int i; public final double t; public final double d;
        public ClosestResult(Vec2 q, int i, double t, double d) { this.q=q; this.i=i; this.t=t; this.d=d; }
    }

    public static ClosestResult closestPointOnPolyline(Vec2 p, List<Vec2> path) {
        double bestD = Double.POSITIVE_INFINITY;
        Vec2 bestQ = path.get(0);
        int bestI = 0;
        double bestT = 0.0;

        for (int i = 0; i < path.size() - 1; i++) {
            double[] tOut = new double[1];
            Vec2 q = closestPointOnSegment(p, path.get(i), path.get(i + 1), tOut);
            double d = p.sub(q).norm();
            if (d < bestD) {
                bestD = d; bestQ = q; bestI = i; bestT = tOut[0];
            }
        }
        return new ClosestResult(bestQ, bestI, bestT, bestD);
    }

    public static Vec2 pointAtArcLength(List<Vec2> path, double[] s, double[] seg, double sQuery) {
        if (sQuery <= 0.0) return path.get(0);
        if (sQuery >= s[s.length - 1]) return path.get(path.size() - 1);

        int j = 0;
        while (j + 1 < s.length && s[j + 1] <= sQuery) j++;
        double ds = sQuery - s[j];
        double t = ds / Math.max(seg[j], 1e-12);
        Vec2 a = path.get(j);
        Vec2 b = path.get(j + 1);
        return a.mul(1.0 - t).add(b.mul(t));
    }

    public static class Output {
        public final double kappa;
        public final Vec2 closest;
        public final Vec2 lookahead;
        public Output(double kappa, Vec2 closest, Vec2 lookahead) { this.kappa=kappa; this.closest=closest; this.lookahead=lookahead; }
    }

    public static Output curvature(Pose2D pose, List<Vec2> path, double Ld) {
        Ld = Math.max(Ld, 1e-3);
        Vec2 p = new Vec2(pose.x, pose.y);

        double[] seg = new double[path.size() - 1];
        double[] s = polylineArcLength(path, seg);

        ClosestResult cr = closestPointOnPolyline(p, path);
        double sClosest = s[cr.i] + cr.t * seg[cr.i];

        Vec2 pLook = pointAtArcLength(path, s, seg, sClosest + Ld);
        Vec2 lookB = worldToBody(pose, pLook);
        double L = Math.hypot(lookB.x, lookB.y);
        double kappa = (L <= 1e-9) ? 0.0 : (2.0 * lookB.y / (L * L));
        return new Output(kappa, cr.q, pLook);
    }

    public static Pose2D bicycleStep(Pose2D pose, double v, double delta, double wheelbase, double dt) {
        double x = pose.x + v * Math.cos(pose.theta) * dt;
        double y = pose.y + v * Math.sin(pose.theta) * dt;
        double th = wrapAngle(pose.theta + (v / wheelbase) * Math.tan(delta) * dt);
        return new Pose2D(x, y, th);
    }
}

public class Chapter15_Lesson1 {
    public static void main(String[] args) {
        // demo path: sinusoid
        List<Vec2> path = new ArrayList<>();
        for (int i = 0; i < 500; i++) {
            double x = 25.0 * i / 499.0;
            double y = 1.8 * Math.sin(0.22 * x);
            path.add(new Vec2(x, y));
        }

        Pose2D pose = new Pose2D(-2.0, -2.0, 0.2);
        double dt = 0.02, wheelbase = 0.33, v = 1.5;
        double LdMin = 0.8, LdMax = 3.5, kV = 0.8;
        double kappaMax = 1.6;
        double deltaMax = Math.toRadians(32.0);

        for (int k = 0; k < (int)(25.0 / dt); k++) {
            double Ld = Math.min(LdMax, Math.max(LdMin, LdMin + kV * Math.abs(v)));
            PurePursuit.Output out = PurePursuit.curvature(pose, path, Ld);

            double kappa = Math.max(-kappaMax, Math.min(kappaMax, out.kappa));
            double delta = Math.atan(wheelbase * kappa);
            delta = Math.max(-deltaMax, Math.min(deltaMax, delta));

            pose = PurePursuit.bicycleStep(pose, v, delta, wheelbase, dt);

            if (k % 50 == 0) {
                System.out.printf("k=%d pose=(%.3f, %.3f, %.3f) kappa=%.4f delta=%.4f%n",
                        k, pose.x, pose.y, pose.theta, kappa, delta);
            }
        }
        System.out.println("Done.");
    }
}
      

The MATLAB file is a single script with local functions, suitable for: (i) direct MATLAB simulation, and (ii) reuse of the purePursuitCurvature function inside a MATLAB Function block in Simulink (inputs: pose, path, Ld; output: kappa; then map to omega or delta).

Code file: Chapter15_Lesson1.m

% Chapter15_Lesson1.m
% Pure Pursuit and Geometric Path Tracking (MATLAB)
%
% This file contains:
%   1) A demo script (top-level) that simulates a kinematic bicycle model
%   2) Local functions for pure pursuit geometry
%
% Run:
%   Chapter15_Lesson1

clear; clc;

% Path: gentle sinusoid
xs = linspace(0, 25, 500)';
ys = 1.8 * sin(0.22 * xs);
path = [xs, ys];

pose = [-2.0; -2.0; 0.2]; % [x;y;theta]
dt = 0.02;
wheelbase = 0.33;
v = 1.5;

Ld_min = 0.8; Ld_max = 3.5; k_v = 0.8;
kappa_max = 1.6;
delta_max = deg2rad(32);

traj = zeros(round(25/dt), 2);
lookPts = zeros(round(25/dt), 2);

for k = 1:size(traj,1)
    Ld = min(Ld_max, max(Ld_min, Ld_min + k_v * abs(v)));
    [kappa, pClosest, pLook] = purePursuitCurvature(pose, path, Ld);
    kappa = min(kappa_max, max(-kappa_max, kappa));
    delta = atan(wheelbase * kappa);
    delta = min(delta_max, max(-delta_max, delta));

    pose = bicycleStep(pose, v, delta, wheelbase, dt);
    traj(k,:) = pose(1:2)';
    lookPts(k,:) = pLook';
end

figure;
plot(path(:,1), path(:,2)); hold on;
plot(traj(:,1), traj(:,2));
plot(lookPts(1:60:end,1), lookPts(1:60:end,2), '.');
axis equal; grid on;
xlabel('x [m]'); ylabel('y [m]');
title('Pure Pursuit (kinematic bicycle)');
legend('path','robot','lookahead (every 60)');

% -------------------------- Local functions --------------------------

function a = wrapAngle(a)
    a = mod(a + pi, 2*pi) - pi;
end

function R = rot2(th)
    R = [cos(th) -sin(th); sin(th) cos(th)];
end

function pBody = worldToBody(pose, pWorld)
    th = pose(3);
    R = rot2(th)';
    t = pose(1:2);
    pBody = R * (pWorld - t);
end

function [s, seg] = polylineArcLength(path)
    d = diff(path,1,1);
    seg = sqrt(sum(d.^2,2));
    s = [0; cumsum(seg)];
end

function [q, t] = closestPointOnSegment(p, a, b)
    ab = b - a;
    denom = ab' * ab;
    if denom <= 1e-12
        q = a; t = 0;
        return;
    end
    t0 = ((p - a)' * ab) / denom;
    t = min(1, max(0, t0));
    q = a + t * ab;
end

function [qBest, iBest, tBest, dBest] = closestPointOnPolyline(p, path)
    dBest = inf;
    qBest = path(1,:)'; iBest = 1; tBest = 0;
    for i = 1:size(path,1)-1
        [q, t] = closestPointOnSegment(p, path(i,:)', path(i+1,:)');
        d = norm(p - q);
        if d < dBest
            dBest = d; qBest = q; iBest = i; tBest = t;
        end
    end
end

function p = pointAtArcLength(path, s, seg, sQuery)
    if sQuery <= 0
        p = path(1,:)'; return;
    end
    if sQuery >= s(end)
        p = path(end,:)'; return;
    end
    j = find(s <= sQuery, 1, 'last');
    ds = sQuery - s(j);
    t = ds / max(seg(j), 1e-12);
    p = (1-t) * path(j,:)' + t * path(j+1,:)';
end

function [kappa, pClosest, pLook] = purePursuitCurvature(pose, path, Ld)
    Ld = max(Ld, 1e-3);
    p = pose(1:2);
    [s, seg] = polylineArcLength(path);
    [q, i, t, ~] = closestPointOnPolyline(p, path);
    sClosest = s(i) + t * seg(i);
    pLook = pointAtArcLength(path, s, seg, sClosest + Ld);

    lookB = worldToBody(pose, pLook);
    L = norm(lookB);
    if L <= 1e-9
        kappa = 0.0;
    else
        kappa = 2.0 * lookB(2) / (L * L);
    end
    pClosest = q;
end

function poseN = bicycleStep(pose, v, delta, wheelbase, dt)
    x = pose(1) + v * cos(pose(3)) * dt;
    y = pose(2) + v * sin(pose(3)) * dt;
    th = wrapAngle(pose(3) + (v / wheelbase) * tan(delta) * dt);
    poseN = [x; y; th];
end
      

This script symbolically derives \(\kappa\) and the straight-line linearized error dynamics, then runs a lightweight numerical demo (with a coarse discretized lookahead) to visualize tracking.

For Mathematica code blocks, we use .

Code file: Chapter15_Lesson1.nb

(* Chapter15_Lesson1.nb
   Pure Pursuit and Geometric Path Tracking (Wolfram Language)

   Note: This is a plain-text Wolfram Language script saved with .nb extension for convenience.
   You can paste it into a Mathematica notebook and evaluate.
*)

ClearAll["Global`*"];

(* Geometry: derive curvature kappa for a circle passing through origin and a lookahead point (xL,yL),
   with tangent aligned with +x at the origin. The circle center is (0,R) so that tangent is horizontal.
   Then (xL)^2 + (yL - R)^2 == R^2  =>  xL^2 + yL^2 - 2 R yL == 0  =>  R == (xL^2 + yL^2)/(2 yL).
   Curvature kappa == 1/R == 2 yL/(xL^2 + yL^2).
*)

xL =.; yL =.;
kappaExpr = FullSimplify[2 yL/(xL^2 + yL^2)];
Print["kappa(xL,yL) = ", kappaExpr];

(* Linearized straight-line error dynamics (small errors):
   e_y' = v e_theta
   e_theta' = - v kappa = - v * (2 (e_y + Ld e_theta)/Ld^2)  (approx yL ~ e_y + Ld e_theta)
   => e_y'' + (2 v/Ld) e_y' + (2 v^2/Ld^2) e_y = 0.
*)
v = Symbol["v", Positive -> True];
Ld = Symbol["Ld", Positive -> True];
ey[t_] := ey[t];
etheta[t_] := etheta[t];

(* characteristic polynomial for e_y: s^2 + (2 v/Ld) s + 2 (v^2/Ld^2) *)
poly[s_] := s^2 + (2 v/Ld) s + 2 (v^2/Ld^2);
Print["Characteristic polynomial: ", poly[s]];
Print["Roots: ", Solve[poly[s] == 0, s] // FullSimplify];

(* Simple numerical simulation of kinematic bicycle with pure pursuit on a sinusoid path *)
wrap[ang_] := Mod[ang + Pi, 2 Pi] - Pi;

path = Table[{x, 1.8 Sin[0.22 x]}, {x, 0, 25, 25/499.}];
(* helper: nearest index by Euclidean distance (coarse, for demo) *)
nearestIndex[p_] := Ordering[Norm /@ (path - p), 1][[1]];

pointAtIndex[i_] := path[[Min[Max[i, 1], Length[path]]]];

(* lookahead by index advance (coarse: converts Ld to a fixed index step) *)
lookaheadPoint[p_, LdMeters_] := Module[{i = nearestIndex[p], step},
  step = Round[20 LdMeters]; (* heuristic: depends on discretization *)
  pointAtIndex[i + step]
];

(* body-frame transform *)
worldToBody[{x_, y_, th_}, {px_, py_}] := Module[{c = Cos[th], s = Sin[th], dx, dy},
  dx = px - x; dy = py - y;
  {c dx + s dy, -s dx + c dy}
];

purePursuitKappa[state_, LdMeters_] := Module[{p = state[[1 ;; 2]], lookW, lookB, xR, yR, L},
  lookW = lookaheadPoint[p, LdMeters];
  lookB = worldToBody[state, lookW];
  xR = lookB[[1]]; yR = lookB[[2]];
  L = Sqrt[xR^2 + yR^2];
  If[L < 10^-9, 0, 2 yR/(L^2)]
];

bicycleStep[state_, v_, wheelbase_, delta_, dt_] := Module[{x = state[[1]], y = state[[2]], th = state[[3]]},
  {
    x + v Cos[th] dt,
    y + v Sin[th] dt,
    wrap[th + (v/wheelbase) Tan[delta] dt]
  }
];

dt = 0.02; wheelbase = 0.33; v0 = 1.5;
Ld0 = 1.2;
kappaMax = 1.6;
deltaMax = 32 Degree;

state = {-2.0, -2.0, 0.2};
traj = Reap[
  Do[
    kappa = Clip[purePursuitKappa[state, Ld0], {-kappaMax, kappaMax}];
    delta = Clip[ArcTan[wheelbase kappa], {-deltaMax, deltaMax}];
    state = bicycleStep[state, v0, wheelbase, delta, dt];
    Sow[state[[1 ;; 2]]];
  , {Round[25/dt]}]
][[2, 1]];

ListLinePlot[{path, traj}, AspectRatio -> Automatic, PlotLegends -> {"path", "robot"}]
      

Problem 1 (Curvature Derivation): Starting from a circle centered at \((0,R)\) in the robot frame, show that the curvature command for a lookahead point \((x_r,y_r)\) is \(\kappa = 2y_r/(x_r^2+y_r^2)\). State clearly what geometric condition enforces the tangent at the origin.

Solution: The tangent at the origin is aligned with the +x axis if the circle center lies on the y-axis (no x-offset), so the radius at the origin is vertical. Using \(x^2+(y-R)^2=R^2\) and substituting \((x_r,y_r)\) yields \(R=(x_r^2+y_r^2)/(2y_r)\) and hence \(\kappa=1/R=2y_r/(x_r^2+y_r^2)\).

Problem 2 (Mapping to Commands): For a unicycle, prove that \(\omega=v\kappa\). For a bicycle model with wheelbase \(L\), derive \(\delta=\arctan(L\kappa)\).

Solution: A planar curve parameterized by arclength has curvature \(\kappa = d\theta/ds\). With forward speed \(v=ds/dt\), we get \(\omega=\dot\theta=(d\theta/ds)(ds/dt)=\kappa v\). For the kinematic bicycle, \(\dot\theta=(v/L)\tan\delta\), so \(\kappa=\dot\theta/v=\tan\delta/L\) and thus \(\delta=\arctan(L\kappa)\).

Problem 3 (Linearized Stability): Using the small-error approximation \(y_r \approx e_y + L_d e_\theta\), show that the lateral error satisfies \(\ddot e_y + (2v/L_d)\dot e_y + 2(v^2/L_d^2)e_y = 0\) for straight-line tracking. Conclude exponential stability for \(v > 0\), \(L_d > 0\).

Solution: From \(\dot e_y \approx v e_\theta\) and \(\dot e_\theta \approx -(2v/L_d^2)(e_y + L_d e_\theta)\), differentiate the first: \(\ddot e_y \approx v\dot e_\theta\), substitute \(\dot e_\theta\), and eliminate \(e_\theta = \dot e_y/v\) to obtain the stated ODE. The characteristic polynomial has positive coefficients, so its roots have negative real parts.

Problem 4 (Lookahead Tuning): Assume fixed speed \(v\) and straight-line tracking. Using the ODE in Problem 3, identify the natural frequency and damping ratio as functions of \(L_d\). Discuss how increasing \(L_d\) changes (i) convergence speed and (ii) smoothness.

Solution: Write the ODE as \(\ddot e_y + 2\zeta\omega_n \dot e_y + \omega_n^2 e_y = 0\). Matching coefficients gives \(\omega_n = \sqrt{2}\,v/L_d\) and \(\zeta = 1/\sqrt{2}\) (constant). Increasing \(L_d\) decreases \(\omega_n\), so convergence is slower but oscillations are reduced and commands are smoother.

Problem 5 (Circle-Intersection Lookahead): Design an algorithm to compute the lookahead point by intersecting the circle of radius \(L_d\) centered at the robot position with each polyline segment ahead of the current progress index. If two intersections exist on a segment, which one should be chosen?

Solution (algorithm sketch):

flowchart TD
  ST["Start with segment index i = progress"] --> SEG["For segment [w_i, w_i+1]"]
  SEG --> INT["Solve circle-segment intersection (0/1/2 points)"]
  INT -->|0| NEXT["i <- i + 1 \n(next segment)"]
  NEXT --> SEG
  INT -->|1| RET1["Return that point \nas lookahead"]
  INT -->|2| PICK["Choose 'forward' point \n(larger projection \nalong segment)"]
  PICK --> RET2["Return chosen point \nas lookahead"]
  INT -->|none found| END["If end reached: \nuse final waypoint"]
        

If two intersections lie on the same segment, choose the one that is farther along the segment direction (larger projection onto the segment vector), because it corresponds to a point “ahead” on the path. See Chapter15_Lesson1_Ex1.py for a concrete implementation.

Pure pursuit converts a discrete path into a curvature command by (i) selecting a lookahead point and (ii) fitting a circle arc from the robot to that point in the robot frame. We derived the closed-form curvature \(\kappa = 2y_r/(x_r^2+y_r^2)\), showed how to map it to unicycle and Ackermann command interfaces, and proved local exponential stability for straight-line tracking under small-error linearization. The provided implementations illustrate two common lookahead selection strategies (arclength and circle intersection) and show how to incorporate saturation and speed-dependent lookahead.