---
title: "Robotics: Planning and Control"
module: Frontiers
moduleNumber: 6
lessonNumber: 4
order: 604
summary: >
  A robot that knows where it is still has to decide how to move, and then make a
  slipping, sensing-imperfect body actually go there. This lesson takes the pose
  estimate forward: planning motion in configuration space with cell decomposition
  and sampling-based roadmaps (PRMs and RRTs), planning under uncertainty with
  most-likely-state and online replanning, closing the loop with P/PD/PID control
  and potential fields, and finally the software architectures — subsumption,
  three-layer, and pipeline — that assemble it all, plus the learning-based turn in
  modern robotics.
topics: [Frontiers]
sources:
  - book: AIMA
    ref: "Ch. 25 — Robotics; §25.4 Planning to Move; §25.5 Planning Uncertain Movements"
  - book: AIMA
    ref: "§25.6 Moving; §25.7 Robotic Software Architectures; §25.8 Application Domains"
---

This builds on [Robotics](/artificial-intelligence/frontiers/robotics), which
grounded the abstract agent in a body — hardware, degrees of freedom, and
perception cast as probabilistic filtering — and left off with the robot able to
estimate its own pose and a map from noisy motion and range readings. That estimate
is where deliberation begins. Here we take it forward into a plan, and then into the
motor torques that make a drifting physical body actually follow the plan.

## Planning to move

Every robot deliberation ends in a decision about how to move an effector. The
**point-to-point motion** problem is to deliver the effector to a target location;
the harder **compliant motion** problem has the robot move while in physical
contact with an obstacle (screwing in a bulb, pushing a box). The first job is to
pick a representation in which such problems can be stated, and the key step is
to plan not in physical space but in **configuration space**.

### Configuration space

Take a two-joint arm. Describing it by the Cartesian coordinates of its parts —
$(x_e, y_e)$ for the elbow, $(x_g, y_g)$ for the gripper — gives a **workspace
representation**, good for collision checking but burdened with **linkage
constraints**: the elbow and gripper are a fixed distance apart because a rigid
forearm joins them, and a planner must generate only paths that respect that
nonlinear constraint. The alternative is to represent the state by the arm's
joint angles, $\mathbf{q} = (\varphi_s, \varphi_e)$ for the shoulder and elbow —
its **configuration**. The space $\mathcal{C}$ of all configurations is
**configuration space**, and in it a path can be a straight line: move each joint
at constant velocity from start to goal.

$$
% caption: Configuration space. The workspace arm (left) with obstacles maps to a
% 2-D C-space (right) in joint angles: white is free space (collision-free), dark
% is occupied space, and one dot marks the arm's current configuration q.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  % --- workspace (left) ---
  \draw[black] (0,0) rectangle (3.2,3.2);
  \node[anchor=south] at (1.6,3.25) {workspace};
  % base + arm
  \fill[black] (1.5,0) rectangle (1.7,0.5);
  \draw[acc, very thick] (1.6,0.5) -- (2.4,1.6);
  \draw[acc, very thick] (2.4,1.6) -- (1.9,2.6);
  \fill[acc] (1.6,0.5) circle (1.4pt);
  \fill[acc] (2.4,1.6) circle (1.4pt);
  % hanging obstacle
  \fill[black] (1.0,3.2) rectangle (1.25,1.9);
  \node[black, anchor=west, font=\scriptsize] at (0.05,2.4) {obstacle};
  % --- C-space (right) ---
  \begin{scope}[xshift=4.6cm]
    \draw[black] (0,0) rectangle (3.2,3.2);
    \node[anchor=south] at (1.6,3.25) {C-space};
    % occupied blobs
    \fill[black] (2.1,2.0) ellipse (0.9 and 1.1);
    \fill[black] (0.55,0.7) ellipse (0.6 and 0.9);
    \fill[black] (1.5,2.6) .. controls (2.0,2.2) and (1.8,1.4) .. (1.3,1.6)
      .. controls (0.9,1.8) and (1.0,2.5) .. (1.5,2.6);
    % free-config dot
    \fill[acc] (1.05,1.15) circle (1.8pt);
    \node[acc, anchor=west] at (1.15,1.15) {q};
    % axis labels
    \node[anchor=north, font=\scriptsize] at (1.6,-0.15) {shoulder angle};
    \node[anchor=east, font=\scriptsize] at (-0.1,1.6) {elbow angle};
  \end{scope}
\end{tikzpicture}
$$

Configuration space has its own costs. The task is usually stated in workspace
coordinates, so we must map between the two. Going from configuration to workspace
is easy — a chain of coordinate transforms, linear for prismatic joints and
trigonometric for revolute ones — a computation called **kinematics**. The inverse,
computing the configuration that puts the effector at a specified workspace point,
is **inverse kinematics**, and it is hard: the solution is rarely unique, since
several joint arrangements can place the gripper identically.

### Velocity kinematics: the Jacobian

Kinematics maps _positions_; robots also need to map _velocities_. If the joints
turn at rates $\dot{\mathbf{q}}$, how fast and in what direction does the effector
move? Differentiating the forward-kinematics map $\mathbf{x} = k(\mathbf{q})$ — the
effector position $\mathbf{x}$ as a function of the joint configuration $\mathbf{q}$
— by the chain rule gives a linear relation between the two velocity vectors,

$$
\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\,\dot{\mathbf{q}},
\qquad
\mathbf{J}(\mathbf{q}) = \frac{\partial k}{\partial \mathbf{q}}
= \begin{pmatrix}
\partial x_1/\partial q_1 & \cdots & \partial x_1/\partial q_n \\
\vdots & & \vdots \\
\partial x_m/\partial q_1 & \cdots & \partial x_m/\partial q_n
\end{pmatrix}.
$$

The **manipulator Jacobian** $\mathbf{J}(\mathbf{q})$ is the matrix of partial
derivatives of effector coordinates with respect to joint coordinates, one row per
task dimension and one column per joint. It is the same object the extended Kalman
filter linearized $f$ and $h$ with earlier — a first-order local map — here relating
the joint-velocity space to the workspace-velocity space at the current
configuration. Because it depends on $\mathbf{q}$, the Jacobian changes as the arm
moves: the identical joint rate produces a different effector velocity in a folded
pose than in an extended one. AIMA develops the kinematic map and its inverse in
§25.4; the Jacobian is the derivative of that map, and the velocity relation and
singularities here are its standard consequences.[^planning]

> **Definition (Manipulator Jacobian).** For an effector position $\mathbf{x} =
> k(\mathbf{q})$, the Jacobian $\mathbf{J}(\mathbf{q}) = \partial k/\partial
> \mathbf{q}$ is the matrix mapping joint velocities to effector velocity,
> $\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\,\dot{\mathbf{q}}$. Its columns are the
> effector velocities produced by each joint moving alone at unit rate.

$$
% caption: The velocity Jacobian of a 2-link planar arm. Joint rates
% $\dot q_1, \dot q_2$ at the shoulder and elbow map through $\mathbf{J}(\mathbf{q})$
% to an end-effector velocity $\dot{\mathbf{x}}$ in the plane. Column 1 of
% $\mathbf{J}$ is the effector velocity from the shoulder turning alone; column 2
% from the elbow turning alone; their weighted sum is the actual motion.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  % base
  \fill[black] (0,-0.25) rectangle (0,0.25);
  \draw[black] (-0.25,-0.25) rectangle (0.25,0.0);
  \fill[acc] (0,0) circle (2pt);
  \node[acc, anchor=north east, font=\scriptsize] at (-0.05,0.0) {shoulder};
  % link 1 to elbow at (2.6, 1.5) (30 deg, length 3)
  \draw[acc, very thick] (0,0) -- (2.6,1.5);
  \fill[acc] (2.6,1.5) circle (2pt);
  \node[acc, anchor=north west, font=\scriptsize] at (2.7,1.5) {elbow};
  % joint 1 rate arc
  \draw[red, ->, thick] (0.9,0.52) arc (30:70:1.04);
  \node[red, anchor=south, font=\scriptsize] at (0.55,1.0) {q1 rate};
  % link 2 to end-effector at (2.6+2.6, 1.5) ~ elbow +60 deg turn; go to (3.6,3.4)
  \draw[acc, very thick] (2.6,1.5) -- (3.6,3.4);
  \fill[black] (3.6,3.4) circle (2.4pt);
  \node[black, anchor=west, font=\scriptsize] at (3.75,3.4) {end-ef\/fector};
  % joint 2 rate arc at elbow
  \draw[red, ->, thick] (3.35,1.75) arc (60:110:0.9);
  \node[red, anchor=east, font=\scriptsize] at (2.95,2.35) {q2 rate};
  % end-effector velocity arrow
  \draw[red, ->, very thick] (3.6,3.4) -- (2.4,3.75);
  \node[red, anchor=south, font=\scriptsize] at (2.6,3.8) {x-dot};
  % the mapping label
  \node[black, anchor=west, font=\scriptsize] at (4.7,2.2) {x-dot = J(q) q-dot};
\end{tikzpicture}
$$

#### Worked example: the Jacobian of a two-link arm

Carry the two-link planar arm through the arithmetic. With link lengths $\ell_1 =
\ell_2 = 1$ and joint angles $\mathbf{q} = (\varphi_s, \varphi_e)$, forward
kinematics places the effector at

$$
x = \ell_1\cos\varphi_s + \ell_2\cos(\varphi_s + \varphi_e), \qquad
y = \ell_1\sin\varphi_s + \ell_2\sin(\varphi_s + \varphi_e).
$$

Differentiating each coordinate with respect to each angle gives the Jacobian

$$
\mathbf{J}(\mathbf{q}) = \begin{pmatrix}
-\ell_1\sin\varphi_s - \ell_2\sin(\varphi_s + \varphi_e) & -\ell_2\sin(\varphi_s + \varphi_e) \\
\phantom{-}\ell_1\cos\varphi_s + \ell_2\cos(\varphi_s + \varphi_e) & \phantom{-}\ell_2\cos(\varphi_s + \varphi_e)
\end{pmatrix}.
$$

Evaluate at $\varphi_s = 30^\circ$, $\varphi_e = 60^\circ$, so $\varphi_s +
\varphi_e = 90^\circ$. Using $\sin 30^\circ = 0.5$, $\cos 30^\circ = 0.866$,
$\sin 90^\circ = 1$, $\cos 90^\circ = 0$,

$$
\mathbf{J} = \begin{pmatrix} -0.5 - 1 & -1 \\ 0.866 + 0 & 0 \end{pmatrix}
= \begin{pmatrix} -1.5 & -1 \\ 0.866 & 0 \end{pmatrix}.
$$

Now turn the joints at $\dot{\mathbf{q}} = (0.1,\, 0.2)$ rad/s. The effector velocity
is the matrix-vector product

$$
\dot{\mathbf{x}} = \mathbf{J}\dot{\mathbf{q}} =
\begin{pmatrix} -1.5 & -1 \\ 0.866 & 0 \end{pmatrix}
\begin{pmatrix} 0.1 \\ 0.2 \end{pmatrix}
= \begin{pmatrix} -1.5(0.1) - 1(0.2) \\ 0.866(0.1) + 0 \end{pmatrix}
= \begin{pmatrix} -0.35 \\ 0.0866 \end{pmatrix} \text{ m/s}.
$$

The columns read off cleanly: turning only the shoulder ($\dot{\mathbf{q}} = (1, 0)$)
sweeps the effector at $(-1.5, 0.866)$, the velocity of a point $2$ units out on a
rotating link; turning only the elbow ($\dot{\mathbf{q}} = (0, 1)$) gives $(-1, 0)$,
the velocity of the forearm tip about the elbow. The actual motion is their weighted
sum.

#### Singularities

At some configurations the Jacobian loses rank — its columns become linearly
dependent — and the arm cannot move its effector in certain directions no matter how
it drives the joints. These are **singularities**. For the two-link arm, take the
elbow straight, $\varphi_e = 0^\circ$, so both links point the same way. Then
$\sin(\varphi_s + \varphi_e) = \sin\varphi_s$ and the two columns of $\mathbf{J}$
become parallel, giving

$$
\det \mathbf{J} = \ell_1\ell_2 \sin\varphi_e = 0 \quad\text{when } \varphi_e = 0.
$$

With the arm fully extended, both joints can only move the effector _across_ the
outstretched line, never _along_ it — the reachable effector velocities collapse
from the full plane to a single line. The determinant $\ell_1\ell_2\sin\varphi_e$
makes this exact: it vanishes at $\varphi_e = 0$ (and $180^\circ$), the boundary of
the workspace and the fold-back pose. Near a singularity the trouble is quantitative,
not just qualitative: to move the effector a little in the near-forbidden direction,
the joints must move a lot, so joint rates blow up.

#### Resolved-rate control

The Jacobian runs the other way to _command_ motion. Given a desired effector
velocity $\dot{\mathbf{x}}_d$ — follow a straight line, track a moving target — the
joint rates that produce it come from inverting the relation,

$$
\dot{\mathbf{q}} = \mathbf{J}(\mathbf{q})^{-1}\,\dot{\mathbf{x}}_d,
$$

recomputed at every step as $\mathbf{q}$ changes. This is **resolved-rate control**:
rather than solving the full nonlinear inverse kinematics for each target position,
resolve the desired _velocity_ into joint velocities through the local linear map,
integrate, and repeat. It sidesteps the multiplicity of inverse kinematics because it
tracks a velocity from a known starting configuration instead of jumping to an
arbitrary goal pose. For a **redundant** arm — more joints than task dimensions, so
$\mathbf{J}$ is not square — the inverse is replaced by the **pseudoinverse**
$\mathbf{J}^{+} = \mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top)^{-1}$, which returns the
smallest joint motion achieving $\dot{\mathbf{x}}_d$ and leaves the extra freedom to
a secondary objective (avoid a joint limit, stay clear of an obstacle). The catch is
the singularity: as $\det\mathbf{J} \to 0$ the inverse's entries diverge and
resolved-rate control demands impossible joint speeds, so practical controllers
damp the inverse near singularities, trading a little tracking accuracy for bounded
joint rates.

Obstacles complicate $\mathcal{C}$ further. It splits into **free space** — the
configurations the robot may attain — and **occupied space**, the rest. An obstacle
with a simple polygonal shape in the workspace can map to a wildly nonlinear,
even concave, region in configuration space; the shape of the free space is
generally too complex to construct explicitly. In practice a planner _probes_
$\mathcal{C}$: generate a candidate configuration, apply the kinematics, and check
for collisions in workspace coordinates to decide whether it lies in free space.
This is the classic **piano-mover's problem** — sliding a rigid body (a piano, a
robot) among obstacles reduces to path planning in the body's configuration space.

Two families of planner reduce this continuous problem to a discrete graph search.

### Cell decomposition

**Cell decomposition** carves free space into finitely many contiguous regions,
each simple enough that a path across it is trivial (a straight line). Planning
then becomes a discrete graph search over cells — the search of the earlier
[search lessons](/artificial-intelligence/search/informed-search). The simplest
version is a regular grid.

$$
% caption: Grid cell decomposition. Free cells become graph nodes; a shortest
% path from start to goal is found by A* or by value iteration over the grid.
% Mixed cells (part free, part occupied) are the source of unsoundness.
\begin{tikzpicture}[>=stealth, font=\footnotesize, scale=0.52]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  % grid
  \draw[black] (0,0) grid (8,6);
  % occupied cells (shaded)
  \foreach \x/\y in {3/1,3/2,3/3,4/3,4/4,4/5,5/1,5/2,2/4,2/5}
    {\fill[black] (\x,\y) rectangle ++(1,1);}
  % start + goal
  \node[circle, fill=acc, inner sep=1.6pt, label={[acc]below:start}] at (0.5,0.5) {};
  \node[circle, fill=red, inner sep=1.6pt, label={[red]above:goal}] at (6.5,5.5) {};
  % a path
  \draw[acc, very thick, ->] (0.5,0.5) -- (0.5,3.5) -- (1.5,3.5) -- (1.5,5.5)
    -- (5.5,5.5) -- (6.5,5.5);
\end{tikzpicture}
$$

The grid is trivial to implement but has three limitations. It scales badly: cell
count grows exponentially with dimension $d$ — the **curse of dimensionality**
again. It can be _unsound_ if it uses **mixed cells** (part free, part occupied),
since there may be no straight crossing, or _incomplete_ if it forbids them, since
the only route may pass through one. And any path through a grid is jagged, with
sharp corners no real robot can execute at speed. Refinements help: recursive
subdivision of mixed cells (complete if a minimum passage width is bounded, but
each split spawns $2^d$ children), **exact cell decomposition** into
irregularly-shaped simple cells, and **hybrid A\***, which stores the exact
continuous state each cell was reached in so the recovered trajectory is smooth
and executable.

The grid also invites a change of cost. A shortest path hugs obstacles — a parking
space with one millimeter of clearance is no parking space at all — so we add a
**potential field**, a function whose value grows as the configuration nears an
obstacle, into the cost. Minimizing path length plus potential trades a longer path
for a safer one that keeps its distance.

### Skeletonization

The second family, **skeletonization**, reduces free space to a one-dimensional
**skeleton** on which planning is a simple graph search. One skeleton is the
**Voronoi graph**: the set of points equidistant from two or more obstacles. To
plan, the robot moves in a straight line onto the graph, follows it to the point
nearest the goal, then leaves it for the target. Voronoi paths maximize clearance
but detour needlessly in open space and are hard to compute in high dimensions.

The alternative skeleton, the **probabilistic roadmap**, scales much better.
Rather than compute a skeleton analytically, _sample_ one: scatter many
random configurations, discard those not in free space, and join two survivors by
an arc when a straight line between them stays in free space. Adding the start and
goal yields a graph to search.

$$
% caption: A probabilistic roadmap. Random samples that land in free space (dots)
% become nodes; pairs joined by a collision-free straight segment become edges.
% Adding start and goal reduces planning to graph search over the sampled roadmap.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  \draw[black] (0,0) rectangle (6,3.6);
  % two obstacles
  \fill[black] (2.1,1.2) ellipse (0.7 and 0.9);
  \fill[black] (4.2,2.3) ellipse (0.8 and 0.7);
  % sampled nodes
  \foreach \n/\x/\y in {a/0.6/0.6, b/1.2/2.7, c/3.2/0.7, d/3.3/2.9, e/5.1/1.0,
    f/5.4/3.0, g/2.9/2.0, h/4.9/2.1}
    \fill[acc] (\x,\y) coordinate (\n) circle (1.6pt);
  % edges (collision-free straight segments)
  \draw[acc] (a) -- (b); \draw[acc] (a) -- (c); \draw[acc] (b) -- (g);
  \draw[acc] (c) -- (g); \draw[acc] (g) -- (d); \draw[acc] (c) -- (e);
  \draw[acc] (e) -- (h); \draw[acc] (h) -- (f); \draw[acc] (d) -- (f);
  % start + goal
  \node[circle, fill=red, inner sep=1.6pt, label={[red]below:start}] at (0.4,0.35) {};
  \node[circle, fill=red, inner sep=1.6pt, label={[red]above:goal}] at (5.6,3.25) {};
\end{tikzpicture}
$$

The method is theoretically incomplete — an unlucky sample set can leave start and
goal disconnected — but the failure probability shrinks with more samples, and
directing samples toward promising regions (or growing a tree outward from the
start, the **rapidly-exploring random tree**, RRT) makes it the method that scales
best to high-dimensional configuration spaces. An RRT grows by repeatedly picking a
random point, finding the nearest tree node, and extending a short branch toward
it, so the tree reaches quickly into unexplored free space.

$$
% caption: A rapidly-exploring random tree grows from the start. Each step samples
% a random point, finds the nearest existing node, and adds a short branch toward
% it, so the tree fans out to fill free space and reach the goal region.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  \draw[black] (0,0) rectangle (6,3.6);
  % root
  \fill[acc] (0.7,0.6) coordinate (r) circle (2pt);
  \node[acc, anchor=north] at (0.7,0.55) {start};
  % branches fanning out
  \draw[acc] (r) -- (1.7,1.3) coordinate (n1);
  \draw[acc] (r) -- (1.3,0.2) coordinate (n2);
  \draw[acc] (n1) -- (2.6,1.0) coordinate (n3);
  \draw[acc] (n1) -- (2.4,2.2) coordinate (n4);
  \draw[acc] (n3) -- (3.6,0.7) coordinate (n5);
  \draw[acc] (n4) -- (3.3,3.0) coordinate (n6);
  \draw[acc] (n4) -- (3.7,2.0) coordinate (n7);
  \draw[acc] (n5) -- (4.6,1.2) coordinate (n8);
  \draw[acc] (n7) -- (4.7,2.6) coordinate (n9);
  \draw[acc] (n8) -- (5.3,2.0) coordinate (n10);
  \foreach \p in {n1,n2,n3,n4,n5,n6,n7,n8,n9,n10} \fill[acc] (\p) circle (1.3pt);
  % goal region
  \draw[red, dashed] (5.2,2.6) circle (5mm);
  \node[red, anchor=west] at (5.7,2.6) {goal};
\end{tikzpicture}
$$

## Planning uncertain movements

None of the planners above face the defining trait of robotics: **uncertainty**.
It arrives from partial observability, from stochastic or unmodeled effects of
actions, and from the approximations of filtering itself, which never hands the
robot an exact belief.

The cheapest response is to ignore it: extract the **most likely state** from the
belief distribution and plan a single path through it as if it were certain. This
works when uncertainty is small. And because incorporating each new measurement
shifts the belief, many robots replan on the fly during execution — the **online
replanning** technique — so a stale path is repaired rather than blindly
followed.

When uncertainty is not small, single paths are too brittle, and the right object
is a policy. If the robot is uncertain only in its transitions but its state is
fully observable, the problem is a Markov decision process, whose solution is an
optimal **policy** (in robotics, a **navigation function**) telling the robot what
to do in every state. Under partial observability it becomes a POMDP, whose policy
is defined over the entire belief distribution — which lets the robot act on what it
does _not_ know, for instance by taking an **information gathering action** to
resolve a critical uncertainty (impossible in an MDP, which assumes full
observability). Exact POMDP solvers do not scale to continuous robot state, so
practical systems fall back on heuristics such as **coastal navigation**, which
keeps the robot near known landmarks to hold uncertainty down.

### Robust methods

A different stance handles uncertainty without probabilities at all. **Robust
control** assumes only that error is _bounded_, not distributed, and seeks a plan
that works for every value inside the bound. Its use in assembly is **fine-motion
planning** (FMP): moving an arm in close proximity to a static object, where the
motions and features are so small the robot cannot accurately measure or control
its position.

An FMP plan is a series of **guarded motions**, each a motion command paired with a
termination condition on the sensors. The commands are typically **compliant
motions** that let the effector slide along a surface rather than jam against it.
The design insight is to exploit the geometry so that _every_ outcome consistent
with the uncertainty bound still succeeds.

$$
% caption: Fine-motion planning for peg insertion. A single command aimed at the
% hole (left) may miss on either side, given the velocity uncertainty cone; a
% two-step guarded plan (right) deliberately hits to one side, then slides along
% the surface into the hole so every allowed trajectory succeeds.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  % --- left: naive, ambiguous ---
  \fill[black] (0,0) rectangle (3.4,0.9);
  \fill[white] (1.55,0) rectangle (1.85,0.9);
  \draw[black] (0,0) rectangle (3.4,0.9);
  \draw[black] (1.55,0) -- (1.55,0.9); \draw[black] (1.85,0) -- (1.85,0.9);
  % uncertainty cone
  \fill[acc!12] (1.7,2.3) -- (1.15,0.9) -- (2.25,0.9) -- cycle;
  \fill[acc] (1.7,2.3) circle (1.6pt);
  \draw[red, ->, thick] (1.7,2.3) -- (1.7,1.0);
  \node[red, anchor=west, font=\scriptsize] at (1.95,1.6) {v};
  \node[acc, anchor=south, font=\scriptsize] at (1.7,2.35) {start};
  \node[anchor=north, font=\scriptsize] at (1.7,-0.15) {which side of the hole?};
  % --- right: guarded, robust ---
  \begin{scope}[xshift=5cm]
    \fill[black] (0,0) rectangle (3.4,0.9);
    \fill[white] (1.55,0) rectangle (1.85,0.9);
    \draw[black] (0,0) rectangle (3.4,0.9);
    \draw[black] (1.55,0) -- (1.55,0.9); \draw[black] (1.85,0) -- (1.85,0.9);
    \fill[acc] (0.5,2.3) circle (1.6pt);
    \node[acc, anchor=south, font=\scriptsize] at (0.5,2.35) {start};
    % step 1: hit surface to the left
    \draw[acc, ->, thick] (0.5,2.3) -- (0.9,0.95);
    \node[acc, anchor=west, font=\scriptsize] at (0.85,1.6) {1: hit surface};
    % step 2: slide right into hole
    \draw[acc, ->, thick] (0.9,0.95) -- (1.55,0.95);
    \draw[acc, ->, thick] (1.7,0.9) -- (1.7,0.1);
    \node[acc, anchor=north, font=\scriptsize] at (1.7,-0.15) {2: slide in};
  \end{scope}
\end{tikzpicture}
$$

Robust plans are worst-case optimal — designed for the worst outcome rather than
the expected one — which is the right objective precisely when a failure during
execution costs far more than any of the other costs.

## Moving

So far we have planned motions; now the robot has to _move_. Plans from a
deterministic path planner assume the robot can follow any path exactly, but
robots have inertia and cannot execute arbitrary paths except at arbitrarily slow
speeds. In most cases the robot exerts _forces_ rather than commanding positions,
and this section computes those forces.

### Dynamics and control

The **dynamic state** extends the kinematic state with velocity (and possibly
acceleration). Its transition model is expressed as **differential equations**
relating a quantity to its rate of change. Planning directly in dynamic space would
give better performance, but the space has higher dimension than the kinematic
space, so the curse of dimensionality rules it out for all but the simplest robots.
Practical systems therefore plan a kinematic path and hand it to a **controller** —
a mechanism that generates controls in real time using feedback, to keep the robot
on the planned **reference path**.

Keeping a robot on a path sounds trivial and is not. Suppose the controller, on
seeing a deviation, applies an opposing force proportional to it. Let $y(t)$ be the
reference path and $x_t$ the state; this is a **P controller** (proportional):

$$
a_t = K_P\,(y(t) - x_t),
$$

with **gain parameter** $K_P$ setting how hard it corrects. A P controller is,
in the absence of friction, a spring law — driven back to the reference it
overshoots, then overshoots the other way, and oscillates forever. Shrinking $K_P$
only slows the oscillation; it does not stop it.

$$
% caption: Three controllers tracking a reference path (gray). The P controller
% (left) oscillates about the path; a smaller gain (middle) oscillates more slowly
% but still fails; the PD controller (right) adds a derivative term that damps the
% overshoot into smooth tracking.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  % --- P, high gain ---
  \draw[black, very thick] (0,0) -- (0,2.4);
  \draw[acc, thick] (0,0)
    .. controls (0.5,0.5) and (-0.5,0.9) .. (0.4,1.3)
    .. controls (-0.4,1.6) and (0.4,2.0) .. (0,2.4);
  \node[anchor=north, font=\scriptsize] at (0,-0.15) {P (high gain)};
  % --- P, low gain ---
  \begin{scope}[xshift=3cm]
    \draw[black, very thick] (0,0) -- (0,2.4);
    \draw[acc, thick] (0,0)
      .. controls (0.6,0.6) and (-0.6,1.0) .. (0.3,1.5)
      .. controls (-0.3,1.9) and (0.3,2.2) .. (0,2.4);
    \node[anchor=north, font=\scriptsize] at (0,-0.15) {P (low gain)};
  \end{scope}
  % --- PD ---
  \begin{scope}[xshift=6cm]
    \draw[black, very thick] (0,0) -- (0,2.4);
    \draw[acc, thick] (0,0)
      .. controls (0.5,0.5) and (0.15,1.0) .. (0.08,1.5)
      .. controls (0.03,2.0) and (0.0,2.2) .. (0,2.4);
    \node[anchor=north, font=\scriptsize] at (0,-0.15) {PD};
  \end{scope}
\end{tikzpicture}
$$

A controller is **stable** if small perturbations leave a bounded error, and
**strictly stable** if it returns to the reference. The P controller is stable but
not strictly stable. The **PD controller** adds a _derivative_ term:

$$
a_t = K_P\,(y(t) - x_t) + K_D\,\frac{\partial\,(y(t) - x_t)}{\partial t}.
$$

The derivative term dampens the system: when the error changes rapidly it opposes
the proportional term, killing the overshoot; when the error is steady it vanishes
and the proportional term takes over. A PD controller tracks smoothly where a P
controller thrashed. PD controllers still fail against a _systematic_ external
force — a car on a banked road pulled steadily to one side — that they never fully
cancel. To address this, add a third, _integral_ term accumulating error over time,
giving the **PID controller** (proportional–integral–derivative):

$$
a_t = K_P\,(y(t) - x_t) + K_I \int (y(t) - x_t)\, dt
    + K_D\,\frac{\partial\,(y(t) - x_t)}{\partial t}.
$$

The integral grows while a long-lived deviation persists until the control forces it
to shrink, wiping out systematic error at the cost of more oscillation risk. PID
controllers are the industrial standard across a wide range of control problems.

#### Worked example: why the derivative term damps

For example, track a reference $y(t) = 0$
(hold the path) with gains $K_P = 0.8$ and $K_D = 0.5$, sampling error at
$\Delta t = 1$. Suppose the robot has overshot to $x_t$ and is still rushing
further off course. Compare a P controller against a PD controller at two
consecutive steps, using the discrete derivative $\dot e \approx (e_t -
e_{t-1})/\Delta t$ with error $e_t = y(t) - x_t = -x_t$.

| Step | $x_t$ | error $e_t$ | $\dot e_t$ | P: $a = K_P e$ | PD: $a = K_P e + K_D \dot e$ |
| --- | --- | --- | --- | --- | --- |
| 1 | $0.50$ | $-0.50$ | $-0.30$ | $-0.400$ | $-0.400 + (0.5)(-0.30) = -0.550$ |
| 2 | $0.80$ | $-0.80$ | $-0.30$ | $-0.640$ | $-0.640 + (0.5)(-0.30) = -0.790$ |

At both steps the error is _growing more negative_ (the robot is accelerating away
from the path), so $\dot e < 0$ and the derivative term $K_D\dot e = -0.15$ adds to
the corrective push in the same direction. The PD controller commands a stronger
correction ($-0.550$ vs the P controller's $-0.400$) precisely _because_ the error
is worsening — it reacts to the trend, not just the present offset, and so it
begins braking before the overshoot peaks. Now flip to the return swing, when the
robot is racing back _toward_ the path: there $e_t$ is shrinking in magnitude, so
$\dot e$ has the _opposite_ sign to $e$, and $K_D\dot e$ subtracts from the
proportional term, easing off the throttle before the robot shoots past. That
asymmetry — push harder while diverging, ease off while converging — is what
converts the P controller's endless spring oscillation into the PD controller's
smooth settle.

$$
% caption: P versus PD tracking of a reference at zero. The P controller (blue)
% overshoots and oscillates because it reacts only to the present error; the PD
% controller (red) reads the error trend through the derivative term and brakes
% early, settling onto the reference.
\begin{tikzpicture}[>=stealth, font=\footnotesize]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  % axes
  \draw[->, black] (0,0) -- (8.4,0);
  \draw[->, black] (0,-1.6) -- (0,1.7);
  \draw[black, very thick] (0,0) -- (8.2,0);
  \node[anchor=south west, black, font=\scriptsize] at (8.5,0.03) {reference};
  \node[anchor=north] at (4.2,-1.65) {time};
  % P controller: decaying-but-persistent oscillation
  \draw[acc, thick]
    (0,0) .. controls (0.7,1.35) and (1.4,1.35) .. (2.1,0)
    .. controls (2.8,-1.15) and (3.5,-1.15) .. (4.2,0)
    .. controls (4.9,1.0) and (5.6,1.0) .. (6.3,0)
    .. controls (7.0,-0.9) and (7.7,-0.9) .. (8.1,-0.05);
  \node[acc, anchor=west, font=\scriptsize] at (8.15,0.55) {P};
  % PD controller: single overshoot then settle
  \draw[red, thick]
    (0,0) .. controls (0.7,1.2) and (1.6,1.1) .. (2.4,0.2)
    .. controls (3.2,-0.35) and (4.2,-0.2) .. (5.2,0.05)
    .. controls (6.2,0.12) and (7.2,0.03) .. (8.1,0.0);
  \node[red, anchor=west, font=\scriptsize] at (8.15,-0.55) {PD};
\end{tikzpicture}
$$

### Potential-field control

The **potential field** met earlier as a cost term can also generate motion
directly, skipping the planning phase. Define an attractive force pulling the robot
toward the goal and a repellent one pushing it from obstacles; the field's single
global minimum sits at the goal, and its value is the sum of distance-to-goal and
proximity-to-obstacle. The robot simply descends the field. No planning was
involved, and evaluating the gradient at the current configuration is cheap —
compared to path planners that are exponential in the DOFs, extremely so.

But potential fields have **local minima** that trap the robot: it may rotate a
single joint toward the goal until it wedges against the wrong side of an obstacle,
the field too coarse to make it bend its elbow. Potential-field control is
excellent for local motion, but global planning is sometimes still needed. And
because its forces depend on positions, not velocities, it is a kinematic method
that can fail if the robot moves fast.

## Robotic software architectures

An **architecture** is a methodology for structuring the algorithms — the languages,
tools, and overall philosophy for bringing programs together. A robot architecture
must decide how to combine two techniques with orthogonal strengths: _reactive_
control, sensor-driven and fast but blind to anything not sensed at the moment of
decision, and _deliberative_ planning, which sees the global picture but is slow.
Most architectures put reactive techniques at the low levels and deliberative ones
at the high levels; those that combine both are **hybrid architectures**.

### The subsumption architecture

The **subsumption architecture** (Brooks, 1986) assembles reactive controllers out
of finite state machines. Nodes may test sensor variables, arcs may emit messages to
motors or to other machines, and internal clocks time the traversals — the machines
are therefore **augmented finite state machines** (AFSMs). A four-state AFSM can
generate the cyclic leg motion of a hexapod walker: the swing phase watches its
sensor, and if the leg is stuck it retracts, lifts higher, and swings again. The
architecture composes complex controllers bottom-up from such machines.

Its virtues are also its limits. The AFSMs are driven by raw sensor input, which
works only when that input is reliable and complete; the lack of deliberation makes
it hard to change the robot's task; and the interplay of dozens of AFSMs becomes
impossible for a human to understand. Subsumption is rarely used in robotics today
despite its historical importance, though it left its mark on later architectures.

### The three-layer architecture

The most popular hybrid is the **three-layer architecture**: a reactive layer, an
executive layer, and a deliberative layer, distinguished by how fast they think.

$$
% caption: The three-layer architecture. The deliberative layer plans (minutes);
% the executive layer sequences its directives into reactive behaviors and hosts
% localization and mapping (seconds); the reactive layer runs the tight
% sensor-action loop on the hardware (milliseconds).
\begin{tikzpicture}[>=stealth, font=\footnotesize,
  lyr/.style={draw, minimum width=44mm, minimum height=11mm, align=center}]
  \definecolor{acc}{HTML}{2348F2}
  \node[lyr, draw=acc, text=acc, thick] (del) at (0,2.6) {deliberative layer\\planning : minutes};
  \node[lyr] (exe) at (0,1.0) {executive layer\\sequencing, localization : seconds};
  \node[lyr] (rea) at (0,-0.6) {reactive layer\\sensor-action loop : milliseconds};
  \node[draw, fill=black!8, minimum width=44mm, minimum height=8mm] (hw) at (0,-2.0) {sensors and e\/f\/fectors};
  \draw[->, acc, thick] (del) -- (exe) node[midway, right, font=\scriptsize] {directives};
  \draw[->, acc, thick] (exe) -- (rea) node[midway, right, font=\scriptsize] {behaviors};
  \draw[->, thick] (rea) -- (hw) node[midway, right, font=\scriptsize] {controls};
  \draw[->, thick, black] (hw.west) .. controls (-3.4,-2.0) and (-3.4,1.0) .. (exe.west)
    node[midway, left, font=\scriptsize] {sensor data};
\end{tikzpicture}
$$

The **reactive layer** provides low-level control with a tight sensor–action loop
cycling in milliseconds. The **executive layer** (or sequencing layer) is the glue:
it accepts directives from the deliberative layer — a set of via-points from a path
planner, say — decides which reactive behavior to invoke, and integrates sensor
information into an internal state, hosting the localization and online-mapping
routines, cycling in seconds. The **deliberative layer** generates global solutions
by planning, using models learned or supplied, cycling in minutes. The three-way
split is loose; real systems add layers for user interaction or multi-robot
coordination.

### The pipeline architecture

The **pipeline architecture** also runs many processes in parallel, but its modules
resemble the three-layer ones. Data enters at the **sensor interface layer**; the
**perception layer** updates the robot's world models; the **planning and control
layer** turns those into controls; and the **vehicle interface layer** sends them to
the hardware.

$$
% caption: The pipeline architecture (as in a robot car). Every stage runs in
% parallel and asynchronously: perception digests the freshest sensor data while
% control acts on slightly older data, the way perceiving, planning, and acting
% overlap in the brain rather than taking strict turns.
\begin{tikzpicture}[>=stealth, font=\footnotesize,
  st/.style={draw, minimum width=24mm, minimum height=12mm, align=center, font=\scriptsize}]
  \definecolor{acc}{HTML}{2348F2}
  \node[st] (s) at (0,0)   {sensor\\interface};
  \node[st] (p) at (3.1,0) {perception\\(world models)};
  \node[st] (c) at (6.2,0) {planning \&\\control};
  \node[st, draw=acc, text=acc] (v) at (9.3,0) {vehicle\\interface};
  \draw[->, acc, thick] (s) -- (p);
  \draw[->, acc, thick] (p) -- (c);
  \draw[->, acc, thick] (c) -- (v);
  \node[anchor=north, black, font=\scriptsize] at (4.65,-0.85) {all stages run at once, on data of di\/f\/ferent ages};
\end{tikzpicture}
$$

The key is that all of this happens at once. While perception digests the most
recent scan, control acts on slightly older data — the way we do not switch off our
motion controllers to process new sensory input, but perceive, plan, and act
simultaneously. Computation is data-driven and asynchronous, and the result is fast.

## Learning-based robotics

The pipeline this lesson built — localize with a filter, plan in configuration
space, track with a PID loop — is model-based end to end. Every stage runs on an
equation someone wrote by hand: the kinematic motion model, the sensor model, the
collision predicate, the control law. That works when the model is available and
accurate. The frontier past AIMA's Chapter 25 is what to do when it is not — when
the contact dynamics of a hand closing on an object, or the friction of a leg on
loose scree, or the mapping from raw camera pixels to a grasp, is too tangled to
write down. The answer that has taken over since the mid-2010s is to _learn_ the
hard stages from data while keeping the classical scaffolding around them.[^learning]

**Deep reinforcement learning for control.** The clearest demonstration that a
learned policy can replace a hand-derived controller is OpenAI et al.,
_Learning Dexterous In-Hand Manipulation_ (2018; IJRR 2020). They trained a
policy entirely in simulation to reorient a block held by a five-fingered Shadow
hand, then transferred it to the physical hand with no real-robot fine-tuning. The
key was **domain randomization**: randomizing the simulator's masses, frictions,
and visual appearance so widely that the real world looks to the policy like just
another random instance, closing the **sim-to-real** gap that had defeated earlier
transfer attempts. The policy learned dynamics the team never modeled — regrasping,
finger gaiting — that no PID loop over a written contact model would have produced.

**Legged locomotion via RL.** Hwangbo et al., _Learning Agile and Dynamic Motor
Skills for Legged Robots_ (Science Robotics, 2019), trained control policies for
the ANYmal quadruped in simulation and ran them on the real machine, recovering
from falls and running faster than the robot's prior hand-tuned controllers, with
an actuator network learned from data to bridge the sim-to-real gap in the motors
themselves. Lee et al., _Learning Quadrupedal Locomotion over Challenging Terrain_
(Science Robotics, 2020), pushed this to blind walking over mud, snow, and rubble
by training a policy in simulation and transferring it to a physical ANYmal that
had never seen those surfaces. Both replace the static-versus-dynamic-stability
gait analysis of this lesson with a policy that discovered its own dynamically
stable gaits — but note what stayed classical: the state estimation feeding the
policy is still a filter of the kind built above.

**Learned visuomotor policies.** Levine et al., _End-to-End Training of Deep
Visuomotor Policies_ (JMLR, 2016), trained a single convolutional network mapping
raw camera images straight to motor torques for manipulation tasks like screwing a
cap on a bottle, jointly optimizing perception and control rather than pipelining a
separate vision module into a separate controller. This is the sharpest contrast
with the three-layer and pipeline architectures: instead of perception updating a
world model that planning then consumes, one network is trained so its internal
representation is whatever best serves the control objective.

**Imitation and modern manipulation.** Where a reward is hard to specify but
demonstrations are easy to collect, **behavior cloning** learns a policy by
supervised regression from observed states to expert actions. A recent line
represents the policy as a generative model over action sequences: Chi et al.,
_Diffusion Policy_ (RSS, 2023), models the action distribution with a denoising
diffusion process, which handles the multimodality of human demonstrations (there
are several good ways to grasp a mug) better than a network that regresses to a
single averaged action. These methods layer directly onto classical stacks —
the learned policy still emits set-points that a PID loop tracks.

**Modern SLAM.** The EKF-SLAM sketched above — augment the state vector with
landmark positions, update quadratically — is one point in a large design space.
The canonical modern counterpart is Mur-Artal et al., _ORB-SLAM_ (IEEE T-RO,
2015): a feature-based **visual SLAM** system that tracks ORB keypoints across
frames, builds a sparse map, closes loops by place recognition, and refines the
whole trajectory-and-map estimate with **bundle adjustment** — a graph
optimization over all poses and points at once, rather than the single Gaussian an
EKF maintains. Bundle-adjustment SLAM scales to far larger maps than the EKF's
quadratic update allows and is what runs on today's drones and phones. It is
the graph-relaxation successor mentioned in the SLAM section.

$$
% caption: Classical model-based pipeline (top) versus an end-to-end learned
% policy (bottom). The classical stack chains hand-written filter, planner, and
% controller; the learned policy maps raw sensors to motor commands through one
% trained network, with the model-based estimator often still feeding it state.
\begin{tikzpicture}[>=stealth, font=\footnotesize,
  cbox/.style={draw, minimum width=21mm, minimum height=9mm, align=center, font=\scriptsize},
  lbox/.style={draw=acc, thick, minimum width=44mm, minimum height=11mm, align=center}]
  \definecolor{acc}{HTML}{2348F2}
  \definecolor{red}{HTML}{C0392B}
  % --- top: classical ---
  \node[cbox] (s1) at (0.4,1.6)   {sensors};
  \node[cbox] (f1) at (3.0,1.6) {f\/ilter\\(localize)};
  \node[cbox] (p1) at (5.6,1.6) {planner\\(C-space)};
  \node[cbox] (c1) at (8.0,1.6) {PID\\controller};
  \node[cbox] (m1) at (10.4,1.6) {motors};
  \foreach \a/\b in {s1/f1, f1/p1, p1/c1, c1/m1} \draw[->, thick] (\a) -- (\b);
  \node[anchor=east, black, font=\scriptsize] at (-0.9,1.6) {classical:};
  % --- bottom: learned ---
  \node[cbox] (s2) at (0.4,-0.6)   {sensors\\(raw pixels)};
  \node[lbox] (net) at (5.4,-0.6) {learned policy network\\(perception + control)};
  \node[cbox] (m2) at (10.4,-0.6) {motors};
  \draw[->, thick] (s2) -- (net);
  \draw[->, acc, thick] (net) -- (m2);
  \node[anchor=east, black, font=\scriptsize] at (-1.0,0.2) {learned:};
  % state estimate still fed in
  \draw[->, red, dashed] (f1.south) .. controls (2.6,0.4) and (3.6,0.3) .. (net.north west);
  \node[red, anchor=south, font=\scriptsize] at (3.2,0.15) {state estimate still classical};
\end{tikzpicture}
$$

Learning did not replace the classical machinery; it is layered onto it.
Configuration-space planning, particle and Kalman
filters, and PID control remain the backbone — the parts of the problem where a
model _is_ available and a guarantee matters. Learning is added at the stages where
writing the model down is the bottleneck: contact-rich manipulation, locomotion
over unmodeled terrain, and perception from raw high-dimensional sensors. A modern
robot is a hybrid, and every learned block in it still sits on a filter or a
controller from this lesson.

## Application domains

Robots are deployed across many domains. In **industry and agriculture**,
manipulators run assembly lines (welding, part placement, painting) more
cost-effectively than people, and outdoor machines harvest, mine, and strip paint
off ships far faster than human crews. In **transportation**, autonomous
straddle carriers move shipping containers, indoor gofers like the Helpmate robot
carry goods through hospitals, and Kiva systems shuttle shelves in fulfillment
centers. **Robotic cars** — spurred by the DARPA Grand and Urban Challenges, won by
STANLEY and BOSS — aim to cut the million-plus annual traffic deaths. In **health
care**, surgical robots place instruments in brains, eyes, and hearts with high
precision, and rehabilitation aids assist the elderly and handicapped. In
**hazardous environments**, robots clean nuclear waste (Chernobyl, Three Mile
Island), searched the World Trade Center rubble, and clear minefields. In
**exploration**, they reach the surface of Mars, the deep sea, and abandoned mines
that they map in 3D. And in **personal service** the Roomba became the
best-selling mobile robot of all, while **entertainment** (robotic soccer) and
**human augmentation** (exoskeletons, prosthetic limbs, teleoperation) round out
the list.

The common thread: the abstract agent computes an answer, while the robot must
enact it in a body that is already moving, sensing imperfectly, and slipping off
course. Each of the field's inventions — configuration space, particle filters,
PID loops, layered architectures — adapts the abstract machinery to those physical
constraints.

[^planning]: **AIMA**, §25.4 Planning to Move and §25.5 Planning Uncertain Movements: configuration space, free and occupied space, kinematics and inverse kinematics; cell decomposition and skeletonization (Voronoi graphs, probabilistic roadmaps, RRTs); most-likely-state planning, online replanning, POMDP navigation, and robust fine-motion planning with guarded compliant motions.
[^moving]: **AIMA**, §25.6 Moving and §25.7 Robotic Software Architectures: dynamics versus kinematics, P/PD/PID control and stability, potential-field control; the subsumption, three-layer, and pipeline architectures. §25.8 surveys the application domains.
[^learning]: Beyond AIMA Ch. 25, the learning-based robotics literature: OpenAI et al., "Learning Dexterous In-Hand Manipulation" (IJRR 2020; arXiv 2018) — sim-to-real RL with domain randomization on a Shadow hand. Hwangbo et al., "Learning Agile and Dynamic Motor Skills for Legged Robots" (Science Robotics, 2019) — RL locomotion on the ANYmal quadruped with a learned actuator network. Lee et al., "Learning Quadrupedal Locomotion over Challenging Terrain" (Science Robotics, 2020) — blind walking over rough terrain via sim-to-real transfer. Levine et al., "End-to-End Training of Deep Visuomotor Policies" (JMLR, 2016) — a single network from pixels to torques. Chi et al., "Diffusion Policy" (RSS, 2023) — action-sequence generation via denoising diffusion for manipulation. Mur-Artal, Montiel & Tardos, "ORB-SLAM: A Versatile and Accurate Monocular SLAM System" (IEEE Transactions on Robotics, 2015) — feature-based visual SLAM with bundle adjustment and loop closure.