From Chapter 1, we know that a robot's configuration space (C-space) is a manifold in which feasible configurations respect kinematic and dynamic constraints. In this chapter we assume we have already discretized C-space (e.g., via sampling or lattices) into a graph \( G = (V,E) \), where vertices are robot configurations and edges approximate locally feasible motions, each with nonnegative cost \( c(e) \ge 0 \).

A path \( P = (v_0,\dots,v_k) \) from start \( s = v_0 \) to goal \( t = v_k \) has accumulated cost

\[ J(P) = \sum_{i=0}^{k-1} c(v_i,v_{i+1}), \quad c(v_i,v_{i+1}) \ge 0. \]

Classical algorithms like Dijkstra's search are uninformed: they use only past cost \( g(v) \) and explore outward roughly in cost-contours, which is expensive in high-DOF spaces. Heuristic search introduces a function

\[ h : V \to \mathbb{R}_{\ge 0}, \quad h(v) \approx \text{optimal cost-to-go } h^*(v) = \min_{P:v \rightsquigarrow t} J(P), \]

that predicts remaining cost from a vertex to the goal. The quality and structural properties of \( h \) strongly influence the efficiency and optimality of heuristic search algorithms.

We formalize two key properties for heuristics:

• Admissibility:

  \[ \forall v \in V : 0 \le h(v) \le h^*(v). \]

  The heuristic never overestimates true cost-to-go.
• Consistency (monotonicity):

  \[ \forall (v,u)\in E : h(v) \le c(v,u) + h(u), \quad h(t) = 0. \]

  Heuristic estimates respect the triangle inequality along edges.

In high-DOF robotics, heuristics often arise from:

• Joint-space metrics (e.g., weighted Euclidean distance in configuration space).
• Workspace metrics on end-effector pose (using forward kinematics).
• Relaxations that ignore obstacles, dynamics, or differential constraints.

A* augments Dijkstra's search with heuristic information by ranking nodes using an evaluation function \( f(v) = g(v) + h(v) \), where \( g(v) \) is the current best-known cost of a path from \( s \) to \( v \). At any time, the algorithm expands the node in the frontier with minimal \( f \).

A* (graph-search variant):

1. Initialize OPEN with \( s \), set \( g(s) = 0 \), \( f(s)=h(s) \), and CLOSED empty.
2. While OPEN is not empty:
   1. Pop \( v \) from OPEN with minimal \( f(v) \).
   2. If \( v = t \), terminate and reconstruct the path using parent pointers.
   3. Add \( v \) to CLOSED.
   4. For each neighbor \( u \) of \( v \):
      • Compute tentative \( g_{\text{tent}} = g(v) + c(v,u) \).
      • If \( g_{\text{tent}} < g(u) \) (or \( u \) was unseen), update \( g(u) \), parent, and \( f(u) = g(u) + h(u) \), and insert/adjust \( u \) in OPEN.

The algorithm maintains two invariants: (i) each time a node is expanded, \( g(v) \) is the cost of the best known path reaching it, and (ii) for consistent heuristics, nodes never need to be re-opened.

flowchart TD
  S["Start: g(s)=0, f(s)=h(s), OPEN={s}"] --> L["While OPEN not empty"]
  L --> P["Pop v with smallest f(v) from OPEN"]
  P --> GQ{"v is goal t?"}
  GQ -->|yes| DONE["Return reconstructed \npath to t"]
  GQ -->|no| EXPAND["Add v to CLOSED; \nfor each neighbor u of v"]
  EXPAND --> CHECK["Compute g_tent = g(v)+c(v,u)"]
  CHECK --> IMP{"g_tent < g(u) ?"}
  IMP -->|yes| UPDATE["Update g(u), parent(u), \nf(u)=g(u)+h(u); \npush/adjust u in OPEN"]
  IMP -->|no| SKIP["Skip u"]
  UPDATE --> L
  SKIP --> L
        

2.1 Optimality of A*

Assume all edge costs are strictly positive \( c(e) > 0 \) and \( h \) is admissible. Let \( C^* \) denote the cost of an optimal path from \( s \) to \( t \).

\[ C^* = \min_{P:s \rightsquigarrow t} J(P). \]

For any vertex \( v \) on some optimal path, let \( g^*(v) \) be the cost from \( s \) to \( v \) along that path and \( h^*(v) \) the optimal cost from \( v \) to \( t \). Then

\[ g^*(v) + h^*(v) = C^*. \]

Because \( h \) is admissible,

\[ \forall v \in V : h(v) \le h^*(v) \quad \Rightarrow \quad f(v) = g(v)+h(v) \le g^*(v) + h^*(v) = C^*, \]

for all \( v \) that lie on at least one optimal path and for which \( g(v) = g^*(v) \) has been discovered.

Suppose for contradiction that A* terminates with a suboptimal goal path of cost \( C > C^* \). Let \( v \) be the first node on some optimal path not expanded by A* at termination. Its predecessor \( p \) on that optimal path must have been expanded earlier (since A* expands nodes in nondecreasing \( f \), and edge costs are positive).

When \( p \) was expanded, the algorithm generated \( v \) with

\[ g(v) = g^*(v), \quad f(v) = g^*(v) + h(v) \le g^*(v) + h^*(v) = C^*. \]

At termination, the goal node \( t \) with suboptimal cost \( C \) was selected from OPEN. Because A* always pops the node with smallest \( f \):

\[ f(t) = C \le f(v) \le C^*, \]

which contradicts \( C > C^* \). Hence the path returned by A* is optimal.

With a consistent heuristic, we additionally obtain that no node needs to be re-opened: the first time it is removed from OPEN, its \( g \)-value is final, and A* behaves like a label-setting algorithm (in the sense of Dijkstra).

In high-DOF robots operating with onboard perception, the cost structure of the graph \( G \) can change online: an unknown obstacle appears in C-space, a narrow passage is revealed, or some region becomes traversable. Re-running A* from scratch after each local map update would be too expensive.

The D* family (Dynamic A*) maintains and incrementally repairs solution paths as edge costs change. Modern variants like D* Lite maintain for each vertex \( v \):

• \( g(v) \): current best path cost estimate.
• \( \mathrm{rhs}(v) \): one-step lookahead cost, e.g.

  \[ \mathrm{rhs}(v) = \begin{cases} 0, & v = t, \\ \min_{u \in \text{Succ}(v)} \bigl( c(v,u) + g(u) \bigr), & v \neq t, \end{cases} \]

  where \( t \) is the goal.

The key idea is to maintain consistency conditions \( g(v) = \mathrm{rhs}(v) \) for all vertices along the current tree. A priority queue orders inconsistent vertices by a key derived from \( \min(g(v),\mathrm{rhs}(v)) + h_{\text{start}}(v) \) so that updates are focused near changed edges and along the current path.

flowchart TD
  INIT["Initialize search from goal t (backward)"] --> EXEC["Robot executes current path from s"]
  EXEC --> SENSE["Perception updates \nlocal costs c(v,u)"]
  SENSE --> CHG{"Any edge \ncost changed?"}
  CHG -->|no| CONT["Continue executing current path"]
  CONT --> EXEC
  CHG -->|yes| QUEUE["Insert affected vertices \nin priority queue"]
  QUEUE --> REPAIR["Process queue, \nupdate g and rhs"]
  REPAIR --> NEWPATH["Extract repaired path \nfrom s to t"]
  NEWPATH --> EXEC
        

For high-DOF robots, D*-style algorithms are particularly useful when a global lattice or roadmap is precomputed, but detailed local collision information is only available near the currently executed path. D* reuses costs from previous searches, resulting in substantially fewer node expansions relative to replanning with A* from scratch after each update.

In time-critical robotic applications (e.g., motion planning under hard control-loop deadlines), we often prefer to obtain a feasible path quickly, then improve it if additional time becomes available. Anytime A* achieves this by using an inflated heuristic and gradually tightening suboptimality bounds.

4.1 Weighted A*

Given a heuristic \( h \) and inflation factor \( \varepsilon \ge 1 \), define

\[ f_\varepsilon(v) = g(v) + \varepsilon\, h(v). \]

Weighted A* is simply A* with \( f_\varepsilon \) in place of \( f \). If \( h \) is admissible, Weighted A* returns a path whose cost \( C_\varepsilon \) satisfies

\[ C_\varepsilon \le \varepsilon\, C^*. \]

Sketch proof: For an optimal path with cost \( C^* \), at least one node \( v \) on that path in the OPEN list always satisfies

\[ f_\varepsilon(v) = g(v) + \varepsilon h(v) \le g^*(v) + \varepsilon h^*(v) \le \varepsilon (g^*(v)+h^*(v)) = \varepsilon C^*, \]

using admissibility \( h(v) \le h^*(v) \) and \( h^*(v) \le C^* \). Weighted A* only stops when the best-node key in OPEN is at least \( g(t) \), implying \( g(t) \le \varepsilon C^* \).

4.2 Anytime A*

Anytime A* runs a sequence of Weighted A* searches with a decreasing inflation schedule

\[ \varepsilon_0 > \varepsilon_1 > \dots > 1, \]

while reusing OPEN/CLOSED information from previous runs. After each completed run \( k \) it yields a solution with \( C_k \le \varepsilon_k C^* \). The sequence \( \{C_k\} \) is nonincreasing, and in the limit, \( \varepsilon_k \to 1 \) gives an optimal path.

For robots, Anytime A* lets us:

• Quickly obtain a safe, kinematically feasible path.
• Refine the path as time permits (e.g., between control cycles).
• Expose explicit performance vs. optimality trade-offs to higher-level planners.

Heuristic design is the main way domain knowledge enters A*/D*/Anytime A*. In high-DOF manipulators, we typically have a cost model based on joint motion, such as:

\[ J(P) \approx \int_0^T \sqrt{ \sum_{i=1}^n w_i \dot{q}_i(t)^2 }\,dt, \]

where \( q \) is the joint configuration, and \( w_i \) are joint weights (e.g., accounting for gear ratios or power limits). A natural admissible heuristic between two configurations \( q \) and \( q_{\text{goal}} \) is the weighted joint distance:

\[ h(q) = d_q(q,q_{\text{goal}}) = \sqrt{\sum_{i=1}^n w_i (q_i - q_{\text{goal},i})^2 }. \]

If each edge cost lower-bounds the local arc length in this metric, then any path must be at least \( d_q \) long, making \( h \) admissible.

For tasks specified in workspace (end-effector pose), we can build heuristics from the task space:

• Let \( x(q) \) be the end-effector pose (position or pose vector). Then define

  \[ h(q) = \alpha \,\| x(q) - x_{\text{goal}} \|, \]

  with a scaling factor \( \alpha \) chosen so that \( h \) remains a lower bound on path cost.
• For redundant manipulators, we can embed IK relaxation: ignore joint limits and collisions when computing a cheap estimate \( \tilde{q}_{\text{goal}} \), and use \( d_q(q,\tilde{q}_{\text{goal}}) \) as a heuristic.

In practice, high-quality heuristics for robots often combine several components: joint-space distances, projected distances in task space, and even precomputed lower bounds from simplified models (e.g., ignoring some joints or obstacles).

Python is widely used in robotics for prototyping, often together with ROS, networkx, and Python bindings of OMPL. Below is a self-contained A* implementation on a generic graph. This can be wrapped around a discrete C-space lattice or around an OMPL roadmap for debugging.

import heapq
from typing import Dict, Tuple, Callable, Hashable, Iterable

Node = Hashable
Graph = Dict[Node, Dict[Node, float]]  # adjacency: g[u][v] = cost(u,v)

def astar(
    graph: Graph,
    start: Node,
    goal: Node,
    heuristic: Callable[[Node], float]
) -> Tuple[float, Tuple[Node, ...]]:
    """A* search on a directed weighted graph."""
    open_heap = []  # (f, g, node)
    heapq.heappush(open_heap, (heuristic(start), 0.0, start))
    g_cost = {start: 0.0}
    parent: Dict[Node, Node] = {}

    closed = set()

    while open_heap:
        f, g, v = heapq.heappop(open_heap)
        if v in closed:
            continue
        if v == goal:
            # reconstruct path
            path = [v]
            while v in parent:
                v = parent[v]
                path.append(v)
            path.reverse()
            return g, tuple(path)

        closed.add(v)
        for u, c in graph.get(v, {}).items():
            if c < 0:
                raise ValueError("Negative edge costs are not allowed for A*.")
            g_tent = g + c
            if u not in g_cost or g_tent < g_cost[u]:
                g_cost[u] = g_tent
                parent[u] = v
                f_u = g_tent + heuristic(u)
                heapq.heappush(open_heap, (f_u, g_tent, u))

    raise RuntimeError("No path found from start to goal.")

# Example heuristic for a 2D grid C-space (e.g., planar mobile base):
def manhattan_heuristic(goal_xy):
    gx, gy = goal_xy
    def h(node_xy):
        x, y = node_xy
        return abs(x - gx) + abs(y - gy)
    return h
      

To connect a high-DOF manipulator, you can discretize joint space and map nodes to joint vectors q, with edge costs approximating joint distances. For more realistic planning, Python bindings of OMPL allow you to delegate low-level collision checking and kinodynamic constraints while keeping heuristic search logic in Python.

A minimal skeleton for D* Lite-style repair in Python might expose methods update_edge(u, v, new_cost) and a repair() procedure that reprocesses affected nodes in a priority queue. Anytime A* can reuse the astar implementation but call it repeatedly with decreasing heuristic inflation, caching g and OPEN whenever possible.

C++ is the dominant language for high-performance robotics planning libraries such as OMPL and ROS MoveIt. The following snippet shows a minimal A* implementation using std::priority_queue and adjacency lists. In a real system, nodes would wrap robot states (e.g., OMPL ob::State instances, or MoveIt joint states).

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

struct Edge {
    int to;
    double cost;
};

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

struct NodeRecord {
    int v;
    double f;
    double g;
    bool operator<(const NodeRecord& other) const {
        // reversed for min-heap behavior
        return f > other.f;
    }
};

std::pair<double, std::vector<int>>
astar(const Graph& graph,
      int start,
      int goal,
      const std::function<double(int)>& heuristic)
{
    const double INF = std::numeric_limits<double>::infinity();
    std::priority_queue<NodeRecord> open;
    std::vector<double> g(graph.size(), INF);
    std::vector<int> parent(graph.size(), -1);
    std::vector<bool> closed(graph.size(), false);

    g[start] = 0.0;
    open.push({start, heuristic(start), 0.0});

    while (!open.empty()) {
        NodeRecord rec = open.top();
        open.pop();
        int v = rec.v;

        if (closed[v]) continue;
        if (v == goal) {
            // reconstruct path
            std::vector<int> path;
            for (int u = goal; u != -1; u = parent[u])
                path.push_back(u);
            std::reverse(path.begin(), path.end());
            return {rec.g, path};
        }

        closed[v] = true;
        for (const Edge& e : graph[v]) {
            if (e.cost < 0.0) {
                throw std::runtime_error("Negative edge cost not allowed.");
            }
            double g_tent = rec.g + e.cost;
            if (g_tent < g[e.to]) {
                g[e.to] = g_tent;
                parent[e.to] = v;
                double f = g_tent + heuristic(e.to);
                open.push({e.to, f, g_tent});
            }
        }
    }
    throw std::runtime_error("No path found.");
}
      

In OMPL, the internal planners often implement variants of A*/D* for discrete searches over state lattices or roadmaps. You can either use these built-in planners or integrate a custom A*/D* implementation at the task level, using OMPL only for low-level feasibility and distance computation.

Java is less common for low-level robotics but appears in middleware (e.g., ROSJava) and high-level planning tools. A* can be implemented with PriorityQueue and used to plan over discrete abstractions (e.g., symbolic task graphs or simplified C-space grids).

import java.util.*;

class Edge {
    public final int to;
    public final double cost;
    public Edge(int to, double cost) {
        this.to = to;
        this.cost = cost;
    }
}

public class AStar {
    public static class NodeRecord implements Comparable<NodeRecord> {
        public final int v;
        public final double f;
        public final double g;
        public NodeRecord(int v, double f, double g) {
            this.v = v; this.f = f; this.g = g;
        }
        @Override
        public int compareTo(NodeRecord other) {
            return Double.compare(this.f, other.f);
        }
    }

    public static class Result {
        public final double cost;
        public final List<Integer> path;
        public Result(double cost, List<Integer> path) {
            this.cost = cost; this.path = path;
        }
    }

    public static Result astar(
        List<List<Edge>> graph,
        int start,
        int goal,
        java.util.function.DoubleUnaryOperator heuristic)
    {
        int n = graph.size();
        double[] g = new double[n];
        int[] parent = new int[n];
        boolean[] closed = new boolean[n];
        Arrays.fill(g, Double.POSITIVE_INFINITY);
        Arrays.fill(parent, -1);
        PriorityQueue<NodeRecord> open = new PriorityQueue<>();

        g[start] = 0.0;
        open.add(new NodeRecord(start, heuristic.applyAsDouble(start), 0.0));

        while (!open.isEmpty()) {
            NodeRecord rec = open.poll();
            int v = rec.v;
            if (closed[v]) continue;
            if (v == goal) {
                LinkedList<Integer> path = new LinkedList<>();
                for (int u = goal; u != -1; u = parent[u]) {
                    path.addFirst(u);
                }
                return new Result(rec.g, path);
            }
            closed[v] = true;
            for (Edge e : graph.get(v)) {
                if (e.cost < 0.0)
                    throw new IllegalArgumentException("Negative edge cost");
                double gTent = rec.g + e.cost;
                if (gTent < g[e.to]) {
                    g[e.to] = gTent;
                    parent[e.to] = v;
                    double f = gTent + heuristic.applyAsDouble(e.to);
                    open.add(new NodeRecord(e.to, f, gTent));
                }
            }
        }
        throw new RuntimeException("No path found");
    }
}
      

In a robotics stack, Java-based planners might operate at the task level (e.g., picking symbolic actions) and invoke lower-level C++ planners (OMPL/MoveIt) for continuous motion generation, while still leveraging heuristic search principles such as A* and Anytime A*.

9.1 MATLAB Script for Grid-Based A*

MATLAB (with Robotics System Toolbox and Navigation Toolbox) is widely used for rapid prototyping of motion planning algorithms. Below is a basic grid-based A* implementation using a 2D occupancy grid.

function [path, cost] = astar_grid(occGrid, start, goal)
% occGrid: logical matrix, true for obstacle
% start, goal: [row, col]

[nr, nc] = size(occGrid);
INF = inf;

% 4-connected neighborhood
dirs = [ -1 0; 1 0; 0 -1; 0 1 ];

g = INF * ones(nr, nc);
parent = zeros(nr, nc, 2);

    function h = heuristic(cell)
        % Manhattan distance
        h = abs(cell(1) - goal(1)) + abs(cell(2) - goal(2));
    end

open = java.util.PriorityQueue();
startH = heuristic(start);
g(start(1), start(2)) = 0;
open.add({startH, start, 0}); % {f, [r c], g}

closed = false(nr, nc);

while ~open.isEmpty()
    entry = open.poll();
    cell = entry{2};
    r = cell(1); c = cell(2);
    g_val = entry{3};

    if closed(r, c)
        continue;
    end
    if r == goal(1) && c == goal(2)
        % reconstruct path
        path = [r, c];
        while parent(r, c, 1) ~= 0
            pr = parent(r, c, 1);
            pc = parent(r, c, 2);
            path = [pr, pc; path]; %#ok<AGROW>
            r = pr; c = pc;
        end
        cost = g_val;
        return;
    end

    closed(r, c) = true;
    for k = 1:size(dirs, 1)
        rr = r + dirs(k,1);
        cc = c + dirs(k,2);
        if rr < 1 || rr > nr || cc < 1 || cc > nc
            continue;
        end
        if occGrid(rr, cc)
            continue;
        end
        gTent = g_val + 1;
        if gTent < g(rr, cc)
            g(rr, cc) = gTent;
            parent(rr, cc, :) = [r, c];
            f = gTent + heuristic([rr, cc]);
            open.add({f, [rr, cc], gTent});
        end
    end
end

error('No path found');
end
      

In Simulink, one can encapsulate the A* logic in a MATLAB Function block that runs at a slower planning rate, feeding waypoints to a lower-level controller subsystem. D*/Anytime A* can be implemented similarly by exposing cost updates and inflation parameters as block inputs.

9.2 Wolfram Mathematica Implementation

Mathematica has rich graph functionality. A simple way to prototype A* is to explicitly manage OPEN/CLOSED lists while using built-in graph data structures.

Clear[astar]
astar[g_Graph, start_, goal_, h_] := Module[
  {open, closed, gCost, parent, extractPath, v, neighbors, gTent, f},
  
  extractPath[x_] := Module[{p = {x}},
    While[KeyExistsQ[parent, p[[1]]],
      p = Prepend[p, parent[p[[1]]]];
    ];
    p
  ];
  
  open = Association[];
  closed = Association[];
  gCost = Association[start -> 0.0];
  parent = <||>;
  
  open[start] = h[start];
  
  While[Length[open] > 0,
    v = First@First@SortBy[Normal[open], Last];
    open = KeyDrop[open, v];
    If[KeyExistsQ[closed, v], Continue[]];
    If[v === goal,
      Return[{gCost[v], extractPath[v]}];
    ];
    closed[v] = True;
    neighbors = AdjacencyList[g, v];
    Do[
      gTent = gCost[v] + EdgeWeight[g, v \[UndirectedEdge] u];
      If[!KeyExistsQ[gCost, u] || gTent < gCost[u],
        gCost[u] = gTent;
        parent[u] = v;
        f = gTent + h[u];
        open[u] = f;
      ],
      {u, neighbors}
    ];
  ];
  $Failed
]

(* Example heuristic for 2D grid graph with vertex labels {x,y} *)
hGrid[{x_, y_}, goal_] := Module[{gx, gy},
  {gx, gy} = goal;
  Abs[x - gx] + Abs[y - gy]
];
      

This style of implementation is useful for quickly exploring heuristic variants and proving properties symbolically (e.g., admissibility or consistency) using Mathematica's algebraic capabilities.

Problem 1 (Admissibility vs. Consistency): Let \( h_1 \) and \( h_2 \) be two admissible heuristics on the same graph, and define \( h(v) = \max\{h_1(v), h_2(v)\} \).
(a) Prove that \( h \) is admissible.
(b) If both \( h_1 \) and \( h_2 \) are consistent, show that \( h \) is consistent.

Solution:

(a) For any vertex \( v \) we have \( h_1(v) \le h^*(v) \) and \( h_2(v) \le h^*(v) \). Hence

\[ h(v) = \max\{h_1(v), h_2(v)\} \le \max\{h^*(v), h^*(v)\} = h^*(v), \]

so \( h \) is admissible. Nonnegativity follows from nonnegativity of \( h_1, h_2 \).

(b) For consistency we must show \( h(v) \le c(v,u) + h(u) \) for every edge \( (v,u) \). Because each \( h_i \) is consistent,

\[ h_i(v) \le c(v,u) + h_i(u), \quad i = 1,2. \]

Taking maxima on both sides,

\[ \max\{h_1(v), h_2(v)\} \le c(v,u) + \max\{h_1(u), h_2(u)\}, \]

which is precisely \( h(v) \le c(v,u)+h(u) \). Hence \( h \) is consistent.

Problem 2 (A* and Node Reexpansions): Consider A* with an admissible but not necessarily consistent heuristic.
(a) Give a condition under which a node must be re-opened.
(b) Explain why consistency implies that A* never needs to reopen nodes.

Solution:

(a) A node \( v \) must be re-opened if after being expanded, a cheaper path is later found, i.e., if there exists a neighbor \( u \) and path such that

\[ g_{\text{new}}(v) = g(u) + c(u,v) < g_{\text{old}}(v). \]

This can occur when the heuristic violates the triangle inequality: \( h(u) > c(u,v) + h(v) \).

(b) If \( h \) is consistent, then for any edge \( (u,v) \) we have

\[ h(u) \le c(u,v) + h(v). \]

Combining this with the definition of \( f \) gives \( f(v) \ge g(v) + h(v) \ge g(u) + c(u,v) + h(v) \) for any path passing through \( u \). The first time \( v \) is removed from OPEN, its \( g \) is minimal and cannot be improved by any later path, so no reexpansion is necessary.

Problem 3 (Suboptimality of Weighted A*): Let Weighted A* with inflation \( \varepsilon \ge 1 \) and admissible heuristic \( h \) return a solution path of cost \( C_\varepsilon \). Prove rigorously that \( C_\varepsilon \le \varepsilon C^* \).

Solution:

Let \( v \) be any node on an optimal path, with \( g^*(v) \) and \( h^*(v) \) defined as before. Then

\[ f_\varepsilon(v) = g(v) + \varepsilon h(v) \le g^*(v) + \varepsilon h^*(v) = \\ g^*(v) + (\varepsilon - 1)h^*(v) + h^*(v) \le (\varepsilon - 1)C^* + C^* = \varepsilon C^*, \]

since \( h(v) \le h^*(v) \le C^* \). Weighted A* always expands the state with minimal \( f_\varepsilon \). At the time the goal \( t \) is chosen for expansion, \( f_\varepsilon(t) = g(t) \), because \( h(t) = 0 \). Moreover,

\[ g(t) = f_\varepsilon(t) \le \min_{v \in \text{OPEN}} f_\varepsilon(v) \le \varepsilon C^*. \]

Thus \( C_\varepsilon = g(t) \le \varepsilon C^* \), establishing the suboptimality bound.

Problem 4 (Heuristic for a 6-DOF Manipulator): Consider a 6-DOF manipulator with joint configuration \( q \in \mathbb{R}^6 \). Edge costs approximate the weighted joint distance along piecewise-linear paths. Show that the heuristic

\[ h(q) = \sqrt{\sum_{i=1}^6 w_i (q_i - q_{\text{goal},i})^2 } \]

is admissible if each edge corresponds to a straight-line segment in joint space and the edge cost is at least the Euclidean length of that segment in the weighted metric.

Solution:

Let a path \( P = (q^{(0)},\dots,q^{(k)}) \) from \( q^{(0)} = q \) to \( q^{(k)} = q_{\text{goal}} \) be composed of edge segments \( \Delta q^{(j)} = q^{(j+1)} - q^{(j)} \). By assumption, each edge cost satisfies

\[ c(q^{(j)},q^{(j+1)}) \ge \sqrt{\sum_{i=1}^6 w_i (\Delta q^{(j)}_i)^2 }. \]

Using the triangle inequality for the weighted Euclidean norm:

\[ \sum_{j=0}^{k-1} c(q^{(j)},q^{(j+1)}) \ge \sum_{j=0}^{k-1} \sqrt{\sum_{i=1}^6 w_i (\Delta q^{(j)}_i)^2 } \ge \sqrt{\sum_{i=1}^6 w_i \Bigl(\sum_{j=0}^{k-1} \Delta q^{(j)}_i \Bigr)^2} = h(q), \]

because the total change in joint \( i \) along the path is \( q_{\text{goal},i} - q_i \). Since this holds for every path, the optimal path cost \( h^*(q) \) satisfies \( h^*(q) \ge h(q) \), i.e., the heuristic is admissible.

Problem 5 (Anytime A* Schedule): Suppose Anytime A* uses the schedule \( \varepsilon_k = 1 + 2^{-k} \) for \( k = 0,1,2,\dots \) and that each run terminates with a valid solution. Prove that the sequence of solution costs \( C_k \) converges to \( C^* \).

Solution:

From the suboptimality bound we have for each \( k \):

\[ C^* \le C_k \le \varepsilon_k C^* = \bigl(1 + 2^{-k}\bigr) C^*. \]

As \( k \to \infty \), we have \( \varepsilon_k \to 1 \), so \( \varepsilon_k C^* \to C^* \). By the squeeze theorem, \( C_k \to C^* \). In other words, Anytime A* converges asymptotically to the optimal cost as the heuristic inflation is annealed to 1.

In this lesson we formalized heuristic search on discrete C-space graphs for high-DOF robots. We defined admissible and consistent heuristics and proved that A* with an admissible heuristic is optimal, with consistency further guaranteeing no node reexpansions. We introduced D* as an incremental replanning method suitable for online cost changes, and we analyzed Anytime A* and Weighted A*, including rigorous suboptimality bounds. Finally, we gave multi-language implementations (Python, C++, Java, MATLAB, and Mathematica) and derived joint-space and workspace heuristics appropriate for robotic manipulators.