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)
