Implementation of A *

A* use a heuristic function, for our example we will use the python tool.

Python Implementation

There are a few extra bits that you can find in implementation.py. These use Python 3 so if you use Python 2, you will need to change the super() call and the print function to the Python 2 equivalents.

Breadth First Search

Let’s implement Breadth First Search in Python. The main article shows the Python code for the search algorithm, but we also need to define the graph it works on. These are the abstractions I’ll use:

Graph

a data structure that can tell me the neighbors for each location (see this tutorial). A weighted graph can also tell me the cost of moving along an edge.

Locations

a simple value (int, string, tuple, etc.) that labels locations in the graph

Search

an algorithm that takes a graph, a starting location, and optionally a goal location, and calculates some useful information (visited, parent pointer, distance) for some or all locations

Queue

a data structure used by the search algorithm to decide the order in which to process the locations

In the main article, I focused on search. On this page, I’ll fill in the rest of the details to make complete working programs. Let’s start with a graph:

class SimpleGraph:
    def __init__(self):
        self.edges = {}
    
    def neighbors(self, id):
        return self.edges[id]

Early Exit

Following the code from the main article, all we need to do is add an if statement in the main loop. This test is optional for Breadth First Search or Dijkstra’s Algorithm and effectively required for Greedy Best-First Search and A*:

from implementation import *

def breadth_first_search_3(graph, start, goal):
    frontier = Queue()
    frontier.put(start)
    came_from = {}
    came_from[start] = None
    
    while not frontier.empty():
        current = frontier.get()
        
        if current == goal:
            break
        
        for next in graph.neighbors(current):
            if next not in came_from:
                frontier.put(next)
                came_from[next] = current
    
    return came_from

g = SquareGrid(30, 15)
g.walls = DIAGRAM1_WALLS

parents = breadth_first_search_3(g, (8, 7), (17, 2))
draw_grid(g, width=2, point_to=parents, start=(8, 7), goal=(17, 2))

 . > > > v v v v v v v v v v v v < . . . . ####. . . . . . . 
> > > > > v v v v v v v v v v < < < . . . ####. . . . . . . 
> > > > > v v v v v v v v v < < < Z . . . ####. . . . . . . 
> > ^ ####v v v v v v v v < < < < < < . . ####. . . . . . . 
. ^ ^ ####> v v v v v v < ####^ < < . . . ####. . . . . . . 
. . ^ ####> > v v v v < < ####^ ^ . . . . ##########. . . . 
. . . ####> > > v v < < < ####^ . . . . . ##########. . . . 
. . . ####> > > A < < < < ####. . . . . . . . . . . . . . . 
. . . ####> > ^ ^ ^ < < < ####. . . . . . . . . . . . . . . 
. . v ####> ^ ^ ^ ^ ^ < < ####. . . . . . . . . . . . . . . 
. v v ####^ ^ ^ ^ ^ ^ ^ < ####. . . . . . . . . . . . . . . 
> v v ####^ ^ ^ ^ ^ ^ ^ ^ ####. . . . . . . . . . . . . . . 
> > > > > ^ ^ ^ ^ ^ ^ ^ ^ ####. . . . . . . . . . . . . . . 
> > > > ^ ^ ^ ^ ^ ^ ^ ^ ^ ####. . . . . . . . . . . . . . . 
. > > ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ ####. . . . . . . . . . . . . . . 

You can see that the algorithm stops when it finds the goal Z.

A* Search

Both Greedy Best-First Search and A* use a heuristic function. The only difference is that A* uses both the heuristic and the ordering from Dijkstra’s Algorithm. I’m going to show A* here.

def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)

def a_star_search(graph, start, goal):
    frontier = PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    cost_so_far = {}
    came_from[start] = None
    cost_so_far[start] = 0
    
    while not frontier.empty():
        current = frontier.get()
        
        if current == goal:
            break
        
        for next in graph.neighbors(current):
            new_cost = cost_so_far[current] + graph.cost(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
    
    return came_from, cost_so_far

Let’s try it out:

from implementation import *
start, goal = (1, 4), (7, 8)
came_from, cost_so_far = a_star_search(diagram4, start, goal)
draw_grid(diagram4, width=3, point_to=came_from, start=start, goal=goal)
print()
draw_grid(diagram4, width=3, number=cost_so_far, start=start, goal=goal)
print() 

.  .  .  .  .  .  .  .  .  .  
.  v  v  v  .  .  .  .  .  .  
v  v  v  v  <  .  .  .  .  .  
v  v  v  <  <  .  .  .  .  .  
>  A  <  <  <  .  .  .  .  .  
>  ^  <  <  <  .  .  .  .  .  
>  ^  <  <  <  <  .  .  .  .  
^  #########^  .  v  .  .  .  
^  #########v  v  v  Z  .  .  
^  <  <  <  <  <  <  <  .  .  

.  .  .  .  .  .  .  .  .  .  
.  3  4  5  .  .  .  .  .  .  
3  2  3  4  9  .  .  .  .  .  
2  1  2  3  8  .  .  .  .  .  
1  A  1  6  11 .  .  .  .  .  
2  1  2  7  12 .  .  .  .  .  
3  2  3  4  9  14 .  .  .  .  
4  #########14 .  18 .  .  .  
5  #########15 16 13 Z  .  .  
6  7  8  9  10 11 12 13 .  .