[Python] Implementation of A* Search Algorithm

I have been trying to implement the A* search algorithm to no avail. The problem boils down to my algorithm not using the Manhattan heuristic nor the euclidean heuristic. I end up getting the same cost either way. It's as if its a uniform search.

Here is my implementation:

def aStarSearch(problem: SearchProblem, heuristic=nullHeuristic):
"""Search the node that has the lowest combined cost and heuristic first."""
"*** YOUR CODE HERE ***"
new_nodes = [] #stores nodes that need to be explored as a tuple
visited_states = set() #stores states that have been explored.
cost_tracker = {} #stores the lowest cost path to the goal.

#get the starting state.
start_position = problem.getStartState()

#this is where we are starting, hence the cost to reach this node is zero.
initial_cost = 0
cost_tracker[start_position] = initial_cost

#makes a call to one of the heuristic functions.
heuristic_estimate = heuristic(start_position, problem)

#f(n) = g(n) + h(n)
estimated_total_cost = initial_cost + heuristic_estimate
new_nodes.append((estimated_total_cost, initial_cost, start_position, []))


#runs as long as there are nodes to explore.
while new_nodes:
#sorts the list by f(n)
new_nodes.sort(key=lambda item: (item[0], heuristic(item[2], problem)))
#sort by (f(n), g(n))

#removes the node with the lowest cost first.
estimated_total_cost, path_cost, current_position, path_traveled = new_nodes[0]
#print(f"Expanding: {current_position}, g(n)={path_cost}, h(n)={heuristic(current_position, problem)}, f(n)={estimated_total_cost}") debugging
new_nodes = new_nodes[1:]

#if this state has already been explored, skip it.
if current_position in visited_states:
continue
visited_states.add(current_position)

#return the path if the current state is the goal state.
if problem.isGoalState(current_position):
return path_traveled

for next_state, move, step_cost in problem.getSuccessors(current_position):
new_cost = path_cost + step_cost

#if the successor has not been visited or if there is a lower cost, then update the path cost.
if next_state not in cost_tracker or new_cost < cost_tracker[next_state]:
cost_tracker[next_state] = new_cost

#f(n) = g(n) + h(n)
estimated_remaining_cost = heuristic(next_state, problem)
new_estimated_total_cost = new_cost + estimated_remaining_cost
new_nodes.append((new_estimated_total_cost, new_cost, next_state, path_traveled + [move]))

#no solution was found.
return []


def manhattanDistance(position, problem):
"""Manhattan Distance"""
goal_col, goal_row = problem.goal
pos_col, pos_row = position
return abs(pos_col - goal_col) + abs(pos_row - goal_row)

def euclideanDistance(position, problem):
"""Euclidean Distance"""
goal_col, goal_row = problem.goal
pos_col, pos_row = position
return ((pos_col - goal_col) ** 2 + (pos_row - goal_row) ** 2) ** 0.5


I tried to add debug print statements to see the behaviour of the algorithm:

Expanding: (33, 12), g(n)=11, h(n)=43, f(n)=54
Expanding: (4, 7), g(n)=45, h(n)=9, f(n)=54
Expanding: (24, 12), g(n)=20, h(n)=34, f(n)=54
Expanding: (23, 12), g(n)=21, h(n)=33, f(n)=54
Expanding: (22, 12), g(n)=22, h(n)=32, f(n)=54
Expanding: (21, 12), g(n)=23, h(n)=31, f(n)=54
Expanding: (20, 12), g(n)=24, h(n)=30, f(n)=54
Expanding: (20, 11), g(n)=25, h(n)=29, f(n)=54
Expanding: (19, 12), g(n)=25, h(n)=29, f(n)=54
Expanding: (20, 10), g(n)=26, h(n)=28, f(n)=54
Expanding: (19, 11), g(n)=26, h(n)=28, f(n)=54
Expanding: (18, 12), g(n)=26, h(n)=28, f(n)=54
Expanding: (20, 9), g(n)=27, h(n)=27, f(n)=54
Expanding: (17, 12), g(n)=27, h(n)=27, f(n)=54
Expanding: (16, 12), g(n)=28, h(n)=26, f(n)=54
Expanding: (15, 12), g(n)=29, h(n)=25, f(n)=54
Expanding: (14, 12), g(n)=30, h(n)=24, f(n)=54
Expanding: (14, 11), g(n)=31, h(n)=23, f(n)=54
Expanding: (13, 12), g(n)=31, h(n)=23, f(n)=54
Expanding: (14, 10), g(n)=32, h(n)=22, f(n)=54
Expanding: (12, 12), g(n)=32, h(n)=22, f(n)=54
Expanding: (12, 11), g(n)=33, h(n)=21, f(n)=54
Expanding: (12, 10), g(n)=34, h(n)=20, f(n)=54
Expanding: (11, 10), g(n)=35, h(n)=19, f(n)=54
Expanding: (10, 10), g(n)=36, h(n)=18, f(n)=54
Expanding: (34, 12), g(n)=12, h(n)=44, f(n)=56
Expanding: (4, 8), g(n)=46, h(n)=10, f(n)=56
Expanding: (21, 9), g(n)=28, h(n)=28, f(n)=56
Expanding: (17, 13), g(n)=28, h(n)=28, f(n)=56
Expanding: (15, 10), g(n)=33, h(n)=23, f(n)=56
Expanding: (12, 13), g(n)=33, h(n)=23, f(n)=56
Expanding: (10, 11), g(n)=37, h(n)=19, f(n)=56
Expanding: (34, 11), g(n)=13, h(n)=43, f(n)=56
Expanding: (34, 10), g(n)=14, h(n)=42, f(n)=56
Expanding: (33, 10), g(n)=15, h(n)=41, f(n)=56
Expanding: (32, 10), g(n)=16, h(n)=40, f(n)=56
Expanding: (31, 10), g(n)=17, h(n)=39, f(n)=56
Expanding: (30, 10), g(n)=18, h(n)=38, f(n)=56
Expanding: (30, 9), g(n)=19, h(n)=37, f(n)=56
Expanding: (29, 10), g(n)=19, h(n)=37, f(n)=56
Expanding: (30, 8), g(n)=20, h(n)=36, f(n)=56
Expanding: (4, 9), g(n)=47, h(n)=11, f(n)=58
Expanding: (22, 9), g(n)=29, h(n)=29, f(n)=58
Expanding: (17, 14), g(n)=29, h(n)=29, f(n)=58
Expanding: (16, 10), g(n)=34, h(n)=24, f(n)=58
Expanding: (12, 14), g(n)=34, h(n)=24, f(n)=58
Expanding: (10, 12), g(n)=38, h(n)=20, f(n)=58
Expanding: (31, 8), g(n)=21, h(n)=37, f(n)=58
Expanding: (16, 14), g(n)=30, h(n)=28, f(n)=58
Expanding: (15, 14), g(n)=31, h(n)=27, f(n)=58
Expanding: (14, 14), g(n)=32, h(n)=26, f(n)=58
Expanding: (13, 14), g(n)=33, h(n)=25, f(n)=58
Expanding: (4, 10), g(n)=48, h(n)=12, f(n)=60
Expanding: (23, 9), g(n)=30, h(n)=30, f(n)=60
Expanding: (17, 10), g(n)=35, h(n)=25, f(n)=60
Expanding: (13, 14), g(n)=35, h(n)=25, f(n)=60
Expanding: (10, 13), g(n)=39, h(n)=21, f(n)=60
Expanding: (32, 8), g(n)=22, h(n)=38, f(n)=60
Expanding: (3, 10), g(n)=49, h(n)=11, f(n)=60
Expanding: (17, 9), g(n)=36, h(n)=24, f(n)=60
Expanding: (17, 8), g(n)=37, h(n)=23, f(n)=60
Expanding: (16, 8), g(n)=38, h(n)=22, f(n)=60
Expanding: (15, 8), g(n)=39, h(n)=21, f(n)=60
Expanding: (14, 8), g(n)=40, h(n)=20, f(n)=60
Expanding: (13, 8), g(n)=41, h(n)=19, f(n)=60
Expanding: (12, 8), g(n)=42, h(n)=18, f(n)=60
Expanding: (11, 8), g(n)=43, h(n)=17, f(n)=60
Expanding: (11, 7), g(n)=44, h(n)=16, f(n)=60
Expanding: (11, 6), g(n)=45, h(n)=15, f(n)=60
Expanding: (10, 6), g(n)=46, h(n)=14, f(n)=60
Expanding: (9, 6), g(n)=47, h(n)=13, f(n)=60
Expanding: (8, 6), g(n)=48, h(n)=12, f(n)=60
Expanding: (7, 6), g(n)=49, h(n)=11, f(n)=60
Expanding: (6, 6), g(n)=50, h(n)=10, f(n)=60
Expanding: (6, 5), g(n)=51, h(n)=9, f(n)=60
Expanding: (5, 5), g(n)=52, h(n)=8, f(n)=60
Expanding: (4, 5), g(n)=53, h(n)=7, f(n)=60
Expanding: (3, 5), g(n)=54, h(n)=6, f(n)=60
Expanding: (2, 5), g(n)=55, h(n)=5, f(n)=60
Expanding: (1, 5), g(n)=56, h(n)=4, f(n)=60
Expanding: (1, 4), g(n)=57, h(n)=3, f(n)=60
Expanding: (1, 3), g(n)=58, h(n)=2, f(n)=60
Expanding: (4, 11), g(n)=49, h(n)=13, f(n)=62
Expanding: (24, 9), g(n)=31, h(n)=31, f(n)=62
Expanding: (10, 14), g(n)=40, h(n)=22, f(n)=62
Expanding: (33, 8), g(n)=23, h(n)=39, f(n)=62
Expanding: (2, 3), g(n)=59, h(n)=3, f(n)=62
Expanding: (9, 14), g(n)=41, h(n)=21, f(n)=62
Expanding: (8, 14), g(n)=42, h(n)=20, f(n)=62
Expanding: (8, 13), g(n)=43, h(n)=19, f(n)=62
Expanding: (8, 12), g(n)=44, h(n)=18, f(n)=62
Expanding: (8, 11), g(n)=45, h(n)=17, f(n)=62
Expanding: (8, 10), g(n)=46, h(n)=16, f(n)=62
Expanding: (8, 9), g(n)=47, h(n)=15, f(n)=62
Expanding: (8, 8), g(n)=48, h(n)=14, f(n)=62
Expanding: (7, 8), g(n)=49, h(n)=13, f(n)=62
Expanding: (6, 8), g(n)=50, h(n)=12, f(n)=62
Expanding: (4, 12), g(n)=50, h(n)=14, f(n)=64
Expanding: (25, 9), g(n)=32, h(n)=32, f(n)=64
Expanding: (34, 8), g(n)=24, h(n)=40, f(n)=64
Expanding: (3, 3), g(n)=60, h(n)=4, f(n)=64
Expanding: (6, 9), g(n)=51, h(n)=13, f(n)=64
Expanding: (34, 7), g(n)=25, h(n)=39, f(n)=64
Expanding: (34, 6), g(n)=26, h(n)=38, f(n)=64
Expanding: (33, 6), g(n)=27, h(n)=37, f(n)=64
Expanding: (32, 6), g(n)=28, h(n)=36, f(n)=64
Expanding: (31, 6), g(n)=29, h(n)=35, f(n)=64
Expanding: (30, 6), g(n)=30, h(n)=34, f(n)=64
Expanding: (30, 5), g(n)=31, h(n)=33, f(n)=64
Expanding: (29, 6), g(n)=31, h(n)=33, f(n)=64
Expanding: (30, 4), g(n)=32, h(n)=32, f(n)=64
Expanding: (30, 3), g(n)=33, h(n)=31, f(n)=64
Expanding: (4, 13), g(n)=51, h(n)=15, f(n)=66
Expanding: (26, 9), g(n)=33, h(n)=33, f(n)=66
Expanding: (4, 3), g(n)=61, h(n)=5, f(n)=66
Expanding: (6, 10), g(n)=52, h(n)=14, f(n)=66
Expanding: (31, 4), g(n)=33, h(n)=33, f(n)=66
Expanding: (31, 3), g(n)=34, h(n)=32, f(n)=66
Expanding: (4, 14), g(n)=52, h(n)=16, f(n)=68
Expanding: (27, 9), g(n)=34, h(n)=34, f(n)=68
Expanding: (5, 3), g(n)=62, h(n)=6, f(n)=68
Expanding: (6, 11), g(n)=53, h(n)=15, f(n)=68
Expanding: (32, 4), g(n)=34, h(n)=34, f(n)=68
Expanding: (27, 8), g(n)=35, h(n)=33, f(n)=68
Expanding: (27, 7), g(n)=36, h(n)=32, f(n)=68
Expanding: (27, 6), g(n)=37, h(n)=31, f(n)=68
Expanding: (27, 5), g(n)=38, h(n)=30, f(n)=68
Expanding: (27, 4), g(n)=39, h(n)=29, f(n)=68
Expanding: (27, 3), g(n)=40, h(n)=28, f(n)=68
Expanding: (27, 2), g(n)=41, h(n)=27, f(n)=68
Expanding: (27, 1), g(n)=42, h(n)=26, f(n)=68
Expanding: (26, 2), g(n)=42, h(n)=26, f(n)=68
Expanding: (25, 2), g(n)=43, h(n)=25, f(n)=68
Expanding: (24, 2), g(n)=44, h(n)=24, f(n)=68
Expanding: (23, 2), g(n)=45, h(n)=23, f(n)=68
Expanding: (22, 2), g(n)=46, h(n)=22, f(n)=68
Expanding: (21, 2), g(n)=47, h(n)=21, f(n)=68
Expanding: (20, 2), g(n)=48, h(n)=20, f(n)=68
Expanding: (19, 2), g(n)=49, h(n)=19, f(n)=68
Expanding: (18, 2), g(n)=50, h(n)=18, f(n)=68
Expanding: (17, 2), g(n)=51, h(n)=17, f(n)=68
Expanding: (16, 2), g(n)=52, h(n)=16, f(n)=68
Expanding: (15, 2), g(n)=53, h(n)=15, f(n)=68
Expanding: (14, 2), g(n)=54, h(n)=14, f(n)=68
Expanding: (13, 2), g(n)=55, h(n)=13, f(n)=68
Expanding: (12, 2), g(n)=56, h(n)=12, f(n)=68
Expanding: (11, 2), g(n)=57, h(n)=11, f(n)=68
Expanding: (10, 2), g(n)=58, h(n)=10, f(n)=68
Expanding: (10, 1), g(n)=59, h(n)=9, f(n)=68
Expanding: (9, 1), g(n)=60, h(n)=8, f(n)=68
Expanding: (8, 1), g(n)=61, h(n)=7, f(n)=68
Expanding: (7, 1), g(n)=62, h(n)=6, f(n)=68
Expanding: (6, 1), g(n)=63, h(n)=5, f(n)=68
Expanding: (5, 1), g(n)=64, h(n)=4, f(n)=68
Expanding: (4, 1), g(n)=65, h(n)=3, f(n)=68
Expanding: (3, 1), g(n)=66, h(n)=2, f(n)=68
Expanding: (2, 1), g(n)=67, h(n)=1, f(n)=68
Expanding: (1, 1), g(n)=68, h(n)=0, f(n)=68


I end up getting the same value for f(n) every time, and I don't know why.

Continue reading...