I'm working on a front-end for a robotic project (an 'autonomous' car that localizes itself using some sensors and a map - generated from an SVG file).
For the robot to be controllable, we must generate paths between its current position and its goal. I used the easiest algorithm for that : A*.
I got some weird results doing that : The car tends to go on multiples of 45° degree, and one especially annoying problem : some generated paths are quite noisy !
See the noisy path near the orange rectangle in this case :
Is there anyway to avoid those weird/noisy results ? Eventually we'd want to build a path with the minimum number of heading angle changes. (the car can turn without moving, so we don't need any path 'smoothing').
Here's my A* implementation :
def search(self, begin, goal):
if goal.x not in range(self.width) or goal.y not in range(self.height):
print "Goal is out of bound"
return []
elif not self.grid[begin.y][begin.x].reachable:
print "Beginning is unreachable"
return []
elif not self.grid[goal.y][goal.x].reachable:
print "Goal is unreachable"
return []
else:
self.cl = set()
self.ol = set()
curCell = begin
self.ol.add(curCell)
while len(self.ol) > 0:
# We choose the cell in the open list having the minimum score as our current cell
curCell = min(self.ol, key = lambda x : x.f)
# We add the current cell to the closed list
self.ol.remove(curCell)
self.cl.add(curCell)
# We check the cell's (reachable) neighbours :
neighbours = self.neighbours(curCell)
for cell in neighbours:
# If the goal is a neighbour cell :
if cell == goal:
cell.parent = curCell
self.path = cell.path()
self.display()
self.clear()
return self.path
elif cell not in self.cl:
# We process the cells that are not in the closed list
# (processing <-> calculating the "F" score)
cell.process(curCell, goal)
self.ol.add(cell)
EDIT 1: By popuplar demand, here's the score calculation function (process) :
def process(self, parent, goal):
self.parent = parent
self.g = parent.distance(self)
self.h = self.manhattanDistance(goal)
self.f = self.g + self.h
EDIT Here's the neighbours method (updated following user1884905's answer) :
def neighbours(self, cell, radius = 1, unreachables = False, diagonal = True):
neighbours = set()
for i in xrange(-radius, radius + 1):
for j in xrange(-radius, radius + 1):
x = cell.x + j
y = cell.y + i
if 0 <= y < self.height and 0 <= x < self.width and ( self.grid[y][x].reachable or unreachables ) and (diagonal or (x == cell.x or y == cell.y)) :
neighbours.add(self.grid[y][x])
return neighbours
(this looks complicated but it just gives the 8 neighbours - including diagonal neighbours - of a cell ; it can also take a radius different from 1 because it's used for other features)
And distance calculations (depending on the use of diagonals neighbours or not : )
def manhattanDistance(self, cell):
return abs(self.x - cell.x) + abs(self.y - cell.y)
def diagonalDistance(self, cell):
xDist = abs(self.x - cell.x)
yDist = abs(self.y - cell.y)
if xDist > yDist:
return 1.4 * yDist + (xDist - yDist)
else:
return 1.4 * xDist + (yDist - xDist)
It seems the implementation is not correct, because it's moving to the cell not yet in the examined one which is nearest (as the crow flies) to the goal, while it should try it and undo the path when finding an obstacle in order to find the optimal one. See this nice animation on Wikipedia to get the idea.
The issue here is related to how you calculate cell.f, maybe you aren't adding the score of the current cell when doing the calculus, in general the A* should take the steps marked in red here and generate sub optimal paths like that.
Since the space is divided into discrete cells, when the best path (always as the crow flies) in a continuous world is right in between two discrete moves, it approximate it as best as it can with that weird path.
I see two approaches here:
Fix the algorithm (here the pseudocode) keeping the correct distance value for each evaluated cell (in the pasted one there's no information about how cell.f is calculated).
Use Djikstra, it should be easy to be implemented with a few changes to the current algorithm. In fact, A* is just an optimized version of it.
Without being able to see how you have implemented your neighbour and distance functions, I still have a good guess about what is going wrong:
You should not use manhattan distance if you allow for diagonal traversal.
The manhattan distance in the goal-function should be a measure of the shortest distance to the goal. (Which it isn't, if you can drive diagonally through building-blocks.)
The easiest way to fix this would be to keep the manhattan distance as a goal-function and change the definition of neighbours to only include the four left-right-up-down adjacent cells.
Edit
There are still problems with your code. The following pseudo code is taken from wikipedia. I have marked important lines that you will have to check. You must ensure that i) you are updating the nodes in the open set if you find a better solution and ii) you always take into account the previously traveled distance.
function A*(start,goal)
closedset := the empty set // The set of nodes already evaluated.
openset := {start} // The set of tentative nodes to be evaluated, initially containing the start node
came_from := the empty map // The map of navigated nodes.
g_score[start] := 0 // Cost from start along best known path.
// Estimated total cost from start to goal through y.
f_score[start] := g_score[start] + heuristic_cost_estimate(start, goal)
while openset is not empty
current := the node in openset having the lowest f_score[] value
if current = goal
return reconstruct_path(came_from, goal)
remove current from openset
add current to closedset
for each neighbor in neighbor_nodes(current)
// -------------------------------------------------------------------
// This is the way the tentative_g_score should be calculated.
// Do you include the current g_score in your calculation parent.distance(self) ?
tentative_g_score := g_score[current] + dist_between(current,neighbor)
// -------------------------------------------------------------------
if neighbor in closedset
if tentative_g_score >= g_score[neighbor]
continue
// -------------------------------------------------------------------
// You never make this comparrison
if neighbor not in openset or tentative_g_score < g_score[neighbor]
// -------------------------------------------------------------------
came_from[neighbor] := current
g_score[neighbor] := tentative_g_score
f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
if neighbor not in openset
add neighbor to openset
return failure
function reconstruct_path(came_from, current_node)
if current_node in came_from
p := reconstruct_path(came_from, came_from[current_node])
return (p + current_node)
else
return current_node
Related
I am following this pseudocode trying find the best path from a maze using a* algorithm in python.
The maze will be something like this. It's a 2d array you can only navigate in the zeros the x are walls:
You have one entrance and one exit.
xxx0xx
xx00x0
xx0000
xxxx0x
My biggest problem is to understand how I set infinity in order to calculate the distance cost.
Looking in the pseudocode I though in doing something like this. But it's giving me error when I try to get the infinity value of next element
g_score = {dist: float("inf") for dist in range(len(array))}
pseudocode:
// A* finds a path from start to goal.
// h is the heuristic function. h(n) estimates the cost to reach goal from node n.
function A_Star(start, goal, h)
// The set of discovered nodes that may need to be (re-)expanded.
// Initially, only the start node is known.
// This is usually implemented as a min-heap or priority queue rather than a hash-set.
openSet := {start}
// For node n, cameFrom[n] is the node immediately preceding it on the cheapest path from start
// to n currently known.
cameFrom := an empty map
// For node n, gScore[n] is the cost of the cheapest path from start to n currently known.
gScore := map with default value of Infinity
gScore[start] := 0
// For node n, fScore[n] := gScore[n] + h(n). fScore[n] represents our current best guess as to
// how short a path from start to finish can be if it goes through n.
fScore := map with default value of Infinity
fScore[start] := h(start)
while openSet is not empty
// This operation can occur in O(Log(N)) time if openSet is a min-heap or a priority queue
current := the node in openSet having the lowest fScore[] value
if current = goal
return reconstruct_path(cameFrom, current)
openSet.Remove(current)
for each neighbor of current
// d(current,neighbor) is the weight of the edge from current to neighbor
// tentative_gScore is the distance from start to the neighbor through current
tentative_gScore := gScore[current] + d(current, neighbor)
if tentative_gScore < gScore[neighbor]
// This path to neighbor is better than any previous one. Record it!
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := tentative_gScore + h(neighbor)
if neighbor not in openSet
openSet.add(neighbor)
// Open set is empty but goal was never reached
return failure
My code: I still need to add the track parent part
def a_star(a, start, goal):
dir = [[0,1],[-1,0],[0,-1],[1,0]] #up, right, left, down.
#setting all vertices to infinity
g_dist = {dist: float("inf") for dist in range(len(a))}
total_dist = {dist: float("inf") for dist in range(len(a))}
g_dist[start] = 0
total_dist[start] = heuristic(start,goal)
queue = PriorityQueue()
queue.put(start)
while queue:
currVertice = queue.get()
if currVertice == goal:
return True
for d in dir:
newVertice = (currVertice[0] + d[0], currVertice[1] + d[1])
#IsValid: checking if is out of bounds or if is a wall
if isValid(newVertice[0], newVertice[1]):
temp_g_dist = g_dist[currVertice] + 1
temp_total_dist = temp_g_dist + heuristic(newVertice,goal)
if temp_total_dist < total_dist[newVertice]:
g_dist[newVertice] = temp_g_dist
total_dist[newVertice] = temp_total_dist
queue.put(newVertice)
return False
My problem was in fact the way I setting the infinity values to my nodes distance. I fixed the code by creating the distances as a map in the following way:
g_dist = {(p,x): float("inf") for p in range(len(a)) for x in range(len(a[p]))}
total_dist = {(p,x): float("inf") for p in range(len(a)) for x in range(len(a[p]))}
"a" is the 2d array holding the maze.
I am trying to find the nearest points on an x,y plane within given radius using kd trees. I mapped all the points in the tree, but the problem arise since the points I am providing to the algorithm to search for the nearest point are themself mapped in the tree. In other words, I'm getting the same point in return so the distance is basically 0. So I should be looking for the second nearest point I guess. The way I thought it might be achievable was doing the same thing but getting the result that was worst than the best_result but better than the root. I have been trying to implement this but have been unsuccessful so far. I did this with nested for loops its way easier with for loops to get the second-best result.
here's my kd tree implementation:
def kdtree(points, depth = 0 ):
n = len(points)
if n<=0:
return None
axis = depth % k
sorted_points = sorted(points, key = lambda point: point[axis])
if n % 2 == 0:
median_idx = int(n/2)
else:
median_idx = floor(n/2)
return{
'point': sorted_points[median_idx],
'left': kdtree(sorted_points[:median_idx], depth+1),
'right': kdtree(sorted_points[median_idx+1:], depth+1)
}
Here is my function for searching the nearest point
kd_nearest_search (root, point, depth = 0):
if root is None:
return None
axis = depth % k
apposite_branch = None
if point[axis] < root['point'][axis]:
next_branch = root['left']
opposite_branch = root['right']
else:
next_branch = root['right']
opposite_branch = root['left']
best_result = best_distance(point,kdtree_closest_point(next_branch,point,depth+1), root['point'])
if distance(point,best_result) > (abs(point[axis] - root['point'][axis])):
best_result = best_distance(point,kdtree_closest_point(next_branch,point,depth+1), best_result)
return best_result
I don't know your kd implementation. But I'll give you the basic idea.
Create tuples of (min_possible_distance, type, object) where the distance is the minimum possible distance to your searched for point the type can be either "box" or "point", and the object is whatever the thing is.
Put those tuples into a heap.
And now, in pseudocode, you do this:
place (0, "box", root_of_tree) into new heap
while heap is not empty:
(distance, type, obj) = pop minimal element from heap.
if type == "point":
yield obj
else:
if obj has a point:
store (distance, "point", point) in heap
for child_box from obj:
store (min_distance", "box", child_box) in heap
This will yield points in the kd tree, from closest to farthest. So you can just search until you find the second one. But alternately you could ask for the 10 nearest and it would give you that instead.
Me and my colleague are discussing a implementation of Dijkstra's algorithm. Here is the implementation using Python:
def dijkstra(self, origin, destination):
"""Use Dijkstra's algorithm to find the cheapest path."""
routes = Heap()
for neighbor in self.neighbors(origin):
price = self.get_price(origin, neighbor)
routes.push(Route(price=price, path=[origin, neighbor]))
visited = set()
visited.add(origin)
while routes:
# Find the nearest yet-to-visit airport
price, path = routes.pop()
airport = path[-1]
if airport in visited:
continue
# We have arrived! Wo-hoo!
if airport is destination:
return price, path
# Tentative distances to all the unvisited neighbors
for neighbor in self.neighbors(airport):
if neighbor not in visited:
# Total spent so far plus the price of getting there
new_price = price + self.get_price(airport, neighbor)
new_path = path + [neighbor]
routes.push(Route(new_price, new_path))
visited.add(airport)
return float('infinity')
The controversial line here is:
if neighbor not in visited:
My point is that this line must be replaced with something like
if neighbor not in visited or new_price < cost_so_far[neighbor]
In all the implementations that I found of the algorithm says that you must check for the case when you reach a node with a cost lower than the current cost of the node. For example, the lines 17 and 18 of this pseudocode from Wikipedia:
1 function Dijkstra(Graph, source):
2 dist[source] ← 0 // Initialization
3
4 create vertex set Q
5
6 for each vertex v in Graph:
7 if v ≠ source
8 dist[v] ← INFINITY // Unknown distance from source to v
9 prev[v] ← UNDEFINED // Predecessor of v
10
11 Q.add_with_priority(v, dist[v])
12
13
14 while Q is not empty: // The main loop
15 u ← Q.extract_min() // Remove and return best vertex
16 for each neighbor v of u: // only v that is still in Q
17 alt = dist[u] + length(u, v)
18 if alt < dist[v]
19 dist[v] ← alt
20 prev[v] ← u
21 Q.decrease_priority(v, alt)
22
23 return dist[], prev[]
The question is: Is my colleague's implementation correct or the code should be modified in order to check if you reach some neighbour with a price lower than the current one?
Note: Here is the source code of my colleague's implementation.
So, the question is whether the line in your code
if neighbor not in visited:
can or must be replaced by
if neighbor not in visited or new_price < cost_so_far[neighbor]:
The answer is: can, yes; must, no.
Adding new_price < cost_so_far[neighbor] will not change anything in the flow of the algorithm, it will be false every time neighbor not in visited is false.
The reason is how Dijkstra's algorithm works.
Essentially, it builds a tree of shortest paths.
Whenever an airport is added to visited, it is considered to be in the tree: by this time, the algorithm has already found the shortest path to this airport.
Assume that at step x, we add a certain airport A to visited.
Further assume that at step y > x, cost_so_far to airport A decreased.
How could it decrease?
It would require that some new_price = price + self.get_price(airport, neighbor) is less than the price at step y.
Recall now that routes is a priority queue, so it supplies prices in non-decreasing order.
The edges of the graph are also non-negative (otherwise, Dijkstra's algorithm indeed produces a wrong result and is not applicable).
So, we arrive at a contradiction: the new new_price is at least the old price, but turned out to be less than that.
The source of confusion is perhaps that the main loop of the implementation considers some routes one-by-one.
Essentially, these routes correspond to edges of the graph.
So, there can be |E| routes, but only |V| of them will get accepted, all others will fail the condition if airport in visited: continue.
If we implement the algorithm so that each iteration of the main loop adds exactly one airport to visited (exactly one vertex to the tree of shortest paths), the whole matter may become clearer.
I have a blank grid of 100, 100 tiles. Start point is (0,0), goal is (99,99). Tiles are 4-way connections.
My floodfill algorithm finds the shortest path in 30ms, but my A* implementation is around 10x slower.
Note: A* is consistently slower (3 - 10x) than my floodfill, no matter what kind of size of grid or layout. Because the floodfill is simple, then I suspect I'm missing some kind of optimisation in the A*.
Here's the function. I use Python's heapq to maintain a f-sorted list. The 'graph' holds all nodes, goals, neighbours and g/f values.
import heapq
def solve_astar(graph):
open_q = []
heapq.heappush(open_q, (0, graph.start_point))
while open_q:
current = heapq.heappop(open_q)[1]
current.seen = True # Equivalent of being in a closed queue
for n in current.neighbours:
if n is graph.end_point:
n.parent = current
open_q = [] # Clearing the queue stops the process
# Ignore if previously seen (ie, in the closed queue)
if n.seen:
continue
# Ignore If n already has a parent and the parent is closer
if n.parent and n.parent.g <= current.g:
continue
# Set the parent, or switch parents if it already has one
if not n.parent:
n.parent = current
elif n.parent.g > current.g:
remove_from_heap(n, n.f, open_q)
n.parent = current
# Set the F score (simple, uses Manhattan)
set_f(n, n.parent, graph.end_point)
# Push it to queue, prioritised by F score
heapq.heappush(open_q, (n.f, n))
def set_f(point, parent, goal):
point.g += parent.g
h = get_manhattan(point, goal)
point.f = point.g + h
It's a tie-breaker issue. On an empty grid, starting at (0,0) and going to (99,99) produces many tiles with the same f-score.
By adding a tiny nudge to the heuristic, tiles that are slightly closer to the destination will get selected first, meaning the goal is reached quicker and fewer tiles need to be checked.
def set_f(point, parent, goal):
point.g += parent.g
h = get_manhattan(point, goal) * 1.001
point.f = point.g + h
This resulted in around a 100x improvement, making it much faster than floodfill.
Goal: Trying to convert some of the lines of an algorithm written in python to pseudocode.
Goal of the given algorithm: Find all cycles in a directed graph with cycles.
Where I stand: I well understand the theory behind the algorithm, I have also coded different versions on my own, however I cannot write an algorithm that small, efficient and correct on my own.
Source: stackoverflow
What I have done so far: I cannot describe enough how many weeks spent on it, have coded Tarjan, various versions DFS, Flloyd etc in php but unfortunately they are partial solutions only and one have to extend them more.
In addition: I have run this algorithm online and it worked, I need it for a school project that I am stack and cannot proceed further.
This is the algorithm:
def paths_rec(path,edges):
if len(path) > 0 and path[0][0] == path[-1][1]:
print "cycle", path
return #cut processing when find a cycle
if len(edges) == 0:
return
if len(path) == 0:
#path is empty so all edges are candidates for next step
next_edges = edges
else:
#only edges starting where the last one finishes are candidates
next_edges = filter(lambda x: path[-1][1] == x[0], edges)
for edge in next_edges:
edges_recursive = list(edges)
edges_recursive.remove(edge)
#recursive call to keep on permuting possible path combinations
paths_rec(list(path) + [edge], edges_recursive)
def all_paths(edges):
paths_rec(list(),edges)
if __name__ == "__main__":
#edges are represented as (node,node)
# so (1,2) represents 1->2 the edge from node 1 to node 2.
edges = [(1,2),(2,3),(3,4),(4,2),(2,1)]
all_paths(edges)
This is what I have managed to write in pseudocode from it, I have marked with #? the lines I do not understand. Once I have them in pseudocode I can code them in php with which I am a lot familiar.
procedure paths_rec (path, edges)
if size(path) > 0 and path[0][0] equals path[-1][1]
print "cycle"
for each element in path
print element
end of for
return
end of if
if size(edges) equals 0
return
end of if
if size(path) equals 0
next_edges equals edges
else
next edges equals filter(lambda x: path[-1][1] == x[0], edges) #?
end of else
for each edge in next_edges
edges_recursive = list(edges) #?
edges_recursive.remove(edge)#?
#recursive call to keep on permuting possible path combinations
paths_rec(list(path) + [edge], edges_recursive)#?
The line next_edges = filter(lambda x: path[-1][1] == x[0], edges) creates a new list containing those edges in edges whose first point is the same as the second point of the last element of the current path, and is equivalent to
next_edges = []
for x in edges:
if path[len(path) - 1][1] == x[0]
next_edges.append[x]
The lines create a new copy of the list of edges edges, so that when edge is removed from this copy, it doesn't change the list edges
edges_recursive = list(edges)
edges_recursive.remove(edge)
paths_rec(list(path) + [edge], edges_recursive) is just the recursive call with the path with edge added to the end and the new list of edges that has edge removed from it.