bidirectional_a_star.py

"""
https://en.wikipedia.org/wiki/Bidirectional_search
"""

from __future__ importannotations

importtime
frommathimportsqrt

# 1 for manhattan, 0 for euclidean
HEURISTIC=0

grid= [
 [0, 0, 0, 0, 0, 0, 0],
 [0, 1, 0, 0, 0, 0, 0], # 0 are free path whereas 1's are obstacles
 [0, 0, 0, 0, 0, 0, 0],
 [0, 0, 1, 0, 0, 0, 0],
 [1, 0, 1, 0, 0, 0, 0],
 [0, 0, 0, 0, 0, 0, 0],
 [0, 0, 0, 0, 1, 0, 0],
]

delta= [[-1, 0], [0, -1], [1, 0], [0, 1]] # up, left, down, right

TPosition=tuple[int, int]


classNode:
"""
 >>> k = Node(0, 0, 4, 3, 0, None)
 >>> k.calculate_heuristic()
 5.0
 >>> n = Node(1, 4, 3, 4, 2, None)
 >>> n.calculate_heuristic()
 2.0
 >>> l = [k, n]
 >>> n == l[0]
 False
 >>> l.sort()
 >>> n == l[0]
 True
 """

def__init__(
self,
pos_x: int,
pos_y: int,
goal_x: int,
goal_y: int,
g_cost: int,
parent: Node|None,
 ) ->None:
self.pos_x=pos_x
self.pos_y=pos_y
self.pos= (pos_y, pos_x)
self.goal_x=goal_x
self.goal_y=goal_y
self.g_cost=g_cost
self.parent=parent
self.h_cost=self.calculate_heuristic()
self.f_cost=self.g_cost+self.h_cost

defcalculate_heuristic(self) ->float:
"""
 Heuristic for the A*
 """
dy=self.pos_x-self.goal_x
dx=self.pos_y-self.goal_y
ifHEURISTIC==1:
returnabs(dx) +abs(dy)
else:
returnsqrt(dy**2+dx**2)

def__lt__(self, other: Node) ->bool:
returnself.f_cost<other.f_cost


classAStar:
"""
 >>> astar = AStar((0, 0), (len(grid) - 1, len(grid[0]) - 1))
 >>> (astar.start.pos_y + delta[3][0], astar.start.pos_x + delta[3][1])
 (0, 1)
 >>> [x.pos for x in astar.get_successors(astar.start)]
 [(1, 0), (0, 1)]
 >>> (astar.start.pos_y + delta[2][0], astar.start.pos_x + delta[2][1])
 (1, 0)
 >>> astar.retrace_path(astar.start)
 [(0, 0)]
 >>> astar.search() # doctest: +NORMALIZE_WHITESPACE
 [(0, 0), (1, 0), (2, 0), (2, 1), (2, 2), (2, 3), (3, 3),
 (4, 3), (4, 4), (5, 4), (5, 5), (6, 5), (6, 6)]
 """

def__init__(self, start: TPosition, goal: TPosition):
self.start=Node(start[1], start[0], goal[1], goal[0], 0, None)
self.target=Node(goal[1], goal[0], goal[1], goal[0], 99999, None)

self.open_nodes= [self.start]
self.closed_nodes: list[Node] = []

self.reached=False

defsearch(self) ->list[TPosition]:
whileself.open_nodes:
# Open Nodes are sorted using __lt__
self.open_nodes.sort()
current_node=self.open_nodes.pop(0)

ifcurrent_node.pos==self.target.pos:
returnself.retrace_path(current_node)

self.closed_nodes.append(current_node)
successors=self.get_successors(current_node)

forchild_nodeinsuccessors:
ifchild_nodeinself.closed_nodes:
continue

ifchild_nodenotinself.open_nodes:
self.open_nodes.append(child_node)
else:
# retrieve the best current path
better_node=self.open_nodes.pop(self.open_nodes.index(child_node))

ifchild_node.g_cost<better_node.g_cost:
self.open_nodes.append(child_node)
else:
self.open_nodes.append(better_node)

return [self.start.pos]

defget_successors(self, parent: Node) ->list[Node]:
"""
 Returns a list of successors (both in the grid and free spaces)
 """
successors= []
foractionindelta:
pos_x=parent.pos_x+action[1]
pos_y=parent.pos_y+action[0]
ifnot (0<=pos_x<=len(grid[0]) -1and0<=pos_y<=len(grid) -1):
continue

ifgrid[pos_y][pos_x] !=0:
continue

successors.append(
Node(
pos_x,
pos_y,
self.target.pos_y,
self.target.pos_x,
parent.g_cost+1,
parent,
 )
 )
returnsuccessors

defretrace_path(self, node: Node|None) ->list[TPosition]:
"""
 Retrace the path from parents to parents until start node
 """
current_node=node
path= []
whilecurrent_nodeisnotNone:
path.append((current_node.pos_y, current_node.pos_x))
current_node=current_node.parent
path.reverse()
returnpath


classBidirectionalAStar:
"""
 >>> bd_astar = BidirectionalAStar((0, 0), (len(grid) - 1, len(grid[0]) - 1))
 >>> bd_astar.fwd_astar.start.pos == bd_astar.bwd_astar.target.pos
 True
 >>> bd_astar.retrace_bidirectional_path(bd_astar.fwd_astar.start,
 ... bd_astar.bwd_astar.start)
 [(0, 0)]
 >>> bd_astar.search() # doctest: +NORMALIZE_WHITESPACE
 [(0, 0), (0, 1), (0, 2), (1, 2), (1, 3), (2, 3), (2, 4),
 (2, 5), (3, 5), (4, 5), (5, 5), (5, 6), (6, 6)]
 """

def__init__(self, start: TPosition, goal: TPosition) ->None:
self.fwd_astar=AStar(start, goal)
self.bwd_astar=AStar(goal, start)
self.reached=False

defsearch(self) ->list[TPosition]:
whileself.fwd_astar.open_nodesorself.bwd_astar.open_nodes:
self.fwd_astar.open_nodes.sort()
self.bwd_astar.open_nodes.sort()
current_fwd_node=self.fwd_astar.open_nodes.pop(0)
current_bwd_node=self.bwd_astar.open_nodes.pop(0)

ifcurrent_bwd_node.pos==current_fwd_node.pos:
returnself.retrace_bidirectional_path(
current_fwd_node, current_bwd_node
 )

self.fwd_astar.closed_nodes.append(current_fwd_node)
self.bwd_astar.closed_nodes.append(current_bwd_node)

self.fwd_astar.target=current_bwd_node
self.bwd_astar.target=current_fwd_node

successors= {
self.fwd_astar: self.fwd_astar.get_successors(current_fwd_node),
self.bwd_astar: self.bwd_astar.get_successors(current_bwd_node),
 }

forastarin [self.fwd_astar, self.bwd_astar]:
forchild_nodeinsuccessors[astar]:
ifchild_nodeinastar.closed_nodes:
continue

ifchild_nodenotinastar.open_nodes:
astar.open_nodes.append(child_node)
else:
# retrieve the best current path
better_node=astar.open_nodes.pop(
astar.open_nodes.index(child_node)
 )

ifchild_node.g_cost<better_node.g_cost:
astar.open_nodes.append(child_node)
else:
astar.open_nodes.append(better_node)

return [self.fwd_astar.start.pos]

defretrace_bidirectional_path(
self, fwd_node: Node, bwd_node: Node
 ) ->list[TPosition]:
fwd_path=self.fwd_astar.retrace_path(fwd_node)
bwd_path=self.bwd_astar.retrace_path(bwd_node)
bwd_path.pop()
bwd_path.reverse()
path=fwd_path+bwd_path
returnpath


if__name__=="__main__":
# all coordinates are given in format [y,x]
init= (0, 0)
goal= (len(grid) -1, len(grid[0]) -1)
forelemingrid:
print(elem)

start_time=time.time()
a_star=AStar(init, goal)
path=a_star.search()
end_time=time.time() -start_time
print(f"AStar execution time = {end_time:f} seconds")

bd_start_time=time.time()
bidir_astar=BidirectionalAStar(init, goal)
bd_end_time=time.time() -bd_start_time
print(f"BidirectionalAStar execution time = {bd_end_time:f} seconds")