Week 9 — Advanced Motion Planning Day 4 of 7 | Difficulty: ⭐⭐⭐⭐⭐
Why Optimize Trajectories?
RRT and A* produce feasible paths — collision-free sequences of waypoints. But they are often:
- Jerky: Sudden direction changes
- Too close to obstacles: Minimum clearance not guaranteed
- Dynamically infeasible: Velocity/acceleration limits violated
Trajectory optimization starts with an initial path (even a collision-prone straight line) and iteratively improves it using gradient descent.
The Optimization Formulation
Represent a trajectory as a sequence of configurations:
$$Q = [q_1, q_2, \dots, q_N]$$
The objective function combines three terms:
$$U(Q) = U_{smooth}(Q) + \lambda_{obs} U_{obs}(Q) + \lambda_{goal} U_{goal}(Q)$$
1. Smoothness Cost
Penalizes jerk (third derivative) or acceleration:
$$U_{smooth}(Q) = \sum_{i=2}^{N-1} | q_{i-1} - 2q_i + q_{i+1} |^2$$
This is a finite-difference approximation of curvature. Minimizing it produces straight or smoothly curved trajectories.
2. Obstacle Cost
Measures proximity to obstacles using a distance field or collision potential:
$$U_{obs}(q) = \begin{cases} 0 & \text{if } d(q) > d_{safe} \ \frac{1}{2}(d_{safe} - d(q))^2 & \text{if } d(q) \leq d_{safe} \end{cases}$$
Where $d(q)$ = distance to nearest obstacle in C-space.
3. Goal Cost
Pulls the final configuration toward the target:
$$U_{goal}(Q) = | q_N - q_{goal} |^2$$
CHOMP: Covariant Hamiltonian Optimization
CHOMP (Ratliff et al., 2009) optimizes trajectories using covariant gradient descent:
- Compute the gradient $\nabla U(Q)$
- Precondition with the inverse dynamics metric $A^{-1}$
- Update: $Q_{new} = Q - \alpha A^{-1} \nabla U(Q)$
The $A^{-1}$ preconditioner ensures the gradient respects the robot’s dynamics — a joint with high inertia receives a smaller update than a lightweight joint.
Key Data: CHOMP converges in 50–200 iterations for a 7-DOF arm, producing smoother trajectories than RRT in comparable time.
STOMP: Stochastic Trajectory Optimization
STOMP (Kalakrishnan et al., 2011) avoids explicit gradient computation:
- Start with an initial trajectory (e.g., linear interpolation)
- Generate $K$ noisy perturbations around the current trajectory
- Score each perturbed trajectory using $U(Q)$
- Update the trajectory as a weighted average of the best perturbations
This is derivative-free optimization — it works even when the obstacle cost is non-differentiable (e.g., from discrete collision checking).
| CHOMP | STOMP | |
|---|---|---|
| Requires gradients? | Yes | No |
| Smoothness guarantee | Strong | Moderate |
| Collision handling | Continuous potential | Discrete/noisy robust |
| Best for | Differentiable cost functions | Black-box collision checkers |
TrajOpt: Sequential Convex Optimization
TrajOpt (Schulman et al., 2014) uses sequential convex programming (SCP):
- Approximate the non-convex collision constraints with convex polytopes
- Solve a convex optimization subproblem
- Re-linearize around the new solution
- Repeat until convergence
TrajOpt is particularly fast because it uses the convex hull of obstacles and solves a quadratic program (QP) at each iteration — solvable in milliseconds.
When to Use Optimization vs. Sampling
| Sampling (RRT/PRM) | Optimization (CHOMP/TrajOpt) | |
|---|---|---|
| Finds feasible path? | Yes (guaranteed if exists) | No — needs initialization |
| Optimizes smoothness? | No (post-processing only) | Yes, built-in |
| Handles tight spaces? | Yes | Yes, but sensitive to initialization |
| Real-time replanning? | RRT is fast | TrajOpt is fast with warm start |
| Typical workflow | Plan first, smooth second | Optimize from straight-line init |
Best practice: Use RRT* to find a feasible path, then run TrajOpt or CHOMP to smooth and optimize it.
Review Days 1–3
Day 1: C-space. Day 2: Search-based (A*). Day 3: Sampling-based (RRT/PRM). Today: Optimization-based smoothing and trajectory generation.
Preview Day 5
Day 5: Kinodynamic planning — handling velocity, acceleration, and torque limits during planning.
FAQ
Q: Why not just optimize the RRT path directly?
A: You can — this is the standard pipeline. RRT finds a feasible path; CHOMP/TrajOpt optimizes it. But TrajOpt can also start from a straight-line interpolation (which may collide) and optimize its way to a collision-free trajectory.
Details: Straight-line initialization + TrajOpt sometimes finds better solutions than RRT + optimization because it is not constrained by the RRT tree topology. However, it can get stuck in local minima if the straight line passes through a narrow gap.
Q: What is a “distance field” in trajectory optimization?
A: A precomputed grid that stores, for every point in workspace, the distance to the nearest obstacle. During optimization, the obstacle cost is looked up in $O(1)$ time instead of running expensive collision detection.
Details: Distance fields are typically computed on a 3D voxel grid using the fast marching method or GPU-accelerated signed distance functions (SDFs). For articulated robots, the distance field is in workspace; the C-space obstacle cost is approximated via forward kinematics.
Q: Can trajectory optimization guarantee no collisions?
A: No — it minimizes a soft penalty for collisions. To guarantee safety, post-process the optimized trajectory with exact collision checking or inflate obstacles by the robot’s maximum reach. Some variants (TrajOpt with exact penalty) use exact collision constraints but sacrifice differentiability.