In [10]:
import numpy as np
from matplotlib import pyplot as plt
from matplotlib.animation import FuncAnimation

plt.style.use('seaborn-darkgrid')

def print_line(line, delimiter):
print('[',end="")
for c in line:
print("{:^3}".format(str(c)), end="")
print(']',end="")
if delimiter:
print(',',end="")

def print_2d_list(input_list):
print('[', end="")
print_line(input_list[0], True)
print()
for row in input_list[1:-1]:
print(" ", end="")
print_line(row, True)
print()
print(" ", end="")
print_line(input_list[-1], False)
print(']')

def is_in_map(grid, x, y):
if x >= 0 and x < len(grid) and y >=0 and y < len(grid[0]):
return True
return False

def valid_step(grid, observed,  x, y):
if is_in_map(grid, x, y):
# check if the cell is blocked or has been visited
if grid[x][y] == 0 and observed[x][y] == 0:
return True
return False

def search(grid, heuristic, init, goal, cost, delta, delta_name):

# initial location characteristics
x = init[0]
y = init[1]
f = heuristic[x][y]
g = 0
observed_cells = [[0 for cell in row] for row in grid]
moves = [[0 for cell in row] for row in grid]
expansion = [[0 for cell in row] for row in grid]

# cells that shall be examined next, unless the target has been found
search_queue = [[f,g,x,y]]

found = False
resign = False

# check if there is sill a potential path to the target
if len(search_queue) ==0:
resign = True
else:
# by examining the cell with the smallest f value
# it is guaranteed to find the shortest path if it exists
search_queue.sort()
next = search_queue.pop(0) # cell to be examined

f = next[0]
g = next[1]
x = next[2]
y = next[3]

# check if the cell is the goal
if x == goal[0] and y == goal[1]:
found = True
else:
# expand to new cells if possible
for i in range(len(delta)):
x_next = x + delta[i][0]
y_next = y + delta[i][1]
if valid_step(grid, observed_cells, x_next, y_next):
g_next = g + cost
f_next = g_next + heuristic[x_next][y_next]
# expand the search queue
search_queue.append([f_next, g_next, x_next, y_next])
observed_cells[x_next][y_next] = 1  # so it will not be readded to the search_queue
moves[x_next][y_next] = i
expansion[x][y] =  delta_name[i]

return [expansion, moves]

def shortest_path_finder(actions, delta, delta_name, init, goal):
# the shortest path is traced back from the goal location
x = goal[0]
y = goal[1]

policy = [["x" for cell in row] for row in actions]
policy[x][y] = '*'

shortest_path = [[x,y]]

while x != init[0] or  y != init[1]: # while we have not reached back to the initial location
x_back = x - delta[actions[x][y]][0]
y_back = y - delta[actions[x][y]][1]
policy[x_back][y_back] = delta_name[actions[x][y]]
x = x_back
y = y_back
shortest_path.append([x,y])

return policy, shortest_path

def main():
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 1, 0, 0],
[0, 1, 0, 1, 0, 0],
[0, 0, 0, 1, 0, 0]]

heuristic = [[9, 8, 7, 6, 5, 4],
[8, 7, 6, 5, 4, 3],
[7, 6, 5, 4, 3, 2],
[6, 5, 4, 3, 2, 1],
[5, 4, 3, 2, 1, 0]]

init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0 ], # go up
[ 0, -1], # go left
[ 1, 0 ], # go down
[ 0, 1 ]] # go right

delta_name = ['^', '<', 'v', '>']

expansions, actions_ids  = search(grid, heuristic, init, goal, cost, delta, delta_name)
policy, shortest_path = shortest_path_finder(actions_ids, delta, delta_name, init, goal)

print_2d_list( expansions )
print()
print(shortest_path)
print_2d_list( policy )

if __name__=="__main__":
main()


[[ v  0  0  0  0  0 ],
[ v  0  >  >  >  v ],
[ v  0  ^  0  v  v ],
[ v  0  ^  0  v  v ],
[ >  >  ^  0  0  0 ]]

[[4, 5], [3, 5], [2, 5], [1, 5], [1, 4], [1, 3], [1, 2], [2, 2], [3, 2], [4, 2], [4, 1], [4, 0], [3, 0], [2, 0], [1, 0], [0, 0]]
[[ v  x  x  x  x  x ],
[ v  x  >  >  >  v ],
[ v  x  ^  x  x  v ],
[ v  x  ^  x  x  v ],
[ >  >  ^  x  x  * ]]


In [ ]: