def rrtstar(g, start, goal, max_iter=10000): nodes = {start: {'cost': 0, 'parent': None}} for i in range(max_iter): # Pick a random point in the grid q = (random.randrange(g.width), random.randrange(g.height)) # Find the cell in the tree closest to random point dists = {n: grid.dist(q, n) for n in nodes} closest_tree_node = min(dists, key=lambda k: dists[k]) # Find a neighbour of closest that is even closer to q dists = {n: grid.dist(q, n) for n in g.neighbours(*closest_tree_node)} best_new_node = min(dists, key=lambda k: dists[k]) if best_new_node in nodes: continue # skip nodes if they are already in the tree # Find the neighbour of the best cell with the lowest cost costs = {n: nodes[n]['cost'] + grid.cost(best_new_node, n) for n in g.neighbours(*best_new_node) if n in nodes} best_parent = min(costs, key=lambda k: costs[k]) # Add new node cost = nodes[best_parent]['cost'] + grid.cost(best_parent, best_new_node) nodes[best_new_node] = {'cost': cost, 'parent': best_parent} if best_new_node == goal: return _reconstruct_path(goal, nodes), _build_edge_list(nodes) # Check the neighbours if their cost can be decreased by # connecting to the new node for n in g.neighbours(*best_new_node): if n not in nodes: continue if nodes[n]['cost'] > cost + grid.cost(best_new_node, n): nodes[n]['parent'] = best_new_node nodes[n]['cost'] = cost + grid.cost(best_new_node, n) _update_child_cost(nodes, n) print('no path') return None, _build_edge_list(nodes)
def __expand_space(grid, fringe, visited, g, h, parent, curr): for n in neighbors(grid, curr): if n not in visited: if n not in fringe: g[n] = float('inf') parent[n] = None if g[curr] + cost(grid, curr, n) < g[n]: g[n] = g[curr] + cost(grid, curr, n) parent[n] = curr fringe[n] = g[n] + h(n)
def update_shortest_path(grid, nodes, edges, node): for n in g.neighbours(*node): if n in nodes and nodes[n] + grid.cost(n, node) < nodes[node]: # remove current edge for parent in nodes: if (parent, node) in edges: edges.remove((parent, node)) break # set new edge edges.append((n, node)) nodes[node] = nodes[n] + grid.cost(n, node) # update all children update_costs(grid, nodes, edges, node)
def astar(g, start, goal): closed = set() open_list = [] heapq.heappush(open_list, (0, start)) g_scores = {start: 0} came_from = {} while open_list != []: prio, current = heapq.heappop(open_list) if current == goal: return reconstruct(came_from, goal) closed.add(current) for n in g.neighbours(*current): if n in closed: continue # skip already evaluated neighbours g_score = g_scores[current] + grid.cost(current, n) if n in g_scores and g_score > g_scores[n]: continue # This is not a better path # Add the neighbour on the heap came_from[n] = current g_scores[n] = g_score heapq.heappush(open_list, (g_score + grid.dist(n, goal), n)) return None
def update_costs(grid, nodes, edges, node): update = [] for s, e in edges: if e == node: nodes[node] = nodes[s] + grid.cost(s, node) if s == node: update.append(e) for n in update: update_costs(grid, nodes, edges, n)
def path_cost(grid, parent, curr): p = path(parent, curr) c = 0 prev = None for s in p: if prev: c = c + cost(grid, prev, s) prev = s return c
def __expand_space_integrated(grid, o, c_a, c_i, g, anchor, inad, bp, w1, w2, curr): for k in o: if curr in o[k]: o[k].remove(curr) for n in neighbors(grid, curr): if n not in g: g[n] = float('inf') bp[n] = None g_new = g[curr] + cost(grid, curr, n) if g[n] > g[curr] + cost(grid, curr, n): g[n] = g_new bp[n] = curr if n not in c_a: o[anchor][n] = key(g, anchor, n, w1) if n not in c_i: for i in inad: if key(g, i, n, w1) <= w2*o[anchor][n]: o[i][n] = key(g, i, n, w1)
def iddfs_rec(g, pos, goal, path, max_cost, cost): if cost > max_cost: return None if pos == goal: return path for n in g.neighbours(*pos): if n in path: continue # don't follow loops p = iddfs_rec(g, n, goal, path + [n], max_cost, cost + grid.cost(pos, n)) if p != None: return p return None
def test_cost(self): def approx_equal(f1, f2): return abs(f1 - f2) < .01 assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[0, 0]), 2.**.5) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[0, 1]), 1.) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[1, 1]), 1.) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[1, 0]), 1.) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[0, 2]), 2.**.5) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[1, 2]), 1.) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[2, 2]), 2.**.5) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[2, 1]), 1.) assert approx_equal(cost(self.grid, self.grid[1, 1], self.grid[2, 0]), 2.**.5)
def h_favor_highways_smart(grid, s, goal, *args, **kwargs): # Only useful for searching for highways beyond manhattan distance bc we only need to compare manhattan distance # d_x = s.coords[0] - goal.coords[0] # d_y = s.coords[1] - goal.coords[1] m_d = manhattan_distance(s, goal) c = h(grid, s, goal, *args, **kwargs) for n in neighbors(grid, s): if is_horizontal(s, n) or is_vertical(s, n): if s.is_highway() and n.is_highway(): m_d_n = manhattan_distance(n, goal) if m_d < m_d_n: c = cost(grid, s, n) * m_d # In order to take into account searching for highways beyond the manhattan distance, # we would need to know the parent of s # if s.parent is n: # continue return c
def rrtstar(g, start, goal, max_iter=10000): nodes = {start: 0} edges = [] for i in range(max_iter): q = (random.randrange(g.width), random.randrange(g.height)) # Find closest node to the sample closest = start dist = grid.dist(closest, q) for n in nodes: if grid.dist(n, q) < dist: closest = n dist = grid.dist(n, q) # Find grid cell that is most in direction of sample c = g.neighbours(*closest)[0] dist = grid.dist(c, q) for n in g.neighbours(*closest): if grid.dist(n, q) < dist: c = n dist = grid.dist(n, q) if c in nodes: continue # skip already visited cells # find the neighbour with the lowest cost cost = nodes[closest] for n in g.neighbours(*c): if n in nodes and nodes[n] < cost: cost = nodes[n] closest = n nodes[c] = nodes[closest] + grid.cost(closest, c) edges.append((closest, c)) # Update edges from other nodes for n in g.neighbours(*c): if n in nodes: update_shortest_path(grid, nodes, edges, n) if c == goal: print('arrived at goal') return (reconstruct(edges, start, goal), edges) print('No path found') return None, edges
def _update_child_cost(nodes, parent): for node in nodes: if nodes[node]['parent'] == parent: nodes[node]['cost'] = nodes[parent]['cost'] + grid.cost(parent, node) _update_child_cost(nodes, node)
#astar with dijkstra and heuristic import grid import math from Queue import PriorityQueue def heuristic(a, b): return math.fabs(a.x - b.x) + math.abs(a.y - b.y) frontier = PriorityQueue() frontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0 while not frontier.empty(): current = frontier.get() if current == goal: break for next in graph.neighbors(current): new_cost = cost_so_far[current] + grid.cost( current, next) #TODO: implement on grid.py cost if next not in cost_so_far or new_cost < cost_so_far[next]: cost_so_far[next] = new_cost priority = new_cost + heuristic(goal, next) frontier.put(next, priority) came_from[next] = current
def test_cost_for_diagonal_move_is_sqrt_two(self): answer = math.sqrt(2) self.assertEqual(grid.cost((0, 0), (1, 1)), answer) self.assertEqual(grid.cost((8, 2), (7, 1)), answer) self.assertEqual(grid.cost((9, 6), (8, 7)), answer) self.assertEqual(grid.cost((2, 3), (3, 2)), answer)
def test_cost_for_cardinal_move_is_one(self): self.assertEqual(grid.cost((0, 0), (1, 0)), 1) self.assertEqual(grid.cost((7, 1), (7, 2)), 1) self.assertEqual(grid.cost((9, 4), (8, 4)), 1) self.assertEqual(grid.cost((6, 2), (6, 1)), 1)
#astar with dijkstra and heuristic import grid import math from Queue import PriorityQueue def heuristic(a,b): return math.fabs(a.x-b.x)+math.abs(a.y-b.y) frontier = PriorityQueue() frontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0 while not frontier.empty(): current = frontier.get() if current == goal: break for next in graph.neighbors(current): new_cost = cost_so_far[current] + grid.cost(current, next) #TODO: implement on grid.py cost if next not in cost_so_far or new_cost < cost_so_far[next]: cost_so_far[next] = new_cost priority = new_cost+heuristic(goal,next) frontier.put(next,priority) came_from[next] = current
def test_cost_is_zero_for_same_cell(self): self.assertEqual(grid.cost((0, 0), (0, 0)), 0) self.assertEqual(grid.cost((7, 8), (7, 8)), 0) self.assertEqual(grid.cost((2, 5), (2, 5)), 0)