""" a star variants author: Sarim Mehdi(muhammadsarim.mehdi@studio.unibo.it) Source: http://theory.stanford.edu/~amitp/GameProgramming/Variations.html """ import numpy as np import matplotlib.pyplot as plt show_animation = True use_beam_search = False use_iterative_deepening = False use_dynamic_weighting = False use_theta_star = False use_jump_point = False beam_capacity = 30 max_theta = 5 only_corners = False max_corner = 5 w, epsilon, upper_bound_depth = 1, 4, 500 def draw_horizontal_line(start_x, start_y, length, o_x, o_y, o_dict): for i in range(start_x, start_x + length): for j in range(start_y, start_y + 2): o_x.append(i) o_y.append(j) o_dict[(i, j)] = True def draw_vertical_line(start_x, start_y, length, o_x, o_y, o_dict): for i in range(start_x, start_x + 2): for j in range(start_y, start_y + length): o_x.append(i) o_y.append(j) o_dict[(i, j)] = True def in_line_of_sight(obs_grid, x1, y1, x2, y2): t = 0 while t <= 0.5: xt = (1 - t) * x1 + t * x2 yt = (1 - t) * y1 + t * y2 if obs_grid[(int(xt), int(yt))]: return False, None xt = (1 - t) * x2 + t * x1 yt = (1 - t) * y2 + t * y1 if obs_grid[(int(xt), int(yt))]: return False, None t += 0.001 dist = np.linalg.norm(np.array([x1, y1] - np.array([x2, y2]))) return True, dist def key_points(o_dict): offsets1 = [(1, 0), (0, 1), (-1, 0), (1, 0)] offsets2 = [(1, 1), (-1, 1), (-1, -1), (1, -1)] offsets3 = [(0, 1), (-1, 0), (0, -1), (0, -1)] c_list = [] for grid_point, obs_status in o_dict.items(): if obs_status: continue empty_space = True x, y = grid_point for i in [-1, 0, 1]: for j in [-1, 0, 1]: if (x + i, y + j) not in o_dict.keys(): continue if o_dict[(x + i, y + j)]: empty_space = False break if empty_space: continue for offset1, offset2, offset3 in zip(offsets1, offsets2, offsets3): i1, j1 = offset1 i2, j2 = offset2 i3, j3 = offset3 if ((x + i1, y + j1) not in o_dict.keys()) or \ ((x + i2, y + j2) not in o_dict.keys()) or \ ((x + i3, y + j3) not in o_dict.keys()): continue obs_count = 0 if o_dict[(x + i1, y + j1)]: obs_count += 1 if o_dict[(x + i2, y + j2)]: obs_count += 1 if o_dict[(x + i3, y + j3)]: obs_count += 1 if obs_count == 3 or obs_count == 1: c_list.append((x, y)) if show_animation: plt.plot(x, y, ".y") break if only_corners: return c_list e_list = [] for corner in c_list: x1, y1 = corner for other_corner in c_list: x2, y2 = other_corner if x1 == x2 and y1 == y2: continue reachable, _ = in_line_of_sight(o_dict, x1, y1, x2, y2) if not reachable: continue x_m, y_m = int((x1 + x2) / 2), int((y1 + y2) / 2) e_list.append((x_m, y_m)) if show_animation: plt.plot(x_m, y_m, ".y") return c_list + e_list class SearchAlgo: def __init__(self, obs_grid, goal_x, goal_y, start_x, start_y, limit_x, limit_y, corner_list=None): self.start_pt = [start_x, start_y] self.goal_pt = [goal_x, goal_y] self.obs_grid = obs_grid g_cost, h_cost = 0, self.get_hval(start_x, start_y, goal_x, goal_y) f_cost = g_cost + h_cost self.all_nodes, self.open_set = {}, [] if use_jump_point: for corner in corner_list: i, j = corner h_c = self.get_hval(i, j, goal_x, goal_y) self.all_nodes[(i, j)] = {'pos': [i, j], 'pred': None, 'gcost': np.inf, 'hcost': h_c, 'fcost': np.inf, 'open': True, 'in_open_list': False} self.all_nodes[tuple(self.goal_pt)] = \ {'pos': self.goal_pt, 'pred': None, 'gcost': np.inf, 'hcost': 0, 'fcost': np.inf, 'open': True, 'in_open_list': True} else: for i in range(limit_x): for j in range(limit_y): h_c = self.get_hval(i, j, goal_x, goal_y) self.all_nodes[(i, j)] = {'pos': [i, j], 'pred': None, 'gcost': np.inf, 'hcost': h_c, 'fcost': np.inf, 'open': True, 'in_open_list': False} self.all_nodes[tuple(self.start_pt)] = \ {'pos': self.start_pt, 'pred': None, 'gcost': g_cost, 'hcost': h_cost, 'fcost': f_cost, 'open': True, 'in_open_list': True} self.open_set.append(self.all_nodes[tuple(self.start_pt)]) @staticmethod def get_hval(x1, y1, x2, y2): x, y = x1, y1 val = 0 while x != x2 or y != y2: if x != x2 and y != y2: val += 14 else: val += 10 x, y = x + np.sign(x2 - x), y + np.sign(y2 - y) return val def get_farthest_point(self, x, y, i, j): i_temp, j_temp = i, j counter = 1 got_goal = False while not self.obs_grid[(x + i_temp, y + j_temp)] and \ counter < max_theta: i_temp += i j_temp += j counter += 1 if [x + i_temp, y + j_temp] == self.goal_pt: got_goal = True break if (x + i_temp, y + j_temp) not in self.obs_grid.keys(): break return i_temp - 2*i, j_temp - 2*j, counter, got_goal def jump_point(self): """Jump point: Instead of exploring all empty spaces of the map, just explore the corners.""" goal_found = False while len(self.open_set) > 0: self.open_set = sorted(self.open_set, key=lambda x: x['fcost']) lowest_f = self.open_set[0]['fcost'] lowest_h = self.open_set[0]['hcost'] lowest_g = self.open_set[0]['gcost'] p = 0 for p_n in self.open_set[1:]: if p_n['fcost'] == lowest_f and \ p_n['gcost'] < lowest_g: lowest_g = p_n['gcost'] p += 1 elif p_n['fcost'] == lowest_f and \ p_n['gcost'] == lowest_g and \ p_n['hcost'] < lowest_h: lowest_h = p_n['hcost'] p += 1 else: break current_node = self.all_nodes[tuple(self.open_set[p]['pos'])] x1, y1 = current_node['pos'] for cand_pt, cand_node in self.all_nodes.items(): x2, y2 = cand_pt if x1 == x2 and y1 == y2: continue if np.linalg.norm(np.array([x1, y1] - np.array([x2, y2]))) > max_corner: continue reachable, offset = in_line_of_sight(self.obs_grid, x1, y1, x2, y2) if not reachable: continue if list(cand_pt) == self.goal_pt: current_node['open'] = False self.all_nodes[tuple(cand_pt)]['pred'] = \ current_node['pos'] goal_found = True break g_cost = offset + current_node['gcost'] h_cost = self.all_nodes[cand_pt]['hcost'] f_cost = g_cost + h_cost cand_pt = tuple(cand_pt) if f_cost < self.all_nodes[cand_pt]['fcost']: self.all_nodes[cand_pt]['pred'] = current_node['pos'] self.all_nodes[cand_pt]['gcost'] = g_cost self.all_nodes[cand_pt]['fcost'] = f_cost if not self.all_nodes[cand_pt]['in_open_list']: self.open_set.append(self.all_nodes[cand_pt]) self.all_nodes[cand_pt]['in_open_list'] = True if show_animation: plt.plot(cand_pt[0], cand_pt[1], "r*") if goal_found: break if show_animation: plt.pause(0.001) if goal_found: current_node = self.all_nodes[tuple(self.goal_pt)] while goal_found: if current_node['pred'] is None: break x = [current_node['pos'][0], current_node['pred'][0]] y = [current_node['pos'][1], current_node['pred'][1]] current_node = self.all_nodes[tuple(current_node['pred'])] if show_animation: plt.plot(x, y, "b") plt.pause(0.001) if goal_found: break current_node['open'] = False current_node['in_open_list'] = False if show_animation: plt.plot(current_node['pos'][0], current_node['pos'][1], "g*") del self.open_set[p] current_node['fcost'], current_node['hcost'] = np.inf, np.inf if show_animation: plt.title('Jump Point') plt.show() def a_star(self): """Beam search: Maintain an open list of just 30 nodes. If more than 30 nodes, then get rid of nodes with high f values. Iterative deepening: At every iteration, get a cut-off value for the f cost. This cut-off is minimum of the f value of all nodes whose f value is higher than the current cut-off value. Nodes with f value higher than the current cut off value are not put in the open set. Dynamic weighting: Multiply heuristic with the following: (1 + epsilon - (epsilon*d)/N) where d is the current iteration of loop and N is upper bound on number of iterations. Theta star: Same as A star but you don't need to move one neighbor at a time. In fact, you can look for the next node as far out as you can as long as there is a clear line of sight from your current node to that node.""" if show_animation: if use_beam_search: plt.title('A* with beam search') elif use_iterative_deepening: plt.title('A* with iterative deepening') elif use_dynamic_weighting: plt.title('A* with dynamic weighting') elif use_theta_star: plt.title('Theta*') else: plt.title('A*') goal_found = False curr_f_thresh = np.inf depth = 0 no_valid_f = False w = None while len(self.open_set) > 0: self.open_set = sorted(self.open_set, key=lambda x: x['fcost']) lowest_f = self.open_set[0]['fcost'] lowest_h = self.open_set[0]['hcost'] lowest_g = self.open_set[0]['gcost'] p = 0 for p_n in self.open_set[1:]: if p_n['fcost'] == lowest_f and \ p_n['gcost'] < lowest_g: lowest_g = p_n['gcost'] p += 1 elif p_n['fcost'] == lowest_f and \ p_n['gcost'] == lowest_g and \ p_n['hcost'] < lowest_h: lowest_h = p_n['hcost'] p += 1 else: break current_node = self.all_nodes[tuple(self.open_set[p]['pos'])] while len(self.open_set) > beam_capacity and use_beam_search: del self.open_set[-1] f_cost_list = [] if use_dynamic_weighting: w = (1 + epsilon - epsilon*depth/upper_bound_depth) for i in range(-1, 2): for j in range(-1, 2): x, y = current_node['pos'] if (i == 0 and j == 0) or \ ((x + i, y + j) not in self.obs_grid.keys()): continue if (i, j) in [(1, 0), (0, 1), (-1, 0), (0, -1)]: offset = 10 else: offset = 14 if use_theta_star: new_i, new_j, counter, goal_found = \ self.get_farthest_point(x, y, i, j) offset = offset * counter cand_pt = [current_node['pos'][0] + new_i, current_node['pos'][1] + new_j] else: cand_pt = [current_node['pos'][0] + i, current_node['pos'][1] + j] if use_theta_star and goal_found: current_node['open'] = False cand_pt = self.goal_pt self.all_nodes[tuple(cand_pt)]['pred'] = \ current_node['pos'] break if cand_pt == self.goal_pt: current_node['open'] = False self.all_nodes[tuple(cand_pt)]['pred'] = \ current_node['pos'] goal_found = True break cand_pt = tuple(cand_pt) no_valid_f = self.update_node_cost(cand_pt, curr_f_thresh, current_node, f_cost_list, no_valid_f, offset, w) if goal_found: break if show_animation: plt.pause(0.001) if goal_found: current_node = self.all_nodes[tuple(self.goal_pt)] while goal_found: if current_node['pred'] is None: break if use_theta_star or use_jump_point: x, y = [current_node['pos'][0], current_node['pred'][0]], \ [current_node['pos'][1], current_node['pred'][1]] if show_animation: plt.plot(x, y, "b") else: if show_animation: plt.plot(current_node['pred'][0], current_node['pred'][1], "b*") current_node = self.all_nodes[tuple(current_node['pred'])] if goal_found: break if use_iterative_deepening and f_cost_list: curr_f_thresh = min(f_cost_list) if use_iterative_deepening and not f_cost_list: curr_f_thresh = np.inf if use_iterative_deepening and not f_cost_list and no_valid_f: current_node['fcost'], current_node['hcost'] = np.inf, np.inf continue current_node['open'] = False current_node['in_open_list'] = False if show_animation: plt.plot(current_node['pos'][0], current_node['pos'][1], "g*") del self.open_set[p] current_node['fcost'], current_node['hcost'] = np.inf, np.inf depth += 1 if show_animation: plt.show() def update_node_cost(self, cand_pt, curr_f_thresh, current_node, f_cost_list, no_valid_f, offset, w): if not self.obs_grid[tuple(cand_pt)] and \ self.all_nodes[cand_pt]['open']: g_cost = offset + current_node['gcost'] h_cost = self.all_nodes[cand_pt]['hcost'] if use_dynamic_weighting: h_cost = h_cost * w f_cost = g_cost + h_cost if f_cost < self.all_nodes[cand_pt]['fcost'] and \ f_cost <= curr_f_thresh: f_cost_list.append(f_cost) self.all_nodes[cand_pt]['pred'] = \ current_node['pos'] self.all_nodes[cand_pt]['gcost'] = g_cost self.all_nodes[cand_pt]['fcost'] = f_cost if not self.all_nodes[cand_pt]['in_open_list']: self.open_set.append(self.all_nodes[cand_pt]) self.all_nodes[cand_pt]['in_open_list'] = True if show_animation: plt.plot(cand_pt[0], cand_pt[1], "r*") if curr_f_thresh < f_cost < \ self.all_nodes[cand_pt]['fcost']: no_valid_f = True return no_valid_f def main(): # set obstacle positions obs_dict = {} for i in range(51): for j in range(51): obs_dict[(i, j)] = False o_x, o_y = [], [] s_x = 5.0 s_y = 5.0 g_x = 35.0 g_y = 45.0 # draw outer border of maze draw_vertical_line(0, 0, 50, o_x, o_y, obs_dict) draw_vertical_line(48, 0, 50, o_x, o_y, obs_dict) draw_horizontal_line(0, 0, 50, o_x, o_y, obs_dict) draw_horizontal_line(0, 48, 50, o_x, o_y, obs_dict) # draw inner walls all_x = [10, 10, 10, 15, 20, 20, 30, 30, 35, 30, 40, 45] all_y = [10, 30, 45, 20, 5, 40, 10, 40, 5, 40, 10, 25] all_len = [10, 10, 5, 10, 10, 5, 20, 10, 25, 10, 35, 15] for x, y, l in zip(all_x, all_y, all_len): draw_vertical_line(x, y, l, o_x, o_y, obs_dict) all_x[:], all_y[:], all_len[:] = [], [], [] all_x = [35, 40, 15, 10, 45, 20, 10, 15, 25, 45, 10, 30, 10, 40] all_y = [5, 10, 15, 20, 20, 25, 30, 35, 35, 35, 40, 40, 45, 45] all_len = [10, 5, 10, 10, 5, 5, 10, 5, 10, 5, 10, 5, 5, 5] for x, y, l in zip(all_x, all_y, all_len): draw_horizontal_line(x, y, l, o_x, o_y, obs_dict) if show_animation: plt.plot(o_x, o_y, ".k") plt.plot(s_x, s_y, "og") plt.plot(g_x, g_y, "xb") plt.grid(True) if use_jump_point: keypoint_list = key_points(obs_dict) search_obj = SearchAlgo(obs_dict, g_x, g_y, s_x, s_y, 101, 101, keypoint_list) search_obj.jump_point() else: search_obj = SearchAlgo(obs_dict, g_x, g_y, s_x, s_y, 101, 101) search_obj.a_star() if __name__ == '__main__': main()