Write a program to optimize robot path planning in a grid-based environment enabling
efficient navigation for AI-powered robotics.
import math
import heapq
# Define the Cell class
class Cell:
def __init__(self):
self.parent_i = 0 # Parent cell's row index
self.parent_j = 0 # Parent cell's column index
self.f = float('inf') # Total cost of the cell (g + h)
self.g = float('inf') # Cost from start to this cell
self.h = 0 # Heuristic cost from this cell to destination
# Define the size of the grid
ROW = 9
COL = 10
# Check if a cell is valid (within the grid)
def is_valid(row, col):
return (row >= 0) and (row < ROW) and (col >= 0) and (col < COL)
# Check if a cell is unblocked
def is_unblocked(grid, row, col):
return grid[row][col] == 1
# Check if a cell is the destination
def is_destination(row, col, dest):
return row == dest[0] and col == dest[1]
# Calculate the heuristic value of a cell (Euclidean distance to destination)
def calculate_h_value(row, col, dest):
return ((row - dest[0]) ** 2 + (col - dest[1]) ** 2) ** 0.5
# Trace the path from source to destination
def trace_path(cell_details, dest):
print("The Path is ")
path = []
row = dest[0]
col = dest[1]
# Trace the path from destination to source using parent cells
while not (cell_details[row][col].parent_i == row and cell_details[row][col].parent_j
== col):
path.append((row, col))
temp_row = cell_details[row][col].parent_i
temp_col = cell_details[row][col].parent_j
row = temp_row
col = temp_col
# Add the source cell to the path
path.append((row, col))
# Reverse the path to get the path from source to destination
path.reverse()
# Print the path
for i in path:
print("->", i, end=" ")
print()
# Implement the A* search algorithm
def a_star_search(grid, src, dest):
# Check if the source and destination are valid
if not is_valid(src[0], src[1]) or not is_valid(dest[0], dest[1]):
print("Source or destination is invalid")
return
# Check if the source and destination are unblocked
if not is_unblocked(grid, src[0], src[1]) or not is_unblocked(grid, dest[0], dest[1]):
print("Source or the destination is blocked")
return
# Check if we are already at the destination
if is_destination(src[0], src[1], dest):
print("We are already at the destination")
return
# Initialize the closed list (visited cells)
closed_list = [[False for _ in range(COL)] for _ in range(ROW)]
# Initialize the details of each cell
cell_details = [[Cell() for _ in range(COL)] for _ in range(ROW)]
# Initialize the start cell details
i = src[0]
j = src[1]
cell_details[i][j].f = 0
cell_details[i][j].g = 0
cell_details[i][j].h = 0
cell_details[i][j].parent_i = i
cell_details[i][j].parent_j = j
# Initialize the open list (cells to be visited) with the start cell
open_list = []
heapq.heappush(open_list, (0.0, i, j))
# Initialize the flag for whether destination is found
found_dest = False
# Main loop of A* search algorithm
while len(open_list) > 0:
# Pop the cell with the smallest f value from the open list
p = heapq.heappop(open_list)
# Mark the cell as visited
i = p[1]
j = p[2]
closed_list[i][j] = True
# For each direction, check the successors
directions = [(0, 1), (0, -1), (1, 0), (-1, 0), (1, 1), (1, -1), (-1, 1), (-1, -1)]
for dir in directions:
new_i = i + dir[0]
new_j = j + dir[1]
# If the successor is valid, unblocked, and not visited
if is_valid(new_i, new_j) and is_unblocked(grid, new_i, new_j) and not
closed_list[new_i][new_j]:
# If the successor is the destination
if is_destination(new_i, new_j, dest):
# Set the parent of the destination cell
cell_details[new_i][new_j].parent_i = i
cell_details[new_i][new_j].parent_j = j
print("The destination cell is found")
# Trace and print the path from source to destination
trace_path(cell_details, dest)
found_dest = True
return
else:
# Calculate the new f, g, and h values
g_new = cell_details[i][j].g + 1.0
h_new = calculate_h_value(new_i, new_j, dest)
f_new = g_new + h_new
# If the cell is not in the open list or the new f value is smaller
if cell_details[new_i][new_j].f == float('inf') or cell_details[new_i][new_j].f >
f_new:
# Add the cell to the open list
heapq.heappush(open_list, (f_new, new_i, new_j))
# Update the cell details
cell_details[new_i][new_j].f = f_new
cell_details[new_i][new_j].g = g_new
cell_details[new_i][new_j].h = h_new
cell_details[new_i][new_j].parent_i = i
cell_details[new_i][new_j].parent_j = j
# If the destination is not found after visiting all cells
if not found_dest:
print("Failed to find the destination cell")
def main():
# Define the grid (1 for unblocked, 0 for blocked)
grid = [
[1, 0, 1, 1, 1, 1, 0, 1, 1, 1],
[1, 1, 1, 0, 1, 1, 1, 0, 1, 1],
[1, 1, 1, 0, 1, 1, 0, 1, 0, 1],
[0, 0, 1, 0, 1, 0, 0, 0, 0, 1],
[1, 1, 1, 0, 1, 1, 1, 0, 1, 0],
[1, 0, 1, 1, 1, 1, 0, 1, 0, 0],
[1, 0, 0, 0, 0, 1, 0, 0, 0, 1],
[1, 0, 1, 1, 1, 1, 0, 1, 1, 1],
[1, 1, 1, 0, 0, 0, 1, 0, 0, 1]
]
# Define the source and destination
src = [8, 0]
dest = [0, 0]
# Run the A* search algorithm
a_star_search(grid, src, dest)
if __name__ == "__main__":
main()
out put: The destination cell is found
The Path is
-> (8, 0) -> (7, 0) -> (6, 0) -> (5, 0) -> (4, 1) -> (3, 2) -> (2, 1) -> (1, 0) -> (0, 0)
import heapq – for maintaining the open list as a min-heap priority queue (based
on f = g + h).
🔍 Key Concepts
📌 A* Algorithm Overview
A* is an informed search algorithm that uses:
g(n) = cost from the start node to current node n
h(n) = estimated cost from n to goal (heuristic; here it's Euclidean distance)
f(n) = g(n) + h(n) = total estimated cost of the cheapest solution through node n
📦 Step-by-Step Code Explanation
1. Cell Class – Used to store details of each cell
python
CopyEdit
class Cell:
def __init__(self):
self.parent_i = 0 # parent row
self.parent_j = 0 # parent column
self.f = float('inf') # total cost f = g + h
self.g = float('inf') # cost from source to this cell
self.h = 0 # heuristic estimate from this cell to destination
2. Grid Setup
python
CopyEdit
ROW = 9
COL = 10
A 9x10 grid is used. Each cell is either 1 (unblocked) or 0 (blocked).
3. Helper Functions
is_valid(row, col) – checks if cell is within grid
is_unblocked(grid, row, col) – checks if cell is traversable (1)
is_destination(row, col, dest) – checks if we reached goal
calculate_h_value(row, col, dest) – returns Euclidean distance to destination
trace_path(cell_details, dest) – prints the path by following parent_i and
parent_j
4. Main Algorithm: a_star_search(grid, src, dest)
✅ Initialization
Checks validity of source/destination.
Initializes:
o closed_list – keeps track of visited nodes
o cell_details – holds f, g, h and parent info
o open_list – priority queue storing (f, i, j)
🔄 Loop Through Open List
While open_list is not empty:
o Pop cell with smallest f from heap
o Mark it as visited in closed_list
o Check all 8 neighbors (up, down, diagonals, etc.)
↪ For Each Valid Neighbor:
If it's the destination, trace path and return.
Else:
o Compute new g, h, and f
o If new path is better, update cell's info and push to open_list
❌ If No Path Found:
Print failure message.
5. Main Function
python
CopyEdit
grid = [ ... ] # 9x10 grid with 1s and 0s
src = [8, 0] # start at bottom-left
dest = [0, 0] # end at top-left
a_star_search(grid, src, dest)
🧠 Heuristic Used:
python
CopyEdit
calculate_h_value = sqrt((row - dest[0])^2 + (col - dest[1])^2)
This is the Euclidean distance, suitable when diagonal movement is allowed (which is
the case here).
✅ Sample Output (if destination found):
rust
CopyEdit
The destination cell is found
The Path is
-> (8, 0) -> (7, 0) -> (6, 0) -> (5, 0) -> (4, 0) -> (3, 0) -> (2, 0) -> (1, 0) -> (0, 0)
📌 Summary of Key Components:
Component Purpose
Cell class Stores cost and parent info for path reconstruction
open_list Min-heap of frontier cells (priority by f)
closed_list Tracks visited cells
f=g+h Total cost heuristic used for decision making
heapq Efficient priority queue for lowest-cost selection
trace_path() Reconstructs path from destination to source using parents
Would you like a visual diagram or a step-by-step dry run of how the path is calculated
for the given grid?
You said:
