In kinodynamic planning, we must respect both geometric constraints (collision avoidance in configuration space) and dynamic constraints (system equations of motion and bounds on velocities, accelerations and controls). A general continuous-time model is

\[ \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(t) \in \mathcal{X} \subset \mathbb{R}^n, \; \mathbf{u}(t) \in \mathcal{U} \subset \mathbb{R}^m. \]

A state-lattice planner constructs a discrete subset \( \mathcal{X}_d \subset \mathcal{X} \) with a regular, grid-like (lattice) structure and connects states only by dynamically feasible trajectories called motion primitives. This yields a directed graph on which classical graph search (e.g., A*) can be applied, while each edge corresponds to an executable control sequence.

flowchart TD
  X["Dynamics model dot x = f(x,u)"] --> L["Choose lattice discretization in state space"]
  L --> P["Design motion primitive set (short trajectories)"]
  P --> G["Build directed graph over lattice nodes"]
  G --> S["Run graph search (A* or similar)"]
  S --> T["Dynamically feasible trajectory"]
        

Compared to arbitrary discretizations, lattices exploit structure (e.g., translation and rotation invariance for mobile robots), which allows reusing a small primitive library across many states. This structure is crucial for efficiency and for establishing resolution-completeness and resolution-optimality properties.

We formalize the underlying optimal kinodynamic planning problem. Let \( \mathcal{X}_{\text{free}} \subset \mathcal{X} \) denote the collision-free state space, induced by the configuration space obstacles and possibly velocity bounds. A trajectory \( (\mathbf{x}, \mathbf{u}) \) on time interval \( [0,T] \) is feasible if

\[ \begin{aligned} &\dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)) \quad \text{for almost all } t \in [0,T], \\ &\mathbf{x}(t) \in \mathcal{X}_{\text{free}}, \quad \mathbf{u}(t) \in \mathcal{U} \quad \forall t \in [0,T], \\ &\mathbf{x}(0) = \mathbf{x}_{\text{start}}, \quad \mathbf{x}(T) \in \mathcal{X}_{\text{goal}}. \end{aligned} \]

The objective is typically to minimize an integral cost

\[ J[\mathbf{x}, \mathbf{u}] = \int_0^T c(\mathbf{x}(t), \mathbf{u}(t)) \,\mathrm{d}t, \]

where the running cost \( c \) may encode path length, control effort, or a combination (e.g., \( c = 1 \) for minimum time, \( c = \|\mathbf{u}\|^2 \) for minimum control energy).

Solving this continuous optimal control problem exactly is generally intractable for high-dimensional, nonlinear robotic systems. State lattices approximate it by:

• restricting the state to a discrete lattice \( \mathcal{X}_d \), and
• restricting controls to a finite set of parameterized motion primitives whose trajectories can be precomputed.

Consider a planar car-like robot modeled on \( \mathrm{SE}(2) \) with state \( \mathbf{x} = (x, y, \theta) \). A common lattice discretization is uniform translation and heading sampling:

\[ \mathcal{X}_d = \left\{ (i \Delta x, \; j \Delta y, \; k \Delta \theta) \,\middle|\, i,j \in \mathbb{Z}, \; k \in \{0,1,\dots,N_\theta - 1\} \right\}. \]

In a bounded workspace \( \mathcal{W} \subset \mathbb{R}^2 \), we restrict to nodes whose positions lie inside the free region and whose footprints (with given clearance) do not intersect obstacles:

\[ \mathcal{V} = \left\{ \mathbf{x} \in \mathcal{X}_d \,\middle|\, \mathrm{Footprint}(\mathbf{x}) \subset \mathcal{W}_{\text{free}} \right\}. \]

The set \( \mathcal{V} \) will be the vertex set of our planning graph. The lattice structure is characterized by the fact that \( \mathcal{X}_d \) is generated by integer combinations of a finite set of basis vectors (translations and heading increments). For example, in a simple square-grid pose lattice

\[ (x,y,\theta) = i (\Delta x, 0, 0) + j (0, \Delta y, 0) + k (0, 0, \Delta \theta). \]

This regularity allows us to define a local motion primitive set from a canonical origin state and then reuse it by translation and rotation throughout the lattice.

For a differential-drive or unicycle model, the continuous dynamics are

\[ \dot{x} = v \cos\theta, \quad \dot{y} = v \sin\theta, \quad \dot{\theta} = \omega, \]

with bounded controls \( v \in [v_{\min}, v_{\max}] \), \( \omega \in [\omega_{\min}, \omega_{\max}] \). The lattice resolution parameters \( \Delta x, \Delta y, \Delta \theta \) should be chosen consistent with achievable displacements over reasonable primitive durations.

A motion primitive is a short finite-time trajectory generated by a constrained control profile, typically piecewise-constant. Let a primitive be parameterized by a control sequence \( \mathbf{u}_p(t) \) over duration \( T_p \), starting from canonical state \( \mathbf{x}_0 \). The primitive trajectory satisfies

\[ \dot{\mathbf{x}}_p(t) = f(\mathbf{x}_p(t), \mathbf{u}_p(t)), \quad \mathbf{x}_p(0) = \mathbf{x}_0, \quad t \in [0,T_p]. \]

Its terminal state is \( \mathbf{x}_p(T_p) \). We want this terminal state to coincide exactly with a lattice node displacement relative to \( \mathbf{x}_0 \), or at least to be snapped to the nearest lattice node. The primitive's cost is

\[ C_p = \int_0^{T_p} c(\mathbf{x}_p(t), \mathbf{u}_p(t)) \, \mathrm{d}t. \]

A motion primitive library \( \mathcal{P} \) is a finite set of such trajectories \( \{\sigma_1, \dots, \sigma_M\} \), each with an associated terminal displacement and cost.

For a unicycle robot, a convenient primitive parameterization is constant controls \( (v,\omega) \) over fixed duration \( T \). For \( \omega \neq 0 \), the closed-form solution is

\[ \begin{aligned} x(T) &= x_0 + \frac{v}{\omega} \left( \sin(\theta_0 + \omega T) - \sin\theta_0 \right), \\ y(T) &= y_0 - \frac{v}{\omega} \left( \cos(\theta_0 + \omega T) - \cos\theta_0 \right), \\ \theta(T) &= \theta_0 + \omega T. \end{aligned} \]

For \( \omega = 0 \), we obtain straight-line motion:

\[ x(T) = x_0 + v T \cos\theta_0, \quad y(T) = y_0 + v T \sin\theta_0, \quad \theta(T) = \theta_0. \]

By selecting a discrete set of control pairs \( (v_i, \omega_i) \) and a duration \( T \), we can analytically compute the terminal displacements and choose those that best align with the underlying lattice grid.

flowchart TD
  C["Control samples (v, omega, T)"] --> INTEG["Integrate dynamics from canonical state"]
  INTEG --> SNAP["Snap terminal state to nearest lattice node"]
  SNAP --> EDGE["Store edge with cost and primitive index"]
  EDGE --> LIB["Primitive library used by planner"]
        

Once the lattice nodes \( \mathcal{V} \) and motion primitive library \( \mathcal{P} \) are defined, we construct a directed graph \( G = (\mathcal{V}, \mathcal{E}) \) where each edge corresponds to the application of some primitive, transformed to the local frame of the source node.

For a node \( v \in \mathcal{V} \), we denote its outgoing neighbors by \( \mathcal{N}(v) \subset \mathcal{V} \). Each edge \( e = (v, v') \) has a cost \( c(e) \) equal to the primitive cost \( C_p \) provided the entire trajectory remains in \( \mathcal{X}_{\text{free}} \).

A shortest path on the graph is a sequence \( v_0, v_1, \dots, v_K \) with \( v_0 = v_{\text{start}}, v_K \in \mathcal{V}_{\text{goal}} \) that minimizes

\[ J_h^\star = \min_{(v_0,\dots,v_K)} \sum_{k=0}^{K-1} c(v_k, v_{k+1}). \]

Here the subscript \( h \) denotes dependence on the lattice resolution parameters \( h = (\Delta x, \Delta y, \Delta \theta) \).

Intuitively, as \( h \) becomes finer and the motion primitive set becomes richer, the discrete path cost \( J_h^\star \) approximates the true optimal cost \( J^\star \) of the continuous kinodynamic problem. Under mild regularity assumptions on dynamics and cost (e.g., Lipschitz continuity) and suitable coverage conditions on the primitive library, one can show resolution-optimality:

\[ |J^\star - J_h^\star| \le \alpha(h), \quad \lim_{\|h\|\to 0} \alpha(h) = 0. \]

Moreover, if every collision-free continuous trajectory with clearance at least some radius can be approximated by a lattice path, then shortest-path search on the lattice is resolution-complete: for a fixed resolution \( h \), the algorithm finds a discrete solution whenever such an approximate solution exists, or certifies failure otherwise.

Practically, A* search is used with an admissible heuristic. For a unicycle with maximum speed \( v_{\max} \) and cost equal to time, the heuristic

\[ h(\mathbf{x}) = \frac{\|\mathbf{x} - \mathbf{x}_{\text{goal}}\|_2}{v_{\max}} \]

is admissible, because the Euclidean distance between current and goal position is a lower bound on the path length of any feasible trajectory, and time is at least length divided by \( v_{\max} \). Therefore, \( h(\mathbf{x}) \le J^\star(\mathbf{x}) \) for all states \( \mathbf{x} \), guaranteeing that A* returns an optimal path on the lattice.

We now sketch a minimal Python implementation of a state-lattice planner for a unicycle robot in a rectangular grid world. For clarity, we focus on:

• building a lattice in \( (x,y,\theta) \),
• defining a small motion primitive set with constant \( (v,\omega) \), and
• running A* search over the resulting graph.

In practice, one would integrate this with robotics libraries such as OMPL's Python bindings or ROS 2 navigation stacks; here we implement the core mechanics from scratch.

import math
from heapq import heappush, heappop

# ------------- State and lattice utilities -------------

LATTICE_DX = 1.0
LATTICE_DY = 1.0
N_THETA    = 16
DELTA_TH   = 2.0 * math.pi / N_THETA

def wrap_theta(th):
    """Wrap heading to [0, 2*pi)."""
    th = th % (2.0 * math.pi)
    return th

def quantize_state(x, y, th):
    """Map continuous state to lattice indices (i,j,k)."""
    i = int(round(x / LATTICE_DX))
    j = int(round(y / LATTICE_DY))
    k = int(round(wrap_theta(th) / DELTA_TH)) % N_THETA
    return (i, j, k)

def dequantize_state(i, j, k):
    """Map indices back to continuous state representative."""
    x = i * LATTICE_DX
    y = j * LATTICE_DY
    th = k * DELTA_TH
    return (x, y, th)

# ------------- Motion primitive generation -------------

class Primitive:
    def __init__(self, v, omega, T, dt=0.1):
        self.v = v
        self.omega = omega
        self.T = T
        self.dt = dt
        # Precompute displacement from canonical state (0,0,0)
        self.disp, self.cost = self._simulate_from(0.0, 0.0, 0.0)

    def _simulate_from(self, x0, y0, th0):
        x, y, th = x0, y0, th0
        t = 0.0
        cost = 0.0
        while t < self.T:
            # simple Euler integration of unicycle model
            x += self.v * math.cos(th) * self.dt
            y += self.v * math.sin(th) * self.dt
            th += self.omega * self.dt
            t += self.dt
            cost += self.dt  # time cost
        th = wrap_theta(th)
        return (x - x0, y - y0, th - th0), cost

# Define a small symmetric set of primitives in canonical frame
PRIMITIVES = [
    Primitive(v=1.0,  omega=0.0,  T=1.0),   # straight
    Primitive(v=1.0,  omega=0.8,  T=1.0),   # left arc
    Primitive(v=1.0,  omega=-0.8, T=1.0),   # right arc
]

def apply_primitive(i, j, k, prim):
    """Apply primitive starting from lattice node (i,j,k)."""
    x0, y0, th0 = dequantize_state(i, j, k)
    dx, dy, dth = prim.disp
    # rotate displacement by th0
    x1 = x0 + math.cos(th0) * dx - math.sin(th0) * dy
    y1 = y0 + math.sin(th0) * dx + math.cos(th0) * dy
    th1 = wrap_theta(th0 + dth)
    return quantize_state(x1, y1, th1), prim.cost

# ------------- Collision checking -------------

def is_free(i, j, occupancy):
    """Check grid occupancy: occupancy[j][i] == 0 means free."""
    if j < 0 or j >= len(occupancy):
        return False
    if i < 0 or i >= len(occupancy[0]):
        return False
    return occupancy[j][i] == 0

# ------------- A* search on lattice -------------

def heuristic(node, goal):
    (ix, iy, _) = node
    (gx, gy, _) = goal
    dx = (ix - gx) * LATTICE_DX
    dy = (iy - gy) * LATTICE_DY
    dist = math.hypot(dx, dy)
    v_max = 1.0
    return dist / v_max

def astar_lattice(start_state, goal_state, occupancy):
    start = quantize_state(*start_state)
    goal = quantize_state(*goal_state)

    open_heap = []
    heappush(open_heap, (0.0, start))
    g_score = {start: 0.0}
    parent = {start: None}

    while open_heap:
        _, current = heappop(open_heap)
        if current == goal:
            # reconstruct path of lattice nodes
            path = []
            n = current
            while n is not None:
                path.append(dequantize_state(*n))
                n = parent[n]
            path.reverse()
            return path

        (i, j, k) = current
        if not is_free(i, j, occupancy):
            continue

        for prim in PRIMITIVES:
            nxt, cost = apply_primitive(i, j, k, prim)
            (ni, nj, nk) = nxt
            if not is_free(ni, nj, occupancy):
                continue

            tentative_g = g_score[current] + cost
            old_g = g_score.get(nxt, float("inf"))
            if tentative_g < old_g:
                g_score[nxt] = tentative_g
                parent[nxt] = current
                f = tentative_g + heuristic(nxt, goal)
                heappush(open_heap, (f, nxt))

    return None  # no path found

# Example usage (simple empty grid)
if __name__ == "__main__":
    width, height = 20, 20
    occupancy = [[0 for _ in range(width)] for _ in range(height)]

    start_state = (1.0, 1.0, 0.0)
    goal_state  = (10.0, 10.0, 0.0)
    path = astar_lattice(start_state, goal_state, occupancy)
    print("Path length:", len(path) if path is not None else 0)
      

This implementation demonstrates the essential ideas: a regular lattice, a reusable primitive library, and A* search with an admissible heuristic defined in state space.

In C++, state-lattice planners are often implemented using libraries such as OMPL or SBPL. Below is a simplified, self-contained snippet illustrating how one might represent lattice nodes and motion primitives in C++, ready to be integrated into an A* planner.

#include <cmath>
#include <vector>
#include <limits>

struct LatticeState {
    int i, j, k; // indices in x, y, theta
};

struct ContinuousState {
    double x, y, th;
};

static const double DX = 1.0;
static const double DY = 1.0;
static const int    N_TH = 16;
static const double DTH = 2.0 * M_PI / N_TH;

double wrapTheta(double th) {
    double twoPi = 2.0 * M_PI;
    th = fmod(th, twoPi);
    if (th < 0.0) th += twoPi;
    return th;
}

LatticeState quantize(const ContinuousState& s) {
    int i = static_cast<int>(std::round(s.x / DX));
    int j = static_cast<int>(std::round(s.y / DY));
    int k = static_cast<int>(std::round(wrapTheta(s.th) / DTH)) % N_TH;
    return {i, j, k};
}

ContinuousState dequantize(const LatticeState& q) {
    ContinuousState s;
    s.x  = q.i * DX;
    s.y  = q.j * DY;
    s.th = q.k * DTH;
    return s;
}

struct Primitive {
    double v;
    double omega;
    double T;
    double dt;
    // displacement from canonical (0,0,0)
    ContinuousState disp;
    double cost;

    Primitive(double v_, double o_, double T_, double dt_ = 0.1)
        : v(v_), omega(o_), T(T_), dt(dt_) {
        simulateCanonical();
    }

    void simulateCanonical() {
        double x = 0.0, y = 0.0, th = 0.0;
        double t = 0.0;
        double c = 0.0;
        while (t < T) {
            x += v * std::cos(th) * dt;
            y += v * std::sin(th) * dt;
            th += omega * dt;
            t  += dt;
            c  += dt;
        }
        th = wrapTheta(th);
        disp = {x, y, th};
        cost = c;
    }
};

std::vector<Primitive> makePrimitiveSet() {
    std::vector<Primitive> prims;
    prims.emplace_back(1.0,  0.0, 1.0);  // straight
    prims.emplace_back(1.0,  0.8, 1.0);  // left
    prims.emplace_back(1.0, -0.8, 1.0);  // right
    return prims;
}

LatticeState applyPrimitive(const LatticeState& q,
                            const Primitive& p) {
    ContinuousState s0 = dequantize(q);
    double th0 = s0.th;

    // rotate primitive displacement into world frame
    double dx = std::cos(th0) * p.disp.x - std::sin(th0) * p.disp.y;
    double dy = std::sin(th0) * p.disp.x + std::cos(th0) * p.disp.y;
    double dth = p.disp.th;

    ContinuousState s1;
    s1.x  = s0.x + dx;
    s1.y  = s0.y + dy;
    s1.th = wrapTheta(th0 + dth);
    return quantize(s1);
}

// A* or D* search would then use makePrimitiveSet() and applyPrimitive()
// to generate successors from each lattice node.
      

Libraries like OMPL allow implementing this structure within a generic planning framework, but the logic above captures the essential kinematic and dynamic consistency of motion primitives.

Although Java is less common in robotics than C++ or Python, it can be used for planning, e.g., in simulation frameworks. Libraries like JGraphT can handle generic graph search; here we illustrate a small standalone skeleton combining a lattice and A*.

public class StateLatticePlanner {

    static final double DX = 1.0;
    static final double DY = 1.0;
    static final int N_TH = 16;
    static final double DTH = 2.0 * Math.PI / N_TH;

    static class LatticeState {
        int i, j, k;
        LatticeState(int i, int j, int k) { this.i = i; this.j = j; this.k = k; }

        @Override
        public boolean equals(Object o) {
            if (!(o instanceof LatticeState)) return false;
            LatticeState other = (LatticeState)o;
            return i == other.i && j == other.j && k == other.k;
        }

        @Override
        public int hashCode() {
            return (i * 73856093) ^ (j * 19349663) ^ (k * 83492791);
        }
    }

    static class Primitive {
        double v, omega, T, dt;
        double dx, dy, dth;
        double cost;

        Primitive(double v, double omega, double T, double dt) {
            this.v = v;
            this.omega = omega;
            this.T = T;
            this.dt = dt;
            simulateCanonical();
        }

        private void simulateCanonical() {
            double x = 0.0, y = 0.0, th = 0.0;
            double t = 0.0;
            double c = 0.0;
            while (t < T) {
                x += v * Math.cos(th) * dt;
                y += v * Math.sin(th) * dt;
                th += omega * dt;
                t += dt;
                c += dt;
            }
            th = wrapTheta(th);
            dx = x; dy = y; dth = th;
            cost = c;
        }
    }

    static double wrapTheta(double th) {
        double twoPi = 2.0 * Math.PI;
        th = th % twoPi;
        if (th < 0.0) th += twoPi;
        return th;
    }

    static LatticeState quantize(double x, double y, double th) {
        int i = (int)Math.round(x / DX);
        int j = (int)Math.round(y / DY);
        int k = ((int)Math.round(wrapTheta(th) / DTH)) % N_TH;
        return new LatticeState(i, j, k);
    }

    static double heuristic(LatticeState a, LatticeState b) {
        double dx = (a.i - b.i) * DX;
        double dy = (a.j - b.j) * DY;
        double dist = Math.hypot(dx, dy);
        double vMax = 1.0;
        return dist / vMax;
    }

    // Here one would add a priority-queue based A* using Primitive successors.

    public static void main(String[] args) {
        // Example primitive set
        Primitive[] prims = {
            new Primitive(1.0, 0.0, 1.0, 0.1),
            new Primitive(1.0, 0.8, 1.0, 0.1),
            new Primitive(1.0, -0.8, 1.0, 0.1)
        };
        // A* implementation would use these primitives to expand neighbors.
    }
}
      

A full implementation would additionally store parent pointers and g-scores in hash maps, and optionally integrate with graph libraries for more advanced search variants.

MATLAB and Simulink provide convenient tools to validate and visualize motion primitives for dynamical models. The Robotics System Toolbox includes a hybrid A* planner that internally uses a state lattice and motion primitives; here we implement a simpler custom primitive set and suggest a Simulink validation workflow.

function primitives = makeUnicyclePrimitives()
    % Primitive fields: v, omega, T, dt, disp, cost
    primitives(1) = simulatePrimitive(1.0,  0.0, 1.0, 0.05);
    primitives(2) = simulatePrimitive(1.0,  0.8, 1.0, 0.05);
    primitives(3) = simulatePrimitive(1.0, -0.8, 1.0, 0.05);
end

function prim = simulatePrimitive(v, omega, T, dt)
    x = 0.0; y = 0.0; th = 0.0;
    t = 0.0; cost = 0.0;
    while t < T
        x  = x  + v * cos(th) * dt;
        y  = y  + v * sin(th) * dt;
        th = th + omega * dt;
        t  = t + dt;
        cost = cost + dt;
    end
    th = wrapTo2Pi(th);
    prim.v = v;
    prim.omega = omega;
    prim.T = T;
    prim.dt = dt;
    prim.disp = [x; y; th];
    prim.cost = cost;
end

% Example: construct primitives and visualize endpoints
prims = makeUnicyclePrimitives();
figure; hold on; axis equal;
colors = lines(numel(prims));
for i = 1:numel(prims)
    dp = prims(i).disp;
    plot([0, dp(1)], [0, dp(2)], '-', 'Color', colors(i,:));
end
legend('straight', 'left', 'right');
title('Unicycle motion primitive endpoints');
      

To validate a primitive in Simulink:

1. Build a Simulink model of the unicycle dynamics with integrator blocks for x, y, and theta and trigonometric blocks for cos(theta) and sin(theta).
2. Drive the model with constant inputs v and omega corresponding to a primitive, and simulate for duration T.
3. Compare the final state from Simulink with the MATLAB primitive endpoint to confirm the integration scheme and discretization are consistent.

Wolfram Mathematica can generate motion primitives by solving the unicycle differential equations with NDSolve and evaluating terminal states for a grid of control parameters.

clearAll[unicyclePrimitive];

unicyclePrimitive[v_, omega_, T_] := Module[
  {x, y, th, sol, xT, yT, thT},
  sol = NDSolve[
    {
      x'[t] == v Cos[th[t]],
      y'[t] == v Sin[th[t]],
      th'[t] == omega,
      x[0] == 0, y[0] == 0, th[0] == 0
    },
    {x, y, th}, {t, 0, T}
  ];
  xT = x[T] /. sol[[1]];
  yT = y[T] /. sol[[1]];
  thT = Mod[th[T] /. sol[[1]], 2 Pi];
  <|"v" -> v, "omega" -> omega, "T" -> T,
    "disp" -> {xT, yT, thT}|>
]

(* Generate a small library *)
primitiveLibrary = Table[
  unicyclePrimitive[v, omega, 1.0],
  {v, {1.0}},
  {omega, {0.0, 0.8, -0.8}}
] // Flatten;

primitiveLibrary
      

The resulting associations store terminal displacements for each primitive; these can be combined with a lattice quantization function and a standard A* implementation in Mathematica to build a full state-lattice planner.

Problem 1 (Counting Lattice States in a Bounded Region): Consider a pose lattice on \( \mathrm{SE}(2) \) with resolutions \( \Delta x = \Delta y = h \) and \( \Delta \theta = 2\pi / N_{\theta} \). The workspace is an axis-aligned rectangle \( [0,L_x] \times [0,L_y] \) without obstacles. Derive an expression for the number of lattice states \( |\mathcal{V}| \) inside the workspace.

Solution: The discrete positions are \( x_i = i h \) for \( i = 0,1,\dots,\lfloor L_x/h \rfloor \), and similarly \( y_j = j h \) for \( j = 0,1,\dots,\lfloor L_y/h \rfloor \). Thus the number of distinct position grid points is

\[ N_{xy} = \left( \left\lfloor \frac{L_x}{h} \right\rfloor + 1 \right) \left( \left\lfloor \frac{L_y}{h} \right\rfloor + 1 \right). \]

For each position, heading can take \( N_{\theta} \) discrete values, so

\[ |\mathcal{V}| = N_{xy} \, N_{\theta} = \left( \left\lfloor \frac{L_x}{h} \right\rfloor + 1 \right) \left( \left\lfloor \frac{L_y}{h} \right\rfloor + 1 \right) N_{\theta}. \]

Problem 2 (Admissibility of Euclidean Heuristic): Assume a unicycle robot with maximum speed \( v_{\max} \), running cost \( c(\mathbf{x},\mathbf{u}) = 1 \), and no obstacles. Show rigorously that the heuristic \( h(\mathbf{x}) = \|\mathbf{x} - \mathbf{x}_{\text{goal}}\|_2 / v_{\max} \) is admissible for A* on a state lattice.

Solution: Any feasible trajectory from \( \mathbf{x} \) to \( \mathbf{x}_{\text{goal}} \) has some path length \( L \). Because the robot's translational velocity is bounded by \( v_{\max} \), its speed along the trajectory satisfies \( \|\dot{\mathbf{p}}(t)\|_2 \le v_{\max} \) for the planar position \( \mathbf{p}(t) \). Therefore the travel time is

\[ T = \int_0^T 1 \,\mathrm{d}t = \int_0^T \frac{\|\dot{\mathbf{p}}(t)\|_2}{\|\dot{\mathbf{p}}(t)\|_2} \,\mathrm{d}t \ge \frac{1}{v_{\max}}\int_0^T \|\dot{\mathbf{p}}(t)\|_2 \,\mathrm{d}t = \frac{L}{v_{\max}}. \]

By the triangle inequality, the path length \( L \) is at least the Euclidean distance between endpoints: \( L \ge \|\mathbf{x} - \mathbf{x}_{\text{goal}}\|_2 \). Thus for any feasible trajectory

\[ T \ge \frac{L}{v_{\max}} \ge \frac{\|\mathbf{x} - \mathbf{x}_{\text{goal}}\|_2}{v_{\max}} = h(\mathbf{x}). \]

Since the discrete lattice cost approximates this continuous travel time from below, we have \( h(\mathbf{x}) \le J^\star(\mathbf{x}) \) for all lattice states, so the heuristic is admissible.

Problem 3 (Primitive Coverage Condition): Let the unicycle primitive set consist only of straight-line motion with \( \omega = 0 \) and a fixed duration \( T \). Explain why this primitive set is insufficient for completeness on a cluttered workspace and describe a minimal extension that restores local controllability.

Solution: With \( \omega = 0 \) only, the robot can move forward but cannot change heading, so the reachable set from a given state is a one-dimensional ray. Many collision-free states and goals will lie outside this ray, so the induced graph is not even connected in free space. A minimal extension is to add primitives with nonzero positive and negative angular velocities, e.g., \( \omega = \pm \omega_0 \). These arcs allow the robot to change heading and, by concatenating straight and turning primitives, approximate arbitrary feasible trajectories, recovering local controllability and resolution-completeness in the lattice.

Problem 4 (Effect of Lattice Resolution on Suboptimality): Suppose we use a pose lattice with spatial resolution \( h \) and a primitive set whose terminal states are snapped to nearest lattice nodes. Assume the continuous optimal path is Lipschitz continuous and has bounded curvature. Argue qualitatively why the suboptimality gap \( J_h^\star - J^\star \) tends to zero as \( h \) tends to zero.

Solution: As \( h \) decreases, lattice nodes form a finer approximation of the continuous state space. The bounded curvature implies that the optimal path can be approximated by a sequence of short arcs and segments whose endpoints are close to lattice nodes; the Lipschitz property ensures that small perturbations of state produce small perturbations of cost. Hence, there exists a lattice path whose states stay within distance proportional to \( h \) of the optimal path. The cost difference between this approximating lattice path and the continuous optimum is then bounded by a constant times \( h \). Since A* finds the best lattice path, its cost is at most this approximating path cost, so the suboptimality gap converges to zero as \( h \) tends to zero.

Problem 5 (Underactuated Systems and Lattice Design): For an underactuated planar vehicle whose dynamics include sway (side slip) and whose control inputs do not directly actuate all degrees of freedom, what difficulties arise in constructing motion primitives? How could one adapt the primitive generation process to account for these issues?

Solution: In underactuated systems, not every small state displacement is kinematically achievable, and the relationship between controls and state motion can be highly nonlinear and state-dependent (e.g., due to hydrodynamic forces). Primitive endpoints may be sensitive to initial conditions and disturbances, so analytic closed-form primitives are often unavailable. One adaptation is to compute primitives numerically by solving boundary value problems or optimal control problems for a set of representative initial states and control sequences, enforcing the true dynamics and constraints. Another is to parameterize primitives by steering functions that approximate the system's Lie bracket structure, ensuring that the primitive set spans the reachable directions needed for local controllability of the underactuated system.

In this lesson we formalized state-lattice planners as a structured discretization of kinodynamic motion planning problems. We defined lattice nodes in state space, constructed motion primitives as short feasible trajectories, and showed how graph search on the resulting directed graph approximates the continuous optimal control problem. We discussed resolution-completeness, heuristic admissibility, and the role of primitive coverage in guaranteeing high-quality solutions. Finally, we implemented core components in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica to illustrate how these concepts can be realized in software for mobile robot planning and, more generally, for kinodynamically constrained systems.