RRT and PRM

Nathan Sprague

def rrt(problem, q_init, tree_size):
    """ Build a rapidly exploring random tree. 
    """
    tree = Tree(q_init)  # Make the start state the root of the tree
    while tree.num_nodes() < tree_size:
        q_rand = problem.random_state()
        q_near = nearest_neighbor(tree, q_rand)
        u = problem.select_input(q_rand, q_near)
        q_new = problem.new_state(q_near, u)
        tree.add_node(q_new)
        tree.add_edge(q_near, q_new)
    return tree

def prm(problem, delta, roadmap_size):
    """ Create a Probabilistic Roadmap.
    """
    graph = Graph()
    while graph.num_nodes() < roadmap_size:
        q_rand = problem.random_state()
        graph.add_node(q_rand)
        for q in neighbors(graph, q_rand, delta):
            if problem.no_collision(q, q_rand):
                graph.add_edge(q, q_rand)
    return graph