Motion planning: RRT, optimization, and time

Every lesson in this phase has ended with a pose. Registration hands you an object pose; the grasping lesson produced a gripper pose worth reaching; IK turns it into joint angles. Nothing yet answers how the arm travels to those angles without hitting the table, the bin wall, or itself. That is motion planning, and it is a problem shape you know well — search over a continuous space where the only cost that matters is the number of calls to one expensive black-box function. In an LLM eval loop that function is a forward pass or a judge call; here it is the collision check. Every algorithm in this lesson is a strategy for spending fewer of those calls.

Foundations: Configuration Geometry and Timing Derivations

Before deploying sampling algorithms, we must rigorously define the geometric and temporal abstractions they manipulate. The core abstraction is the Configuration Space ($\mathcal{C}$), the set of all possible joint vectors $q$. For a planar 2-DOF arm with link lengths $L_1, L_2$, the forward kinematics (FK) map is $x = L_1 \cos q_1 + L_2 \cos(q_1+q_2)$ and $y = L_1 \sin q_1 + L_2 \sin(q_1+q_2)$. A Cartesian obstacle at $(x_o, y_o)$ with radius $r$ maps to a complex implicit surface in $\mathcal{C}$. A straight line in Cartesian space is generally a curve in $\mathcal{C}$, and vice versa. This non-linearity is why we plan in $\mathcal{C}$: joint limits become simple box constraints, whereas Cartesian limits are non-convex.

Collision checking is not instantaneous. It consists of a Broad-Phase (bounding volume hierarchy traversal, $\sim 1\text{--}10 \mu s$) and a Narrow-Phase (exact distance query, $\sim 10\text{--}100 \mu s$). To certify an edge is free, we sample it at a resolution $\Delta q_{\text{edge}}$. The physical clearance $\delta_{\text{phys}}$ between the arm and an obstacle is bounded by the Jacobian norm times the joint step. If $\Delta q_{\text{edge}}$ is too large, the arm can 'tunnel' through a thin obstacle between samples. We define Edge Resolution as the maximum joint displacement between collision checks, chosen such that $\lVert J(q) \Delta q_{\text{edge}} \rVert < \delta_{\text{safe}}$.

For time parameterization, we derive the trapezoid threshold. A joint moves distance $D$ with limits $v_{\max}$ and $a_{\max}$. The time to accelerate to $v_{\max}$ is $t_a = v_{\max}/a_{\max}$, covering distance $d_a = v_{\max}^2 / (2 a_{\max})$. The deceleration phase mirrors this. The total distance for the two ramps is $2 d_a = v_{\max}^2 / a_{\max}$. If $D < v_{\max}^2 / a_{\max}$, the joint never reaches $v_{\max}$; the profile is triangular. If $D \ge v_{\max}^2 / a_{\max}$, a cruise phase exists; the profile is trapezoidal. This threshold $D_{\text{crit}} = v_{\max}^2 / a_{\max}$ is the critical distance separating the two regimes.

Worked Example: 2-DOF Arm with Obstacle

Consider a 2-DOF arm with $L_1=1.0$ m, $L_2=0.5$ m. Start $q_s = [0, 0]$ (Cartesian $(0, 1.5)$). Goal $q_g = [\pi/2, -\pi/2]$ (Cartesian $(1.5, 0)$). An obstacle of radius $0.15$ m is centered at $(1.1, 0.6)$. We compare a Cartesian straight line to a Joint-space straight line.

1. Cartesian Midpoint: $(0.75, 0.75)$. Distance to obstacle center: $\sqrt{(0.35)^2 + (0.15)^2} \approx 0.38$ m. Clearance: $0.38 - 0.15 = 0.23$ m. Free.
2. Joint Midpoint: $q_m = [\pi/4, -\pi/4]$. FK: $x = 1.0 \cos(\pi/4) + 0.5 \cos(0) = 0.707 + 0.5 = 1.207$. $y = 1.0 \sin(\pi/4) + 0.5 \sin(0) = 0.707$. Cartesian pos: $(1.207, 0.707)$.
3. Distance to obstacle center $(1.1, 0.6)$: $\sqrt{(0.107)^2 + (0.107)^2} \approx 0.151$ m. Clearance: $0.151 - 0.15 = 0.001$ m. Collision (or near-collision).

The Cartesian path is safe, but the Joint-space straight line clips the obstacle. This demonstrates that a 'straight' path in one space is not straight in the other. Planning in $\mathcal{C}$ is necessary to ensure the physical arm does not collide, even if the end-effector path looks clear. The edge resolution must be fine enough to detect this 1 mm clearance.

Configuration space: make the robot a point

The first move is a change of representation. Let $q \in \mathcal{C}$ be the joint-angle vector — for a WidowX AI, $q \in \mathbb{R}^6$, and with joint limits $\mathcal{C}$ is a box (±180° on the base yaw, less elsewhere). A workspace obstacle $\mathcal{O}$ — tabletop, bin wall — maps to a configuration-space obstacle: every joint vector that puts any part of the arm's volume $A(q)$ in contact with it.

Planning happens in joint space for three concrete reasons. First, the arm is a volume, not a point: a straight line for the gripper says nothing about where the elbow sweeps, and on a WidowX AI the elbow is usually what clips the bin. Second, IK multiplicity: a Cartesian path can demand a mid-path jump between elbow-up and elbow-down branches — a teleport no continuous joint motion realizes. Third, the constraints get simple: joint limits become box bounds, and a joint-space straight line is always executable, however curved its Cartesian shadow. In C-space the six-dimensional point travels; the obstacle geometry does the deforming.

The catch: $\mathcal{C}_{\text{obs}}$ is never computed explicitly — its boundary is a 5-dimensional implicit surface, hopeless beyond 2–3 DOF. What you can afford is a membership query: given $q$, run forward kinematics for every link (microseconds, from Lesson 1), then test the posed link meshes against the scene — broad-phase bounding-box overlaps, then narrow-phase exact distance queries. An FCL-class library costs roughly 10–100 μs per configuration on one CPU core, and a planning query issues $10^3$–$10^5$ such checks — hence the rule of thumb that collision checking eats over 90% of a sampling planner's runtime. And points are not enough: certifying an edge means sampling it finely enough that nothing slips between. A 0.5 rad edge at 0.02 rad resolution is 26 more checks.

Sampling-based planning: RRT, RRT-Connect, and PRM

The Rapidly-exploring Random Tree attacks a single query — reach $q_{\text{goal}}$ from $q_{\text{start}}$ — by growing a tree through $\mathcal{C}_{\text{free}}$. One iteration:

1. Sample $q_{\text{rand}}$ uniformly from the joint-limit box (with 5–10% probability, use $q_{\text{goal}}$ itself — goal bias).
2. Nearest: find the tree node $q_{\text{near}}$ closest to the sample.
3. Steer: move from $q_{\text{near}}$ toward $q_{\text{rand}}$ by at most a step $\varepsilon$ (0.1–0.3 rad for a 6-DOF arm).
4. Check and add: if the edge is collision-free at your resolution, add the new node as a child; when it reaches the goal, trace parents back for the path.

Why this explores so well: the probability a node is selected as $q_{\text{near}}$ is proportional to the volume of C-space closer to it than to any other node — its Voronoi cell. Frontier nodes bordering unexplored regions own huge cells, so uniform sampling pulls the tree outward automatically. That trick is why RRT survives where grids die: discretizing the WidowX AI's joint box at 0.05 rad gives on the order of $10^{10}$ cells; the tree touches only the few thousand it needs.

Kuffner and LaValle's RRT-Connect (2000) is the variant to actually run. Grow two trees, one from the start, one from the goal. Each iteration: extend tree A one step toward a random sample, then greedily extend tree B toward A's new node — repeatedly, until it connects or hits an obstacle — and swap roles. The greedy connect is the speedup: manipulation C-spaces are mostly open with a few obstacle shells, and connect crosses the open regions in long strides instead of $\varepsilon$-sized shuffles. Tabletop queries typically resolve in tens of milliseconds and a few thousand collision checks.

When the scene is fixed and queries repeat — a cell picking from the same bin to the same three drop zones all day — the Probabilistic Roadmap moves the cost offline. Sample 1,000 collision-free configurations, connect each to its 10 nearest neighbors with checked edges, and keep the graph. Each query then just connects its endpoints into the roadmap and runs graph search: milliseconds, the checking already paid for. The trade is staleness — move one obstacle and the roadmap silently lies, like any cache in front of a changed backend.

The guarantee on offer is probabilistic completeness: if a solution with some clearance exists, the probability of finding it approaches 1 as samples go to infinity. The contrapositive is the painful part: when the planner times out, you learn nothing definitive. Maybe no path exists; maybe one threads a narrow passage — small measure, so uniform samples rarely land there. There is no certificate of infeasibility, ever. In practice: check start and goal for collision first (cheap, definitive — a goal inside the table is the most common "planner failure"), time-box the query near 1 s, retry with a fresh seed, and treat repeated timeouts as a prompt to inspect the scene.

The path RRT-Connect returns is feasible and ugly: random detours, jagged corners, in cluttered scenes often 1.5–2× longer than necessary, because nothing in the algorithm ever optimized. The fix is embarrassingly simple. Shortcutting: pick two random indices on the path, try the straight segment between them, and splice out everything in between if it checks collision-free. A few hundred iterations typically removes 30–50% of the excess length for a few thousand extra checks. Budget for it — every radian of detour is real seconds of motion and real gearbox wear.

import numpy as np

rng = np.random.default_rng(7)

# 2-DOF toy: q in [-pi, pi]^2, one disc-shaped C-space obstacle.
Q_MIN, Q_MAX = -np.pi, np.pi
OBS_C = np.array([0.6, -0.4])   # obstacle center in C-space (rad)
OBS_R = 0.9                     # obstacle radius (rad)
STEP = 0.15                     # max extension per step (rad)
EDGE_RES = 0.02                 # collision-check resolution along edges

n_checks = 0

def in_collision(q):
    global n_checks
    n_checks += 1
    return np.linalg.norm(q - OBS_C) < OBS_R

def edge_free(a, b):
    n = max(int(np.ceil(np.linalg.norm(b - a) / EDGE_RES)), 1)
    return all(not in_collision(a + (b - a) * (i / n)) for i in range(n + 1))

class Tree:
    def __init__(self, root):
        self.nodes = [np.asarray(root, dtype=float)]
        self.parent = [-1]

    def nearest(self, q):
        d = np.linalg.norm(np.array(self.nodes) - q, axis=1)
        return int(np.argmin(d))

    def add(self, q, parent):
        self.nodes.append(q)
        self.parent.append(parent)
        return len(self.nodes) - 1

    def path_from_root(self, i):
        out = []
        while i != -1:
            out.append(self.nodes[i])
            i = self.parent[i]
        return out[::-1]

def extend(tree, target):
    i = tree.nearest(target)
    q_near = tree.nodes[i]
    d = np.linalg.norm(target - q_near)
    q_new = target if d <= STEP else q_near + (target - q_near) * (STEP / d)
    if edge_free(q_near, q_new):
        return tree.add(q_new, i), bool(d <= STEP)
    return -1, False

def connect(tree, target):
    while True:                  # greedy: stride until blocked or reached
        i, reached = extend(tree, target)
        if i == -1 or reached:
            return i if reached else -1

def rrt_connect(q_start, q_goal, max_iters=2000):
    ta, tb, a_is_start = Tree(q_start), Tree(q_goal), True
    for _ in range(max_iters):
        i, _ = extend(ta, rng.uniform(Q_MIN, Q_MAX, size=2))
        if i != -1:
            j = connect(tb, ta.nodes[i])
            if j != -1:          # trees met: stitch start-side + goal-side
                pa, pb = ta.path_from_root(i), tb.path_from_root(j)
                return pa + pb[::-1][1:] if a_is_start else pb + pa[::-1][1:]
        ta, tb, a_is_start = tb, ta, not a_is_start
    return None

def shortcut(path, iters=300):
    path = list(path)
    for _ in range(iters):
        if len(path) < 3:
            break
        i, j = sorted(rng.choice(len(path), size=2, replace=False))
        if j - i >= 2 and edge_free(path[i], path[j]):
            path = path[: i + 1] + path[j:]
    return path

def length(path):
    return sum(np.linalg.norm(b - a) for a, b in zip(path, path[1:]))

q_start, q_goal = np.array([-2.5, 2.0]), np.array([2.2, -2.2])
raw = rrt_connect(q_start, q_goal)
smooth = shortcut(raw)
print("collision checks:", n_checks)
print("raw path:  ", len(raw), "waypoints,", round(length(raw), 2), "rad")
print("shortcut:  ", len(smooth), "waypoints,", round(length(smooth), 2), "rad")

With the seed shown: about 19,000 collision checks — edge certification at 0.02 rad resolution dominates — a 49-waypoint raw path 14% longer than the straight-line distance, and shortcutting collapsing it to 3 waypoints. Two notes survive contact with a real arm: the linear-scan nearest-neighbor is fine into the thousands of nodes (above roughly 10 dimensions, spatial indices degrade toward linear scan anyway), and n_checks, not wall-clock, is the metric to watch — the hardware-independent cost model, the way token counts beat seconds.

Trajectory optimization: planning as an NLP

Sampling treats the collision checker as a boolean oracle. Trajectory optimization opens the box: given a signed distance $d(q)$ — negative when penetrating — and its gradient, planning becomes numerical optimization. Discretize the path into $N$ waypoints ($N = 50$ is typical) and stack them into one decision vector — a 300-dimensional NLP for a 6-DOF arm:

The CHOMP family precomputes a voxel signed-distance field so $d$ and $\nabla d$ become array lookups; the TrajOpt family sequentially convexifies the collision constraint into a QP per iteration. Both inherit local optimization's central dishonesty: the problem is nonconvex, roughly one basin per way around each obstacle, and the solver lands in whichever basin the initialization chose. The standard init — the straight line from start to goal — passes through the obstacle for any problem worth a planner. Sometimes the gradient pushes the path cleanly out; sometimes it returns a local optimum grazing the obstacle, or an infeasible point that merely stopped improving. "Converged" is not "collision-free," let alone "globally shortest." Verify with the honest boolean checker, at edge resolution.

So when does optimization beat sampling? When a decent initialization exists and path quality matters: it returns short, smooth, margin-respecting trajectories deterministically, and it warm-starts beautifully — feed it the previous solution while replanning at 10 Hz and it polishes rather than re-solves, the economics of incremental recompilation. Sampling wins the global questions: unknown homotopy class (left of the bin or right?), narrow passages, no seed worth trusting. The industrial default is the hybrid: RRT-Connect finds a path, shortcutting cleans it, optimization polishes it. For depth: Underactuated's trajectory optimization chapter (opens in a new tab) and the manipulation book's motion planning chapter (opens in a new tab).

Time parameterization: a path is not a trajectory

Everything so far produced geometry: a sequence of configurations, equivalently a path $q(s)$ indexed by progress $s \in [0,1]$. The robot cannot execute geometry. It needs $q(t)$ — the same curve equipped with a timing law $s(t)$, where velocity, acceleration, and jerk limits enter. The classical factorization: plan the shape once, choose speed separately, re-time without replanning. The simplest useful timing law — the one industrial motion controllers have baked into firmware for decades — is the trapezoidal velocity profile: accelerate at the limit, cruise at the limit, decelerate at the limit. Derive its timing; you will use these formulas constantly.

Setup: one joint travels a distance $D$ (rad), rest to rest, under limits $v_{\max}$ and $a_{\max}$. Phase one accelerates at $a_{\max}$ until it hits the velocity limit:

Deceleration mirrors it, so the two ramps together consume $2 d_a = v_{\max}^2 / a_{\max}$ of travel. A cruise phase exists only if distance is left over — the trapezoid-versus-triangle threshold:

In the trapezoidal case the cruise covers $D - v_{\max}^2/a_{\max}$ at speed $v_{\max}$. Total time is two ramps plus the cruise, and the algebra collapses:

Below the threshold the joint never reaches $v_{\max}$: it accelerates to a peak $v_p$ and immediately decelerates, each half of the triangle covering $v_p^2 / (2 a_{\max})$:

Concrete WidowX AI numbers, with $v_{\max} = 1.57$ rad/s and $a_{\max} = 5$ rad/s² — deliberately conservative next to the spec's 6.3 rad/s proximal-joint velocity limit, the kind of soft limit you would configure in your host-side executor. A $D = 1.2$ rad shoulder move exceeds the threshold $v_{\max}^2/a_{\max} \approx 0.49$ rad, so trapezoid: $T = 1.2/1.57 + 1.57/5 \approx 1.08$ s, 0.63 s of it ramping. A $D = 0.05$ rad servoing nudge: triangle, $T = 0.2$ s, peaking at 0.5 rad/s. Small moves live entirely inside the ramps, so the acceleration limit dominates the fine motion pick-and-place is mostly made of. Multi-joint moves add one rule: the slowest joint sets $T$, and every other profile stretches to finish simultaneously — otherwise the arm bends off the straight C-space segment you planned.

import numpy as np

def trapezoid_time(D, v_max, a_max):
    """Minimum time for one joint to travel distance D, rest to rest."""
    D = abs(float(D))
    if D < 1e-12:
        return 0.0
    if D >= v_max * v_max / a_max:          # reaches the velocity limit
        return D / v_max + v_max / a_max    # cruise + one full ramp
    return 2.0 * np.sqrt(D / a_max)         # triangular: never cruises

def synchronized_segment_time(delta_q, v_max, a_max):
    """All joints start and stop together; the slowest joint sets T."""
    return max(trapezoid_time(d, v_max, a_max) for d in delta_q)

def total_motion_time(path, v_max=1.57, a_max=5.0):
    """Stop-at-every-waypoint execution of a piecewise-linear path."""
    return sum(
        synchronized_segment_time(b - a, v_max, a_max)
        for a, b in zip(path, path[1:])
    )

One more derivative matters. A trapezoid's acceleration is a square wave: four times per move it steps between $\pm a_{\max}$ and zero, and torque steps with it ($\tau = I\alpha$). That step's derivative — jerk — is impulsive at the corners. A stiff industrial arm registers a click; a light 4 kg arm like the WidowX AI — with a RealSense hanging off the wrist — still rings: the gripper oscillates after each stop, settling can take 100–300 ms, and mid-ramp frames are motion-blurred. The S-curve profile bounds jerk by making acceleration itself trapezoidal, seven phases instead of three. It costs extra nominal time (tens of milliseconds per end) but often wins on time-to-settled — the number that gates when you may open the gripper or trust a wrist-camera frame. For time-optimality along an arbitrary path, the time-optimal path parameterization (TOPP) family computes the fastest $s(t)$ respecting all limits at once.

The tabletop confession

Now the honest part. For the pick-and-place baseline this phase is building — one arm, a clear tabletop, objects in a known region — you do not need the machinery above. The pattern that ships: joint-space move to a home pose above the workspace; from the grasp candidate you ranked last lesson, compute a pregrasp 8–10 cm back along the approach axis; reach it in joint space; descend along the approach axis as a straight Cartesian segment via differential IK at 50–100 interpolated waypoints; close; retreat straight back out; home again. Each segment gets one edge collision check against the table model — planning compute well under a millisecond, where RRT-Connect would spend 50–300 ms producing a worse, random path through the same empty air. The planner earns its complexity only when:

• Clutter and containers: bins and shelves put real $\mathcal{C}_{\text{obs}}$ between start and goal, with multiple homotopy classes to choose among — the planner's home turf.
• Changing scenes: a scripted joint-space move memorizes one scene; when obstacles move between episodes it silently invalidates.
• Long reorientations: moves passing near joint limits or the wrist singularity from the Jacobian lesson, where straight Cartesian segments lose IK feasibility mid-flight.
• Self-collision risk: wrist cameras, dangling cables, dual-arm rigs — the obstacle is the robot, where human intuition about clearance is worst.

So the decision rule: start with straight-line primitives plus one honest edge check per segment. The day that check starts failing — a second bin appears, an approach vector points into clutter — is the day RRT-Connect earns its 200 ms. You will already own the infrastructure: the primitive's edge check and the planner's inner loop are the same collision query against the same scene model.

Where this goes next: the grasp candidates from Grasping: friction, closure, and candidates now have motions that reach them — geometry from a planner or a straight-line primitive, timing from the trapezoid math. But a trajectory is still open-loop: a promise about where the arm should be at each instant, with nothing enforcing it when gravity sags a joint or a contact shoves the wrist. That is feedback's job. Linear systems, LQR, and feedback — the EE refresher dusts off your undergraduate control theory and turns $q(t)$ from a wish into a tracked reference — completing the classical stack: perceive, plan, control.