Python/graphs/a_star.py

101 lines
3.0 KiB
Python
Raw Normal View History

2019-10-05 05:14:13 +00:00
grid = [
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0], # 0 are free path whereas 1's are obstacles
[0, 1, 0, 0, 0, 0],
[0, 1, 0, 0, 1, 0],
[0, 0, 0, 0, 1, 0],
]
"""
2018-10-19 12:48:28 +00:00
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],
2019-10-05 05:14:13 +00:00
[5, 4, 3, 2, 1, 0]]"""
2018-10-19 12:48:28 +00:00
init = [0, 0]
2019-10-05 05:14:13 +00:00
goal = [len(grid) - 1, len(grid[0]) - 1] # all coordinates are given in format [y,x]
2018-10-19 12:48:28 +00:00
cost = 1
2019-10-05 05:14:13 +00:00
# the cost map which pushes the path closer to the goal
2018-10-19 12:48:28 +00:00
heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):
for j in range(len(grid[0])):
2018-10-19 12:48:28 +00:00
heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
if grid[i][j] == 1:
2019-10-05 05:14:13 +00:00
heuristic[i][j] = 99 # added extra penalty in the heuristic map
2018-10-19 12:48:28 +00:00
2019-10-05 05:14:13 +00:00
# the actions we can take
delta = [[-1, 0], [0, -1], [1, 0], [0, 1]] # go up # go left # go down # go right
2018-10-19 12:48:28 +00:00
2019-10-05 05:14:13 +00:00
# function to search the path
def search(grid, init, goal, cost, heuristic):
2018-10-19 12:48:28 +00:00
2019-10-05 05:14:13 +00:00
closed = [
[0 for col in range(len(grid[0]))] for row in range(len(grid))
] # the reference grid
2018-10-19 12:48:28 +00:00
closed[init[0]][init[1]] = 1
2019-10-05 05:14:13 +00:00
action = [
[0 for col in range(len(grid[0]))] for row in range(len(grid))
] # the action grid
2018-10-19 12:48:28 +00:00
x = init[0]
y = init[1]
g = 0
f = g + heuristic[x][y] # cost from starting cell to destination cell
2018-10-19 12:48:28 +00:00
cell = [[f, g, x, y]]
found = False # flag that is set when search is complete
2019-10-05 05:14:13 +00:00
resign = False # flag set if we can't find expand
2018-10-19 12:48:28 +00:00
while not found and not resign:
if len(cell) == 0:
return "FAIL"
else: # to choose the least costliest action so as to move closer to the goal
cell.sort()
2018-10-19 12:48:28 +00:00
cell.reverse()
next = cell.pop()
x = next[2]
y = next[3]
g = next[1]
if x == goal[0] and y == goal[1]:
found = True
else:
2019-10-05 05:14:13 +00:00
for i in range(len(delta)): # to try out different valid actions
2018-10-19 12:48:28 +00:00
x2 = x + delta[i][0]
y2 = y + delta[i][1]
2019-10-05 05:14:13 +00:00
if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]):
2018-10-19 12:48:28 +00:00
if closed[x2][y2] == 0 and grid[x2][y2] == 0:
g2 = g + cost
f2 = g2 + heuristic[x2][y2]
cell.append([f2, g2, x2, y2])
closed[x2][y2] = 1
action[x2][y2] = i
invpath = []
x = goal[0]
y = goal[1]
2019-10-05 05:14:13 +00:00
invpath.append([x, y]) # we get the reverse path from here
2018-10-19 12:48:28 +00:00
while x != init[0] or y != init[1]:
x2 = x - delta[action[x][y]][0]
y2 = y - delta[action[x][y]][1]
x = x2
y = y2
invpath.append([x, y])
path = []
for i in range(len(invpath)):
2019-10-05 05:14:13 +00:00
path.append(invpath[len(invpath) - 1 - i])
2018-10-19 12:48:28 +00:00
print("ACTION MAP")
for i in range(len(action)):
print(action[i])
2018-10-19 12:48:28 +00:00
return path
2018-10-19 12:48:28 +00:00
2019-10-05 05:14:13 +00:00
a = search(grid, init, goal, cost, heuristic)
for i in range(len(a)):
print(a[i])