Planning Algorithm Visualizations

Project Description

This project contains Python implementations of the popular planning algorithms with visual animation. For now, A-Star and Rapidly-exploring Random Tree (RRT) are included while more will be added in the future.

---

A*

The search-based algortihm A* treats the search space as in a weighted graph. Given a starting node and an end node, A* can find the path between the nodes with the smallest cost. A* is the combination of two algorithms: Dijkstra’s algorithm and Greedy Best-First search. The first one focuses on calculating the distance from the starting point while the latter focuses on calculating the distance to the goal point. A* uses both perspective to find the path. This algorithm is demonstrated in a discrete space. In the demo, the blue grid is the starting point, the orange grid is the goal point, the gray grids are the walls, the white grids are the empty space, the green grids are the current explored space, and the red grids will be the final output path.

Pseudocode[1]:

define start_point
define end_point

list open_list = []
list close_list = []

open_list append start_point

while open_list not empty:
    q = node with least f value in open_list
    open_list.pop(f)
    for each neighbor n of q:
        n.parents append q
        if q is end_point:
            end search
        
        n.g = q.g+h(n,q)
        n.h = h(end_point, n)
        n.f = n.g+n.h


        if n is in open_list as m and m.f < n.f:
            skip adding this neighbor
        if n is in close_list as m and m.f < n.f:
            skip adding this neighbor

        otherwise open_list append n

    close_list append q

Demo

---

RRT

RRT stands for Rapidly-exploring random tree. It is especially good for single-query planning problems. The high-level idea is to explore the state space by building a tree and look for path that can reach from the starting point. This algorithm is demonstrated in a continuous space. In the demo, the white spaces are the empty space, the black spaces are the obstacles, the black edges are the walls, the blue lines and nodes are the paths, the gray point is the goal point, the red lines and nodes are the final output path.

Pseudocode[2]:

Init graph G with start_point
for K times:
    q_rand = random point within the planning domain
    q_near = nearest_vertex(q_rand, G)
    q_new = new_config(q_near, q_rand, unit_step)
    G add vertex q_new
    G add edge (q_new, q_near)

return G

Demo

---

Source Code

More detaild can be found on Github.

Reference

[1]http://coecsl.ece.illinois.edu/ge423/lecturenotes/AstarHandOut.pdf
[2]LaValle, Steven M. “Rapidly-exploring random trees: A new tool for path planning.” (1998): 98-11.