Graph-based Planning Algorithms¶

Overview¶

Graph-based planning algorithms represent a fundamental approach to path finding where the continuous planning problem is discretized into a structured graph or grid representation. In this paradigm, nodes represent discrete states and edges represent valid transitions between states, with associated costs or weights. These algorithms systematically explore the state space to find optimal or feasible paths while balancing completeness, optimality, and computational efficiency.

Core Characteristics of Graph-based Planning¶

1. Structured Exploration: Algorithms follow predetermined connectivity patterns (4-connectivity, 8-connectivity for grids) or predefined graph edges.
2. Optimality Guarantees: Under appropriate conditions (admissible heuristics, non-negative edge weights), many graph-based algorithms guarantee optimal solutions.
3. Deterministic Search: Exploration follows systematic rules rather than random sampling, providing predictable performance characteristics.
4. Resolution-dependent: Solution quality depends on the discretization resolution of the state space.

Comparison with Sampling-based Algorithms (RRT, PRM)¶

Graph-based planning differs fundamentally from sampling-based approaches:

Aspect	Graph-based Planning	Sampling-based Planning
State Space	Discrete grid/graph	Continuous configuration space
Exploration	Systematic, following graph connectivity	Random sampling guided by heuristics
Optimality	Provably optimal under conditions	Asymptotically optimal (RRT*), probabilistic completeness
Completeness	Resolution-complete	Probabilistically complete
Data Structure	Graph/Grid with fixed connectivity	Tree (RRT) or roadmap (PRM) built incrementally
Applications	Known environments with regular structure	High-dimensional, complex, or unknown environments

When to use graph-based planning:

• Environment can be efficiently discretized into a grid or graph
• Optimality guarantees are required
• Deterministic behavior is preferred
• Search space has regular structure (e.g., urban roads, indoor environments)

Historical Evolution¶

Classical graph search algorithms form the foundation of modern motion planning. Dijkstra's algorithm (1956) introduced systematic shortest-path search on weighted graphs, guaranteeing optimality at the cost of uniform exploration. A* (1968) improved efficiency through heuristic guidance while maintaining optimality with admissible heuristics.

The evolution continued with D* (Dynamic A, 1990s) for real-time replanning in dynamic environments, efficiently repairing paths when obstacles appear. Hybrid A (2000s) bridged discrete graph search with continuous motion planning, incorporating vehicle kinematics for autonomous driving applications.

These algorithms demonstrate a progression from basic graph search to sophisticated planning systems that handle dynamic environments and complex vehicle dynamics. This tutorial provides implementations and visualizations of these core algorithms for grid-based navigation problems.

Algorithm	Year	Search Type	Strategy	State Space	Dynamic	Optimality	Use Case
Dijkstra	1956	Graph	None	Discrete/Grid	No	Yes	Static path planning
A*	1968	Graph	Heuristic	Discrete/Grid	No	Yes	Goal-directed search
D*	1990s	Graph	Incremental	Discrete/Grid	Yes	Yes	Dynamic environments
D* Lite	2000s	Graph	Incremental	Discrete/Grid	Yes	Yes	Dynamic environments
Hybrid A*	2000s	Graph + Continuous	Heuristic	Continuous	Partial	Resolution-complete	Autonomous driving

In [1]:

hide_input

%matplotlib notebook

import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Rectangle
from IPython.display import HTML


from tactics2d.search import AStar, Dijkstra, grid_to_csr
from tactics2d.map.generator import GridMapGenerator

# For reproducibility
random_seed = 2048
np.random.seed(random_seed)

mpl.rcParams["animation.embed_limit"] = 50 * 1024 * 1024  # 50 MB

%matplotlib notebook import numpy as np import matplotlib as mpl import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from matplotlib.patches import Rectangle from IPython.display import HTML from tactics2d.search import AStar, Dijkstra, grid_to_csr from tactics2d.map.generator import GridMapGenerator # For reproducibility random_seed = 2048 np.random.seed(random_seed) mpl.rcParams["animation.embed_limit"] = 50 * 1024 * 1024 # 50 MB

In [2]:

hide_input

connectivity=8
fps = 10
size=(10, 10)

map_generator = GridMapGenerator(
    size=size, min_cost=0, max_cost=5, obstacle_proportion=0.1, random_seed=random_seed
)

grid_map, start, goal = map_generator.generate()
print(f"Grid map shape: {grid_map.shape}")
print(f"Start: {start}, Goal: {goal}")

graph = grid_to_csr(grid_map, obstacle_value=np.inf, connectivity=connectivity)
print(f"Graph shape: {graph.shape}, number of edges: {graph.nnz}")

connectivity=8 fps = 10 size=(10, 10) map_generator = GridMapGenerator( size=size, min_cost=0, max_cost=5, obstacle_proportion=0.1, random_seed=random_seed ) grid_map, start, goal = map_generator.generate() print(f"Grid map shape: {grid_map.shape}") print(f"Start: {start}, Goal: {goal}") graph = grid_to_csr(grid_map, obstacle_value=np.inf, connectivity=connectivity) print(f"Graph shape: {graph.shape}, number of edges: {graph.nnz}")

Grid map shape: (10, 10)
Start: (3, 5), Goal: (0, 1)
Graph shape: (100, 100), number of edges: 560

In [3]:

hide_input

# Color configuration as per requirements
COST_COLOR_MAP = {
    0: "#FFFFFF",
    1: "#EEEEEE",
    2: "#DDDDDD",
    3: "#CCCCCC",
    4: "#BBBBBB",
    5: "#AAAAAA",
}

# For costs beyond the map, use the color of max key
MAX_COST_COLOR = "#888888"

# Start and goal colors (priority over cost colors)
START_COLOR = "#FFCDD2"  # Light red
START_EDGE = "#B71C1C"  # Dark red
GOAL_COLOR = "#C8E6C9"  # Light green (from cost 2)
GOAL_EDGE = "#1B5E20"  # Dark green

# Cell border color
CELL_EDGE_COLOR = "#DDDDDD"  # Light gray

# Text styling for cost labels
COST_TEXT_STYLE = {
    "fontsize": 8,
    "color": "black",
    "alpha": 0.8,
    "ha": "center",
    "va": "center",
    "fontweight": "normal",
}

# Animation colors for search states
PATH_COLOR = "#229453"
OPEN_SET_COLOR = "#FFF59D"  # Light yellow for nodes in open set
OPEN_SET_EDGE = "#F9A825"  # Darker yellow edge
CURRENT_NODE_COLOR = "#F8BBD0"  # Light pink
CURRENT_NODE_EDGE = "#AD1457"  # Deep pink / magenta
VISITED_COLOR = "#B3E5FC"  # Light blue for visited nodes
VISITED_EDGE = "#0288D1"  # Blue edge for visited
OBSTACLE_COLOR = "#333333"  # Dark gray for obstacles
OBSTACLE_EDGE = "#111111"  # Black edge for obstacles

# Color configuration as per requirements COST_COLOR_MAP = { 0: "#FFFFFF", 1: "#EEEEEE", 2: "#DDDDDD", 3: "#CCCCCC", 4: "#BBBBBB", 5: "#AAAAAA", } # For costs beyond the map, use the color of max key MAX_COST_COLOR = "#888888" # Start and goal colors (priority over cost colors) START_COLOR = "#FFCDD2" # Light red START_EDGE = "#B71C1C" # Dark red GOAL_COLOR = "#C8E6C9" # Light green (from cost 2) GOAL_EDGE = "#1B5E20" # Dark green # Cell border color CELL_EDGE_COLOR = "#DDDDDD" # Light gray # Text styling for cost labels COST_TEXT_STYLE = { "fontsize": 8, "color": "black", "alpha": 0.8, "ha": "center", "va": "center", "fontweight": "normal", } # Animation colors for search states PATH_COLOR = "#229453" OPEN_SET_COLOR = "#FFF59D" # Light yellow for nodes in open set OPEN_SET_EDGE = "#F9A825" # Darker yellow edge CURRENT_NODE_COLOR = "#F8BBD0" # Light pink CURRENT_NODE_EDGE = "#AD1457" # Deep pink / magenta VISITED_COLOR = "#B3E5FC" # Light blue for visited nodes VISITED_EDGE = "#0288D1" # Blue edge for visited OBSTACLE_COLOR = "#333333" # Dark gray for obstacles OBSTACLE_EDGE = "#111111" # Black edge for obstacles

In [4]:

hide_input

def init_figure(size):
    height, width = size
    fig, ax = plt.subplots(figsize=(max(8, width * 0.8), max(8, height * 0.8)))
    fig.set_dpi(100)
    ax.set_aspect("equal")
    ax.set_xlim(-0.5, width - 0.5)
    ax.set_ylim(-0.5, height - 0.5)
    ax.invert_yaxis()  # Match matrix indexing (row 0 at top)
    ax.set_xticks(range(width))
    ax.set_yticks(range(height))
    ax.set_title("Search Algorithm Animation")
    ax.set_xlabel("Column (x)")
    ax.set_ylabel("Row (y)")

    return fig, ax

def init_figure(size): height, width = size fig, ax = plt.subplots(figsize=(max(8, width * 0.8), max(8, height * 0.8))) fig.set_dpi(100) ax.set_aspect("equal") ax.set_xlim(-0.5, width - 0.5) ax.set_ylim(-0.5, height - 0.5) ax.invert_yaxis() # Match matrix indexing (row 0 at top) ax.set_xticks(range(width)) ax.set_yticks(range(height)) ax.set_title("Search Algorithm Animation") ax.set_xlabel("Column (x)") ax.set_ylabel("Row (y)") return fig, ax

In [5]:

hide_input

# Initialize grid cells (all cells, including obstacles)
def init_grid(ax, vis_objects, grid_map, start, goal, width, height):
    """Initialize all grid cells as Rectangle patches with cost labels."""
    # Clear any existing patches/texts
    for patch in vis_objects["cell_patches"]:
        patch.remove()
    for text in vis_objects["cell_texts"]:
        text.remove()
    vis_objects["cell_patches"].clear()
    vis_objects["cell_texts"].clear()

    # Create patches and texts for each cell
    for row in range(height):
        for col in range(width):
            cost = grid_map[row, col]
            is_obstacle = np.isinf(cost)

            # Determine cell color based on cost/obstacle
            if is_obstacle:
                facecolor = OBSTACLE_COLOR
                edgecolor = OBSTACLE_EDGE
                display_cost = "∞"
            else:
                cost_int = int(cost)
                if cost_int in COST_COLOR_MAP:
                    facecolor = COST_COLOR_MAP[cost_int]
                else:
                    facecolor = MAX_COST_COLOR
                edgecolor = CELL_EDGE_COLOR
                display_cost = str(cost_int)

            # Create rectangle patch for cell
            rect = Rectangle(
                (col - 0.5, row - 0.5),  # Bottom-left corner
                1.0,
                1.0,  # Width and height
                facecolor=facecolor,
                edgecolor=edgecolor,
                linewidth=1.0,
            )
            ax.add_patch(rect)
            vis_objects["cell_patches"].append(rect)

            # Add cost text label
            text = ax.text(col, row, display_cost, **COST_TEXT_STYLE)  # Center position
            vis_objects["cell_texts"].append(text)

    # Create special patches for start and goal (will be on top)
    sr, sc = start[0], start[1]
    gr, gc = goal[0], goal[1]

    # Start cell patch (on top of regular cell)
    start_rect = Rectangle(
        (sc - 0.5, sr - 0.5),
        1.0,
        1.0,
        facecolor=START_COLOR,
        edgecolor=START_EDGE,
        linewidth=2.0,
        alpha=0.7,
    )
    ax.add_patch(start_rect)
    vis_objects["start_patch"] = start_rect

    # Goal cell patch (on top of regular cell)
    goal_rect = Rectangle(
        (gc - 0.5, gr - 0.5),
        1.0,
        1.0,
        facecolor=GOAL_COLOR,
        edgecolor=GOAL_EDGE,
        linewidth=2.0,
        alpha=0.7,
    )
    ax.add_patch(goal_rect)
    vis_objects["goal_patch"] = goal_rect

    # Initialize current node patch (will be moved)
    current_rect = Rectangle(
        (-10, -10),  # Start off-screen
        1.0,
        1.0,
        facecolor=CURRENT_NODE_COLOR,
        edgecolor=CURRENT_NODE_EDGE,
        linewidth=2.0,
        alpha=0.8,
    )
    ax.add_patch(current_rect)
    vis_objects["current_patch"] = current_rect

    # Initialize path line (empty initially)
    (path_line,) = ax.plot([], [], color=PATH_COLOR, linewidth=2, alpha=0.8, label="Final Path")
    vis_objects["path_line"] = path_line

    ax.legend(loc="upper right")

    # Return all artists for blitting (including empty visited_patches list)
    return (
        vis_objects["cell_patches"]
        + vis_objects["cell_texts"]
        + [
            vis_objects["start_patch"],
            vis_objects["goal_patch"],
            vis_objects["current_patch"],
            vis_objects["path_line"],
        ]
        + vis_objects["visited_patches"]
    )

# Initialize grid cells (all cells, including obstacles) def init_grid(ax, vis_objects, grid_map, start, goal, width, height): """Initialize all grid cells as Rectangle patches with cost labels.""" # Clear any existing patches/texts for patch in vis_objects["cell_patches"]: patch.remove() for text in vis_objects["cell_texts"]: text.remove() vis_objects["cell_patches"].clear() vis_objects["cell_texts"].clear() # Create patches and texts for each cell for row in range(height): for col in range(width): cost = grid_map[row, col] is_obstacle = np.isinf(cost) # Determine cell color based on cost/obstacle if is_obstacle: facecolor = OBSTACLE_COLOR edgecolor = OBSTACLE_EDGE display_cost = "∞" else: cost_int = int(cost) if cost_int in COST_COLOR_MAP: facecolor = COST_COLOR_MAP[cost_int] else: facecolor = MAX_COST_COLOR edgecolor = CELL_EDGE_COLOR display_cost = str(cost_int) # Create rectangle patch for cell rect = Rectangle( (col - 0.5, row - 0.5), # Bottom-left corner 1.0, 1.0, # Width and height facecolor=facecolor, edgecolor=edgecolor, linewidth=1.0, ) ax.add_patch(rect) vis_objects["cell_patches"].append(rect) # Add cost text label text = ax.text(col, row, display_cost, **COST_TEXT_STYLE) # Center position vis_objects["cell_texts"].append(text) # Create special patches for start and goal (will be on top) sr, sc = start[0], start[1] gr, gc = goal[0], goal[1] # Start cell patch (on top of regular cell) start_rect = Rectangle( (sc - 0.5, sr - 0.5), 1.0, 1.0, facecolor=START_COLOR, edgecolor=START_EDGE, linewidth=2.0, alpha=0.7, ) ax.add_patch(start_rect) vis_objects["start_patch"] = start_rect # Goal cell patch (on top of regular cell) goal_rect = Rectangle( (gc - 0.5, gr - 0.5), 1.0, 1.0, facecolor=GOAL_COLOR, edgecolor=GOAL_EDGE, linewidth=2.0, alpha=0.7, ) ax.add_patch(goal_rect) vis_objects["goal_patch"] = goal_rect # Initialize current node patch (will be moved) current_rect = Rectangle( (-10, -10), # Start off-screen 1.0, 1.0, facecolor=CURRENT_NODE_COLOR, edgecolor=CURRENT_NODE_EDGE, linewidth=2.0, alpha=0.8, ) ax.add_patch(current_rect) vis_objects["current_patch"] = current_rect # Initialize path line (empty initially) (path_line,) = ax.plot([], [], color=PATH_COLOR, linewidth=2, alpha=0.8, label="Final Path") vis_objects["path_line"] = path_line ax.legend(loc="upper right") # Return all artists for blitting (including empty visited_patches list) return ( vis_objects["cell_patches"] + vis_objects["cell_texts"] + [ vis_objects["start_patch"], vis_objects["goal_patch"], vis_objects["current_patch"], vis_objects["path_line"], ] + vis_objects["visited_patches"] )

In [6]:

hide_input

def create_update_frame(algorithm_name, states_history):
    """Create a generic update frame function for search algorithm animation.

    Parameters
    ----------
    algorithm_name : str
        Name of the algorithm (e.g., "Dijkstra", "A*")
    states_history : list
        List of algorithm states collected via callback

    Returns
    -------
    function
        Update function compatible with FuncAnimation
    """

    def update_frame(frame_idx):
        """Update visualization for a given algorithm state."""
        if frame_idx >= len(states_history):
            return (
                vis_objects["cell_patches"]
                + vis_objects["cell_texts"]
                + [
                    vis_objects["start_patch"],
                    vis_objects["goal_patch"],
                    vis_objects["current_patch"],
                    vis_objects["path_line"],
                ]
                + vis_objects["visited_patches"]
            )

        state = states_history[frame_idx]
        iteration = state["iteration"]
        current_idx = state["current_idx"]
        current_coords = state["current_coords"]
        open_set_size = state["open_set_size"]

        # Update title with iteration info
        ax.set_title(f"{algorithm_name} Search - Iteration {iteration} | Open Set: {open_set_size}")

        # Clear previous visited node highlights (we'll recreate them)
        for patch in vis_objects["visited_patches"]:
            patch.remove()
        vis_objects["visited_patches"].clear()

        # Update current node highlight
        if current_coords is not None:
            x, y = current_coords
            vis_objects["current_patch"].set_xy((x - 0.5, y - 0.5))
            vis_objects["current_patch"].set_visible(True)
        else:
            vis_objects["current_patch"].set_visible(False)

        # Highlight open set nodes (nodes yet to be explored)
        if "open_set_indices" in state:
            open_indices = state["open_set_indices"]
            width_state = state["width"]

            for idx in open_indices:
                row = idx // width_state
                col = idx % width_state
                # Skip start and goal
                if (row, col) == (start[0], start[1]) or (row, col) == (goal[0], goal[1]):
                    continue

                # Add open set highlight
                open_rect = Rectangle(
                    (col - 0.5, row - 0.5),
                    1.0,
                    1.0,
                    facecolor=OPEN_SET_COLOR,
                    edgecolor=OPEN_SET_EDGE,
                    linewidth=1.5,
                    alpha=0.5,
                )
                ax.add_patch(open_rect)
                vis_objects["visited_patches"].append(open_rect)

        # Highlight closed set nodes (already explored nodes)
        if "closed_indices" in state:
            closed_indices = state["closed_indices"]
            width_state = state["width"]

            for idx in closed_indices:
                row = idx // width_state
                col = idx % width_state
                # Skip start and goal
                if (row, col) == (start[0], start[1]) or (row, col) == (goal[0], goal[1]):
                    continue

                # Add closed set highlight
                closed_rect = Rectangle(
                    (col - 0.5, row - 0.5),
                    1.0,
                    1.0,
                    facecolor=VISITED_COLOR,
                    edgecolor=VISITED_EDGE,
                    linewidth=1.5,
                    alpha=0.5,
                )
                ax.add_patch(closed_rect)
                vis_objects["visited_patches"].append(closed_rect)

        # Update final path if found
        if state.get("path_found", False) and "path" in state:
            final_path = state["path"]
            # Convert path from (x, y) global coords to (row, col) grid coords
            path_rows = final_path[:, 1]  # y coordinates
            path_cols = final_path[:, 0]  # x coordinates
            vis_objects["path_line"].set_data(path_cols, path_rows)

        return (
            vis_objects["cell_patches"]
            + vis_objects["cell_texts"]
            + [
                vis_objects["start_patch"],
                vis_objects["goal_patch"],
                vis_objects["current_patch"],
                vis_objects["path_line"],
            ]
            + vis_objects["visited_patches"]
        )

    return update_frame

def create_update_frame(algorithm_name, states_history): """Create a generic update frame function for search algorithm animation. Parameters ---------- algorithm_name : str Name of the algorithm (e.g., "Dijkstra", "A*") states_history : list List of algorithm states collected via callback Returns ------- function Update function compatible with FuncAnimation """ def update_frame(frame_idx): """Update visualization for a given algorithm state.""" if frame_idx >= len(states_history): return ( vis_objects["cell_patches"] + vis_objects["cell_texts"] + [ vis_objects["start_patch"], vis_objects["goal_patch"], vis_objects["current_patch"], vis_objects["path_line"], ] + vis_objects["visited_patches"] ) state = states_history[frame_idx] iteration = state["iteration"] current_idx = state["current_idx"] current_coords = state["current_coords"] open_set_size = state["open_set_size"] # Update title with iteration info ax.set_title(f"{algorithm_name} Search - Iteration {iteration} | Open Set: {open_set_size}") # Clear previous visited node highlights (we'll recreate them) for patch in vis_objects["visited_patches"]: patch.remove() vis_objects["visited_patches"].clear() # Update current node highlight if current_coords is not None: x, y = current_coords vis_objects["current_patch"].set_xy((x - 0.5, y - 0.5)) vis_objects["current_patch"].set_visible(True) else: vis_objects["current_patch"].set_visible(False) # Highlight open set nodes (nodes yet to be explored) if "open_set_indices" in state: open_indices = state["open_set_indices"] width_state = state["width"] for idx in open_indices: row = idx // width_state col = idx % width_state # Skip start and goal if (row, col) == (start[0], start[1]) or (row, col) == (goal[0], goal[1]): continue # Add open set highlight open_rect = Rectangle( (col - 0.5, row - 0.5), 1.0, 1.0, facecolor=OPEN_SET_COLOR, edgecolor=OPEN_SET_EDGE, linewidth=1.5, alpha=0.5, ) ax.add_patch(open_rect) vis_objects["visited_patches"].append(open_rect) # Highlight closed set nodes (already explored nodes) if "closed_indices" in state: closed_indices = state["closed_indices"] width_state = state["width"] for idx in closed_indices: row = idx // width_state col = idx % width_state # Skip start and goal if (row, col) == (start[0], start[1]) or (row, col) == (goal[0], goal[1]): continue # Add closed set highlight closed_rect = Rectangle( (col - 0.5, row - 0.5), 1.0, 1.0, facecolor=VISITED_COLOR, edgecolor=VISITED_EDGE, linewidth=1.5, alpha=0.5, ) ax.add_patch(closed_rect) vis_objects["visited_patches"].append(closed_rect) # Update final path if found if state.get("path_found", False) and "path" in state: final_path = state["path"] # Convert path from (x, y) global coords to (row, col) grid coords path_rows = final_path[:, 1] # y coordinates path_cols = final_path[:, 0] # x coordinates vis_objects["path_line"].set_data(path_cols, path_rows) return ( vis_objects["cell_patches"] + vis_objects["cell_texts"] + [ vis_objects["start_patch"], vis_objects["goal_patch"], vis_objects["current_patch"], vis_objects["path_line"], ] + vis_objects["visited_patches"] ) return update_frame

Dijkstra's Algorithm¶

Dijkstra's algorithm, developed by Dutch computer scientist Edsger W. Dijkstra in 1956, is a foundational graph search algorithm for solving the single-source shortest path problem on weighted graphs with non-negative edge weights. The algorithm works by systematically expanding nodes in order of their distance from the start node, ensuring that when a node is selected for expansion, the shortest path to that node has already been found.

Key Data Structures¶

Dijkstra maintains three key structures:

1. Priority Queue (Open Set)
   Stores nodes that are waiting to be explored, ordered by their current distance from the start node.

2. Closed Set
   Stores nodes that have been fully processed and will not be expanded again.

3. Predecessor Mapping
   Records the optimal predecessor of each node, used to reconstruct the final shortest path.

Algorithm Process¶

At each iteration:

1. Extract the node with the smallest distance from the priority queue.
2. Examine all of its neighbors.
3. Update neighbors' distances if a shorter path is found through the current node.

This process repeats until the goal is reached or all reachable nodes have been expanded.

Properties¶

• Completeness: Guaranteed to find a path if one exists.
• Optimality: Produces the shortest path in terms of cumulative edge weights.
• Computational Complexity: Using a binary heap, the complexity is (O((V + E) \log V)), where (V) is the number of vertices and (E) the number of edges.
• Search Pattern: Expands uniformly from the start node, forming a circular wavefront. This makes Dijkstra suitable when all paths need consideration, but less efficient for goal-directed search in large state spaces.

Visualization¶

Below is an example of Dijkstra's algorithm applied to a grid-based navigation problem:

• Light blue nodes: Visited nodes showing uniform expansion
• Yellow nodes: Nodes in the priority queue, waiting to be explored
• Path reconstruction: After reaching the goal, the optimal path is reconstructed by backtracking through the predecessor mapping

This visualization demonstrates the systematic exploration and open/closed set mechanism of Dijkstra, making it ideal for teaching the fundamentals of graph search algorithms.

In [7]:

hide_input

def plan_with_dijkstra(graph, start, goal, size, callback=None):
    """Plan a path using Dijkstra's algorithm on a random grid map with optional callback."""
    # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0
    width, height = size  # grid_map shape is (height, width)
    boundary = [0, width, 0, height]

    # Convert start and goal from (row, col) to (x, y) = (col, row) for Dijkstra.plan
    start_xy = np.array([start[1], start[0]])  # (x, y) = (col, row)
    goal_xy = np.array([goal[1], goal[0]])  # (x, y) = (col, row)

    # Run Dijkstra planning with callback
    path = Dijkstra.plan(
        start=start_xy,
        target=goal_xy,
        boundary=np.array(boundary),
        graph=graph,
        grid_resolution=1.0,
        max_iter=100000,
        callback=callback,
    )

    if path is None:
        print("Dijkstra failed to find a path")
        return None
    else:
        print(f"Dijkstra found a path with {len(path)} waypoints")
        return grid_map, start, goal, path, graph

def plan_with_dijkstra(graph, start, goal, size, callback=None): """Plan a path using Dijkstra's algorithm on a random grid map with optional callback.""" # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0 width, height = size # grid_map shape is (height, width) boundary = [0, width, 0, height] # Convert start and goal from (row, col) to (x, y) = (col, row) for Dijkstra.plan start_xy = np.array([start[1], start[0]]) # (x, y) = (col, row) goal_xy = np.array([goal[1], goal[0]]) # (x, y) = (col, row) # Run Dijkstra planning with callback path = Dijkstra.plan( start=start_xy, target=goal_xy, boundary=np.array(boundary), graph=graph, grid_resolution=1.0, max_iter=100000, callback=callback, ) if path is None: print("Dijkstra failed to find a path") return None else: print(f"Dijkstra found a path with {len(path)} waypoints") return grid_map, start, goal, path, graph

In [8]:

hide_input

dijkstra_states_history = []


def dijkstra_callback(state):
    """Callback function to collect Dijkstra algorithm state at each iteration."""
    dijkstra_states_history.append(state)


print("=== Running Dijkstra with state collection ===")
# Use the same graph, start, and goal generated earlier
result = plan_with_dijkstra(
    graph=graph, start=start, goal=goal, size=size, callback=dijkstra_callback
)

dijkstra_states_history = [] def dijkstra_callback(state): """Callback function to collect Dijkstra algorithm state at each iteration.""" dijkstra_states_history.append(state) print("=== Running Dijkstra with state collection ===") # Use the same graph, start, and goal generated earlier result = plan_with_dijkstra( graph=graph, start=start, goal=goal, size=size, callback=dijkstra_callback )

=== Running Dijkstra with state collection ===
Dijkstra found a path with 7 waypoints

In [9]:

hide_input

grid_map, start, goal, path, graph = result
height, width = grid_map.shape

print(f"Collected {len(dijkstra_states_history)} algorithm states")

# Store visualization objects for Dijkstra animation
vis_objects = {
    "cell_patches": [],  # List of Rectangle patches for each cell
    "cell_texts": [],  # List of text objects for cost labels
    "start_patch": None,  # Start cell patch (special)
    "goal_patch": None,  # Goal cell patch (special)
    "current_patch": None,  # Current node highlight patch
    "visited_patches": [],  # Visited nodes highlight patches
    "path_line": None,  # Final path line
}

fig, ax = init_figure(size)

# Create animation for Dijkstra
dijkstra_animation = FuncAnimation(
    fig,
    create_update_frame("Dijkstra", dijkstra_states_history),
    frames=len(dijkstra_states_history),
    init_func=lambda: init_grid(ax, vis_objects, grid_map, start, goal, width, height),
    interval=1000 // fps,  # ms per frame
    blit=False,
    repeat=True,
)

# dijkstra_animation.save(
#     "dijkstra_search_animation.mp4", writer="ffmpeg", fps=fps, dpi=300
# )

# Display animation in Jupyter
display(HTML(dijkstra_animation.to_jshtml()))

grid_map, start, goal, path, graph = result height, width = grid_map.shape print(f"Collected {len(dijkstra_states_history)} algorithm states") # Store visualization objects for Dijkstra animation vis_objects = { "cell_patches": [], # List of Rectangle patches for each cell "cell_texts": [], # List of text objects for cost labels "start_patch": None, # Start cell patch (special) "goal_patch": None, # Goal cell patch (special) "current_patch": None, # Current node highlight patch "visited_patches": [], # Visited nodes highlight patches "path_line": None, # Final path line } fig, ax = init_figure(size) # Create animation for Dijkstra dijkstra_animation = FuncAnimation( fig, create_update_frame("Dijkstra", dijkstra_states_history), frames=len(dijkstra_states_history), init_func=lambda: init_grid(ax, vis_objects, grid_map, start, goal, width, height), interval=1000 // fps, # ms per frame blit=False, repeat=True, ) # dijkstra_animation.save( # "dijkstra_search_animation.mp4", writer="ffmpeg", fps=fps, dpi=300 # ) # Display animation in Jupyter display(HTML(dijkstra_animation.to_jshtml()))

Collected 81 algorithm states

Once Loop Reflect

A* (A-star) Algorithm¶

The A* algorithm, developed by Peter Hart, Nils Nilsson, and Bertram Raphael in 1968, is an extension of Dijkstra's algorithm that uses heuristic guidance to direct the search toward the goal. A* combines the actual cost from the start node (g-score) with an estimated cost to the goal (h-score) to determine the priority of node expansion. The total estimated cost is:

[ f(n) = g(n) + h(n) ]

where (g(n)) is the exact cost from the start to node (n), and (h(n)) is a heuristic estimate of the cost from (n) to the goal.

Importance of the Heuristic Function¶

The choice of heuristic is crucial to A*'s performance:

• Admissible heuristic: Never overestimates the true cost to the goal, ensuring that A* finds optimal paths.
• Common heuristics:
  • Continuous spaces: Euclidean distance
  • Grid worlds: Manhattan distance
• Consistent (monotone) heuristic: Satisfies the triangle inequality
  • When a heuristic is both admissible and consistent, A* is optimally efficient: no other optimal algorithm using the same heuristic will expand fewer nodes.

Comparison with Dijkstra¶

• Dijkstra: Expands nodes uniformly in all directions; search wavefront is circular.
• A*: Heuristic guides the search toward the goal; wavefront is elongated like an ellipse.

Advantages of A*:

• Reduces the search space significantly
• Maintains completeness and optimality
• Achieves higher computational efficiency in practice

Notes:

• The quality of the heuristic directly affects performance
• Poor heuristics $\to$ little improvement over Dijkstra
• Perfect heuristics (exact distance to goal) $\to$ A* follows the optimal path directly without exploring alternatives

With a well-chosen heuristic, A* can solve complex pathfinding problems efficiently, making it a popular choice in robotics, game development, and AI applications.

Visualization¶

In the example visualization:

• A* expands significantly fewer nodes than Dijkstra to reach the same goal
• Search is guided by the heuristic, focusing on the goal direction
• Open set (yellow nodes): nodes prioritized by f-score, waiting to be expanded
• Current node (pink): the most promising node being expanded at each iteration

This demonstration clearly illustrates the heuristic-guided search, the open set mechanism, and how A* efficiently finds the shortest path toward the goal.

In [10]:

hide_input

def plan_with_astar(graph, start, goal, size, callback=None):
    """Plan a path using A* algorithm on a random grid map with optional callback."""
    # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0
    width, height = size  # grid_map shape is (height, width)
    boundary = [0, width, 0, height]

    # Heuristic function (Euclidean distance)
    def heuristic(p1, p2):
        return np.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)

    # Convert start and goal from (row, col) to (x, y) = (col, row) for AStar.plan
    start_xy = np.array([start[1], start[0]])  # (x, y) = (col, row)
    goal_xy = np.array([goal[1], goal[0]])  # (x, y) = (col, row)

    # Run A* planning with callback
    path = AStar.plan(
        start=start_xy,
        target=goal_xy,
        boundary=np.array(boundary),
        graph=graph,
        heuristic_fn=heuristic,
        grid_resolution=1.0,
        max_iter=100000,
        callback=callback,
    )

    if path is None:
        print("A* failed to find a path")
        return None
    else:
        print(f"A* found a path with {len(path)} waypoints")
        return grid_map, start, goal, path, graph

def plan_with_astar(graph, start, goal, size, callback=None): """Plan a path using A* algorithm on a random grid map with optional callback.""" # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0 width, height = size # grid_map shape is (height, width) boundary = [0, width, 0, height] # Heuristic function (Euclidean distance) def heuristic(p1, p2): return np.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2) # Convert start and goal from (row, col) to (x, y) = (col, row) for AStar.plan start_xy = np.array([start[1], start[0]]) # (x, y) = (col, row) goal_xy = np.array([goal[1], goal[0]]) # (x, y) = (col, row) # Run A* planning with callback path = AStar.plan( start=start_xy, target=goal_xy, boundary=np.array(boundary), graph=graph, heuristic_fn=heuristic, grid_resolution=1.0, max_iter=100000, callback=callback, ) if path is None: print("A* failed to find a path") return None else: print(f"A* found a path with {len(path)} waypoints") return grid_map, start, goal, path, graph

In [11]:

hide_input

a_star_states_history = []


def a_star_callback(state):
    """Callback function to collect A* algorithm state at each iteration."""
    a_star_states_history.append(state)


print("=== Running A* with state collection ===")
# Use the graph, start, and goal generated in cell 5
result = plan_with_astar(
    graph=graph, start=start, goal=goal, size=size, callback=a_star_callback
)

a_star_states_history = [] def a_star_callback(state): """Callback function to collect A* algorithm state at each iteration.""" a_star_states_history.append(state) print("=== Running A* with state collection ===") # Use the graph, start, and goal generated in cell 5 result = plan_with_astar( graph=graph, start=start, goal=goal, size=size, callback=a_star_callback )

=== Running A* with state collection ===
A* found a path with 7 waypoints

In [12]:

hide_input

grid_map, start, goal, path, graph = result
height, width = grid_map.shape

print(f"Collected {len(a_star_states_history)} algorithm states")

fig, ax = init_figure(size)

# Store visualization objects for animation updates
vis_objects = {
    "cell_patches": [],  # List of Rectangle patches for each cell
    "cell_texts": [],  # List of text objects for cost labels
    "start_patch": None,  # Start cell patch (special)
    "goal_patch": None,  # Goal cell patch (special)
    "current_patch": None,  # Current node highlight patch
    "visited_patches": [],  # Visited nodes highlight patches
    "path_line": None,  # Final path line
}

a_star_animation = FuncAnimation(
    fig,
    create_update_frame("A*", a_star_states_history),
    frames=len(a_star_states_history),
    init_func=lambda: init_grid(ax, vis_objects, grid_map, start, goal, width, height),
    interval=1000 // fps,  # ms per frame
    blit=False,
    repeat=True,
)

# a_star_animation.save(
#     "a_star_search_animation.mp4", writer="ffmpeg", fps=fps, dpi=300
# )

# Display animation in Jupyter
display(HTML(a_star_animation.to_jshtml()))

grid_map, start, goal, path, graph = result height, width = grid_map.shape print(f"Collected {len(a_star_states_history)} algorithm states") fig, ax = init_figure(size) # Store visualization objects for animation updates vis_objects = { "cell_patches": [], # List of Rectangle patches for each cell "cell_texts": [], # List of text objects for cost labels "start_patch": None, # Start cell patch (special) "goal_patch": None, # Goal cell patch (special) "current_patch": None, # Current node highlight patch "visited_patches": [], # Visited nodes highlight patches "path_line": None, # Final path line } a_star_animation = FuncAnimation( fig, create_update_frame("A*", a_star_states_history), frames=len(a_star_states_history), init_func=lambda: init_grid(ax, vis_objects, grid_map, start, goal, width, height), interval=1000 // fps, # ms per frame blit=False, repeat=True, ) # a_star_animation.save( # "a_star_search_animation.mp4", writer="ffmpeg", fps=fps, dpi=300 # ) # Display animation in Jupyter display(HTML(a_star_animation.to_jshtml()))

Collected 44 algorithm states

Once Loop Reflect

D* (Dynamic A*) Algorithm¶

The D* (Dynamic A*) algorithm, developed by Anthony Stentz in the 1990s, addresses a key limitation of classical search algorithms: their inability to efficiently handle changes in edge costs in dynamic environments.

• Background: When the environment changes, standard A* must replan from scratch. D* efficiently repairs the existing path by propagating cost changes locally, making it especially suitable for real-time robotics or autonomous navigation, where sensors may detect new obstacles during movement.

Algorithm Process¶

D* operates in two phases:

1. Initial Search

   • A forward search similar to A*, planning from the start to the goal and finding an optimal path.

2. Incremental Replanning

   • As the agent moves and discovers cost changes (e.g., new obstacles), D* updates only the affected nodes and propagates the changes through the graph.
   • Usually only local repairs are required, avoiding a full replanning.

Core Innovation: rhs-value (right-hand side)¶

• The rhs-value represents a one-step lookahead estimate of the optimal cost.
• Using rhs, D* can quickly identify which nodes are affected by cost changes.
• When an edge cost increases (e.g., an obstacle appears), D* updates affected nodes and propagates changes, achieving efficient path repair.

Node States and Consistency¶

• Each node maintains two values:
  1. g-value: the actual cost from the start
  2. rhs-value: estimated cost based on successors
• Consistent node: g = rhs
• Inconsistent node: g ≠ rhs (overconsistent or underconsistent)
• D* focuses repair efforts on inconsistent nodes, efficiently restoring optimality while minimizing computational overhead.

This strategy makes D* particularly effective in partially known environments, where the agent must adapt to newly discovered information.

Relation between D* and D* Lite¶

• D* Lite is a simplified variant of D*
• Features:
  • Maintains similar performance while simplifying the original algorithm
  • Reverses the search direction, planning from goal to start
  • Easier updates when the start position changes as the agent moves

In practice, D* Lite is often preferred for its simplicity and efficiency, while retaining the core benefits of dynamic replanning in changing environments. In our implementation, we deploy D* Lite for dynamic pathfinding tasks.

In [13]:

hide_input

# TODO: Demo for dynamic map

# TODO: Demo for dynamic map

Hybrid A* Algorithm¶

Hybrid A*** represents a significant advancement in search-based planning by **bridging the gap between discrete graph search and continuous motion planning. Developed in the mid-2000s for autonomous vehicle navigation, Hybrid A* extends classical A* to handle continuous state spaces and vehicle kinematics constraints that standard grid-based planners cannot accommodate.

Core Challenge¶

Hybrid A* addresses the curse of dimensionality in continuous configuration spaces:

• Standard A* operates on discrete grids
• Real vehicles (e.g., cars) have continuous states (position $x, y$, heading $\theta$, velocity, etc.)
• Vehicles are subject to non-holonomic constraints and cannot move sideways

Solution: Hybrid A* discretizes the search space coarsely while maintaining continuous state representations during node expansion. Each node stores continuous state information $(x,y,\theta)$ and motion primitives are used to generate new states that respect vehicle kinematics.

Algorithm Details¶

1. Cost Function

   • Similar to standard A, Hybrid A maintains two cost components:
     • g(n): actual cost from start to the current node
     • h(n): heuristic estimate to the goal

2. Dual Heuristic Approach

   • Non-holonomic heuristic: accounts for vehicle constraints, often computed with Reeds-Shepp curves
   • Holonomic heuristic: standard Euclidean distance, provides an optimistic estimate
   • Node priority is determined by g(n) + max(non-holonomic heuristic, holonomic heuristic)

3. Continuous Trajectory Generation

   • Unlike grid-based A* which moves between adjacent cells, Hybrid A* generates continuous feasible trajectories using motion primitives
   • Primitives include straight lines, curves of varying radii, and reverse motions
   • Analytic expansions: attempt to directly connect the current node to the goal using feasible paths like Dubins or Reeds-Shepp curves, significantly improving convergence

Features and Advantages¶

• Resolution-complete: finds a solution at the chosen discretization level if one exists
• Kinematically feasible paths: resulting paths respect vehicle dynamics
• Applications:
  • Complex maneuvers for autonomous vehicles, e.g., parallel parking, three-point turns, navigating tight spaces
  • Hybrid A* has become a cornerstone algorithm for modern vehicle motion planning

Highlights¶

1. Clear structure: Concept $\to$ Core Challenge $\to$ Algorithm Details $\to$ Features & Applications
2. Intuitive technical terms: continuous states, non-holonomic constraints, motion primitives, analytic expansions
3. Teaching-friendly: emphasizes **improvements over grid A***, including continuous trajectories, dual heuristics, and vehicle kinematics