Dijkstra's Algorithm for Grids in Python
There are many resources for implementing Dijkstra's algorithm for graphs on the internet, but there are few resources for implementing the algorithm for the special case where the graph is a 2d grid with obstacles. Moreover, the various library implementations all assume some sort of graph data structure which makes sense; graphs are the natural structure for the algorithm, and you could convert an obstacle-grid to a graph. However, sometimes all you want and need is a 2d grid which is exactly what I have implemented for your convenience. If you just want the code, you can find it at the end of the article, but first let us discuss the algorithm.
Dijkstra's Algorithm
Dijkstra's algorithm is an algorithm for finding the shortest path between two points potentially avoiding obstacles on the way. It works by exploring the distances to all nodes in the grid starting at the source node, always considering the shortest path yet, until the desired node is found in a bread-first search manner.
The idea is to choose a tile that is the current shortest path, and then compute the distance to neighbouring tiles (up down left right) repeat until the destination node is found.
- Initialize the distances as "infinite" and mark all tiles in the matrix as unvisited. Furthermore, initialize an obstacle matrix that contains ones for all tiles except the obstacles which contain a very high number - "infinite", so we discourage the algorithm from going to those tiles.
- Set the distance of the entry and destination tile to 0, indicating that it can be reached directly from the source.
- Repeat the following steps until all vertices have been visited:
- Select the unvisited tile with the smallest distance from the source.
- Mark this tile as visited.
- For each unvisited neighbor (up down left right) of this tile, calculate the distance to that neighbor as the sum of the distance to the current tile and the cost of moving on that tile. If this distance is smaller than the current estimated distance to the neighbor, update the neighbor's distance with the new value - Once we have reached the destination tile, the algorithm is finished, and we have found the shortest path.
In code, we get:
Initialize the maps:
# initialize cost heuristic map obstacles = obstacles.copy() # obstacles should have very high cost, so we avoid them. obstacles *= 1000 # normal tiles should have 1 cost (1 so we can backtrack) obstacles += np.ones(obstacles.shape) # source and destination are free obstacles[initial_node[0],initial_node[1]] = 0 obstacles[desired_node[0],desired_node[1]] = 0 # initialize maps for distances and visited nodes size_of_floor = obstacles.shape[0] # we only want to visit nodes once visited = np.zeros([size_of_floor,size_of_floor],bool) # initiate matrix to keep track of distance to source node # initial distance to nodes is infinity so we always get a lower actual distance distances = np.ones([size_of_floor,size_of_floor]) * np.inf # initial node has a distance of 0 to itself distances[initial_node[0],initial_node[1]] = 0
Then we repeat the following:
For each potential direction we can go (up down left right), we check that the potential tile is valid (within the grid) and not visited.
directions = [up, down, left, right] for direction in directions: potential_node = direction(current_node) if valid_node(potential_node, size_of_floor): # boundary checking if not visited[potential_node[0],potential_node[1]]:
If so, we want to update the distance to that node if it's the shortest path to that node we have discovered so far.
# update distance if it is the shortest discovered if distance < distances[potential_node[0],potential_node[1]]: distances[potential_node[0],potential_node[1]] = distance
Finally, after exhausting all the potential nodes, we mark the current node as visited, and choose the next node to be the node with the current shortest path.
# mark current node as visited visited[current_node[0] ,current_node[1]] = True # select next node # by choosing the the shortest path so far t=distances.copy() # we don't want to visit nodes that have already been visited t[np.where(visited)]=np.inf # choose the shortest path node_index = np.argmin(t) # convert index to row,col. node_row = node_index//size_of_floor node_col = node_index%size_of_floor # update current node. current_node = (node_row, node_col)
If the next node we select is the destination node, we stop the algorithm, and backtrack to get the actual path.
Backtracking is very simple, we just start with the destination node, and choose the next node to be the one with the shortest distance in a greedy fashion until we reach the source node.
The code
(for copy-pasting)
import numpy as np def valid_node(node, size_of_grid): """Checks if node is within the grid boundaries.""" if node[0] < 0 or node[0] >= size_of_grid: return False if node[1] < 0 or node[1] >= size_of_grid: return False return True def up(node): return (node[0]-1,node[1]) def down(node): return (node[0]+1,node[1]) def left(node): return (node[0],node[1]-1) def right(node): return (node[0],node[1]+1) def backtrack(initial_node, desired_node, distances): # idea start at the last node then choose the least number of steps to go back # last node path = [desired_node] size_of_grid = distances.shape[0] while True: # check up down left right - choose the direction that has the least distance potential_distances = [] potential_nodes = [] directions = [up,down,left,right] for direction in directions: node = direction(path[-1]) if valid_node(node, size_of_grid): potential_nodes.append(node) potential_distances.append(distances[node[0],node[1]]) least_distance_index = np.argmin(potential_distances) path.append(potential_nodes[least_distance_index]) if path[-1][0] == initial_node[0] and path[-1][1] == initial_node[1]: break return list(reversed(path)) def dijkstra(initial_node, desired_node, obstacles): """Dijkstras algorithm for finding the shortest path between two nodes in a graph. Args: initial_node (list): [row,col] coordinates of the initial node desired_node (list): [row,col] coordinates of the desired node obstacles (array 2d): 2d numpy array that contains any obstacles as 1s and free space as 0s Returns: list[list]: list of list of nodes that form the shortest path """ # initialize cost heuristic map obstacles = obstacles.copy() # obstacles should have very high cost, so we avoid them. obstacles *= 1000 # normal tiles should have 1 cost (1 so we can backtrack) obstacles += np.ones(obstacles.shape) # source and destination are free obstacles[initial_node[0],initial_node[1]] = 0 obstacles[desired_node[0],desired_node[1]] = 0 # initialize maps for distances and visited nodes size_of_floor = obstacles.shape[0] # we only want to visit nodes once visited = np.zeros([size_of_floor,size_of_floor],bool) # initiate matrix to keep track of distance to source node # initial distance to nodes is infinity so we always get a lower actual distance distances = np.ones([size_of_floor,size_of_floor]) * np.inf # initial node has a distance of 0 to itself distances[initial_node[0],initial_node[1]] = 0 # start algorithm current_node = [initial_node[0], initial_node[1]] while True: directions = [up, down, left, right] for direction in directions: potential_node = direction(current_node) if valid_node(potential_node, size_of_floor): # boundary checking if not visited[potential_node[0],potential_node[1]]: # check if we have visited this node before # update distance to node distance = distances[current_node[0], current_node[1]] + obstacles[potential_node[0],potential_node[1]] # update distance if it is the shortest discovered if distance < distances[potential_node[0],potential_node[1]]: distances[potential_node[0],potential_node[1]] = distance # mark current node as visited visited[current_node[0] ,current_node[1]] = True # select next node # by choosing the the shortest path so far t=distances.copy() # we don't want to visit nodes that have already been visited t[np.where(visited)]=np.inf # choose the shortest path node_index = np.argmin(t) # convert index to row,col. node_row = node_index//size_of_floor node_col = node_index%size_of_floor # update current node. current_node = (node_row, node_col) # stop if we have reached the desired node if current_node[0] == desired_node[0] and current_node[1]==desired_node[1]: break # backtrack to construct path return backtrack(initial_node,desired_node,distances)
For running the code:
import matplotlib.pyplot as plt obstacles = np.array([[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,1,0,1,0,0,0,0], [1,1,1,1,0,1,1,0,0,0], [0,0,0,1,0,1,0,0,0,0], [0,0,0,0,0,1,1,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]], dtype=float) path = dijkstra([0,0],[3,4],obstacles) print(path) p = np.zeros(shape=obstacles.shape) for i in range(len(path)): p[path[i][0],path[i][1]] = np.nan plt.imshow(p+obstacles, cmap='jet') plt.show()
