From 8cd515750715cb5fa170a3e737c09d1784795341 Mon Sep 17 00:00:00 2001 From: Yashasvi Baweja Date: Fri, 27 Oct 2017 11:34:50 +0530 Subject: [PATCH] Added Multi Heuristic Astar Implemented the paper www.cs.cmu.edu/~venkatrn/papers/ijrr16.pdf --- Multi_Hueristic_Astar.py | 262 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 262 insertions(+) create mode 100644 Multi_Hueristic_Astar.py diff --git a/Multi_Hueristic_Astar.py b/Multi_Hueristic_Astar.py new file mode 100644 index 000000000..03652d35a --- /dev/null +++ b/Multi_Hueristic_Astar.py @@ -0,0 +1,262 @@ +import heapq +import numpy as np +import math +import copy + + +class PriorityQueue: + def __init__(self): + self.elements = [] + self.set = set() + + def minkey(self): + if not self.empty(): + return self.elements[0][0] + else: + return float('inf') + + def empty(self): + return len(self.elements) == 0 + + def put(self, item, priority): + if item not in self.set: + heapq.heappush(self.elements, (priority, item)) + self.set.add(item) + else: + # update + # print("update", item) + temp = [] + (pri, x) = heapq.heappop(self.elements) + while x != item: + temp.append((pri, x)) + (pri, x) = heapq.heappop(self.elements) + temp.append((priority, item)) + for (pro, xxx) in temp: + heapq.heappush(self.elements, (pro, xxx)) + + def remove_element(self, item): + if item in self.set: + self.set.remove(item) + temp = [] + (pro, x) = heapq.heappop(self.elements) + while x != item: + temp.append((pro, x)) + (pro, x) = heapq.heappop(self.elements) + for (prito, yyy) in temp: + heapq.heappush(self.elements, (prito, yyy)) + + def top_show(self): + return self.elements[0][1] + + def get(self): + (priority, item) = heapq.heappop(self.elements) + self.set.remove(item) + return (priority, item) + +def consistent_hueristic(P, goal): + # euclidean distance + a = np.array(P) + b = np.array(goal) + return np.linalg.norm(a - b) + +def hueristic_2(P, goal): + # integer division by time variable + return consistent_hueristic(P, goal) // t + +def hueristic_1(P, goal): + # manhattan distance + return abs(P[0] - goal[0]) + abs(P[1] - goal[1]) + +def key(start, i, goal, g_function): + ans = g_function[start] + W1 * hueristics[i](start, goal) + return ans + +def do_something(back_pointer, goal, start): + grid = np.chararray((n, n)) + for i in range(n): + for j in range(n): + grid[i][j] = '*' + + for i in range(n): + for j in range(n): + if (j, (n-1)-i) in blocks: + grid[i][j] = "#" + + grid[0][(n-1)] = "-" + x = back_pointer[goal] + while x != start: + (x_c, y_c) = x + # print(x) + grid[(n-1)-y_c][x_c] = "-" + x = back_pointer[x] + grid[(n-1)][0] = "-" + + + for i in xrange(n): + for j in range(n): + if (i, j) == (0, n-1): + print grid[i][j], + print "<-- End position", + else: + print grid[i][j], + print + print("^") + print("Start position") + print + print("# is an obstacle") + print("- is the path taken by algorithm") + print("PATH TAKEN BY THE ALGORITHM IS:-") + x = back_pointer[goal] + while x != start: + print x, + x = back_pointer[x] + print x + quit() + +def valid(p): + if p[0] < 0 or p[0] > n-1: + return False + if p[1] < 0 or p[1] > n-1: + return False + return True + +def expand_state(s, j, visited, g_function, close_list_anchor, close_list_inad, open_list, back_pointer): + for itera in range(n_hueristic): + open_list[itera].remove_element(s) + # print("s", s) + # print("j", j) + (x, y) = s + left = (x-1, y) + right = (x+1, y) + up = (x, y+1) + down = (x, y-1) + + for neighbours in [left, right, up, down]: + if neighbours not in blocks: + if valid(neighbours) and neighbours not in visited: + # print("neighbour", neighbours) + visited.add(neighbours) + back_pointer[neighbours] = -1 + g_function[neighbours] = float('inf') + + if valid(neighbours) and g_function[neighbours] > g_function[s] + 1: + g_function[neighbours] = g_function[s] + 1 + back_pointer[neighbours] = s + if neighbours not in close_list_anchor: + open_list[0].put(neighbours, key(neighbours, 0, goal, g_function)) + if neighbours not in close_list_inad: + for var in range(1,n_hueristic): + if key(neighbours, var, goal, g_function) <= W2 * key(neighbours, 0, goal, g_function): + # print("why not plssssssssss") + open_list[j].put(neighbours, key(neighbours, var, goal, g_function)) + + + # print + +def make_common_ground(): + some_list = [] + # block 1 + for x in range(1, 5): + for y in range(1, 6): + some_list.append((x, y)) + + # line + for x in range(15, 20): + some_list.append((x, 17)) + + # block 2 big + for x in range(10, 19): + for y in range(1, 15): + some_list.append((x, y)) + + # L block + for x in range(1, 4): + for y in range(12, 19): + some_list.append((x, y)) + for x in range(3, 13): + for y in range(16, 19): + some_list.append((x, y)) + return some_list + +hueristics = {0: consistent_hueristic, 1: hueristic_1, 2: hueristic_2} + +blocks_blk = [(0, 1),(1, 1),(2, 1),(3, 1),(4, 1),(5, 1),(6, 1),(7, 1),(8, 1),(9, 1),(10, 1),(11, 1),(12, 1),(13, 1),(14, 1),(15, 1),(16, 1),(17, 1),(18, 1), (19, 1)] +blocks_no = [] +blocks_all = make_common_ground() + + + + +blocks = blocks_blk +# hyper parameters +W1 = 1 +W2 = 1 +n = 20 +n_hueristic = 3 # one consistent and two other inconsistent + +# start and end destination +start = (0, 0) +goal = (n-1, n-1) + +t = 1 +def multi_a_star(start, goal, n_hueristic): + g_function = {start: 0, goal: float('inf')} + back_pointer = {start:-1, goal:-1} + open_list = [] + visited = set() + + for i in range(n_hueristic): + open_list.append(PriorityQueue()) + open_list[i].put(start, key(start, i, goal, g_function)) + + close_list_anchor = [] + close_list_inad = [] + while open_list[0].minkey() < float('inf'): + for i in range(1, n_hueristic): + # print("i", i) + # print(open_list[0].minkey(), open_list[i].minkey()) + if open_list[i].minkey() <= W2 * open_list[0].minkey(): + global t + t += 1 + # print("less prio") + if g_function[goal] <= open_list[i].minkey(): + if g_function[goal] < float('inf'): + do_something(back_pointer, goal, start) + else: + _, get_s = open_list[i].top_show() + visited.add(get_s) + expand_state(get_s, i, visited, g_function, close_list_anchor, close_list_inad, open_list, back_pointer) + close_list_inad.append(get_s) + else: + # print("more prio") + if g_function[goal] <= open_list[0].minkey(): + if g_function[goal] < float('inf'): + do_something(back_pointer, goal, start) + else: + # print("hoolla") + get_s = open_list[0].top_show() + visited.add(get_s) + expand_state(get_s, 0, visited, g_function, close_list_anchor, close_list_inad, open_list, back_pointer) + close_list_anchor.append(get_s) + print("No path found to goal") + print + for i in range(n-1,-1, -1): + for j in range(n): + if (j, i) in blocks: + print '#', + elif (j, i) in back_pointer: + if (j, i) == (n-1, n-1): + print '*', + else: + print '-', + else: + print '*', + if (j, i) == (n-1, n-1): + print '<-- End position', + print + print("^") + print("Start position") + print + print("# is an obstacle") + print("- is the path taken by algorithm") +multi_a_star(start, goal, n_hueristic) \ No newline at end of file