Frames, Transforms, and the Kinematics of Wheels

Chapter 1: Two Frames, One Point — Coordinates Are a Choice

A point in space is just a point. But its coordinates — the pair of numbers we write down for it — depend entirely on which reference frame we measure from. The same charging dock can be "(3, 1) in the world" and "(2, −1) as seen by the robot" at the same instant. Both are correct. They describe one physical spot through two different lenses.

A reference frame (or coordinate frame) is an origin point plus a set of axis directions. We will use two throughout:

• The world frame, written W: a fixed origin O_W on the floor, with axes x_W (east) and y_W (north). Nothing moves it. The map lives here.
• The body frame, written B: an origin O_B at the robot's center, with x_B pointing out the nose and y_B out the left side. It is bolted to the robot, so it translates and rotates as the robot does.

When we write a point's coordinates we should always say which frame they are in. The standard notation puts the frame as a leading superscript: ^Wp is "point p expressed in the world frame," and ^Bp is "the same point expressed in the body frame." Same p, two columns of numbers.

A worked frame conversion, by hand

Suppose the robot sits at world position (2, 1) and — to keep this chapter pure translation — is not rotated: its body axes are parallel to the world's. A charging dock is at world point ^Wp = (5, 3). What are the dock's coordinates in the body frame?

Geometrically, the body frame's origin is at (2, 1). To get the dock's position as seen from the robot, subtract the robot's origin from the dock's world position:

So the dock is "3 units ahead and 2 units to the left" of the robot. Check it the other way: if the robot knows the dock is at ^Bp = (3, 2) in its own frame, it recovers the world position by adding its origin back: (3, 2) + (2, 1) = (5, 3). The conversions are inverses, as they must be.

That subtraction handled displacement only, because we forced the frames to be parallel. The moment the robot can rotate, a plain subtraction is no longer enough — we will need to rotate the difference vector too. That rotation is the subject of Chapter 2, and it is where matrices enter. For now, play with the parallel-frames case so the "same point, two readouts" idea is concrete.

From-scratch code: the parallel-frame conversion

python
import numpy as np

# Robot (body-frame origin) and a point, both in WORLD coordinates.
O_B   = np.array([2.0, 1.0])   # where the robot stands
p_world = np.array([5.0, 3.0]) # the dock

# Frames are PARALLEL (no rotation yet) -> conversion is a subtraction.
p_body  = p_world - O_B          # [3., 2.]  "3 ahead, 2 left"
p_back  = p_body  + O_B          # [5., 3.]  recovers the world point

print(p_body, p_back)             # [3. 2.] [5. 3.]  -- inverses, as required

Chapter 2: Rotation in the Plane — Deriving SO(2) From Scratch

Chapter 1's subtraction worked only because the frames were parallel. To handle a robot that has turned, we need a machine that takes a vector and rotates it by an angle θ. That machine is a 2×2 matrix, and we are going to build it, not look it up.

The trick to deriving any rotation rule is to ask only one question: where do the basis vectors go? If I know where the x-axis unit vector and the y-axis unit vector land after rotation, I know where everything lands, because every vector is just a combination of those two.

Rotating the two basis vectors, by hand

Start with the x-axis unit vector, e_x = (1, 0). Rotate it counter-clockwise by an angle θ about the origin. Trigonometry of the unit circle tells us its tip traces from angle 0 to angle θ, so it lands at:

Now the y-axis unit vector, e_y = (0, 1). It already sits at angle 90°; rotating it by θ sends it to angle 90° + θ. Using cos(90°+θ) = −sinθ and sin(90°+θ) = cosθ:

A matrix's columns are exactly "where the basis vectors go." So we stack those two results as columns and the rotation matrix simply appears:

(first row: cosθ, −sinθ  ·  second row: sinθ, cosθ)

A fully numeric example: rotate (1, 0) and (0, 1) by 30°

Plug in θ = 30°, where cos30° ≈ 0.866 and sin30° = 0.5. The matrix is:

Apply it to e_x = (1, 0). Matrix-times-vector means "row dotted with the vector," one row at a time:

The x-axis tipped up to 30° — exactly cos30° right and sin30° up. Now apply it to e_y = (0, 1):

The y-axis swung left and up, landing at 120°, exactly 90° + 30°. The matrix does precisely what our basis-vector reasoning predicted.

A second numeric example: rotate the vector (2, 1) by 90°

At θ = 90°, cos = 0 and sin = 1, so R(90°) = [0, −1 ; 1, 0]. Apply it to (2, 1):

Sanity check by hand: a 90° counter-clockwise turn should send "right and a little up" to "up and a little left." (2, 1) was mostly-rightward; (−1, 2) is mostly-upward. Correct. Rotating by 90° always does (x, y) → (−y, x), which you can now read straight off the matrix.

Verify R⁻¹ = R^⊤ with the 30° matrix. Its transpose (swap off-diagonal entries) is [0.866, 0.500 ; −0.500, 0.866], which is exactly R(−30°) since cos is even and sin is odd. Multiply them: R(30°)·R(−30°) = R(0°) = I. Undo and redo cancel, as geometry demands.

From-scratch code vs. the library one-liner

python
import numpy as np

# From scratch: build R(theta) by typing in the four entries.
def rot2(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s],
                     [s,  c]])

th = np.deg2rad(30)
R  = rot2(th)
print(R @ np.array([1.0, 0.0]))   # [0.866 0.5  ] -- rotated x-axis
print(R @ np.array([0.0, 1.0]))   # [-0.5   0.866] -- rotated y-axis

# Properties, verified numerically:
print(np.round(R.T @ R, 9))         # identity  -> orthogonal
print(np.round(np.linalg.det(R), 9)) # 1.0       -> a rotation, not a reflection
print(np.allclose(np.linalg.inv(R), R.T))  # True  -> inverse is the transpose

# The library one-liner (scipy) does the same thing in 3D-aware form:
# from scipy.spatial.transform import Rotation as Rot
# R3 = Rot.from_euler('z', 30, degrees=True).as_matrix()  # 3x3, z-rotation

Chapter 3: Rotation + Translation = SE(2) — One Matrix to Rule a Frame

A real robot does two things at once: it sits somewhere (translation) and faces some way (rotation). Chapter 1 gave us translation; Chapter 2 gave us rotation. Now we fuse them into a single object — the rigid-body transform — that captures a frame's complete pose in one matrix.

Physically, to convert a point from the body frame to the world frame you must do both operations in the right order: first rotate the body-frame coordinates so the axes line up with the world's, then translate by the body origin's world position. In symbols:

Here t = (t_x, t_y) is the body origin's position in the world — the "where the robot stands" vector — and R(θ) is the rotation that aligns the body axes to the world. This "rotate then add" form is correct and complete. But carrying a separate matrix and vector around, and remembering the order, is error-prone. So we play a clever trick.

Why homogeneous coordinates

We want a single matrix that does "rotate and translate" in one multiply. The obstacle: matrix multiplication can only scale and mix coordinates (that gives rotation), it can never add a constant (that is what translation needs). The fix is to add a fake third coordinate, always equal to 1, to every point:

This "1" is a smuggler. When a matrix multiplies it, the entries in the matrix's third column get added in as constants — which is exactly translation. We call (x, y, 1) the homogeneous coordinates of the point, and the 3×3 matrix that packages R and t together is the SE(2) transform ("special Euclidean group in 2D"):

top-left 2×2 block is R(θ); top-right column is t; bottom row is (0, 0, 1)

The bottom row (0, 0, 1) is bookkeeping: it keeps the output's third coordinate equal to 1, so the result is still a valid homogeneous point you can transform again. A whole pose — position and heading — now lives in one matrix.

A point transform, fully by hand

Robot at world position t = (2, 1), rotated θ = 90°, so R(90°) = [0, −1 ; 1, 0]. A point sits at ^Bp = (3, 0) in the body frame — "3 units straight ahead of the robot." Build T and apply it:

Multiply row by row:

So ^Wp = (2, 4). Sanity check: the robot stands at (2, 1) facing north (90°), and the point is 3 ahead of it. "3 ahead" while facing north means 3 units up in y, landing at (2, 1+3) = (2, 4). The matrix agrees with the picture exactly — the θ=90° rotation turned "3 ahead in body x" into "+3 in world y," then the translation shifted by (2, 1).

Composing two transforms, by hand

Let the robot's pose in the world be T_WB: position (2, 1), heading 0° (axes parallel to world), so R = I. Mounted on the robot is a camera at body position (1, 0) rotated 90°, giving the camera's pose-on-robot T_BC. We want the camera's pose in the world, T_WC = T_WB · T_BC.

Because T_WB is the identity rotation with a (2,1) shift, multiplying just adds (2,1) to the translation column of T_BC and leaves its rotation block alone:

Read it off: the camera is in the world at (3, 1) — the robot's (2,1) plus the camera's (1,0) offset — rotated 90°. With a non-identity robot rotation the camera offset would itself get rotated first; the matrix product handles that automatically. That is the whole point of doing it with matrices instead of bookkeeping by hand.

python
import numpy as np

def SE2(x, y, theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, x],
                     [s,  c, y],
                     [0,  0, 1]])

def apply(T, p):
    p_h = np.array([p[0], p[1], 1.0])   # homogeneous: append the smuggler 1
    return (T @ p_h)[:2]                # drop the bookkeeping coordinate

T_WB = SE2(2, 1, np.deg2rad(90))      # robot pose in world
print(apply(T_WB, [3, 0]))         # [2. 4.]  -- "3 ahead" while facing north

T_BC = SE2(1, 0, np.deg2rad(90))      # camera-on-robot
T_WC = T_WB @ T_BC                  # compose: camera pose in world
T_WB_inv = np.linalg.inv(T_WB)      # undo a transform (a frame's inverse pose)

Chapter 4: Configuration — The Numbers That Pin a Robot Down

SE(2) describes a single pose. But a moving robot has a pose at every instant, and to talk about motion we need to track those numbers as they change over time. The minimal set of numbers that completely specifies a robot's configuration are its generalized coordinates, collected in a vector written ξ (the Greek letter xi):

Here n is how many numbers it takes, ℝⁿ means "an ordered list of n real numbers," and the small ⊤ just means we treat ξ as a column. The set of all possible configurations is the configuration space. For a wheeled robot rolling on a flat floor, three numbers suffice:

where (x, y) is the robot's position and θ its heading — the same pose SE(2) packed into a matrix, now written as a plain vector for doing calculus on. The pair (position, orientation) is the robot's pose.

Trajectories: configuration as a function of time

A robot in motion is a configuration that changes with time, written ξ(t) — the trajectory. Its time-derivative is the velocity in configuration space:

The dot over a symbol (read "x-dot") means "rate of change." So ẋ and ẏ are the velocity components along the world axes, and θ̇ is the turning rate. Whole chunks of robotics — planning, control, estimation — are really statements about ξ(t) and ξ̇(t).

The wheel example — why coordinates are a modeling choice

Consider a single wheel rolling on a line. What is its configuration? You might pick its position s along the line. But if you also care about which spoke is up — the wheel's rolling angle φ — then ξ = (s, φ) with n = 2. Yet for a wheel that rolls without slipping, s and φ are locked together: s = rφ (radius times angle). So the two coordinates have only one degree of freedom. Choosing coordinates is a modeling decision: include exactly what you need to specify the configuration, no more.

Below, set a planar robot's configuration directly and read its ξ vector, with the body frame drawn on top so you see the pose the three numbers encode.

python
import numpy as np

# A trajectory is just configuration as a function of time.
def xi_of_t(t):
    # example: drive east while slowly turning
    return np.array([0.5*t, 0.0, 0.1*t])   # [x, y, theta]

def xidot_num(t, h=1e-5):
    return (xi_of_t(t+h) - xi_of_t(t-h)) / (2*h)  # numerical d/dt

print(xi_of_t(2.0))      # [1.  0.  0.2]   pose at t=2
print(xidot_num(2.0))    # [0.5 0.  0.1]   velocity xi-dot

Chapter 5: Kinematic Constraints — The Pfaffian Form

Chapter 4 hinted that a robot's velocities are often more restricted than its configuration space. We now write those restrictions as equations. A kinematic constraint is a rule that the robot's configuration and/or velocity must obey at all times — "you cannot slide sideways," "this rod has fixed length." We write a constraint as a function that must stay zero:

Two flavors. A constraint on configuration alone — h(ξ) = 0 — restricts where the robot can be (a pendulum bob stuck at fixed distance from its pivot). A constraint on velocity — involving ξ̇ — restricts how the robot can move (no sideways sliding). The velocity constraints we care about are linear in the velocities: each velocity component appears to the first power, never squared or multiplied together. That linear-in-velocity form has a name — the Pfaffian form:

Read it carefully. A(ξ) is a matrix whose entries depend only on where the robot is (its configuration), not on how fast it's going. It multiplies the velocity vector ξ̇, and the product must be zero. Each row of A^⊤ is one constraint — one forbidden combination of velocities. If A^⊤ has k rows, there are k constraints, and they carve down the allowed velocity directions.

Worked example: the planar pendulum, derived to Pfaffian form

A bead is connected to the origin by a rigid rod of length L; it can swing but the rod can't stretch. Its configuration is ξ = (x, y). The "rod can't stretch" rule is a constraint on position: the distance from the origin is always L. Squaring to avoid the square root:

Use real numbers: L = 5, and the bead currently at (3, 4). Check: 3² + 4² − 5² = 9 + 16 − 25 = 0. On the circle, good. Now — here's the key move — differentiate the constraint with respect to time. As the bead moves, h must stay zero, so its rate of change is zero. Using the chain rule (d/dt of x² is 2x·ẋ):

Divide by 2 and you have a Pfaffian (velocity-linear) constraint:

At the point (3, 4): the constraint reads 3·ẋ + 4·ẏ = 0. The row vector (3, 4) points radially outward, and the rule says the velocity must be perpendicular to it — the bead can only move tangent to the circle. That matches physical intuition: a pendulum bob moves along its arc, never along the rod. We just derived that fact from one differentiation.

Numeric check of an allowed velocity. The direction tangent to the circle at (3, 4) is (−4, 3) (swap and negate — perpendicular to the radius). Plug in ξ̇ = (−4, 3): 3·(−4) + 4·3 = −12 + 12 = 0. Allowed. Try a radial (outward) velocity ξ̇ = (3, 4): 3·3 + 4·4 = 25 ≠ 0. Forbidden — the rod would have to stretch. The Pfaffian equation correctly accepts the tangent and rejects the radial.

python
import numpy as np

# Pendulum: A^T(xi) for h = x^2 + y^2 - L^2, derived by d/dt.
def A_pendulum(xi):
    x, y = xi
    return np.array([[x, y]])      # one row, one constraint

xi  = np.array([3.0, 4.0])     # on the circle of radius 5
v_tan = np.array([-4.0, 3.0])  # tangent (perpendicular to radius)
v_rad = np.array([3.0, 4.0])   # radial (outward)

print(A_pendulum(xi) @ v_tan)   # [0.]   allowed
print(A_pendulum(xi) @ v_rad)   # [25.]  forbidden (rod would stretch)

Chapter 6: Holonomic vs. Nonholonomic — The Difference That Makes Parking Hard

Both the pendulum and the no-slip wheel give a Pfaffian constraint A^⊤·ξ̇ = 0. But they are profoundly different. The pendulum's constraint secretly traps the bead on a circle — it shrinks the reachable space. The wheel's constraint forbids sideways motion at each instant, yet a car can still reach any (x, y, θ) given enough wiggling. That distinction is the most important idea in this lesson.

• A constraint is holonomic if it can be integrated back into a pure position constraint h(ξ) = 0. It really does pin you to a lower-dimensional surface. The pendulum is holonomic: its velocity constraint came from differentiating h = x²+y²−L², so you can integrate it right back. A robot with k holonomic constraints loses k dimensions of reachable configuration: it lives in an (n − k)-dimensional space.
• A constraint is nonholonomic if it canNOT be integrated into a position constraint. It limits your instantaneous velocities but not the configurations you can eventually reach. The rolling wheel is nonholonomic: no sideways motion at any instant, yet every (x, y, θ) is reachable.

Deriving the no-slip constraint from the geometry

A wheeled robot at heading θ has a body x-axis (its rolling direction) e_v = (cosθ, sinθ), and a body y-axis (its axle direction, the forbidden direction) which is e_v rotated 90°:

The no-slip rule says the wheel cannot move along its axle — its velocity has zero component in the e_⊥ direction. "Zero component along e_⊥" is exactly a dot product equal to zero. The robot's world velocity is (ẋ, ẏ), so:

Rearranging the signs to the conventional form:

This is the famous no-slip Pfaffian constraint, with A^⊤(ξ) = [sinθ, −cosθ, 0] (the θ̇ column is 0 — turning in place doesn't violate no-slip). It depends on configuration through θ, exactly as the Pfaffian form promised.

A numeric check, and why it's nonholonomic

At θ = 90° (facing north): sin90° = 1, cos90° = 0, so the constraint is ẋ·1 − ẏ·0 = ẋ = 0. Translated: a north-facing robot may have any ẏ (drive north) and any θ̇ (turn), but ẋ must be zero — it cannot slide east or west. Exactly right: it can drive straight forward (north) or pivot, but not slide sideways.

Why can't we integrate this back to an h(ξ) = 0? Because the constraint row changes with θ, and as the robot turns, the "forbidden direction" rotates too. By alternately driving and turning, the robot threads its velocity through a forbidden direction that keeps moving — netting a sideways displacement no single allowed velocity could produce. There is no fixed surface it's trapped on. (The formal test is whether the constraints' Lie brackets stay inside the allowed directions; for the wheel they don't, which is the rigorous reason it's nonholonomic.)

python
import numpy as np

# No-slip constraint row for a wheeled robot at heading theta.
def A_noslip(theta):
    return np.array([[np.sin(theta), -np.cos(theta), 0.0]])  # [sin, -cos, 0]

th = np.deg2rad(90)                 # facing north
print(A_noslip(th) @ np.array([1.0, 0, 0]))  # [1.] -> sliding east FORBIDDEN
print(A_noslip(th) @ np.array([0, 2.0, 0]))  # [0.] -> driving north ALLOWED
print(A_noslip(th) @ np.array([0, 0, 1.0]))  # [0.] -> turning in place ALLOWED

Chapter 7: The Unicycle Model — From Constraint to Motion

The no-slip constraint told us what the robot cannot do. The unicycle model flips that around and says what it can do, in the cleanest possible form. It is the workhorse model for nearly every wheeled robot in this course.

Recall the constraint ẋ sinθ − ẏ cosθ = 0 carves out a 2D space of allowed velocities inside the 3D configuration. We need a basis for that allowed space — two "knobs" that generate every legal velocity. They have obvious physical meaning:

• v — the forward (linear) speed, how fast the robot drives along its nose, in m/s.
• ω — the turning (angular) rate, how fast the heading changes, in rad/s. (This is the Greek letter omega, the standard symbol for angular velocity.)

Driving forward at speed v while pointed at θ advances the world position along the heading vector (cosθ, sinθ), and turning at rate ω changes θ directly. That gives the unicycle equations — three first-order differential equations for the configuration:

The two control inputs are (v, ω); everything else follows. Plug these back into the no-slip constraint to confirm they obey it: ẋ sinθ − ẏ cosθ = (v cosθ)sinθ − (v sinθ)cosθ = 0. Always zero, for any v and ω — the unicycle model lives exactly on the allowed-velocity surface. The model is the constraint, solved.

Integrating one step, by hand

Numbers make it real. Start at ξ₀ = (x, y, θ) = (0, 0, 0) — at the origin, facing east. Command v = 2 m/s and ω = 1 rad/s for a small time step Δt = 0.5 s. We use forward Euler integration: new value = old value + rate × Δt. Compute the rates at the current state first:

Now step each coordinate by rate × Δt:

After the step the robot is at (1.0, 0.0) facing 0.5 rad. Take a second step from there, now with θ = 0.5: ẋ = 2cos0.5 ≈ 2·0.878 = 1.755, ẏ = 2sin0.5 ≈ 2·0.479 = 0.959. So x ← 1.0 + 1.755·0.5 = 1.88, y ← 0.0 + 0.959·0.5 = 0.48, θ ← 0.5 + 0.5 = 1.0 rad. The path curves leftward — constant v with constant ω traces a circular arc, exactly what you'd expect from "drive while turning at a steady rate."

python
import numpy as np

def unicycle_step(xi, v, w, dt):
    x, y, th = xi
    # rates from the current state, then forward-Euler step
    x  = x  + v*np.cos(th)*dt
    y  = y  + v*np.sin(th)*dt
    th = th + w*dt
    return np.array([x, y, th])

xi = np.array([0.0, 0.0, 0.0])
xi = unicycle_step(xi, v=2.0, w=1.0, dt=0.5)
print(np.round(xi, 3))   # [1.  0.  0.5]  matches the hand calc
xi = unicycle_step(xi, v=2.0, w=1.0, dt=0.5)
print(np.round(xi, 3))   # [1.878 0.479 1. ]  the path curves left

Chapter 8: Differential Drive & the Simple Car — Real Wheels

The unicycle commands (v, ω) directly — but no real robot has a "ω knob." A TurtleBot has two driven wheels; you set their individual speeds, ω_L and ω_R. A car has a throttle and a steering wheel. We need to translate between those physical inputs and the unicycle's (v, ω). That translation is the last piece of the body-to-wheel dictionary.

Differential drive: two wheels, one body

A differential-drive robot has two wheels of radius r, mounted a distance L apart on a common axle (L is the wheelbase, or track width). Each wheel rolls without slipping, so a wheel spinning at angular rate ω_R contributes a ground speed of r·ω_R at its contact point (speed = radius × spin rate). The robot's forward speed is the average of the two wheel speeds; its turn rate comes from their difference:

The intuition is exactly how you'd steer a wheelchair or a tank. Spin both wheels equally and you go straight (the difference is zero, so ω = 0). Spin the right wheel faster than the left and you veer left (positive ω). Spin them opposite ways and you pivot in place (v = 0, the average is zero). These two formulas are the entire kinematic interface of a differential-drive robot.

Worked example, with real numbers

Wheel radius r = 0.05 m (5 cm), wheelbase L = 0.30 m (30 cm). Command the left wheel ω_L = 10 rad/s and the right wheel ω_R = 14 rad/s. Forward speed:

Turn rate:

So these wheel speeds produce a unicycle command of (v, ω) ≈ (0.60 m/s, 0.667 rad/s) — driving forward while curving left, because the right wheel is the faster one. Feed that (v, ω) straight into the Chapter 7 unicycle integrator and you've simulated the differential-drive robot. The two models compose perfectly: wheels → (v, ω) → pose.

The mapping inverts too: given a desired (v, ω) from a controller, solve for the wheel speeds. Adding and subtracting the two equations gives ω_R = (v + ωL/2)/r and ω_L = (v − ωL/2)/r — which is what actually gets sent to the motors. Check it: with v = 0.60, ω = 0.667, L = 0.30, r = 0.05: ω_R = (0.60 + 0.667·0.15)/0.05 = (0.60 + 0.10)/0.05 = 14, ω_L = (0.60 − 0.10)/0.05 = 10. We recover the original wheel speeds. The dictionary works in both directions.

The simple car (bicycle model)

A car can't pivot in place — it steers. The simple car (or bicycle model) has rear wheels that drive at speed v and front wheels that steer by an angle φ (the Greek letter phi). With wheelbase L (front-to-rear distance this time), the heading changes at:

Same ẋ, ẏ as the unicycle — it's still a no-slip ground vehicle — but now the turn rate θ̇ is set by the steering angle φ, not commanded directly. So the car's "ω" is ω = (v/L)tanφ. Two consequences fall out immediately and define why cars are harder to plan for than diff-drives:

• At v = 0 the heading can't change (θ̇ = 0): a car cannot turn while standing still. A diff-drive can pivot in place; a car cannot. That's why three-point turns exist.
• The steering angle φ is mechanically limited (±30° or so), so tanφ is bounded — the car has a minimum turning radius R_min = L/tanφ_max. It cannot turn arbitrarily tightly.

python
import numpy as np

# Differential drive: wheel speeds -> unicycle command (v, omega).
def diffdrive(wL, wR, r=0.05, L=0.30):
    v = r*(wR + wL)/2.0      # forward speed = average
    w = r*(wR - wL)/L        # turn rate = scaled difference
    return v, w

print(diffdrive(10, 14))     # (0.6, 0.667)  forward + left curve

# Inverse: desired (v, w) -> wheel speeds the motors must spin.
def inverse_diffdrive(v, w, r=0.05, L=0.30):
    wR = (v + w*L/2)/r
    wL = (v - w*L/2)/r
    return wL, wR
print(np.round(inverse_diffdrive(0.6, 0.667), 2))  # [10. 14.]  recovered

# Simple car (bicycle): steering angle phi sets the turn rate.
def car_step(xi, v, phi, dt, L=2.5):
    x, y, th = xi
    x  = x  + v*np.cos(th)*dt
    y  = y  + v*np.sin(th)*dt
    th = th + (v/L)*np.tan(phi)*dt   # theta-dot = (v/L) tan(phi)
    return np.array([x, y, th])       # at v=0, theta never changes

Chapter 9: Showcase — Drive a Robot, Trace a Trajectory

This is the payoff. Everything from the last eight chapters is wired into one simulator: a robot whose pose ξ = (x, y, θ) you drive in real time. You command it two ways — directly as a unicycle (v, ω), or as a differential drive (ω_L, ω_R) that gets mapped to (v, ω) using Chapter 8's formulas. Either way the same forward-Euler integrator from Chapter 7 advances the configuration and traces the path.

Try the three signature moves and watch the kinematics produce them:

• Straight line: in unicycle mode set ω = 0; in diff-drive mode set ω_L = ω_R. The no-slip constraint guarantees motion stays along the heading.
• Perfect circle: hold v and ω both nonzero and constant — the arc closes into a circle of radius v/ω. Larger v/ω = wider circle.
• Pivot in place: only possible in diff-drive with ω_L = −ω_R (or unicycle v = 0) — the robot rotates without translating. A simple car can't do this.

Chapter 10: Into 3D & the Series Map — SE(3) Preview, Connections, Cheat-Sheet

Everything so far lived on a flat floor: one rotation angle, two translation components, the group SE(2). The instant a robot leaves the ground — a drone, a manipulator arm, a camera tilting to look up — the same ideas scale up, and the bookkeeping grows.

What changes in 3D

• Rotation becomes 3×3. In 2D a rotation is one number, θ, and one matrix R(θ) ∈ SO(2). In 3D it takes three angles (roll, pitch, yaw — or an axis-and-angle), and the rotations form SO(3): 3×3 orthogonal matrices with determinant +1. Same properties (R^⊤R = I, R⁻¹ = R^⊤), one more dimension.
• 3D rotations don't commute. Roll-then-pitch ≠ pitch-then-roll — a real gotcha that 2D never has (2D rotations always commute). This is why engineers often store orientation as quaternions (4 numbers) instead of angles: they avoid the "gimbal lock" singularities that plague roll-pitch-yaw.
• The rigid transform becomes 4×4. SE(2) was a 3×3 matrix (2×2 rotation + 2 translation + bookkeeping row). In 3D it's SE(3): a 4×4 matrix with a 3×3 rotation block, a 3-vector translation, and a (0,0,0,1) bottom row. Pose is now 6 numbers (3 position + 3 orientation). Composition is still just matrix multiplication, subscripts still cancel.

Cheat-sheet — every formula in this lesson

Where this sits on the autonomy stack

Related lessons on Engineermaxxing

• Lesson 1: See-Think-Act & the Autonomy Stack — the map this lesson plugs into.
• Robotics Simulation — kinematics, dynamics, and the integrators that power the showcase.
• 3D Geometry (VNAV) — SO(3)/SE(3) in full, rotation representations, the 3D version of everything here.
• Lie Groups (VNAV) — the deep structure behind SO(2)/SO(3)/SE(3) and why exponentials connect velocities to transforms.
• Inverse Kinematics — chains of SE transforms for robot arms.