Lab 06-02: A* Path Planning with Heuristics

Objectives

By the end of this lab, you will be able to:

• Implement the A* search algorithm for grid-based planning
• Design and compare different heuristic functions
• Understand admissibility and consistency of heuristics
• Apply A* to robot configuration space planning

Prerequisites

• Completed Lab 06-01 (RRT Path Planning)
• Understanding of graph search algorithms
• Basic data structures (priority queues)

Materials

Type	Name	Tier	Notes
software	Python 3.10+	required	Programming environment
software	NumPy, Matplotlib, heapq	required	Computation and visualization
simulation	Grid world environments	required	2D planning scenarios

Background

A* Algorithm

A* is an optimal graph search algorithm that uses:

• g(n): Cost from start to node n
• h(n): Heuristic estimate from n to goal
• f(n) = g(n) + h(n): Total estimated cost

A* expands nodes in order of lowest f(n), guaranteeing optimality when h(n) is admissible (never overestimates).

Heuristics for Grid Planning

Common heuristics:

• Manhattan distance: |x₁-x₂| + |y₁-y₂| (4-connected grid)
• Euclidean distance: √((x₁-x₂)² + (y₁-y₂)²)
• Diagonal distance: max(|Δx|, |Δy|) + (√2-1)·min(|Δx|, |Δy|)

Instructions

Step 1: Set Up the Grid Environment

Create a 2D occupancy grid with obstacles:

import numpy as np
import matplotlib.pyplot as plt
import heapq
from collections import defaultdict

class GridWorld:
    """2D grid environment for path planning."""

    def __init__(self, width, height, obstacle_ratio=0.2):
        """
        Create grid world with random obstacles.

        Args:
            width, height: Grid dimensions
            obstacle_ratio: Fraction of cells that are obstacles
        """
        self.width = width
        self.height = height

        # Create grid (0 = free, 1 = obstacle)
        self.grid = np.zeros((height, width), dtype=int)

        # Add random obstacles
        n_obstacles = int(width * height * obstacle_ratio)
        obstacle_indices = np.random.choice(width * height, n_obstacles, replace=False)
        for idx in obstacle_indices:
            y, x = divmod(idx, width)
            self.grid[y, x] = 1

    def add_rectangular_obstacle(self, x1, y1, x2, y2):
        """Add rectangular obstacle."""
        self.grid[y1:y2, x1:x2] = 1

    def is_valid(self, x, y):
        """Check if position is valid (in bounds and free)."""
```python
```python
        return (0 <= x < self.width and

                0 <= y < self.height and
                self.grid[y, x] == 0)

    def get_neighbors(self, x, y, connectivity=8):
        """
        Get valid neighboring cells.

        Args:
            connectivity: 4 (cardinal) or 8 (with diagonals)
        """
```python
```python
        if connectivity == 4:

            directions = [(0, 1), (1, 0), (0, -1), (-1, 0)]
        else:
            directions = [(0, 1), (1, 0), (0, -1), (-1, 0),
                         (1, 1), (1, -1), (-1, 1), (-1, -1)]

        neighbors = []
        for dx, dy in directions:
            nx, ny = x + dx, y + dy
            if self.is_valid(nx, ny):
                # Cost: 1 for cardinal, sqrt(2) for diagonal
                cost = np.sqrt(dx*dx + dy*dy)
                neighbors.append((nx, ny, cost))
        return neighbors

    def visualize(self, path=None, start=None, goal=None, explored=None):
        """Visualize grid with optional path."""
        fig, ax = plt.subplots(figsize=(10, 10))

        # Show grid
        ax.imshow(self.grid, cmap='binary', origin='lower')

        # Show explored cells
        if explored:
            for (x, y) in explored:
                ax.plot(x, y, 'c.', markersize=3, alpha=0.3)

        # Show path
        if path:
            path_x = [p[0] for p in path]
            path_y = [p[1] for p in path]
            ax.plot(path_x, path_y, 'b-', linewidth=2, label='Path')

        # Show start and goal
        if start:
            ax.plot(start[0], start[1], 'go', markersize=15, label='Start')
        if goal:
            ax.plot(goal[0], goal[1], 'r*', markersize=15, label='Goal')

        ax.set_xlabel('X')
        ax.set_ylabel('Y')
        ax.legend()
        ax.set_title(f'Grid World ({self.width}x{self.height})')

        return fig, ax

# Create environment
np.random.seed(42)
world = GridWorld(50, 50, obstacle_ratio=0.0)

# Add structured obstacles
world.add_rectangular_obstacle(10, 10, 15, 40)
world.add_rectangular_obstacle(25, 5, 30, 35)
world.add_rectangular_obstacle(35, 20, 45, 25)

# Define start and goal
start = (5, 5)
goal = (45, 45)

# Ensure start and goal are free
world.grid[start[1], start[0]] = 0
world.grid[goal[1], goal[0]] = 0

world.visualize(start=start, goal=goal)
plt.savefig('grid_world.png', dpi=150)
plt.show()

Expected: Grid visualization showing obstacles, start (green), and goal (red).

Step 2: Implement Heuristic Functions

Create multiple heuristics for comparison:

def manhattan_distance(p1, p2):
    """Manhattan (L1) distance - admissible for 4-connected grid."""
    return abs(p1[0] - p2[0]) + abs(p1[1] - p2[1])

def euclidean_distance(p1, p2):
    """Euclidean (L2) distance - admissible for any grid."""
    return np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

def diagonal_distance(p1, p2):
    """Octile distance - admissible for 8-connected grid."""
    dx = abs(p1[0] - p2[0])
    dy = abs(p1[1] - p2[1])
    return max(dx, dy) + (np.sqrt(2) - 1) * min(dx, dy)

def chebyshev_distance(p1, p2):
    """Chebyshev (L∞) distance."""
    return max(abs(p1[0] - p2[0]), abs(p1[1] - p2[1]))

def zero_heuristic(p1, p2):
    """Zero heuristic - turns A* into Dijkstra's algorithm."""
    return 0

# Test heuristics
test_start = (0, 0)
test_goal = (10, 10)

print("Heuristic comparison for (0,0) to (10,10):")
print(f"  Manhattan: {manhattan_distance(test_start, test_goal):.2f}")
print(f"  Euclidean: {euclidean_distance(test_start, test_goal):.2f}")
print(f"  Diagonal:  {diagonal_distance(test_start, test_goal):.2f}")
print(f"  Chebyshev: {chebyshev_distance(test_start, test_goal):.2f}")

Expected: Different heuristic values - Euclidean ≈ 14.14, Manhattan = 20, Diagonal ≈ 14.14.

Step 3: Implement A* Algorithm

Build the core A* search:

class AStarPlanner:
    """A* path planning algorithm."""

    def __init__(self, world, heuristic=euclidean_distance, connectivity=8):
        """
        Initialize A* planner.

        Args:
            world: GridWorld instance
            heuristic: Heuristic function h(current, goal)
            connectivity: 4 or 8 connected grid
        """
        self.world = world
        self.heuristic = heuristic
        self.connectivity = connectivity

        # Statistics
        self.nodes_expanded = 0
        self.nodes_generated = 0

    def plan(self, start, goal):
        """
        Find optimal path from start to goal.

        Args:
            start: (x, y) start position
            goal: (x, y) goal position

        Returns:
            path: List of (x, y) positions, or None if no path
            explored: Set of explored positions
        """
        self.nodes_expanded = 0
        self.nodes_generated = 0

        # Priority queue: (f_score, counter, x, y)
        # Counter breaks ties for same f_score
        counter = 0
        open_set = [(self.heuristic(start, goal), counter, start[0], start[1])]

        # Track visited nodes and their costs
        g_score = {start: 0}
        came_from = {}

        # For visualization
        explored = set()

        while open_set:
            # Get node with lowest f_score
            f, _, x, y = heapq.heappop(open_set)
            current = (x, y)

            # Skip if we've found a better path to this node
            if current in explored:
                continue

            explored.add(current)
            self.nodes_expanded += 1

            # Goal check
```python
```python
            if current == goal:

                return self._reconstruct_path(came_from, current), explored

            # Expand neighbors
            for nx, ny, cost in self.world.get_neighbors(x, y, self.connectivity):
                neighbor = (nx, ny)
                self.nodes_generated += 1

                # Calculate tentative g_score
                tentative_g = g_score[current] + cost

                # If this path is better than any previous
```python
```python
                if neighbor not in g_score or tentative_g < g_score[neighbor]:

                    g_score[neighbor] = tentative_g
                    f_score = tentative_g + self.heuristic(neighbor, goal)
                    came_from[neighbor] = current

                    counter += 1
                    heapq.heappush(open_set, (f_score, counter, nx, ny))

        # No path found
        return None, explored

    def _reconstruct_path(self, came_from, current):
        """Reconstruct path from came_from dictionary."""
        path = [current]
        while current in came_from:
            current = came_from[current]
            path.append(current)
        return path[::-1]

# Test A* with Euclidean heuristic
planner = AStarPlanner(world, heuristic=euclidean_distance, connectivity=8)
path, explored = planner.plan(start, goal)

if path:
    print(f"Path found!")
    print(f"  Path length: {len(path)} nodes")
    print(f"  Nodes expanded: {planner.nodes_expanded}")
    print(f"  Nodes generated: {planner.nodes_generated}")

    # Calculate path cost
    path_cost = 0
    for i in range(len(path) - 1):
        dx = path[i+1][0] - path[i][0]
        dy = path[i+1][1] - path[i][1]
        path_cost += np.sqrt(dx*dx + dy*dy)
    print(f"  Path cost: {path_cost:.2f}")
else:
    print("No path found!")

# Visualize
world.visualize(path=path, start=start, goal=goal, explored=explored)
plt.title(f'A* with Euclidean Heuristic (expanded: {planner.nodes_expanded})')
plt.savefig('astar_euclidean.png', dpi=150)
plt.show()

Expected: Path found with visualization showing explored nodes (cyan) and final path (blue).

Step 4: Compare Heuristics

Evaluate different heuristics on the same problem:

def compare_heuristics(world, start, goal):
    """Compare different heuristics on same problem."""
    heuristics = {
        'Zero (Dijkstra)': zero_heuristic,
        'Manhattan': manhattan_distance,
        'Euclidean': euclidean_distance,
        'Diagonal': diagonal_distance,
        'Chebyshev': chebyshev_distance
    }

    results = {}

    for name, h in heuristics.items():
        planner = AStarPlanner(world, heuristic=h, connectivity=8)
        path, explored = planner.plan(start, goal)

        if path:
            # Calculate path cost
            path_cost = 0
            for i in range(len(path) - 1):
                dx = path[i+1][0] - path[i][0]
                dy = path[i+1][1] - path[i][1]
                path_cost += np.sqrt(dx*dx + dy*dy)

            results[name] = {
                'path_length': len(path),
                'path_cost': path_cost,
                'nodes_expanded': planner.nodes_expanded,
                'nodes_generated': planner.nodes_generated,
                'explored': explored
            }
        else:
            results[name] = None

    return results

# Run comparison
results = compare_heuristics(world, start, goal)

# Print comparison table
print("\nHeuristic Comparison:")
print("-" * 70)
print(f"{'Heuristic':<20} {'Path Cost':<12} {'Expanded':<12} {'Generated':<12}")
print("-" * 70)
for name, r in results.items():
    if r:
        print(f"{name:<20} {r['path_cost']:<12.2f} {r['nodes_expanded']:<12} {r['nodes_generated']:<12}")
print("-" * 70)

# Verify all paths have same cost (optimality)
costs = [r['path_cost'] for r in results.values() if r]
```python
```python
if len(set([round(c, 2) for c in costs])) == 1:

    print(f"\n✓ All heuristics found optimal path (cost = {costs[0]:.2f})")
else:
    print(f"\n✗ Path costs differ: {costs}")

Expected: All admissible heuristics find same-cost path; better heuristics expand fewer nodes.

Step 5: Visualize Heuristic Comparison

Create side-by-side comparison:

def visualize_heuristic_comparison(world, start, goal, results):
    """Visualize explored areas for different heuristics."""
    fig, axes = plt.subplots(2, 3, figsize=(15, 10))
    axes = axes.flatten()

    heuristic_names = list(results.keys())

    for idx, name in enumerate(heuristic_names):
        ax = axes[idx]
        r = results[name]

        if r is None:
            ax.set_title(f'{name}: No path')
            continue

        # Show grid
        ax.imshow(world.grid, cmap='binary', origin='lower', alpha=0.5)

        # Show explored cells
        explored_x = [p[0] for p in r['explored']]
        explored_y = [p[1] for p in r['explored']]
        ax.scatter(explored_x, explored_y, c='cyan', s=5, alpha=0.5)

        # Show path would go here (skipping for clarity)

        # Start and goal
        ax.plot(start[0], start[1], 'go', markersize=10)
        ax.plot(goal[0], goal[1], 'r*', markersize=10)

        ax.set_title(f'{name}\nExpanded: {r["nodes_expanded"]}')
        ax.set_xlim(-1, world.width)
        ax.set_ylim(-1, world.height)

    # Hide unused subplot
```python
```python
    if len(heuristic_names) < 6:

        axes[-1].axis('off')

    plt.tight_layout()
    plt.savefig('heuristic_comparison.png', dpi=150)
    plt.show()

visualize_heuristic_comparison(world, start, goal, results)

Expected: Six panels showing different exploration patterns; Dijkstra explores most, informed heuristics explore less.

Step 6: Apply to Configuration Space

Extend A* to robot joint space planning:

class ConfigSpaceAstar:
    """A* planner in robot configuration space."""

    def __init__(self, joint_limits, resolution=0.1, collision_checker=None):
        """
        Initialize C-space A* planner.

        Args:
            joint_limits: [(min, max), ...] for each joint
            resolution: Grid resolution in radians
            collision_checker: Function(q) -> bool (True if collision)
        """
        self.joint_limits = np.array(joint_limits)
        self.resolution = resolution
        self.collision_checker = collision_checker
        self.n_joints = len(joint_limits)

        # Compute grid dimensions
        self.grid_dims = []
        for (lo, hi) in joint_limits:
            self.grid_dims.append(int((hi - lo) / resolution) + 1)

    def config_to_grid(self, q):
        """Convert continuous config to grid indices."""
        indices = []
        for i, qi in enumerate(q):
            idx = int((qi - self.joint_limits[i, 0]) / self.resolution)
            idx = np.clip(idx, 0, self.grid_dims[i] - 1)
            indices.append(idx)
        return tuple(indices)

    def grid_to_config(self, indices):
        """Convert grid indices to continuous config."""
        q = []
        for i, idx in enumerate(indices):
            qi = self.joint_limits[i, 0] + idx * self.resolution
            q.append(qi)
        return np.array(q)

    def get_neighbors(self, indices):
        """Get neighboring grid cells."""
        neighbors = []
        for dim in range(self.n_joints):
            for delta in [-1, 1]:
                new_indices = list(indices)
                new_indices[dim] += delta

                # Check bounds
```python
```python
                if 0 <= new_indices[dim] < self.grid_dims[dim]:

                    new_indices = tuple(new_indices)

                    # Check collision
                    q = self.grid_to_config(new_indices)
                    if self.collision_checker is None or not self.collision_checker(q):
                        neighbors.append((new_indices, self.resolution))

        return neighbors

    def heuristic(self, idx1, idx2):
        """Euclidean distance in grid space."""
        return np.sqrt(sum((i1 - i2)**2 for i1, i2 in zip(idx1, idx2))) * self.resolution

    def plan(self, q_start, q_goal):
        """Plan path from q_start to q_goal."""
        start_idx = self.config_to_grid(q_start)
        goal_idx = self.config_to_grid(q_goal)

        # A* search
        counter = 0
        open_set = [(self.heuristic(start_idx, goal_idx), counter, start_idx)]

        g_score = {start_idx: 0}
        came_from = {}
        explored = set()

        while open_set:
            f, _, current = heapq.heappop(open_set)

            if current in explored:
                continue
            explored.add(current)

```python
```python
            if current == goal_idx:

                # Reconstruct path
                path_indices = [current]
                while current in came_from:
                    current = came_from[current]
                    path_indices.append(current)
                path_indices.reverse()

                # Convert to configurations
                path = [self.grid_to_config(idx) for idx in path_indices]
                return path, explored

            for neighbor, cost in self.get_neighbors(current):
                tentative_g = g_score[current] + cost

```python
```python
                if neighbor not in g_score or tentative_g < g_score[neighbor]:

                    g_score[neighbor] = tentative_g
                    f_score = tentative_g + self.heuristic(neighbor, goal_idx)
                    came_from[neighbor] = current
                    counter += 1
                    heapq.heappush(open_set, (f_score, counter, neighbor))

        return None, explored

# Example: Plan for 2-DOF arm
def simple_collision_check(q):
    """Simple collision checker for demo."""
    # Obstacle in C-space
    obstacle_center = np.array([1.0, 0.5])
    obstacle_radius = 0.3
```python
```python
    return np.linalg.norm(q - obstacle_center) < obstacle_radius

# Create planner
joint_limits = [(-np.pi, np.pi), (-np.pi, np.pi)]
cspace_planner = ConfigSpaceAstar(joint_limits, resolution=0.1,
                                   collision_checker=simple_collision_check)

# Plan
q_start = np.array([0.0, 0.0])
q_goal = np.array([1.5, 1.0])

path, explored = cspace_planner.plan(q_start, q_goal)

if path:
    print(f"C-space path found with {len(path)} waypoints")
    print(f"Explored {len(explored)} grid cells")
else:
    print("No path found in C-space")

Expected: Path found in 2D configuration space, avoiding the circular obstacle region.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • astar.py: A* algorithm implementation
   • heuristics.py: Collection of heuristic functions
   • cspace_astar.py: Configuration space planner

2. Visual outputs:

   • grid_world.png: Environment visualization
   • astar_euclidean.png: A* search with explored nodes
   • heuristic_comparison.png: Side-by-side heuristic comparison

3. Understanding:

   • A* optimality conditions
   • Trade-offs between heuristic informativeness and computation
   • Grid-based vs. sampling-based planning

Rubric

Criterion	Points	Description
Grid Environment	15	Correct obstacle representation and neighbor generation
Heuristic Functions	20	Multiple correct, admissible heuristics
A* Implementation	30	Optimal path finding with proper data structures
Heuristic Comparison	20	Fair comparison with performance analysis
C-Space Extension	15	Working configuration space planner
Total	100

Common Errors

Error: A* returns suboptimal path Solution: Check heuristic admissibility. Ensure h(n) never overestimates true cost.

Error: A* is very slow Solution: Use more informed heuristic. Check priority queue implementation efficiency.

Error: Different heuristics give different path costs Solution: One or more heuristics may be inadmissible or implementation has bugs.

Extensions

For advanced students:

1. Weighted A*: Implement A* with w*h(n) for faster but suboptimal search
2. Anytime A*: Return best solution found so far, improve over time
3. Multi-goal planning: Find paths to multiple goals efficiently
4. D Lite*: Implement incremental replanning for changing environments

Related Content

• Theory: Module 06 theory.md, Section 6.3 (Graph Search)
• Previous Lab: Lab 06-01 (RRT Path Planning)
• Next Lab: Lab 06-03 (Trajectory Optimization)