MIT 6.821: Underactuated Robotics

Spring 2024, Lecture 18

Follow **live** at https://slides.com/d/syF900Q/live

(or later at https://slides.com/russtedrake/spring24-lec18)

Image credit: Boston Dynamics

*Input*: source \(v_s\), target \(v_t\), \(\tilde{g}\), \(\tilde{h}\), `GetSuccessors()`

*Output*: shortest path from \(v_s\) to \(v_t\)

\(p = \text{Path}([v_s])\)

\(S = \{ v_s : p \}\)

\(Q.\text{insert}(p)\)

while not \(Q.\text{empty}()\):

\(p = Q.\text{pop}()\)

\(u = p.\text{last}()\)

if \(u = v_t,\) then return \(p\)

for all \(v \in\) `GetSuccessors`

\((u)\), where \(v \notin p\):

\(p' = \text{Path}(p, u)\)

if \(v \notin S\) or \(\tilde{g}(p') < \tilde{g}(S[v]),\):

\(S[v] = p'\)

\(Q.\text{insert}(p')\)

return failure

Visited dictionary

Priority queue using \(\tilde{f}\)

path with lowest \(\tilde{f}\)

Discrete A* algorithm

\begin{aligned} \min_{x_0, ..., x_N} \quad & \sum_{n=0}^{N-1} | x_{n+1} - x_n|_2^2 & \\
\text{subject to} \quad & x_0 = x_{start} \\ & x_N = x_{goal} \\ & |x_n|_1 \ge 1 & \forall n \end{aligned}

start

goal

from Choset, Howie M., et al. *Principles of robot motion: theory, algorithms, and implementation*. MIT press, 2005.

Amato, Nancy M., and Yan Wu. "A randomized roadmap method for path and manipulation planning." *Proceedings of IEEE international conference on robotics and automation*. Vol. 1. IEEE, 1996.

```
BUILD_ROADMAP () {
V = {}, E = {}
for k = 1 to K
repeat
q = RANDOM_CONFIG()
until q is collision free
V.insert(q)
for all q in V
for all qn in NearestNeighbors(q, V)
if {q,qn} is collision free
E.insert({q,qn})
}
```

**The Probabilistic Roadmap (PRM)**

from Choset, Howie M., et al. *Principles of robot motion: theory, algorithms, and implementation*. MIT press, 2005.

- PRM: A roadmap of points (vertices) connected by line segments
- GCSTrajOpt: A roadmap of
*convex sets*(vertices) of continuous curves connected by continuity constraints

- IRIS (Fast approximate convex segmentation).
*Deits and Tedrake, 2014* - New versions work in configuration space:
- Nonlinear optimization for speed.
*IRIS-NP* - Sums-of-squares for rigorous certification.
*C-IRIS*

- Nonlinear optimization for speed.

q_2

q_1

- Naive sampling can lead to inefficient coverage
- New algorithms optimize a convex
*cover*- Insight:
*Cliques*in the*visibility graph*(almost) correspond to convex sets in configuration space - Approach: (Approximate) minimum clique cover

- Insight:

```
BUILD_RRT (qinit) {
T.init(q_init);
for k = 1 to K do
q_rand = RandomSample()
q_near = ClosestPoint(T, q_rand)
q_new = Extend(q_near, q_rand)
T.add(q_near, q_new)
}
```

Naive Sampling

RRTs have a "Voronoi-bias"

http://www.kuffner.org/james/plan

**Cost-to-go for the obstacle-free case**

Basic RRT

Reachability-Guided RRT

Google "drake+ompl" to find some examples (on stackoverflow) of drake integration in C++. Using the python bindings should work, too.