A motion primitive is a short, parameterized motion segment generated by applying a simple control law (often constant) over a finite horizon. In a planner, primitives act as edges in a graph whose nodes are robot states (typically pose and discretized heading). A planner expands a state by applying a small set of primitives and checking feasibility/collisions, thereby turning continuous motion planning into graph search.

For ground vehicles, the key constraint is nonholonomy: lateral velocity is constrained by rolling. Therefore, feasible local motions are not arbitrary straight-line translations in the plane, but instead are curves whose curvature and heading evolution must obey kinematics and limits.

flowchart TD
  S0["State s_k = (x, y, theta)"] --> A["Choose primitive u(t) from library"]
  A --> B["Propagate kinematics for horizon T"]
  B --> C["Endpoint s_{k+1} = f(s_k, primitive)"]
  C --> D["Feasibility checks: curvature bounds, map, obstacles"]
  D -->|valid| E["Add edge to search graph with cost"]
  D -->|invalid| F["Reject primitive"]
  E --> G["Graph search: A*, D*, etc."]
        

In this chapter, we assume students already know rigid-body pose composition and planar rotation matrices. We will rely on the applied kinematic model introduced in earlier lessons of this chapter (nonholonomic constraint, curvature bounds, feasible families).

Consider the planar pose \( \mathbf{s} = (x,y,\theta) \). For a car-like robot in kinematic form, a convenient abstraction for primitives is the constant-curvature segment, where curvature is constant over a short horizon \( T \).

Using the curvature form (consistent with Lesson 2 and Lesson 3 of this chapter), we write:

\[ \dot{x}(t) = v \cos\theta(t),\quad \dot{y}(t) = v \sin\theta(t),\quad \dot{\theta}(t) = v\,\kappa \]

A motion primitive is then parameterized by the tuple \( \pi = (v,\kappa,T) \), with the implicit assumption that \( v \) and \( \kappa \) are constant on \( [0,T] \).

The primitive defines an endpoint map in the local frame where the primitive starts at \( (0,0,0) \). Denote this local endpoint by \( \Delta(\pi) = (\Delta x,\Delta y,\Delta\theta) \).

\[ \Delta\theta = \int_{0}^{T} v\kappa\,dt = v\kappa T. \]

If \( \kappa \neq 0 \), then \( \theta(t)=v\kappa t \). Substituting into \( \dot{x},\dot{y} \) yields the closed-form endpoint:

\[ \Delta x = \int_{0}^{T} v \cos(v\kappa t)\,dt = \frac{\sin(v\kappa T)}{\kappa}, \qquad \Delta y = \int_{0}^{T} v \sin(v\kappa t)\,dt = \frac{1-\cos(v\kappa T)}{\kappa}. \]

For the straight primitive \( \kappa = 0 \), we take the limit \( \kappa → 0 \) (or integrate directly):

\[ \kappa = 0 \;\Rightarrow\; \Delta x = vT,\quad \Delta y = 0,\quad \Delta\theta = 0. \]

Therefore, the local endpoint map is fully analytic, which is why constant-curvature primitives are so attractive: no numerical integration is needed to compute successors in a search graph.

Let the robot be at global state \( \mathbf{s}_0 = (x_0,y_0,\theta_0) \). Applying a primitive whose local endpoint is \( (\Delta x,\Delta y,\Delta\theta) \) produces:

\[ \begin{bmatrix} x_1 \\ y_1 \end{bmatrix} = \begin{bmatrix} x_0 \\ y_0 \end{bmatrix} + \mathbf{R}(\theta_0) \begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix}, \qquad \theta_1 = \theta_0 + \Delta\theta, \quad \mathbf{R}(\theta_0) = \begin{bmatrix} \cos\theta_0 & -\sin\theta_0\\ \sin\theta_0 & \cos\theta_0 \end{bmatrix}. \]

This is precisely the standard pose composition in SE(2): the local displacement is rotated into the world frame.

In a lattice planner, we often discretize the state space to: \( x \in \delta_{xy}\mathbb{Z} \), \( y \in \delta_{xy}\mathbb{Z} \), and \( \theta \in \{0,\tfrac{2\pi}{N_\theta},\dots,\tfrac{2\pi(N_\theta-1)}{N_\theta}\} \). Define the snapping operator \( \mathcal{Q} \):

\[ \mathcal{Q}(x,y,\theta) = \left( \delta_{xy}\,\mathrm{round}\!\left(\frac{x}{\delta_{xy}}\right), \delta_{xy}\,\mathrm{round}\!\left(\frac{y}{\delta_{xy}}\right), \delta_{\theta}\,\mathrm{round}\!\left(\frac{\theta}{\delta_{\theta}}\right) \right), \quad \delta_{\theta} = \frac{2\pi}{N_{\theta}}. \]

Discretization introduces approximation error; however, with sufficiently fine resolution, lattice planners can be made resolution-complete for practical purposes (conceptually; formal completeness depends on assumptions).

Earlier in this chapter we introduced curvature and turning radius limits. For a car-like ground vehicle, assume an admissible curvature set: \( |\kappa| \le \kappa_{\max} \) where \( \kappa_{\max} = 1/R_{\min} \).

A primitive \( \pi=(v,\kappa,T) \) is kinematically feasible if: \( v \ge 0 \) (or includes reverse if allowed), and \( |\kappa| \le \kappa_{\max} \). If we parameterize using steering angle \( \delta \) and wheelbase \( L \), then \( \kappa = \tan\delta/L \), so:

\[ |\kappa| \le \kappa_{\max} \;\Longleftrightarrow\; \left|\frac{\tan\delta}{L}\right| \le \kappa_{\max} \;\Longleftrightarrow\; |\delta| \le \arctan(L\kappa_{\max}) \triangleq \delta_{\max}. \]

Proposition 1 (Feasibility of constant-curvature primitives). Under the kinematic model of Section 2, if \( v \) is bounded and \( |\kappa| \le \kappa_{\max} \), then the generated trajectory satisfies the turning radius constraint \( R(t) \ge R_{\min} \) for all \( t \in [0,T] \).

Proof. For planar motion with nonzero forward speed, curvature is defined as:

\[ \kappa(t) = \frac{\dot{\theta}(t)}{v(t)} \quad \text{(for } v(t) > 0\text{)}. \]

In our primitive, \( v(t)=v \) and \( \dot{\theta}(t)=v\kappa \), hence \( \kappa(t)=\kappa \) for all \( t \). The instantaneous turning radius is \( R(t)=1/|\kappa(t)| \) (for nonzero curvature), so:

\[ |\kappa(t)| = |\kappa| \le \kappa_{\max} = \frac{1}{R_{\min}} \;\Rightarrow\; R(t) = \frac{1}{|\kappa(t)|} \ge R_{\min}. \]

If \( \kappa=0 \), then the motion is straight and the radius is infinite, which also satisfies the bound. Therefore the trajectory respects the turning radius constraint for all \( t \in [0,T] \). □

Planners rely on repeatedly composing primitives. A critical theoretical convenience is that applying a primitive at any state is equivalent to applying the same primitive in the local frame and then composing with the current pose.

Define a primitive endpoint transform (local frame) as an SE(2) element: \( \mathbf{T}(\pi) \in SE(2) \) with translation \( (\Delta x,\Delta y) \) and rotation \( \Delta\theta \).

\[ \mathbf{T}(\pi) = \begin{bmatrix} \cos\Delta\theta & -\sin\Delta\theta & \Delta x\\ \sin\Delta\theta & \cos\Delta\theta & \Delta y\\ 0 & 0 & 1 \end{bmatrix}. \]

Let the current pose be \( \mathbf{T}_0 \in SE(2) \). Applying the primitive results in: \( \mathbf{T}_1 = \mathbf{T}_0\,\mathbf{T}(\pi) \).

Proposition 2 (Closure under composition). If each primitive is feasible (satisfies curvature bounds), then any finite composition of primitives \( \mathbf{T}_0 \mathbf{T}(\pi_1)\mathbf{T}(\pi_2)\cdots\mathbf{T}(\pi_m) \) yields a trajectory that is feasible at all times (kinematic constraints satisfied), assuming piecewise continuity at switching instants.

Proof (sketch, sufficient for planner use).

Each primitive \( \pi_i \) generates a trajectory segment \( \mathbf{s}_i(t) \) for \( t \in [0,T_i] \) that satisfies the kinematic model and curvature bound by Proposition 1. Define the concatenated trajectory \( \mathbf{s}(t) \) over the total time horizon by stitching segments end-to-start: the initial condition of segment \( i+1 \) is the endpoint of segment \( i \).

On each open interval between switch times, the trajectory satisfies the same ODE with constant controls, hence is feasible. At switching instants, the pose is continuous by construction (endpoints match). The control may be discontinuous, but the kinematic constraints are algebraic in velocity direction and remain satisfied on each side.

Therefore the overall piecewise trajectory is feasible in the standard sense used in motion planning: it is continuous and satisfies the system dynamics almost everywhere (except a finite set of switching points). □

flowchart TD
  A["Primitive pi_1: (v1,k1,T1)"] --> B["Endpoint T(pi_1)"]
  B --> C["Compose with current pose: T0 * T(pi_1)"]
  C --> D["Next primitive pi_2"]
  D --> E["Compose: T0 * T(pi_1) * T(pi_2)"]
  E --> F["Repeat until goal reached"]
        

A primitive library is a finite set \( \Pi \). Common design choices include:

• Constant curvature set: choose \( \kappa \in \{\kappa_1,\dots,\kappa_M\} \) with \( |\kappa_i| \le \kappa_{\max} \) and fixed arc length \( s \).
• Fixed arc length parametrization: set \( T = s/v \), so every primitive advances by the same traveled distance \( s \) (helps lattice regularity).
• Heading lattice matching: choose \( \kappa_i \) so that the heading increment \( \Delta\theta_i = v\kappa_iT = \kappa_i s \) aligns with discrete headings: \( \Delta\theta_i \approx m_i \delta_\theta \).

The last point is critical. If we fix arc length \( s \), then: \( \Delta\theta = \kappa s \). To exactly land on heading bins:

\[ \kappa \in \left\{ \frac{m\delta_\theta}{s} : m \in \mathbb{Z} \right\}, \quad \text{with } \left|\frac{m\delta_\theta}{s}\right| \le \kappa_{\max}. \]

This provides a constructive way to build a curvature set that is both feasible and lattice-compatible.

Costs are typically additive along a path. A simple primitive cost model used in practice is:

\[ c(\pi) = s + w_\kappa\,|\kappa|\,s + w_T\,T, \quad T = \frac{s}{v}. \]

The term \( w_\kappa|\kappa|s \) discourages sharp turns; \( w_TT \) discourages slow motions if variable speed is allowed.

The following code constructs a primitive library and demonstrates endpoint propagation and snapping to an SE(2) lattice. In Python robotics workflows, common libraries include numpy, scipy, and ROS2 client libraries (rclpy) for integration in navigation stacks.

File: Chapter3_Lesson4.py

#!/usr/bin/env python3
"""
Chapter 3 — Lesson 4: Motion Primitives for Ground Vehicles (conceptual use)
"""
# (Full code is included in the downloadable ZIP.)
from dataclasses import dataclass
import math, json
import numpy as np

@dataclass(frozen=True)
class State:
    x: float; y: float; theta: float

@dataclass(frozen=True)
class Primitive:
    name: str; v: float; kappa: float; T: float; cost: float
    dx: float; dy: float; dtheta: float

def endpoint_constant_curvature(v, kappa, T):
    dtheta = v*kappa*T
    if abs(kappa) < 1e-12:
        return v*T, 0.0, dtheta
    return math.sin(dtheta)/kappa, (1.0-math.cos(dtheta))/kappa, dtheta

def apply_primitive(state, prim):
    c, s = math.cos(state.theta), math.sin(state.theta)
    x = state.x + c*prim.dx - s*prim.dy
    y = state.y + s*prim.dx + c*prim.dy
    th = (state.theta + prim.dtheta + math.pi)%(2*math.pi) - math.pi
    return State(x,y,th)

def snap_to_lattice(state, xy_res, n_theta):
    xq = round(state.x/xy_res)*xy_res
    yq = round(state.y/xy_res)*xy_res
    dth = 2*math.pi/n_theta
    thq = round(state.theta/dth)*dth
    thq = (thq + math.pi)%(2*math.pi) - math.pi
    return State(xq,yq,thq)

def main():
    L = 0.35; R_min = 1.2
    kappa_max = 1.0/R_min
    delta_max = math.atan(kappa_max*L)

    v = 0.6; s_arc = 0.5
    T = s_arc/max(v,1e-9)
    delta_set = np.linspace(-delta_max, delta_max, 5)

    prims = []
    for delta in delta_set:
        kappa = math.tan(delta)/L
        dx,dy,dth = endpoint_constant_curvature(v,kappa,T)
        cost = s_arc + 0.2*abs(kappa)*s_arc + 0.1*T
        prims.append(Primitive(f"delta={delta:+.3f}_kappa={kappa:+.3f}", v,kappa,T,cost,dx,dy,dth))

    s0 = State(1.0, 2.0, math.radians(30))
    for p in prims:
        s1 = apply_primitive(s0,p)
        print(p.name, "->", s1, "->", snap_to_lattice(s1, 0.25, 16))

    with open("Chapter3_Lesson4_primitives.json","w") as f:
        json.dump([p.__dict__ for p in prims], f, indent=2)

if __name__=="__main__":
    main()

In C++ robotics stacks, primitives are often used inside real-time planning loops. Typical libraries include Eigen (linear algebra) and OMPL (planning). The following implements the analytic endpoint map and lattice snapping.

File: Chapter3_Lesson4.cpp

/*
Chapter 3 — Lesson 4: Motion Primitives for Ground Vehicles (conceptual use)
*/
// (Full code is included in the downloadable ZIP.)
#include <cmath>
#include <iostream>
struct State{double x,y,theta;};
static double wrap(double a){a=fmod(a+M_PI,2*M_PI); if(a<0) a+=2*M_PI; return a-M_PI;}
int main(){
  double v=0.6,kappa=0.4,T=1.2;
  double dth=v*kappa*T;
  double dx=(fabs(kappa)<1e-12)? v*T : sin(dth)/kappa;
  double dy=(fabs(kappa)<1e-12)? 0.0 : (1-cos(dth))/kappa;
  std::cout<<"dx="<<dx<<" dy="<<dy<<" dtheta="<<dth<<"\n";
  return 0;
}

Java-based robotics systems are less common in AMR planning than C++/Python, but Java remains relevant for Android robotics, education, and certain research stacks. The code below mirrors the analytic endpoint logic.

File: Chapter3_Lesson4.java

/*
Chapter 3 — Lesson 4: Motion Primitives for Ground Vehicles (conceptual use)
*/
// (Full code is included in the downloadable ZIP.)
public class Chapter3_Lesson4 {
  static double[] endpoint(double v, double kappa, double T){
    double dth = v*kappa*T;
    if (Math.abs(kappa) < 1e-12) return new double[]{v*T, 0.0, dth};
    return new double[]{Math.sin(dth)/kappa, (1.0-Math.cos(dth))/kappa, dth};
  }
  public static void main(String[] args){
    double[] ep = endpoint(0.6, 0.4, 1.2);
    System.out.printf("dx=%.6f dy=%.6f dtheta=%.6f%n", ep[0], ep[1], ep[2]);
  }
}

MATLAB is widely used for control-oriented validation and prototyping. The script below generates a primitive library, plots trajectories, and optionally builds a minimal Simulink model with integrator blocks.

File: Chapter3_Lesson4.m

% Chapter 3 — Lesson 4: Motion Primitives for Ground Vehicles (conceptual use)
% (Full code is included in the downloadable ZIP.)
v = 0.6; kappa = 0.4; T = 1.2;
dtheta = v*kappa*T;
if abs(kappa) < 1e-12
    dx = v*T; dy = 0;
else
    dx = sin(dtheta)/kappa;
    dy = (1-cos(dtheta))/kappa;
end
fprintf('dx=%f dy=%f dtheta=%f\n', dx, dy, dtheta);

Symbolic verification is useful for checking endpoint formulas and limits. The notebook in the ZIP derives the endpoint map using Integrate and verifies it numerically using NDSolveValue.

File: Chapter3_Lesson4.nb

(* Chapter 3 — Lesson 4: Motion Primitives for Ground Vehicles (conceptual use) *)
(* (Full notebook expression is included in the downloadable ZIP.) *)
ClearAll[v, kappa, T];
theta[t_] := v kappa t;
dx = FullSimplify[Integrate[v Cos[theta[t]], {t, 0, T}]];
dy = FullSimplify[Integrate[v Sin[theta[t]], {t, 0, T}]];
dtheta = FullSimplify[theta[T]];
{dx, dy, dtheta}

Problem 1 (Endpoint derivation): Starting from \( \dot{x}=v\cos\theta \), \( \dot{y}=v\sin\theta \), \( \dot{\theta}=v\kappa \) with constant \( v \) and \( \kappa \), derive the closed-form endpoint \( (\Delta x,\Delta y,\Delta\theta) \) for \( \kappa \neq 0 \).

Solution: Integrate \( \dot{\theta}=v\kappa \):

\[ \theta(t) = \theta(0) + \int_0^t v\kappa\,d\tau = v\kappa t \quad (\theta(0)=0). \]

Then integrate \( \dot{x},\dot{y} \):

\[ \Delta x = \int_0^T v\cos(v\kappa t)\,dt = \frac{\sin(v\kappa T)}{\kappa}, \qquad \Delta y = \int_0^T v\sin(v\kappa t)\,dt = \frac{1-\cos(v\kappa T)}{\kappa}. \]

Finally, \( \Delta\theta = v\kappa T \).

Problem 2 (Straight-motion limit): Show that the curved endpoint formula converges to the straight endpoint when \( \kappa → 0 \).

Solution: Use Taylor expansions around 0:

\[ \sin(z) = z - \frac{z^3}{6} + \mathcal{O}(z^5),\quad \cos(z) = 1 - \frac{z^2}{2} + \mathcal{O}(z^4), \quad z = v\kappa T. \]

Then:

\[ \Delta x = \frac{\sin(v\kappa T)}{\kappa} = \frac{v\kappa T + \mathcal{O}(\kappa^3)}{\kappa} = vT + \mathcal{O}(\kappa^2) \;\xrightarrow{\kappa → 0}\; vT, \]

\[ \Delta y = \frac{1-\cos(v\kappa T)}{\kappa} = \frac{\frac{(v\kappa T)^2}{2} + \mathcal{O}(\kappa^4)}{\kappa} = \frac{v^2T^2}{2}\kappa + \mathcal{O}(\kappa^3) \;\xrightarrow{\kappa → 0}\; 0. \]

Thus the straight primitive is recovered.

Problem 3 (Curvature feasibility): Suppose a robot has minimum turning radius \( R_{\min} \). Prove that any constant-curvature primitive with \( |\kappa| \le 1/R_{\min} \) respects the turning constraint.

Solution: Since turning radius is \( R=1/|\kappa| \) for nonzero curvature,

\[ |\kappa| \le \frac{1}{R_{\min}} \;\Rightarrow\; R = \frac{1}{|\kappa|} \ge R_{\min}. \]

If \( \kappa=0 \) then \( R=\infty \), also feasible.

Problem 4 (Lattice-compatible curvature set): Let arc length per primitive be \( s \) and heading resolution \( \delta_\theta \). Construct a curvature set \( \{\kappa_m\} \) such that each primitive lands exactly on a heading bin.

Solution: With fixed arc length, \( \Delta\theta = \kappa s \). Require:

\[ \kappa_m s = m\delta_\theta \;\Rightarrow\; \kappa_m = \frac{m\delta_\theta}{s}, \quad m \in \mathbb{Z}. \]

Apply feasibility: keep only \( m \) such that \( |\kappa_m| \le \kappa_{\max} \).

Problem 5 (Composition cost additivity): Suppose primitive cost is \( c(\pi)=s+w_\kappa|\kappa|s \) (ignore time). Show that the total cost of a path that is a sequence of primitives is the sum of primitive costs.

Solution: Define a path as \( P=(\pi_1,\dots,\pi_m) \) and total cost:

\[ C(P) \triangleq \sum_{i=1}^m c(\pi_i) = \sum_{i=1}^m \left(s + w_\kappa|\kappa_i|s\right) = ms + w_\kappa s \sum_{i=1}^m |\kappa_i|. \]

This additivity is exactly what makes graph search natural: edges (primitives) have costs that accumulate.

Motion primitives convert continuous nonholonomic motion generation into a discrete expansion mechanism suitable for graph-based planning. For car-like robots, constant-curvature primitives provide analytic endpoint maps, straightforward feasibility enforcement via curvature bounds, and clean SE(2) composition rules. These results directly support the next lesson, where candidate primitive trajectories are filtered by feasibility checks (constraints, obstacles, and map validity).