The Lab: Three Robots, Three Worlds, One Simulator

Rusty, Reach, and Hitch; the Apartment, the Workbench, and the Lot; configurations as typed values, Reach's forward kinematics, the four questions a collision checker answers, and the deterministic simulator every chapter of this book runs in.

The problem

Three cockpits, one engine. Below, Rusty is circling the Apartment's corridor, Reach is sweeping the Workbench, and Hitch is backing through the Lot. Each robot follows a scripted tour drawn from the seed in the transport bar, and each flashes red for a few frames when it touches something.

Take over. Click a pane, then drive. Rusty takes a forward speed and a turn rate from the arrow keys; Hitch takes a speed and a steering angle; Reach has no keys at all — you drag its elbow and its tip. Three things will happen to you, and each is a sentence of this chapter.

You will try to drive Reach's tip and fail. Drag the elbow handle and the tip traces a circle, whether you wanted a circle or not. Reach does not have a tip you can command; it has two angles, and the tip is where the angles put it. That is the forward kinematics map $\varphi$, and it is the first piece of mathematics below.

You will try to slide Hitch sideways into a bay and fail. No combination of arrow keys produces a lateral motion. The car's model has no input that does that — not a missing feature, a constraint, and the subject of Chapter 20. For now it is physics the simulator enforces.

You will drive Rusty into a wall and stop short of it. Rusty halts a body radius before contact, not at contact. A disc at $(x, y)$ collides when the wall is within $r$ of its center, and the collision checker says so. The red flash is the only place in this book where red means "bad"; it borrows the goal's color because a collision is where a plan ends.

The readouts under the panes print each configuration as its type: Pose2 { x, y, theta } for Rusty, T2 { th1, th2 } for Reach, HitchCfg { body, trailer } for Hitch. Those are not display strings. They are the names of three different Rust types, and the rest of the book is written against them.

Building intuition

Three configurations, three types

Choset writes "we use $q$" and leaves the reader to remember what kind of thing $q$ is. This book does not leave it to the reader. Rusty, considered as a disc for collision purposes, is a point of $\R^2$. Reach is a point of the torus $\Torus{2} = \Sone \times \Sone$: two angles, each wrapped to $(-\pi, \pi]$, with no joint limits so the links may pass over each other. Hitch's body is a pose in $\SEtwo$, and with a trailer the configuration picks up one more circle for the trailer's heading, $\SEtwo \times \Sone$.

Those are four different spaces with four different shapes. $\Torus{2}$ is compact and has finite area; $\R^2$ is flat and infinite; $\SEtwo$ is three-dimensional and not compact; a circle wraps and a line does not. Chapter 5 makes these differences theorems. Chapter 2 makes them types, which is the version a compiler can check:

/// A point of R²: the disc's center. Rusty, as the planner sees it.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct R2(pub Point2<f64>);

/// A point of T²: two joint angles, each wrapped to (−π, π]. Reach.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct T2 { pub th1: f64, pub th2: f64 }

/// A point of SE(2) × S¹ — or SE(2) alone when there is no trailer. Hitch.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct HitchCfg { pub body: Pose2, pub trailer: Option<f64> }

Try to hand the wrong one to the arm and the compiler answers before the program runs:

error[E0308]: mismatched types
  --> examples/lab_tour.rs:31:20
   |
31 |     let tip = reach.fk(&HitchCfg { body, trailer: None });
   |                     -- ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ expected `&Tn<2>`, found `&HitchCfg`

The compiler just told you that $\Torus{2} \ne \SEtwo \times \Sone$. Chapter 5 tells you why.

What a collision checker returns

The second thing to unlearn is that a collision checker returns a boolean. Freeze one scene and ask it the four questions a planner is allowed to ask.

The default scene is the Workbench with Choset's own arm — link lengths $L_1 = L_2 = 1$, zero thickness — at his own configuration $q = (\pi/4, \pi/2)$ from Example 3.8.1. The numbers on the canvas are the chapter's worked numbers: the distance tab reads $0.0607$ m to the block, and the witness tab puts $b^*$ at the block's corner $(0.5, 1.0)$. Slide $\theta_2$ and watch the distance fall to exactly zero at the instant the body turns red; keep sliding and it stays at zero while the footprint is inside. The distance query is not a smoothed boolean. It is a boolean and a number, and when the number is zero the boolean is what you have.

The witness pair is what Chapters 7 and 8 run on. Drag the tip around the block and watch $b^*$ jump from the corner to an edge the moment the nearest feature changes; the potential field of Chapter 7 pushes along exactly that segment, and the Voronoi diagram of Chapter 8 is the set of configurations where two witnesses tie.

The swept tab is the one planners cannot live without. It ghosts a second configuration one chart step away and asks whether the motion between the two is free — not the endpoints, the motion. Set the step large and you will find pairs of perfectly free endpoints with a red ghost between them: the arm passes through the post on its way from one to the other. Every edge of every roadmap in Part II and Part III is checked with this query, and no planner in this book ever checks only the endpoints of anything.

A world is not a bitmap

The third thing to unlearn is that a map is a picture. The three worlds are lists of named geometry: wall segments with endpoints, obstacle polygons with vertices, and places with names.

The Apartment is the sister book's floor plan, ported unchanged: five rooms off a long corridor, with rooms A and C mirror images of each other so that a range sensor cannot tell them apart. The Workbench is a 4.8 m square table with Reach bolted to its center and three obstacles inside the arm's reach: the block, which is the square $[0.5, 1.0] \times [1.0, 1.5]$ of the worked example, the post, a column in the arm's left-hand half, and the shelf, a bar along the near edge. The Lot is 16 m by 10 m, curbed, with four bays along the north side and two pillars Hitch must back around.

Because every wall is a segment and every obstacle a polygon, the distance from a point to the nearest wall is an exact number, a ray cast is an exact intersection, and the collision queries of the previous section are exact too. When Chapter 7 needs a grid it rasterizes these worlds at a chosen resolution; the grid is derived from the world, never the other way round. Click anywhere in the atlas to teleport the resident robot and the checker will refuse a configuration whose footprint overlaps anything — the same refusal, from the same function, that stops the dashboard's robots.

The mathematics

Definitions

Reach's forward kinematics

Reach's base is pinned at the origin. Joint 1 rotates the whole arm by $\theta_1$; joint 2, which rides at the end of link 1, rotates link 2 by a further $\theta_2$ relative to link 1. That relativity is the whole derivation.

The workspace is an annulus, not a configuration space

Choset's §3.1 makes a point that is easy to nod along to and hard to internalize: the set of positions the tip can reach is not the arm's configuration space, because knowing where the tip is does not tell you where the elbow is.

A disc's collision is a point query

Hitch's model is a definition, not yet a theorem

The worked example, assembled

Put the pieces together on the Workbench. Reach with $L_1 = L_2 = 1$ and zero-thickness links at $q = (\pi/4, \pi/2)$; the block $[0.5, 1.0] \times [1.0, 1.5]$.

The elbow is at $(0.7071, 0.7071)$ and the tip at $(0, 1.4142)$. Link 2 runs from the elbow to the tip along the line $x + y = \sqrt 2 \approx 1.4142$. Every point of the block has $x + y \ge 1.5$, so the block lies entirely on one side of that line and the nearest pair is the block's corner closest to the line against the foot of its perpendicular:

The foot $a^*$ lies between elbow and tip, so it is on the link and not clipped to an endpoint; intersects is false. These are the numbers the Query Anatomy widget shows at its default pose, and the library check reach_distance_to_square holds them to $10^{-9}$ for the distance and $10^{-6}$ for the witnesses.

One honest footnote. Zero-thickness links make the number exact and the arm unrealistic. The default Workbench arm wears 3 cm capsules, and the same query then reads $0.0607 - 0.03 = 0.0307$ m. Toggle "3 cm capsule links" in the widget and watch the number drop by exactly the radius.

The algorithm

What a collision checker computes is stated here as a contract and left sealed. The Rust crate delegates every geometric primitive to parry2d; the TypeScript port that runs the widgets hand-rolls them. Chapter 4 opens the box for polygons, Appendix C documents what the engine does, and Appendix F.5 of Choset — the GJK algorithm of Gilbert, Johnson, and Keerthi — is the name of the idea inside.

The browser port has no shape-cast, so it marches:

The bound $L$ is what makes the march conservative rather than hopeful. For a disc it is the Euclidean length of the edge. For Reach, a point on any link moves at most $(L_1 + L_2)$ per radian of either joint, so $L = (L_1 + L_2)(|\Delta\theta_1| + |\Delta\theta_2|)$ with the differences taken the short way round the circle. For Hitch it is the translation plus the body's circumscribed radius times the rotation. The library check swept compares the march against dense sampling at 2 mm chart spacing on 264 random edges across all three robots and finds no disagreement.

The remaining named routines of the chapter are small enough to state in a line each:

The sensor is worth one remark now and a chapter later. Its scan is Choset's saturated raw distance function $\rho_R$ sampled at $n$ bearings, and $\min_k \rho_R(x, s_k)$ is a sensor's estimate of $D(x)$, the distance to the nearest obstacle. It can never be smaller than $D(x)$ — a ray cannot hit something closer than the closest thing — and it equals $D(x)$ exactly when a ray happens to point at the nearest point. From Rusty's $(2, 1)$ with the wall at $x = 3$, ray 0 does, and the library check scan_is_rho_R sees $1.000000$.

Implementation in Rust

This section is the book's architectural contract. Later chapters add crates; none restructures what is fixed here.

Crates. nalgebra 0.35 for Point2 and Isometry2; parry2d 0.30 as parry2d-f64 for shapes, the Bvh broad phase, and query::{distance, closest_points, intersection_test, cast_shapes} — WASM-clean, no rayon; rand 0.9 with SmallRng for seeded streams; serde for the replay log. Geometry is the one thing this book does not hand-roll.

crates/
  prob/          # PORTED: Rng = seeded SmallRng wrapper
  geom/          # PORTED: Pose2 = SE(2)
  edt/           # PORTED: exact Euclidean distance transform
  sim/           # PORTED: World (Apartment), Rusty (diff-drive step, encoders), ray cast
  robots/        # THIS CHAPTER: Reach<N> (FK, IK), Hitch (car + trailer), Workbench, Lot
  collide/       # THIS CHAPTER: trait Collision over parry2d; Footprint; Scene; RangeSensor
  widget-kit/    # THIS CHAPTER: Role enum (the only place a color is named), capture, replay
  ch01_hello/    # Chapter 1 (std-only)
  bugs/ cspace/ manifold/ search/ …                    # added by their chapters
web/lib/
  prob/ geom/ sim/ mapping/edt                         # ported unchanged
  robots/ collide/                                     # this chapter's TypeScript mirrors

The arm

use nalgebra::Point2;

/// A point of Tⁿ: N joint angles, each wrapped to (−π, π].
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Tn<const N: usize>(pub [f64; N]);
pub type T2 = Tn<2>;

/// Planar revolute arm with N links. `Reach<2>` is the book's Reach; `Reach<3>`
/// is the redundant variant of Exercise 5 and Chapter 14.
pub struct Reach<const N: usize> {
    pub base: Point2<f64>,
    pub lengths: [f64; N],
    /// Capsule radius of each link. 0 gives exact segments — every worked number.
    pub link_radius: f64,
}

impl<const N: usize> Reach<N> {
    /// Choset's Example 3.8.1: L₁ = L₂ = 1 at the origin, zero-thickness links.
    pub fn choset() -> Reach<2> {
        Reach { base: Point2::origin(), lengths: [1.0, 1.0], link_radius: 0.0 }
    }

    /// φ as the chain T₀₁(θ₁) T₁₂(θ₂) ⋯: joint positions base..tip.
    ///
    /// One running heading and one running position. Joint angles are relative,
    /// so the absolute heading of link k is θ₁ + ⋯ + θₖ — that single fact is
    /// the whole derivation, and it is why the same loop serves any N.
    pub fn fk(&self, q: &Tn<N>) -> Vec<Point2<f64>> {
        let mut pts = Vec::with_capacity(N + 1);
        let (mut heading, mut p) = (0.0, self.base);
        pts.push(p);
        for i in 0..N {
            heading += q.0[i];
            p += nalgebra::Vector2::new(heading.cos(), heading.sin()) * self.lengths[i];
            pts.push(p);
        }
        pts
    }

    pub fn tip(&self, q: &Tn<N>) -> Point2<f64> {
        *self.fk(q).last().expect("N + 1 points")
    }

    /// R(q): one capsule per link, as parry2d shapes with their poses.
    pub fn footprint(&self, q: &Tn<N>) -> Footprint {
        let pts = self.fk(q);
        Footprint::capsules(pts.windows(2).map(|w| (w[0], w[1])), self.link_radius)
    }
}

impl Reach<2> {
    /// Closed-form IK (Choset problem 20): [elbow-down, elbow-up]; None outside the annulus.
    pub fn ik(&self, tip: Point2<f64>) -> [Option<T2>; 2] {
        let [l1, l2] = self.lengths;
        let d = tip - self.base;
        let c2 = (d.norm_squared() - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
        if !(-1.0..=1.0).contains(&c2) {
            return [None, None];
        }
        let s2 = (1.0 - c2 * c2).sqrt();
        let solve = |sign: f64| {
            let th2 = (sign * s2).atan2(c2);
            let th1 = d.y.atan2(d.x) - (l2 * th2.sin()).atan2(l1 + l2 * th2.cos());
            Tn([wrap(th1), wrap(th2)])
        };
        [Some(solve(1.0)), Some(solve(-1.0))]
    }
}

The browser runs lib/robots/reach.ts, a line-for-line port with Tn as a readonly number array; fmtTn is what prints T2 { th1: 0.785, th2: 1.571 } in the dashboard.

The car

/// SE(2) × S¹ with a trailer; SE(2) without.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct HitchCfg { pub body: Pose2, pub trailer: Option<f64> }

/// Speed and steering angle. φ is a *control*: it is not part of the configuration.
#[derive(Clone, Copy, Debug)]
pub struct CarCmd { pub v: f64, pub phi: f64 }

pub struct Hitch {
    pub wheelbase: f64,
    pub max_steer: f64,
    pub body: (f64, f64),            // length, width of the chassis
    pub hitch_len: Option<f64>,      // rear axle → trailer axle
}

impl Hitch {
    /// One tick. The constraint is *in* the model: no input produces a lateral
    /// velocity, so no command sequence slides the car sideways. (Chapter 20:
    /// a sideways *displacement* is still reachable, by a Lie bracket.)
    pub fn step(&self, q: &HitchCfg, u: &CarCmd, dt: f64) -> HitchCfg {
        let phi = u.phi.clamp(-self.max_steer, self.max_steer);
        let kappa = phi.tan() / self.wheelbase;
        let ds = u.v * dt;
        // Exact arc: the tangent vector (ds, 0, κ ds) pushed through exp on SE(2).
        let body = q.body.boxplus(&Vector3::new(ds, 0.0, kappa * ds));
        let trailer = match (q.trailer, self.hitch_len) {
            (Some(psi0), Some(dh)) => {
                // ψ̇ = (v/d_h) sin(θ(t) − ψ), θ(t) = θ₀ + κ v t known in closed form;
                // fixed-step RK4 over the tick keeps the run a pure function.
                let f = |t: f64, psi: f64| (u.v / dh) * (q.body.theta + kappa * u.v * t - psi).sin();
                Some(wrap(rk4(f, psi0, 0.0, dt, 4)))
            }
            _ => None,
        };
        HitchCfg { body, trailer }
    }

    /// R(q): the chassis rectangle, and the trailer's when attached. Both poses
    /// are functions of (x, y, θ, ψ) — the content of Exercise 2.
    pub fn footprint(&self, q: &HitchCfg) -> Footprint { /* two oriented rectangles */ }
}

The black box

use nalgebra::{Isometry2, Point2};
use parry2d::math::{Pose, Vector};
use parry2d::query::{self, ClosestPoints, ShapeCastOptions};

pub struct Witness { pub on_robot: Point2<f64>, pub on_obstacle: Point2<f64>, pub dist: f64 }

/// The four questions, and no others.
pub trait Collision {
    fn intersects(&self, fp: &Footprint) -> bool;
    /// 0.0 when intersecting.
    fn distance(&self, fp: &Footprint) -> f64;
    fn witness(&self, fp: &Footprint) -> Option<Witness>;
    /// Rigid straight-line sweep in the chart from fp_a's poses to fp_b's.
    fn swept(&self, fp_a: &Footprint, fp_b: &Footprint) -> bool;
}

/// Walls as thin segments, obstacles as convex polygons, all under one Bvh. Obstacles are
/// stored in parry's own types; only the robot's footprint arrives in nalgebra's.
pub struct Scene { obstacles: Vec<Obstacle>, bvh: Bvh, pub bounds: Aabb }

/// parry ≥ 0.26 does its math in glam; the rest of the workspace speaks nalgebra.
/// The two meet here and nowhere else.
fn to_parry(p: &Isometry2<f64>) -> Pose {
    Pose::new(Vector::new(p.translation.x, p.translation.y), p.rotation.angle())
}
fn from_parry(v: Vector) -> Point2<f64> { Point2::new(v.x, v.y) }

impl Collision for Scene {
    // The only module in the workspace that imports parry2d::query.

    fn intersects(&self, fp: &Footprint) -> bool {
        fp.parts.iter().any(|(pose, shape)| {
            let pose = to_parry(pose);
            self.candidates(&pose, shape).any(|o| {
                query::intersection_test(&pose, &**shape, &o.pose, &*o.shape).unwrap_or(true)
            })
        })
    }

    fn distance(&self, fp: &Footprint) -> f64 {
        self.witness(fp).map_or(f64::INFINITY, |w| w.dist)   // 0.0 on contact; ∞ in an empty scene
    }

    fn witness(&self, fp: &Footprint) -> Option<Witness> {
        let mut best: Option<Witness> = None;
        for (pose, shape) in &fp.parts {
            let pose = to_parry(pose);
            for o in self.candidates(&pose, shape) {
                // GJK, inside the engine (Choset App. F.5). We never see the simplex.
                match query::closest_points(&pose, &**shape, &o.pose, &*o.shape, f64::MAX).ok()? {
                    ClosestPoints::Intersecting => return Some(Witness::contact(&pose)),
                    ClosestPoints::WithinMargin(a, b) => {
                        let dist = a.distance(b);
                        if best.as_ref().is_none_or(|w| dist < w.dist) {
                            best = Some(Witness { on_robot: from_parry(a), on_obstacle: from_parry(b), dist });
                        }
                    }
                    ClosestPoints::Disjoint => {} // cannot happen: the margin is infinite
                }
            }
        }
        best
    }

    fn swept(&self, fp_a: &Footprint, fp_b: &Footprint) -> bool {
        fp_a.parts.iter().zip(&fp_b.parts).any(|((pa, shape), (pb, _))| {
            let (pa, pb) = (to_parry(pa), to_parry(pb));
            let vel = pb.translation - pa.translation;   // over unit time
            self.candidates(&pa, shape).any(|o| {
                // A shape-cast reports a time of impact in [0, 1] if the swept volume hits.
                query::cast_shapes(&pa, vel, &**shape, &o.pose, Vector::ZERO, &*o.shape,
                                   ShapeCastOptions::with_max_time_of_impact(1.0))
                    .map(|hit| hit.is_some()).unwrap_or(true)
            })
        })
    }
}

The worked example as a program

fn main() {
    let arm = Reach::<2>::choset();
    let q = Tn([FRAC_PI_4, FRAC_PI_2]);
    let pts = arm.fk(&q);
    println!("elbow ({:.4}, {:.4})  tip ({:.4}, {:.4})", pts[1].x, pts[1].y, pts[2].x, pts[2].y);

    // A scratch scene with the worked example's square — which is also the Workbench's block.
    let scene = Scene::empty(WORKBENCH_BOUNDS).with_obstacle("the block", Cuboid::from_aabb(0.5, 1.0, 1.0, 1.5));
    let fp = arm.footprint(&q);
    let w = scene.witness(&fp).expect("a witness exists");
    println!("intersects: {}  distance: {:.4}", scene.intersects(&fp), scene.distance(&fp));
    println!("witness a*=({:.4}, {:.4}) b*=({:.4}, {:.4})", w.on_robot.x, w.on_robot.y, w.on_obstacle.x, w.on_obstacle.y);

    // Rusty against one wall.
    let wall = Scene::of_walls(&[Segment::new(point![3.0, 0.0], point![3.0, 3.0])]);
    let rusty = Footprint::disc(point![2.0, 1.0], RUSTY.body_radius);
    println!("distance: {:.4}", wall.distance(&rusty));

    // The 60 s tour at BOOK_SEED, recorded and replayed.
    let log = Run::record(TourConfig::default(), BOOK_SEED);
    assert_eq!(Run::replay(&log), log.frames, "replay must be bit-identical");
    print_final(&log);
    svg::write("figures/ch02_tour.svg", &log);
}

elbow (0.7071, 0.7071)  tip (0.0000, 1.4142)
intersects: false  distance: 0.0607
witness a*=(0.4571, 0.9571) b*=(0.5000, 1.0000)
distance: 0.8900

Every line of that output is locked by a test in the crate and by a check in the browser library:

#[test]
fn fk_matches_choset_example() {
    let tip = Reach::<2>::choset().tip(&Tn([FRAC_PI_4, FRAC_PI_2]));
    assert_abs_diff_eq!(tip.x, 0.0, epsilon = 1e-12);
    assert_abs_diff_eq!(tip.y, SQRT_2, epsilon = 1e-12);
}

#[test]
fn reach_distance_to_square() {
    let (scene, fp) = worked_scene();
    assert_abs_diff_eq!(scene.distance(&fp), 1.5 / SQRT_2 - 1.0, epsilon = 1e-9);
    let w = scene.witness(&fp).unwrap();
    assert_abs_diff_eq!(w.on_obstacle, point![0.5, 1.0], epsilon = 1e-6);
    assert_abs_diff_eq!(w.on_robot, point![0.4571, 0.9571], epsilon = 1e-4);
}

#[test]
fn rusty_wall_distance() { assert_abs_diff_eq!(wall_scene().distance(&rusty_at(2.0, 1.0)), 0.89, epsilon = 1e-12); }

#[test]
fn ik_roundtrip() { /* 1,000 seeded q: fk(ik(fk(q))) == fk(q) to 1e-9, both elbows */ }

#[test]
fn swept_catches_midpoint() {
    // Two free configurations whose straight sweep passes link 1 through the post.
    let c = ChainChecker::new(Scene::workbench(), Reach::<2>::workbench());
    let (a, b) = (Tn([2.0, 0.0]), Tn([3.1, 0.0]));
    assert!(c.is_free(&a) && c.is_free(&b));
    assert!(!c.edge_free(&a, &b));
}

#[test]
fn tour_replays_bitwise() {
    let (a, b) = (Run::record(TourConfig::default(), BOOK_SEED), Run::record(TourConfig::default(), BOOK_SEED));
    assert_eq!(a.frames, b.frames); // every f64 equal exactly, not approximately
}

#[test]
fn scan_is_rho_r() {
    // min over 360 rays from (2, 1) equals the point distance to the wall at x = 3.
    let scan = RangeSensor { n_rays: 360, max_range: 4.0 }.scan(&wall_scene(), point![2.0, 1.0]);
    assert_abs_diff_eq!(scan.iter().cloned().fold(f64::INFINITY, f64::min), 1.0, epsilon = 1e-9);
}

Putting it together

The integration lab is the tour itself. Sixty seconds at 20 Hz, three robots, one seed: Rusty pursues a loop of waypoints out of room A, down the corridor, into room C and back, with the sister book's wheel slip as the run's one stochastic subsystem; Reach follows two incommensurate sinusoids whose amplitudes, frequencies, and phases the seed chose; Hitch runs a six-leg reversing script with the seed's steering angles. Every tick, each robot's footprint goes to its own Collision implementation; a contact flashes, is counted, and — for Rusty and Hitch, whose motion is integrated — holds the body where it is while the wheels keep turning, which is exactly what a stuck robot's encoders do.

Run it twice and compare every frame. The library check tour_replays_bitwise does: 1,200 frames, every coordinate equal to the last bit, and a different seed gives a different log. After the full minute at BOOK_SEED = 2005 the browser port reports

Rusty  Pose2 { x: 6.212, y: 4.466 }        contacts: 0
Reach  T2 { th1: 0.186, th2: 1.324 }       contacts: 9
Hitch  Pose2 { x: 6.245, y: 1.351, theta: 0.272 }   contacts: 12

and the check tour at BOOK_SEED pins those digits. Press re-roll on the dashboard and every one of them changes; type the seed back and every one of them returns. Rusty's zero contacts are not luck: the waypoint controller closes its loop on ground truth, which — as the sister book is careful to say wherever it uses the same trick — is a quantity no real robot has.

One honesty item about the two implementations. The geometry agrees across them to the digit, and each replays itself bit for bit. The tours do not agree across them, because SmallRng and the browser's generator draw different numbers from the same seed, and Rusty's slip and the scripts' parameters come from those draws. The claim this book makes is the one it can test: that each implementation is a pure function of its seed, and that both compute the same geometry.

The anatomy of a widget

Every interactive figure in this book is built from the same parts, fixed here and never renegotiated. A WidgetFrame carries the id from the chapter's design manifest, the title, one line naming the misconception the widget kills, and a color key. Inside it, one or more SimCanvas instances draw in metres and radians and let the framework map to pixels, resize, and re-read the palette when the reader flips theme. A useSimulation hook runs a fixed-rate clock from a seed; Transport shows play, step, reset, re-roll, and the seed itself, because reproducibility is part of what is being taught. Colors are never named in a widget — they come from the palette the canvas hands to draw, which is how hovering a tinted term in an equation can fade every other role on the page without any widget knowing the feature exists. Each widget autoplays a seeded default, foregrounds one parameter, and hides the rest behind a disclosure. The frame also carries a <noscript> slot for a static description, so a reader without JavaScript is told what the simulation would have drawn rather than shown a blank.

Which chapter opens which box: Chapter 3 builds the Bug algorithms on RangeSensor and the Apartment; Chapter 4 opens collide and adds Reach's Jacobian beside the forward kinematics defined here; Chapter 5 implements Manifold for the configuration types defined here; every planner from Chapter 7 to Chapter 21 takes a &dyn Collision and a robot from robots; and Appendix F is this section, expanded into a reference.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Inverse kinematics for general link lengths

   Derive Reach's inverse kinematics for general $L_1, L_2$ using the law of cosines and $\operatorname{atan2}$ (Choset problem 20). State the reachable annulus and show exactly which tip positions have one solution rather than two. Then explain why Reach::ik returns [Option<T2>; 2] rather than Option<[T2; 2]> — what does it mean for exactly one entry to be None?

   Stuck? Get a hint
2. Foundation exerciseFDifficulty 2 of 3R(q) for a car and trailer

   Write $\Rq$ as a set for Hitch with the trailer: show it is the union of two rectangles whose poses are functions of $(x, y, \theta, \psi)$, and give those two poses explicitly in terms of the wheelbase, the overhangs, and $d_h$. Then explain why $\psi$ is a configuration variable but the steering angle $\phi$ is not. (The compiler already knows: $\phi$ lives in CarCmd, not in HitchCfg.)

3. Conceptual exerciseCDifficulty 2 of 3Predict a witness, then watch it jump

   Put Reach at $q = (0, \pi/2)$ and imagine the square obstacle of side $0.2$ centered on $(1, 1)$.

   Predict first

   What does the distance query return for this configuration?

   About 0.1 — the half-side of the square, measured from its edge to the tip

   Exactly 0 — the configuration is in collision

   About 0.9 — the distance from the elbow to the square

   Distance from link 2 to the square at q = (0, π/2 + 0.3), in metres

   mCheck Reveal
4. Conceptual exerciseCDifficulty 2 of 3One metre to the left

   In the dashboard, turn the trailer on and try to make Hitch's trailer end up exactly one metre to the left of where it started, with the same heading. Describe the maneuver you find and count its direction reversals.

   Stuck? Get a hint
5. Practical exercisePDifficulty 2 of 3Redundancy: a tip that does not move

   Add Reach<3> with lengths = [0.6, 0.5, 0.4] to the dashboard's Workbench pane and show that the tip can stay fixed while the arm moves (redundancy, Choset §3.3). Write a test that takes 200 seeded configurations with the same tip — generate them by a one-dimensional walk in the null space of the tip map, re-solving the last two joints by the 2R inverse kinematics after each step of $\theta_1$ — and asserts tip constant to $10^{-9}$.

6. Practical exercisePDifficulty 3 of 3Lie to the Bug (stretch)

   Replace the exact RangeSensor by the sister book's noisy LiDAR port (lib/sim/rusty.ts, raycastScan) in the Bug1 stub of Chapter 1 and measure, over 100 seeds, how often the leave point is misjudged. Write one paragraph on why Chapter 3's guarantee needs the ideal sensor, and what a real robot does instead (the sister book's Chapter 10 is the pointer).

References

1. Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L. E., and Thrun, S. (2005) Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press.link to Principles of Robot Motion: Theory, Algorithms, and Implementations (opens in a new tab)

   §3.1 for the circular robot's R(q) and the arm's torus; §3.8, Example 3.8.1, for the forward kinematics and its worked numbers; Appendix F.5 for GJK, the name of what the black box does.

2. Gilbert, E. G., Johnson, D. W., and Keerthi, S. S. (1988) A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal on Robotics and Automation 4(2), 193–203.doi:10.1109/56.2083 (opens in a new tab)

   The GJK distance algorithm, Choset's Algorithm 23: the iteration inside every distance and witness query the Rust crate delegates to parry2d.

3. Lozano-Pérez, T. (1983) Spatial Planning: A Configuration Space Approach. IEEE Transactions on Computers C-32(2), 108–120.doi:10.1109/TC.1983.1676196 (opens in a new tab)

   Where the disc's one-line Minkowski inflation comes from, and where Chapter 4 picks it up for polygons.

4. Pan, J., Chitta, S., and Manocha, D. (2012) FCL: A general purpose library for collision and proximity queries. IEEE International Conference on Robotics and Automation (ICRA), 3859–3866.doi:10.1109/ICRA.2012.6225337 (opens in a new tab)

   The collision library behind MoveIt and OMPL's default checkers, with the same four-query vocabulary — intersection, distance, closest points, continuous collision — that this chapter's Collision trait adopts.

5. Şucan, I. A., Moll, M., and Kavraki, L. E. (2012) The Open Motion Planning Library. IEEE Robotics & Automation Magazine 19(4), 72–82.doi:10.1109/MRA.2012.2205651 (opens in a new tab)

   A planner that takes a state validity checker and a state space as abstract interfaces: the architecture this chapter's Collision trait object and typed configurations mirror.

6. Dimforge (2024) parry: 2D and 3D collision-detection library for the Rust programming language. Documentation and source.link to parry: 2D and 3D collision-detection library for the Rust programming language (opens in a new tab)

   The crate behind crates/collide: shapes, the Bvh broad phase, and query::{distance, closest_points, intersection_test, cast_shapes}. Appendix C documents what it does.

7. Thrun, S., Burgard, W., and Fox, D. (2005) Probabilistic Robotics. MIT Press.link to Probabilistic Robotics (opens in a new tab)

   The source of Rusty's differential-drive model, encoders, and LiDAR, ported here from the sister book's Chapter 4 and never re-derived.