def greedy(graph): n = graph.get_total_vertex_count() s = graph.s t = graph.t cost = graph.cost h_list = heuristic(graph.point_list, graph.point_list[t]) path = [s] p_remain = list(range(1, n)) while path[-1] != t: u = path[-1] next_vertices = [] check = False for v in range(n): if cost[u][v] < 1e9 and v in p_remain: next_vertices.append(h_list[v]) check = True else: next_vertices.append(1e9) if not check: return False, [] u = min(enumerate(next_vertices), key=itemgetter(1))[0] p_remain.remove(u) path.append(u) return True, path
def __init__(self, map: OccupancyGridMap, s_start: (int, int, int), s_goal: (int, int, int)): """ :param map: the ground truth map of the environment provided by gui :param s_start: start location :param s_goal: end location """ ## we need to also update our occupancy grid map self.new_edges_and_old_costs = None # algorithm start self.s_start = s_start # now takes in an x, y and z self.s_goal = s_goal # now takes in an x, y and z self.s_last = s_start # now takes in an x, y and z self.k_m = 0 # accumulation self.U = PriorityQueue() self.rhs = np.ones((map.x_dim, map.y_dim, map.z_dim)) * np.inf self.g = self.rhs.copy() self.sensed_map = OccupancyGridMap(x_dim=map.x_dim, y_dim=map.y_dim, z_dim=map.z_dim, exploration_setting='26N') self.rhs[self.s_goal] = 0 self.U.insert(self.s_goal, Priority(heuristic(self.s_start, self.s_goal), 0))
def calculate_key(self, s: (int, int)): """ :param s: the vertex we want to calculate key :return: Priority class of the two keys """ k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m k2 = min(self.g[s], self.rhs[s]) return Priority(k1, k2)
def move_and_replan(self, robot_position: (int, int, int)): path = [robot_position] self.s_start = robot_position self.s_last = self.s_start self.compute_shortest_path() while self.s_start != self.s_goal: assert (self.rhs[self.s_start] != float('inf')), "There is no known path!" succ = self.sensed_map.succ(self.s_start, avoid_obstacles=False) min_s = float('inf') arg_min = None for s_ in succ: temp = self.c(self.s_start, s_) + self.g[s_] if temp < min_s: min_s = temp arg_min = s_ ### algorithm sometimes gets stuck here for some reason !!! FIX self.s_start = arg_min path.append(self.s_start) # scan graph for changed costs changed_edges_with_old_cost = self.rescan() #print("len path: {}".format(len(path))) # if any edge costs changed if changed_edges_with_old_cost: self.k_m += heuristic(self.s_last, self.s_start) self.s_last = self.s_start # for all directed edges (u,v) with changed edge costs vertices = changed_edges_with_old_cost.vertices for vertex in vertices: v = vertex.pos succ_v = vertex.edges_and_c_old for u, c_old in succ_v.items(): c_new = self.c(u, v) if c_old > c_new: if u != self.s_goal: self.rhs[u] = min(self.rhs[u], self.c(u, v) + self.g[v]) elif self.rhs[u] == c_old + self.g[v]: if u != self.s_goal: min_s = float('inf') succ_u = self.sensed_map.succ(vertex=u) for s_ in succ_u: temp = self.c(u, s_) + self.g[s_] if min_s > temp: min_s = temp self.rhs[u] = min_s self.update_vertex(u) self.compute_shortest_path() if len(path) > 1: return path, self.g, self.rhs return None, None, None
def c(self, u: (int, int), v: (int, int)) -> float: """ calcuclate the cost between nodes :param u: from vertex :param v: to vertex :return: euclidean distance to traverse. inf if obstacle in path """ if not self.sensed_map.is_unoccupied(u) or not self.sensed_map.is_unoccupied(v): return float('inf') else: return heuristic(u, v)
def move_and_replan(self, robot_position: (int, int)): path = [robot_position] self.s_start = robot_position self.s_last = self.s_start self.compute_shortest_path() while self.s_start != self.s_goal: assert (self.rhs[self.s_start] != float('inf')), "There is no known path!" succ = self.sensed_map.succ(self.s_start, avoid_obstacles=False) min_s = float('inf') arg_min = None for s_ in succ: temp = self.c(self.s_start, s_) + self.g[s_] if temp < min_s: min_s = temp arg_min = s_ self.s_start = arg_min path.append(self.s_start) changed_edges_with_old_cost = self.rescan() if changed_edges_with_old_cost: self.k_m += heuristic(self.s_last, self.s_start) self.s_last = self.s_start vertices = changed_edges_with_old_cost.vertices for vertex in vertices: v = vertex.pos succ_v = vertex.edges_and_c_old for u, c_old in succ_v.items(): c_new = self.c(u, v) if c_old > c_new: if u != self.s_goal: self.rhs[u] = min(self.rhs[u], self.c(u, v) + self.g[v]) elif self.rhs[u] == c_old + self.g[v]: if u != self.s_goal: min_s = float('inf') succ_u = self.sensed_map.succ(vertex=u) for s_ in succ_u: temp = self.c(u, s_) + self.g[s_] if min_s > temp: min_s = temp self.rhs[u] = min_s self.update_vertex(u) self.compute_shortest_path() print("path found!") return path, self.g, self.rhs
def ida_star(graph, start_id, end_id): start_node = [ node for node in list(graph.graph.keys()) if node.vertex_id == start_id ][0] end_node = [ node for node in list(graph.graph.keys()) if node.vertex_id == end_id ][0] #Variaveis de estatisticas start_time = timeit.default_timer() expanded = [0] cost = 0 visited = [] sum_visited = 0 branching_factor = [] success = False fail = False solution = [] limit = utils.heuristic(start_node, end_node) while not success or not fail: distance = ida_star_aux(graph, start_node, end_node, 0, visited, limit, solution, expanded, branching_factor) if distance == float("inf"): fail = True break elif distance < 0: success = True break else: limit = distance solution = [] sum_visited += len(visited) visited = [] end_time = timeit.default_timer() cost = -1 * distance depth = len(solution) - 1 exec_time = end_time - start_time average_branching_factor = sum(branching_factor) / len(branching_factor) return [node.vertex_id for node in solution], depth, cost, expanded[ 0], sum_visited, average_branching_factor, exec_time, "success" if success else "fail"
def a_star(graph): n = graph.get_total_vertex_count() s = graph.s t = graph.t cost = graph.cost pq = [] f = [1e9] * n g = [1e9] * n h = heuristic(graph.point_list, graph.point_list[t]) p = [-1] * n closed = [False] * n g[s] = 0 f[s] = g[s] + h[s] p[s] = -2 heappush(pq, (f[s], s)) while len(pq) > 0 and f[t] >= pq[0][0]: f_u, u = heappop(pq) if f_u > f[u]: continue closed[u] = True for v in range(n): if closed[v]: continue g[v] = min(g[v], g[u] + cost[u][v]) if f[v] > g[v] + h[v]: f[v] = g[v] + h[v] p[v] = u heappush(pq, (f[v], v)) if p[t] == -1: return False, [] path = [] v = t while v != -2: path.append(v) v = p[v] path.reverse() return True, path
def __init__(self, map: OccupancyGridMap, s_start: (int, int), s_goal: (int, int)): self.new_edges_and_old_costs = None self.s_start = s_start self.s_goal = s_goal self.s_last = s_start self.k_m = 0 self.U = PriorityQueue() self.rhs = np.ones((map.x_dim, map.y_dim)) * np.inf self.g = self.rhs.copy() self.sensed_map = OccupancyGridMap(x_dim=map.x_dim, y_dim=map.y_dim, exploration_setting='8N') self.rhs[self.s_goal] = 0 self.U.insert(self.s_goal, Priority(heuristic(self.s_start, self.s_goal), 0))
def ida_star_aux(graph, node, end_node, distance, visited, limit, path, expanded, branching_factor): path.append(node) visited.append(node) if node == end_node: return -distance estimate = distance + utils.heuristic(node, end_node) if estimate > limit: path.pop() return estimate n_limit = float("inf") edges = list(graph[node].keys()) edges.sort(key=lambda edge: edge.vertex_id) n = 0 expanded[0] += 1 for edge in edges: if edge not in visited: n += 1 edge_dist = ida_star_aux(graph, edge, end_node, distance + graph[node][edge].weight, visited, limit, path, expanded, branching_factor) if edge_dist < 0: return edge_dist elif edge_dist < n_limit: n_limit = edge_dist branching_factor.append(n) if len(path) > 0: path.pop() return n_limit
def AStar(start: str, end: str, trasbordos: bool): G = getAtenas() # Get the Godamm MAP visitedList = [] # Nodes we alredy looked into toVisit = PriorityQueue( ) # The Nodes to visit: get will return de min of all (f(n),count,node) final = None found = False count = 0 currentLine = 0 lessChanges = False # Opcion para decir si queremos menos trasbordos posibles, google maps implementa algo similar toVisit.put((0, 0, nodeState(start, None, 0))) # Obvious but add the start to toVisit while (not toVisit.empty() and (not found)): # We have nodes to visit lowest = toVisit.get() # Lowest value of f(n) from priority Queue lowestVertex = lowest[ 2] # Get the node with f(n) lowest nodeState(node,parent) visitedList.append( lowestVertex.node) # We visit the node, add it to the list if (lowestVertex.node == end): found = True # Yeay we have found it, lets stop final = lowestVertex # Save the node for backtracking else: ''' Miramos si hemos cambiado de linea, de ese modo podremos calcular el coste real de pasar a la siguiente parada ''' if (lessChanges and currentLine not in G.nodes[lowestVertex.node]["lineas"]): currentLine = G.nodes[lowestVertex.node]["lineas"][0] children = [i for i in G.neighbors(lowestVertex.node) ] # All the children of the LowestVertex for child in children: # Iterate through all children count += 1 ''' f(n)=g(n)+h(n) g(n): Distance between Parent & Child --> Stored in the Edge of the graph ++ Change of trains h(n): How far are we from the end Node --> Need a function to calculate ''' g = lowestVertex.distance + G.edges[(lowestVertex.node, child)]["g(n)"] change = False if (lessChanges and currentLine not in G.nodes[child]["lineas"] ): #Change of metro line g += 6.666 # Se aumenta G ya que el coste para pasar por esa arista es mayor al haber trasbordo change = True # Si hemos cambiado de linea guardarlo para luego buscar en numero de trasbordos f = g + heuristic(G, child, end) #Calculate f(n)=g(n)+h(n) toInsert = nodeState( child, lowestVertex, g, change ) # Create the tuple (Node,Parent,cost2getHere,changeOfTrain) if (toInsert.node not in visitedList ): # Check if we have alredy been there (NO endless Loops) toVisit.put( (f, count, toInsert )) # We will need further examination of the node if (found): # Congrats we have found something path, cost, changes = backTracking(final) print(path) print(cost) print(changes) else: # Something smells like a Bug in the Code..... Or the really Intelligent USER print("No path could be found. ¯\_(ツ)_/¯ ")
def greedy(graph, start_id, end_id): start_time = timeit.default_timer() expanded = 0 branching_factor = [] start_node = [ node for node in list(graph.graph.keys()) if node.vertex_id == start_id ][0] end_node = [ node for node in list(graph.graph.keys()) if node.vertex_id == end_id ][0] parentMap = {} visited = [] stack = [] solution = [] current = start_node stack.append(current) success = False while stack: current = stack[-1] stack.pop() if current not in visited: visited.append(current) if current == end_node: success = True break else: expanded += 1 n = 0 children = {} for child in graph[current]: if child not in visited: children[child] = utils.heuristic(child, end_node) parentMap[child] = current n += 1 branching_factor.append(n) if children: sorted_children = list({ k: v for k, v in sorted(children.items(), key=lambda item: item[1]) }.keys()) sorted_children.reverse() stack += sorted_children cost = 0 if success: while current != start_node: solution.append(current.vertex_id) cost += graph[current][parentMap[current]].weight current = parentMap[current] solution.append(current.vertex_id) solution.reverse() end_time = timeit.default_timer() depth = len(solution) - 1 exec_time = end_time - start_time average = sum(branching_factor) / len(branching_factor) return solution, depth, cost, expanded, len( visited), average, exec_time, 'success' else: end_time = timeit.default_timer() exec_time = end_time - start_time return solution, -1, cost, expanded, len( visited), -1, exec_time, 'failure'
def algorithm(self, algorithm, color): if self.start is None or self.goal is None: self.curr_grid = self.clean_grid.copy() return self.reset_world('soft', new_run=True, keep_paths=True) # avoid increment when algorithm is applied two times on the same world if not self.applied_this_run[algorithm]: self.algorithm_runs[algorithm] += 1 self.applied_this_run[algorithm] = True start = self.start goal = self.goal h = heuristic(self.clean_grid, goal) data_algo = { 'h': h, 'frontier': {start}, 'inner': set(), 'g_score': { start: 0 }, 'f_score': { start: h[start] }, 'come_from': { start: None } } steps = 0 self.curr_grid = self.clean_grid.copy() while len(data_algo['frontier']) > 0: if self.step_by_step: for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.type == pygame.QUIT: sys.exit(0) if event.key == self.control["R"]: self.paths = [] self.curr_grid = self.clean_grid return # update only when SPACE is pressed if event.type == pygame.KEYDOWN and event.key == self.control[ "SPACE"]: # apply one step of the selected algorithm data_algo, done = self.step( data_algo, algorithm, color) steps += 1 if done: self.print_this, self.printed_infos = \ build_print_line(algorithm, steps, self.paths, self.run_num, self.printed_infos) self.caption = self.print_this return else: data_algo, done = self.step(data_algo, algorithm, color) steps += 1 if done: self.print_this, self.printed_infos = \ build_print_line(algorithm, steps, self.paths, self.run_num, self.printed_infos) self.caption = self.print_this self.update_screen() if self.gif: pygame.image.save(self.screen, './temp/{}.bmp'.format(steps)) create_gif(self.run_num, algorithm) return self.update_screen() if self.gif: pygame.image.save(self.screen, './temp/{}.bmp'.format(steps))
def c(self, u: (int, int), v: (int, int)) -> float: if not self.sensed_map.is_unoccupied( u) or not self.sensed_map.is_unoccupied(v): return float('inf') else: return heuristic(u, v)
def calculate_key(self, s: (int, int)): k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m k2 = min(self.g[s], self.rhs[s]) return Priority(k1, k2)
start = [0, 0] goal1 = [len(map[0]) - 1, len(map) - 1] goal2 = [4, 2] goal = goal2 #Define the possible directions the robot can move dirs = [ [-1, 0], # go up [0, -1], # go left [1, 0], # go down [0, 1] ] # go right dirs_name = ['^', '<', 'v', '>'] #Create heuristic map heuristic = heuristic(map, goal, method='Manhattan', show=False) # Output example: # [6, 5, 4, 3, 2, 3] # [5, 4, 3, 2, 1, 2] # [4, 3, 2, 1, 0, 1] # [5, 4, 3, 2, 1, 2] # [6, 5, 4, 3, 2, 3] expansion_grid = [[-1 for row in range(len(map[0]))] for col in range(len(map))] expansion_grid[goal[1]][goal[0]] = '*' #Define blocked #blocked corresponds to the limits of the map and the walls def is_blocked(map, node):
def a_star(graph, start_id, end_id): start_time = timeit.default_timer() expanded = 0 branching_factor = [] start_node = [ node for node in list(graph.graph.keys()) if node.vertex_id == start_id ][0] end_node = [ node for node in list(graph.graph.keys()) if node.vertex_id == end_id ][0] openList = {} closedList = {} # A lista contém respectivamente: g(custo acumulado), funcao h da heuristica e funcao f(g + h) openList[start_node] = [ 0, utils.heuristic(start_node, end_node), 0 + utils.heuristic(start_node, end_node) ] success = False solution = [] parentMap = {} while openList: current = utils.find_smaller(openList, 'a_star') closedList[current] = openList[current] del openList[current] if current == end_node: success = True break else: expanded += 1 n = 0 for child in graph[current]: if child in closedList: continue if child in openList: new_g = closedList[current][0] + graph[current][ child].weight if openList[child][0] > new_g: openList[child][0] = new_g openList[child][2] = new_g + openList[child][1] parentMap[child] = current n += 1 else: child_g = closedList[current][0] + graph[current][ child].weight child_h = utils.heuristic(child, end_node) openList[child] = [child_g, child_h, child_g + child_h] parentMap[child] = current n += 1 branching_factor.append(n) cost = 0 if success: while current != start_node: solution.append(current.vertex_id) cost += graph[current][parentMap[current]].weight current = parentMap[current] solution.append(current.vertex_id) solution.reverse() end_time = timeit.default_timer() depth = len(solution) - 1 exec_time = end_time - start_time average = sum(branching_factor) / len(branching_factor) return solution, depth, cost, expanded, len(openList) + len( closedList), average, exec_time, 'success' else: end_time = timeit.default_timer() exec_time = end_time - start_time return solution, -1, cost, expanded, len( visited), -1, exec_time, 'failure'
def h(self, position): rp = tuple(position) if rp not in self.h_table: self.h_table[rp] = heuristic(position, self.goal) return self.h_table[rp]