def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
def heuristic(row, col):
#############################################################################
h = abs(goal_row - row) + abs(goal_col - col)
return h
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue()
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
h_start = heuristic(start_row, start_col)
g_start = costFn(start)
f_start = g_start + h_start
open_set.put(start, f_start)
path = []
while open_set:
node, cost = open_set.pop()
if node == goal:
print("PATH FOUND")
j = goal
while j != start:
par_j = parent[j[0]][j[1]]
path.append(par_j)
j = par_j
closed_set.add(node)
break
closed_set.add(node)
for i in ACTIONS:
child = (node[0] + i[0], node[1] + i[1])
h_cost = heuristic(child[0], child[1])
g_node = cost - heuristic(node[0], node[1])
f_cost = g_node + h_cost + costFn(child)
if child not in open_set and child not in closed_set:
if child not in obstacles and 0 <= child[
0] < n_rows and 0 <= child[1] < n_cols:
open_set.put(child, f_cost)
parent[child[0]][child[1]] = (node[0], node[1])
elif child in open_set:
if f_cost < open_set.get(child):
open_set.put(child, f_cost)
parent[child[0]][child[1]] = (node[0], node[1])
path.reverse()
path.append(goal)
for i in range(len(path) - 1):
move = (path[i + 1][0] - path[i][0], path[i + 1][1] - path[i][1])
movement.append(move)
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
#############################################################################
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: v)
closed_set = Queue()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
path_storage = {}
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
return abs(goal_row - row) + abs(goal_col - col)
#############################################################################
open_set.put(start, 0)
i = 0
while open_set:
child = []
node_current, v = open_set.pop()
closed_set.add(node_current)
current_row, current_col = node_current
h = heuristic(current_row, current_col)
cost_child = v - h
child = [((current_row + 1, current_col), cost_child + costFn(
(current_row + 1, current_col)) +
heuristic(current_row + 1, current_col)),
((current_row, current_col - 1), cost_child + costFn(
(current_row, current_col - 1)) +
heuristic(current_row, current_col - 1)),
((current_row - 1, current_col), cost_child + costFn(
(current_row - 1, current_col)) +
heuristic(current_row - 1, current_col)),
((current_row, current_col + 1), cost_child + costFn(
(current_row, current_col + 1)) +
heuristic(current_row, current_col + 1))]
for node in child:
child_node, cost = node
child_row, child_col = child_node
if child_node not in obstacles and child_row >= 0 and child_col >= 0 and child_row < n_rows and child_col < n_cols:
if child_node not in closed_set and child_node not in open_set:
print('Child node is', node)
if child_node == goal:
path_storage[child_node] = node_current
path_list = [goal]
print('The goal is', goal)
while path_list[-1] != start:
print('The parent node is',
path_storage[path_list[-1]])
path_list.append(path_storage[path_list[-1]])
path_list.reverse()
iter = 0
for iter in range(len(path_list) - 1):
r, c = path_list[iter]
r2, c2 = path_list[iter + 1]
row = r2 - r
col = c2 - c
if (row == -1 and col == 0):
movement.append((-1, 0))
if (row == 0 and col == -1):
movement.append((0, -1))
if (row == 1 and col == 0):
movement.append((1, 0))
if (row == 0 and col == 1):
movement.append((0, 1))
iter += 1
return movement, closed_set
else:
open_set.put(child_node, cost)
path_storage[child_node] = node_current
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)). In all of the grid maps, the cost is always 1.
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: abs(v))
closed_set = Queue()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
#############################################################################
def heuristic(row, col):
R = goal[0] - row
C = goal[1] - col
hyp = R * R + C * C
return abs(math.sqrt(hyp))
def Gvalue(row, col):
return abs(goal[0] - row + goal[1] - col)
def backtrace(parent, start, goal):
path = [goal]
while path[-1] != start:
path.append(parent[path[-1]])
path.reverse()
iter = len(path)
i = 0
while i < iter - 1:
R = path[i + 1][0] - path[i][0]
C = path[i + 1][1] - path[i][1]
if (R == -1 and C == 0):
movement.append(ACTIONS[0])
if (R == 0 and C == -1):
movement.append(ACTIONS[1])
if (R == 1 and C == 0):
movement.append(ACTIONS[2])
if (R == 0 and C == 1):
movement.append(ACTIONS[3])
i += 1
return movement, closed_set
def TransitionModel(node, value):
current_row = node[0]
current_col = node[1]
direction_can_go = []
#down
TempRow = current_row
TempCol = current_col + 1
TempNode = (TempRow, TempCol)
# print("TempNode ::",TempNode)
if TempNode not in obstacles and TempRow >= 0 and TempCol >= 0 and TempRow <= n_rows and TempCol <= n_cols:
returnnode = [
TempNode,
Gvalue(TempRow, TempCol) + heuristic(TempRow, TempCol)
]
direction_can_go.append(returnnode)
#right
TempRow = current_row + 1
TempCol = current_col
TempNode = (TempRow, TempCol)
if TempNode not in obstacles and TempRow >= 0 and TempCol >= 0 and TempRow <= n_rows and TempCol <= n_cols:
returnnode = [
TempNode,
Gvalue(TempRow, TempCol) + heuristic(TempRow, TempCol)
]
direction_can_go.append(returnnode)
#left
TempRow = current_row - 1
TempCol = current_col
TempNode = (TempRow, TempCol)
if TempNode not in obstacles and TempRow >= 0 and TempCol >= 0 and TempRow <= n_rows and TempCol <= n_cols:
returnnode = [
TempNode,
Gvalue(TempRow, TempCol) + heuristic(TempRow, TempCol)
]
direction_can_go.append(returnnode)
#up
TempRow = current_row
TempCol = current_col - 1
TempNode = (TempRow, TempCol)
if TempNode not in obstacles and TempRow >= 0 and TempCol >= 0 and TempRow <= n_rows and TempCol <= n_cols:
returnnode = [
TempNode,
Gvalue(TempRow, TempCol) + heuristic(TempRow, TempCol)
]
direction_can_go.append(returnnode)
# print("direction_can_go ::",direction_can_go)
#print("node ::",node)
return direction_can_go
open_set.put(start, 0)
parent = {}
while len(open_set):
node, value = open_set.pop()
if node == goal:
print("found it")
movement, closed_set = backtrace(parent, start, goal)
print("movement ::", movement)
return movement, closed_set
#costFn(node)
closed_set.add(node)
actions_posible = TransitionModel(node, value)
for nextnode in actions_posible:
#print("nextnode ::" ,nextnode[0])
if nextnode[0] not in closed_set and nextnode[0] not in open_set:
open_set.put(nextnode[0], nextnode[1])
parent[nextnode[0]] = node
elif nextnode[0] in open_set and nextnode[1] < open_set.get(
nextnode[0]):
open_set.put(nextnode[0], nextnode[1])
parent[nextnode[0]] = node
#############################################################################
return movement, closed_set
def uniform_cost_search(grid_size, start, goal, obstacles, costFn, logger):
"""
Uniform-cost search algorithm finds the optimal path from
the start cell to the goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: abs(v))
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below
# ----------------------------------------
"""
Unique operations of PriorityQueue:
PriorityQueue(order="min", f=lambda v: v): build up a priority queue
using the function f to compute the priority based on the value
of an element
s.put(x, v): add the element x with value v to the queue
update the value of x if x is already in the queue
s.get(x): get the value of the element x
raise KeyError if x is not in s
s.pop(): return and remove the element with highest priority in s;
raise IndexError if s is empty
if order is "min", the element with minimum f(v) will be popped;
if order is "max", the element with maximum f(v) will be popped.
Example:
s = PriorityQueue(order="max", f=lambda v: abs(v))
s.put((1,1), -1)
s.put((2,2), -20)
s.put((3,3), 10)
x, v = s.pop() # the element with maximum value of abs(v) will be popped
assert(x == (2,2) and v == -20)
assert(x not in s)
assert(x.get((1,1)) == -1)
assert(x.get((3,3)) == 10)
"""
#############################################################################
open_set.put(start, 0)
while open_set:
node = open_set.pop()
if node[0] == goal:
node = node[0]
while node is not start:
movement.insert(0, actions[node[0]][node[1]])
node = parent[node[0]][node[1]]
return movement, closed_set
closed_set.add(node[0])
for action in ACTIONS:
child = (node[0][0] + action[0], node[0][1] + action[1])
cost = costFn(child)
if child not in open_set and child not in closed_set and child not in obstacles and child[
0] >= 0 and child[0] < n_rows and child[1] >= 0 and child[
1] < n_cols:
open_set.put(child, node[1] + cost)
parent[(child)[0]][(child)[1]] = node[0]
actions[(child)[0]][(child)[1]] = action
elif child in open_set and open_set.get(child) > node[1] + cost:
open_set.put(child, node[1] + cost)
parent[(child)[0]][(child)[1]] = node[0]
actions[(child)[0]][(child)[1]] = action
#############################################################################
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: abs(v))
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
return abs(row - goal_row) + abs(col - goal_col)
open_set.put(start, (0 + heuristic(start_row, start_col)))
while open_set:
node = open_set.pop()
if node[0] == goal:
node = node[0]
while node is not start:
movement.insert(0, actions[node[0]][node[1]])
node = parent[node[0]][node[1]]
return movement, closed_set
closed_set.add(node[0])
for action in ACTIONS:
child = (node[0][0] + action[0], node[0][1] + action[1])
cost = costFn(child)
heur = heuristic(child[0], child[1])
old_heur = heuristic(node[0][0], node[0][1])
if child not in open_set and child not in closed_set and child not in obstacles and child[
0] >= 0 and child[0] < n_rows and child[1] >= 0 and child[
1] < n_cols:
open_set.put(child, node[1] + cost + heur - old_heur)
parent[(child)[0]][(child)[1]] = node[0]
actions[(child)[0]][(child)[1]] = action
elif child in open_set and open_set.get(
child) > node[1] + cost + heur:
open_set.put(child, node[1] + cost + heur - old_heur)
parent[(child)[0]][(child)[1]] = node[0]
actions[(child)[0]][(child)[1]] = action
#############################################################################
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)). In all of the grid maps, the cost is always 1.
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue()
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
return abs(row - goal_row) + abs(col - goal_col)
movement = []
temp_movement={}
movement_rev=[]
#put start node into the open_set with fval 0
open_set.put((start_row,start_col),0)
#looping until open_set is empty
while len(open_set) != 0:
node = open_set.pop()
curr_node, node_fval = node
curr_row, curr_col = curr_node
if curr_node == goal:
temp = goal
# code to traceback path form the goal to start
while temp!=start:
temp_row,temp_col=temp
temp1_row,temp1_col=temp_movement[temp]
temp_row=temp_row-(temp1_row)
temp_col=temp_col-(temp1_col)
movement_rev.append(temp_movement[temp])
temp=temp_row,temp_col
movement=Reverse(movement_rev)
return movement,closed_set
closed_set.add(curr_node)
neighbors,my_move = negh(curr_row, curr_col, n_rows, n_cols, obstacles)
for next_node in range(len(neighbors)):
total_cost = 0
child_row, child_col = neighbors[next_node]
#to calculate the F-value: for A* F(node) = g(node) + h(node)
#cost_g is the sum of path cost between current node and child_node + path_cost of current node
#and h(node) is heuristics value between child_node and goal node
cost_g = costFn((child_row, child_col))+ (node_fval - heuristic(curr_row, curr_col))
cost_h = heuristic(child_row, child_col)
total_cost = cost_g + cost_h
if neighbors[next_node] not in open_set and neighbors[next_node] not in closed_set:
#insert child_node and f_value into openset
open_set.put(neighbors[next_node], total_cost)
temp_movement.update({neighbors[next_node]:my_move[neighbors[next_node]]})
elif neighbors[next_node] in open_set and total_cost < node_fval+heuristic(curr_row,curr_col):
curr_node = neighbors[next_node]
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: v)
closed_set = Queue()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
h = abs(goal_row - row) + abs(goal_col - col)
return h
#############################################################################
'''
Based on the pseudo code from the slides
'''
open_set.put(start, 0 + heuristic(start_row, start_col))
while open_set:
children = []
currentNode, v = open_set.pop()
if currentNode == goal: # reach the goal start backtrace
closed_set.add(
currentNode
) # for the same outcome with the demo video provided
p_row, p_col = goal
while (p_row, p_col) != start:
movement.append(actions[p_row][p_col])
p_row, p_col = parent[p_row][p_col]
movement.reverse()
return movement, closed_set
currentNode_row, currentNode_col = currentNode
closed_set.add(currentNode)
currentNodeHeuristic = heuristic(currentNode_row, currentNode_col)
# get the g cost for compute the children's total costs
g_cost = v - currentNodeHeuristic
# get the children
children = [((currentNode_row - 1, currentNode_col), g_cost + costFn(
(currentNode_row - 1, currentNode_col)) +
heuristic(currentNode_row - 1, currentNode_col)),
((currentNode_row, currentNode_col - 1), g_cost + costFn(
(currentNode_row, currentNode_col - 1)) +
heuristic(currentNode_row, currentNode_col - 1)),
((currentNode_row + 1, currentNode_col), g_cost + costFn(
(currentNode_row + 1, currentNode_col)) +
heuristic(currentNode_row + 1, currentNode_col)),
((currentNode_row, currentNode_col + 1), g_cost + costFn(
(currentNode_row, currentNode_col + 1)) +
heuristic(currentNode_row, currentNode_col + 1))]
# expand the children
for child in children:
node, cost = child
child_row, child_col = node
if (
child_row, child_col
) not in obstacles and child_row >= 0 and child_col >= 0 and child_row < n_rows and child_col < n_cols:
if node not in closed_set and node not in open_set:
open_set.put(node, cost)
parent[child_row][child_col] = currentNode
action = (child_row - currentNode_row,
child_col - currentNode_col)
if action == (-1, 0):
actions[child_row][child_col] = ACTIONS[0]
if action == (0, -1):
actions[child_row][child_col] = ACTIONS[1]
if action == (1, 0):
actions[child_row][child_col] = ACTIONS[2]
if action == (0, 1):
actions[child_row][child_col] = ACTIONS[3]
# if need to update with a lower cost
elif child in open_set and open_set.get(
(child_row, child_col)) > cost:
open_set.put(node, cost)
parent[child_row][child_col] = currentNode
action = (child_row - currentNode_row,
child_col - currentNode_col)
if action == (-1, 0):
actions[child_row][child_col] = ACTIONS[0]
if action == (0, -1):
actions[child_row][child_col] = ACTIONS[1]
if action == (1, 0):
actions[child_row][child_col] = ACTIONS[2]
if action == (0, 1):
actions[child_row][child_col] = ACTIONS[3]
def uniform_cost_search(grid_size, start, goal, obstacles, costFn, logger):
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: v)
closed_set = Queue()
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
parents = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# Variables initialization
open_set.put(start, 0)
child_rows = 0
child_cols = 0
g = 0 # Path cost
# Define UCS
while (open_set != []):
# Take out (x,y)(parent) and path v(cost) from openset
temp_parent = open_set.pop()
parent, v = temp_parent
# Determine whether the agent reach to goal
if parent == goal:
# Search path
for j in range(n_cols * n_rows):
parent_rows, parent_cols = parent
# Stop and return
if parent == start:
print(movement)
return movement, closed_set
# record action and put it in to movement()
act = actions[parent_rows][parent_cols]
movement.insert(0, act)
# Update parent
parent = parents[parent_rows][parent_cols]
# Add parent to closedset
closed_set.add(parent)
# Search each direction of the parent
for i in range(4):
# Moving direction
action_rows = action[i][0]
action_cols = action[i][1]
# Calculate child position
parent_rows, parent_cols = parent
child_rows, child_cols = parent_rows + action_rows, parent_cols + action_cols
child = child_rows, child_cols
# Update g(path cost)
g = v + costFn((parent_rows, parent_cols))
# Restrict the node in the map
if (child not in obstacles) and (child_rows < n_rows) and (
child_cols < n_cols) and (child_rows >= 0) and (child_cols
>= 0):
if (child not in open_set) and (child not in closed_set):
# Add into openset
open_set.put(child, g)
# Record parent and action for each step
parents[child_rows][child_cols] = parent
actions[child_rows][child_cols] = tuple(action[i])
# Update path cost if agent already in open list
elif (child in open_set) and (g < open_set.get(child)):
# Add into openset
open_set.put(child, g)
# Record parent and action for each step
parents[child_rows][child_cols] = parent
actions[child_rows][child_cols] = tuple(action[i])
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue()
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[(-2, -2) for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[(-2, -2) for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
distance = abs(row - goal[0]) + abs(col - goal[1])
return distance
#############################################################################
open_set.put(start, heuristic(start[0], start[1]))
while open_set:
current, current_cost = open_set.pop()
current_cost = current_cost - heuristic(current[0], current[1])
closed_set.add(current)
up = (current[0] - 1, current[1])
left = (current[0], current[1] - 1)
down = (current[0] + 1, current[1])
right = (current[0], current[1] + 1)
if (current == goal):
break
if (up[0] >= 0 and up not in closed_set and up not in obstacles
and up not in open_set):
cost = current_cost + costFn(up) + heuristic(up[0], up[1])
open_set.put(up, cost)
parent[up[0]][up[1]] = current
actions[up[0]][up[1]] = ACTIONS[0]
if (left[1] >= 0 and left not in closed_set and left not in obstacles
and left not in open_set):
cost = current_cost + costFn(left) + heuristic(left[0], left[1])
open_set.put(left, cost)
parent[left[0]][left[1]] = current
actions[left[0]][left[1]] = ACTIONS[1]
if (down[0] < grid_size[0] and down not in closed_set
and down not in obstacles and down not in open_set):
cost = current_cost + costFn(down) + heuristic(down[0], down[1])
open_set.put(down, cost)
parent[down[0]][down[1]] = current
actions[down[0]][down[1]] = ACTIONS[2]
if (right[1] < grid_size[1] and right not in closed_set
and right not in obstacles and right not in open_set):
cost = current_cost + costFn(right) + heuristic(right[0], right[1])
open_set.put(right, cost)
parent[right[0]][right[1]] = current
actions[right[0]][right[1]] = ACTIONS[3]
movement = find_path(parent, actions, start, goal)
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: abs(v))
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# Student Code Below:
# ----------------------------------------
def heuristic(q):
return (abs(goal_row - q[0]) + abs(goal_col - q[1]))
open_set.put((start_row, start_col), 0 + heuristic((start_row, start_col)))
parent[start_row][start_col] = (start_row, start_col)
while len(open_set) != 0:
node, f_node = open_set.pop()
g_node = f_node - heuristic(node)
if node[0] == goal_row and node[1] == goal_col:
break
closed_set.add(node)
for action in ACTIONS:
child = (node[0] + action[0], node[1] + action[1])
if collision_check(child, obstacles) == False and out_of_bounds(
child, n_rows, n_cols) == False:
f = (g_node + costFn(child)) + heuristic(child)
if child not in open_set and child not in closed_set:
parent[child[0]][child[1]] = node
actions[child[0]][child[1]] = action
open_set.put(child, f)
elif child in open_set and child not in closed_set and f < open_set.get(
child):
open_set.remove(child)
open_set.put(child, f)
parent[child[0]][child[1]] = node
actions[child[0]][child[1]] = action
while True:
movement.append(actions[node[0]][node[1]])
node = parent[node[0]][node[1]]
if node[0] == start_row and node[1] == start_col: break
movement.reverse()
#############################################################################
#############################################################################
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: v)
closed_set = OrderedSet()
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
parents = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
def heuristic(row, col):
h = abs(goal_row - row) + abs(goal_col - col)
#print(h)
return h
pass
# Variable Initialization
g = 0 # g(n)
h = heuristic(start_row, start_col) # h(n)
f = g + h # f(n) Path cost
# Add start point to openset
open_set.put(start, f)
# Define A*
while (open_set != []):
# Take out (x,y)(parent) and path v(cost) from openset
temp_parent = open_set.pop()
parent, v = temp_parent
parent_rows, parent_cols = parent
# Update h
h = heuristic(parent_rows, parent_cols)
# Determine whether the agent reach to goal
if parent == goal:
# Search path
for j in range(n_cols * n_rows):
parent_rows, parent_cols = parent
# Stop and return
if parent == start:
return movement, closed_set
# record action and put it in to movement()
act = actions[parent_rows][parent_cols]
movement.insert(0, act)
# Update parent
parent = parents[parent_rows][parent_cols]
# Add parent to closedset
closed_set.add(parent)
# Search each direction of the parent
for i in range(4):
# Moving direction
action_rows = action[i][0]
action_cols = action[i][1]
# # Calculate child position
child_rows, child_cols = parent_rows + action_rows, parent_cols + action_cols
child = child_rows, child_cols
# Update g, h and f value
g = (v - h) + costFn((parent_rows, parent_cols))
h_child = heuristic(child_rows, child_cols)
f = g + h_child
# Restrict the node in the map
if (child not in obstacles) and (child_rows < n_rows) and (
child_cols < n_cols) and (child_rows >= 0) and (child_cols
>= 0):
if (child not in open_set) and (child not in closed_set):
# Add into openset
open_set.put(child, f)
# Record parent and action for each step
parents[child_rows][child_cols] = parent
actions[child_rows][child_cols] = tuple(action[i])
elif (child in open_set) and (f < open_set.get(child)):
# Add into openset
open_set.put(child, f)
# Record parent and action for each step
parents[child_rows][child_cols] = parent
actions[child_rows][child_cols] = tuple(action[i])
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
open_set = PriorityQueue(order="min",
f=lambda v: v) # priority data structure
closed_set = OrderedSet() # OrderedSet data Structure
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
parents = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
# Manhanttan distance
def heuristic(row, col):
Manhanttan_dis = abs(goal_row - row) + abs(goal_col - col)
return Manhanttan_dis
g_start = 0 # initial path-cost
h_start = heuristic(start_row, start_col) # initial heuristic
f_start = g_start + h_start # initial total cost
open_set.put(start, f_start) # initial state
while len(open_set) > 0:
out = open_set.pop() # next expanded node
node, f = out
x, y = node # node location
if node == goal: # when expanded node is goal, stop
closed_set.add(node)
movement = path_backtrack(start, node, parents, actions)
return movement, closed_set
closed_set.add(node) # add expaned node to close_set
children = [(x - 1, y), (x, y - 1), (x + 1, y),
(x, y + 1)] # possible children(up,left,down right)
boundary = [(x, y) for x in range(n_rows)
for y in range(n_cols)] # boundary restriction
for child in children:
# child cost
h_parent = heuristic(node[0], node[1])
g_parent = f - h_parent
f_child = heuristic(child[0], child[1]) + g_parent + costFn(
(child[0], child[1]))
if child in boundary and child not in obstacles:
if child not in open_set and child not in closed_set:
open_set.put(child, f_child)
parents[child[0]][
child[1]] = node # remark parent for child
actions[child[0]][child[1]] = (
child[0] - node[0], child[1] - node[1]
) # remark actions for child
elif child in open_set and child not in closed_set:
if f_child < open_set.get(
child
): # update parent, action,cost of child existed in open_set
open_set.put(child, f_child)
parents[child[0]][child[1]] = node
actions[child[0]][child[1]] = (child[0] - node[0],
child[1] - node[1])
def uniform_cost_search(grid_size, start, goal, obstacles, costFn, logger):
"""
Uniform-cost search algorithm finds the optimal path from
the start cell to the goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
open_set = PriorityQueue(order="min", f=lambda v: v) # Priority sequence
closed_set = OrderedSet() # Orderedset data structure
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
parents = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
g_n = 0 # initial path_cost
open_set.put(start, g_n) # initial state
while len(open_set) > 0:
node, g = open_set.pop() # next expanded node
x, y = node # next expand node location
if node == goal: # when expended node is goal that return path (optimal)
closed_set.add(node)
movement = path_backtrack(start, node, parents, actions)
return movement, closed_set
closed_set.add(node) # add expaned node to close_set
children = [(x - 1, y), (x, y - 1), (x + 1, y),
(x, y + 1)] # possible children(up,left,down right)
boundary = [(x, y) for x in range(n_rows)
for y in range(n_cols)] # boundary restriction
for child in children:
g_child = g + costFn((child[0], child[1])) # child cost
if child in boundary and child not in obstacles:
if child not in open_set and child not in closed_set:
open_set.put(child, g_child)
parents[child[0]][child[1]] = node # parent remark
actions[child[0]][child[1]] = (child[0] - node[0],
child[1] - node[1]
) # actions remark
elif child in open_set and child not in closed_set: # update that OPEN_node
if g_child < open_set.get(
child
): # update the cost and parent,action of child existed in open_set
open_set.put(child, g_child)
parents[child[0]][child[1]] = node
actions[child[0]][child[1]] = (child[0] - node[0],
child[1] - node[1])
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)). In all of the grid maps, the cost is always 1.
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: v)
#closed_set = PriorityQueue(order="min", f=lambda v: abs(v))
closed_set = Queue()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
movement = []
#the path below is the dictionary that maintains key value pairs of parent and child
pathTrace = {}
path = {}
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
#pass
return abs(goal_row - row) + abs(goal_col - col)
#############################################################################
def Backtrace():
#############################################################################
path = [goal]
print('goal is', goal)
while path[-1] != start:
#if pathTrace[path[-1]] in closed_set and pathTrace[path[-1]] not in path:
print('parent is', pathTrace[path[-1]])
path.append(pathTrace[path[-1]])
path.reverse()
print(path)
num = 0
while num < len(path) - 1:
row, col = path[num]
parentRow, parentCol = path[num + 1]
dRow = parentRow - row
dCol = parentCol - col
if (dRow == -1 and dCol == 0):
movement.append((-1, 0))
if (dRow == 0 and dCol == -1):
movement.append((0, -1))
if (dRow == 1 and dCol == 0):
movement.append((1, 0))
if (dRow == 0 and dCol == 1):
movement.append((0, 1))
num += 1
return movement
#############################################################################
open_set.put(start, 0)
i = 0
while open_set:
children = []
currentNode, v = open_set.pop()
if currentNode == goal:
movement = Backtrace()
return movement, closed_set
currentNode_row, currentNode_col = currentNode
closed_set.add(currentNode)
currentNodeHeuristic = heuristic(currentNode_row, currentNode_col)
costG = v - currentNodeHeuristic
children = [((currentNode_row - 1, currentNode_col), costG + costFn(
(currentNode_row - 1, currentNode_col)) +
heuristic(currentNode_row - 1, currentNode_col)),
((currentNode_row + 1, currentNode_col), costG + costFn(
(currentNode_row + 1, currentNode_col)) +
heuristic(currentNode_row + 1, currentNode_col)),
((currentNode_row, currentNode_col - 1), costG + costFn(
(currentNode_row, currentNode_col - 1)) +
heuristic(currentNode_row, currentNode_col - 1)),
((currentNode_row, currentNode_col + 1), costG + costFn(
(currentNode_row, currentNode_col + 1)) +
heuristic(currentNode_row, currentNode_col + 1))]
for childNode in children:
node, cost = childNode
node_row, node_col = node
if (
node_row, node_col
) not in obstacles and node_row >= 0 and node_col >= 0 and node_row < n_rows and node_col < n_cols:
if node not in closed_set and node not in open_set:
open_set.put(node, cost)
pathTrace[node] = currentNode
print('Path Trace loop 1', pathTrace[node])
elif childNode in open_set and open_set.get(
(node_row, node_col)) > cost:
open_set.put(node, cost)
pathTrace[node] = currentNode
print('Path Trace loop 2', pathTrace[node_row, node_col])
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue()
closed_set = PriorityQueue()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
temp_goal = list(goal)
goal_r = temp_goal[0]
goal_c = temp_goal[1]
return abs(goal_r - row) + abs(goal_c - col)
open_set.put(start, 0)
while not len(open_set) == 0:
node = open_set.pop()
if node[0] == goal:
movement = construct_movement(start_row, start_col, goal_row,
goal_col, actions, parent)
return movement, closed_set
temp = node[0]
tup = list(temp)
closed_set.put(node[0], costFn(node[0]) + heuristic(tup[0], tup[1]))
temp = node[0]
tup = list(temp)
tup[0] += 1
if n_rows > tup[0] >= 0:
temp = tuple(tup)
child = [temp, node[1]]
if child[0] not in open_set and child[
0] not in closed_set and child[0] not in obstacles:
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
actions[tup[0]][tup[1]] = (1, 0)
parent[tup[0]][tup[1]] = node[0]
elif child[0] in open_set:
if open_set.get(child[0]) > costFn(child[0]):
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
temp = node[0]
tup = list(temp)
tup[0] -= 1
if n_rows > tup[0] >= 0:
temp = tuple(tup)
child = [temp, node[1]]
if child[0] not in open_set and child[
0] not in closed_set and child[0] not in obstacles:
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
actions[tup[0]][tup[1]] = (-1, 0)
parent[tup[0]][tup[1]] = node[0]
elif child[0] in open_set:
if open_set.get(child[0]) > costFn(child[0]):
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
temp = node[0]
tup = list(temp)
tup[1] += 1
if n_cols > tup[1] >= 0:
temp = tuple(tup)
child = [temp, node[1]]
if child[0] not in open_set and child[
0] not in closed_set and child[0] not in obstacles:
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
actions[tup[0]][tup[1]] = (0, 1)
parent[tup[0]][tup[1]] = node[0]
elif child[0] in open_set:
if open_set.get(child[0]) > costFn(child[0]):
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
temp = node[0]
tup = list(temp)
tup[1] -= 1
if n_cols > tup[1] >= 0:
temp = tuple(tup)
child = [temp, node[1]]
if child[0] not in open_set and child[
0] not in closed_set and child[0] not in obstacles:
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
actions[tup[0]][tup[1]] = (0, -1)
parent[tup[0]][tup[1]] = node[0]
elif child[0] in open_set:
if open_set.get(child[0]) > costFn(child[0]):
open_set.put(child[0],
costFn(child[0]) + heuristic(tup[0], tup[1]))
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min", f=lambda v: abs(v))
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
#############################################################################
return abs(row - goal[0]) + abs(col - goal[1])
open_set.put(start, heuristic(start[0], start[1]))
flag = 0
dict = {}
dict[start] = ([None], 0)
while len(open_set) != 0:
temp, value = open_set.pop()
up = ((temp[0] - 1), temp[1])
down = ((temp[0] + 1), temp[1])
left = (temp[0], (temp[1] - 1))
right = (temp[0], (temp[1] + 1))
children = [left, up, right, down]
closed_set.add(temp)
for child in children:
if child not in obstacles and child[0] >= 0 and child[
0] < n_rows and child[1] >= 0 and child[1] < n_cols:
if child not in closed_set:
if child == goal:
closed_set.add(child)
dict[child] = (temp, value)
flag = 1
break
else:
value = dict[temp][1] + costFn(child)
if (child not in open_set):
open_set.put(child,
value + heuristic(child[0], child[1]))
dict[child] = (temp, value)
elif (value < dict[child][1]):
open_set.remove(child)
open_set.put(child,
value + heuristic(child[0], child[1]))
dict[child] = (temp, value)
if flag == 1:
break
if flag == 0:
movement = []
else:
node = goal
while node != start:
cur = node
node = (dict[node][0])
action = (cur[0] - node[0], cur[1] - node[1])
if (action in ACTIONS):
movement.insert(0, action)
#############################################################################
return movement, closed_set
def astar_search(grid_size, start, goal, obstacles, costFn, logger):
"""
A* search algorithm finds the optimal path from the start cell to the
goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)).
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue(order="min")
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
parent = [ # the immediate predecessor cell from which to reach a cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
actions = [ # the action that the parent took to reach the cell
[None for __ in range(n_cols)] for _ in range(n_rows)
]
movement = []
# ----------------------------------------
# finish the code below to implement a Manhattan distance heuristic
# ----------------------------------------
def heuristic(row, col):
value = abs(row - goal_row) + abs(
col - goal_col) #calculating manhattan distance from goal
return value
#############################################################################
flag = 0 #counter variable to check whether goal reached or not
g = 0 #initialising g value of start node
h = heuristic(start_row, start_col) #calculating h value of start node
open_set.put(start, g + h)
while (open_set and flag == 0):
node, f_parent = open_set.pop()
if node == goal:
flag = 1 #goal reached
else:
closed_set.add(node)
x = list(
node
) #converting the parent node to list for ease in performing operations
h_parent = heuristic(x[0], x[1])
g_parent = f_parent - h_parent
for action in ACTIONS:
r1, c1 = action #converting the action to list for ease in performing operations
z1 = x[0] + r1 #row coordinate of child node
z2 = x[1] + c1 #column coordinate of child node
child = tuple((z1, z2))
h_child = heuristic(z1, z2) #calulating h value of child
g_child = g_parent + costFn(
child) #calculating g value of child
f_child = g_child + h_child
if child not in open_set and child not in closed_set and child not in obstacles and 0 <= z1 < n_rows and 0 <= z2 < n_cols: #checking if child not in obstacles and lies in grid
parent[z1][z2] = node #storing parent of child
actions[z1][
z2] = action #storing action taken by parent to reach child
open_set.put(child, f_child)
elif child in open_set:
f_child_prev = open_set.get(child)
if f_child_prev > f_child: #updating child with lower g value to open list
parent[z1][z2] = node #updating new parent
actions[z1][
z2] = action #updating action taken by new parent to reach child
open_set.put(child, f_child)
#generating path once goal has been reached
if flag == 1:
y = parent[goal_row][goal_col]
movement.append(actions[goal_row][goal_col])
while (y != start):
p = list(y)
movement.append(actions[p[0]][p[1]])
y = parent[p[0]][p[1]]
movement.reverse()
#############################################################################
return movement, closed_set
def uniform_cost_search(grid_size, start, goal, obstacles, costFn, logger):
"""
Uniform-cost search algorithm finds the optimal path from
the start cell to the goal cell in the 2D grid world.
After expanding a node, to retrieve the cost of a child node at location (x,y),
please call costFn((x,y)). In all of the grid maps, the cost is always 1.
See depth_first_search() for details.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
##########################################
# Choose a proper container yourself from
# OrderedSet, Stack, Queue, PriorityQueue
# for the open set and closed set.
open_set = PriorityQueue()
closed_set = OrderedSet()
##########################################
##########################################
# Set up visualization logger hook
# Please do not modify these four lines
closed_set.logger = logger
logger.closed_set = closed_set
open_set.logger = logger
logger.open_set = open_set
##########################################
movement = []
# ----------------------------------------
# finish the code below
# ----------------------------------------
# dictionary to store neighbor and action
temp_movement={}
movement_rev=[]
#put start node into the open_set
open_set.put((start_row, start_col),0)
#looping until open_set is empty
while(len(open_set))!=0:
node=open_set.pop()
noderc , node_val = node
current_row,current_col=noderc
if noderc==goal:
temp=goal
# code to traceback path form the goal to start
while temp!=start:
temp_row,temp_col=temp
temp1_row,temp1_col=temp_movement[temp]
temp_row=temp_row-(temp1_row)
temp_col=temp_col-(temp1_col)
movement_rev.append(temp_movement[temp])
temp=temp_row,temp_col
movement=Reverse(movement_rev)
return movement,closed_set
closed_set.add(noderc)
#call neighbor function to get child/neighbor and actions of current node
list_neg,list_mom=negh(current_row,current_col,n_rows,n_cols,obstacles)
for x in range(len(list_neg)):
child_row, child_col=list_neg[x]
if list_neg[x] not in open_set and list_neg[x] not in closed_set:
open_set.put(list_neg[x],costFn(list_neg[x]))
temp_movement.update({list_neg[x]:list_mom[list_neg[x]]})
elif list_neg[x] in open_set and costFn((child_row,child_col))>node_val:
noderc=list_neg[x]
#############################################################################
#############################################################################
return movement, closed_set