Rapidly-exploring random trees

Search, don't solve

There's no formula for $\mathcal{C}_{\text{free}}$, so planners search it. A rapidly-exploring random tree (RRT) grows a tree of free configurations out of the start:

1. Sample a random $q_{\text{rand}}$ within the joint limits. About 10% of the time, use the goal instead. This goal bias pulls the tree towards the goal.
2. Find the tree node $q_{\text{near}}$ nearest to it.
3. Steer: take at most one step towards it, $q_{\text{new}} = q_{\text{near}} + \min(1, \text{step} / \lVert d \rVert)\, d$ with $d = q_{\text{rand}} - q_{\text{near}}$.
4. If the edge $q_{\text{near}} \to q_{\text{new}}$ is free, add $q_{\text{new}}$ with parent $q_{\text{near}}$.
5. If $q_{\text{new}}$ is within one step of the goal and the edge to it is free, add the goal and stop. (When a goal sample was within reach, $q_{\text{new}}$ already is the goal: just stop.)

Each node remembers its parent. Walking the parents back from the goal gives you the path.

Why it works

A node on the tree's frontier is nearest to a huge region of unexplored space, so random samples keep dragging the tree outwards. RRT is probabilistically complete: if a path exists, the chance of finding it goes to 1 as the tree grows.

New tool: arm.segment_free(qa, qb)

It's your Step 2 function, built in.

Your task

Implement rrt(start, goal) and set path = rrt(START, GOAL). START is the home pose and GOAL is 20 cm above the cup. Draw the tree with draw_tree, then drive the path inside phase("execute").

The starter's rrt returns the straight line [start, goal], which goes through the wall. Call np.random.seed(…) first so every run plans the same path.