In previous chapters you studied configuration spaces \( \mathcal{C} \), obstacles \( \mathcal{C}_{\text{obs}} \), and free space \( \mathcal{C}_{\text{free}} \) for kinematic planning. A discrete planner replaces the continuum \( \mathcal{C}_{\text{free}} \) by a finite or countable collection of representative configurations and transitions between them. Planning then becomes a graph search problem.

At a high level, the discrete planning pipeline is:

flowchart TD
  W["Robot model + workspace obstacles"] --> C["C-space and C_free"]
  C --> D["Discretize C_free into states"]
  D --> CHK["Check local motions for collision"]
  CHK --> G["Build directed weighted graph G(V,E)"]
  G --> S["Run graph search (e.g. BFS/Dijkstra)"]
  S --> P["Discrete plan: sequence of states/actions"]
  P --> EXEC["Track plan with low-level controller"]
        

This lesson focuses on the formulation level: state-transition systems, graphs, path costs, and the relationship between continuous and discrete models. Specific search algorithms (A*, D*, etc.) are deferred to the next lesson.

A deterministic discrete-time planning model for a robot can be written as a tuple

\[ \mathcal{P} = \bigl( X, U, f, X_{\text{init}}, X_{\text{goal}}, X_{\text{forb}}, c \bigr), \]

where:

• \( X \) is the (finite or countable) set of discrete states (e.g. configurations, grid cells, or abstract task states).
• \( U \) is the set of actions (e.g. motion primitives, joint increments).
• \( f : X \times U \to X \cup \{\bot\} \) is a (partial) transition function, with \( f(x,u) = \bot \) if the transition is infeasible (collision, joint limits, etc.).
• \( X_{\text{init}} \subseteq X \) is the set of admissible initial states.
• \( X_{\text{goal}} \subseteq X \) is the set of goal states (usually the discretized goal region).
• \( X_{\text{forb}} \subseteq X \) is a set of forbidden states (e.g. approximated collision states).
• \( c : X \times U \to \mathbb{R}_{\geq 0} \cup \{+\infty\} \) assigns a nonnegative cost to each feasible action.

A plan of finite horizon \( K \) is a sequence of actions \( \pi = (u_0, \dots, u_{K-1}) \) together with the induced state sequence

\[ x_0 \in X_{\text{init}}, \quad x_{k+1} = f(x_k, u_k), \quad k = 0,\dots,K-1. \]

The plan is feasible if:

\[ x_k \notin X_{\text{forb}} \text{ for all } k, \quad x_K \in X_{\text{goal}}. \]

The cost of a plan is

\[ J(\pi) = \sum_{k=0}^{K-1} c(x_k, u_k). \]

The discrete optimal planning problem is:

\[ \text{find a feasible plan } \pi^\star \text{ such that } J(\pi^\star) = \inf_{\pi \text{ feasible}} J(\pi). \]

In many high-DOF applications we restrict \( X \) to be finite, so the problem becomes a finite combinatorial optimization problem amenable to graph search.

From \( \mathcal{P} \) we construct a directed weighted graph to represent all feasible one-step transitions. Define

\[ V = X \setminus X_{\text{forb}}, \]

and a directed edge between states \( x, x' \in V \) if there exists an action \( u \in U \) such that \( f(x,u) = x' \) and \( c(x,u) < +\infty \). Formally,

\[ E = \bigl\{ (x,x') \in V \times V \;\bigm|\; \exists u \in U : f(x,u) = x',\ c(x,u) < +\infty \bigr\}. \]

For each edge \( e = (x,x') \in E \), define its weight

\[ w(e) = \inf \bigl\{ c(x,u) \;\bigm|\; u \in U,\ f(x,u) = x' \bigr\}. \]

This yields a directed weighted graph \( G = (V,E,w) \). Let \( V_{\text{init}} = X_{\text{init}} \cap V \) and \( V_{\text{goal}} = X_{\text{goal}} \cap V \).

A path in \( G \) is a sequence \( x_0, x_1, \dots, x_K \) with \( (x_k, x_{k+1}) \in E \). Its cost is

\[ C(x_0,\dots,x_K) = \sum_{k=0}^{K-1} w(x_k, x_{k+1}). \]

We say that the path is goal-reaching if \( x_0 \in V_{\text{init}} \) and \( x_K \in V_{\text{goal}} \).

The key theoretical observation is the equivalence between optimal planning in \( \mathcal{P} \) and shortest paths in \( G \).

Proposition 3.1 (Planning as Shortest Path).

Assume all edge weights are nonnegative: \( w(e) \geq 0 \) for all \( e \in E \). Then

• Every feasible plan for \( \mathcal{P} \) induces a goal-reaching path in \( G \) with the same cost.
• Conversely, every goal-reaching path in \( G \) corresponds to a feasible plan (up to choice of minimizing actions) with the same cost.
• Therefore the optimal cost \( J(\pi^\star) \) equals the minimal path cost in \( G \) between \( V_{\text{init}} \) and \( V_{\text{goal}} \).

Sketch of Proof.

A feasible plan \( (x_0,u_0,\dots,x_{K-1},u_{K-1},x_K) \) satisfies \( x_{k+1} = f(x_k,u_k) \), hence by construction \( (x_k,x_{k+1}) \in E \), and by definition of \( w \) we have \( c(x_k,u_k) \geq w(x_k,x_{k+1}) \). Choosing minimizing actions yields equality and gives a path of identical cost. Conversely, given a path, for each edge \( (x_k,x_{k+1}) \) choose an action \( u_k \) such that \( f(x_k,u_k) = x_{k+1} \) and \( c(x_k,u_k) \approx w(x_k,x_{k+1}) \). Concatenating these actions yields a feasible plan with matching cost. Minimizing over plans or paths leads to the same value.

This equivalence justifies using graph algorithms as core planners for high-DOF robots, provided the discrete model faithfully approximates the continuous robot and environment.

For a manipulator with configuration space \( \mathcal{C} \subset \mathbb{R}^n \) and free space \( \mathcal{C}_{\text{free}} \), we construct a discrete state set \( X \) by sampling or gridding \( \mathcal{C}_{\text{free}} \). A common simple scheme is a regular grid with spacing \( h > 0 \):

\[ X_h = \bigl\{ q \in \mathcal{C}_{\text{free}} \;\bigm|\; q_i = k_i h,\ k_i \in \mathbb{Z},\ i = 1,\dots,n \bigr\}. \]

For each state \( q \in X_h \) we define a local neighborhood

\[ \mathcal{N}(q) = \bigl\{ q' \in X_h \;\bigm|\; 0 < \lVert q' - q \rVert \leq \eta h \bigr\}, \]

where \( \eta \geq 1 \) is a parameter controlling the connectivity (e.g. 4-neighborhood, 8-neighborhood in a plane, or higher-dimensional analogues).

To construct edges, we associate each pair \( (q,q') \) with a simple continuous path segment \( \sigma_{q,q'} : [0,1] \to \mathcal{C} \) (e.g. a straight line in joint space). Then we define:

\[ (q,q') \in E_h \quad \Longleftrightarrow \quad q' \in \mathcal{N}(q) \text{ and } \sigma_{q,q'}(t) \in \mathcal{C}_{\text{free}} \text{ for all } t \in [0,1]. \]

The edge cost can approximate geometric path length:

\[ w_h(q,q') = \int_0^1 \lVert \dot{\sigma}_{q,q'}(t) \rVert \, dt, \]

or, under straight-line interpolation, \( w_h(q,q') = \lVert q' - q \rVert \).

As we refine the grid resolution, the discrete optimal path approaches the continuous optimum under mild regularity and clearance assumptions.

Proposition 4.1 (Informal Resolution Completeness).

Suppose there exists a continuous path \( \sigma^\star : [0,1] \to \mathcal{C}_{\text{free}} \) connecting \( q_{\text{init}} \) to \( q_{\text{goal}} \) with clearance \( \delta > 0 \) from obstacles. Then for sufficiently small grid spacing \( h \) there exists a path in \( G_h = (X_h, E_h) \) from some discrete initial state to a discrete goal state whose cost approximates the length of \( \sigma^\star \).

Idea of Proof.

The clearance condition implies that a tube of radius \( \delta \) around \( \sigma^\star \) is entirely collision-free. For small enough \( h \), every segment of \( \sigma^\star \) of arc-length at most \( \delta/2 \) contains at least one grid point of \( X_h \). By choosing these grid points in order, we obtain a sequence \( q_0,\dots,q_K \) that shadows \( \sigma^\star \). The local segments between consecutive grid points remain inside the tube, so the corresponding edges are collision-free. Path length distortion vanishes as \( h \to 0 \).

flowchart TD
  CFREE["Continuous C_free"] --> GRID["Finite grid X_h"]
  GRID --> NEIGH["Neighborhood relation N(q)"]
  NEIGH --> EDGES["Feasible edges via collision check"]
  EDGES --> GRAPH["Discrete graph G_h(V,E)"]
        

Different planning objectives correspond to different choices of edge weights \( w \). Common choices include:

• Unit cost: \( w(x,x') = 1 \) for all edges. Minimizing cost yields the fewest-step path.
• Geometric length: \( w(x,x') = \lVert q' - q \rVert \) when states are configurations \( q, q' \).
• Energy proxy: \( w(x,x') \) approximates integral of torque or squared joint velocity along the local motion.
• Task-dependent costs: penalties for moving certain joints, approaching joint limits, or leaving a preferred region of workspace.

The optimal planning problem becomes:

\[ \text{Given } G=(V,E,w),\ V_{\text{init}},\ V_{\text{goal}},\ \text{find a path } x_0,\dots,x_K \]

\[ \text{such that } x_0 \in V_{\text{init}},\ x_K \in V_{\text{goal}},\ C(x_0,\dots,x_K) \text{ is minimal.} \]

When all edge weights are equal, breadth-first search (BFS) is sufficient. For nonuniform nonnegative weights, Dijkstra's algorithm (or later, A*) is appropriate.

A basic finiteness property is often useful.

Lemma 5.1 (No Infinite Optimal Paths).

Suppose \( G = (V,E,w) \) has finite \( V \) and there exists \( \alpha > 0 \) such that \( w(e) \geq \alpha \) for all edges \( e \in E \). Then every optimal path from a given start to a goal has finite length (finite number of edges).

Proof.

Assume for contradiction that some optimal solution uses an infinite number of edges but has finite cost. Since each edge costs at least \( \alpha \), the cost of the first \( K \) edges is at least

\[ K \alpha, \]

which diverges as \( K \to \infty \). Hence any infinite path has infinite cost and cannot be optimal.

In Python, high-level graph operations are conveniently supported by networkx. For robotics-specific work, discrete planners are often embedded within larger frameworks (e.g., grid planners in ROS navigation stacks, or discrete abstractions inside OMPL-based systems). Below we show both a from-scratch BFS on an adjacency list and a networkx-based implementation.

6.1 Adjacency-List Representation and BFS

from collections import deque
from typing import Dict, List, Hashable, Optional, Tuple

# Directed graph with nonnegative weights
Adjacency = Dict[Hashable, List[Tuple[Hashable, float]]]

def bfs_unweighted(adj: Dict[Hashable, List[Hashable]],
                   start,
                   goal) -> Optional[List[Hashable]]:
    """
    Breadth-first search for unweighted graphs.
    Returns a path with the minimal number of edges, or None if unreachable.
    """
    queue = deque([start])
    parent = {start: None}

    while queue:
        v = queue.popleft()
        if v == goal:
            break
        for w in adj.get(v, []):
            if w not in parent:
                parent[w] = v
                queue.append(w)

    if goal not in parent:
        return None

    # Reconstruct path
    path = []
    cur = goal
    while cur is not None:
        path.append(cur)
        cur = parent[cur]
    path.reverse()
    return path

def dijkstra(adj: Adjacency,
             start,
             goal) -> Optional[Tuple[float, List[Hashable]]]:
    """
    Dijkstra's algorithm for nonnegative-weight directed graphs.
    Returns (cost, path) or None if goal unreachable.
    """
    import heapq

    dist = {start: 0.0}
    parent: Dict[Hashable, Optional[Hashable]] = {start: None}
    heap = [(0.0, start)]

    while heap:
        cost_v, v = heapq.heappop(heap)
        if v == goal:
            break
        if cost_v > dist[v]:
            continue
        for w, w_cost in adj.get(v, []):
            new_cost = cost_v + w_cost
            if w not in dist or new_cost < dist[w]:
                dist[w] = new_cost
                parent[w] = v
                heapq.heappush(heap, (new_cost, w))

    if goal not in dist:
        return None

    # Reconstruct path
    path = []
    cur = goal
    while cur is not None:
        path.append(cur)
        cur = parent[cur]
    path.reverse()
    return dist[goal], path

# Example: 2-DOF grid in joint space
def make_joint_grid_graph(q1_vals, q2_vals, step_cost=1.0) -> Adjacency:
    """
    Build a simple 4-connected grid in joint space (q1, q2).
    Here we ignore collisions; in practice insert collision checks.
    """
    adj: Adjacency = {}
    for q1 in q1_vals:
        for q2 in q2_vals:
            v = (q1, q2)
            nbrs = []
            # 4-connected neighbors
            for dq1, dq2 in [(1, 0), (-1, 0), (0, 1), (0, -1)]:
                nq1 = q1 + dq1
                nq2 = q2 + dq2
                if nq1 in q1_vals and nq2 in q2_vals:
                    nbrs.append(((nq1, nq2), step_cost))
            adj[v] = nbrs
    return adj

if __name__ == "__main__":
    q_values = list(range(-2, 3))
    graph = make_joint_grid_graph(q_values, q_values)
    start = (0, 0)
    goal = (2, 2)
    cost, path = dijkstra(graph, start, goal)
    print("Optimal cost:", cost)
    print("Path:", path)
      

6.2 Using networkx

import networkx as nx

G = nx.DiGraph()

# Add nodes and weighted edges
for v, nbrs in graph.items():
    G.add_node(v)
    for w, cost in nbrs:
        G.add_edge(v, w, weight=cost)

# Shortest path with Dijkstra
path = nx.shortest_path(G, source=start, target=goal, weight="weight")
cost = nx.shortest_path_length(G, source=start, target=goal, weight="weight")
print("networkx path:", path)
print("networkx cost:", cost)
      

In C++, discrete planners often appear inside larger motion-planning libraries. The Open Motion Planning Library (OMPL) provides state-space abstractions, but its core focus is sampling-based planning. For purely discrete problems, a lightweight adjacency-list representation with STL containers is often sufficient.

7.1 Basic C++ Adjacency List and Dijkstra

#include <iostream>
#include <vector>
#include <queue>
#include <limits>
#include <unordered_map>

struct Edge {
    int to;
    double cost;
};

using Graph = std::vector<std::vector<Edge> >;

std::pair<double, std::vector<int> >
dijkstra(const Graph& g, int start, int goal) {
    const double INF = std::numeric_limits<double>::infinity();
    std::vector<double> dist(g.size(), INF);
    std::vector<int> parent(g.size(), -1);

    using Node = std::pair<double, int>; // (dist, vertex)
    auto cmp = [](const Node& a, const Node& b) {
        return a.first > b.first;
    };
    std::priority_queue<Node, std::vector<Node>, decltype(cmp)> pq(cmp);

    dist[start] = 0.0;
    pq.emplace(0.0, start);

    while (!pq.empty()) {
        auto [d, v] = pq.top();
        pq.pop();
        if (v == goal) break;
        if (d > dist[v]) continue;

        for (const auto& e : g[v]) {
            double nd = d + e.cost;
            if (nd < dist[e.to]) {
                dist[e.to] = nd;
                parent[e.to] = v;
                pq.emplace(nd, e.to);
            }
        }
    }

    if (dist[goal] == INF) {
        return {INF, {}};
    }

    std::vector<int> path;
    for (int v = goal; v != -1; v = parent[v]) {
        path.push_back(v);
    }
    std::reverse(path.begin(), path.end());
    return {dist[goal], path};
}

int main() {
    // Small example graph
    Graph g(4);
    g[0].push_back({1, 1.0});
    g[0].push_back({2, 2.0});
    g[1].push_back({3, 1.0});
    g[2].push_back({3, 0.5});

    auto [cost, path] = dijkstra(g, 0, 3);
    std::cout << "Cost: " << cost << "\nPath:";
    for (int v : path) std::cout << " " << v;
    std::cout << std::endl;
    return 0;
}
      

7.2 Comment on OMPL

OMPL's C++ API represents states via ompl::base::State and their transitions through abstract planners. When using OMPL for discretized planning (e.g. on a lattice), the underlying discrete model can be embedded into a custom state space where each state index corresponds to a vertex of the discrete graph, and local motion validators perform collision checks similar to the construction in Section 4.

Java is less common in low-level robotics, but is widely used in high-level planning and simulation frameworks. The JGraphT library provides a generic graph abstraction that can encode discrete planning problems.

import org.jgrapht.GraphPath;
import org.jgrapht.graph.DefaultWeightedEdge;
import org.jgrapht.graph.SimpleDirectedWeightedGraph;
import org.jgrapht.alg.shortestpath.DijkstraShortestPath;

public class DiscretePlanner {
    public static void main(String[] args) {
        SimpleDirectedWeightedGraph<String, DefaultWeightedEdge> g =
            new SimpleDirectedWeightedGraph<>(DefaultWeightedEdge.class);

        // States
        String s0 = "(0,0)";
        String s1 = "(1,0)";
        String s2 = "(1,1)";
        String s3 = "(2,1)";

        g.addVertex(s0);
        g.addVertex(s1);
        g.addVertex(s2);
        g.addVertex(s3);

        // Transitions
        DefaultWeightedEdge e01 = g.addEdge(s0, s1);
        g.setEdgeWeight(e01, 1.0);

        DefaultWeightedEdge e12 = g.addEdge(s1, s2);
        g.setEdgeWeight(e12, 1.0);

        DefaultWeightedEdge e02 = g.addEdge(s0, s2);
        g.setEdgeWeight(e02, 2.5);

        DefaultWeightedEdge e23 = g.addEdge(s2, s3);
        g.setEdgeWeight(e23, 1.0);

        // Dijkstra shortest path
        DijkstraShortestPath<String, DefaultWeightedEdge> dsp =
            new DijkstraShortestPath<>(g);

        GraphPath<String, DefaultWeightedEdge> path =
            dsp.getPath(s0, s3);

        System.out.println("Cost: " + path.getWeight());
        System.out.println("Path: " + path.getVertexList());
    }
}
      

The mapping from robot C-space to vertex labels (here strings like "(q1,q2)") follows the discretization process discussed in Section 4.

MATLAB has built-in support for graphs via digraph. In robotics toolboxes, such as the Robotics System Toolbox, discrete planners are often built on occupancy grids or state lattices. Below we construct a simple directed weighted graph and plan with shortestpath.

% Define vertices as integer indices
s = [1 1 2 2 3];  % source nodes
t = [2 3 3 4 4];  % target nodes
w = [1 2 1 1 0.5]; % edge weights

G = digraph(s, t, w);

start = 1;
goal  = 4;

[path, cost] = shortestpath(G, start, goal);

disp("Path:");
disp(path);
disp("Cost:");
disp(cost);
      

For a grid-based manipulator C-space, one can map each grid configuration to an integer node index, and connect neighbors after collision checks (e.g., using a function that tests for self-collision and environment collision based on the kinematic model you already know).

9.1 MATLAB Function Block for Simulink

In Simulink, discrete planning logic can be encapsulated in a MATLAB Function block. The function below performs one step of BFS expansion on a fixed graph (represented by an adjacency matrix) and can be iterated over successive simulation steps.

function [frontierOut, parentOut] = bfsStep(frontierIn, parentIn, A)
%#codegen
% A is adjacency matrix, A(i,j) = 1 if edge i->j exists.
% frontierIn is a logical vector for current frontier nodes.
% parentIn stores parent indices (0 for "none").

N = size(A,1);
frontierOut = false(N,1);
parentOut = parentIn;

for i = 1:N
    if frontierIn(i)
        for j = 1:N
            if A(i,j) ~= 0 && parentOut(j) == 0
                parentOut(j) = i;
                frontierOut(j) = true;
            end
        end
    end
end
      

Iterating this block over simulation steps implements a time-unfolded search process, useful for conceptual demonstrations of discrete planning in a Simulink model.

Wolfram Mathematica provides rich graph-theoretic functionality, making it convenient for prototyping discrete planners and analyzing graph structure (e.g., counting paths, verifying connectivity).

(* Define a directed weighted graph *)
g = Graph[
  {
    1 <-> 2,
    1 <-> 3,
    2 <-> 4,
    3 <-> 4
  },
  EdgeWeight -> {1.0, 2.0, 1.0, 0.5},
  DirectedEdges -> True
];

start = 1;
goal = 4;

path = FindShortestPath[g, start, goal, 
  Method -> "Dijkstra"];

cost = Total[
  PropertyValue[{g, #}, EdgeWeight] & /@ 
    Partition[path, 2, 1]
];

Print["Path: ", path];
Print["Cost: ", cost];

(* Number of paths of length k using powers of adjacency matrix *)
adj = AdjacencyMatrix[g];
k = 3;
numPaths = MatrixPower[adj, k][[start, goal]];
Print["Number of walks of length ", k, " from start to goal: ", numPaths];
      

The adjacency matrix representation connects naturally to algebraic properties used in some theoretical analyses of discrete planners (e.g., path counting and connectivity).

Problem 1 (Equivalence of Planning and Shortest Path).

Let \( \mathcal{P} = (X,U,f,X_{\text{init}},X_{\text{goal}},X_{\text{forb}},c) \) be a deterministic discrete planning model with finite \( X \) and \( c(x,u) \geq 0 \). Let \( G = (V,E,w) \) be the associated graph as in Section 3. Prove that the optimal cost over feasible plans equals the weight of a minimum-cost path in \( G \) from an initial state to a goal state.

Solution.

Every feasible plan \( \pi = (u_0,\dots,u_{K-1}) \) induces a state sequence \( x_0,\dots,x_K \) with \( x_{k+1} = f(x_k,u_k) \). By construction, \( x_k \notin X_{\text{forb}} \), so each \( x_k \in V \), and \( (x_k,x_{k+1}) \in E \). Hence we obtain a path in \( G \). The plan cost is

\[ J(\pi) = \sum_{k=0}^{K-1} c(x_k,u_k) \geq \sum_{k=0}^{K-1} w(x_k,x_{k+1}) = C(x_0,\dots,x_K). \]

By choosing actions attaining the infimum in the definition of \( w \), we can realize equality for each edge. Thus for every feasible plan there exists a path of equal cost, and for every path there exists a plan with equal cost. Minimizing over all feasible plans or over all paths therefore yields the same infimum cost, proving the equivalence.

Problem 2 (BFS Optimality for Unit-Cost Graphs).

Let \( G = (V,E) \) be a directed graph with unit edge cost \( w(e) = 1 \). Show that breadth-first search starting from a single start vertex visits vertices in order of nondecreasing shortest-path distance (in number of edges).

Solution.

BFS maintains a queue of frontier vertices and discovers vertices level by level. When a vertex \( v \) is first removed from the queue, its distance label \( d(v) \) equals \( d(u) + 1 \) for some predecessor \( u \) already processed. By induction on the level, all vertices at distance \( k \) are discovered before any vertex at distance \( k+1 \). Any path from the start to a newly discovered vertex \( v \) has length at least \( d(v) \); otherwise some shorter path would visit an intermediate vertex earlier and lead to discovering \( v \) with a smaller label. Hence \( d(v) \) equals the minimal path length from the start to \( v \), so BFS is optimal for unit-cost graphs.

Problem 3 (Adjacency Matrix and Walk Counting).

Let \( G = (V,E) \) be a directed graph with \( |V| = n \), and let \( A \in \{0,1\}^{n \times n} \) be its adjacency matrix, where \( A_{ij} = 1 \) if and only if there is an edge from vertex \( i \) to vertex \( j \). Show that the entry \( (A^k)_{ij} \) equals the number of directed walks of length \( k \) from vertex \( i \) to vertex \( j \).

Solution.

For \( k = 1 \) the statement holds by definition. Assume it holds for some \( k \). Then

\[ (A^{k+1})_{ij} = \sum_{\ell=1}^n (A^k)_{i\ell} A_{\ell j}. \]

For each intermediate vertex \( \ell \), \( (A^k)_{i\ell} \) counts the number of walks of length \( k \) from \( i \) to \( \ell \), and \( A_{\ell j} \) indicates whether there is an edge from \( \ell \) to \( j \). Thus each term in the sum counts walks of length \( k+1 \) reaching \( j \) via \( \ell \). Summing over all \( \ell \) yields the total number of walks of length \( k+1 \), completing the induction.

Problem 4 (Effect of Grid Resolution).

Let \( \mathcal{C}_{\text{free}} \subset \mathbb{R}^n \) and consider a regular grid discretization \( X_h \) and graph \( G_h \) as in Section 4. Assume the continuous optimal path length between \( q_{\text{init}} \) and \( q_{\text{goal}} \) is \( L^\star \), with clearance \( \delta > 0 \). Argue qualitatively (no need for a full proof) that as \( h \to 0 \), the optimal discrete path length \( L_h^\star \) converges to \( L^\star \).

Solution.

Clearance implies that a tube of radius \( \delta \) around the optimal continuous path is collision-free. For sufficiently small \( h \), every small region of the tube contains at least one grid point, so we can approximate the continuous path by a polyline whose vertices lie on \( X_h \). As \( h \) shrinks, the approximation error in path length can be made arbitrarily small. On the other hand, any discrete path in \( G_h \) corresponds to a continuous path whose length is at least \( L^\star \). Thus \( L_h^\star \to L^\star \) as \( h \to 0 \).

Problem 5 (Finite Branching and Complexity).

Suppose \( G = (V,E) \) is a finite directed graph where each vertex has out-degree at most \( b \). Let \( d \) be the minimal number of edges in any path from start to goal. Give a big-O bound on the worst-case number of vertices that BFS must explore to discover an optimal path.

Solution.

BFS explores vertices level by level. At depth \( k \), there are at most

\[ 1 + b + b^2 + \dots + b^{k} = \frac{b^{k+1} - 1}{b - 1} \]

vertices reachable from the start. To discover an optimal path of length \( d \), BFS must expand all vertices up to depth \( d \), giving a worst-case complexity of \( \mathcal{O}(b^{d}) \). This exponential dependence on depth is a fundamental motivation for more informed search methods (studied in the next lesson).

In this lesson we defined rigorous discrete planning models for high-DOF robots and showed how they induce directed weighted graphs. We formalized the equivalence between deterministic planning and shortest paths, and examined how discretization of configuration space yields finite graphs whose optimal paths approximate continuous solutions under clearance assumptions. We also connected these formulations to practical implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. Subsequent lessons build on this foundation to develop heuristic search algorithms and lattice-based planners suitable for large, structured discrete state spaces.