RRT

What it computes

A rapidly-exploring random tree (RRT) finds a collision-free path through a space too big to grid, such as this arm's 4-D joint space. It never builds an explicit map of the obstacles. It only asks one question, over and over: is this short straight motion free?

The algorithm

tree = {start}
repeat:
    q_rand = goal with probability goal_bias, else a uniform random q
    q_near = the tree node nearest q_rand
    q_new  = q_near moved at most step towards q_rand
    if segment q_near -> q_new is free:
        add q_new to the tree, with parent q_near
        if q_new is within step of goal and q_new -> goal is free:
            add goal (parent q_new) and stop
walk the parents back from the goal to get the path

Nodes on the tree's frontier are nearest to the most unexplored space, so random samples keep pulling the tree outwards. RRT is probabilistically complete: if a path exists, the probability of finding it tends to 1. The path it finds is valid, not short.

Tools

arm.segment_free(qa, qb) checks a straight joint-space motion. arm.limits bounds the samples.

See it in context

Motion Planning builds the collision checker and edge checker behind segment_free, then shortens RRT's paths and uses them for pick-and-place.

Your task

Implement rrt(start, goal). It returns a list of configurations from start to goal. The program plans from home to 20 cm above the cup (the wall is in the way), draws the path and drives it. The grader then plans two more queries with your function.