Nonholonomic Systems II — Steering Cars, Trailers, and Underactuated Systems

Chapter 20 proved Hitch can reach every pose; this chapter is the catalog of ways to actually get there — Brockett's sinusoids and chained forms, gradient steering, differential flatness, Dubins and Reeds–Shepp words, Choset's CAR GRID SEARCH and its descendants (state lattices, hybrid A*), forward-propagation trees and Kinodynamic RRT*, and Reach's dead third joint driven like a car along decoupling vector fields.

The problem: same destination, six fares

Hitch is at the south curb of the Lot, heading east. Bay 3 is up and to the left. Three copies of Hitch leave at once.

The three paths are all correct, and all different, because each one answers a different question. The Reeds–Shepp path is the shortest path for a car that may reverse; it has a cusp because stopping and backing up is free in its cost. The flat-output path is the simplest polynomial consistent with the two poses; it never stops, but nothing in its derivation stops it from asking for more steering than Hitch has. The tree path is the first thing a randomized search found by simulating Hitch; it is jittery and does not end exactly on the goal, but it is the only one of the three that looked at the pillars while planning. Chapter 20's controllability theorem guaranteed all three exist. It said nothing about which one you want, and the rest of this chapter is about making that choice deliberately.

Building intuition

One query, several optimality criteria

Start with the flagship. Hitch now pulls its trailer, and the scoreboard grows a column the planners were never asked about: how far the trailer swings.

Three observations, each a section below.

The trailer is a hidden cost. With the trailer on, the chained-form sinusoids reach the bay exactly — in the car's chained coordinates — while the trailer, simply towed along, swings far past $|\theta_1| = \pi/2$. The sinusoid method steers the four-state car; it does not know the trailer exists. The flat-output method with the trailer is different in kind: its flat output is the trailer's axle, so the trailer is parked by construction and the car's motion is derived from it.

Exactness and obstacle-awareness trade off. The Reeds–Shepp word and the sinusoids land on the goal pose to round-off and do not look at the pillars. Turn "obstacles" on and the third method becomes Algorithm 22, which looks at everything and stops anywhere within $\pm 0.5$ m and $\pm 0.2$ rad of the goal. Exact steering is an obstacle-free subroutine; search is how obstacles enter.

Control bounds are where each method is weakest. The flat planner's red $\max|\phi|$ is the curvature its polynomial asks for, not what Hitch can do. The sinusoids ask for $1.22$ rad of steering on the default query — twice Hitch's limit. Only the methods built from full-lock arcs (Reeds–Shepp, Algorithm 22) respect $|\phi| \le 0.6$ by construction.

Shortest paths have words

The shortest path between two poses of a car with a minimum turning radius $\rho$ is a short word in three letters: $L$ and $R$ for full-lock arcs, $S$ for straight. Forward-only (Dubins), six words suffice; allowed to reverse (Reeds–Shepp), a handful of families with cusps join them.

The tint is the synthesis: for the goal heading on the slider, every point of the plane is colored by the family of its shortest path. Drag the goal along a boundary and the word flips — but the length in the readout moves continuously, because the optimal length is a continuous function of the goal even though the optimal word is not. Toggle "Dubins only" and compare: near the start, behind it and beside it, the forward-only car pays enormously (a loop of nearly $2\pi\rho$) for what the Reeds–Shepp car does with one cusp. The Reeds–Shepp length is never longer — the chapter's check confirms it on 400 random pairs — and reversing is not a Dubins word run backward: the reversing words are new words.

State from a curve

Here is a planner with no differential equation in it.

Drag the curve. Hitch is placed on it by differentiation alone: its heading is the curve's tangent, its steering angle is $\arctan$ of the wheelbase times the curve's curvature. Nothing was integrated. That is differential flatness: the car has two outputs — the rear-axle midpoint — from which state and inputs are algebraic functions of finitely many derivatives. Turn on the trailer and the flat output moves to the trailer's axle; now the car's position is a derivative away (the trailer tangent tells you where the hitch is) and its steering is two more. Make a corner tight and part of the curve turns red: the curve is still a perfectly good flat output, but it asks for more curvature than $1/\rho$, and flatness alone has no way to say no.

A tree without a steering function

Every planner so far needed to invert the car — find controls that go from here to there. Choset's §7.5.1 points out that a tree planner needs much less: a way to propagate a state forward under a control.

With one sampled control per extension the tree drifts; with eight it reaches. Push the heading weight $w_\theta$ up and the tree spends its effort matching headings of random samples instead of covering the Lot — the planner is only as good as its notion of "near", and the metric is a tuning knob, not a derived quantity. Turn on Kinodynamic RRT* and the tree changes model: rewiring compares the cost of reaching a node through two different parents, which needs the exact optimal cost between two states. For the kinematic car that is the Reeds–Shepp length; for Hitch's dynamic model nobody has a closed form, which is why the toggle switches systems rather than pretending.

Grid search, then and now

Barraquand and Latombe's 1990s answer to car planning among obstacles was a best-first search over six motion primitives — full left, straight, full right, forward and reverse — with an occupancy grid on $(x, y, \theta)$ to stop it revisiting. Today's planners in autonomous vehicles are recognizably the same algorithm.

The left panel is a state lattice: the primitives were built so that every motion ends exactly on a lattice state, so the search is plain A* on a graph. The right panel is hybrid A*: the states float freely as in Algorithm 22, each grid cell keeps the best one, and every few expansions the planner tries an analytic Reeds–Shepp "shot" to the goal (the dashed purple fans are shots that hit something). Coarsen the grid: the lattice, whose primitives must land on coarse lattice points, takes long detours; hybrid A* barely notices because its states are continuous. Neither is new; both are Algorithm 22 plus a named change.

Driving a dead joint

The last widget leaves the Lot. Reach's three-link arm lies flat on the Workbench with its third motor switched off.

The free third link cannot be torqued. Chapter 20 showed the arm is nevertheless accessible from rest, and Choset's §12.5.7 shows how to use that: there are two motions the dead link can perform at any speed — sliding along its own length, and rotating about its center of percussion, the one point on the link that a force at the joint perpendicular to the link leaves momentarily unaccelerated. Drive with those two (forward and reverse each) and the link behaves like a car with four actions. The planner is Algorithm 22 with cost "number of switches"; the execution is Chapter 18's time-optimal scaling, segment by segment, coming to rest at every switch. In the inset one torque bar is always full.

The mathematics

Throughout, the steering problem is: given $\htmlClass{term-start}{x_{start}}$ and $\htmlClass{term-goal}{x_{goal}}$, find controls driving the system from one to the other, ignoring obstacles. It is exactly what PRM and RRT call a local planner. Choset splits the state of many systems into shape variables, driven directly by the inputs, and fiber variables, which change only when the shape variables move around closed loops — the astronaut who reorients by cycling her arms. Every exact method below is a way to engineer the right loop.

Brockett's sinusoids

The simplest system with a fiber is Choset's (12.23):

with $g_1 = [1, 0, x_2]^\top$, $g_2 = [0, 1, 0]^\top$ and $[g_1, g_2] = -e_3$. Shape $(x_1, x_2)$, fiber $x_3$.

So to move the fiber by $0.3$ choose $k = -1$ and any $(u_1(0), u_2(0))$ on the circle of radius $\sqrt{4\pi \cdot 0.3} = 1.9416$, at cost $J = 0.6\pi \approx 1.885$; the library's steerBrockett returns that circle's point on the $u_1$-axis, and integrating (12.23) under it lands on $(0, 0, 0.3)$ to round-off. The unicycle is this system in disguise: with Choset's

the unicycle's $(\dot x_1, \dot x_2, \dot x_3) = (v\cos x_3, v\sin x_3, \omega)$ becomes $\dot z = (v_1, v_2, z_2 v_1)$ — check it: $\dot z_3 = v(\cos x_3\sin x_3 - \sin x_3\cos x_3) + (x_1\cos x_3 + x_2\sin x_3)\omega = z_2 v_1$. The heading is a shape variable, the position along the heading is the other, and the sideways offset is the fiber. Brockett's cost is not a physical one for a car, which is Choset's own caveat.

Chained form and the sinusoid ladder

Generalize to $n$ states and two inputs (Choset eq. 12.28):

Hitch's four-state car $q = (x, y, \phi, \theta)$ has a chained form too (Murray and Sastry):

where the primes are coordinates in a chart rotated by an angle $\beta$. In the original chart the transformation is singular at $\theta = \pm\pi/2$ — which is exactly where a parking query into a north-facing bay ends. The library sets $\beta$ to the bisector of the start and goal headings, so both ends sit inside $(-\pi/2, \pi/2)$; inside the motion $\theta' = \arctan z_3$ never reaches $\pm\pi/2$ by construction. On w21.1's default query, $(5.5, 1.2, 0) \to (9.25, 6.4, \pi/2)$, two sinusoid cycles land on the goal in chained coordinates to better than $10^{-9}$ — with three cusps and a peak steering angle of $1.22$ rad, twice what Hitch can do. The method is exact in its coordinates and blind to everything else. Car-and-trailer systems with any number of trailers also have chained forms (Sørdalen's result, Choset ref. 393), at the price of coordinate changes that blow up at the jackknife.

Gradient steering, and the cross-link to Chapter 19

Exact methods need structure. Choset's §12.5.4 needs only a simulator: parameterize the control history by a finite vector $u^p \in \mathbb{R}^r$ (here, piecewise-constant $(v, \omega)$ on $K = 6$ equal intervals of $[0, 1]$ for the reduced car), call $f(x_{start}, u^p)$ the end-state map, and descend the error $e = f(x_{start}, u^p) - x_{goal}$.

For a pure sideways move of $0.5$ m from rest the library's steerGradient finds rank $2$ at $u^p = 0$, appends a two-piece generic loop (rank $3$), and then needs $58$ backtracking gradient steps to bring $\|e\|$ below $10^{-4}$ — slow, because the sideways direction is a bracket direction and the Jacobian's singular value along it is small; Chapter 20's $\epsilon^2$ reappears as conditioning. Hand the same end-state map to Chapter 19's Levenberg–Marquardt — §12.5.3's "nonlinear optimization", with no re-derivation needed — and five iterations reach round-off. Choset's two caveats carry over unchanged: a good initial guess is required, and nothing guarantees convergence; here the generic loop is the initial guess that makes it work.

Differential flatness

Choset's Example 12.5.1 fits $y_1 = t$, $y_2 = 3t^2 - 2t^3$ between $(0, 0, 0)$ and $(1, 1, 0)$; the library's Hermite fit with unit end speed returns exactly those polynomials. At $t = 0.25$: $y = (0.25, 0.15625)$, $\dot y = (1, 1.125)$, $\ddot y = (0, 3)$, so

$\kappa = \omega/v = 0.879709$, and for wheelbase $L = 1$ the steering angle is $\phi = \arctan 0.879709 = 0.721491$ rad ($41.34°$). At $t = 0.5$ the curve inflects and $\omega = \phi = 0$ exactly. The chapter's check goes one step further: it takes the controls $(v(t), \phi(t))$ read off a quintic flat output, integrates the true car (and car-with-trailer) ODE forward from the lifted initial state, and finds the integrated state agreeing with the lifted one along the whole trajectory to better than $10^{-11}$. Flatness is not an approximation.

What it is not, either, is a planner with constraints. A polynomial of minimal degree has no freedom left to respect $|\phi| \le \gamma$ or avoid a pillar; extra coefficients plus a numerical optimizer can, and Choset calls that "a topic of current research". On w21.1's trailer query the trailer-axle quintic asks for $|\phi| = 1.39$ rad and clips an obstacle.

Shortest paths: Dubins and Reeds–Shepp

The micro-example. $q_0 = (0, 0, 0)$, $q_1 = (3, 3, \pi/2)$, $\rho = 1$. The start's left circle is centered at $(0, 1)$, the goal's at $(3, 3) + (-\sin\tfrac\pi2, \cos\tfrac\pi2) = (2, 3)$; the centers are $2\sqrt2$ apart along direction $\pi/4$, so $LSL$ has $t = \pi/4$, $p = 2\sqrt2$, $q = \pi/4$ and length $\pi/2 + 2\sqrt2 = 4.39922$. By integration: after the first arc Hitch is at $(0.70711, 0.29289)$ heading $\pi/4$; the straight adds $(2, 2)$; the last arc about $(2, 3)$ ends at $(3, 3)$ heading $\pi/2$. The others are longer — $LSR = RSL = 10.56698$, $LRL = 10.99557$, $RSR = 16.65243$ — and $RLR$ is infeasible. Reeds–Shepp returns the same $L^+S^+L^+$: here a cusp cannot help.

Reeds–Shepp by symmetry. Nobody codes forty-eight formulas. Reeds and Shepp's §8 gives a handful of base formulas — $L^+S^+L^+$, $L^+S^+R^+$, $L^+R^-L$, $L^+R^+_aL^-_aR^-$, $L^+R^-_aL^-_aR^+$, $L^+R^-_{\pi/2}S^-L^-$, $L^+R^-_{\pi/2}S^-R^-$, $L^+R^-_{\pi/2}S^-L^-_{\pi/2}R^+$ — and three symmetries of the normalized goal $(x, y, \varphi)$ generate the rest:

Time-flip negates every segment ($L^+ \leftrightarrow L^-$), reflect swaps $L$ and $R$, backwards reads the word right to left. The library evaluates eleven base formulas (four of them backwards readings) under the four flip/reflect combinations — forty-four evaluations, the construction OMPL's ReedsSheppStateSpace uses. Several base formulas leave the sign of their last segment free, so the forty-four evaluations cover the forty-eight words; the module does not prove that itself, and the check tests the two consequences that would fail if it were false. On 400 seeded pairs, every one of the 2,700 candidate words integrates to its goal to round-off (worst error $10^{-14}$), and the shortest Reeds–Shepp length never exceeds the shortest Dubins length (Dubins paths are Reeds–Shepp candidates, so a missing family or a mistyped formula would show up as a violation). All nine families appear among the optimal words. The Reeds–Shepp length is also symmetric — $d(q_0, q_1) = d(q_1, q_0)$ — because reversing time maps a path to a path; that is what makes it a metric on $SE(2)$, and the reason PRM can use it as an undirected local planner while a Dubins-steered PRM needs directed edges.

Local planners with the topological property

The library implements both: ReedsSheppLocal and DubinsLocal satisfy Chapter 11's Steer<Pose2> interface (symmetric and not, respectively), and pathTransform is Laumond's bisection. Their price is the next honesty item: the curvature jumps at every arc–straight junction, so either the steering wheel turns instantaneously, the car stops at each junction, or execution has error. Smoothed primitives — clothoids, or flat-output polynomials — are the standard repair.

CAR GRID SEARCH

Choset's description includes two practicalities the implementation keeps. The occupancy grid is checked when a node is popped (line 7), so a cell can be reached several times but expanded once; and with a trailer the grid gains a fourth axis for the hitch angle, the trailer's change over one action being "numerically integrated, or stored in a lookup table" — here by Chapter 2's Hitch.step, whose RK4 trailer integration is the lookup table computed on demand. On the Lot's reverse-parking query the search with $a = 1, c = 0$ and the search with $c = 300$ both find a 20-step path with one cusp; the second expands half as many nodes, because the cusp penalty prunes every branch that reverses early. Choset's remark that the planner "actually runs faster in cluttered spaces because the obstacles prune the search tree" is the same effect from the other side.

Forward propagation

Two honesty items follow from the proof. The constant $p$ depends on the metric through step 3 — a metric that ranks a node with the wrong heading as "near" wastes extensions, which is the w21.5 slider — and nothing in the proof bounds path quality. On the Lot query from w21.1, Hitch's dynamic model (acceleration and steering-rate inputs, speed and steering angle in the state) with best-of-eight controls and $\Delta t = 0.8$ s first lands in the goal region at extension 106 for seed 21, and re-running the simulator on the stored controls reproduces every state bit for bit. Time can be added to the state to handle moving obstacles (Choset ref. 195); the theorem does not change.

Kinodynamic RRT*: optimality needs a steer

Chapter 13's RRT* is asymptotically optimal because its rewiring step asks, for a node $v$ near a new node $x$, whether reaching $v$ through $x$ is cheaper — a question about the exact optimal cost $c^*(x, v)$ and the trajectory achieving it. Forward propagation cannot answer it. Webb and van den Berg's Kinodynamic RRT* answers it for linear dynamics $\dot x = Ax + Bu$ with cost $\int_0^\tau (1 + u^\top R u)\,dt$: for a fixed arrival time $\tau$ the minimum-energy cost is

and the free final time is $\tau^* = \arg\min_\tau c(\tau)$. For the planar double integrator with $R = rI$ each axis gives $G(\tau) = \tfrac1r\left[\begin{smallmatrix}\tau^3/3 & \tau^2/2\\ \tau^2/2 & \tau\end{smallmatrix}\right]$ and $c(\tau) = \tau + r\sum(12\Delta p^2/\tau^3 - 12\Delta p\,\Delta v/\tau^2 + 4\Delta v^2/\tau)$ with $\Delta p = p_1 - p_0 - v_0\tau$, $\Delta v = v_1 - v_0$. From $(0, 0, 1, 0)$ to $(2, 1, 0, 0.5)$ with $r = 0.5$ the library finds $\tau^* = 2.3893$ s at cost $3.0075$, and integrating the closed-form optimal control $u(t) = R^{-1}B^\top e^{A^\top(\tau - t)}G(\tau)^{-1}d$ lands on the target to round-off. For the reduced kinematic car the exact steer is Reeds–Shepp, which is what w21.5's toggle uses; Hitch's dynamic model has neither, and Exercise 6 asks you to linearize it about rest and use the LQR edge anyway.

Kinematic reductions and decoupling vector fields

Chapter 20 left Reach's dead-motor arm with an uncomfortable verdict: accessible from rest, with Lewis–Murray unable to certify STLC. §12.4.2 changes the question from "can it get anywhere?" to "can I plan it like a kinematic system?".

For the planar body with thrusters Choset finds $V_1 = Y_1$ (translation along the thrust line) and $V_2 = Y_2$ (rotation about the center of percussion at distance $1/d$), with $\nabla_{V_1}V_1 = 0$ and $\nabla_{V_2}V_2 \in \mathrm{span}(Y_1)$; but $\langle Y_1 : Y_2\rangle \notin \mathrm{span}(\mathcal{Y})$, so $V_1 + V_2$ is not decoupling (Example 12.4.8). For the 3R arm with $u_3 = 0$ the free third link is that body, its "thrusters" the forces joints 1 and 2 apply at joint 3. The two decoupling fields are translation along link 3 and rotation about its center of percussion with respect to joint 3, at distance

for Choset's Table 12.1 robot. In joint coordinates both are built through the inverse of the $2\times2$ Jacobian of joint 3's position. The check evaluates Theorem 12.4.7 numerically with Chapter 20's kinetic-energy connection at three configurations: both fields pass with residuals below $5 \times 10^{-10}$; rotation about the center of mass fails (it is not even in span($\mathcal{Y}$) — a rotation about the COM needs a torque the dead joint cannot supply); and the sum of the two fails the second condition, exactly as for the free body.

Modern descendants: lattices and hybrid A*

Choset's chapter ends in 2005 with Algorithm 22 and PRM-with-Reeds–Shepp. The planners that parked the DARPA Urban Challenge vehicles, and their successors, are those two ideas recombined, with three deltas:

1. Lattice-consistent primitives (state lattices, Pivtoraiko, Knepper and Kelly). Choose primitives that start and end exactly on lattice states — grid points times a set of headings — so the search graph is a real graph and A* runs unchanged. The library builds them with this chapter's Dubins solver: for each start heading and each category (straight, ±45°, ±90°, lane change), the cheapest Dubins path to a lattice endpoint whose total turning equals the heading change. The cost of exactness is that the start and goal must be lattice states.
2. One continuous state per cell (hybrid A*, Dolgov, Thrun, Montemerlo and Diebel). Keep Algorithm 22's free-running continuous configurations and its occupancy grid, but let each cell hold the cheapest state that reached it rather than the first.
3. Analytic expansions and a dual heuristic (hybrid A*). Every $N$ expansions try a Reeds–Shepp shot from the popped state straight to the goal and accept it if it is free — the PRM local planner, inside the search. Order OPEN by $\max(\text{Reeds–Shepp length ignoring obstacles},\ \text{2-D shortest distance ignoring kinematics})$: each term is a lower bound on the true cost, so their maximum is too. (The library's 2-D table is scaled by $1/1.0824$, the worst octile-to-Euclidean ratio, to keep it admissible.)

On the Lot, reversing into bay 3 from $(2, 1, 0)$ to $(9.5, 8.5, -\pi/2)$ at $0.5$ m resolution with the dual heuristic: the six-primitive state lattice expands 298 states and returns a $24.368$ m path with three cusps; hybrid A* with a shot every ten expansions expands 161, fires seventeen shots, and returns $17.107$ m with three cusps, ending exactly on the goal. Both are at least the obstacle-blind Reeds–Shepp distance, $15.189$ m. Hybrid A* is not optimal (the cell pruning discards states that might have led to cheaper paths) and the lattice is optimal only over its primitives; w21.3 lets you watch both trade-offs.

The algorithm

Implementation in Rust

The nonholonomic crate of Chapter 20 gains a steer/ directory, car_grid.rs, lattice/ and reduction.rs; forward propagation gets its own crate, kinodynamic, which owns the Propagate trait Chapter 14's preview borrowed. The TypeScript port in web/lib/nonholonomic/ — dubins.ts, reeds-shepp.ts, chained.ts, gradient.ts, flat.ts, car-grid.ts, lattice.ts, kinodynamic.ts, reduction.ts, steer.ts, park.ts — is the code behind every widget on this page, and its __checks_ch21__.ts pins every number in the prose.

use manifold::SE2;

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Word { LSL, RSR, LSR, RSL, RLR, LRL }

/// Dubins shortest path for the reduced car (Choset eq. 12.35). Segment lengths are
/// normalized by ρ — arcs in radians, the straight in units of ρ — so the closed forms
/// are scale-free; every word returned is checked by integration in the tests.
#[derive(Clone, Copy, Debug)]
pub struct DubinsPath { pub word: Word, pub segs: [f64; 3], pub rho: f64 }

impl DubinsPath {
    pub fn len(&self) -> f64 { self.rho * self.segs.iter().sum::<f64>() }
}

fn mod2pi(a: f64) -> f64 { a.rem_euclid(std::f64::consts::TAU) }

/// Start at the origin, goal at (d, 0): headings measured from the bearing of the goal.
fn normalize(q0: &SE2, q1: &SE2, rho: f64) -> (f64, f64, f64) {
    let (dx, dy) = (q1.x - q0.x, q1.y - q0.y);
    let bearing = dy.atan2(dx);
    (dx.hypot(dy) / rho, mod2pi(q0.theta - bearing), mod2pi(q1.theta - bearing))
}

/// LSL: the two left circles and their external tangent (Derivation 5, step 5).
fn lsl(d: f64, a: f64, b: f64) -> Option<[f64; 3]> {
    let p2 = 2.0 + d * d - 2.0 * (a - b).cos() + 2.0 * d * (a.sin() - b.sin());
    if p2 < 0.0 { return None; }
    let th = (b.cos() - a.cos()).atan2(d + a.sin() - b.sin());
    Some([mod2pi(-a + th), p2.sqrt(), mod2pi(b - th)])
}

pub fn dubins_shortest_path(q0: &SE2, q1: &SE2, rho: f64) -> Option<DubinsPath> {
    let (d, a, b) = normalize(q0, q1, rho);
    // RSR, LSR, RSL, RLR, LRL follow from LSL by reflection (L <-> R, headings negated)
    // and by reading the word backward; each has its own feasibility test.
    [(Word::LSL, lsl(d, a, b)), (Word::RSR, rsr(d, a, b)), (Word::LSR, lsr(d, a, b)),
     (Word::RSL, rsl(d, a, b)), (Word::RLR, rlr(d, a, b)), (Word::LRL, lrl(d, a, b))]
        .into_iter()
        .filter_map(|(word, s)| s.map(|segs| DubinsPath { word, segs, rho }))
        .min_by(|x, y| x.len().total_cmp(&y.len()))
}

use manifold::SE2;

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Steer { L, S, R }

/// A signed segment: arcs in radians, straights in units of ρ; negative means reverse.
#[derive(Clone, Copy, Debug)]
pub struct Seg { pub steer: Steer, pub len: f64 }

/// The three symmetries of the normalized goal (x, y, φ). Time-flip negates every
/// segment, reflect swaps L and R; "backwards" (applied separately) reads the word
/// right to left. Eleven base formulas under these four maps: 44 evaluations that
/// cover the 48 Reeds–Shepp words.
const SYMMETRIES: [(f64, f64, f64, bool, bool); 4] = [
    (1.0, 1.0, 1.0, false, false),   // base
    (-1.0, 1.0, -1.0, true, false),  // time-flip
    (1.0, -1.0, -1.0, false, true),  // reflect
    (-1.0, -1.0, 1.0, true, true),   // both
];

pub fn reeds_shepp_candidates(x: f64, y: f64, phi: f64) -> Vec<Vec<Seg>> {
    let mut out = Vec::new();
    for &(sx, sy, sp, flip, refl) in &SYMMETRIES {
        let sign = if flip { -1.0 } else { 1.0 };
        let word = |w: [Steer; 3], l: [f64; 3]| -> Vec<Seg> {
            w.iter().zip(l).map(|(&s, len)| Seg {
                steer: if refl { s.mirrored() } else { s },
                len: sign * len,
            }).collect()
        };
        if let Some((t, u, v)) = lp_sp_lp(sx * x, sy * y, sp * phi) {
            out.push(word([Steer::L, Steer::S, Steer::L], [t, u, v]));
        }
        if let Some((t, u, v)) = lp_sp_rp(sx * x, sy * y, sp * phi) {
            out.push(word([Steer::L, Steer::S, Steer::R], [t, u, v]));
        }
        // … L+R-L (and its backwards reading), the two CCCC, the four CCSC, the CCSCC.
    }
    out
}

/// The shortest word in world units, or None (never, for ρ > 0).
pub fn reeds_shepp_shortest_path(q0: &SE2, q1: &SE2, rho: f64) -> Option<(Vec<Seg>, f64)> {
    let local = q0.inverse() * *q1;           // goal in the start's frame
    reeds_shepp_candidates(local.x / rho, local.y / rho, local.theta)
        .into_iter()
        .map(|w| { let l = rho * w.iter().map(|s| s.len.abs()).sum::<f64>(); (w, l) })
        .min_by(|a, b| a.1.total_cmp(&b.1))
}

impl Steer {
    fn mirrored(self) -> Self { match self { Steer::L => Steer::R, Steer::R => Steer::L, Steer::S => Steer::S } }
}