Skip to content

Added A* Algorithm #140

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Oct 17, 2017
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
101 changes: 101 additions & 0 deletions Graphs/A*.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@

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]]

'''
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] #all coordinates are given in format [y,x]
cost = 1

#the cost map which pushes the path closer to the goal
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])):
heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
if grid[i][j] == 1:
heuristic[i][j] = 99 #added extra penalty in the heuristic map


#the actions we can take
delta = [[-1, 0 ], # go up
[ 0, -1], # go left
[ 1, 0 ], # go down
[ 0, 1 ]] # go right


#function to search the path
def search(grid,init,goal,cost,heuristic):

closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]# the referrence grid
closed[init[0]][init[1]] = 1
action = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]#the action grid

x = init[0]
y = init[1]
g = 0
f = g + heuristic[init[0]][init[0]]
cell = [[f, g, x, y]]

found = False # flag that is set when search is complete
resign = False # flag set if we can't find expand

while not found and not resign:
if len(cell) == 0:
resign = True
return "FAIL"
else:
cell.sort()#to choose the least costliest action so as to move closer to the goal
cell.reverse()
next = cell.pop()
x = next[2]
y = next[3]
g = next[1]
f = next[0]


if x == goal[0] and y == goal[1]:
found = True
else:
for i in range(len(delta)):#to try out different valid actions
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]):
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]
invpath.append([x, y])#we get the reverse path from here
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)):
path.append(invpath[len(invpath) - 1 - i])
print "ACTION MAP"
for i in range(len(action)):
print action[i]

return path

a = search(grid,init,goal,cost,heuristic)
for i in range(len(a)):
print a[i]