Sampling-based Planning Algorithms¶

Overview¶

Sampling-based planning algorithms represent a complementary paradigm to graph-based methods for motion planning in continuous spaces. Instead of discretizing the entire state space into a predefined grid or graph, sampling-based planners operate directly in the continuous configuration space** by randomly sampling states and incrementally constructing connectivity structures.

In this paradigm, nodes correspond to sampled configurations in continuous space, and edges represent feasible local motions that satisfy collision constraints and system dynamics. Sampling-based algorithms are particularly effective for high-dimensional planning problems, where uniform discretization becomes computationally intractable.

Core Characteristics of Sampling-based Planning¶

1. Randomized Exploration
   Exploration is driven by random sampling rather than fixed connectivity, allowing the planner to efficiently discover feasible regions in large or complex spaces.

2. Probabilistic Guarantees
   Sampling-based algorithms are typically probabilistically complete: if a solution exists, the probability of finding it approaches 1 as the number of samples increases.

3. Continuous State Space
   Planning is performed directly in continuous configuration space, avoiding explicit grid discretization.

4. Incremental Construction
   Connectivity structures such as trees (RRT) or roadmaps (PRM) are built incrementally as sampling proceeds.

5. Resolution Independence
   Solution quality is not tied to a predefined grid resolution, but instead improves gradually with increased sampling density.

When to use sampling-based planning:

• State space is high-dimensional or continuous
• Explicit discretization is impractical
• A feasible path is sufficient, or optimality can be refined over time
• Robot dynamics or kinematic constraints are complex

Historical Evolution¶

Sampling-based motion planning emerged in the mid-to-late 1990s as a response to the limitations of classical graph-based planners in high-dimensional configuration spaces.

One of the earliest influential approaches was the Probabilistic Roadmap Method (PRM), introduced in the mid-1990s. PRM demonstrated that random sampling could effectively capture the global connectivity of complex spaces and support efficient multi-query planning.

In 1998, Rapidly-exploring Random Trees (RRT) were introduced by Steven M. LaValle. RRT shifted the focus toward single-query planning, emphasizing rapid exploration of unexplored regions and fast discovery of feasible paths. This made RRT particularly suitable for real-time robotic applications.

To further improve practical performance, RRT-Connect was proposed in 2000. By growing trees simultaneously from both the start and goal configurations, RRT-Connect significantly reduced planning time in many high-dimensional problems.

A major theoretical advancement occurred in 2011 with the introduction of RRT* by Karaman and Frazzoli. RRT* addressed the lack of optimality in earlier sampling-based planners, proving that the solution converges to the optimal path as the number of samples increases.

Together, these algorithms illustrate the evolution of sampling-based planning from fast heuristic methods to planners with strong theoretical guarantees and broad applicability in robotics and autonomous systems.

Algorithm Summary¶

Algorithm	Year	Search Type	Strategy	State Space	Dynamic	Optimality	Use Case
RRT	1998	Sampling-based	Random sampling	Continuous	Partial	No	Fast feasible planning
RRT*	2011	Sampling-based	Random sampling	Continuous	Partial	Asymptotically optimal	Optimal motion planning
RRT-Connect	2000	Sampling-based	Bi-directional	Continuous	Partial	No	High-dimensional planning
PRM	1996	Sampling-based	Random roadmap	Continuous	No	Probabilistically complete	Multi-query planning

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 shapely.geometry import LineString, Polygon

from tactics2d.search import RRT, RRTStar, RRTConnect, PRM
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 shapely.geometry import LineString, Polygon from tactics2d.search import RRT, RRTStar, RRTConnect, PRM 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

def extract_obstacles_from_grid(grid_map):
    """Extract obstacle coordinates from grid map.

    Args:
        grid_map: 2D numpy array with obstacles marked as np.inf

    Returns:
        list: List of (x, y) obstacle cell centers where x=col, y=row
    """
    height, width = grid_map.shape
    obstacles = []
    for row in range(height):
        for col in range(width):
            if np.isinf(grid_map[row, col]):
                obstacles.append((col, row))
    return obstacles


def create_grid_collision_checker(obstacles):
    """Create a collision checking function for grid obstacles.

    Args:
        obstacles: List of (x, y) obstacle cell centers

    Returns:
        function: collide_fn(p1, p2, obstacles) that returns True if line segment
                  from p1 to p2 intersects any obstacle unit square.
    """

    def collide_fn(p1, p2, obstacles):
        # p1, p2: (x, y) coordinates
        # obstacles: list of (x, y) obstacle cell centers
        # Each obstacle is a unit square centered at (x, y) with side length 1

        line = LineString([p1, p2])
        for ox, oy in obstacles:
            # Create obstacle polygon (unit square centered at (ox, oy))
            obstacle_poly = Polygon(
                [
                    (ox - 0.5, oy - 0.5),
                    (ox + 0.5, oy - 0.5),
                    (ox + 0.5, oy + 0.5),
                    (ox - 0.5, oy + 0.5),
                ]
            )
            if line.intersects(obstacle_poly):
                return True
        return False

    return collide_fn

def extract_obstacles_from_grid(grid_map): """Extract obstacle coordinates from grid map. Args: grid_map: 2D numpy array with obstacles marked as np.inf Returns: list: List of (x, y) obstacle cell centers where x=col, y=row """ height, width = grid_map.shape obstacles = [] for row in range(height): for col in range(width): if np.isinf(grid_map[row, col]): obstacles.append((col, row)) return obstacles def create_grid_collision_checker(obstacles): """Create a collision checking function for grid obstacles. Args: obstacles: List of (x, y) obstacle cell centers Returns: function: collide_fn(p1, p2, obstacles) that returns True if line segment from p1 to p2 intersects any obstacle unit square. """ def collide_fn(p1, p2, obstacles): # p1, p2: (x, y) coordinates # obstacles: list of (x, y) obstacle cell centers # Each obstacle is a unit square centered at (x, y) with side length 1 line = LineString([p1, p2]) for ox, oy in obstacles: # Create obstacle polygon (unit square centered at (ox, oy)) obstacle_poly = Polygon( [ (ox - 0.5, oy - 0.5), (ox + 0.5, oy - 0.5), (ox + 0.5, oy + 0.5), (ox - 0.5, oy + 0.5), ] ) if line.intersects(obstacle_poly): return True return False return collide_fn

In [3]:

hide_input

def grid_to_continuous_coords(grid_coords):
    """Convert grid coordinates (row, col) to continuous coordinates (x, y).

    Args:
        grid_coords: Tuple (row, col) or list [row, col]

    Returns:
        tuple: (x, y) where x = col, y = row
    """
    row, col = grid_coords
    return (col, row)


def continuous_to_grid_coords(continuous_coords):
    """Convert continuous coordinates (x, y) to grid coordinates (row, col).

    Args:
        continuous_coords: Tuple (x, y) or list [x, y]

    Returns:
        tuple: (row, col) where row = int(round(y)), col = int(round(x))
    """
    x, y = continuous_coords
    return (int(round(y)), int(round(x)))

def grid_to_continuous_coords(grid_coords): """Convert grid coordinates (row, col) to continuous coordinates (x, y). Args: grid_coords: Tuple (row, col) or list [row, col] Returns: tuple: (x, y) where x = col, y = row """ row, col = grid_coords return (col, row) def continuous_to_grid_coords(continuous_coords): """Convert continuous coordinates (x, y) to grid coordinates (row, col). Args: continuous_coords: Tuple (x, y) or list [x, y] Returns: tuple: (row, col) where row = int(round(y)), col = int(round(x)) """ x, y = continuous_coords return (int(round(y)), int(round(x)))

In [4]:

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}")

# For sampling-based algorithms, we need obstacle information from grid map
# Use shared function to extract obstacles
obstacles = extract_obstacles_from_grid(grid_map)
print(f"Number of obstacles: {len(obstacles)}")

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}") # For sampling-based algorithms, we need obstacle information from grid map # Use shared function to extract obstacles obstacles = extract_obstacles_from_grid(grid_map) print(f"Number of obstacles: {len(obstacles)}")

Grid map shape: (10, 10)
Start: (3, 5), Goal: (0, 1)
Number of obstacles: 10

In [5]:

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 [6]:

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 [7]:

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 [8]:

hide_input

def create_sampling_update_frame(algorithm_name, states_history):
    """Create a generic update frame function for sampling-based algorithm animation.

    Parameters
    ----------
    algorithm_name : str
        Name of the algorithm (e.g., "RRT", "RRT*", "RRT-Connect", "PRM")
    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 sampling algorithm state."""
        # Access global variables needed for visualization
        global width, height, start, goal, grid_map

        if frame_idx >= len(states_history):
            # Return all artists to maintain display
            artists = (
                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"]
            )
            # Add tree/roadmap-specific artists if they exist
            if "tree_lines" in vis_objects:
                artists += vis_objects["tree_lines"]
            if "tree_nodes" in vis_objects:
                artists += vis_objects["tree_nodes"]
            if "tree_lines_a" in vis_objects:
                artists += vis_objects["tree_lines_a"]
            if "tree_nodes_a" in vis_objects:
                artists += vis_objects["tree_nodes_a"]
            if "tree_lines_b" in vis_objects:
                artists += vis_objects["tree_lines_b"]
            if "tree_nodes_b" in vis_objects:
                artists += vis_objects["tree_nodes_b"]
            if "roadmap_nodes" in vis_objects:
                artists += vis_objects["roadmap_nodes"]
            if "roadmap_edges" in vis_objects:
                artists += vis_objects["roadmap_edges"]
            if (
                "random_point_marker" in vis_objects
                and vis_objects["random_point_marker"] is not None
            ):
                artists.append(vis_objects["random_point_marker"])
            return artists

        state = states_history[frame_idx]
        iteration = state["iteration"]

        # Determine algorithm type based on state keys
        is_rrt_connect = "tree_a" in state and "tree_b" in state
        is_rrt_star = "node_costs" in state  # RRT* includes node costs
        is_prm = "phase" in state  # PRM has phase field

        # Update title based on algorithm type
        if is_prm:
            phase = state["phase"]
            n_nodes = state.get("n_nodes", 0)
            n_edges = state.get("n_edges", 0)
            ax.set_title(
                f"{algorithm_name} ({phase}) - Iteration {iteration} | Nodes: {n_nodes}, Edges: {n_edges}"
            )
        elif is_rrt_connect:
            tree_a_size = state["tree_a_size"]
            tree_b_size = state["tree_b_size"]
            ax.set_title(
                f"{algorithm_name} Search - Iteration {iteration} | Tree A: {tree_a_size}, Tree B: {tree_b_size}"
            )
        else:
            tree_size = state.get("tree_size", 0)
            ax.set_title(
                f"{algorithm_name} Search - Iteration {iteration} | Tree Size: {tree_size}"
            )

        # Clear previous visualization
        # Single tree case
        if "tree_lines" in vis_objects:
            for line in vis_objects["tree_lines"]:
                line.remove()
            vis_objects["tree_lines"].clear()
        if "tree_nodes" in vis_objects:
            for node in vis_objects["tree_nodes"]:
                node.remove()
            vis_objects["tree_nodes"].clear()

        # Dual tree case (RRT-Connect)
        if "tree_lines_a" in vis_objects:
            for line in vis_objects["tree_lines_a"]:
                line.remove()
            vis_objects["tree_lines_a"].clear()
        if "tree_nodes_a" in vis_objects:
            for node in vis_objects["tree_nodes_a"]:
                node.remove()
            vis_objects["tree_nodes_a"].clear()
        if "tree_lines_b" in vis_objects:
            for line in vis_objects["tree_lines_b"]:
                line.remove()
            vis_objects["tree_lines_b"].clear()
        if "tree_nodes_b" in vis_objects:
            for node in vis_objects["tree_nodes_b"]:
                node.remove()
            vis_objects["tree_nodes_b"].clear()

        # PRM roadmap visualization
        if "roadmap_nodes" in vis_objects:
            for node in vis_objects["roadmap_nodes"]:
                node.remove()
            vis_objects["roadmap_nodes"].clear()
        if "roadmap_edges" in vis_objects:
            for edge in vis_objects["roadmap_edges"]:
                edge.remove()
            vis_objects["roadmap_edges"].clear()

        # Clear visited patches (grid cells with tree/roadmap nodes)
        for patch in vis_objects["visited_patches"]:
            patch.remove()
        vis_objects["visited_patches"].clear()

        # Clear random point marker
        if vis_objects.get("random_point_marker") is not None:
            vis_objects["random_point_marker"].remove()
            vis_objects["random_point_marker"] = None

        # Track visited grid cells to avoid duplicates
        visited_cells = set()

        # Helper function to visualize a single tree (for RRT/RRT*/RRT-Connect)
        def visualize_tree(
            tree, tree_lines_list, tree_nodes_list, node_color="blue", edge_color="gray"
        ):
            """Visualize a tree and mark visited grid cells."""
            for i, node in enumerate(tree):
                # Handle different node formats: RRT/RRT-Connect: (x, y, parent_idx), RRT*: (x, y, parent_idx, cost)
                x, y = node[0], node[1]
                parent_idx = node[2]

                # Convert continuous coordinates to grid cell indices
                col = int(round(x))
                row = int(round(y))

                # Ensure coordinates are within grid bounds
                if 0 <= col < width and 0 <= row < height:
                    cell_key = (row, col)

                    # Skip start and goal cells (they have special highlighting)
                    if (row, col) != (start[0], start[1]) and (row, col) != (goal[0], goal[1]):
                        visited_cells.add(cell_key)

                # Plot tree node
                node_circle = plt.Circle((x, y), radius=0.1, color=node_color, alpha=0.6, zorder=5)
                ax.add_patch(node_circle)
                tree_nodes_list.append(node_circle)

                # Plot tree edge to parent (if not root)
                if parent_idx is not None and parent_idx < len(tree):
                    parent = tree[parent_idx]
                    px, py = parent[0], parent[1]
                    (edge_line,) = ax.plot(
                        [px, x], [py, y], color=edge_color, linewidth=1, alpha=0.4, zorder=4
                    )
                    tree_lines_list.append(edge_line)

        # Helper function to visualize PRM roadmap
        def visualize_prm_roadmap(
            nodes, edges, node_color="purple", edge_color="lightgray", visited_cells_set=None
        ):
            """Visualize PRM roadmap (nodes and edges)."""
            # Ensure visualization lists exist
            if "roadmap_nodes" not in vis_objects:
                vis_objects["roadmap_nodes"] = []
            if "roadmap_edges" not in vis_objects:
                vis_objects["roadmap_edges"] = []

            # Visualize nodes
            for i, node in enumerate(nodes):
                x, y = node

                # Convert continuous coordinates to grid cell indices
                col = int(round(x))
                row = int(round(y))

                # Ensure coordinates are within grid bounds
                if 0 <= col < width and 0 <= row < height:
                    cell_key = (row, col)

                    # Skip start and goal cells (they have special highlighting)
                    if (row, col) != (start[0], start[1]) and (row, col) != (goal[0], goal[1]):
                        if visited_cells_set is not None:
                            visited_cells_set.add(cell_key)

                # Plot roadmap node (smaller than tree nodes for clarity)
                node_circle = plt.Circle((x, y), radius=0.08, color=node_color, alpha=0.6, zorder=5)
                ax.add_patch(node_circle)
                vis_objects["roadmap_nodes"].append(node_circle)

            # Visualize edges
            for edge in edges:
                # Edge format: (i, j, cost) or (i, j) depending on state
                if len(edge) >= 2:
                    i, j = edge[0], edge[1]
                    if i < len(nodes) and j < len(nodes):
                        x1, y1 = nodes[i]
                        x2, y2 = nodes[j]
                        (edge_line,) = ax.plot(
                            [x1, x2], [y1, y2], color=edge_color, linewidth=0.8, alpha=0.3, zorder=4
                        )
                        vis_objects["roadmap_edges"].append(edge_line)

        # Visualize based on algorithm type
        if is_prm:
            # PRM: visualize roadmap
            nodes = state.get("nodes", [])
            edges = state.get("edges", [])

            # For PRM, we visualize the current roadmap state
            visualize_prm_roadmap(nodes, edges, visited_cells_set=visited_cells)

            # Highlight current sample or current node
            current_sample = state.get("current_sample")
            current_node = state.get("current_node")

            if current_sample is not None:
                # In sampling phase: show current sample point
                sx, sy = current_sample
                sample_marker = plt.Circle(
                    (sx, sy), radius=0.12, color="orange", alpha=0.8, zorder=6
                )
                ax.add_patch(sample_marker)
                vis_objects["roadmap_nodes"].append(sample_marker)

            if current_node is not None:
                # In connecting or searching phase: show current node
                cx, cy = current_node
                current_node_marker = plt.Circle(
                    (cx, cy), radius=0.15, color="red", alpha=0.8, zorder=7
                )
                ax.add_patch(current_node_marker)
                vis_objects["roadmap_nodes"].append(current_node_marker)

                # Update current patch position (grid alignment)
                col = int(round(cx))
                row = int(round(cy))
                if 0 <= col < width and 0 <= row < height:
                    vis_objects["current_patch"].set_xy((col - 0.5, row - 0.5))
                    vis_objects["current_patch"].set_visible(True)
                else:
                    vis_objects["current_patch"].set_visible(False)
            else:
                vis_objects["current_patch"].set_visible(False)

        elif is_rrt_connect:
            # RRT-Connect: two trees with different colors
            tree_a = state["tree_a"]
            tree_b = state["tree_b"]

            # Ensure visualization lists exist
            if "tree_lines_a" not in vis_objects:
                vis_objects["tree_lines_a"] = []
            if "tree_nodes_a" not in vis_objects:
                vis_objects["tree_nodes_a"] = []
            if "tree_lines_b" not in vis_objects:
                vis_objects["tree_lines_b"] = []
            if "tree_nodes_b" not in vis_objects:
                vis_objects["tree_nodes_b"] = []

            # Tree A (from start): blue, Tree B (from goal): green
            visualize_tree(
                tree_a,
                vis_objects["tree_lines_a"],
                vis_objects["tree_nodes_a"],
                "blue",
                "lightblue",
            )
            visualize_tree(
                tree_b,
                vis_objects["tree_lines_b"],
                vis_objects["tree_nodes_b"],
                "green",
                "lightgreen",
            )
        else:
            # Single tree (RRT or RRT*)
            tree = state["tree"]

            # Ensure visualization lists exist
            if "tree_lines" not in vis_objects:
                vis_objects["tree_lines"] = []
            if "tree_nodes" not in vis_objects:
                vis_objects["tree_nodes"] = []

            visualize_tree(tree, vis_objects["tree_lines"], vis_objects["tree_nodes"])

        # Highlight visited grid cells (similar to graph search algorithms)
        for row, col in visited_cells:
            # Skip obstacles (they're already marked with obstacle color)
            if not np.isinf(grid_map[row, col]):
                visited_rect = Rectangle(
                    (col - 0.5, row - 0.5),
                    1.0,
                    1.0,
                    facecolor=VISITED_COLOR,
                    edgecolor=VISITED_EDGE,
                    linewidth=1.5,
                    alpha=0.5,
                    zorder=3,  # Below tree nodes but above grid background
                )
                ax.add_patch(visited_rect)
                vis_objects["visited_patches"].append(visited_rect)

        # For RRT/RRT*/RRT-Connect: highlight new node and edge to parent
        if not is_prm:
            new_node = state.get("new_node")
            parent_node = state.get("parent_node")

            if new_node is not None:
                nx, ny = new_node

                # Determine color based on algorithm and extending tree
                if is_rrt_connect:
                    extending_tree = state.get("extending_tree", None)
                    if extending_tree == "a":
                        new_node_color = "darkblue"
                        new_edge_color = "blue"
                    elif extending_tree == "b":
                        new_node_color = "darkgreen"
                        new_edge_color = "green"
                    else:
                        new_node_color = "red"
                        new_edge_color = "red"
                else:
                    new_node_color = "red"
                    new_edge_color = "red"

                # Highlight new node
                new_node_circle = plt.Circle(
                    (nx, ny), radius=0.15, color=new_node_color, alpha=0.8, zorder=6
                )
                ax.add_patch(new_node_circle)

                # Add to appropriate tree's node list for cleanup
                if is_rrt_connect:
                    extending_tree = state.get("extending_tree", None)
                    if extending_tree == "a":
                        vis_objects["tree_nodes_a"].append(new_node_circle)
                    elif extending_tree == "b":
                        vis_objects["tree_nodes_b"].append(new_node_circle)
                    else:
                        # Fallback: add to both
                        vis_objects["tree_nodes_a"].append(new_node_circle)
                else:
                    vis_objects["tree_nodes"].append(new_node_circle)

                # Highlight edge to parent
                if parent_node is not None:
                    px, py = parent_node
                    (new_edge_line,) = ax.plot(
                        [px, nx], [py, ny], color=new_edge_color, linewidth=2, alpha=0.8, zorder=5
                    )
                    # Add to appropriate tree's line list
                    if is_rrt_connect:
                        extending_tree = state.get("extending_tree", None)
                        if extending_tree == "a":
                            vis_objects["tree_lines_a"].append(new_edge_line)
                        elif extending_tree == "b":
                            vis_objects["tree_lines_b"].append(new_edge_line)
                        else:
                            vis_objects["tree_lines_a"].append(new_edge_line)
                    else:
                        vis_objects["tree_lines"].append(new_edge_line)

            # Show connection line to nearest node in other tree (for RRT-Connect when path is found)
            if is_rrt_connect and state.get("to_nearest_node") is not None and new_node is not None:
                tx, ty = state["to_nearest_node"]
                nx, ny = new_node
                (connect_line,) = ax.plot(
                    [nx, tx],
                    [ny, ty],
                    color="purple",
                    linewidth=3,
                    alpha=0.9,
                    linestyle="--",
                    zorder=5,
                )
                # Add to appropriate list for cleanup
                vis_objects["tree_lines_a"].append(connect_line)

            # Show random sampling point (for RRT algorithms)
            random_point = state.get("random_point")
            if random_point is not None:
                rx, ry = random_point
                random_marker = plt.Circle(
                    (rx, ry), radius=0.08, color="orange", alpha=0.7, zorder=7
                )
                ax.add_patch(random_marker)
                vis_objects["random_point_marker"] = random_marker

            # Update current node patch (for consistency with other visualizations)
            # Align with grid by rounding to nearest integer coordinates
            if new_node is not None:
                nx, ny = new_node
                # Round to nearest grid cell
                col = int(round(nx))
                row = int(round(ny))
                # Ensure within bounds
                if 0 <= col < width and 0 <= row < height:
                    vis_objects["current_patch"].set_xy((col - 0.5, row - 0.5))
                    vis_objects["current_patch"].set_visible(True)
                else:
                    vis_objects["current_patch"].set_visible(False)
            else:
                vis_objects["current_patch"].set_visible(False)

        # Update final path if found (common for all algorithms)
        if state.get("path_found", False) and "path" in state:
            path = state["path"]
            if len(path) > 0:
                # Convert path from list of [x, y] to arrays
                path_array = np.array(path)
                # Note: path coordinates are already in (x, y) format
                path_cols = path_array[:, 0]
                path_rows = path_array[:, 1]
                vis_objects["path_line"].set_data(path_cols, path_rows)

        # Return all artists for blitting
        artists = (
            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"]
        )

        # Add tree/roadmap-specific artists
        if "tree_lines" in vis_objects:
            artists += vis_objects["tree_lines"]
        if "tree_nodes" in vis_objects:
            artists += vis_objects["tree_nodes"]
        if "tree_lines_a" in vis_objects:
            artists += vis_objects["tree_lines_a"]
        if "tree_nodes_a" in vis_objects:
            artists += vis_objects["tree_nodes_a"]
        if "tree_lines_b" in vis_objects:
            artists += vis_objects["tree_lines_b"]
        if "tree_nodes_b" in vis_objects:
            artists += vis_objects["tree_nodes_b"]
        if "roadmap_nodes" in vis_objects:
            artists += vis_objects["roadmap_nodes"]
        if "roadmap_edges" in vis_objects:
            artists += vis_objects["roadmap_edges"]
        if "random_point_marker" in vis_objects and vis_objects["random_point_marker"] is not None:
            artists.append(vis_objects["random_point_marker"])

        return artists

    return update_frame

def create_sampling_update_frame(algorithm_name, states_history): """Create a generic update frame function for sampling-based algorithm animation. Parameters ---------- algorithm_name : str Name of the algorithm (e.g., "RRT", "RRT*", "RRT-Connect", "PRM") 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 sampling algorithm state.""" # Access global variables needed for visualization global width, height, start, goal, grid_map if frame_idx >= len(states_history): # Return all artists to maintain display artists = ( 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"] ) # Add tree/roadmap-specific artists if they exist if "tree_lines" in vis_objects: artists += vis_objects["tree_lines"] if "tree_nodes" in vis_objects: artists += vis_objects["tree_nodes"] if "tree_lines_a" in vis_objects: artists += vis_objects["tree_lines_a"] if "tree_nodes_a" in vis_objects: artists += vis_objects["tree_nodes_a"] if "tree_lines_b" in vis_objects: artists += vis_objects["tree_lines_b"] if "tree_nodes_b" in vis_objects: artists += vis_objects["tree_nodes_b"] if "roadmap_nodes" in vis_objects: artists += vis_objects["roadmap_nodes"] if "roadmap_edges" in vis_objects: artists += vis_objects["roadmap_edges"] if ( "random_point_marker" in vis_objects and vis_objects["random_point_marker"] is not None ): artists.append(vis_objects["random_point_marker"]) return artists state = states_history[frame_idx] iteration = state["iteration"] # Determine algorithm type based on state keys is_rrt_connect = "tree_a" in state and "tree_b" in state is_rrt_star = "node_costs" in state # RRT* includes node costs is_prm = "phase" in state # PRM has phase field # Update title based on algorithm type if is_prm: phase = state["phase"] n_nodes = state.get("n_nodes", 0) n_edges = state.get("n_edges", 0) ax.set_title( f"{algorithm_name} ({phase}) - Iteration {iteration} | Nodes: {n_nodes}, Edges: {n_edges}" ) elif is_rrt_connect: tree_a_size = state["tree_a_size"] tree_b_size = state["tree_b_size"] ax.set_title( f"{algorithm_name} Search - Iteration {iteration} | Tree A: {tree_a_size}, Tree B: {tree_b_size}" ) else: tree_size = state.get("tree_size", 0) ax.set_title( f"{algorithm_name} Search - Iteration {iteration} | Tree Size: {tree_size}" ) # Clear previous visualization # Single tree case if "tree_lines" in vis_objects: for line in vis_objects["tree_lines"]: line.remove() vis_objects["tree_lines"].clear() if "tree_nodes" in vis_objects: for node in vis_objects["tree_nodes"]: node.remove() vis_objects["tree_nodes"].clear() # Dual tree case (RRT-Connect) if "tree_lines_a" in vis_objects: for line in vis_objects["tree_lines_a"]: line.remove() vis_objects["tree_lines_a"].clear() if "tree_nodes_a" in vis_objects: for node in vis_objects["tree_nodes_a"]: node.remove() vis_objects["tree_nodes_a"].clear() if "tree_lines_b" in vis_objects: for line in vis_objects["tree_lines_b"]: line.remove() vis_objects["tree_lines_b"].clear() if "tree_nodes_b" in vis_objects: for node in vis_objects["tree_nodes_b"]: node.remove() vis_objects["tree_nodes_b"].clear() # PRM roadmap visualization if "roadmap_nodes" in vis_objects: for node in vis_objects["roadmap_nodes"]: node.remove() vis_objects["roadmap_nodes"].clear() if "roadmap_edges" in vis_objects: for edge in vis_objects["roadmap_edges"]: edge.remove() vis_objects["roadmap_edges"].clear() # Clear visited patches (grid cells with tree/roadmap nodes) for patch in vis_objects["visited_patches"]: patch.remove() vis_objects["visited_patches"].clear() # Clear random point marker if vis_objects.get("random_point_marker") is not None: vis_objects["random_point_marker"].remove() vis_objects["random_point_marker"] = None # Track visited grid cells to avoid duplicates visited_cells = set() # Helper function to visualize a single tree (for RRT/RRT*/RRT-Connect) def visualize_tree( tree, tree_lines_list, tree_nodes_list, node_color="blue", edge_color="gray" ): """Visualize a tree and mark visited grid cells.""" for i, node in enumerate(tree): # Handle different node formats: RRT/RRT-Connect: (x, y, parent_idx), RRT*: (x, y, parent_idx, cost) x, y = node[0], node[1] parent_idx = node[2] # Convert continuous coordinates to grid cell indices col = int(round(x)) row = int(round(y)) # Ensure coordinates are within grid bounds if 0 <= col < width and 0 <= row < height: cell_key = (row, col) # Skip start and goal cells (they have special highlighting) if (row, col) != (start[0], start[1]) and (row, col) != (goal[0], goal[1]): visited_cells.add(cell_key) # Plot tree node node_circle = plt.Circle((x, y), radius=0.1, color=node_color, alpha=0.6, zorder=5) ax.add_patch(node_circle) tree_nodes_list.append(node_circle) # Plot tree edge to parent (if not root) if parent_idx is not None and parent_idx < len(tree): parent = tree[parent_idx] px, py = parent[0], parent[1] (edge_line,) = ax.plot( [px, x], [py, y], color=edge_color, linewidth=1, alpha=0.4, zorder=4 ) tree_lines_list.append(edge_line) # Helper function to visualize PRM roadmap def visualize_prm_roadmap( nodes, edges, node_color="purple", edge_color="lightgray", visited_cells_set=None ): """Visualize PRM roadmap (nodes and edges).""" # Ensure visualization lists exist if "roadmap_nodes" not in vis_objects: vis_objects["roadmap_nodes"] = [] if "roadmap_edges" not in vis_objects: vis_objects["roadmap_edges"] = [] # Visualize nodes for i, node in enumerate(nodes): x, y = node # Convert continuous coordinates to grid cell indices col = int(round(x)) row = int(round(y)) # Ensure coordinates are within grid bounds if 0 <= col < width and 0 <= row < height: cell_key = (row, col) # Skip start and goal cells (they have special highlighting) if (row, col) != (start[0], start[1]) and (row, col) != (goal[0], goal[1]): if visited_cells_set is not None: visited_cells_set.add(cell_key) # Plot roadmap node (smaller than tree nodes for clarity) node_circle = plt.Circle((x, y), radius=0.08, color=node_color, alpha=0.6, zorder=5) ax.add_patch(node_circle) vis_objects["roadmap_nodes"].append(node_circle) # Visualize edges for edge in edges: # Edge format: (i, j, cost) or (i, j) depending on state if len(edge) >= 2: i, j = edge[0], edge[1] if i < len(nodes) and j < len(nodes): x1, y1 = nodes[i] x2, y2 = nodes[j] (edge_line,) = ax.plot( [x1, x2], [y1, y2], color=edge_color, linewidth=0.8, alpha=0.3, zorder=4 ) vis_objects["roadmap_edges"].append(edge_line) # Visualize based on algorithm type if is_prm: # PRM: visualize roadmap nodes = state.get("nodes", []) edges = state.get("edges", []) # For PRM, we visualize the current roadmap state visualize_prm_roadmap(nodes, edges, visited_cells_set=visited_cells) # Highlight current sample or current node current_sample = state.get("current_sample") current_node = state.get("current_node") if current_sample is not None: # In sampling phase: show current sample point sx, sy = current_sample sample_marker = plt.Circle( (sx, sy), radius=0.12, color="orange", alpha=0.8, zorder=6 ) ax.add_patch(sample_marker) vis_objects["roadmap_nodes"].append(sample_marker) if current_node is not None: # In connecting or searching phase: show current node cx, cy = current_node current_node_marker = plt.Circle( (cx, cy), radius=0.15, color="red", alpha=0.8, zorder=7 ) ax.add_patch(current_node_marker) vis_objects["roadmap_nodes"].append(current_node_marker) # Update current patch position (grid alignment) col = int(round(cx)) row = int(round(cy)) if 0 <= col < width and 0 <= row < height: vis_objects["current_patch"].set_xy((col - 0.5, row - 0.5)) vis_objects["current_patch"].set_visible(True) else: vis_objects["current_patch"].set_visible(False) else: vis_objects["current_patch"].set_visible(False) elif is_rrt_connect: # RRT-Connect: two trees with different colors tree_a = state["tree_a"] tree_b = state["tree_b"] # Ensure visualization lists exist if "tree_lines_a" not in vis_objects: vis_objects["tree_lines_a"] = [] if "tree_nodes_a" not in vis_objects: vis_objects["tree_nodes_a"] = [] if "tree_lines_b" not in vis_objects: vis_objects["tree_lines_b"] = [] if "tree_nodes_b" not in vis_objects: vis_objects["tree_nodes_b"] = [] # Tree A (from start): blue, Tree B (from goal): green visualize_tree( tree_a, vis_objects["tree_lines_a"], vis_objects["tree_nodes_a"], "blue", "lightblue", ) visualize_tree( tree_b, vis_objects["tree_lines_b"], vis_objects["tree_nodes_b"], "green", "lightgreen", ) else: # Single tree (RRT or RRT*) tree = state["tree"] # Ensure visualization lists exist if "tree_lines" not in vis_objects: vis_objects["tree_lines"] = [] if "tree_nodes" not in vis_objects: vis_objects["tree_nodes"] = [] visualize_tree(tree, vis_objects["tree_lines"], vis_objects["tree_nodes"]) # Highlight visited grid cells (similar to graph search algorithms) for row, col in visited_cells: # Skip obstacles (they're already marked with obstacle color) if not np.isinf(grid_map[row, col]): visited_rect = Rectangle( (col - 0.5, row - 0.5), 1.0, 1.0, facecolor=VISITED_COLOR, edgecolor=VISITED_EDGE, linewidth=1.5, alpha=0.5, zorder=3, # Below tree nodes but above grid background ) ax.add_patch(visited_rect) vis_objects["visited_patches"].append(visited_rect) # For RRT/RRT*/RRT-Connect: highlight new node and edge to parent if not is_prm: new_node = state.get("new_node") parent_node = state.get("parent_node") if new_node is not None: nx, ny = new_node # Determine color based on algorithm and extending tree if is_rrt_connect: extending_tree = state.get("extending_tree", None) if extending_tree == "a": new_node_color = "darkblue" new_edge_color = "blue" elif extending_tree == "b": new_node_color = "darkgreen" new_edge_color = "green" else: new_node_color = "red" new_edge_color = "red" else: new_node_color = "red" new_edge_color = "red" # Highlight new node new_node_circle = plt.Circle( (nx, ny), radius=0.15, color=new_node_color, alpha=0.8, zorder=6 ) ax.add_patch(new_node_circle) # Add to appropriate tree's node list for cleanup if is_rrt_connect: extending_tree = state.get("extending_tree", None) if extending_tree == "a": vis_objects["tree_nodes_a"].append(new_node_circle) elif extending_tree == "b": vis_objects["tree_nodes_b"].append(new_node_circle) else: # Fallback: add to both vis_objects["tree_nodes_a"].append(new_node_circle) else: vis_objects["tree_nodes"].append(new_node_circle) # Highlight edge to parent if parent_node is not None: px, py = parent_node (new_edge_line,) = ax.plot( [px, nx], [py, ny], color=new_edge_color, linewidth=2, alpha=0.8, zorder=5 ) # Add to appropriate tree's line list if is_rrt_connect: extending_tree = state.get("extending_tree", None) if extending_tree == "a": vis_objects["tree_lines_a"].append(new_edge_line) elif extending_tree == "b": vis_objects["tree_lines_b"].append(new_edge_line) else: vis_objects["tree_lines_a"].append(new_edge_line) else: vis_objects["tree_lines"].append(new_edge_line) # Show connection line to nearest node in other tree (for RRT-Connect when path is found) if is_rrt_connect and state.get("to_nearest_node") is not None and new_node is not None: tx, ty = state["to_nearest_node"] nx, ny = new_node (connect_line,) = ax.plot( [nx, tx], [ny, ty], color="purple", linewidth=3, alpha=0.9, linestyle="--", zorder=5, ) # Add to appropriate list for cleanup vis_objects["tree_lines_a"].append(connect_line) # Show random sampling point (for RRT algorithms) random_point = state.get("random_point") if random_point is not None: rx, ry = random_point random_marker = plt.Circle( (rx, ry), radius=0.08, color="orange", alpha=0.7, zorder=7 ) ax.add_patch(random_marker) vis_objects["random_point_marker"] = random_marker # Update current node patch (for consistency with other visualizations) # Align with grid by rounding to nearest integer coordinates if new_node is not None: nx, ny = new_node # Round to nearest grid cell col = int(round(nx)) row = int(round(ny)) # Ensure within bounds if 0 <= col < width and 0 <= row < height: vis_objects["current_patch"].set_xy((col - 0.5, row - 0.5)) vis_objects["current_patch"].set_visible(True) else: vis_objects["current_patch"].set_visible(False) else: vis_objects["current_patch"].set_visible(False) # Update final path if found (common for all algorithms) if state.get("path_found", False) and "path" in state: path = state["path"] if len(path) > 0: # Convert path from list of [x, y] to arrays path_array = np.array(path) # Note: path coordinates are already in (x, y) format path_cols = path_array[:, 0] path_rows = path_array[:, 1] vis_objects["path_line"].set_data(path_cols, path_rows) # Return all artists for blitting artists = ( 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"] ) # Add tree/roadmap-specific artists if "tree_lines" in vis_objects: artists += vis_objects["tree_lines"] if "tree_nodes" in vis_objects: artists += vis_objects["tree_nodes"] if "tree_lines_a" in vis_objects: artists += vis_objects["tree_lines_a"] if "tree_nodes_a" in vis_objects: artists += vis_objects["tree_nodes_a"] if "tree_lines_b" in vis_objects: artists += vis_objects["tree_lines_b"] if "tree_nodes_b" in vis_objects: artists += vis_objects["tree_nodes_b"] if "roadmap_nodes" in vis_objects: artists += vis_objects["roadmap_nodes"] if "roadmap_edges" in vis_objects: artists += vis_objects["roadmap_edges"] if "random_point_marker" in vis_objects and vis_objects["random_point_marker"] is not None: artists.append(vis_objects["random_point_marker"]) return artists return update_frame

RRT (Rapidly-exploring Random Tree)¶

Rapidly-exploring Random Tree (RRT), proposed by Steven M. LaValle in 1998, is a sampling-based algorithm for solving high-dimensional continuous space motion planning problems. RRT finds feasible paths by constructing a rapidly-exploring random tree in free space.

Core Principle¶

The core idea of RRT is to build a rapidly-expanding tree in free space by randomly sampling points in configuration space and connecting them to the nearest node in the tree. The algorithm iteratively grows the tree structure until it connects to the goal region or reaches the maximum number of iterations.

Key Features¶

• Probabilistic Completeness: As the number of iterations increases, the probability of finding a solution approaches 1 (if a solution exists)
• Non-optimality: RRT only finds feasible paths, not guaranteed to be optimal
• Rapid Space Exploration: Efficient exploration of high-dimensional space through random sampling strategy
• Simple Implementation: Clear algorithm logic, easy to implement and understand
• No Grid Discretization: Operates directly in continuous space, avoiding the curse of dimensionality

Algorithm Process¶

1. Initialization: Set the start point as the root node of the tree
2. Random Sampling: Generate a random point in configuration space
3. Nearest Neighbor Search: Find the nearest node in the existing tree to the sampled point
4. Tree Extension: Extend from the nearest node toward the sampled point by a fixed step size
5. Collision Detection: Check if the newly generated edge collides with obstacles
6. Node Addition: If no collision, add the new point to the tree
7. Goal Detection: Check if the new node is within the goal region
8. Iterative Repeat: Repeat steps 2-7 until connecting to the goal or reaching maximum iterations

Property Analysis¶

• Completeness: Probabilistically complete - as iterations approach infinity, probability of finding a solution is 1 (if exists)
• Optimality: Non-optimal - does not guarantee finding the shortest path
• Time Complexity: (O(\log n)) per iteration (with efficient nearest neighbor data structures), where (n) is the number of nodes in the tree
• Space Complexity: (O(n)), storing all tree nodes
• Convergence Rate: Exponential relationship with space dimension, but performs well in practice

Application Scenarios¶

• High-dimensional Motion Planning: Path planning for complex systems like robotic arms, UAVs
• Non-holonomic Constraint Systems: Can be extended to consider vehicle kinematic constraints
• Real-time Planning: Suitable for scenarios requiring quick feasible solutions
• Complex Obstacle Environments: Can handle irregularly shaped obstacles

Comparison with Other Algorithms¶

• Compared to Graph Search Algorithms (Dijkstra, A*): RRT does not require state space discretization, more suitable for high-dimensional continuous problems
• Compared to PRM (Probabilistic Roadmap): RRT is a single-query algorithm, more efficient for individual planning problems
• **Compared to RRT***: RRT is faster but non-optimal, RRT* achieves asymptotic optimality through rewiring

Visualization Explanation¶

In the animation demonstration:

• Blue nodes and gray edges: Represent the current structure of the RRT tree
• Red nodes and edges: Highlight newly added nodes and edges in the current iteration
• Orange points: Positions of random sampling points
• Light blue grid cells: Discrete grid cells visited by tree nodes
• Pink rectangle: Grid cell corresponding to the current node
• Green path: Final feasible path found (if exists)

RRT rapidly explores free space through random sampling strategy, particularly suitable for solving high-dimensional continuous motion planning problems. Note that the algorithm guarantees feasibility but not optimality.

In [9]:

hide_input

def plan_with_rrt(grid_map, start, goal, size, callback=None, reset_seed=True):
    """Plan a path using RRT algorithm on a grid map with optional callback."""
    # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0
    if reset_seed:
        np.random.seed()  # Reset random seed for variability

    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 RRT
    start_xy = np.array(grid_to_continuous_coords(start))
    goal_xy = np.array(grid_to_continuous_coords(goal))

    # Extract obstacles from grid map using shared function
    obstacles = extract_obstacles_from_grid(grid_map)

    # Create collision checking function using shared function
    collide_fn = create_grid_collision_checker(obstacles)

    # Run RRT planning with callback
    path, tree = RRT.plan(
        start=start_xy,
        target=goal_xy,
        boundary=np.array(boundary),
        obstacles=obstacles,
        collide_fn=collide_fn,
        extension_step=1.0,
        max_iter=10000,
        callback=callback,
    )

    if len(path) == 0:
        print("RRT failed to find a path")
        return None
    else:
        print(f"RRT found a path with {len(path)} waypoints")
        return grid_map, start, goal, path, tree

def plan_with_rrt(grid_map, start, goal, size, callback=None, reset_seed=True): """Plan a path using RRT algorithm on a grid map with optional callback.""" # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0 if reset_seed: np.random.seed() # Reset random seed for variability 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 RRT start_xy = np.array(grid_to_continuous_coords(start)) goal_xy = np.array(grid_to_continuous_coords(goal)) # Extract obstacles from grid map using shared function obstacles = extract_obstacles_from_grid(grid_map) # Create collision checking function using shared function collide_fn = create_grid_collision_checker(obstacles) # Run RRT planning with callback path, tree = RRT.plan( start=start_xy, target=goal_xy, boundary=np.array(boundary), obstacles=obstacles, collide_fn=collide_fn, extension_step=1.0, max_iter=10000, callback=callback, ) if len(path) == 0: print("RRT failed to find a path") return None else: print(f"RRT found a path with {len(path)} waypoints") return grid_map, start, goal, path, tree

In [10]:

hide_input

rrt_states_history = []


def rrt_callback(state):
    rrt_states_history.append(state)


print("=== Running RRT with state collection ===")
# Use the same grid_map, start, and goal generated earlier
result = plan_with_rrt(
    grid_map=grid_map,
    start=start,
    goal=goal,
    size=size,
    callback=rrt_callback,
    reset_seed=False,  # You can observe different runs by changing this to True
)

if result is not None:
    grid_map, start, goal, path, tree = result
    height, width = grid_map.shape

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

    # Initialize visualization objects for RRT animation
    global ax, vis_objects
    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
        "tree_lines": [],  # Lines representing tree edges (single tree)
        "tree_nodes": [],  # Circles representing tree nodes (single tree)
        "tree_lines_a": [],  # Lines representing tree A edges (dual tree) - not used for RRT
        "tree_nodes_a": [],  # Circles representing tree A nodes (dual tree) - not used for RRT
        "tree_lines_b": [],  # Lines representing tree B edges (dual tree) - not used for RRT
        "tree_nodes_b": [],  # Circles representing tree B nodes (dual tree) - not used for RRT
        "random_point_marker": None,  # Marker for random sampling point
    }

    fig, ax = init_figure(size)

    # Create animation for RRT using unified update function
    rrt_animation = FuncAnimation(
        fig,
        create_sampling_update_frame("RRT", rrt_states_history),
        frames=len(rrt_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,
    )

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

    # Display animation in Jupyter
    display(HTML(rrt_animation.to_jshtml()))
else:
    print("RRT planning failed")

rrt_states_history = [] def rrt_callback(state): rrt_states_history.append(state) print("=== Running RRT with state collection ===") # Use the same grid_map, start, and goal generated earlier result = plan_with_rrt( grid_map=grid_map, start=start, goal=goal, size=size, callback=rrt_callback, reset_seed=False, # You can observe different runs by changing this to True ) if result is not None: grid_map, start, goal, path, tree = result height, width = grid_map.shape print(f"Collected {len(rrt_states_history)} algorithm states") # Initialize visualization objects for RRT animation global ax, vis_objects 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 "tree_lines": [], # Lines representing tree edges (single tree) "tree_nodes": [], # Circles representing tree nodes (single tree) "tree_lines_a": [], # Lines representing tree A edges (dual tree) - not used for RRT "tree_nodes_a": [], # Circles representing tree A nodes (dual tree) - not used for RRT "tree_lines_b": [], # Lines representing tree B edges (dual tree) - not used for RRT "tree_nodes_b": [], # Circles representing tree B nodes (dual tree) - not used for RRT "random_point_marker": None, # Marker for random sampling point } fig, ax = init_figure(size) # Create animation for RRT using unified update function rrt_animation = FuncAnimation( fig, create_sampling_update_frame("RRT", rrt_states_history), frames=len(rrt_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, ) # rrt_animation.save( # "rrt_animation.mp4", writer="ffmpeg", fps=fps, dpi=300 # ) # Display animation in Jupyter display(HTML(rrt_animation.to_jshtml())) else: print("RRT planning failed")

=== Running RRT with state collection ===
RRT found a path with 9 waypoints
Collected 84 algorithm states

Once Loop Reflect

RRT* (Optimal Rapidly-exploring Random Tree)¶

RRT* (Optimal Rapidly-exploring Random Tree), proposed by Sertac Karaman and Emilio Frazzoli in 2011, is an asymptotically optimal variant of the RRT algorithm. RRT* maintains RRT's rapid exploration capability while gradually optimizing path quality through a rewiring mechanism, converging to the optimal solution as the number of iterations increases.

Core Principle¶

RRT* adds two key optimization steps to the basic RRT algorithm:

1. Optimal Parent Selection: When adding a new node, instead of only considering the nearest node as the parent, RRT* searches all possible parent nodes within a radius and selects the one that minimizes the path cost from the start to the new node
2. Rewiring Optimization: After adding a new node, RRT* checks if the new node can serve as a better parent for other nodes within the radius; if so, it reconnects those nodes to the new node

Key Features¶

• Asymptotic Optimality: As the number of iterations increases, the found path converges to the optimal path with probability 1
• Probabilistic Completeness: Inherited from RRT, probability of finding a solution approaches 1 as iterations approach infinity
• Incremental Optimization: Path quality continuously improves as the tree grows
• Computational Complexity: Slightly higher than RRT but still acceptable
• Rewiring Radius: Critical parameter balancing computational overhead and optimization effectiveness

Algorithm Process¶

1. Initialization: Set the start point as the root node with cost 0
2. Random Sampling: Generate a random point in configuration space
3. Nearest Neighbor Search: Find the nearest node in the existing tree to the sampled point
4. Tree Extension: Extend from the nearest node toward the sampled point by a fixed step size
5. Optimal Parent Selection: Search all nodes within the radius and select the parent that minimizes the new node's cost
6. Collision Detection: Check if the new edge collides with obstacles
7. Node Addition: If no collision, add the new node to the tree and record its cost
8. Rewiring Optimization: Check if the new node can serve as a better parent for other nodes within the radius; if so, reconnect them
9. Goal Detection: Check if the new node is within the goal region
10. Iterative Repeat: Repeat steps 2-9 until connecting to the goal or reaching maximum iterations

Property Analysis¶

• Completeness: Probabilistically complete - inherited from RRT
• Optimality: Asymptotically optimal - as iterations approach infinity, the found path converges to the optimal path with probability 1
• Time Complexity: (O(\log n + k)) per iteration, where (n) is the number of nodes in the tree and (k) is the number of nodes within the radius
• Space Complexity: (O(n)), storing all tree nodes and their costs
• Convergence Rate: Convergence speed to the optimal solution depends on the rewiring radius and sampling distribution

Application Scenarios¶

• Optimal Path Planning: Applications requiring high-quality paths, such as autonomous driving, UAV navigation
• Adequate Computational Resources: Can tolerate slightly higher computational overhead than RRT
• Offline Planning: Suitable for non-real-time scenarios requiring optimal solutions
• Dynamic Environment Adaptation: Can adapt to environmental changes through incremental rewiring

Comparison with Other Algorithms¶

• Compared to RRT: RRT* achieves asymptotic optimality through rewiring but with slightly higher computational overhead
• **Compared to PRM***: RRT* is a single-query algorithm, more suitable for optimizing specific problems
• Compared to Graph Search Algorithms: RRT* works in continuous space, avoiding discretization errors
• Compared to RRT-Connect: RRT* focuses on path optimality, while RRT-Connect focuses on connection speed

Visualization Explanation¶

In the animation demonstration:

• Blue nodes and gray edges: Represent the current structure of the RRT* tree
• Node color intensity: May reflect the cumulative cost of nodes (darker color for higher cost)
• Red nodes and edges: Highlight newly added nodes and edges in the current iteration
• Orange points: Positions of random sampling points
• Light blue grid cells: Discrete grid cells visited by tree nodes
• Pink rectangle: Grid cell corresponding to the current node
• Green path: Final optimized path found (if exists)
• Rewiring edges: May be displayed in different colors to show reconnected edges

RRT* optimizes path quality through intelligent rewiring mechanisms while maintaining exploration efficiency, making it an important algorithm for achieving asymptotic optimality in sampling-based motion planning.

In [11]:

hide_input

def plan_with_rrt_star(grid_map, start, goal, size, callback=None, reset_seed=True, radius=3.0):
    """Plan a path using RRT* algorithm on a grid map with optional callback."""
    # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0
    if reset_seed:
        np.random.seed()  # Reset random seed for variability

    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 RRT*
    start_xy = np.array(grid_to_continuous_coords(start))
    goal_xy = np.array(grid_to_continuous_coords(goal))

    # Extract obstacles from grid map using shared function
    obstacles = extract_obstacles_from_grid(grid_map)

    # Create collision checking function using shared function
    collide_fn = create_grid_collision_checker(obstacles)

    # Run RRT* planning with callback
    path, tree = RRTStar.plan(
        start=start_xy,
        target=goal_xy,
        boundary=np.array(boundary),
        obstacles=obstacles,
        collide_fn=collide_fn,
        extension_step=1.0,
        max_iter=10000,
        radius=radius,
        callback=callback,
    )

    if len(path) == 0:
        print("RRT* failed to find a path")
        return None
    else:
        print(f"RRT* found a path with {len(path)} waypoints")
        return grid_map, start, goal, path, tree

def plan_with_rrt_star(grid_map, start, goal, size, callback=None, reset_seed=True, radius=3.0): """Plan a path using RRT* algorithm on a grid map with optional callback.""" # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0 if reset_seed: np.random.seed() # Reset random seed for variability 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 RRT* start_xy = np.array(grid_to_continuous_coords(start)) goal_xy = np.array(grid_to_continuous_coords(goal)) # Extract obstacles from grid map using shared function obstacles = extract_obstacles_from_grid(grid_map) # Create collision checking function using shared function collide_fn = create_grid_collision_checker(obstacles) # Run RRT* planning with callback path, tree = RRTStar.plan( start=start_xy, target=goal_xy, boundary=np.array(boundary), obstacles=obstacles, collide_fn=collide_fn, extension_step=1.0, max_iter=10000, radius=radius, callback=callback, ) if len(path) == 0: print("RRT* failed to find a path") return None else: print(f"RRT* found a path with {len(path)} waypoints") return grid_map, start, goal, path, tree

In [12]:

hide_input

rrt_star_states_history = []


def rrt_star_callback(state):
    rrt_star_states_history.append(state)


print("=== Running RRT* with state collection ===")
# Use the same grid_map, start, and goal generated earlier
result = plan_with_rrt_star(
    grid_map=grid_map,
    start=start,
    goal=goal,
    size=size,
    callback=rrt_star_callback,
    reset_seed=False,  # You can observe different runs by changing this to True
    radius=3.0,
)

if result is not None:
    grid_map, start, goal, path, tree = result
    height, width = grid_map.shape

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

    # Initialize visualization objects for RRT* animation
    global ax, vis_objects
    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
        "tree_lines": [],  # Lines representing tree edges (single tree)
        "tree_nodes": [],  # Circles representing tree nodes (single tree)
        "tree_lines_a": [],  # Lines representing tree A edges (dual tree) - not used for RRT*
        "tree_nodes_a": [],  # Circles representing tree A nodes (dual tree) - not used for RRT*
        "tree_lines_b": [],  # Lines representing tree B edges (dual tree) - not used for RRT*
        "tree_nodes_b": [],  # Circles representing tree B nodes (dual tree) - not used for RRT*
        "random_point_marker": None,  # Marker for random sampling point
    }

    fig, ax = init_figure(size)

    # Create animation for RRT* using unified update function
    rrt_star_animation = FuncAnimation(
        fig,
        create_sampling_update_frame("RRT*", rrt_star_states_history),
        frames=len(rrt_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,
    )

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

    # Display animation in Jupyter
    display(HTML(rrt_star_animation.to_jshtml()))
else:
    print("RRT* planning failed")

rrt_star_states_history = [] def rrt_star_callback(state): rrt_star_states_history.append(state) print("=== Running RRT* with state collection ===") # Use the same grid_map, start, and goal generated earlier result = plan_with_rrt_star( grid_map=grid_map, start=start, goal=goal, size=size, callback=rrt_star_callback, reset_seed=False, # You can observe different runs by changing this to True radius=3.0, ) if result is not None: grid_map, start, goal, path, tree = result height, width = grid_map.shape print(f"Collected {len(rrt_star_states_history)} algorithm states") # Initialize visualization objects for RRT* animation global ax, vis_objects 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 "tree_lines": [], # Lines representing tree edges (single tree) "tree_nodes": [], # Circles representing tree nodes (single tree) "tree_lines_a": [], # Lines representing tree A edges (dual tree) - not used for RRT* "tree_nodes_a": [], # Circles representing tree A nodes (dual tree) - not used for RRT* "tree_lines_b": [], # Lines representing tree B edges (dual tree) - not used for RRT* "tree_nodes_b": [], # Circles representing tree B nodes (dual tree) - not used for RRT* "random_point_marker": None, # Marker for random sampling point } fig, ax = init_figure(size) # Create animation for RRT* using unified update function rrt_star_animation = FuncAnimation( fig, create_sampling_update_frame("RRT*", rrt_star_states_history), frames=len(rrt_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, ) # rrt_star_animation.save( # "rrt_star_animation.mp4", writer="ffmpeg", fps=fps, dpi=300 # ) # Display animation in Jupyter display(HTML(rrt_star_animation.to_jshtml())) else: print("RRT* planning failed")

=== Running RRT* with state collection ===
RRT* found a path with 4 waypoints
Collected 44 algorithm states

Once Loop Reflect

RRT-Connect¶

RRT-Connect, proposed by James J. Kuffner and Steven M. LaValle in 2000, is a bidirectional extension variant of the RRT algorithm. RRT-Connect significantly accelerates path discovery by simultaneously growing two trees from the start and goal points and attempting to connect them.

Core Principle¶

The core innovation of RRT-Connect lies in its bidirectional search strategy:

1. Dual Tree Growth: Maintain two trees simultaneously - one growing from the start (Tree A) and one from the goal (Tree B)
2. Alternating Extension: Each iteration alternates between extending the two trees, with each tree attempting to connect to the other after extension
3. Connection Attempt: After successfully extending a node, immediately attempt to connect that node to the nearest node in the other tree
4. Rapid Connection: When the two trees successfully connect, the algorithm terminates and returns the path

Key Features¶

• Accelerated Convergence: Bidirectional search significantly reduces the number of iterations needed to find a path
• Probabilistic Completeness: Inherited from RRT, guaranteeing probabilistic completeness
• Non-optimality: Same as RRT, only finds feasible paths without optimality guarantees
• Connection Heuristic: Active connection attempts after each extension accelerate path discovery
• Simple and Efficient: Adds limited computational overhead to the base RRT algorithm

Algorithm Process¶

1. Dual Tree Initialization: Initialize Tree A (root at start) and Tree B (root at goal)
2. Alternating Extension:
   • If even iteration: Extend Tree A, then attempt to connect to Tree B
   • If odd iteration: Extend Tree B, then attempt to connect to Tree A
3. Tree Extension Steps:
   • Randomly sample a point
   • Find the nearest node in the tree to be extended
   • Extend one step toward the sampled point
   • Check collision; if no collision, add the new node
4. Connection Attempt: After adding a new node, immediately attempt to connect to the nearest node in the other tree
5. Connection Check: If the edge between the new node and the nearest node in the other tree has no collision, connection succeeds
6. Path Reconstruction: After successful connection, reconstruct the complete path from start to goal
7. Termination Condition: Connection success or reaching maximum iterations

Property Analysis¶

• Completeness: Probabilistically complete - as iterations approach infinity, probability of finding a solution is 1 (if exists)
• Optimality: Non-optimal - same as RRT, does not guarantee finding the shortest path
• Time Complexity: Similar to RRT, but typically requires fewer iterations to find a path
• Space Complexity: (O(n + m)), where (n) and (m) are the numbers of nodes in the two trees respectively
• Convergence Rate: Typically several times faster than single-tree RRT, especially in narrow passage environments

Application Scenarios¶

• Rapid Feasible Solutions: Scenarios requiring quick finding of feasible paths
• Narrow Passage Environments: Bidirectional search performs exceptionally well in complex narrow environments
• Real-time Planning: Suitable for computation-time-sensitive applications
• High-dimensional Spaces: Inherits RRT's ability to handle high-dimensional spaces
• Single-query Problems: Planning problems for specific start-goal pairs

Comparison with Other Algorithms¶

• Compared to RRT: RRT-Connect accelerates convergence through bidirectional search but with slightly higher computational cost per iteration
• **Compared to RRT***: RRT-Connect focuses on connection speed, while RRT* focuses on path optimality
• Compared to Bidirectional RRT: RRT-Connect includes active connection attempts, making connections more efficient
• Compared to Graph Search Algorithms: Works in continuous space, avoiding discretization

Visualization Explanation¶

In the animation demonstration:

• Blue nodes and edges: Represent Tree A growing from the start
• Green nodes and edges: Represent Tree B growing from the goal
• Red nodes and edges: Highlight newly added nodes and edges in the current iteration
• Orange points: Positions of random sampling points
• Light blue grid cells: Discrete grid cells visited by nodes from both trees
• Pink rectangle: Grid cell corresponding to the current node
• Purple dashed line: Attempted connection edge between the two trees (when path is found)
• Green path: Final feasible path found (if exists)

RRT-Connect significantly accelerates path discovery through clever bidirectional search strategies while maintaining the advantages of RRT, making it a widely used efficient motion planning algorithm in practice.

In [13]:

hide_input

def plan_with_rrt_connect(grid_map, start, goal, size, callback=None, reset_seed=True):
    """Plan a path using RRT-Connect algorithm on a grid map with optional callback."""
    # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0
    if reset_seed:
        np.random.seed()  # Reset random seed for variability

    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 RRT-Connect
    start_xy = np.array(grid_to_continuous_coords(start))
    goal_xy = np.array(grid_to_continuous_coords(goal))

    # Extract obstacles from grid map using shared function
    obstacles = extract_obstacles_from_grid(grid_map)

    # Create collision checking function using shared function
    collide_fn = create_grid_collision_checker(obstacles)

    # Run RRT-Connect planning with callback
    path, trees = RRTConnect.plan(
        start=start_xy,
        target=goal_xy,
        boundary=np.array(boundary),
        obstacles=obstacles,
        collide_fn=collide_fn,
        extension_step=1.0,
        max_iter=10000,
        callback=callback,
    )

    if len(path) == 0:
        print("RRT-Connect failed to find a path")
        return None
    else:
        print(f"RRT-Connect found a path with {len(path)} waypoints")
        return grid_map, start, goal, path, trees

def plan_with_rrt_connect(grid_map, start, goal, size, callback=None, reset_seed=True): """Plan a path using RRT-Connect algorithm on a grid map with optional callback.""" # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0 if reset_seed: np.random.seed() # Reset random seed for variability 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 RRT-Connect start_xy = np.array(grid_to_continuous_coords(start)) goal_xy = np.array(grid_to_continuous_coords(goal)) # Extract obstacles from grid map using shared function obstacles = extract_obstacles_from_grid(grid_map) # Create collision checking function using shared function collide_fn = create_grid_collision_checker(obstacles) # Run RRT-Connect planning with callback path, trees = RRTConnect.plan( start=start_xy, target=goal_xy, boundary=np.array(boundary), obstacles=obstacles, collide_fn=collide_fn, extension_step=1.0, max_iter=10000, callback=callback, ) if len(path) == 0: print("RRT-Connect failed to find a path") return None else: print(f"RRT-Connect found a path with {len(path)} waypoints") return grid_map, start, goal, path, trees

In [14]:

hide_input

rrt_connect_states_history = []


def rrt_connect_callback(state):
    rrt_connect_states_history.append(state)


print("=== Running RRT-Connect with state collection ===")
# Use the same grid_map, start, and goal generated earlier
result = plan_with_rrt_connect(
    grid_map=grid_map,
    start=start,
    goal=goal,
    size=size,
    callback=rrt_connect_callback,
    reset_seed=False,  # You can observe different runs by changing this to True
)

if result is not None:
    grid_map, start, goal, path, trees = result
    tree_a, tree_b = trees
    height, width = grid_map.shape

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

    # Initialize visualization objects for RRT-Connect animation
    global ax, vis_objects
    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
        "tree_lines": [],  # Lines representing tree edges (single tree) - not used for RRT-Connect
        "tree_nodes": [],  # Circles representing tree nodes (single tree) - not used for RRT-Connect
        "tree_lines_a": [],  # Lines representing tree A edges (dual tree)
        "tree_nodes_a": [],  # Circles representing tree A nodes (dual tree)
        "tree_lines_b": [],  # Lines representing tree B edges (dual tree)
        "tree_nodes_b": [],  # Circles representing tree B nodes (dual tree)
        "random_point_marker": None,  # Marker for random sampling point
    }

    fig, ax = init_figure(size)

    # Create animation for RRT-Connect using unified update function
    rrt_connect_animation = FuncAnimation(
        fig,
        create_sampling_update_frame("RRT-Connect", rrt_connect_states_history),
        frames=len(rrt_connect_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,
    )

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

    # Display animation in Jupyter
    display(HTML(rrt_connect_animation.to_jshtml()))
else:
    print("RRT-Connect planning failed")

rrt_connect_states_history = [] def rrt_connect_callback(state): rrt_connect_states_history.append(state) print("=== Running RRT-Connect with state collection ===") # Use the same grid_map, start, and goal generated earlier result = plan_with_rrt_connect( grid_map=grid_map, start=start, goal=goal, size=size, callback=rrt_connect_callback, reset_seed=False, # You can observe different runs by changing this to True ) if result is not None: grid_map, start, goal, path, trees = result tree_a, tree_b = trees height, width = grid_map.shape print(f"Collected {len(rrt_connect_states_history)} algorithm states") # Initialize visualization objects for RRT-Connect animation global ax, vis_objects 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 "tree_lines": [], # Lines representing tree edges (single tree) - not used for RRT-Connect "tree_nodes": [], # Circles representing tree nodes (single tree) - not used for RRT-Connect "tree_lines_a": [], # Lines representing tree A edges (dual tree) "tree_nodes_a": [], # Circles representing tree A nodes (dual tree) "tree_lines_b": [], # Lines representing tree B edges (dual tree) "tree_nodes_b": [], # Circles representing tree B nodes (dual tree) "random_point_marker": None, # Marker for random sampling point } fig, ax = init_figure(size) # Create animation for RRT-Connect using unified update function rrt_connect_animation = FuncAnimation( fig, create_sampling_update_frame("RRT-Connect", rrt_connect_states_history), frames=len(rrt_connect_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, ) # rrt_connect_animation.save( # "rrt_connect_animation.mp4", writer="ffmpeg", fps=fps, dpi=300 # ) # Display animation in Jupyter display(HTML(rrt_connect_animation.to_jshtml())) else: print("RRT-Connect planning failed")

=== Running RRT-Connect with state collection ===
RRT-Connect found a path with 3 waypoints
Collected 2 algorithm states

Once Loop Reflect

PRM (Probabilistic Roadmap Method)¶

Probabilistic Roadmap Method (PRM), introduced in the mid-1990s by Lydia E. Kavraki, is one of the earliest and most influential sampling-based motion planning algorithms. PRM is designed for multi-query planning problems, where the same environment is used for multiple start-goal queries. It constructs a roadmap (graph) that captures the connectivity of free space, enabling efficient path queries.

Core Principle¶

The core idea of PRM is to construct a roadmap of the free space through random sampling and then use graph search algorithms (like Dijkstra or A*) to find paths between arbitrary start and goal points.

Key Features¶

• Multi-query Capability: Build roadmap once, query many times - highly efficient for environments with multiple planning queries
• Probabilistic Completeness: As the number of samples increases, the probability of capturing the connectivity of free space approaches 1
• Two-Phase Approach: Clear separation between roadmap construction (learning phase) and path query (query phase)
• Graph-based Representation: Represents free space as a graph, enabling the use of efficient graph search algorithms
• Sampling Flexibility: Can adapt sampling strategies to different environment characteristics

Algorithm Process¶

Phase 1: Roadmap Construction (Learning Phase)

1. Random Sampling: Generate random configurations (points) in the configuration space
2. Node Addition: Add collision-free points to the roadmap as nodes
3. Local Connection: For each new node, connect it to its k-nearest neighbors (or all nodes within a radius)
4. Edge Validation: Check if the connection edges are collision-free
5. Graph Building: Build an undirected graph of the free space

Phase 2: Path Query (Query Phase)

1. Start/Goal Connection: Connect start and goal points to the roadmap using the same connection strategy
2. Graph Search: Use Dijkstra, A*, or other graph search algorithms to find a path from start to goal in the roadmap
3. Path Extraction: Extract the sequence of nodes and edges as the final path

Property Analysis¶

• Completeness: Probabilistically complete - as the number of samples approaches infinity, probability of capturing free space connectivity is 1
• Optimality: Path quality depends on roadmap density and connection strategy, not inherently optimal
• Time Complexity: Construction phase is O(n log n + nk) where n is number of samples and k is connection attempts per node; query phase is efficient graph search
• Space Complexity: O(n + m) where n is number of nodes and m is number of edges in roadmap
• Convergence Rate: Depends on sampling strategy and connection parameters

Application Scenarios¶

• Multi-query Planning: Environments requiring repeated path planning with different start-goal pairs
• Complex Static Environments: Environments with complex obstacle structures but relatively static configuration
• Offline Roadmap Construction: When preprocessing time is acceptable for faster subsequent queries
• High-dimensional Spaces: Particularly effective in high-dimensional configuration spaces
• Robot Manipulator Planning: Classic application area for PRM due to high-dimensional joint spaces

Comparison with Other Algorithms¶

• Compared to RRT: PRM is multi-query, RRT is single-query; PRM builds global roadmap, RRT grows tree from start
• Compared to Graph Search Algorithms: PRM works in continuous space without grid discretization
• **Compared to RRT***: PRM focuses on multi-query efficiency, RRT* focuses on single-query optimality
• Compared to RRT-Connect: PRM constructs complete roadmap, RRT-Connect focuses on fast single-query connection

Visualization Explanation¶

In the animation demonstration:

• Purple nodes: Represent sampled configurations in free space (roadmap nodes)
• Light gray edges: Represent collision-free connections between roadmap nodes
• Orange points: Highlight current random sample during sampling phase
• Red nodes: Highlight current node being processed during connection or search phase
• Light blue grid cells: Discrete grid cells visited by roadmap nodes
• Pink rectangle: Grid cell corresponding to the current node
• Green path: Final path found through the roadmap (if exists)
• Phase indicators: Different phases shown in title (sampling, connecting, searching, complete)

PRM's strength lies in its ability to construct a reusable roadmap that captures the global connectivity of complex spaces, making it particularly valuable for applications requiring repeated planning queries in the same environment.

In [15]:

hide_input

def plan_with_prm(
    grid_map,
    start,
    goal,
    size,
    callback=None,
    reset_seed=True,
    n_samples=100,
    k_nearest=5,
    connection_radius=None,
):
    """Plan a path using PRM algorithm on a grid map with optional callback."""
    # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0
    if reset_seed:
        np.random.seed()  # Reset random seed for variability

    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 PRM
    start_xy = np.array(grid_to_continuous_coords(start))
    goal_xy = np.array(grid_to_continuous_coords(goal))

    # Extract obstacles from grid map using shared function
    obstacles = extract_obstacles_from_grid(grid_map)

    # Create collision checking function using shared function
    collide_fn = create_grid_collision_checker(obstacles)

    # Run PRM planning with callback
    path, roadmap = PRM.plan(
        start=start_xy,
        target=goal_xy,
        boundary=np.array(boundary),
        obstacles=obstacles,
        collide_fn=collide_fn,
        n_samples=n_samples,
        k_nearest=k_nearest,
        connection_radius=connection_radius,
        max_iter=10000,
        callback=callback,
    )

    if len(path) == 0:
        print("PRM failed to find a path")
        return None
    else:
        print(f"PRM found a path with {len(path)} waypoints")
        return grid_map, start, goal, path, roadmap

def plan_with_prm( grid_map, start, goal, size, callback=None, reset_seed=True, n_samples=100, k_nearest=5, connection_radius=None, ): """Plan a path using PRM algorithm on a grid map with optional callback.""" # Define boundary [xmin, xmax, ymin, ymax] with grid_resolution=1.0 if reset_seed: np.random.seed() # Reset random seed for variability 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 PRM start_xy = np.array(grid_to_continuous_coords(start)) goal_xy = np.array(grid_to_continuous_coords(goal)) # Extract obstacles from grid map using shared function obstacles = extract_obstacles_from_grid(grid_map) # Create collision checking function using shared function collide_fn = create_grid_collision_checker(obstacles) # Run PRM planning with callback path, roadmap = PRM.plan( start=start_xy, target=goal_xy, boundary=np.array(boundary), obstacles=obstacles, collide_fn=collide_fn, n_samples=n_samples, k_nearest=k_nearest, connection_radius=connection_radius, max_iter=10000, callback=callback, ) if len(path) == 0: print("PRM failed to find a path") return None else: print(f"PRM found a path with {len(path)} waypoints") return grid_map, start, goal, path, roadmap

In [16]:

prm_states_history = []


def prm_callback(state):
    prm_states_history.append(state)


print("=== Running PRM with state collection ===")
# Use the same grid_map, start, and goal generated earlier
result = plan_with_prm(
    grid_map=grid_map,
    start=start,
    goal=goal,
    size=size,
    callback=prm_callback,
    reset_seed=False,  # You can observe different runs by changing this to True
    n_samples=100,
    k_nearest=5,
    connection_radius=None,
)

if result is not None:
    grid_map, start, goal, path, roadmap = result
    height, width = grid_map.shape

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

    # Initialize visualization objects for PRM animation
    global ax, vis_objects
    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
        "tree_lines": [],  # Lines representing tree edges (single tree) - not used for PRM
        "tree_nodes": [],  # Circles representing tree nodes (single tree) - not used for PRM
        "tree_lines_a": [],  # Lines representing tree A edges (dual tree) - not used for PRM
        "tree_nodes_a": [],  # Circles representing tree A nodes (dual tree) - not used for PRM
        "tree_lines_b": [],  # Lines representing tree B edges (dual tree) - not used for PRM
        "tree_nodes_b": [],  # Circles representing tree B nodes (dual tree) - not used for PRM
        "roadmap_nodes": [],  # Circles representing roadmap nodes (for PRM)
        "roadmap_edges": [],  # Lines representing roadmap edges (for PRM)
        "random_point_marker": None,  # Marker for random sampling point
    }

    fig, ax = init_figure(size)

    # Create animation for PRM using unified update function
    prm_animation = FuncAnimation(
        fig,
        create_sampling_update_frame("PRM", prm_states_history),
        frames=len(prm_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,
    )

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

    # Display animation in Jupyter
    display(HTML(prm_animation.to_jshtml()))
else:
    print("PRM planning failed")

prm_states_history = [] def prm_callback(state): prm_states_history.append(state) print("=== Running PRM with state collection ===") # Use the same grid_map, start, and goal generated earlier result = plan_with_prm( grid_map=grid_map, start=start, goal=goal, size=size, callback=prm_callback, reset_seed=False, # You can observe different runs by changing this to True n_samples=100, k_nearest=5, connection_radius=None, ) if result is not None: grid_map, start, goal, path, roadmap = result height, width = grid_map.shape print(f"Collected {len(prm_states_history)} algorithm states") # Initialize visualization objects for PRM animation global ax, vis_objects 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 "tree_lines": [], # Lines representing tree edges (single tree) - not used for PRM "tree_nodes": [], # Circles representing tree nodes (single tree) - not used for PRM "tree_lines_a": [], # Lines representing tree A edges (dual tree) - not used for PRM "tree_nodes_a": [], # Circles representing tree A nodes (dual tree) - not used for PRM "tree_lines_b": [], # Lines representing tree B edges (dual tree) - not used for PRM "tree_nodes_b": [], # Circles representing tree B nodes (dual tree) - not used for PRM "roadmap_nodes": [], # Circles representing roadmap nodes (for PRM) "roadmap_edges": [], # Lines representing roadmap edges (for PRM) "random_point_marker": None, # Marker for random sampling point } fig, ax = init_figure(size) # Create animation for PRM using unified update function prm_animation = FuncAnimation( fig, create_sampling_update_frame("PRM", prm_states_history), frames=len(prm_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, ) # prm_animation.save( # "prm_animation.mp4", writer="ffmpeg", fps=fps, dpi=300 # ) # Display animation in Jupyter display(HTML(prm_animation.to_jshtml())) else: print("PRM planning failed")

=== Running PRM with state collection ===
PRM found a path with 7 waypoints
Collected 112 algorithm states

Once Loop Reflect