From a Polyline to a Trajectory the Robot Can Actually Drive

Chapter 1: Path vs. Trajectory — the Clock Is the Difference

The two words sound interchangeable in everyday speech, but in robotics they are precisely different, and the difference is the whole point of this lesson. Let's pin both down.

A path is a geometric object: a curve in space, a set of positions with no notion of time. "Drive along this arc, then this line." It answers where, never when. Formally it's a function of a path parameter — often arc length s — written ξ(s). Slide s from 0 to its end and you sweep out the shape, but nothing tells you how fast.

A trajectory is a path with a clock bolted on: a function of time that says where the robot is at every instant. We write it

Read this as: "ξ is a function that takes a time t (a real number) and returns the robot's full configuration at that time — here a point in ℝ³." Here n is the dimension of the configuration (3 for our wheeled robot: x, y, heading θ). Because ξ(t) is a function of time, we can differentiate it — and that derivative is the velocity, exactly what the motors need.

What boundary conditions does a trajectory need?

To specify a trajectory we don't list every point — that's infinitely many numbers. Instead we pin down its boundary conditions: the values it must take at the start and end. For a wheeled robot a complete boundary spec is:

• Start pose (x₀, y₀, θ₀) — where it begins and which way it faces.
• Start velocity V₀ — how fast it's already moving (often you launch from rest, V₀=0, or from a cruising speed).
• Goal pose (x_f, y_f, θ_f) — where it must end and facing where.
• Goal velocity V_f — arriving at rest? at speed? mid-maneuver?
• Total time t_f (often written T) — the duration of the motion.

That's it. A handful of numbers at the two ends. The job of trajectory generation is to interpolate a smooth curve through them — smooth enough that the robot's wheels can produce it. Notice the velocity boundary conditions: a path never mentions velocity, but a trajectory must, because a controller hand-off ("start tracking at speed 0.5 m/s, heading south") only makes sense if the trajectory was built to honor it.

Play with the boundary conditions below. Each handle sets a start/goal position; the heading arrows set θ at the ends. The curve redraws to honor all of them — that's the interpolation problem we'll solve by hand in the next chapter.

Chapter 2: Polynomial Trajectories — a Cubic by Hand

We need a smooth function of time that hits our boundary conditions. The simplest, most useful choice is a polynomial in t. Each coordinate of the robot gets its own polynomial; we'll do one coordinate now and the rest are identical.

A polynomial is smooth (infinitely differentiable), easy to differentiate to get velocity, and — crucially — linear in its coefficients, which turns "find the curve" into "solve a linear system." We parameterize a coordinate x(t) as a weighted sum of basis functions ψ_i(t):

The textbook's choice of basis is the monomials: ψ₀=1, ψ₁=t, ψ₂=t², ψ₃=t³. So

The unknowns are the four coefficients a₀, a₁, a₂, a₃. Why exactly four? Because we have four boundary conditions to honor on this one coordinate: position and velocity at both ends. Four conditions, four unknowns — a square system. The velocity is the time derivative, which for a polynomial is trivial:

Worked example with real numbers

Let's design one coordinate concretely. Suppose we want, on a horizon T = 2 seconds:

"Start at 0 moving at speed 1, end at 4 having coasted to a stop." Plug each condition into the polynomial. This is the entire method — watch every step.

Condition 1 — x(0)=0. Set t=0: every term with a t vanishes, leaving

Condition 2 — &xdot;(0)=1. Set t=0 in the velocity: again everything with a t dies, leaving

So the first two coefficients fall out for free — the t=0 conditions always read off a₀ and a₁ directly. The remaining two come from the t=T end.

Condition 3 — x(2)=4. Plug t=2 (and the known a₀=0, a₁=1):

Condition 4 — &xdot;(2)=0. Plug t=2 into the velocity (a₁=1):

Solve the 2×2 that remains. Subtract the first from the second to cancel a₂:

Back-substitute into 4a₂ + 8a₃ = 2:

The trajectory. Assembling all four coefficients:

Sanity check the goal: x(2) = 2 + 8 − 6 = 4 ✓, and &xdot;(2) = 1 + 8 − 9 = 0 ✓. It starts at 0 with speed 1, ends at 4 at rest. We just designed a motion with arithmetic.

The same thing as a 4×4 linear system

Doing it by elimination is fine for two unknowns, but the general recipe writes all four conditions as one matrix equation M·a = b. Each row evaluates a basis (or its derivative) at an endpoint. With basis [1, t, t², t³] and its derivative [0, 1, 2t, 3t²] at t=0 and t=T:

With T=2 and the right-hand side b = [0, 1, 4, 0]^⊤, solving this 4×4 gives back exactly a = [0, 1, 2, −0.75]^⊤ — the same coefficients we found by hand. The top-left 2×2 being the identity is why a₀ and a₁ read off directly.

From scratch, in Python

The hand derivation maps line-for-line onto code. Build M and b, then solve.

python
import numpy as np

def cubic_coeffs(x0, xdot0, xf, xdotf, T):
    # Solve for [a0,a1,a2,a3] so that x(t)=a0+a1 t+a2 t^2+a3 t^3
    # matches position+velocity at t=0 and t=T.
    M = np.array([
        [1, 0, 0,        0       ],   # x(0)   = a0
        [0, 1, 0,        0       ],   # xdot(0)= a1
        [1, T, T**2,    T**3    ],   # x(T)
        [0, 1, 2*T,     3*T**2  ],   # xdot(T)
    ], dtype=float)
    b = np.array([x0, xdot0, xf, xdotf], dtype=float)
    return np.linalg.solve(M, b)

a = cubic_coeffs(0, 1, 4, 0, T=2)
print(a)            # [ 0.    1.    2.   -0.75]  -- matches the hand solve

def eval_poly(a, t):
    return a[0] + a[1]*t + a[2]*t**2 + a[3]*t**3
print(eval_poly(a, 2))  # 4.0

And the idiomatic one-liner once you trust the pattern — NumPy can evaluate the polynomial and its derivative for you:

python
# a is in increasing-power order [a0,a1,a2,a3]; np.polyval wants decreasing power
p   = np.polynomial.Polynomial(a)       # increasing-power, clean
pos = p(2.0)                            # 4.0
vel = p.deriv()(2.0)                     # 0.0   (xdot at T)

What about cubic vs. quintic?

A cubic has four coefficients, so it pins down four conditions: position and velocity at each end. If you also care about acceleration at the ends (smooth start/stop with no jerk in the force), you need two more conditions, hence two more coefficients — a degree-5 polynomial, the quintic:

Same recipe, bigger matrix: six conditions (position, velocity, acceleration at both ends) → a 6×6 solve. The rule is mechanical: each derivative you constrain at each endpoint costs one more coefficient. 2 ends × (pos, vel) = 4 → cubic. 2 ends × (pos, vel, accel) = 6 → quintic.

Chapter 3: Why a Smooth Curve Still Isn't Enough — Nonholonomy

We can now fit a beautiful smooth cubic through any boundary conditions. So we're done — just fit a curve and drive it? No. There's a subtler trap, and it's the deepest idea in wheeled-robot motion. The curve might be smooth and still be physically impossible for the robot to follow.

Recall the unicycle model from Lesson 2 — the simplest wheeled robot. Its motion obeys

where V is the forward speed and ω the turning rate — the two things you actually command. Stare at the first two equations. They say the velocity vector (&xdot;, &ydot;) always points along the heading θ. The robot can only ever move in the direction it faces. It cannot slide sideways. Ever.

This is the nonholonomic constraint — "rolling without side-slip." A wheel can roll forward or pivot, but it cannot translate along its own axle. Written as an equation, the sideways velocity is forced to zero:

So an arbitrary smooth (x(t), y(t)) is not automatically drivable. We need the heading θ(t) and the turn rate ω(t) it implies to be ones the robot can actually produce, in step with where it's pointing. The naive approach — fit x and y independently and hope — can produce a curve no wheeled robot on Earth can track.

Try it below: drag the curve into a shape and watch the required heading arrows. When the curve doubles back or makes a hairpin, the heading has to swing violently — demanding a turn rate ω the robot can't hit. The red flag means "infeasible: this needs a turn faster than ω_max."

So the question becomes: how do we generate a smooth curve that is guaranteed to be consistent with the robot's dynamics? If we could pick a few variables to be smooth and have the heading, speed, and turn rate fall out automatically — always feasible by construction — we'd be done. That is exactly what differential flatness gives us. It's the hero of this lesson, and it's next.

Chapter 4: Differential Flatness — the Idea That Makes It Easy

Here is the beautiful idea, due to Fliess and made practical for robots by Van Nieuwstadt and Murray. For a special class of systems, there exists a magic choice of outputs — the flat outputs — such that every state and every input of the system can be written as an algebraic function of those outputs and a finite number of their time derivatives. No integration. No solving differential equations. Just plug in derivatives.

Why does this matter so much? Because if the position (x, y) is a valid flat output, then any smooth curve we draw for (x(t), y(t)) — like the cubics from Chapter 2 — automatically defines a heading, speed, and turn rate that satisfy the dynamics exactly. The infeasibility worry from Chapter 3 evaporates: feasibility is guaranteed by construction (as long as we avoid one singularity, Chapter 6). We get to design in the easy space (just draw x and y) and the hard variables follow.

The unicycle is flat — let's prove it

Claim: the unicycle's flat output is its position z = (x, y). We'll recover θ, V, and ω from x, y and their derivatives, using nothing but algebra. Start from the model:

Step 1 — recover the speed V. Square (i) and (ii) and add them. Using cos²θ + sin²θ = 1:

The speed is just the magnitude of the velocity vector — obvious in hindsight, but now it's derived purely from the flat output's first derivatives.

Step 2 — recover the heading θ. Divide (ii) by (i): the V's cancel, leaving the slope of the velocity vector.

(We use atan2, the two-argument arctangent, instead of plain arctan(&ydot;/&xdot;) so the heading lands in the correct quadrant for all directions, not just the right half-plane.) The robot's heading is simply the direction its position is currently moving — which makes total sense for a robot that can only go where it points.

Step 3 — recover the turn rate ω. This is the only one needing a second derivative. We want ω = &thetadot;. Since θ = atan2(&ydot;, &xdot;), differentiate. The derivative of atan2(g, f) with respect to time is (f·ġ − g·&fdot;)/(f² + g²). Here f = &xdot; so &fdot; = &xddot;, and g = &ydot; so ġ = &yddot;:

Some books write the numerator as &xddot;&ydot; − &yddot;&xdot; with the opposite sign convention — it's the same magnitude, just a sign choice for turn direction. The denominator is V² again. So the turn rate is the cross product of velocity and acceleration, normalized by speed squared — precisely the signed curvature times speed, which is what a turn rate is.

Recover the inputs in code

python
import numpy as np

def flat_to_state_input(xd, yd, xdd, ydd):
    # xd,yd = first derivs of flat output; xdd,ydd = second derivs.
    # Returns (theta, V, omega) for the unicycle, all algebraic.
    V2    = xd*xd + yd*yd
    V     = np.sqrt(V2)
    theta = np.arctan2(yd, xd)                 # quadrant-correct heading
    omega = (xd*ydd - yd*xdd) / V2             # cross(vel, acc) / V^2
    return theta, V, omega

Three lines turn a position curve into a fully feasible robot trajectory. This little function is, quite literally, the heart of the entire homework problem for this lecture — and of countless real trajectory generators.

Below, draw a position curve and watch θ, V, and ω pop out at any point you hover.

Chapter 5: Designing a Flat Trajectory — End to End with Numbers

Now we assemble the whole pipeline. The recipe (straight from the lecture notes) is three steps:

Step 1 — translate pose+velocity into flat-output conditions

The boundary spec is given in robot terms: start/end position, heading, and speed. We must convert these into conditions on x, y and their derivatives, because that's what the polynomial fit consumes. The bridge is the flatness map run backwards: from &xdot; = V cosθ and &ydot; = V sinθ,

So a start heading and speed become a start velocity vector for the flat output. Let's use this homework-style spec:

Convert the ends. At θ₀ = −π/2 (pointing straight down, in −y): cos(−π/2)=0, sin(−π/2)=−1. So

And identically at the end (same heading and speed): &xdot;(T)=0, &ydot;(T)=−0.5. Now we have four conditions per coordinate — exactly a cubic each.

Step 2 — fit the two cubics

x-coordinate. Conditions: x(0)=0, &xdot;(0)=0, x(15)=5, &xdot;(15)=0. From t=0: a₀=0, a₁=0. From t=15:

From the velocity equation a₂ = −22.5 a₃. Substitute: 225(−22.5 a₃) + 3375 a₃ = 5 → −5062.5 a₃ + 3375 a₃ = 5 → −1687.5 a₃ = 5 → a₃ ≈ −0.002963, and a₂ ≈ 0.06667. So

y-coordinate. Conditions: y(0)=0, &ydot;(0)=−0.5, y(15)=5, &ydot;(15)=−0.5. From t=0: b₀=0, b₁=−0.5. From t=15:

Velocity gives b₂ = −22.5 b₃; substituting as before: −1687.5 b₃ = 12.5 → b₃ ≈ −0.007407, b₂ ≈ 0.16667. So

Step 3 — recover the inputs at a sample time

Let's evaluate everything at t = 7.5 (the midpoint). First the derivatives. For x: &xdot; = 0.13333 t − 0.008889 t², &xddot; = 0.13333 − 0.017778 t.

For y: &ydot; = −0.5 + 0.33333 t − 0.022222 t², &yddot; = 0.33333 − 0.044444 t.

Now the flatness formulas at t=7.5:

So at the midpoint the robot is moving at about 0.90 m/s, heading up-and-right at 56°, and (because both accelerations happen to vanish there) momentarily not turning. Every number came from evaluating polynomials and plugging into three formulas — no differential equation was ever solved. That is differential flatness doing its job.

The whole pipeline, end to end, in code:

python
import numpy as np

def cubic_coeffs(p0, v0, pf, vf, T):
    M = np.array([[1,0,0,0],[0,1,0,0],
                  [1,T,T**2,T**3],[0,1,2*T,3*T**2]], float)
    return np.linalg.solve(M, np.array([p0,v0,pf,vf], float))

T = 15.0; V0 = Vf = 0.5; th0 = thf = -np.pi/2
# Step 1: pose+speed -> flat-output velocity boundary conditions
ax = cubic_coeffs(0, V0*np.cos(th0), 5, Vf*np.cos(thf), T)   # x(t)
ay = cubic_coeffs(0, V0*np.sin(th0), 5, Vf*np.sin(thf), T)   # y(t)

def deriv(a, t, k):                 # k-th derivative of the cubic at t
    c = np.polynomial.Polynomial(a)
    for _ in range(k): c = c.deriv()
    return c(t)

t = 7.5
xd, yd   = deriv(ax,t,1), deriv(ay,t,1)
xdd, ydd = deriv(ax,t,2), deriv(ay,t,2)
V     = np.hypot(xd, yd)
theta = np.arctan2(yd, xd)
omega = (xd*ydd - yd*xdd) / (xd*xd + yd*yd)
print(round(V,3), round(theta,3), round(omega,3))  # 0.901 0.983 0.0

Chapter 6: Singularities & Feasibility — When the Magic Breaks

Differential flatness looked too good. There's a catch, and it lives right in the denominators. Look again at the recovery formulas:

What happens when the robot stops — when V = √(&xdot;²+&ydot;²) = 0? The denominator of ω becomes zero. ω is undefined. And θ = atan2(0, 0) is also undefined — if you're not moving, your direction of motion is meaningless. This is the flatness singularity at V = 0.

The fix in practice: keep V > 0 throughout the trajectory — never plan a stop in the middle of a flat segment, and give the endpoints a small nonzero speed. If you genuinely must stop, you stitch trajectories: stop is its own segment handled by a different controller, then a fresh flat segment launches from a small speed.

Two flavors of feasibility limit

Even away from the singularity, the recovered inputs must respect the robot's physical limits. Two show up constantly:

• Speed limit |V(t)| ≤ V_max. The wheels can only spin so fast. The homework uses |V| ≤ 0.5 m/s.
• Turn-rate limit |ω(t)| ≤ ω_max. The robot can only spin so fast (or, equivalently, its turning radius can't be smaller than some minimum). The homework uses |ω| ≤ 1.0 rad/s.

The trouble is that polynomial coefficients don't obviously control these — you fit for position, and speed/turn-rate are whatever derivatives fall out. So you generate the trajectory, then check. If a limit is violated, the cleanest cure is the elegant idea of time scaling.

Time scaling: same path, slower clock

Here is the key separation. A trajectory bundles two things: a geometric path (the shape on the floor) and a timing law (how fast you move along it). Time scaling says: keep the shape, change only the clock. If you traverse the identical curve but take longer, every velocity shrinks — without changing where the robot goes.

The lecture's clean demonstration: take a 1D path x(s) = x₀ + s(x_f − x₀) for s ∈ [0,1], and drive the parameter s with a smooth cubic timing law that starts and ends at rest:

This satisfies s(0)=0, s(T)=1, ⋅(0)=⋅(T)=0. The maximum speed along the path comes out to

Read that: doubling T halves the peak speed. The geometry is untouched — same start, same end, same shape — but the whole motion is gentler. So if your candidate trajectory's peak speed is, say, 0.9 m/s and your limit is 0.5, you scale time by the ratio (here ~1.8× longer) and the speed drops under the cap while the path stays exactly the same.

Below, watch a candidate trajectory's speed profile against the limit, and stretch T with the slider. When the peak speed dips under V_max, the trajectory turns feasible — same path, slower clock.

Chapter 7: The Optimization View — the Best Trajectory, Not Just a Good One

Flatness gives us a feasible trajectory quickly. But often we want the best one: smoothest, least energy, fastest. That's the realm of trajectory optimization (also called optimal control). Instead of just interpolating boundary conditions, we minimize a cost.

The general optimal control problem, straight from the lecture, looks like this. We choose the state trajectory x(t) and inputs u(t) to minimize a cost while obeying the dynamics and limits:

Decoding the symbols: g is the stage cost (the price you pay at each instant — e.g. control effort), h is the terminal cost (a price on where you end up), and the integral sums the stage cost over the whole motion. The constraint &xdot; = a(x,u) forces the trajectory to obey physics; the set memberships enforce limits.

What cost do we pick?

The cost is the engineering decision — it encodes what "good" means:

• Minimum effort / energy. g = u^⊤u (sum of squared inputs). Penalizes hard accelerations — gentle, battery-friendly motion.
• Minimum time. g = 1 so the integral is just t_f. Get there as fast as possible — the trajectory rides the input limits.
• Minimum snap. Minimize the integral of the squared 4th derivative of position. The famous choice for quadrotors — snap (the derivative of jerk) maps to motor commands, so minimizing it gives the smoothest flyable path.

How is it solved? Discretize, then optimize.

The problem above is infinite-dimensional — the unknown is a whole continuous function. Computers need finitely many numbers. The dominant approach is the direct method: chop time into N points, treat the values x_i, u_i at those points as the unknowns, and approximate the dynamics with a discrete update (e.g. forward Euler x_i+1 = x_i + Δt·a(x_i, u_i)). The cost integral becomes a sum. This turns the calculus problem into a nonlinear program (NLP) — minimize-a-function-over-vectors — that solvers like IPOPT, SNOPT, or CasADi crunch.

The special case that's just a QP

Here's where flatness and optimization shake hands. If you parameterize the flat output as a polynomial (as in Chapter 2), the trajectory is linear in the coefficients. A smoothness cost like "integral of squared acceleration" is then a quadratic function of those coefficients, and the boundary conditions are linear equality constraints. Minimizing a quadratic subject to linear constraints is a quadratic program (QP) — convex, fast, with a unique global minimum:

where a stacks all polynomial coefficients, Q is built from integrals of products of basis derivatives, and M a = b is exactly the boundary-condition system from Chapter 2 (plus rows for any interior waypoints). When you have more coefficients than constraints, the QP picks the smoothest feasible trajectory among the infinitely many that fit. This is the backbone of minimum-snap planning.

Chapter 8: The Trajectory Designer — Build, Drive, Read the Inputs

Time for the payoff. Everything we've built — cubic fitting, flatness recovery, time scaling — lives in one interactive tool. Drag the start and goal poses; the lesson fits a flat cubic trajectory for x(t) and y(t), then drives a robot along it while plotting the recovered speed V(t) and turn rate ω(t) live. A slider for the total time T shows time scaling in action: slower T lowers the required V and ω.

Three things to look for as you play:

• The robot's heading always points along the curve. That's flatness — θ = atan2(&ydot;, &xdot;) is enforced by construction, so the motion is always feasible (away from V=0).
• The V(t) and ω(t) plots are computed, not assumed. They come straight from the flatness formulas applied to the polynomial's derivatives. When a plot pokes above its red limit line, that part of the trajectory is infeasible.
• Stretch T and watch both plots shrink. Same path, gentler inputs — time scaling rescuing an over-aggressive trajectory.

This is the whole lesson in one widget: a planner-style endpoint spec goes in; a smooth, timed, dynamically-feasible trajectory comes out, complete with the exact velocity and turn-rate commands the controller in Lesson 6 will track. You designed it by dragging two dots.

Chapter 9: In the Real World — Receding Horizon & the Hand-off

Our trajectory is computed open-loop: a function of time and the initial condition only, with no feedback. The lecture is blunt about the consequence: "open-loop control laws suffer from robustness issues since they do not make corrections based on real-time observations." If a gust of wind, a wheel slip, or a modeling error nudges the robot off the plan, an open-loop trajectory has no idea and no recourse. It walks to the chair with its eyes closed.

Two practical patterns fix this.

Pattern 1 — the two-step design (reference + tracker)

This is the dominant architecture, and it's exactly the bridge to Lesson 6. Split control into two layers:

The open-loop trajectory provides the nominal motion — what should happen in a perfect world. The closed-loop tracker measures the actual pose, compares it to the reference, and applies corrections to kill the tracking error. The reference says "be here, going this fast"; the tracker makes sure you actually are. Our whole lesson built the reference half. Lesson 6 builds the tracker.

Pattern 2 — receding horizon (MPC)

Model Predictive Control (MPC) takes the idea further: don't compute one long trajectory once. Instead, re-plan continuously. At each instant, solve a short trajectory-optimization problem over a finite look-ahead window (the horizon), execute only the first slice of the resulting plan, then — one tick later, with fresh sensor data — re-solve from the new actual state. The horizon slides forward with time, hence "receding."

So the trajectory you generate in this lesson is never the end of the story — it's the reference that a closed-loop layer (a tracker, or a re-planning MPC loop) consumes to stay robust. Open-loop generation and closed-loop tracking are partners, not rivals.

Chapter 10: Connections — the Cheat-Sheet & What's Next

You now own one of the most useful tricks in robotics: turning a planner's waypoints into a smooth, timed, dynamically-feasible trajectory, almost for free, via differential flatness. Let's consolidate.

The one-page cheat-sheet

Where this sits on the autonomy stack

Trajectory generation is the lower half of the Motion Planning & Control module. It consumes the discrete plan from the search algorithms above it and produces the continuous reference the controller below it tracks:

• Lesson 2 — Rigid-Body Transforms & the Unicycle Model: where the nonholonomic constraint and the &xdot;=Vcosθ, &ydot;=Vsinθ, &thetadot;=ω model come from — the model flatness exploits.
• Lesson 6 — Trajectory Tracking (PID / LQR): the next lesson. It takes the reference trajectory you just learned to build and closes the loop, driving the tracking error to zero despite disturbances.
• Visual Navigation — Trajectory Optimization: a deeper, aerial-robotics take on the same optimal-control machinery (minimum-snap, direct methods, MPC) for drones.