Shorter, smoother paths

RRT paths wander

Every RRT edge points at a random sample, and the search stops at the first path it finds. So the path zig-zags: the arm swings back and forth on its way, and the motion takes far longer than it needs to. Run the starter and watch the purple route.

Measure a path by its length in joint space, summed over its segments:

Random shortcutting

The cheapest fix is also the most common. MoveIt runs a version of it on almost every plan:

1. Pick two different segments $i < j$ of the path, and a random point on each: $a = q_i + u\,(q_{i+1} - q_i)$ and $b = q_j + v\,(q_{j+1} - q_j)$, with $u, v$ uniform in $[0, 1]$.
2. If the straight edge $a \to b$ is free, splice it in: keep the path up to $q_i$, then $a$, $b$, and the rest from $q_{j+1}$.
3. Repeat, say, 200 times.

A straight line is the shortest way between two points, so every accepted shortcut makes the path shorter. Picking points part-way along segments matters: it lets the path cut corners, not just skip whole waypoints.

Your task

The starter comes with a working rrt. Implement shortcut(path, iters=200), then set path = shortcut(rrt(START, GOAL)) and drive it inside phase("execute").

The arm's actual motion must be at most 3.0 rad long in joint space. A good shortcut lands near 2 rad, so that leaves room for an unlucky run.