Path-planning, as the name suggests, is the process of determining how to get from one point to another. Path-planning, also called motion planning, has applications in a number of fields, like autonomous robotics and GPS navigation, to name a couple. Before getting into the heart of the matter, let’s start with a demonstration of the Python script and interactive plot we’re going to have up and running by the end of this post:
As you can see from the video, we’re going to learn about and implement a path-planning algorithm that tries to find the shortest path (yellow) on a grid from a start cell (the green square) to a destination cell (the red square) while navigating around obstacles (the black cells). Read on for the details.
There exist a number of techniques and algorithms for path-planning, one of which includes graph-based methods. In graph theory, a graph is defined as a structure consisting of “nodes,” i.e., points, and “edges,” i.e., lines that connect the nodes. In graph-based path-planning, we travel from node to node along the edges, with the aim of figuring out how to get from some initial node to a destination node.
We’re going to examine a particular type of graph: the grid, also known as a lattice graph. Each cell of our grid can be thought of as a node, and the path between each node can be thought of as an edge. The defining characteristic of a grid is that all the edges are the same length. In this post, we’ll be working with a square grid.
In the figure above, left, we see our grid, which looks kind of like a crossword puzzle. The white squares represent empty cells to which we can travel. The black squares are obstacles. The green square is the start cell, and the red square is the destination cell. In the figure, top right, the graph representation of the grid is superimposed on the grid. Each orange dot is a node, and the lines between nodes are edges. In the figure, bottom right, I’ve isolated the graph representation to get a better look at it. Notice how the nodes are connected—from any given node, it’s only possible to travel up, down, right, or left, not diagonally. We don’t have to limit ourselves to this, of course. If we wanted to, we could also define our grid to have diagonal edges between nodes, but we’ll keep things simple for this example.
Finding the shortest path
Our goal will be to find the shortest path from the start cell to the destination cell. A number of methods exist to get from start to finish, including depth-first search, which follows a single path until it either finds the destination or, more likely, hits a dead-end, at which point it backtracks to the last fork in the road and follows a different path—repeat until the destination is found or until there are no paths left to explore. However, this is not guaranteed to find the shortest path, only to find a path if one exists. Another method is the breadth-first search, which explores evenly out from the start cell and is guaranteed to find the shortest path. One of the simplest types of breadth-first search algorithms, which we saw in the video at the beginning of this post, is sometimes referred to as the wavefront or grassfire algorithm, so named because the search path looks like a spreading shockwave or brush fire. The idea behind the algorithm is to mark each cell (node) with its distance from the start cell, exploring the cells adjacent to all cells at the current depth (distance) before moving on to the next depth:
First, we mark our start cell with “0” (see figure above, top left). Then, we look at the cells above, below, to the right, and to the left of the start cell. We mark each of the cells in those directions with a “1”, assuming they’re not obstacles or outside the boundaries of the grid, because they’re 1 step away from the start cell. Since there’s only one cell with a depth of “0”, the start cell, that’s it for the first iteration. On the next iteration, we consider each cell marked with depth “1” and look at the cells adjacent to those, then mark them with “2” to indicate that they’re 2 steps away from the start cell. We continue in this manner, only marking adjacent cells with the next depth if they’re not already marked with a smaller number, which would indicate that they’re already closer to the start cell than the current cell. This process continues until either a) the destination is found or b) all reachable cells have been explored and no path to the destination exists. If the destination is found, we can backtrack to find the shortest path by following the cells in reverse numerical order back to the start cell:
Note that, in our implementation of the algorithm, backtracking will be arbitrary—if there are multiple shortest paths, our algorithm will only choose one of them.
The code, part 1
First, create a file named Grassfire.py (or get it from the Github link above).
After taking care of our imports and defining pi as a constant, we define a
class we’ll call
Grassfire. We’ll use the variables and methods in
this class to create and manipulate our grid, as well as to execute the
Grassfire algorithm. Our grid will be 2D numpy integer array. As discussed
above, we’ll utilize 0 to denote the start cell and positive integers to
indicate the distance of visited cells from the start cell. The negative
integers on lines 14-17 are reserved for, respectively: the destination,
unvisited cells, obstacles, and cells that are part of the shortest path if the
destination is found. Note that these are class variables (which we’re
treating as constants), meaning that they are shared across all instances of a
In addition to the main grid, which contains the locations of the start and
destination, obstacles, distances of visited cells, etc., we’ll also need
a corresponding grid that translates that information into colors. This
colorGrid will simply be an RGB pixel map, with each pixel representing one
cell of our grid. If the main grid has dimensions MxN, then the
have dimensions MxNx3, where the third axis contains the RGB values for each
cell. Lines 21-26 define the RGB colors for each cell type.
random_grid() method is the only method in our class that actually returns
a fresh, randomized grid. Observe that we prefix class attributes (variables)
with the class name, e.g.,
Grassfire.UNVIS. We use the numpy function
random_sample() to generate an array of floats from 0 to 1,
line 34, we initialize our main grid,
grid, to an array representing
unvisited cells. Since
grid have the same dimensions, we
find the values of
obstacleGrid less than our obstacle probability threshold
and set those elements in
grid to be obstacles on line 35. The comparative
obstacleGrid <= obstacleProb produces a Boolean array of the same
obstacleGrid, where elements satisfying the condition are True and the
rest are False. Since this Boolean array is also the same size as
grid, we can
use it to index into
grid and modify only those elements where the condition
Finally, on line 38, we use the method
set_start_dest(), which we’ll
define next, to randomly select start and destination locations. Note that we
set_start_dest() method with
grid without an assignment and
without worrying about the return value of
set_start_dest(). In other words,
notice how we didn’t write line 38 as
grid = self.set_start_dest(grid).
That’s because numpy arrays are passed by reference, not by value. This means
grid to a function doesn’t pass a copy of
it passes a reference to the original array itself, so that any changes
set_start_dest() makes to
grid are made to the original array.
In the method
set_start_dest(), we use the function
select a random integer within the range of the total number of cells in our
grid. This single integer is essentially a flattened index, i.e., the index of
an element in the grid if the 2D grid were flattened into a 1D array by
arranging all the rows next to one another. The numpy function
takes this 1D index and converts it into a pair of 2D indices (note that the
function can be used to convert an index into a set of indices in any number of
dimensions, not just two dimensions).
As we discussed above, the function
color_grid() returns an RGB pixel array to
grid. In the
reset_grid() method, lines 82-83
demonstrate that Boolean arrays can be added to one another.
Finally, we’ll define the methods that actually implement the algorithm, beginning with two helper methods followed by the main pathfinding method. On each iteration, our implementation of the algorithm will check the cells adjacent to every cell matching the current depth. If any of these adjacent cells are unvisited or have a value greater than the current depth + 1, they will be updated to a value of current depth + 1 (indicating that they’re 1 step farther from the start cell than cells of the current depth). We will keep track of how many adjacent cells are updated during each iteration; if, after going through all cells at the current depth, the number of updated cells is 0, it means we’ve explored all visitable cells that can be reached and that no path to the destination exists. During this process, we’ll also be checking to see if the destination is found.
First, the helper method
_check_adjacent() which, given a grid and a cell
within the grid, checks the four cells adjacent to the given cell. Note the use
of sine and cosine to achieve this compactly. Also observe how, in Python, we
can write comparative statements like
0 <= rowToCheck < rows instead of
splitting them into two statements.
The second helper method,
_backtrack(), is similar to the previous method,
except it modifies the first matching adjacent cell to the value given by the
PATH constant, then returns a tuple containing the row and column of the
modified cell. The modified cell will be used as the input to the next call of
_backtrack(). This will repeat until we’ve arrived back at the start cell.
Now the main event:
find_path(). You may notice a couple peculiarities in this
The algorithm is contained in a generator function,
find_path_generator(), which is nested within and returned by
find_path(). For more information on generator functions and generators in Python, read my earlier post on K-means clustering. The reason for this is that we’ll use the matplotlib animation module’s
FuncAnimationfunction to animate the algorithm, and we’ll supply it with a generator function to update the plot at every iteration. But, from the matplotlib documentation on FuncAnimation, the generator function we supply cannot have any input arguments. Essentially, matplotlib will call the generator function to produce a generator. The generator will be used to update the plot. However, matplotlib doesn’t allow us to provide input arguments to the generator function, which is a problem because our algorithm needs one input argument: the grid on which it’s supposed to operate, i.e.,
grid. Nesting the generator function
find_path()allows us to get circumvent this restriction, which brings us to peculiarity number two.
find_path()with the input argument
grid. On line 145, we put
grid(or, more accurately, a reference to it) in a dictionary,
nonlocalDict. Then, on line 147, in the nested generator function
find_path_generator(), we extract
gridfrom the dictionary and proceed to use it normally. Why is this necessary? In Python 2, scope rules for nested functions are a little unintuitive. The inner function,
find_path_generator(), technically has access to all the variables defined in the outer function,
find_path(). However, Python 2 doesn’t allow a nested function to access variables in the outer function’s namespace unless it’s in some kind of container, like a dictionary. Python 3 solves this issue with the
nonlocalstatement. If we were writing this code to work only in Python 3, we could eliminate line 145 and replace line 147 with the statement
nonlocal grid. This is kind of like using the
globalstatement in a function to allow accessing a global variable. But, to ensure compatibility with Python 2, we instead put
gridinto a dictionary, then extract it from the dictionary in the nested function.
A final note on this function: it contains two
yield statements (the second
one only comes into play if the destination is found). This just highlights the
fact that we can use as many yield statements as we wish in a generator
The code, part 2
Now, create a file named animGrassfire.py (or get it from Github). This file will utilize the class we just defined to animate the algorithm using matplotlib.
First, we take care of the necessary imports, set some initial parameters,
Grassfire class, and use it to initialize our
On lines 20-21, we initialize the matplotlib imshow plot, which will display
the pixel array
colorGrid, and get the axis handle for the axis that’s created
to house it. The keyword argument
interpolation='nearest' ensures that each
cell appears as a discrete pixel. Line 22 has nothing to do with our
path-planning grid, but rather refers to the axis gridlines, which will create
the appearance of distinct cells. Lines 27-29 initialize the parameter
labels below the grid on the plot (see the video at the beginning of this post).
update_annotations() will update these text labels if the user
presses a key. The function
set_axis_properties() updates the axis limits and
axis gridlines if the user creates a new grid. The
gridPlot imshow pixel array
limits must be updated separately on line 37. We call each of these
functions once on lines 45-46 to initialize the plot.
Now, we make the figure interactive to allow the user to change parameters on
the fly and/or randomize the grid or start/destination cells. See the previous
blog post on animating inverse kinematics for a brief introduction to
interactive plotting. To prevent conflicts with default matplotlib keyboard
shortcuts, we disconnect all key bindings on line 49, allowing us to
reassign keys as we wish without any unwanted behavior. The docstring at the
beginning of our
on_key_press() function explains how we want to plot to
respond to the keypresses we specify. Note that we reference
matplotlib animation object, even though we haven’t defined it yet. Also note
that, on line 73, if the user presses Ctrl to randomly generate a new grid,
we have to provide a new generator function to the animation object by modifying
_iter_gen parameter. It isn’t apparent from the matplotlib documentation
that this is required, nor is it apparent how to do it, but perusing the
matplotlib animation module source code helped make these facts clearer.
The rest of the code is fairly self-explanatory if you read the
FuncAnimation documentation. We show the plot with the argument
to ensure the figure remains open until the user chooses to exit the script by
pressing Enter (irony intended).
Hopefully, you now have a better understanding of the Grassfire algorithm, interactive plotting in Python with the animation module, or both!