Week 9 — Advanced Motion Planning Day 5 of 7 | Difficulty: ⭐⭐⭐⭐⭐
The Gap Between Path and Trajectory
A path planner answers: “Where should I go?” A kinodynamic planner answers: “Where should I go, and can I actually get there given my physical limits?”
Most real robots have constraints:
- Velocity limits: Motors have maximum RPM
- Acceleration limits: Torque is finite ($\tau = I\alpha + \dots$)
- Jerk limits: Mechanical stress, passenger comfort
- Non-holonomic constraints: Cars can’t slide sideways
Ignoring these during planning leads to trajectories the controller cannot execute.
The Kinodynamic Planning Problem
The state is now an augmented vector that includes velocities:
$$x = \begin{bmatrix} q \ \dot{q} \end{bmatrix}$$
The dynamics are a differential equation:
$$\dot{x} = f(x, u) = \begin{bmatrix} \dot{q} \ M(q)^{-1}(\tau - C(q, \dot{q})\dot{q} - G(q)) \end{bmatrix}$$
Where $u = \tau$ (joint torques) is the control input.
The planning problem: find a control sequence $u(t)$ that drives $x$ from $x_{start}$ to $x_{goal}$ while:
- Staying in $C_{free}$ (no collisions)
- Respecting $\dot{q}{min} \leq \dot{q} \leq \dot{q}{max}$ (velocity limits)
- Respecting $\ddot{q}{min} \leq \ddot{q} \leq \ddot{q}{max}$ (acceleration limits)
Kinodynamic RRT
Standard RRT samples a configuration $q_{rand}$ and connects it to the tree. Kinodynamic RRT samples a control $u_{rand}$ and integrates the dynamics forward:
def kinodynamic_rrt(start_state, goal_state, dynamics, max_iter=10000, dt=0.05):
"""
start_state: [q, q_dot] — initial position and velocity
goal_state: [q, q_dot] — target position and velocity
dynamics: function f(x, u) returning x_dot
"""
tree = {tuple(start_state.flatten()): None}
nodes = [start_state]
for _ in range(max_iter):
# 1. Sample random control (torque) within limits
u_rand = np.random.uniform(u_min, u_max, size=n_joints)
# 2. Pick random node
idx = np.random.randint(len(nodes))
nearest = nodes[idx]
# 3. Integrate dynamics forward
x_new = nearest.copy()
for _ in range(integration_steps):
x_dot = dynamics(x_new, u_rand)
x_new = x_new + x_dot * dt
# Check velocity limits
if np.any(np.abs(x_new[n_joints:]) > q_dot_max):
break
# 4. Collision check
if is_collision_free_state(x_new):
tree[tuple(x_new.flatten())] = tuple(nearest.flatten())
nodes.append(x_new)
if state_distance(x_new, goal_state) < epsilon:
return reconstruct_path(tree, x_new)
return None
Key Data: Kinodynamic RRT is 10–100× slower than standard RRT because each edge requires numerical integration of the dynamics. But it produces trajectories that are dynamically feasible by construction.
State-Space Lattice Planning
Instead of random controls, lattice planners precompute a fixed set of motion primitives — short, dynamically feasible trajectories (e.g., “drive forward 1m”, “turn left 30°”). Planning becomes a graph search over these primitives.
This is the approach used by the JPL Mars rovers and many autonomous vehicle stacks. It guarantees dynamic feasibility because every primitive is pre-validated.
| Kinodynamic RRT | Lattice Planning | |
|---|---|---|
| Exploration | Random controls | Fixed motion primitives |
| Dynamic feasibility | By construction | By construction |
| Completeness | Probabilistic | Resolution-complete |
| Speed | Slow (integration per edge) | Fast (lookup + graph search) |
| Best for | Arms, drones with complex dynamics | Cars, rovers with repeatable maneuvers |
Time-Optimal Path Parameterization (TOPP)
Given a geometric path $q(s)$, TOPP finds the fastest traversal while respecting velocity and acceleration limits.
The key insight: convert the problem to a 1D optimization in the path parameter $s$:
$$\dot{s}{max}(s) = \min_i \frac{\dot{q}{max,i}}{|dq_i/ds|}$$
$$\ddot{s}(s) = \frac{\tau_{max} - C(q,\dot{q})\dot{q} - G(q)}{M(q) \cdot (d^2q/ds^2 \cdot \dot{s}^2 + dq/ds \cdot \ddot{s})}$$
Solving this produces a velocity profile $\dot{s}(t)$ — the answer to “how fast can I drive this path?”
Key Data: TOPP computes the time-optimal traversal in $O(n)$ for $n$ path segments. It is used in industrial robot controllers to maximize throughput while respecting motor limits.
Review Days 1–4
Day 1: C-space. Day 2: Search. Day 3: Sampling. Day 4: Trajectory optimization. Today: The hardest case — planning with full dynamics.
Preview Day 6
Day 6: Python practice — Implement RRT* for a 2-DOF planar arm with visualization.
FAQ
Q: Why is kinodynamic planning so much harder than geometric planning?
A: The state space is doubled (position + velocity), and connectivity is constrained by differential equations. A configuration may be reachable, but only from specific incoming velocities.
Details: Geometric RRT connects any two nearby configurations with a straight line. Kinodynamic RRT can only move along trajectories that satisfy $\dot{q} = f(q, u)$. This means the tree cannot directly “steer” toward a sampled state — it must simulate forward from existing states.
Q: Can standard RRT + smoothing handle dynamics?
A: Sometimes, but not safely. A geometric RRT path may require instantaneous velocity changes that violate torque limits. Post-hoc smoothing (e.g., with B-splines) can help but does not guarantee feasibility.
Details: The safest pipeline is: (1) geometric RRT for feasibility, (2) TOPP for time parameterization, (3) check that resulting accelerations respect torque limits. If not, fall back to kinodynamic planning.
Q: What motion primitives do lattice planners use?
A: For Ackermann-steering cars: straight segments, constant-curvature arcs, clothoids (curvature-varying). For rovers: short forward drives, turns in place, arc turns. Each primitive is precomputed by integrating the vehicle model with boundary conditions.
Details: The set of primitives must be reachability-rich — from any state, a combination of primitives should reach any nearby state. This is proven for Dubins and Reeds-Shepp curves (cars with minimum turning radius).