Lattice Trajectory Planning

A stopped car sits in your lane. You could stay behind it, nudge sideways, or change lanes. Each choice is a path through the next few seconds.

This notebook builds a lattice planner that fans out those paths, scores them, and picks one. The failure mode is simple: pick the shortest curve and you drive straight through the car.

Each idea shows up three times: a picture, a handful of numbers, then a few lines of code. Predict first, then run the cell. The paragraph after the cell says what was surprising, and what it would mean for a car.

In [1]

import os, subprocess, sys
from pathlib import Path

import matplotlib
import matplotlib.pyplot as plt
import numpy as np

def keep_inline():
    # Put figures on the notebook backend so plt.show() renders here.
    matplotlib.use("module://matplotlib_inline.backend_inline", force=True)
    ip = None
    try:
        from IPython import get_ipython
        ip = get_ipython()
    except Exception:
        ip = None
    if ip is not None:
        ip.run_line_magic("matplotlib", "inline")

%matplotlib inline

def find_repo(start: Path) -> Path:
    for p in [start, *start.parents]:
        if (p / "modules" / "06_trajectory_planner" / "lattice_planner.py").is_file():
            return p.resolve()
    return start.resolve()

REPO = find_repo(Path.cwd())
if not (REPO / "modules" / "06_trajectory_planner" / "lattice_planner.py").is_file():
    dest = Path.cwd() / "self-driving-ai-course"
    if not (dest / "modules" / "06_trajectory_planner" / "lattice_planner.py").is_file():
        subprocess.run(
            ["git", "clone", "--depth", "1", "https://github.com/vvknyn/self-driving-ai-course.git", str(dest)],
            check=True,
        )
    REPO = dest.resolve()
    os.chdir(REPO)
else:
    os.chdir(REPO)

MOD = REPO / "modules" / "06_trajectory_planner"
sys.path.insert(0, str(MOD))

from cost_functions import TrajectoryCostEvaluator
from lattice_planner import LatticePlanner, QuinticPolynomial, TrajectoryCandidate

print("Repo:", REPO)
print("module: modules/06_trajectory_planner")
keep_inline()

Output

Repo: /tmp/sdc
module: modules/06_trajectory_planner

1. The destination

A fan of candidate paths starts at the ego car. The geometrically shortest cruise path stays in the center lane and runs through the stopped car at x = 24 m. The scored planner picks a sideways path instead.

You do not need the quintic formulas yet. This picture is where the notebook lands.

Predict: the red curve touches the car. The blue curve ends in a neighboring lane with clearance above zero.

In [2]

ego_state = {"x0": 0.0, "y0": 0.0, "v0": 15.0, "a0": 0.0}
obstacles = [{"x": 24.0, "y": 0.0, "radius": 1.5}]
obstacle = obstacles[0]

planner = LatticePlanner(target_speed=15.0)


def path_length(traj):
    return float(
        sum(
            np.hypot(traj.x[i + 1] - traj.x[i], traj.y[i + 1] - traj.y[i])
            for i in range(len(traj.x) - 1)
        )
    )


def min_clearance(traj, ox=24.0, oy=0.0):
    return float(min(np.hypot(x - ox, y - oy) for x, y in zip(traj.x, traj.y)))


def cruise_traj(d_target, v_target=15.0, T=3.0, dt=0.1, v0=15.0):
    s_target = 0.5 * (v0 + v_target) * T
    lon_poly = QuinticPolynomial(0.0, v0, 0.0, s_target, v_target, 0.0, T)
    lat_poly = QuinticPolynomial(0.0, 0.0, 0.0, d_target, 0.0, 0.0, T)
    traj = TrajectoryCandidate()
    for t in np.arange(0.0, T + 1e-9, dt):
        traj.t.append(float(t))
        traj.x.append(float(lon_poly.calc_point(t)))
        traj.y.append(float(lat_poly.calc_point(t)))
        traj.v.append(float(lon_poly.calc_first_derivative(t)))
        traj.a.append(float(lon_poly.calc_second_derivative(t)))
        traj.jerk.append(float(lon_poly.calc_third_derivative(t)))
    return traj


fan = planner.generate_candidate_trajectories(
    ego_state,
    lateral_offsets=[-3.5, -1.75, 0.0, 1.75, 3.5],
    planning_horizon=3.0,
    dt=0.1,
)
cruise15 = [t for t in fan if abs(t.v[-1] - 15.0) < 1e-6]
cruise15.sort(key=path_length)
shortest = cruise15[0]
center = next(t for t in cruise15 if abs(t.y[-1]) < 1e-6)

fan_runner = planner.generate_candidate_trajectories(
    ego_state,
    lateral_offsets=[-3.5, -2.0, 0.0, 2.0, 3.5],
    planning_horizon=3.0,
    dt=0.1,
)
runner_eval = TrajectoryCostEvaluator(w_collision=1000.0, w_lane_center=3.0, w_jerk=0.2)
chosen = runner_eval.select_optimal_trajectory(
    fan_runner, obstacles, target_lane_y=0.0, target_speed=15.0
)

print(f"candidates in fan: {len(fan)}")
print(
    f"shortest at 15 m/s: end y={shortest.y[-1]:.2f} length={path_length(shortest):.3f} "
    f"clearance={min_clearance(shortest):.3f}"
)
print(
    f"costs pick: end y={chosen.y[-1]:.2f} end v={chosen.v[-1]:.1f} total={chosen.total_cost:.3f} "
    f"clearance={min_clearance(chosen):.3f}"
)

fig, ax = plt.subplots(figsize=(8, 3.6))
for traj in cruise15:
    ax.plot(traj.x, traj.y, color="0.78", lw=1.0, alpha=0.95)
ax.plot(
    center.x,
    center.y,
    color="C3",
    lw=2.0,
    label=f"shortest end y={center.y[-1]:.2f}",
)
ax.plot(
    chosen.x,
    chosen.y,
    color="C0",
    lw=2.2,
    label=f"costs pick end y={chosen.y[-1]:.2f}",
)
ax.add_patch(plt.Circle((obstacle["x"], obstacle["y"]), obstacle["radius"], fill=False, color="black"))
ax.scatter([obstacle["x"]], [obstacle["y"]], c="black", s=30, zorder=4)
ax.set_aspect("equal", adjustable="datalim")
ax.set_xlabel("x (m)  along the road")
ax.set_ylabel("y (m)  sideways")
ax.legend(loc="best", fontsize=8)
ax.set_title("Shortest path through the car vs the scored winner")
plt.tight_layout()
plt.show()

Output

candidates in fan: 15
shortest at 15 m/s: end y=0.00 length=45.000 clearance=0.000
costs pick: end y=-3.50 end v=11.0 total=5708.800 clearance=2.183

The fan has 15 candidates. The shortest cruise path ends at y=0.00 with length 45.000 m and clearance 0.000 m. That is a drive through the obstacle center.

The scored pick ends at y=-3.50 and 11.0 m/s with total 5708.800 and clearance 2.183 m. Same road, same car, different rule. Length alone would crash. Costs are reasons to say no, and they move the car sideways.

2. Many possible futures

Five sideways targets and three end speeds make fifteen paths. They share the same start. None of them has a score yet.

Predict: the paths that finish at the same speed also finish at the same distance along the road.

In [3]

ego = {"x0": 0.0, "y0": 0.0, "v0": 15.0, "a0": 0.0}
offsets = [-3.5, -1.75, 0.0, 1.75, 3.5]
futures = planner.generate_candidate_trajectories(
    ego,
    lateral_offsets=offsets,
    planning_horizon=3.0,
    dt=0.1,
)
cruise = {}
for traj in futures:
    if abs(traj.v[-1] - 15.0) < 1e-6:
        cruise[round(traj.y[-1], 2)] = traj

print(f"candidates: {len(futures)}")
print("car ahead x=24.0 y=0.0")
for key in sorted(cruise):
    traj = cruise[key]
    print(f"end y={traj.y[-1]:.2f}  end x={traj.x[-1]:.2f}  end v={traj.v[-1]:.1f}")

fig, ax = plt.subplots(figsize=(8, 3.4))
for key in sorted(cruise):
    traj = cruise[key]
    ax.plot(traj.x, traj.y, label=f"end y={traj.y[-1]:.2f}")
ax.scatter([24.0], [0.0], c="black", s=36, zorder=3)
ax.annotate("car ahead", (24.0, 0.0), textcoords="offset points", xytext=(6, 8))
ax.set_xlabel("x (m)  along the road")
ax.set_ylabel("y (m)  sideways")
ax.set_aspect("equal", adjustable="datalim")
ax.legend(loc="best", fontsize=8)
ax.set_title("Five futures at 15 m/s")
plt.tight_layout()
plt.show()

Output

candidates: 15
car ahead x=24.0 y=0.0
end y=-3.50  end x=45.00  end v=15.0
end y=-1.75  end x=45.00  end v=15.0
end y=0.00  end x=45.00  end v=15.0
end y=1.75  end x=45.00  end v=15.0
end y=3.50  end x=45.00  end v=15.0

candidates: 15. The five curves at 15.0 m/s all end at end x=45.00. Their sideways ends are -3.50, -1.75, 0.00, 1.75, and 3.50. The car ahead sits at x=24.0 y=0.0 on the middle curve.

Along-road distance matched. Sideways position did not. A planner that only minimizes length will stay on the middle line and hit the car.

3. Frenet: along the road, and sideways

sideways d
  +3.5 | . . . . .   left lane
   0.0 | . . car .   center lane
  -3.5 | . . . . .   right lane
       +---------->  along the road, s

On a straight road pointing along +x, s is x and d is y. Rotate the road and the same pair lands somewhere else:

x = s cos θ − d sin θ

y = s sin θ + d cos θ

Predict: s = 10 m and d = 3.5 m. Heading 0 (road along +x) should put that point at x = 10, y = 3.5. Heading 90° (road along +y) points forward along +y and left along −x.

In [4]

def road_xy(s, d, heading_rad):
    x = s * np.cos(heading_rad) - d * np.sin(heading_rad)
    y = s * np.sin(heading_rad) + d * np.cos(heading_rad)
    return float(x), float(y)

s, d = 10.0, 3.5
xy0 = road_xy(s, d, 0.0)
xy90 = road_xy(s, d, np.pi / 2)
print(f"s={s:.1f} d={d:.1f}")
print(f"heading 0 deg   -> x={xy0[0]:.4f} y={xy0[1]:.4f}")
print(f"heading 90 deg  -> x={xy90[0]:.4f} y={xy90[1]:.4f}")
print("planner stores x along the road and y sideways (heading 0)")

Output

s=10.0 d=3.5
heading 0 deg   -> x=10.0000 y=3.5000
heading 90 deg  -> x=-3.5000 y=10.0000
planner stores x along the road and y sideways (heading 0)

At heading 0, s=10.0 and d=3.5 land at x=10.0000 and y=3.5000. At heading 90 they land at x=-3.5000 and y=10.0000. The lane lines in the sketch are the same sideways offsets the fan used: +3.5, 0.0, and -3.5.

This planner keeps the heading-0 case. It writes along-road position into x and sideways position into y on a straight road.

4. A smooth lane change

Picture one move from the center lane to the left lane over three seconds. Speed along the road stays near 15 m/s. Sideways motion starts and ends at rest so passengers do not feel a snap.

Predict: halfway through time, the car is halfway sideways between the lanes.

In [5]

T = 3.0
lat = QuinticPolynomial(0.0, 0.0, 0.0, 3.5, 0.0, 0.0, T)
lon = QuinticPolynomial(0.0, 15.0, 0.0, 45.0, 15.0, 0.0, T)
ts = np.linspace(0.0, T, 301)
d_of_t = np.array([lat.calc_point(float(t)) for t in ts])
s_of_t = np.array([lon.calc_point(float(t)) for t in ts])
peak_jerk = max(abs(lat.calc_third_derivative(float(t))) for t in ts)

print(f"d(0)={lat.calc_point(0.0):.3f}  d(1.5)={lat.calc_point(1.5):.3f}  d(3)={lat.calc_point(T):.3f}")
print(f"s(0)={lon.calc_point(0.0):.3f}  s(1.5)={lon.calc_point(1.5):.3f}  s(3)={lon.calc_point(T):.3f}")
print(
    "lateral speed at ends: "
    f"{lat.calc_first_derivative(0.0):.3f} and {lat.calc_first_derivative(T):.3f}"
)
print(f"peak |lateral jerk|={peak_jerk:.3f}")

fig, axes = plt.subplots(1, 2, figsize=(9, 3.2))
axes[0].plot(ts, s_of_t)
axes[0].set_xlabel("t (s)")
axes[0].set_ylabel("s (m)")
axes[0].set_title("along the road")
axes[1].plot(ts, d_of_t)
axes[1].axhline(1.75, color="0.55", ls="--", lw=1)
axes[1].set_xlabel("t (s)")
axes[1].set_ylabel("d (m)")
axes[1].set_title("sideways")
plt.tight_layout()
plt.show()

Output

d(0)=0.000  d(1.5)=1.750  d(3)=3.500
s(0)=0.000  s(1.5)=22.500  s(3)=45.000
lateral speed at ends: 0.000 and -0.000
peak |lateral jerk|=7.778

Halfway in time is halfway sideways: d(1.5)=1.750 on the way to d(3)=3.500. End lateral speeds are 0.000 and -0.000 (a rounded zero). Along the road, s(1.5)=22.500 and s(3)=45.000. Peak sideways jerk is 7.778.

Six numbers at the start and end pick one quintic polynomial (powers t⁰ through t⁵). That is how each candidate in the fan is built. This curve has not looked at the car yet.

5. The shortest path hits the car

First attempt: keep the shortest of three cruise paths at 15 m/s. One stays in lane, one ends 1.75 m to the left, one ends a full 3.5 m lane to the left. The stopped car is at x = 24 m, y = 0, radius 1.5 m.

Predict: the straight path is the shortest. Its closest point sits inside the car body.

In [6]

names = [("straight", 0.0), ("half lane", 1.75), ("full lane", 3.5)]
ranked = []
print(f"obstacle x={obstacle['x']:.1f} y={obstacle['y']:.1f} radius={obstacle['radius']:.1f}")
for name, d_target in names:
    traj = cruise_traj(d_target)
    length = path_length(traj)
    clearance = min_clearance(traj)
    ranked.append((length, name, traj, clearance))
    print(f"{name:10} length={length:.3f} m  min clearance={clearance:.3f} m  end y={traj.y[-1]:.2f}")

ranked.sort()
shortest_row = ranked[0]
by_name = {name: (length, clearance) for length, name, _, clearance in ranked}
extra = by_name["full lane"][0] - by_name["straight"][0]
print(f"shortest: {shortest_row[1]}  length={shortest_row[0]:.3f} m  clearance={shortest_row[3]:.3f} m")
print(f"extra length of full lane: {extra:.3f} m")
print("hits the car:", shortest_row[3] < obstacle["radius"])

fig, ax = plt.subplots(figsize=(8, 3.4))
for _length, name, traj, _clearance in ranked:
    ax.plot(traj.x, traj.y, label=name)
ax.add_patch(plt.Circle((obstacle["x"], obstacle["y"]), obstacle["radius"], fill=False, color="black"))
ax.set_aspect("equal", adjustable="datalim")
ax.set_xlabel("x (m)")
ax.set_ylabel("y (m)")
ax.legend(loc="best", fontsize=8)
ax.set_title("Shortest path vs the car")
plt.tight_layout()
plt.show()

Output

obstacle x=24.0 y=0.0 radius=1.5
straight   length=45.000 m  min clearance=0.000 m  end y=0.00
half lane  length=45.049 m  min clearance=0.984 m  end y=1.75
full lane  length=45.193 m  min clearance=1.968 m  end y=3.50
shortest: straight  length=45.000 m  clearance=0.000 m
extra length of full lane: 0.193 m
hits the car: True

The straight path is 45.000 m long with clearance 0.000 m. The car radius is 1.5 m, so that path runs through the body. The half-lane path is 45.049 m with clearance 0.984 m. The full-lane path is 45.193 m, only 0.193 m longer, with clearance 1.968 m.

Length picked the path that hits. A real car needs another rule.

6. Costs are reasons to say no

Each cost term is one reason to reject a path. Collision grows inside a safety bubble. Lane centering punishes sideways drift from y = 0. Jerk punishes harsh longitudinal jerk. Speed punishes missing the target cruise speed.

Predict: the straight path's collision term swamps its total. Among the three cruise paths, the full-lane path has the smallest total, even though it still pays some collision cost inside the bubble.

In [7]

evaluator = TrajectoryCostEvaluator()
print(f"w_collision={evaluator.w_coll:.1f}")
print(f"w_lane_center={evaluator.w_lane:.1f}")
print(f"w_jerk={evaluator.w_jerk:.1f}")
print(f"w_speed_progress={evaluator.w_speed:.1f}")
print(f"safety_radius={evaluator.safety_radius:.1f}")
r_eff = obstacle["radius"] + evaluator.safety_radius
print(f"r_eff={r_eff:.1f}")

scored = []
for name, d_target in names:
    traj = cruise_traj(d_target)
    evaluator.score_trajectory(traj, [obstacle], target_lane_y=0.0, target_speed=15.0)
    costs = traj.costs
    scored.append((traj.total_cost, name))
    print(
        f"{name:10} collision={costs['collision']:.3f}  lane={costs['lane_centering']:.3f}  "
        f"jerk={costs['jerk_comfort']:.3f}  speed={costs['speed_progress']:.3f}  "
        f"total={traj.total_cost:.3f}"
    )
scored.sort()
print(f"lowest total: {scored[0][1]}  total={scored[0][0]:.3f}")

Output

w_collision=500.0
w_lane_center=2.0
w_jerk=0.5
w_speed_progress=1.0
safety_radius=2.2
r_eff=3.7
straight   collision=1091252.950  lane=0.000  jerk=0.000  speed=0.000  total=1091252.950
half lane  collision=65825.197  lane=2.421  jerk=0.000  speed=0.000  total=65827.618
full lane  collision=4393.216  lane=9.684  jerk=0.000  speed=0.000  total=4402.900
lowest total: full lane  total=4402.900

Default weights are w_collision=500.0, w_lane_center=2.0, w_jerk=0.5, and w_speed_progress=1.0, with safety_radius=2.2. The bubble radius is r_eff=3.7.

Straight total 1091252.950 is almost entirely collision 1091252.950. Half lane totals 65827.618 (collision 65825.197, lane 2.421). Full lane totals 4402.900 (collision 4393.216, lane 9.684). Jerk and speed stay 0.000 on all three because this cruise holds speed.

The lowest total is full lane at 4402.900. Even the winner pays collision 4393.216 because the bubble is soft, not a hard cutoff. The score moved the choice sideways.

7. The repo planner picks one

modules/06_trajectory_planner/run_planner.py builds the fan and scores it with its own weights: collision 1000, lane 3, jerk 0.2. Sideways targets are −3.5, −2, 0, 2, and 3.5 m. End speeds are 4 m/s under target, target, and 2 m/s over. Target speed here is 15 m/s. The stopped car is the same one.

Predict: the chosen path ends off center. A mirror path can tie it.

In [8]

candidates = fan_runner
print(f"sampled: {len(candidates)}")
print(f"w_collision={runner_eval.w_coll:.1f}")
print(f"w_lane_center={runner_eval.w_lane:.1f}")
print(f"w_jerk={runner_eval.w_jerk:.1f}")
print(f"w_speed_progress={runner_eval.w_speed:.1f}")

best = runner_eval.select_optimal_trajectory(
    candidates, obstacles, target_lane_y=0.0, target_speed=15.0
)
print(f"{'rank':<6}{'end_y':<8}{'end_v':<8}{'collision':<12}{'total':<12}{'clearance':<10}decision")
for i, cand in enumerate(candidates[:6]):
    decision = "chosen" if i == 0 else "rejected"
    clear = min_clearance(cand)
    print(
        f"{i + 1:<6}{cand.y[-1]:<8.2f}{cand.v[-1]:<8.1f}{cand.costs['collision']:<12.3f}"
        f"{cand.total_cost:<12.3f}{clear:<10.3f}{decision}"
    )

center = next(
    c for c in candidates if abs(c.y[-1]) < 1e-6 and abs(c.v[-1] - 15.0) < 1e-6
)
center_clear = min_clearance(center)
print(
    f"center {center.y[-1]:.2f} {center.v[-1]:.1f} {center.costs['collision']:.3f} "
    f"{center.total_cost:.3f} {center_clear:.3f} rejected"
)
print(
    f"chosen end y={best.y[-1]:.2f} end v={best.v[-1]:.1f} "
    f"total={best.total_cost:.3f} clearance={min_clearance(best):.3f}"
)

fig, ax = plt.subplots(figsize=(8, 3.4))
for cand in candidates:
    ax.plot(cand.x, cand.y, color="0.82", lw=0.7, alpha=0.9)
ax.plot(center.x, center.y, color="C3", label=f"rejected end y={center.y[-1]:.2f}")
ax.plot(best.x, best.y, color="C0", lw=2.2, label=f"chosen end y={best.y[-1]:.2f}")
ax.add_patch(plt.Circle((24.0, 0.0), 1.5, fill=False, color="black"))
ax.set_aspect("equal", adjustable="datalim")
ax.set_xlabel("x (m)")
ax.set_ylabel("y (m)")
ax.legend(loc="best", fontsize=8)
ax.set_title("All candidates, rejected center, chosen curve")
plt.tight_layout()
plt.show()

Output

sampled: 15
w_collision=1000.0
w_lane_center=3.0
w_jerk=0.2
w_speed_progress=1.0
rank  end_y   end_v   collision   total       clearance decision
1     -3.50   11.0    5687.759    5708.800    2.183     chosen
2     3.50    11.0    5687.759    5708.800    2.183     rejected
3     -3.50   15.0    8786.432    8800.958    1.968     rejected
4     3.50    15.0    8786.432    8800.958    1.968     rejected
5     -3.50   17.0    10175.295   10191.449   1.985     rejected
6     3.50    17.0    10175.295   10191.449   1.985     rejected
center 0.00 15.0 2182505.900 2182505.900 0.000 rejected
chosen end y=-3.50 end v=11.0 total=5708.800 clearance=2.183

sampled: 15 paths. Rank 1 is chosen at end y -3.50, end speed 11.0, collision 5687.759, total 5708.800, clearance 2.183. Rank 2 ends at 3.50 with the same total 5708.800. Mirror paths tie and the sort keeps the one built first.

The center path at end speed 15.0 is rejected with clearance 0.000 and total 2182505.900. The gray fan shows every candidate. The red curve drives through the car. The blue curve is what the car would follow.

Clearance 2.183 m is larger than the center path's 0.000 m, and collision is still 5687.759 of total 5708.800. The same end y -3.50 at 15.0 m/s would pay collision 8786.432, higher than 5687.759, so the slower sample wins.

8. Raise the collision weight

Turn the collision weight down, then put it back. Same fan, different winner.

Predict: a tiny collision weight lets a center path win. A large weight keeps the sideways pick from section 7.

In [9]

def choose(w_collision):
    fan = LatticePlanner(target_speed=15.0).generate_candidate_trajectories(
        ego_state,
        lateral_offsets=[-3.5, -2.0, 0.0, 2.0, 3.5],
        planning_horizon=3.0,
        dt=0.1,
    )
    ev = TrajectoryCostEvaluator(w_collision=w_collision, w_lane_center=3.0, w_jerk=0.2)
    return ev.select_optimal_trajectory(
        fan, obstacles, target_lane_y=0.0, target_speed=15.0
    ), fan

fig, axes = plt.subplots(1, 2, figsize=(9, 3.4))
for ax, w_collision in zip(axes, (0.001, 1000.0)):
    picked, fan = choose(w_collision)
    for cand in fan:
        ax.plot(cand.x, cand.y, color="0.82", lw=0.7, alpha=0.9)
    ax.plot(picked.x, picked.y, color="C0", lw=2.2, label=f"end y={picked.y[-1]:.2f}")
    ax.add_patch(plt.Circle((24.0, 0.0), 1.5, fill=False, color="black"))
    ax.set_aspect("equal", adjustable="datalim")
    ax.set_xlabel("x (m)")
    ax.set_ylabel("y (m)")
    shown = f"{w_collision:.3f}" if w_collision < 1 else f"{w_collision:.1f}"
    ax.set_title(f"w_collision={shown}")
    ax.legend(loc="best", fontsize=8)
plt.tight_layout()
plt.show()

for w_collision in (0.001, 1000.0):
    picked, _fan = choose(w_collision)
    shown = f"{w_collision:.3f}" if w_collision < 1 else f"{w_collision:.1f}"
    clear = min_clearance(picked)
    print(
        f"w_collision={shown} end y={picked.y[-1]:.2f} end v={picked.v[-1]:.1f} "
        f"total={picked.total_cost:.3f} min clearance={clear:.3f} "
        f"coll={picked.costs['collision']:.3f}"
    )

Output

w_collision=0.001 end y=0.00 end v=17.0 total=2.048 min clearance=0.667 coll=0.420
w_collision=1000.0 end y=-3.50 end v=11.0 total=5708.800 min clearance=2.183 coll=5687.759

With w_collision=0.001 the winner ends at y=0.00 and v=17.0. Total is 2.048, clearance is 0.667 m, and collision is only 0.420. Lane and speed terms are willing to drive through the obstacle when collision barely counts.

With w_collision=1000.0 the winner ends at y=-3.50 and v=11.0. Total 5708.800, clearance 2.183, collision 5687.759. The side-by-side plots show the same fan with two different picks. Raising collision weight is what moves the car sideways.

9. Exercises

Exercise — min_clearance. Closest distance from a list of points to one obstacle center. Return that distance in meters. Leave the TODO as it is to use the reference implementation.

In [10]

def min_clearance_student(xs, ys, ox, oy):
    # TODO: minimum distance from the points (xs, ys) to (ox, oy)
    raise NotImplementedError

def min_clearance_reference(xs, ys, ox, oy):
    return float(min(np.hypot(x - ox, y - oy) for x, y in zip(xs, ys)))

def get_min_clearance_fn():
    try:
        min_clearance_student([0.0], [0.0], 0.0, 0.0)
    except NotImplementedError:
        print("Using reference min_clearance (TODO not implemented)")
        return min_clearance_reference
    return min_clearance_student

clear_fn = get_min_clearance_fn()
toy_xs = [0.0, 24.0, 45.0]
toy_ys = [0.0, 0.0, 0.0]
print("toy xs:", toy_xs)
print("toy ys:", toy_ys)
print("obstacle: (24.0, 0.0)")
toy_clear = clear_fn(toy_xs, toy_ys, 24.0, 0.0)
print("min_clearance:", f"{toy_clear:.4f}")
assert abs(toy_clear - 0.0) < 1e-9
print("✅ correct: min_clearance =", round(toy_clear, 4))

Output

Using reference min_clearance (TODO not implemented)
toy xs: [0.0, 24.0, 45.0]
toy ys: [0.0, 0.0, 0.0]
obstacle: (24.0, 0.0)
min_clearance: 0.0000
✅ correct: min_clearance = 0.0

✅ correct: min_clearance = 0.0. The toy points include (24.0, 0.0), the same place as the obstacle, so the minimum distance is 0.0000. The reference function ran because the TODO still raises. Replace the TODO and run the cell again if you want the check to call your function instead.

Solution

return float(min(np.hypot(x - ox, y - oy) for x, y in zip(xs, ys)))

Exercise — quintic_row1. First row of the 3×3 system inside QuinticPolynomial: the coefficients of a₃, a₄, a₅ in the position constraint at time T. Return a tuple (T**3, T**4, T**5). Leave the TODO as it is to use the reference implementation.

In [11]

def quintic_row1_student(T):
    # TODO: coefficients of a3, a4, a5 in p(T)
    raise NotImplementedError

def quintic_row1_reference(T):
    return (T ** 3, T ** 4, T ** 5)

def get_quintic_row1_fn():
    try:
        quintic_row1_student(1.0)
    except NotImplementedError:
        print("Using reference quintic_row1 (TODO not implemented)")
        return quintic_row1_reference
    return quintic_row1_student

row_fn = get_quintic_row1_fn()
row = tuple(float(v) for v in row_fn(3.0))
print("T: 3.0")
print("row1:", row)
assert all(abs(a - b) < 1e-9 for a, b in zip(row, (27.0, 81.0, 243.0)))
print("✅ correct: quintic_row1 =", row)

Output

Using reference quintic_row1 (TODO not implemented)
T: 3.0
row1: (27.0, 81.0, 243.0)
✅ correct: quintic_row1 = (27.0, 81.0, 243.0)

✅ correct: quintic_row1 = (27.0, 81.0, 243.0). For T: 3.0 the first row is (27.0, 81.0, 243.0), which is T³, T⁴, and T⁵. The reference function ran because the TODO still raises.

Solution

return (T ** 3, T ** 4, T ** 5)

The checked-in tests cover the same two ideas: a quintic hits its start and end boundaries, and the planner's chosen path ends off the obstacle.

In [12]

try:
    import pytest  # noqa: F401
except ImportError:
    subprocess.run([sys.executable, "-m", "pip", "install", "-q", "pytest"], check=True)

proc = subprocess.run(
    [sys.executable, "-m", "pytest", "modules/06_trajectory_planner/tests/test_planner.py", "-q"],
    cwd=REPO,
    capture_output=True,
    text=True,
)
print(proc.stdout)
if proc.stderr.strip():
    print(proc.stderr)
print("returncode", proc.returncode)
assert proc.returncode == 0

Output

..                                                                       [100%]
2 passed in 0.06s

returncode 0

2 passed, and returncode 0. The module tests agree with the numbers above.

10. Recap

Five sideways targets and three speeds produce 15 candidate paths. The 15 m/s slice finishes at end x=45.00 with end y -3.50, -1.75, 0.00, 1.75, and 3.50.
Frenet on this straight road: s=10.0 and d=3.5 at heading 0 is x=10.0000, y=3.5000. The same pair at heading 90 is x=-3.5000, y=10.0000.
The lane-change quintic gives d(1.5)=1.750 and d(3)=3.500. Along the road, s(3)=45.000. Peak sideways jerk is 7.778.
The shortest cruise path is 45.000 m with clearance 0.000 m. It runs through the car of radius 1.5 m. The full-lane path is only 0.193 m longer.
Default weights 500.0, 2.0, 0.5, 1.0 and safety radius 2.2 give bubble r_eff=3.7. The full-lane total is 4402.900, and collision is still 4393.216 because clearance 1.968 is inside the bubble.
The repo runner (w_collision=1000.0, w_lane_center=3.0, w_jerk=0.2) chooses end y -3.50 at 11.0 m/s, total 5708.800. The mirror path ties at the same total.
Collision weight 0.001 chooses a center path with clearance 0.667 m. Weight 1000.0 chooses end y -3.50.
Module tests: 2 passed.

Go deeper

Werling, Ziegler, Kammel, and Thrun — Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame (ICRA 2010)

Module 07 takes the chosen (x, y) samples and turns them into a steering angle and a speed command.

Lattice Trajectory Planning

Tutorial

1. The destination

2. Many possible futures

3. Frenet: along the road, and sideways

4. A smooth lane change

5. The shortest path hits the car

6. Costs are reasons to say no

7. The repo planner picks one

8. Raise the collision weight

9. Exercises

10. Recap

Go deeper