Probabilistic roadmap

What it computes

RRT starts from scratch for every query. A probabilistic roadmap (PRM) does the expensive part once: it builds a graph of free space, and then each query is a cheap graph search. That pays off when the robot plans many motions in the same workspace.

Build

until the roadmap has n nodes:
    q = uniform random configuration
    if q is collision-free:
        add q as a node
        for each of its k nearest nodes j:
            if segment q -> j is free: add edge (q, j), weighted by its length

Query

Connect the start and the goal to the roadmap as extra nodes, in the same way. Then run Dijkstra's algorithm from start to goal:

dist[start] = 0; heap = [(0, start)]
while heap:
    d, i = pop smallest          # skip i if it's already done
    if i == goal: stop
    for each edge (i, j, w):
        if d + w < dist[j]: dist[j] = d + w; prev[j] = i; push (d + w, j)
follow prev back from the goal

Tools

arm.in_collision(q) checks one configuration and arm.segment_free(qa, qb) checks a straight motion. heapq is Python's priority queue. Sample inside LO–HI, a box that covers the useful part of joint space.

See it in context

Motion Planning's bonus step builds a lazy PRM and uses it for pick-and-place.

Your task

Implement build_roadmap(n, k), which fills nodes and edges, and plan(a, b), which returns a path from a to b through the roadmap. The program builds 120 nodes, then tours three goals: above the cup, low in front, and home. The grader also sends plan two queries of its own.