Consider an \( n \)-DOF revolute manipulator with joint vector \( \mathbf{q} \in \mathbb{R}^n \), joint limits \( q_i^{\min} \le q_i \le q_i^{\max} \), and a uniform joint discretization step \( \Delta_i > 0 \) on joint \( i \). The number of grid points along joint \( i \) is

\[ N_i = 1 + \left\lfloor \frac{q_i^{\max} - q_i^{\min}}{\Delta_i} \right\rfloor , \quad i = 1,\dots,n. \]

The total number of lattice states is the product

\[ |V| = \prod_{i=1}^n N_i . \]

For a 7-DOF manipulator with identical \( N_i = N \), this gives \( |V| = N^7 \). Even for modest \( N=50 \), one obtains \( \approx 7.8\times 10^{11} \) states, far beyond what any graph search can exhaustively explore.

Suppose each joint can change by \( \{-\Delta_i, 0, +\Delta_i\} \) at each step, independently. A naïve branching factor is then

\[ b \le \prod_{i=1}^n 3 - 1 = 3^n - 1, \]

where we subtract 1 to exclude the zero move. If the optimal path has depth \( d \), worst-case search complexity of breadth-first or uniform-cost search scales as \( \mathcal{O}(b^d) \), i.e. doubly exponential in \( n \) when depth itself grows with dimensionality.

Therefore, scaling search to manipulators is fundamentally about structure: designing graphs, motion primitives, and heuristics that dramatically reduce the number of expanded states while preserving completeness and (approximate) optimality guarantees introduced in the previous lessons.

A manipulator lattice is a graph \( G=(V,E) \) whose vertices are discrete joint configurations and whose edges correspond to small, feasible motions (motion primitives).

Let joint angles be discretized to integer indices \( k_i \in \{0,\dots,N_i-1\} \). A discrete configuration is \( \mathbf{k} = (k_1,\dots,k_n) \). A motion primitive \( \mathbf{p} \in \mathbb{Z}^n \) is an integer step such that \( \|\mathbf{p}\|_0 \) (number of nonzero components) is small, typically 1 or 2:

\[ \mathcal{P} \subset \left\{ \mathbf{p}\in\mathbb{Z}^n : \|\mathbf{p}\|_0 \le s,\; \|\mathbf{p}\|_{\infty} \le P_{\max} \right\}. \]

The successor set of a node \( \mathbf{k} \) is

\[ \text{Succ}(\mathbf{k}) = \left\{ \mathbf{k}' = \mathbf{k} + \mathbf{p} : \mathbf{p}\in\mathcal{P},\; 0 \le k'_i \le N_i - 1,\; \mathbf{q}(\mathbf{k}') \in \mathcal{C}_{\text{free}} \right\}, \]

where \( \mathbf{q}(\mathbf{k}) \) maps indices back to real joint angles, and \( \mathcal{C}_{\text{free}} \) is the collision-free subset of configuration space.

A common edge cost for a transition \( \mathbf{k} \to \mathbf{k}' \) is the weighted joint-space displacement:

\[ c(\mathbf{k},\mathbf{k}') = \sum_{i=1}^{n} w_i \left| q_i(\mathbf{k}') - q_i(\mathbf{k}) \right|, \quad w_i > 0. \]

This approximates integral path length in configuration space and allows us to reuse the optimality and completeness notions for weighted graphs discussed in Lesson 4.

flowchart TD
  A["Define joint limits and resolution"] --> B["Build discrete joint grid (states V)"]
  B --> C["Select motion primitives P (single-joint, coupled-joint)"]
  C --> D["Collision checking for each edge (q in C_free)"]
  D --> E["Run heuristic search (A* / Anytime A*)"]
  E --> F["Extract joint path and time-parameterize"]
        

The main levers for scaling are:

• Choosing a sparse but expressive set of primitives \( \mathcal{P} \).
• Efficient collision checking for manipulator configurations.
• Strong heuristics that exploit manipulator kinematics.

Let the goal configuration be \( \mathbf{q}_{\text{goal}} \). A natural class of heuristics is based on joint-space distance:

\[ h_{\ell_p}(\mathbf{q}) = \alpha \left\| \mathbf{q} - \mathbf{q}_{\text{goal}} \right\|_p , \quad p\in\{1,2,\infty\},\; \alpha > 0. \]

Assume that each edge cost satisfies the lower bound

\[ c(\mathbf{q},\mathbf{q}') \ge \underline{c} \, \left\|\mathbf{q}' - \mathbf{q}\right\|_p \quad \text{for all edges}\; (\mathbf{q},\mathbf{q}')\in E, \]

for some constant \( \underline{c} > 0 \), which holds for weighted \( \ell_1 \) cost with \( \underline{c} = \min_i w_i \).

Let \( C^*(\mathbf{q}) \) denote the optimal cost-to-go from \( \mathbf{q} \) to the goal along the lattice. For any path \( \mathbf{q} = \mathbf{q}_0, \mathbf{q}_1,\dots,\mathbf{q}_K = \mathbf{q}_{\text{goal}} \),

\[ \sum_{k=0}^{K-1} c(\mathbf{q}_k,\mathbf{q}_{k+1}) \;\ge\; \underline{c}\sum_{k=0}^{K-1} \left\|\mathbf{q}_{k+1}-\mathbf{q}_k\right\|_p \;\ge\; \underline{c}\left\|\mathbf{q} - \mathbf{q}_{\text{goal}}\right\|_p , \]

where the last inequality uses the triangle inequality. Taking the infimum over all paths,

\[ C^*(\mathbf{q}) \ge \underline{c}\left\|\mathbf{q} - \mathbf{q}_{\text{goal}}\right\|_p . \]

Therefore, choosing \( \alpha \le \underline{c} \) makes \( h_{\ell_p}(\mathbf{q}) \) admissible and, if the heuristic is also consistent, the A* algorithm (from Lesson 2) remains both complete and optimal.

For manipulators with heterogeneous joints, one often uses a weighted \( \ell_2 \) metric

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

where \( w_i \) reflects the effect of joint \( i \) on the end-effector (for example, proportional to the link length). This remains admissible when edge costs dominate the induced distance.

Joint-space distance ignores obstacles but also ignores how much the end-effector actually moves. A more informative heuristic uses task-space (workspace) distance.

Let the forward kinematics map be \( F : \mathcal{C}_{\text{free}} \to \mathbb{R}^3 \times SO(3) \), sending joint angles to end-effector pose. Equip configuration space with some norm \( \|\cdot\|_{\mathcal{C}} \) and task space with metric \( d_{\mathcal{T}}(\cdot,\cdot) \). Suppose \( F \) is globally Lipschitz with constant \( L > 0 \):

\[ d_{\mathcal{T}}\!\left(F(\mathbf{q}_1),F(\mathbf{q}_2)\right) \le L \left\|\mathbf{q}_1-\mathbf{q}_2\right\|_{\mathcal{C}} \quad \text{for all}\; \mathbf{q}_1,\mathbf{q}_2 \in \mathcal{C}_{\text{free}}. \]

If edge costs satisfy \( c(\mathbf{q},\mathbf{q}') \ge \underline{c} \|\mathbf{q}'-\mathbf{q}\|_{\mathcal{C}} \), then for any path from \( \mathbf{q} \) to \( \mathbf{q}_{\text{goal}} \),

\[ C^*(\mathbf{q}) \ge \frac{\underline{c}}{L}\, d_{\mathcal{T}}\!\left(F(\mathbf{q}),F(\mathbf{q}_{\text{goal}})\right). \]

Hence the heuristic

\[ h_{\text{task}}(\mathbf{q}) = \beta\, d_{\mathcal{T}}\!\left(F(\mathbf{q}),F(\mathbf{q}_{\text{goal}})\right), \quad 0 < \beta \le \underline{c}/L, \]

is admissible. In practice, one often estimates \( L \) conservatively from link lengths, e.g. for a planar arm \( L \le \sum_{i=1}^n |l_i| \), and chooses \( \beta = \underline{c}/L \).

Task-space heuristics are significantly more informative for reaching, grasping, and pick-and-place tasks, because they guide the manipulator toward the goal pose even when joint-space distance is misleading (for example, near kinematic singularities).

A powerful way to scale search is to use several graphs of increasing resolution. Let \( G^{(0)}, G^{(1)},\dots,G^{(L)} \) be lattices with resolutions \( \Delta^{(0)} > \Delta^{(1)} > \dots > \Delta^{(L)} \). The coarse graph \( G^{(0)} \) has far fewer states and can be searched quickly, while \( G^{(L)} \) captures fine motion detail.

A typical hierarchical algorithm:

1. Search on \( G^{(0)} \) to obtain a coarse path \( \pi^{(0)} \).
2. Project \( \pi^{(0)} \) to the next finer lattice \( G^{(1)} \), constraining search to a corridor around it.
3. Repeat until the finest lattice \( G^{(L)} \) is solved.

Assume that for each level \( \ell \), the projection of any path on \( G^{(\ell)} \) into \( G^{(\ell+1)} \) lies within the allowed search corridor. Then the hierarchical procedure is complete relative to the finest lattice: if a path exists in \( G^{(L)} \), the algorithm can find it (resolution-completeness).

flowchart TD
  C0["Coarse lattice G(0) (large steps)"] --> P0["Plan coarse path pi(0)"]
  P0 --> C1["Refine around pi(0) on G(1)"]
  C1 --> P1["Plan refined path pi(1)"]
  P1 --> CL["Repeat until finest lattice G(L)"]
  CL --> F["Final high-resolution joint path"]
        

Hierarchical search combines well with Anytime A* and Weighted A* (Lesson 2): we can accept a suboptimal coarse solution quickly, then refine it while gradually reducing the weight factor to recover near-optimality on the finest lattice.

In Python, manipulator search is often integrated with MoveIt and OMPL through ROS. Here we implement a minimal 7-DOF lattice planner from scratch, and indicate where real systems would call the robotics stack for collision checks.

import heapq
import math
from typing import Tuple, List, Dict

# 7-DOF joint limits (radians)
JOINT_LIMITS = [
    (-2.9, 2.9),
    (-2.0, 2.0),
    (-2.0, 2.0),
    (-2.9, 2.9),
    (-2.0, 2.0),
    (-2.0, 2.0),
    (-3.1, 3.1),
]

RESOLUTION = [0.1] * 7  # joint discretization

def discretize(q: List[float]) -> Tuple[int, ...]:
    idx = []
    for i, (q_min, q_max) in enumerate(JOINT_LIMITS):
        delta = RESOLUTION[i]
        k = int(round((q[i] - q_min) / delta))
        # clamp to grid
        max_k = int(round((q_max - q_min) / delta))
        k = max(0, min(k, max_k))
        idx.append(k)
    return tuple(idx)

def undisc(k: Tuple[int, ...]) -> List[float]:
    q = []
    for i, ki in enumerate(k):
        q_min, q_max = JOINT_LIMITS[i]
        delta = RESOLUTION[i]
        q.append(q_min + ki * delta)
    return q

def neighbors(k: Tuple[int, ...]) -> List[Tuple[int, ...]]:
    # single-joint +/- 1-step primitives
    nbrs = []
    for i in range(len(k)):
        for step in (-1, 1):
            kk = list(k)
            kk[i] += step
            # bounds in index space
            q_min, q_max = JOINT_LIMITS[i]
            delta = RESOLUTION[i]
            max_k = int(round((q_max - q_min) / delta))
            if 0 <= kk[i] <= max_k:
                q = undisc(kk)
                if is_collision_free(q):  # hook for robotics middleware
                    nbrs.append(tuple(kk))
    return nbrs

def edge_cost(k1: Tuple[int, ...], k2: Tuple[int, ...]) -> float:
    q1 = undisc(k1)
    q2 = undisc(k2)
    # weighted L1 distance
    w = [1.0] * len(q1)
    return sum(w[i] * abs(q2[i] - q1[i]) for i in range(len(q1)))

def h_joint(k: Tuple[int, ...], goal_k: Tuple[int, ...]) -> float:
    q = undisc(k)
    qg = undisc(goal_k)
    # Euclidean joint-space heuristic (admissible if edge_cost dominates it)
    return math.sqrt(sum((qi - qgi) ** 2 for qi, qgi in zip(q, qg)))

def is_collision_free(q: List[float]) -> bool:
    # Placeholder: in a real system, call MoveIt/OMPL or a physics engine.
    # Here we pretend the whole space is free.
    return True

def astar(start_q: List[float], goal_q: List[float]) -> List[List[float]]:
    start_k = discretize(start_q)
    goal_k = discretize(goal_q)

    open_heap: List[Tuple[float, Tuple[int, ...]]] = []
    heapq.heappush(open_heap, (0.0, start_k))

    g_score: Dict[Tuple[int, ...], float] = {start_k: 0.0}
    parent: Dict[Tuple[int, ...], Tuple[int, ...]] = {}

    while open_heap:
        f, current = heapq.heappop(open_heap)
        if current == goal_k:
            # reconstruct path
            path_k = [current]
            while current in parent:
                current = parent[current]
                path_k.append(current)
            path_k.reverse()
            return [undisc(k) for k in path_k]

        for nb in neighbors(current):
            tentative_g = g_score[current] + edge_cost(current, nb)
            if tentative_g < g_score.get(nb, float("inf")):
                g_score[nb] = tentative_g
                parent[nb] = current
                f_nb = tentative_g + h_joint(nb, goal_k)
                heapq.heappush(open_heap, (f_nb, nb))

    raise RuntimeError("No path found")

if __name__ == "__main__":
    q_start = [0.0, -1.0, 0.5, 0.0, 0.0, 0.5, 0.0]
    q_goal  = [1.0,  0.5, 0.0, 0.5, 0.0, 1.0, 0.0]
    path = astar(q_start, q_goal)
    print("Path length (states):", len(path))
      

In practice, is_collision_free would invoke a robot description (URDF/SRDF), an environment representation, and a collision checker from MoveIt/OMPL or similar libraries, but the search logic remains exactly as shown.

In modern C++ robotics software (e.g., ROS with MoveIt and OMPL), the typical pattern is: use OMPL for continuous-space planning, and optionally layer a custom lattice on top for structured, search-based planning. Below is a minimal stand-alone A* implementation for a manipulator lattice; in a real system, collision checking and kinematics would be delegated to MoveIt/OMPL.

#include <array>
#include <vector>
#include <queue>
#include <unordered_map>
#include <cmath>

constexpr int DOF = 7;

struct Node {
    std::array<int, DOF> k;
};

struct NodeHash {
    std::size_t operator()(Node const& n) const noexcept {
        std::size_t h = 0;
        for (int i = 0; i < DOF; ++i) {
            h = h * 131u + static_cast<std::size_t>(n.k[i]);
        }
        return h;
    }
};

struct NodeEq {
    bool operator()(Node const& a, Node const& b) const noexcept {
        for (int i = 0; i < DOF; ++i)
            if (a.k[i] != b.k[i]) return false;
        return true;
    }
};

struct JointLimit {
    double q_min;
    double q_max;
    double delta;
};

static JointLimit JOINT_LIMITS[DOF] = {
    {-2.9, 2.9, 0.1},
    {-2.0, 2.0, 0.1},
    {-2.0, 2.0, 0.1},
    {-2.9, 2.9, 0.1},
    {-2.0, 2.0, 0.1},
    {-2.0, 2.0, 0.1},
    {-3.1, 3.1, 0.1}
};

std::array<double, DOF> undisc(Node const& n) {
    std::array<double, DOF> q{};
    for (int i = 0; i < DOF; ++i) {
        q[i] = JOINT_LIMITS[i].q_min + n.k[i] * JOINT_LIMITS[i].delta;
    }
    return q;
}

bool isCollisionFree(std::array<double, DOF> const& q) {
    // TODO: call MoveIt/OMPL collision checker here.
    return true;
}

std::vector<Node> neighbors(Node const& n) {
    std::vector<Node> nbrs;
    for (int i = 0; i < DOF; ++i) {
        for (int step : {-1, 1}) {
            Node nb = n;
            nb.k[i] += step;
            int max_k = static_cast<int>(
                std::round((JOINT_LIMITS[i].q_max - JOINT_LIMITS[i].q_min)
                           / JOINT_LIMITS[i].delta)
            );
            if (nb.k[i] < 0 || nb.k[i] > max_k) continue;
            auto q = undisc(nb);
            if (isCollisionFree(q)) {
                nbrs.push_back(nb);
            }
        }
    }
    return nbrs;
}

double edgeCost(Node const& a, Node const& b) {
    auto qa = undisc(a);
    auto qb = undisc(b);
    double cost = 0.0;
    for (int i = 0; i < DOF; ++i)
        cost += std::abs(qb[i] - qa[i]);
    return cost;
}

double h(Node const& n, Node const& goal) {
    auto q = undisc(n);
    auto qg = undisc(goal);
    double s = 0.0;
    for (int i = 0; i < DOF; ++i) {
        double d = q[i] - qg[i];
        s += d * d;
    }
    return std::sqrt(s);
}

struct PQItem {
    double f;
    Node node;
    bool operator<(PQItem const& other) const {
        // reverse for min-heap in std::priority_queue
        return f > other.f;
    }
};

std::vector<Node> astar(Node const& start, Node const& goal) {
    std::priority_queue<PQItem> open;
    open.push({0.0, start});

    std::unordered_map<Node, double, NodeHash, NodeEq> g_score;
    std::unordered_map<Node, Node, NodeHash, NodeEq> parent;
    g_score[start] = 0.0;

    while (!open.empty()) {
        PQItem top = open.top();
        open.pop();
        Node current = top.node;

        if (NodeEq{}(current, goal)) {
            std::vector<Node> path;
            path.push_back(current);
            auto it = parent.find(current);
            while (it != parent.end()) {
                current = it->second;
                path.push_back(current);
                it = parent.find(current);
            }
            std::reverse(path.begin(), path.end());
            return path;
        }

        for (auto& nb : neighbors(current)) {
            double tentative_g = g_score[current] + edgeCost(current, nb);
            auto it = g_score.find(nb);
            if (it == g_score.end() || tentative_g < it->second) {
                g_score[nb] = tentative_g;
                parent[nb] = current;
                double f = tentative_g + h(nb, goal);
                open.push({f, nb});
            }
        }
    }
    throw std::runtime_error("No path found");
}
      

Libraries such as OMPL can be used to construct more sophisticated state spaces (e.g., ompl::base::SE3StateSpace for the end-effector) and to reuse collision-checking and distance computation, while keeping the search logic modular.

Java is often used in conjunction with ROS via rosjava for higher-level coordination and monitoring. Below is a minimal Java implementation of A* on a manipulator lattice that could be wrapped as a rosjava node.

import java.util.*;
import java.util.stream.Collectors;

public class ManipAStar {

    public static final int DOF = 7;

    static class JointLimit {
        double qMin, qMax, delta;
        JointLimit(double qMin, double qMax, double delta) {
            this.qMin = qMin; this.qMax = qMax; this.delta = delta;
        }
    }

    static JointLimit[] LIMITS = new JointLimit[] {
        new JointLimit(-2.9, 2.9, 0.1),
        new JointLimit(-2.0, 2.0, 0.1),
        new JointLimit(-2.0, 2.0, 0.1),
        new JointLimit(-2.9, 2.9, 0.1),
        new JointLimit(-2.0, 2.0, 0.1),
        new JointLimit(-2.0, 2.0, 0.1),
        new JointLimit(-3.1, 3.1, 0.1)
    };

    static class Node {
        int[] k;
        Node(int[] k) {
            this.k = k;
        }
        @Override
        public boolean equals(Object o) {
            if (!(o instanceof Node)) return false;
            return Arrays.equals(k, ((Node)o).k);
        }
        @Override
        public int hashCode() {
            return Arrays.hashCode(k);
        }
    }

    static Node discretize(double[] q) {
        int[] k = new int[DOF];
        for (int i = 0; i < DOF; ++i) {
            JointLimit lim = LIMITS[i];
            int idx = (int)Math.round((q[i] - lim.qMin) / lim.delta);
            int maxK = (int)Math.round((lim.qMax - lim.qMin) / lim.delta);
            if (idx < 0) idx = 0;
            if (idx > maxK) idx = maxK;
            k[i] = idx;
        }
        return new Node(k);
    }

    static double[] undisc(Node n) {
        double[] q = new double[DOF];
        for (int i = 0; i < DOF; ++i) {
            JointLimit lim = LIMITS[i];
            q[i] = lim.qMin + n.k[i] * lim.delta;
        }
        return q;
    }

    static boolean isCollisionFree(double[] q) {
        // TODO: integrate with a collision checker (e.g., via rosjava and MoveIt).
        return true;
    }

    static List<Node> neighbors(Node n) {
        List<Node> nbrs = new ArrayList<>();
        for (int i = 0; i < DOF; ++i) {
            for (int step : new int[]{-1, 1}) {
                int[] kk = Arrays.copyOf(n.k, DOF);
                kk[i] += step;
                JointLimit lim = LIMITS[i];
                int maxK = (int)Math.round((lim.qMax - lim.qMin) / lim.delta);
                if (kk[i] < 0 || kk[i] > maxK) continue;
                double[] q = undisc(new Node(kk));
                if (isCollisionFree(q)) {
                    nbrs.add(new Node(kk));
                }
            }
        }
        return nbrs;
    }

    static double edgeCost(Node a, Node b) {
        double[] qa = undisc(a);
        double[] qb = undisc(b);
        double s = 0.0;
        for (int i = 0; i < DOF; ++i) {
            s += Math.abs(qb[i] - qa[i]);
        }
        return s;
    }

    static double h(Node n, Node goal) {
        double[] q = undisc(n);
        double[] qg = undisc(goal);
        double s = 0.0;
        for (int i = 0; i < DOF; ++i) {
            double d = q[i] - qg[i];
            s += d * d;
        }
        return Math.sqrt(s);
    }

    public static List<double[]> astar(double[] qStart, double[] qGoal) {
        Node start = discretize(qStart);
        Node goal = discretize(qGoal);

        PriorityQueue<Node> open = new PriorityQueue<>(
            Comparator.comparingDouble(n -> gScore.getOrDefault(n, Double.POSITIVE_INFINITY)
                    + h(n, goal))
        );
        open.add(start);

        gScore = new HashMap<>();
        parent = new HashMap<>();
        gScore.put(start, 0.0);

        while (!open.isEmpty()) {
            Node current = open.poll();
            if (current.equals(goal)) {
                List<Node> pathNodes = new ArrayList<>();
                pathNodes.add(current);
                while (parent.containsKey(current)) {
                    current = parent.get(current);
                    pathNodes.add(current);
                }
                Collections.reverse(pathNodes);
                return pathNodes.stream()
                        .map(ManipAStar::undisc)
                        .collect(Collectors.toList());
            }

            for (Node nb : neighbors(current)) {
                double tentativeG = gScore.get(current) + edgeCost(current, nb);
                double oldG = gScore.getOrDefault(nb, Double.POSITIVE_INFINITY);
                if (tentativeG < oldG) {
                    gScore.put(nb, tentativeG);
                    parent.put(nb, current);
                    if (!open.contains(nb)) {
                        open.add(nb);
                    }
                }
            }
        }
        throw new RuntimeException("No path found");
    }

    private static Map<Node, Double> gScore;
    private static Map<Node, Node> parent;

    public static void main(String[] args) {
        double[] qStart = {0.0, -1.0, 0.5, 0.0, 0.0, 0.5, 0.0};
        double[] qGoal  = {1.0,  0.5, 0.0, 0.5, 0.0, 1.0, 0.0};
        List<double[]> path = astar(qStart, qGoal);
        System.out.println("Path length: " + path.size());
    }
}
      

This code demonstrates that the core scaling issues (branching factor, heuristic choice) are language-independent; the robotics middleware primarily supplies kinematics and collision checking services.

MATLAB with Robotics System Toolbox offers built-in kinematics and collision checking for widely used manipulators. Below is a simplified lattice A* implementation using pure MATLAB code; this logic can be wrapped into a Simulink MATLAB Function block inside a larger manipulation pipeline.

function path = planManipAStar(startQ, goalQ, rigidBodyTree)
% PLANMANIPASTAR Lattice A* in joint space for a manipulator.
%   startQ, goalQ: 1xn joint vectors (radians)
%   rigidBodyTree: robotics.RigidBodyTree for collision checking

n = numel(startQ);
delta = 0.1 * ones(1, n);  % resolution

% joint limits from model
qlim = rigidBodyTree.homeConfiguration;
qMin = zeros(1, n);
qMax = zeros(1, n);
for i = 1:n
    qMin(i) = rigidBodyTree.Bodies{i}.Joint.PositionLimits(1);
    qMax(i) = rigidBodyTree.Bodies{i}.Joint.PositionLimits(2);
end

discretize = @(q) round((q - qMin) ./ delta);
undisc = @(k) qMin + k .* delta;

startK = discretize(startQ);
goalK  = discretize(goalQ);

open = java.util.PriorityQueue();
key = @(kvec) sprintf('%d_', kvec);
gScore = containers.Map();
parent = containers.Map('KeyType','char','ValueType','char');

gScore(key(startK)) = 0.0;
open.add({0.0, startK});  % {f, k}

while ~open.isEmpty()
    item = open.remove();
    f = item{1}; %#ok<NASGU>
    k = item{2};
    if all(k == goalK)
        % reconstruct
        pathK = k;
        kstr = key(k);
        while parent.isKey(kstr)
            kstr = parent(kstr);
            toks = sscanf(kstr, '%d_');
            pathK = [toks.'; pathK]; %#ok<AGROW>
        end
        % map to joint space
        path = zeros(size(pathK,1), n);
        for i = 1:size(pathK,1)
            path(i,:) = undisc(pathK(i,:));
        end
        return;
    end

    % neighbors: single joint +/-1 step
    for i = 1:n
        for step = [-1, 1]
            kk = k;
            kk(i) = kk(i) + step;
            if kk(i) <= round((qMin(i) - qMin(i)) / delta(i)) || ...
               kk(i) >= round((qMax(i) - qMin(i)) / delta(i))
                continue;
            end
            q = undisc(kk);
            % collision check via Robotics System Toolbox
            inCollision = checkCollision(rigidBodyTree, q, ...
                                         'IgnoreSelfCollision','on');
            if inCollision
                continue;
            end
            gOld = inf;
            kKey = key(kk);
            if isKey(gScore, kKey)
                gOld = gScore(kKey);
            end
            gNew = gScore(key(k)) + sum(abs(undisc(kk) - undisc(k)));
            if gNew < gOld
                gScore(kKey) = gNew;
                parent(kKey) = key(k);
                % heuristic: Euclidean joint distance
                h = norm(undisc(kk) - undisc(goalK));
                open.add({gNew + h, kk});
            end
        end
    end
end

error('No path found.');
end
      

In Simulink, this function can be invoked in a MATLAB Function block, with the rigidBodyTree provided via a From Workspace block and the resulting joint trajectory streamed into a joint controller block.

Wolfram Mathematica provides high-level graph algorithms (e.g., FindShortestPath) which can be combined with symbolic kinematics to prototype manipulator search quickly.

(* 7-DOF joint limits and resolution *)
jointLimits = {
  {-2.9, 2.9},
  {-2.0, 2.0},
  {-2.0, 2.0},
  {-2.9, 2.9},
  {-2.0, 2.0},
  {-2.0, 2.0},
  {-3.1, 3.1}
};
delta = ConstantArray[0.3, 7];

discretize[q_List] := Round[(q - jointLimits[[All, 1]])/delta];
undisc[k_List] := jointLimits[[All, 1]] + k*delta;

(* adjacency: single-joint moves +/-1 index, collision-free by stub *)
neighbors[k_List] := Module[{nbrs = {}, kk, i, maxK},
  Do[
    Do[
      kk = k;
      kk[[i]] = kk[[i]] + step;
      maxK = Round[(jointLimits[[i, 2]] - jointLimits[[i, 1]])/delta[[i]]];
      If[0 <= kk[[i]] <= maxK,
        (* collision check stub; replace with geometry-based check *)
        AppendTo[nbrs, kk];
      ],
      {step, {-1, 1}}
    ],
    {i, 1, Length[k]}
  ];
  nbrs
];

(* build implicit graph using NearestFunction-like operator *)
Clear[edgeQ];
edgeQ[a_, b_] := MemberQ[neighbors[a], b];

jointDistance[a_, b_] := Norm[undisc[a] - undisc[b]];

ManipPath[startQ_List, goalQ_List] := Module[
  {startK, goalK, pathK},
  startK = discretize[startQ];
  goalK = discretize[goalQ];
  pathK = FindShortestPath[
    startK, goalK,
    Method -> {"GraphSearch", "HeuristicFunction" ->
      (jointDistance[#, goalK] &),
      "EdgeCostFunction" -> (jointDistance[##] &)
    },
    VertexCoordinates -> Automatic,
    VertexNeighbors -> (neighbors[#] &)
  ];
  undisc /@ pathK
];

startQ = {0., -1., 0.5, 0., 0., 0.5, 0.};
goalQ  = {1.,  0.5, 0.0, 0.5, 0., 1.0, 0.};
path = ManipPath[startQ, goalQ];
Length[path]
      

The implicit neighbor function encodes the lattice, while FindShortestPath handles the search with a user-specified heuristic and edge cost. For more realistic models, RegionDistance and geometric regions can be used for collision checking.

Problem 1 (Lattice Size for a 7-DOF Arm): A 7-DOF manipulator has identical joint limits \( q_i^{\min} = -\pi \), \( q_i^{\max} = \pi \) and uniform resolution \( \Delta \) on each joint. Derive the number of lattice states as a function of \( \Delta \). For \( \Delta = 0.05 \) radians, estimate the order of magnitude of \( |V| \).

Solution: For each joint,

\[ N = 1 + \left\lfloor \frac{q^{\max} - q^{\min}}{\Delta} \right\rfloor = 1 + \left\lfloor \frac{2\pi}{\Delta} \right\rfloor . \]

The total number of states for a 7-DOF arm is

\[ |V| = N^7 = \left(1 + \left\lfloor \frac{2\pi}{\Delta} \right\rfloor\right)^7. \]

For \( \Delta = 0.05 \), we have \( 2\pi / 0.05 \approx 125.66 \), so \( N \approx 126 \) and \( |V| \approx 126^7 \approx 4.3 \times 10^{14} \). This illustrates why full uniform lattices are rarely explored exhaustively.

Problem 2 (Admissibility of Joint-Space Heuristic): Assume edge costs satisfy \( c(\mathbf{q},\mathbf{q}') \ge \underline{c} \|\mathbf{q}'-\mathbf{q}\|_2 \). Show that \( h(\mathbf{q}) = \underline{c} \|\mathbf{q}-\mathbf{q}_{\text{goal}}\|_2 \) is an admissible heuristic for A*.

Solution: For any path \( \mathbf{q} = \mathbf{q}_0,\dots,\mathbf{q}_K = \mathbf{q}_{\text{goal}} \),

\[ C^*(\mathbf{q}) \ge \sum_{k=0}^{K-1} c(\mathbf{q}_k,\mathbf{q}_{k+1}) \ge \underline{c} \sum_{k=0}^{K-1} \left\|\mathbf{q}_{k+1}-\mathbf{q}_k\right\|_2 \ge \underline{c} \left\|\mathbf{q}-\mathbf{q}_{\text{goal}}\right\|_2 . \]

Thus \( h(\mathbf{q}) \le C^*(\mathbf{q}) \) for all \( \mathbf{q} \), so \( h \) is admissible.

Problem 3 (Weighted A* Suboptimality Bound): Consider Weighted A* with weight \( w \ge 1 \) and an admissible, consistent heuristic \( h \). Show that the cost \( C_{\text{WA*}} \) of the solution returned by Weighted A* satisfies \( C_{\text{WA*}} \le w C^* \), where \( C^* \) is the optimal cost.

Solution (sketch): Weighted A* uses \( f_w(n) = g(n) + w h(n) \). Let \( n^* \) denote a node on some optimal path with cost \( C^* \). Consistency implies \( f_w(n^*) \le w C^* \) at all times. Weighted A* terminates when a goal node \( n_G \) is popped from the open list, so at that time \( f_w(n_G) = g(n_G) \) (since \( h(n_G)=0 \)) is less than or equal to all remaining \( f_w \) values, in particular \( f_w(n_G) \le f_w(n^*) \le w C^* \). Thus

\[ C_{\text{WA*}} = g(n_G) = f_w(n_G) \le w C^* . \]

Hence Weighted A* is \( w \)-suboptimal.

Problem 4 (Completeness of Hierarchical Search): Let \( G^{(0)},\dots,G^{(L)} \) be lattices with resolutions \( \Delta^{(\ell)} \) as in Section 5. Assume that for any path \( \pi^{(L)} \) on the finest lattice there exists a sequence of projections \( \pi^{(L-1)},\dots,\pi^{(0)} \) such that \( \pi^{(\ell)} \) lies inside the search corridor used at level \( \ell \). Show that if search at each level is complete with respect to its corridor, then the overall hierarchical algorithm is complete on \( G^{(L)} \).

Solution: Let \( \pi^{(L)} \) be any solution path on the finest lattice. By assumption, there is a coarse-level projection \( \pi^{(0)} \) reachable within the \( G^{(0)} \) corridor. Since search at level 0 is complete within this corridor, it will find some path within it; if multiple paths exist, at least one is compatible with a projection of \( \pi^{(L)} \). Repeating this argument inductively for levels \( 1,\dots,L \) shows that a compatible path exists inside the corridor at each level and will be discovered by the complete corridor search. Hence a projection of \( \pi^{(L)} \) will be found on the finest lattice, implying completeness with respect to \( G^{(L)} \).

Problem 5 (Task-Space vs Joint-Space Heuristics): Suppose the forward kinematics of a planar 2-link arm with link lengths \( l_1,l_2 > 0 \) satisfies \( \|F(\mathbf{q}_1)-F(\mathbf{q}_2)\|_2 \le (l_1+l_2) \|\mathbf{q}_1-\mathbf{q}_2\|_2 \). With edge cost \( c(\mathbf{q},\mathbf{q}') = \|\mathbf{q}'-\mathbf{q}\|_2 \), show that the heuristic \( h(\mathbf{q}) = \frac{1}{l_1+l_2} \|F(\mathbf{q})-F(\mathbf{q}_{\text{goal}})\|_2 \) is admissible.

Solution: For any path from \( \mathbf{q} \) to \( \mathbf{q}_{\text{goal}} \),

\[ C^*(\mathbf{q}) \ge \left\|\mathbf{q} - \mathbf{q}_{\text{goal}}\right\|_2 \ge \frac{1}{l_1 + l_2} \left\|F(\mathbf{q}) - F(\mathbf{q}_{\text{goal}})\right\|_2 , \]

where the first inequality uses the fact that \( c \) is exactly the \( \ell_2 \) length of the shortest configuration-space path, and the second follows from the Lipschitz bound. Thus \( h(\mathbf{q}) \le C^*(\mathbf{q}) \) and \( h \) is admissible.

In this lesson we addressed the central challenge of scaling graph search to high-DOF manipulators. We quantified the state explosion of uniform lattices, formalized manipulator lattices via motion primitives, and derived both joint-space and task-space heuristics with explicit admissibility guarantees. We also studied multi-resolution and hierarchical strategies that preserve completeness while dramatically reducing the number of expanded states. Finally, we implemented lattice-based A* across several programming languages, illustrating how these theoretical ideas map into practical software for real robot arms.