Sampling Under Differential Constraints

Chapter 1: State Space — Phase Space Extends C-Space

Chapter 4 gave us C-space: every configuration q of the robot is a point, and C_free is the navigable region. But for a dynamical system like a drone or a car with nonzero mass, knowing where the robot is isn’t enough. You also need to know how fast it’s going. Without velocity, you can’t predict the future.

The state space X is C-space extended with its derivatives. For a unicycle with configuration q = (p_x, p_y, θ), the state is still just x = q — configuration is enough because the controls directly set velocity (first-order system). But for a car with mass, the state is x = (p_x, p_y, θ, v) where v is the current speed — because the control (throttle) changes acceleration, not velocity directly. And for a robot arm, the state is x = (q, q̇) — joint angles and joint velocities — because torque changes acceleration.

State space obstacle region

When we extend to state space, the obstacle region X_obs grows. In C-space, we had C_obs — configurations where the robot collides. In state space, X_obs includes not only colliding configurations but also any state from which collision is unavoidable regardless of control input. A car heading at a wall at 100 m/s with only 10 m to stop is in X_obs even though it isn’t touching the wall yet.

This larger dimension is both the challenge and the reason we need sampling. Combinatorial planners that work in 3D C-space become intractable in 6D state space. Sampling-based methods scale gracefully — the fundamental RRT loop works the same whether the space is 3D or 12D.

Chapter 2: Reachability — Where Can You Go from Here?

From a state x, applying control u for duration Δt produces a new state. The set of all states reachable from x in exactly Δt is the reachable set R(x, Δt). It is the analog of “the disk of radius vΔt around x” from Chapter 5, but shaped by dynamics rather than geometry.

For a unicycle (first-order, unconstrained in v and ω), R(x, Δt) looks like a disk — similar to the unconstrained particle. But for a car with a maximum steering angle δ_max and bounded speed [−v_max, v_max], the reachable set is a curved banana shape: it extends forward and backward, can arc left or right, but cannot point sideways.

Forward and backward reachability

The forward reachable set from x at time t is the set of all states you can reach from x in time t. Planning asks: does the goal state lie in the forward reachable set of the start? Equivalently, we can ask about the backward reachable set of the goal: all states from which the goal can be reached. A feasible plan exists if and only if these two sets intersect.

Computing these sets exactly is intractable for most systems. Sampling-based planning avoids this: instead of computing R(x, Δt) analytically, the RRT samples from it by picking a random control u and integrating forward. The samples approximate the reachable set without needing its closed form.

Chapter 3: Kinodynamic Planning — RRT with Forward Simulation

Kinodynamic planning is the marriage of kinematics (where the robot is) and dynamics (how fast it’s moving) in a single planning problem. The term was coined by Donald, Xavier, Canny, and Reif in 1993. The goal: find a trajectory x(t) and control sequence u(t) that steers the robot from x_init to a neighborhood of x_goal while satisfying &ẋ; = f(x,u), staying in X_free, and respecting control bounds U.

The critical algorithmic change from Chapter 5 RRT to kinodynamic RRT is the extend step. In Ch 5:

# Ch 5 RRT extend (C-space, no dynamics)
def extend(tree, x_rand):
    x_near = nearest(tree, x_rand)
    direction = x_rand - x_near
    x_new = x_near + step_size * direction / norm(direction)  # straight line
    if collision_free(x_near, x_new):
        tree.add(x_new, parent=x_near)

In kinodynamic RRT:

# Ch 14 Kinodynamic RRT extend (state space, dynamics)
def extend(tree, x_rand):
    x_near = nearest(tree, x_rand)       # find nearest in state space
    u = sample_control(U)                 # pick a random control
    x_new = simulate(x_near, u, dt)       # integrate ODE forward dt seconds
    if collision_free(x_near, x_new):       # check state-space path segment
        tree.add(x_new, parent=x_near, control=u, duration=dt)

The simulate(x, u, dt) call integrates &ẋ; = f(x, u) for duration Δt using Euler integration (or Runge-Kutta for accuracy). The result is a state that is physically reachable from x_near by applying control u. No straight-line extrapolation, no approximate interpolation — actual physics.

The nearest-neighbor problem in state space

Finding the nearest node in state space requires a metric. This is subtler than it sounds. State space mixes position (meters) with velocity (m/s) with angle (radians). Naively computing Euclidean distance in the combined space gives nonsensical results because the units don’t match.

The weights w_i normalize dimensions: typically divide each component by its maximum range. For a car with position in [0, 10]m and speed in [−2, 2]m/s: w_x = 1/10, w_v = 1/4. Angle components need special handling for wraparound (θ and θ + 2π are the same configuration).

Chapter 4: Showcase — Car-Like RRT with Steering Limits

This is the payoff. Watch a kinodynamic RRT build a tree for a car-like robot with bounded steering angle. The car’s state is x = (p_x, p_y, θ) and its differential equation is the standard bicycle model:

where v is speed (fixed at 1 m/s here), L is wheelbase (1 m), and δ ∈ [−35°, 35°] is the steering angle — the only control. The car can’t steer more sharply than 35°; this is the physical constraint that makes planning nontrivial.

The tree here is small for clarity. In real kinodynamic RRT, the planner runs 10,000–100,000 iterations. Even so, it typically solves car parking problems in under a second on modern hardware because each iteration is just an ODE integration step — no matrix inversions, no solvers, no optimization.

Chapter 5: Feedback Control Under Differential Constraints

Open-loop RRT outputs a sequence of controls (u₁, u₂, ..., u_N). Execute them in order and, in simulation, you reach the goal. But real robots have noise, model errors, and disturbances. By the time you execute control u₅₀, the accumulated error means you’re no longer on the planned trajectory — and the open-loop plan can’t recover.

Feedback control closes the loop: instead of pre-computing a fixed control sequence, the robot continuously measures its state x(t) and chooses the control u based on x(t). A feedback policy π: X → U maps current state to control. The trajectory is whatever x(t) the ODE &ẋ; = f(x, π(x)) produces under this policy.

Lyapunov stability: will the robot converge?

For a feedback policy to be useful, the system must converge to the goal. A Lyapunov function V(x) proves convergence without solving the ODE explicitly. Think of V(x) as the “potential energy” of the system — if we can show V(x) decreases along every trajectory (V̇(x) < 0), then the system must eventually reach the minimum of V, which we place at the goal.

For a car, finding such a V is hard because the car can’t move sideways: a simple quadratic V = |x − x_goal|² won’t work (the gradient often points sideways). Designing stabilizing feedback for nonholonomic systems is an active research area — Brockett’s theorem shows that smooth time-invariant feedback controllers cannot stabilize some nonholonomic systems.

RRT-based feedback: iterative replanning

One practical workaround: run kinodynamic RRT as a feedback policy. Measure the current state x(t), run RRT from x(t) to x_goal, execute the first few controls of the resulting plan, measure again, replan. This is receding-horizon planning or model-predictive control (MPC) applied to RRT. It provides implicit closed-loop behavior: each replan compensates for drift since the last plan.

Chapter 6: Decoupled Approaches — Plan Path, Then Add Time

Kinodynamic planning in the full state space is powerful but expensive. A simpler strategy: decouple the problem into two stages. First, find a geometric path in C-space using a fast planner (Ch 5 RRT, ignoring dynamics). Second, find a time-scaling — a function s(t) that converts the geometric path into a time-parameterized trajectory that satisfies dynamics and control bounds.

Formally, if the geometric path is q(σ) for σ ∈ [0, 1], the time-scaled trajectory is q(s(t)). The velocity along the path at time t is q̇(t) = (dq/dσ) · ṡ(t). The acceleration is q̈(t) = (dq/dσ) · s̈(t) + (d²q/dσ²) · ṡ(t)². Both must stay within joint velocity and torque limits.

Time-optimal time scaling

Given a fixed geometric path, the time-optimal time scaling finds s(t) that traverses the path as fast as possible subject to dynamics limits. Bobrow (1985) and Shin & McKay (1985) solved this with a phase-plane approach: plot (s, ṡ) and find the limiting velocity curve, then bang-bang control the acceleration to stay below it.

When decoupling fails

Decoupling works well for robot arms where any path in C-space is geometrically feasible and only dynamics limits constrain the time-scaling. It fails for:

Chapter 7: Trajectory Optimization — Gradient-Based Refinement

Sampling-based planning finds a feasible trajectory, not necessarily a good one. The kinodynamic RRT tree grows randomly — the solution path is whatever sequence of arcs happened to reach the goal. It may be jerky, unnecessarily long, and far from time-optimal. Trajectory optimization takes such an initial solution and improves it by minimizing a cost functional.

The canonical trajectory optimization problem: find a trajectory x(t) and controls u(t) over t ∈ [0, T] that minimizes:

subject to &ẋ; = f(x, u), x(0) = x_init, x(T) ∈ X_goal, x(t) ∈ X_free, u(t) ∈ U. Here ℓ is the running cost (energy, time, jerk), φ is the terminal cost (distance to goal), and the constraints couple everything together.

CHOMP: gradient descent on trajectory space

CHOMP (Covariant Hamiltonian Optimization for Motion Planning, Ratliff et al. 2009) treats the entire trajectory as a single high-dimensional parameter vector and does gradient descent. Each trajectory segment is a vertex in a discrete graph; CHOMP simultaneously pushes all vertices away from obstacles and toward smoothness.

The CHOMP objective has two terms:

U_obs penalizes proximity to obstacles using a pre-computed signed distance field. U_smooth penalizes acceleration along the trajectory (∫ |q̈|² dt). The gradient is taken in a Riemannian metric on trajectory space (the “covariant” in CHOMP), which ensures updates are in physically meaningful directions rather than arbitrary Euclidean gradient steps.

TrajOpt: sequential convex approximation

TrajOpt (Schulman et al. 2013) reformulates trajectory optimization as a sequence of convex programs. At each step, linearize the nonlinear constraints (collision, dynamics) around the current trajectory, solve the resulting quadratic program (QP) to get an update, repeat until convergence. TrajOpt handles hard collision avoidance constraints rather than soft penalties.

Chapter 8: Practical Considerations

Textbook kinodynamic RRT works. Deployed kinodynamic planners that solve real problems require a handful of engineering decisions that the algorithm doesn’t specify. Here are the most critical ones.

ODE integration: Euler vs Runge-Kutta

The simplest integrator is Euler integration: x(t + Δt) ≈ x(t) + Δt · f(x(t), u). This accumulates error proportional to Δt. For a car at speed 1 m/s with Δt = 0.1 s, Euler error is O(0.01) per step — acceptable. For a quadrotor at high speed with fast attitude dynamics, Euler blows up unless Δt < 0.001 s, which makes each edge extremely short. Runge-Kutta 4 (RK4) gives fourth-order accuracy for only 4x more function evaluations:

def rk4_step(f, x, u, dt):
    k1 = f(x, u)
    k2 = f(x + dt/2 * k1, u)
    k3 = f(x + dt/2 * k2, u)
    k4 = f(x + dt * k3, u)
    return x + dt/6 * (k1 + 2*k2 + 2*k3 + k4)

RK4 allows 10x larger Δt than Euler for the same accuracy — far fewer, longer tree edges, which means faster coverage of state space.

Control sampling strategy

Uniform random sampling of u ∈ U is unbiased but inefficient. In practice, try multiple controls from x_near (say, 5 or 10 random samples) and keep the one that moves closest to x_rand in state space. This is a greedy multi-sample extend — it doesn’t change the algorithm’s probabilistic completeness but dramatically improves growth speed.

Goal biasing

Pure uniform sampling grows a tree that fills all of X — which is thorough but slow. Standard RRT uses goal biasing: with probability p_goal (typically 5–10%), set x_rand = x_goal directly. This biases tree growth toward the goal without abandoning exploration. Too high p_goal makes the planner greedy and poor at navigating around obstacles; too low makes it slow.

Bidirectional RRT

Grow two trees simultaneously: one from x_init and one from x_goal. On each iteration, grow one tree toward a random sample, then try to grow the other tree toward the newly added node. When the two trees get close enough to connect, the path is found. Bidirectional RRT typically solves problems 5–10x faster than single-tree RRT because both trees grow toward each other.

When to use which method

Chapter 9: Connections — Where This Fits and What’s Next

Chapter 14 sits at the intersection of classical robotics and modern control theory. It takes the geometric planning tools from Chapters 4–7 and merges them with the differential systems of Chapter 13. Understanding this chapter is the bridge to everything in robot motion planning that involves real physics.

How Ch 14 builds on earlier chapters

Limitations of kinodynamic RRT

• Not optimal: Basic RRT is not asymptotically optimal. RRT* (Karaman & Frazzoli, 2011) adds a rewiring step that converges to optimal as n → ∞, but is slower per iteration.
• Hard to steer: The nearest-neighbor + random control strategy doesn’t efficiently steer toward x_rand. Optimal steering (solving the two-point boundary value problem) would be ideal but is often intractable.
• No guarantees on trajectory quality: The found path is whatever random sequence of controls connected start to goal. Post-processing (shortcutting, time-scaling, optimization) is almost always needed.
• Curse of dimensionality: State-space dimension grows with system complexity. In 20+ dimensional spaces, the tree takes exponentially more nodes to achieve good coverage.

Where this connects to modern robotics

Related Parminces lessons

This chapter connects directly to:

• Ch 5: Sampling-Based Motion Planning — the RRT/PRM foundation extended here
• Ch 13: Differential Models — the ODE systems that kinodynamic RRT integrates
• Ch 8: Feedback Motion Planning — stability and Lyapunov methods for feedback control
• Ch 15: System Theory — controllability, observability, and what comes next →

“The difficulty is not in finding the right answer; it is in formulating the right question.”
— Steven M. LaValle, Planning Algorithms, 2006