I started writing up a summary of how the A* path-finding algorithm works, and then came across this site by Ray Wenderlich. I realised I couldn't get across the key points anywhere near as clearly as he has done, so I'll strongly encourage you to read his version before going any further.

It's really important to understand what is happening so you can manipulate the approach for your particular use case.

I found some code online from Christian Careaga that does a great job of runing the A* algorithm over a grid of data, from a starting point to a goal point, finding it's way around 'walls' and providing you the final list of steps for the shortest route. So we'll use this to get started!

Once, we've found the best route we'll plot this over our grid to get a visualisation of the path it found.

First though, let's create our grid and plot it so we know what we're dealing with.

Let's create a 20x20 numpy array filled with 1's and 0's as below. The 0's will be positions that we're allowed to travel on, and the 1's will be walls.

Let's also specify that we want to start in the top left corner (denoted in the plot with a yellow star), and we want to travel to the top right corner (red star).

It's worth noting that with data in this format, the origin (location (0,0)) is at the top left rather than the bottom left

import numpy as np

import heapq

import numpy as np

import matplotlib.pyplot as plt

from matplotlib.pyplot import figure

grid = np.array([

[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0],

[0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])

# start point and goal

start = (0,0)

goal = (0,19)

# plot map and path

fig, ax = plt.subplots(figsize=(12,12))

ax.imshow(grid, cmap=plt.cm.Dark2)

ax.scatter(start[1],start[0], marker = "*", color = "yellow", s = 200)

ax.scatter(goal[1],goal[0], marker = "*", color = "red", s = 200)

plt.show()

So now we know what we need to achieve - we need to implement the path finding algorithm to search through the grid and find the best route from our start point to our goal.

The code that Christian created has two main functions, one for the heuristic (remember from Ray's article F = G + H - our method need to score all our potential next moves although we do it slightly different to Ray here...) and one for the main path-finding logic. I'll run through both in a bit of detail.

Heuristic Function:

def heuristic(a, b):

return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)

All this is doing is taking two points, a and b. We use this primarily to calculate our H metric. In Ray's example, he was using Manhattan distance, i.e. the number of up/down/right/left's you'd need to take. In our scenario, we're going to allow diagonal movement, so instead of Manhattan distance we're just going to use straight line distance as our heuristic.

To do this, we just apply Pythagoras's theorem to find the distance between two points, based on us knowing the difference of the two along the x and y axis.

Note: We will also use this function when updating our G score (the movement cost from the starting point to our current point/potential neighbours)

Main Path Finding Function:

def astar(array, start, goal):

neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]

close_set = set()

came_from = {}

gscore = {start:0}

fscore = {start:heuristic(start, goal)}

oheap = []

heapq.heappush(oheap, (fscore[start], start))

while oheap:

current = heapq.heappop(oheap)[1]

if current == goal:

data = []

while current in came_from:

data.append(current)

current = came_from[current]

return data

close_set.add(current)

for i, j in neighbors:

neighbor = current[0] + i, current[1] + j

tentative_g_score = gscore[current] + heuristic(current, neighbor)

if 0 <= neighbor[0] < array.shape[0]:

if 0 <= neighbor[1] < array.shape[1]:

if array[neighbor[0]][neighbor[1]] == 1:

continue

else:

# array bound y walls

continue

else:

# array bound x walls

continue

if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):

continue

if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:

came_from[neighbor] = current

gscore[neighbor] = tentative_g_score

fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)

heapq.heappush(oheap, (fscore[neighbor], neighbor))

This function takes three arguments, array (our grid), the start location, and the goal location. I'll break the rest of it into chunks when running through it

1. Neighbours list

neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]

We first provide a list of possible neighbours, (or more specifically the movement distances to our neighbours at any point, i.e (0,1) in the neighbours list would refer to "move). This is a list of all 8 directional movements from any location; up, up/right, right, down/right, down, down/left, left, up/left

2. Set up

close_set = set()

came_from = {}

gscore = {start:0}

fscore = {start:heuristic(start, goal)}

oheap = []

heapq.heappush(oheap, (fscore[start], start))

- close_set: You'll remember from Ray's article, that we need a closed list. This is a list where we write down the positions that we do not have to consider again. After each subsequent step, we append the current position to this list

- came_from: A dictionary that contains the all the route paths we've taken in each iteration. This contains our 'parent' positions. When we reach the destination, we lookup the shortest path from this object.

- gscore: A dictionary that contains our G scores by iteration

- fscore: A dictionary that contains our F scores by iteration

- oheap: Our open list, containing all the positions that are being considered to find the shortest path

- heapq.heappush: Pushing the start position and F score onto the open list

3. The loop

while oheap:

We want to check for available positions to move to until there are no more options left

4. Extract the position of the neighbour with the smallest F score

current = heapq.heappop(oheap)[1]

heapq.heappop returns the smallest item from the heap (i.e. in this case, the one with the smallest F score in our open list), and we extract element 1 which is the position

5. If we've reached the goal (i.e. our current position = the goal position - extract and return the shortest path

if current == goal:

data = []

while current in came_from:

data.append(current)

current = came_from[current]

return data

6. Otherwise... (i.e. we've not reached the goal)

a) Add the current position to the closed list

close_set.add(current)

b) Loop through all the possible neighbours, calculating their G score

for i, j in neighbors:

neighbor = current[0] + i, current[1] + j

tentative_g_score = gscore[current] + heuristic(current, neighbor)

c) But, if the neighbour is outside the grid (i.e. the potential neighbour is above, below, left, or right of the bounds we've set) then ignore this neighbour and continue the loop

if 0 <= neighbor[0] < array.shape[0]:

if 0 <= neighbor[1] < array.shape[1]:

if array[neighbor[0]][neighbor[1]] == 1:

continue

else:

# array bound y walls

continue

else:

# array bound x walls

continue

d) Also, if the neighbour is in the closed set and the G score is greater than the G score's for that position then ignore and continue the loop

if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):

continue

e) If the G score for the neighbour is less than the other G score's for that position OR if this neighbour is not in the open list (i.e. a new, untested position) then update our lists and add to the open list

if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:

came_from[neighbor] = current

gscore[neighbor] = tentative_g_score

fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)

heapq.heappush(oheap, (fscore[neighbor], neighbor))

Done - hopefully that explanation makes sense, if not let me know and we can discuss further!

To explore any parts of the function that are unclear, I'd recommend throwing in print (i.e. print(oheap) statements within so you can see what is happening each iteration - this is how I figured it out, so it may help you too.

Calling our function, and finding the shortest path

As mentioned above, this function takes three arguments, array (our grid), the start location, and the goal location, so let's give it a whirl...

route = astar(grid, start, goal)

print(route)

[(0, 19), (1, 18), (2, 18), (3, 18), (4, 18), (5, 18), (6, 18), (7, 17), (8, 16), (9, 15), (9, 14), (10, 13), (10, 12), (11, 11), (12, 10), (13, 11), (13, 12), (13, 13), (14, 14), (15, 14), (16, 14), (17, 13), (18, 12), (18, 11), (18, 10), (18, 9), (18, 8), (18, 7), (18, 6), (18, 5), (18, 4), (18, 3), (18, 2), (17, 1), (16, 1), (15, 1), (14, 2), (13, 3), (12, 4), (11, 5), (10, 5), (9, 5), (8, 5), (7, 4), (6, 3), (5, 2), (4, 2), (3, 2), (2, 1), (1, 1)]

This is the entire list of positions for the shortest route, as found by the algorithm.

Those with a keen eye will notice that it's backwards, i.e. it's starting at the end and working backwards. Those with a keener eye will notice that it's also missing the start position. We can fix both these things easily.

Add start position:

route = route + [start]

Reverse the order:

route = route[::-1]

Now if we print this list, we get what we want...

print(route)

[(0, 0), (1, 1), (2, 1), (3, 2), (4, 2), (5, 2), (6, 3), (7, 4), (8, 5), (9, 5), (10, 5), (11, 5), (12, 4), (13, 3), (14, 2), (15, 1), (16, 1), (17, 1), (18, 2), (18, 3), (18, 4), (18, 5), (18, 6), (18, 7), (18, 8), (18, 9), (18, 10), (18, 11), (18, 12), (17, 13), (16, 14), (15, 14), (14, 14), (13, 13), (13, 12), (13, 11), (12, 10), (11, 11), (10, 12), (10, 13), (9, 14), (9, 15), (8, 16), (7, 17), (6, 18), (5, 18), (4, 18), (3, 18), (2, 18), (1, 18), (0, 19)]

Let's plot this now, to visualise the route:

import matplotlib.pyplot as plt

from matplotlib.pyplot import figure

#extract x and y coordinates from route list

x_coords = []

y_coords = []

for i in (range(0,len(route))):

x = route[i][0]

y = route[i][1]

x_coords.append(x)

y_coords.append(y)

# plot map and path

fig, ax = plt.subplots(figsize=(20,20))

ax.imshow(grid, cmap=plt.cm.Dark2)

ax.scatter(start[1],start[0], marker = "*", color = "yellow", s = 200)

ax.scatter(goal[1],goal[0], marker = "*", color = "red", s = 200)

ax.plot(y_coords,x_coords, color = "black")

plt.show()

I think that's pretty cool. It's also very impressive how quickly it gets there considering all the options it considers in the process.

This is quite a simple implementation of path-finding. The function is completely fine, but I guess you could say the grid we've used is a bit simplistic in regards to real world scenarios. In this example, we were able to travel freely in any direction, but this may not always be the case - while there isn't a wall there, perhaps the terrain is too steep, or perhaps a road was closed. What about if you could travel in one direction but not another, i.e. a one way street?

These are things I'd like to cover in part 2.

In part 3, I'm hoping to take this logic and convert it into a 3D world, i.e. considering x,y, and z coordinates. Watch this space...

FULL CODE:

##############################################################################

# import packages

##############################################################################

import numpy as np

import heapq

import matplotlib.pyplot as plt

from matplotlib.pyplot import figure

##############################################################################

# plot grid

##############################################################################

grid = np.array([

[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0],

[0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],

[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])

# start point and goal

start = (0,0)

goal = (0,19)

##############################################################################

# heuristic function for path scoring

##############################################################################

def heuristic(a, b):

return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)

##############################################################################

# path finding function

##############################################################################

def astar(array, start, goal):

neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]

close_set = set()

came_from = {}

gscore = {start:0}

fscore = {start:heuristic(start, goal)}

oheap = []

heapq.heappush(oheap, (fscore[start], start))

while oheap:

current = heapq.heappop(oheap)[1]

if current == goal:

data = []

while current in came_from:

data.append(current)

current = came_from[current]

return data

close_set.add(current)

for i, j in neighbors:

neighbor = current[0] + i, current[1] + j

tentative_g_score = gscore[current] + heuristic(current, neighbor)

if 0 <= neighbor[0] < array.shape[0]:

if 0 <= neighbor[1] < array.shape[1]:

if array[neighbor[0]][neighbor[1]] == 1:

continue

else:

# array bound y walls

continue

else:

# array bound x walls

continue

if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):

continue

if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:

came_from[neighbor] = current

gscore[neighbor] = tentative_g_score

fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)

heapq.heappush(oheap, (fscore[neighbor], neighbor))

return False

route = astar(grid, start, goal)

route = route + [start]

route = route[::-1]

print(route)

##############################################################################

# plot the path

##############################################################################

#extract x and y coordinates from route list

x_coords = []

y_coords = []

for i in (range(0,len(route))):

x = route[i][0]

y = route[i][1]

x_coords.append(x)

y_coords.append(y)

# plot map and path

fig, ax = plt.subplots(figsize=(20,20))

ax.imshow(grid, cmap=plt.cm.Dark2)

ax.scatter(start[1],start[0], marker = "*", color = "yellow", s = 200)

ax.scatter(goal[1],goal[0], marker = "*", color = "red", s = 200)

ax.plot(y_coords,x_coords, color = "black")

plt.show()