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: 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
# ----------------------------------------
#############################################################################
'''
Based on the pseudo code from the slides
'''
open_set.put(start, 0)
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
closed_set.add(currentNode)
currentNode_row, currentNode_col = currentNode
# get the children
children = [((currentNode_row - 1, currentNode_col), v + costFn(
(currentNode_row - 1, currentNode_col))),
((currentNode_row, currentNode_col - 1), v + costFn(
(currentNode_row, currentNode_col - 1))),
((currentNode_row + 1, currentNode_col), v + costFn(
(currentNode_row + 1, currentNode_col))),
((currentNode_row, currentNode_col + 1), v + costFn(
(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.remove(child)
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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = Stack()
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
# ----------------------------------------
#############################################################################
open_set.add(start)
while open_set:
node = open_set.pop()
closed_set.add(node)
for action in ACTIONS:
child = (node[0] + action[0], node[1] + action[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:
if child == goal:
movement.append(action)
while node is not start:
movement.insert(0, actions[node[0]][node[1]])
node = parent[node[0]][node[1]]
return movement, closed_set
else:
open_set.add(child)
parent[(child)[0]][(child)[1]] = node
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[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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = Stack()
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
# ----------------------------------------
#############################################################################
'''
Based on the pseudo code from the slides
'''
open_set.add(start)
while open_set:
children = []
currentNode = open_set.pop()
closed_set.add(currentNode)
currentNode_row, currentNode_col = currentNode
# get the children
children = [(currentNode_row - 1, currentNode_col),
(currentNode_row, currentNode_col - 1),
(currentNode_row + 1, currentNode_col),
(currentNode_row, currentNode_col + 1)]
# expand the children
for child in children:
child_row, child_col = child
if child not in obstacles and child_row >= 0 and child_col >= 0 and child_row < n_rows and child_col < n_cols:
if child not in closed_set and child not in open_set:
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 child == goal: # child reaching the goal?
parent[child_row][child_col] = currentNode
p_row, p_col = child
while (p_row, p_col) != start:
#print(p_row)
movement.append(actions[p_row][p_col])
p_row, p_col = parent[p_row][p_col]
movement.reverse()
closed_set.add(
child
) # for the same outcome with the demo video provided
return movement, closed_set
else:
open_set.add(child)
parent[child_row][child_col] = currentNode
#############################################################################
return movement, closed_set
def depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = Stack()
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
# ----------------------------------------
#############################################################################
flag = 0 #counter variable to check whether goal reached or not
open_set.add(start)
while (open_set and flag == 0):
node = open_set.pop()
closed_set.add(node)
x = list(
node
) #converting the parent node to list for ease in performing operations
for action in ACTIONS:
r1, c1 = action
z1 = x[0] + r1 #row coordinate of child node
z2 = x[1] + c1 #column coordinate of child node
child = tuple((z1, z2))
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
if child == goal:
flag = 1 #goal reached
else:
open_set.add(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
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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = Stack() #For DFS we required LIFO, so Stack set is selected
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
# ----------------------------------------
open_set.add(start) #Adding start into open set
path = [] #For finding the path
while open_set:
node = open_set.pop()
if node == goal: #If goal is achieved
print("PATH FOUND")
j = goal
while j != start: #For backtracking
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: #Finding children of node
child = (node[0] + i[0], node[1] + i[1])
if child not in open_set and child not in closed_set: #Checking the children in open set and closed set, if not then find children
if child not in obstacles and 0 <= child[
0] < n_rows and 0 <= child[
1] < n_cols: #Entering boundary condition and obstacle check for child
open_set.add(child)
parent[child[0]][child[1]] = (
node[0], node[1]
) #Placing the parent of respective child
path.reverse() #Because of backtracing we have to reverse the path
path.append(goal)
for i in range(
len(path) -
1): #Finding the movement, substracting the parent from child
move = (path[i + 1][0] - path[i][0], path[i + 1][1] - path[i][1])
movement.append(move)
#############################################################################
#############################################################################
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
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 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 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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Return
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
n_rows, n_cols = grid_size
start_row, start_col = start
goal_row, goal_col = goal
open_set = Stack() # LIFO data structure
closed_set = OrderedSet() # orderset 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)
]
open_set.add(start) # initial state
while len(open_set) > 0:
node = open_set.pop() # next expanded node
closed_set.add(node) # add expaned node to close_set
x, y = node # node location
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:
if child in boundary and child not in obstacles:
if child not in open_set and child not in closed_set:
parents[child[0]][
child[1]] = node # remark parent for current children
actions[child[0]][child[1]] = (child[0] - node[0],
child[1] - node[1]
) # remark children action
if child == goal:
movement = path_backtrack(
start, child, parents,
actions) # backtrack to find the path
return movement, closed_set
else:
open_set.add(child)
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 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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = Stack()
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)
]
# ACTIONS = (
# (-1, 0), # go up
#( 0, -1), # go left
# ( 1, 0), # go down
#( 0, 1) # go right
#)
movement = []
# ----------------------------------------
# finish the code below
# ----------------------------------------
#############################################################################
open_set.add(start)
while open_set:
current = open_set.pop()
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):
open_set.add(up)
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):
open_set.add(left)
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):
open_set.add(down)
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):
open_set.add(right)
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 = []
# ----------------------------------------
# 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)).
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
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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the 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)).
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object composed of actions
that should move the agent from the start to goal cell along the path
as found by the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = OrderedSet()
open_set = Stack()
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
# open_set = Stack()
open_set.add(start)
flag = 0
li = []
while len(open_set) != 0:
temp = open_set.pop()
# for i in range(0,4):
up = ((temp[0] - 1), temp[1])
down = ((temp[0] + 1), temp[1])
left = (temp[0], (temp[1] - 1))
right = (temp[0], (temp[1] + 1))
upCost = costFn(up)
downCost = costFn(down)
leftCost = costFn(left)
rightCost = costFn(right)
costs1 = [upCost, downCost, leftCost, rightCost]
costs = {up: upCost, down: downCost, left: leftCost, right: rightCost}
children = []
closed_set.add(temp)
li.append(temp)
if len(set(costs1)) <= 1:
children = [left, up, right, down]
else:
while costs:
keyMax = max(costs, key=costs.get)
if keyMax == up:
children.append(up)
elif keyMax == down:
children.append(down)
elif keyMax == left:
children.append(left)
elif keyMax == right:
children.append(right)
costs.pop(keyMax)
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 open_set and child not in closed_set:
if child == goal:
li.append(child)
closed_set.add(child)
flag = 1
break
else:
open_set.add(child)
if flag == 1:
break
if flag == 0:
movement = []
else:
last = len(li) - 1
mov = li[last]
for i in range(last, -1, -1):
action = (mov[0] - li[i - 1][0], mov[1] - li[i - 1][1])
if (action in ACTIONS):
movement.insert(0, action)
mov = li[i - 1]
# ----------------------------------------
#############################################################################
#############################################################################
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 depth_first_search(grid_size, start, goal, obstacles, costFn, logger):
"""
DFS algorithm finds the path from the start cell to the
goal cell in the 2D grid world.
Parameters
----------
grid_size: tuple, (n_rows, n_cols)
(number of rows of the grid, number of cols of the grid)
start: tuple, (row, col)
location of the start cell;
row and col are counted from 0, i.e. the 1st row is 0
goal: tuple, (row, col)
location of the goal cell
obstacles: tuple, ((row, col), (row, col), ...)
locations of obstacles in the grid
the cells where obstacles are located are not allowed to access
costFn: a function that returns the cost of landing to a cell (x,y)
after taking an action. The default cost is 1, regardless of the
action and the landing cell, i.e. every time the agent moves NSEW
it costs one unit.
logger: a logger to visualize the search process.
Do not do anything to it.
Returns
-------
movement along the path from the start to goal cell: list of actions
The first returned value is the movement list found by the search
algorithm from the start cell to the end cell.
The movement list should be a list object who is composed of actions
that should made moving from the start to goal cell along the path
found the algorithm.
For example, if nodes in the path from the start to end cell are:
(0, 0) (start) -> (0, 1) -> (1, 1) -> (1, 0) (goal)
then, the returned movement list should be
[(0,1), (1,0), (0, -1)]
which means: move right, down, left.
Return an EMPTY list if the search algorithm fails finding any
available path.
closed set: list of location tuple (row, col)
The second returned value is the closed set, namely, the cells are expanded during search.
"""
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 = Stack()
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.add((start_row, start_col))
#looping until the open_set is empty
while len(open_set)!=0:
node=open_set.pop()
closed_set.add(node)
current_row , current_col = node
#call neighbor funcation 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)):
if list_neg[x] not in closed_set and list_neg[x] not in open_set:
temp_movement.update({list_neg[x]:list_mom[list_neg[x]]})
if list_neg[x]==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
else:
open_set.add(list_neg[x])
#######################################################################
#############################################################################
return movement, closed_set
