Assignment 6: Path Planning for a Robot

Objective: Use A* Search to find an optimal path for a robot navigating a 2D grid.

Problem Statement: A robot must move from a start point to a goal in a grid while
avoiding obstacles.

Tasks:
Implement A* with:
 The Manhattan distance heuristic applies to grids without any diagonal movement.
 The Euclidean distance heuristic is applicable to grids that allow diagonal
movement.
 Use a plotting library to visualize the found path.
 Compare A* with BFS and Uniform Cost Search.

CODE:
import numpy as np
import heapq
import matplotlib.pyplot as plt
from collections import deque
import time

class Node:
def __init__(self, x, y, cost=0, heuristic=0):
self.x = x
self.y = y
self.cost = cost
self.heuristic = heuristic
self.total_cost = cost + heuristic
self.parent = None
def __lt__(self, other):
return self.total_cost < other.total_cost
def manhattan_distance(node, goal):
return abs(node.x - goal.x) + abs(node.y - goal.y)
def euclidean_distance(node, goal):
return ((node.x - goal.x)**2 + (node.y - goal.y)**2)**0.5
def a_star(grid, start, goal, heuristic_fn, diagonal=False):
open_list = []
heapq.heappush(open_list, (0, start))
closed_set = set()
moves = [(0, 1), (1, 0), (0, -1), (-1, 0)]
if diagonal:
moves += [(1, 1), (-1, -1), (1, -1), (-1, 1)]
while open_list:
_, current = heapq.heappop(open_list)
if (current.x, current.y) == (goal.x, goal.y):
path = []
while current:
path.append((current.x, current.y))
current = current.parent
return path[::-1]
closed_set.add((current.x, current.y))
for dx, dy in moves:
nx, ny = current.x + dx, current.y + dy
 if 0 <= nx < grid.shape[0] and 0 <= ny < grid.shape[1] and grid[nx][ny] == 0:
if (nx, ny) in closed_set:
continue
neighbor = Node(nx, ny, cost=current.cost + (1 if abs(dx) + abs(dy) == 1 else
1.414))
neighbor.heuristic = heuristic_fn(neighbor, goal)
neighbor.total_cost = neighbor.cost + neighbor.heuristic
neighbor.parent = current
heapq.heappush(open_list, (neighbor.total_cost, neighbor))
return None
def bfs(grid, start, goal):
queue = deque([start])
visited = set()
visited.add((start.x, start.y))
moves = [(0, 1), (1, 0), (0, -1), (-1, 0)]
while queue:
current = queue.popleft()
if (current.x, current.y) == (goal.x, goal.y):
path = []
while current:
path.append((current.x, current.y))
current = current.parent
return path[::-1]
for dx, dy in moves:
nx, ny = current.x + dx, current.y + dy
if 0 <= nx < grid.shape[0] and 0 <= ny < grid.shape[1] and grid[nx][ny] == 0:
if (nx, ny) not in visited:
neighbor = Node(nx, ny)
neighbor.parent = current
queue.append(neighbor)
visited.add((nx, ny))
return None
def uniform_cost_search(grid, start, goal):
open_list = []
heapq.heappush(open_list, (0, start))
closed_set = set()
while open_list:
cost, current = heapq.heappop(open_list)
if (current.x, current.y) == (goal.x, goal.y):
path = []
while current:
path.append((current.x, current.y))
current = current.parent
return path[::-1]
closed_set.add((current.x, current.y))
for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]:
nx, ny = current.x + dx, current.y + dy
if 0 <= nx < grid.shape[0] and 0 <= ny < grid.shape[1] and grid[nx][ny] == 0:
if (nx, ny) in closed_set:
continue
neighbor = Node(nx, ny, cost=current.cost + 1)
neighbor.parent = current
heapq.heappush(open_list, (neighbor.cost, neighbor))
return None
def plot_path(grid, path, title):
plt.imshow(grid, cmap='gray_r')
for x, y in path:
plt.scatter(y, x, c='red')
plt.title(title)
plt.show()
# Example Usage
grid = np.zeros((10, 10))
grid[3:7, 5] = 1 # Example obstacle
start = Node(0, 0)
goal = Node(9, 9)
results = []
# A* Manhattan
start_time = time.time()
path_a_star_manhattan = a_star(grid, start, goal, manhattan_distance)
end_time = time.time()
if path_a_star_manhattan:
print("A* Manhattan Path:", path_a_star_manhattan)
print("Execution Time:", end_time - start_time)
plot_path(grid, path_a_star_manhattan, "A* with Manhattan Distance")
results.append(("A* Manhattan", len(path_a_star_manhattan), end_time - start_time))
# A* Euclidean
start = Node(0, 0)
start_time = time.time()
path_a_star_euclidean = a_star(grid, start, goal, euclidean_distance, diagonal=True)
end_time = time.time()
if path_a_star_euclidean:
print("A* Euclidean Path:", path_a_star_euclidean)
print("Execution Time:", end_time - start_time)
plot_path(grid, path_a_star_euclidean, "A* with Euclidean Distance")
results.append(("A* Euclidean", len(path_a_star_euclidean), end_time - start_time))
# BFS
start = Node(0, 0)
start_time = time.time()
path_bfs = bfs(grid, start, goal)
end_time = time.time()
if path_bfs:
print("BFS Path:", path_bfs)
print("Execution Time:", end_time - start_time)
plot_path(grid, path_bfs, "Breadth-First Search")
results.append(("BFS", len(path_bfs), end_time - start_time))
# Uniform Cost Search
start = Node(0, 0)
start_time = time.time()
path_ucs = uniform_cost_search(grid, start, goal)
end_time = time.time()
if path_ucs:
print("Uniform Cost Search Path:", path_ucs)
print("Execution Time:", end_time - start_time)
plot_path(grid, path_ucs, "Uniform Cost Search")
results.append(("Uniform Cost Search", len(path_ucs), end_time - start_time))
# Compare Results
results.sort(key=lambda x: (x[2], x[1])) # Sort by execution time, then path length
print("\nComparison of Algorithms:")
for result in results:
print(f"{result[0]} - Path Length: {result[1]}, Execution Time: {result[2]:.6f} seconds")
best_method = results[0]
print(f"\nBest Algorithm: {best_method[0]} with Path Length: {best_method[1]} and Execution
Time: {best_method[2]:.6f} seconds")
OUTPUT: