@@ -0,0 +1,1749 @@
+# -*- coding: utf-8 -*-%0Aimport random%0A%0Aimport draw%0Aimport grid%0Aimport util%0A%0Adef rrt(g, start, goal, max_iter=10000):%0A    nodes = %5Bstart%5D%0A    edges = %5B%5D%0A    for i in range(max_iter):%0A        q = (random.randrange(g.width), random.randrange(g.height))%0A        # Find closest node%0A        closest = nodes%5B0%5D%0A        dist = grid.dist(closest, q)%0A        for n in nodes%5B1:%5D:%0A            if grid.dist(n, q) %3C dist:%0A                closest = n%0A                dist = grid.dist(n, q)%0A        # Find grid cell that is most in direction of sample%0A        dx = closest%5B0%5D - q%5B0%5D%0A        dy = closest%5B1%5D - q%5B1%5D%0A        x = max(-1, min(round(dx/dy), 1)) if dy != 0 else 1%0A        y = max(-1, min(round(dy/dx), 1)) if dx != 0 else 1%0A        c = (closest%5B0%5D-x, closest%5B1%5D-y)%0A        if c not in g.neighbours(*closest) or c in nodes:%0A            continue # skip inaccessible and already visited cells%0A        nodes.append(c)%0A        edges.append((closest, c))%0A        if c == goal:%0A            print('arrived at goal')%0A            return reconstruct(edges, start, goal)%0A    print('No path found')%0A%0Adef reconstruct(edges, start, goal):%0A    path = %5Bgoal%5D%0A    current = goal%0A    for i in range(len(edges)*100):%0A        for s, e in edges:%0A            if e == current:%0A                path.append(s)%0A                current = s%0A        if current == start:%0A            path.reverse()%0A            print(path)%0A            return path%0A    raise Exception('Could not reconstruct the path')%0A%0Aif __name__ == '__main__':%0A    g, start, goal = util.generate_problem(16, 16, 0.2)%0A    print('Start:', start, 'goal:', goal)%0A    path = rrt(g, start, goal)%0A    print('Found length vs heuristic:', len(path), grid.dist(start, goal))%0A%0A    draw.draw_path(draw.draw_grid(g), path).show()%0A

Implement RRT