Week 9 — Advanced Motion Planning Day 7 of 7 | Summary & Preview
What We Covered This Week
| Day | Topic | Core Skill |
|---|---|---|
| 1 | C-space & obstacles | Map workspace obstacles to configuration space |
| 2 | Search-based planning | Use A* and Hybrid A* for grid/lattice planning |
| 3 | Sampling-based planning | Build RRT/RRT* trees for high-DOF arms |
| 4 | Trajectory optimization | Smooth and optimize paths with CHOMP/TrajOpt |
| 5 | Kinodynamic planning | Respect velocity, acceleration, and torque limits |
| 6 | Python practice | Implement RRT* for a 2-DOF arm from scratch |
Key Formulas Cheat Sheet
A* Evaluation Function
$$f(n) = g(n) + h(n)$$ Where $g(n)$ = cost from start, $h(n)$ = heuristic estimate to goal. Requires $h(n)$ admissible (never overestimates) for optimality.
RRT* Rewiring Radius
$$r = \gamma \left( \frac{\log n}{n} \right)^{1/d}$$ Where $n$ = tree size, $d$ = C-space dimension, $\gamma$ = tuning constant.
CHOMP Smoothness Cost (Finite Difference)
$$U_{smooth}(Q) = \sum_{i=2}^{N-1} | q_{i-1} - 2q_i + q_{i+1} |^2$$
TOPP Path Parameterization
$$\dot{s}{max}(s) = \min_i \frac{\dot{q}{max,i}}{|dq_i/ds|}$$ Maximum feasible speed along a geometric path given velocity limits.
Common Pitfalls (Learned the Hard Way)
| Pitfall | Why It Happens | Fix |
|---|---|---|
| RRT fails near narrow passages | Random sampling rarely lands in tight gaps | Use bridge sampling (sample near obstacles) or informed RRT* (focus search on ellipsoidal subset) |
| Trajectory optimization collides | Soft penalty allows slight penetration | Post-process with exact collision check; inflate obstacles by safety margin |
| Hybrid A produces jerky paths* | Discretization in $(x, y, \theta)$ cells | Smooth with cubic splines or run TrajOpt as post-processor |
| Kinodynamic RRT is too slow | Dynamics integration per edge is expensive | Precompute motion primitives (lattice planning) or use GPU batch simulation |
| A heuristic overestimates* | Euclidean distance × inflation factor | Use weighted A* ($w > 1$) deliberately, or scale heuristic back to guarantee admissibility |
Algorithm Selection Guide
| Scenario | Recommended Planner | Why |
|---|---|---|
| 2D grid navigation (warehouse) | A* with Manhattan heuristic | Complete, optimal, fast |
| Autonomous vehicle (car) | Hybrid A* + TrajOpt | Respects Ackermann steering, smooth output |
| 6-DOF industrial arm (pick-and-place) | RRT* + CHOMP | Handles high-D C-space, smooths trajectory |
| Multi-query in fixed environment | PRM + lazy collision checking | Build once, query many |
| Time-critical dynamic environments | RRT (standard) | Fastest sampling planner; replan every 100 ms |
| Maximum speed along known path | TOPP | Computes optimal velocity profile in O(n) |
Week 9 → Week 10 Bridge
Week 9 taught you how to move a robot from A to B without collisions.
Week 10 will teach you how to interact with the environment:
| Day | Topic |
|---|---|
| 1 | Grasp fundamentals — contact models, force closure, stability |
| 2 | Grasp planning — antipodal grasps, grasp quality metrics |
| 3 | Pick-and-place pipeline — perception, planning, grasp, motion |
| 4 | In-hand manipulation — regrasping, pivoting, sliding |
| 5 | Dual-arm coordination — bimanual manipulation |
| 6 | Python practice: Grasp a block with a 2-DOF gripper in PyBullet |
| 7 | Week 10 summary |
Exercise Before Week 10
Challenge: Extend the Week 9 Day 6 RRT* planner with a trajectory smoother:
- After RRT* returns a path $[q_1, q_2, \dots, q_N]$, apply CHOMP-style gradient descent: $$q_i \leftarrow q_i - \alpha (q_{i-1} - 2q_i + q_{i+1})$$
- Run 100 iterations of smoothing.
- Verify the smoothed path remains collision-free.
- Plot the path length before and after smoothing — expect 15–40% reduction.
Hint: The discrete Laplacian $q_{i-1} - 2q_i + q_{i+1}$ is the curvature penalty. Larger $\alpha$ = more smoothing, but may push the path into obstacles. Clamp each step with collision checking.
End of Week 9. See you in Week 10 — Grasping and Manipulation!