A* Robot

robot.py

"""
## To run it
> clear && python robot.py

This script compares the performance of A* and Dijkstra's algorithms on a 50x50 grid.
The robot must navigate from the start (0, 0) to a randomly placed goal.
Obstacles are randomly generated in the grid, and the paths are calculated and animated in the terminal.
Performance metrics, including the time taken for both algorithms, are displayed after each test case.

Demo: https://imgur.com/a/gHNdDOE
"""

import random
import time
import heapq


class Solution:
  """
  This class implements the A* algorithm for finding the shortest path from the start (0, 0) to the goal.
  """

  def robot_path(self, grid, goal):
    end = len(grid) - 1
    no_path = 0
    no_path_max = end * 2
    visited_bias = 0.4

    # Heuristic function for A* (Manhattan distance to the goal)
    def h(x, y):
      return abs(x - goal[0]) + abs(y - goal[1])

    # Get the neighboring cells of the current position
    def get_neighbors(x, y):
      neighbors = []
      directions = [(0, 1), (0, -1), (1, 0), (-1, 0)]
      random.shuffle(directions)
      for dx, dy in directions:
        nx, ny = x + dx, y + dy
        if 0 <= nx <= end and 0 <= ny <= end and grid[nx][ny] != 1:
          neighbors.append((nx, ny))
      return neighbors

    start = (0, 0)
    path = [start]
    visited = set()
    visited.add(start)
    visit_count = {}

    # Choose the neighbor with the minimum cost (including a bias for previously visited nodes)
    def min_cost(neighbors):
      min_cost = float('inf')
      min_neighbor = None
      for neighbor in neighbors:
        if neighbor not in visited:
          bias = visit_count.get(neighbor, 0) * visited_bias
          cost = h(neighbor[0], neighbor[1]) + bias
          if cost < min_cost:
            min_cost = cost
            min_neighbor = neighbor
      if min_neighbor:
        visited.add(min_neighbor)
      return min_neighbor

    x, y = start
    while h(x, y) != 0:
      neighbors = get_neighbors(x, y)
      best_neighbor = min_cost(neighbors)

      if best_neighbor is None:
        no_path += 1
        visited.clear()
        visited.add((x, y))
        if no_path > no_path_max:
          break
        continue

      visit_count[best_neighbor] = visit_count.get(best_neighbor, 0) + 1
      path.append(best_neighbor)
      x, y = best_neighbor

    return path


class Evaluation:
  """
  This class handles the setup, pathfinding comparison (A* vs Dijkstra), animation, and performance tracking.
  """

  def pause_for_next_test(self):
    input("Press [enter] to continue to the next test...")
    print("\n")

  def move_cursor_to_top(self):
    # ANSI escape code to move the cursor to the top-left corner
    print("\033[H", end="")

  def print_grid(self, grid, robot_position, visited, dijkstra_path, current_cost, goal_cost, goal, dijkstra_time, astar_time):
    # Display the current state of the grid with the robot, goal, paths, and obstacles
    grid_display = []
    for y in range(len(grid)):
      row = []
      for x in range(len(grid[0])):
        if (y, x) == robot_position:
          row.append("R")
        elif (y, x) == goal:
          row.append("⚑")
        elif (y, x) in visited:
          row.append("\033[30m·\033[0m")
        elif (y, x) in dijkstra_path:
          row.append("\033[32m⋅\033[0m")
        elif grid[y][x] == 1:
          row.append("◇")
        else:
          row.append(" ")
      grid_display.append(" ".join(row))
    print("\n".join(grid_display))

    times = round(dijkstra_time / astar_time, 1)
    faster = "A* is faster by {:.1f}x".format(times) if times > 1 else "Dijkstra is faster by {:.1f}x".format(1 / times)
    print(f"\nCurrent Cost: {current_cost} in {astar_time * 1000:.4f}ms | Goal Minimum Cost: {goal_cost} in {dijkstra_time * 1000:.4f}ms | {faster}")

  def animate_robot_path(self, grid, path, dijkstra_path, goal, dijkstra_time, astar_time):
    # Animate the robot's movement across the grid
    visited = set()
    goal_cost = len(dijkstra_path)
    current_cost = 0
    self.move_cursor_to_top()
    for position in path:
      visited.add(position)
      self.print_grid(grid, position, visited, dijkstra_path, current_cost, goal_cost, goal, dijkstra_time, astar_time)
      current_cost += 1
      time.sleep(0.05)
      self.move_cursor_to_top()

  def dijkstra_min_cost(self, grid, goal):
    # Implements Dijkstra's algorithm to calculate the minimum cost path
    n = len(grid)
    start = (0, 0)
    pq = [(0, start)]
    cost_map = {start: 0}
    directions = [(0, 1), (1, 0), (0, -1), (-1, 0)]
    prev = {}

    while pq:
      current_cost, (x, y) = heapq.heappop(pq)
      if (x, y) == goal:
        path = []
        while (x, y) != start:
          path.append((x, y))
          x, y = prev[(x, y)]
        return path[::-1]

      for dx, dy in directions:
        nx, ny = x + dx, y + dy
        if 0 <= nx < n and 0 <= ny < n and grid[nx][ny] == 0:
          new_cost = current_cost + 1
          if (nx, ny) not in cost_map or new_cost < cost_map[(nx, ny)]:
            cost_map[(nx, ny)] = new_cost
            prev[(nx, ny)] = (x, y)
            heapq.heappush(pq, (new_cost, (nx, ny)))

    return []

  def increase_obstacle_density(self, grid, density=0.3):
    # Randomly place obstacles in the grid
    total_cells = len(grid) * len(grid[0])
    obstacles_to_place = int(total_cells * density)

    while obstacles_to_place > 0:
      x = random.randint(0, len(grid) - 1)
      y = random.randint(0, len(grid[0]) - 1)
      if grid[x][y] == 0 and (x, y) != (0, 0):
        grid[x][y] = 1
        obstacles_to_place -= 1

  def random_goal(self, grid):
    # Randomly place the goal in the last quarter of the grid
    tries = 0
    n = len(grid) // 4
    while tries < 1000:
      tries += 1
      goal = (random.randint(0, n - 1) + n * 3, random.randint(0, n - 1) + n * 3)
      if grid[goal[0]][goal[1]] == 0 and goal != (0, 0):
        return goal

  def verify_grid_has_path(self, grid, goal):
    # Verify that there is a valid path from the start to the goal using Dijkstra's algorithm
    dijkstra_path = self.dijkstra_min_cost(grid, goal)
    return dijkstra_path if dijkstra_path else None

  def test_cases(self, animate=True):
    # Initialize grids and run test cases
    grids = [
        [[0] * 50 for _ in range(50)] for _ in range(5)
    ]

    # Increase obstacle density for each grid
    for grid in grids:
      self.increase_obstacle_density(grid, density=0.3)

    solution = Solution()

    for idx, input_grid in enumerate(grids):
      tries = 0
      while tries < 1000:
        goal = self.random_goal(input_grid)
        dijkstra_start_time = time.time()
        dijkstra_path = self.verify_grid_has_path(input_grid, goal)
        dijkstra_time = time.time() - dijkstra_start_time

        if dijkstra_path:  # Ensure a valid path exists
          break

      print(f"Test Case {idx + 1}: Goal is at {goal}")
      print("Animating the robot's path...\n")

      # Measure time for A* (robot's path)
      astar_start_time = time.time()
      result = solution.robot_path(input_grid, goal)
      astar_time = time.time() - astar_start_time

      # Animate robot path
      if animate:
        self.animate_robot_path(input_grid, result, dijkstra_path, goal, dijkstra_time, astar_time)

      # Pause for next test
      if animate:
        self.pause_for_next_test()


evaluator = Evaluation()
evaluator.test_cases()