Skip to content
LumenaryAcademy
Planning 75 min

Configuration space and collision checking

Turning a robot with a shape into a point in a space of its own — the configuration space of an arm, Minkowski sums, and the bounding volumes, separating axes, GJK and distance fields that answer "does it touch?" millions of times a second.

Code tested · 6AI-draftedAwaiting review by a roboticist

Your guides: Chloe · The Builder, Margaret · The Experimenter, Gideon · The Safety Engineer

MotivationH

Turning a robot into a point

The last two lessons planned for a point. A* moved a point through a grid of cells; RRT grew a tree of points. But robots have shapes: a vacuum cleaner is a disc, a warehouse robot is a rectangle carrying a shelf, an arm is a chain of links shaped like tubes and boxes. The collision question is about shapes overlapping, and it seems to need geometry in every step of every planner.

The trick that makes planning tractable is to change the space, not the planner. Describe the robot completely by a short list of numbers, its configuration: (x,y)(x, y) for a disc, (x,y,θ)(x, y, \theta) for a rectangle that can turn, the joint angles for an arm. Each configuration is a point in a space of such lists, and each point is either free (the robot placed there touches nothing) or not. In that space the robot is a point, and the obstacles become regions with new, often strange shapes. Tomás Lozano-Pérez made this idea the foundation of motion planning in a 1983 paper; every planner in this level works in configuration space, whether it builds the regions explicitly or only samples them.

This lesson does three things. It looks at configuration spaces themselves: their dimension, their shape, and the obstacles in them, computed for a two-link arm and, exactly, for a polygon sliding in the plane. It then opens the black box the planners called "collision checker", which in practice consumes most of a planner's time, and builds its main parts: bounding volumes, hierarchies and the separating axis test. Finally it builds a signed distance field, the data structure that tells a robot not just whether it collides but how far it is from collision, which the next lesson uses to push paths away from obstacles.

IntuitionH

Configurations, and the shapes of the spaces they live in

Start with the simplest robot, a disc of radius ρ\rho moving in the plane without turning. Its configuration is its centre (x,y)(x, y), and it touches an obstacle exactly when its centre is within ρ\rho of it. So its configuration-space obstacles are the real obstacles grown by ρ\rho: the costmap inflation of Planning on graphs and grids was configuration space all along.

A rectangle that can also turn has configuration (x,y,θ)(x, y, \theta): three numbers. Picture its configuration space as a stack of slices, one per angle θ\theta, each slice being the obstacles grown by the rectangle at that angle. The slices change as θ\theta turns, and the top of the stack (θ=2π\theta = 2\pi) is glued to the bottom (θ=0\theta = 0), because a full turn brings the robot back to where it started.

An arm with two revolute joints that turn without limits has configuration (q1,q2)(q_1, q_2). Each angle lives on a circle, so the configuration space is a circle times a circle: a torus, the surface of a ring doughnut. Drawn flat it is a square whose left and right edges are the same place (q1=−180°q_1 = -180° and q1=180°q_1 = 180°), and likewise its top and bottom. A path that runs off one edge comes back on the opposite one.

workspace (metres)startgoalbaseconfiguration spaceq₁q₂−180°180°−180°180°SG
Left: the workspace of a planar two-link arm (links 1.0 m and 0.8 m) with three round obstacles; the arm is drawn at its start configuration (solid, q₁ = 110°, q₂ = −30°) and its goal (gold, dashed, q₁ = 170°, q₂ = 60°), and the dashed circle marks its reach. Right: the same problem in configuration space, computed by the first program below at 10° resolution. Shaded cells are configurations in collision; the line is the path breadth-first search finds. Opposite edges of the square are the same place (marked by matching arrows), so the square is really a torus: the path leaves through the left edge and re-enters at the right, next to the goal.

The figure shows the arm in its world and its configuration space side by side. One obstacle is close enough to the base that the upper arm hits it at a range of shoulder angles whatever the elbow does: that is the vertical band near q1=135°q_1 = 135°, cutting the torus all the way round. The arm's start is just on one side of that band and its goal just on the other, so the only way from one to the other is the long way round: the shoulder turns through 300°, past ±180°\pm 180°, while the elbow folds to keep the forearm out of the other obstacles. In the workspace this is surprising; in configuration space it is obvious.

DefinitionsH

Definitions

Definition— configuration space

The configuration of a robot is a minimal set of numbers that fixes the position of every point of the robot. The set of all configurations is the configuration space C\mathcal{C}, and its dimension is the robot's number of degrees of freedom. For a robot with geometry A(q)\mathcal{A}(q) at configuration qq (the region of space it occupies) among obstacles O\mathcal{O}, the configuration-space obstacle is Cobs={q∈C:A(q)∩O≠∅}\mathcal{C}_{\text{obs}} = \{ q \in \mathcal{C} : \mathcal{A}(q) \cap \mathcal{O} \ne \emptyset \} and the free space is Cfree=C∖Cobs\mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}. A collision-free path is a continuous curve in Cfree\mathcal{C}_{\text{free}}.

The common configuration spaces have names and shapes worth knowing:

RobotConfigurationC\mathcal{C}Dimension
Disc or point in the plane(x,y)(x, y)R2\mathbb{R}^22
One joint turning without limitqqcircle S1S^11
Planar arm with nn unlimited revolute joints(q1,…,qn)(q_1, \ldots, q_n)torus Tn=S1×⋯×S1T^n = S^1 \times \cdots \times S^1nn
Rigid body in the plane(x,y,θ)(x, y, \theta)SE(2)≅R2×S1SE(2) \cong \mathbb{R}^2 \times S^13
Rigid body in space (a drone, a free-floating object)position and orientationSE(3)≅R3×SO(3)SE(3) \cong \mathbb{R}^3 \times SO(3)6
Wheeled base carrying a 7-joint armbase pose and joint anglesSE(2)×SE(2) \times (7 joint ranges)10

Joints with limits (most real ones) turn circles into intervals, so a six-axis industrial arm's space is close to a box in R6\mathbb{R}^6, with the joint limits as walls. Orientation in 3-D is the subtle one: SO(3)SO(3) is not a torus of three angles, as Rotations and quaternions showed, and planners that sample orientations must sample it uniformly (with random unit quaternions) rather than drawing three Euler angles uniformly. Distances matter too: in SE(2)SE(2) a planner must decide how many metres a radian is worth, and the choice changes which configurations count as "nearest".

Definition— Minkowski sum

The Minkowski sum of two sets in the plane is A⊕B={a+b:a∈A, b∈B}A \oplus B = \{ a + b : a \in A,\ b \in B \}, and −B={−b:b∈B}-B = \{ -b : b \in B \}.

Theorem— C-obstacles of a translating robot

Let a robot occupy the region RR when its reference point is at the origin, and let it translate without turning, so at configuration t∈R2t \in \mathbb{R}^2 it occupies R+tR + t. Then its configuration-space obstacle for an obstacle OO is Cobs=O⊕(−R)\mathcal{C}_{\text{obs}} = O \oplus (-R). If RR and OO are convex polygons with mm and nn edges, Cobs\mathcal{C}_{\text{obs}} is a convex polygon with at most m+nm + n edges.

Proof

The robot at tt touches OO exactly when some point of the robot coincides with some point of the obstacle: r+t=or + t = o for some r∈Rr \in R and o∈Oo \in O. That says t=o+(−r)t = o + (-r), which is exactly the statement t∈O⊕(−R)t \in O \oplus (-R). For convex polygons, the sum is convex (a sum of convex combinations is a convex combination of sums), and walking around its boundary, the edges appear in order of direction, each edge of OO and each edge of −R-R appearing once (parallel ones merge), so there are at most m+nm + n of them.

∎

ComputationC

The configuration space of a two-link arm

The first program computes the right-hand side of the figure. For every pair of joint angles on a 10° grid it places the arm by forward kinematics, treats each link as a segment thickened by 5 cm, and tests it against each circle by the distance from the circle's centre to the segment. Then it runs breadth-first search on the grid with wrap-around neighbours (the index arithmetic is modulo 36), and prints the map with the path.

python
from collections import deque
import numpy as np

L1, L2, LINK_R = 1.0, 0.8, 0.05            # link lengths and link half-thickness (m)
CIRCLES = [((1.2, 0.7), 0.25),             # obstacles in the workspace: (centre, radius)
           ((-0.9, 0.9), 0.30),
           ((0.2, -1.3), 0.25)]

def seg_point_dist(a, b, p):
    ab, ap = b - a, p - a
    t = np.clip(ap @ ab / (ab @ ab), 0.0, 1.0)
    return np.linalg.norm(ap - t * ab)

def collides(q1, q2):
    base = np.zeros(2)
    elbow = np.array([L1 * np.cos(q1), L1 * np.sin(q1)])
    hand = elbow + [L2 * np.cos(q1 + q2), L2 * np.sin(q1 + q2)]
    return any(seg_point_dist(a, b, np.array(c)) <= r + LINK_R
               for c, r in CIRCLES for a, b in ((base, elbow), (elbow, hand)))

STEP = 10                                   # degrees per grid cell
angles = np.radians(np.arange(-180, 180, STEP))
N = len(angles)                              # 36 values per joint
cobs = np.array([[collides(a1, a2) for a1 in angles] for a2 in angles])   # rows: q2, cols: q1
print(f"{N} x {N} grid, {cobs.mean():.1%} of configurations collide")

def bfs(start, goal):
    """Breadth-first search on the torus: angles wrap round at +-180 degrees."""
    prev, queue = {start: None}, deque([start])
    while queue:
        cell = queue.popleft()
        if cell == goal:
            break
        r, c = cell
        for nr, nc in ((r + 1, c), (r - 1, c), (r, c + 1), (r, c - 1)):
            nxt = (nr % N, nc % N)
            if not cobs[nxt] and nxt not in prev:
                prev[nxt] = cell
                queue.append(nxt)
    path, cell = [], goal
    while cell is not None:
        path.append(cell); cell = prev.get(cell)
    return path[::-1] if path[-1] == start else None

idx = lambda deg: (deg + 180) // STEP
start, goal = (idx(-30), idx(110)), (idx(60), idx(170))   # (q2 index, q1 index)
path = bfs(start, goal)
print(f"start q = (110, -30) deg, goal q = (170, 60) deg: path of {len(path) - 1} grid steps")
crosses = sum(abs(a[1] - b[1]) > 1 or abs(a[0] - b[0]) > 1 for a, b in zip(path, path[1:]))
print("times the path wraps round through +-180 degrees:", crosses)

canvas = np.where(cobs, "##", "  ").astype(object)
for cell in path:
    canvas[cell] = "<>"
canvas[start], canvas[goal] = "SS", "GG"
print("q1 across, q2 down, both from -180 to +170 degrees in 10-degree cells; ## = collision")
for row in canvas:
    print("|" + "".join(row) + "|")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

About a fifth of all configurations collide (20.8%). The search finds a path of 51 steps of 10° that wraps round once through ±180°\pm 180°, as in the figure. Notice the shapes: the obstacle close to the base gives the full-height band, while the two farther obstacles, which only the forearm can reach, give diagonal bands. The forearm's direction is q1+q2q_1 + q_2, so it points at a given obstacle along lines where q1+q2q_1 + q_2 is roughly constant, slanting across the map.

ComputationC

Minkowski sums: exact C-obstacles for a sliding polygon

When the robot translates without turning, the theorem above gives its C-obstacles exactly. The program builds O⊕(−R)O \oplus (-R) for a triangular robot and a rectangular obstacle by adding every vertex of OO to every vertex of −R-R and taking the convex hull (simple, if not the fastest way). Then it checks the theorem on 5000 random positions of the robot: at each, it tests the actual triangle against the actual rectangle with the separating axis test (next section), and compares the answer with "is the reference point inside the C-obstacle?".

python
import numpy as np

def hull(points):
    """Convex hull by Andrew's monotone chain, anticlockwise, without repeated points."""
    pts = sorted(set(map(tuple, np.round(points, 12))))
    cross = lambda o, a, b: (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
    lower, upper = [], []
    for p in pts:
        while len(lower) >= 2 and cross(lower[-2], lower[-1], p) <= 0:
            lower.pop()
        lower.append(p)
    for p in reversed(pts):
        while len(upper) >= 2 and cross(upper[-2], upper[-1], p) <= 0:
            upper.pop()
        upper.append(p)
    return np.array(lower[:-1] + upper[:-1])

def area(P):
    x, y = P[:, 0], P[:, 1]
    return 0.5 * abs(np.dot(x, np.roll(y, -1)) - np.dot(y, np.roll(x, -1)))

def overlap(A, B):
    """Separating axis test for two convex polygons: they overlap unless some edge normal separates them."""
    for P in (A, B):
        for i in range(len(P)):
            edge = P[(i + 1) % len(P)] - P[i]
            axis = np.array([-edge[1], edge[0]])
            a, b = A @ axis, B @ axis
            if a.max() < b.min() or b.max() < a.min():
                return False
    return True

def inside(p, P):
    """Is p inside the anticlockwise convex polygon P (boundary counts as inside)?"""
    for i in range(len(P)):
        e, d = P[(i + 1) % len(P)] - P[i], p - P[i]
        if e[0] * d[1] - e[1] * d[0] < -1e-12:
            return False
    return True

robot = np.array([[-0.2, -0.1], [0.3, -0.1], [0.0, 0.3]])     # reference point at (0, 0)
obstacle = np.array([[1.0, 0.5], [1.6, 0.5], [1.6, 1.0], [1.0, 1.0]])
cobs = hull(np.array([o - r for o in obstacle for r in robot]))  # O + (-R)
print("C-obstacle vertices:", [tuple(map(float, v.round(3))) for v in cobs])
print(f"areas: obstacle {area(obstacle):.3f}, robot {area(robot):.3f}, C-obstacle {area(cobs):.3f} m^2")

rng = np.random.default_rng(3)
trials = rng.uniform([0.4, 0.0], [2.2, 1.6], size=(5000, 2))   # robot positions to test
agree = sum(overlap(robot + t, obstacle) == inside(t, cobs) for t in trials)
hits = sum(inside(t, cobs) for t in trials)
print(f"{agree} of {len(trials)} random positions agree ({hits} of them in collision)")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The C-obstacle is a hexagon spanning (0.7,0.2)(0.7, 0.2) to (1.8,1.1)(1.8, 1.1): the rectangle has 4 edges and the triangle 3, and two of those 7 edge directions coincide (the rectangle's top edge and the base of the reflected triangle are both horizontal), leaving 6. Its area, 0.890 m², is much more than the sum of the two areas (0.300 + 0.100 m²): growing an obstacle by a robot's shape adds area all around its boundary. All 5000 positions agree, 1523 of them in collision.

Two cautions. The C-obstacle depends on the reference point and on the robot's orientation: rotate the triangle and the hexagon changes, which is why the configuration space of a turning polygon is a stack of different slices. And non-convex shapes are handled by splitting them into convex pieces, because the sum of unions is the union of the pieces' sums.

ExplanationH

How collision checkers work

A planner may ask millions of collision questions per second, and a real robot and scene are made of meshes with thousands to millions of triangles. Testing every triangle against every other is hopeless. Collision libraries answer quickly by doing as little exact geometry as possible, in two phases:

  • Broad phase: cheap tests on simple bounding volumes (spheres, axis-aligned boxes, oriented boxes) wrapped around each object, to discard pairs that are obviously apart. Organised in a bounding volume hierarchy (BVH), a tree in which every node's volume encloses its children's, a query that misses a node discards its whole subtree.
  • Narrow phase: exact tests on the few pairs that survive, with algorithms specialised to convex shapes: the separating axis test for boxes and polygons, and GJK for general convex shapes.

Theorem— Separating axis theorem (convex polygons)

Two convex polygons are disjoint if and only if their projections onto some line are disjoint, and that line can always be chosen perpendicular to one of the polygons' edges. So two rectangles need at most 4 axes tested, and two convex polygons with mm and nn edges at most m+nm + n.

Proof

If projections onto some line are disjoint, the polygons are clearly disjoint. Conversely, AA and BB are disjoint exactly when the origin is not in A⊕(−B)A \oplus (-B), by the same reasoning as for C-obstacles. That set is a convex polygon whose edges are parallel to edges of AA or of BB, and a point lies outside a convex polygon exactly when it is on the outer side of one of its edges. The normal of that edge is an axis on which the projections of AA and BB do not overlap.

∎

Bounding volumes trade tightness for speed. A sphere test is a single distance comparison but fits a long, thin link badly; an axis-aligned box (AABB) is almost as cheap but loose for tilted objects; an oriented box (OBB) fits tightly but needs the separating axis test. The program compares the three on a tilted thin box, then builds a small BVH of axis-aligned boxes over 400 boxes scattered on a floor and checks a robot's footprint against it.

python
import math
import numpy as np

def box(cx, cy, hx, hy, theta):
    """An oriented box: centre, half-lengths along its own axes, and its angle."""
    return {"c": np.array([cx, cy]), "h": np.array([hx, hy]),
            "u": np.array([[math.cos(theta), -math.sin(theta)],      # columns: the box's axes
                           [math.sin(theta),  math.cos(theta)]])}

def sat(A, B):
    """Separating axis test for two oriented boxes: only the 4 face normals need checking."""
    d = B["c"] - A["c"]
    for axis in (*A["u"].T, *B["u"].T):
        rA = np.sum(A["h"] * np.abs(A["u"].T @ axis))   # half-width of A's shadow on the axis
        rB = np.sum(B["h"] * np.abs(B["u"].T @ axis))
        if abs(d @ axis) > rA + rB:
            return False                                  # found a separating axis
    return True

def aabb(B):
    """Axis-aligned bounding box of an oriented box: (min corner, max corner)."""
    ext = np.abs(B["u"]) @ B["h"]
    return B["c"] - ext, B["c"] + ext

def circle_hit(A, B):                                     # bounding circles
    return np.linalg.norm(A["c"] - B["c"]) <= np.linalg.norm(A["h"]) + np.linalg.norm(B["h"])

def aabb_hit(A, B):
    (a0, a1), (b0, b1) = aabb(A), aabb(B)
    return bool(np.all(a0 <= b1) and np.all(b0 <= a1))

A = box(0.0, 0.0, 1.0, 0.2, math.radians(45))             # a long thin box, tilted
for name, B in [("overlapping", box(0.6, 0.6, 0.3, 0.3, 0.0)),
                ("beside the thin box", box(0.9, -0.5, 0.3, 0.3, 0.0)),
                ("off its end", box(1.25, 0.0, 0.3, 0.3, 0.0)),
                ("far away", box(3.0, 0.0, 0.3, 0.3, 0.0))]:
    print(f"{name:20} circles {circle_hit(A, B)!s:5}  AABBs {aabb_hit(A, B)!s:5}  exact (SAT) {sat(A, B)}")

# A bounding volume hierarchy over 400 random boxes on a 40 m x 40 m floor.
rng = np.random.default_rng(1)
scene = [box(*rng.uniform(0, 40, 2), *rng.uniform(0.2, 0.8, 2), rng.uniform(0, math.pi)) for _ in range(400)]

def build(items):
    lo = np.min([aabb(scene[i])[0] for i in items], axis=0)
    hi = np.max([aabb(scene[i])[1] for i in items], axis=0)
    if len(items) <= 4:
        return {"lo": lo, "hi": hi, "leaf": items}
    axis = int(np.argmax(hi - lo))                         # split the longer side at the median
    items = sorted(items, key=lambda i: scene[i]["c"][axis])
    mid = len(items) // 2
    return {"lo": lo, "hi": hi, "kids": [build(items[:mid]), build(items[mid:])]}

def query(node, R, count):
    r0, r1 = aabb(R)
    count["aabb"] += 1
    if np.any(r1 < node["lo"]) or np.any(node["hi"] < r0):
        return []                                          # prune the whole subtree
    if "leaf" in node:
        count["sat"] += len(node["leaf"])
        return [i for i in node["leaf"] if sat(R, scene[i])]
    return query(node["kids"][0], R, count) + query(node["kids"][1], R, count)

tree = build(list(range(len(scene))))
robot = box(20.0, 20.0, 1.2, 0.6, math.radians(30))
count = {"aabb": 0, "sat": 0}
hits = query(tree, robot, count)
brute = [i for i in range(len(scene)) if sat(robot, scene[i])]
print(f"\nrobot vs 400 boxes: {len(hits)} collisions (brute force agrees: {sorted(hits) == brute})")
print(f"brute force: 400 SAT tests; hierarchy: {count['aabb']} box-box bound tests + {count['sat']} SAT tests")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The table shows each level of tightness rejecting more: a box beside the thin one fools both circles and AABBs, while one off its end fools only the circles. In the scene, the hierarchy finds the same 4 collisions as brute force with 39 cheap box-bound tests and 13 exact tests, instead of 400 exact tests. Because the tree halves the set at each level, the work grows roughly with the logarithm of the number of objects, plus the number of near misses.

In three dimensions the same ideas apply with more axes: two oriented boxes need 15 (3 face normals of each, plus the 9 cross products of an edge of one with an edge of the other). For general convex shapes, the GJK algorithm (Elmer Gilbert, Daniel Johnson and Sathiya Keerthi, 1988) avoids listing axes at all. It works on the same Minkowski difference A⊕(−B)A \oplus (-B), never building it, but asking only for its support point in a given direction (the farthest point of AA in direction dd minus the farthest point of BB in direction −d-d). It grows a small simplex (a point, segment, triangle or tetrahedron) inside the difference towards the origin; if the simplex encloses the origin the shapes intersect, otherwise the closest point of the difference to the origin gives the distance between the shapes and the witness points on each. Once shapes overlap, its companion EPA (expanding polytope algorithm) estimates how deeply. Because GJK needs only support functions, it handles spheres, capsules, cylinders and convex meshes with the same code.

ComputationC

Signed distance fields

A collision checker answers yes or no. Many planners, especially the trajectory optimisers of the next lesson, need more: how far from collision, and which way is away. A signed distance field (SDF) stores, at every point of a grid, the distance to the nearest obstacle surface, positive outside obstacles and negative inside (minus the penetration depth). Its gradient points away from the nearest obstacle and has length about 1 almost everywhere.

Robots then approximate their links by a few spheres (circles in 2-D), so that a link's clearance is the SDF at each sphere's centre minus its radius: one array lookup per sphere, no geometry at all. The program builds an SDF on a 5 cm grid for a round column and a crate, prints it in tenths of a metre, checks it against the exact distance to the column, and then nudges a link, modelled as 7 circles, a little way along the gradient at each step.

python
import numpy as np

RES = 0.05                                       # 5 cm cells, a 3 m x 2 m workspace
ny, nx = 40, 60
ys, xs = (np.indices((ny, nx)) + 0.5) * RES      # cell-centre coordinates (y, x) in metres
occ = ((xs - 1.0) ** 2 + (ys - 1.2) ** 2 <= 0.3 ** 2)            # a round column ...
occ |= (xs >= 1.9) & (xs <= 2.5) & (ys >= 0.3) & (ys <= 0.8)     # ... and a crate

def sdf(occ):
    """Signed distance between cell centres: positive outside obstacles, negative inside."""
    pts = np.stack([ys.ravel(), xs.ravel()], axis=1)
    def dist_to(mask):
        tgt = pts[mask.ravel()]
        d = np.full(len(pts), np.inf)
        for chunk in np.array_split(tgt, 20):
            d = np.minimum(d, np.min(np.linalg.norm(pts[:, None] - chunk[None], axis=2), axis=1))
        return d.reshape(occ.shape)
    return np.where(occ, -dist_to(~occ), dist_to(occ))

D = sdf(occ)

def lookup(p):
    """Bilinear interpolation of the field at a point p = (x, y) in metres."""
    gx, gy = p[0] / RES - 0.5, p[1] / RES - 0.5
    i, j = int(np.floor(gy)), int(np.floor(gx))
    fy, fx = gy - i, gx - j
    return ((1 - fy) * ((1 - fx) * D[i, j] + fx * D[i, j + 1]) +
            fy * ((1 - fx) * D[i + 1, j] + fx * D[i + 1, j + 1]))

def gradient(p, h=RES / 2):
    return np.array([lookup(p + [h, 0]) - lookup(p - [h, 0]),
                     lookup(p + [0, h]) - lookup(p - [0, h])]) / (2 * h)

print("signed distance field, top row = y near 2 m ('#' inside; digits = distance in tenths of a metre)")
for row in D[::-2, ::2]:
    print("".join("#" if d < 0 else str(min(int(d / 0.1), 9)) for d in row))

p = np.array([1.5, 1.2])                         # 0.5 m from the column's centre: 0.2 m outside it
print(f"\nat (1.5, 1.2): field {lookup(p):.3f} m, exact {np.linalg.norm(p - [1.0, 1.2]) - 0.3:.3f} m, "
      f"gradient {np.round(gradient(p), 3)}")

# A robot link approximated by 7 circles of radius 0.08 m, from (0.4, 0.7) to (1.6, 1.1).
centres = np.linspace([0.4, 0.7], [1.6, 1.1], 7)
for step in range(5):
    clear = [lookup(c) - 0.08 for c in centres]
    k = int(np.argmin(clear))
    print(f"step {step}: worst clearance {clear[k]:+.3f} m at circle {k}")
    centres = centres + 0.05 * np.array([gradient(c) for c in centres])  # nudge away from obstacles
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The field is 0.225 m at a point whose exact distance to the column is 0.200 m: an error of half a cell, because distances are measured between cell centres. The link starts 8 cm into the column (worst clearance −0.080-0.080 m). The first 5 cm step gains 12.5 cm, because this cell-centre field jumps by about a cell's width across the obstacle's surface; after that, each 5 cm step gains 5 cm, because the gradient has length 1, and after four steps the link is 0.195 m clear. Moving circles independently is only a sketch: for a real arm, each sphere's push must be turned into joint motions through the arm's Jacobian (Jacobians and velocity), which is exactly what the CHOMP optimiser in the next lesson does.

ExplanationH

Collision checking in practice

Libraries. MoveIt checks collisions with FCL by default (Bullet is an alternative), using BVHs of oriented boxes or sphere-swept volumes over meshes and GJK-based narrow phases. Coal, developed from FCL at LAAS-CNRS and Inria in France, speeds up GJK and EPA and is used with the Pinocchio dynamics library. Physics engines (MuJoCo, Bullet, PhysX, Drake's geometry engine) contain their own, tuned for contact simulation rather than planning queries; the Simulation lesson returns to them.

Robot models. Manufacturers' meshes are detailed and non-convex, so they are simplified for collision: convex hulls per link, convex decompositions (tools such as V-HACD and CoACD split a mesh into a few dozen convex pieces), or sets of spheres for GPU planners like cuRobo. Pairs of links that can never touch, or always touch (neighbours at a joint), are listed once in an allowed collision matrix and never checked, which removes most self-collision work.

Maps from sensors. Mobile robots and drones build distance fields directly from depth data: ETH Zürich's Voxblox and NVIDIA's nvblox integrate depth images into a truncated SDF and derive a Euclidean distance field from it, and OctoMap (from the University of Freiburg) stores probabilistic occupancy in an octree that collision checkers can query (the Depth and point clouds lesson introduced the sensors).

Margins and safety. Collision checking certifies a model, not the world. Models get a padding of a few millimetres to centimetres to cover calibration and tracking errors, and the safety of people around a robot is never left to the planner's collision model alone: standards and certified safety functions (speed and separation monitoring, force limits) do that job, as the embedded control and safety lesson explains.

Common misconceptionsH

Common misconceptions

Misconception

"Configuration space is just the workspace with fatter obstacles." Only for a robot that translates. As soon as the robot can rotate or has joints, the configuration space has more dimensions than the workspace and a different shape (a torus, SE(2)SE(2), SE(3)SE(3)), and obstacles become bands, slanted strips and twisted tubes, as the arm's map shows.

Misconception

"Joint angles can be treated as ordinary numbers." For a joint without limits, −179°-179° and 179°179° are 2° apart, not 358°. Distance functions, interpolation and nearest-neighbour searches must wrap angles, or a planner will send the arm the long way round, or miss paths that wrap. Conversely, a joint with limits must never be wrapped.

Misconception

"If the bounding volumes overlap, the objects collide." Bounding volumes are conservative: disjoint bounds prove that objects are apart, but overlapping bounds prove nothing. The narrow phase must still decide, as the thin tilted box shows.

Misconception

"A signed distance field gives exact distances." A grid SDF is accurate to about half a cell, and only as good as the map it was built from. Robots add a margin at least that large, and treat unknown space with care.

Exercises

Exercises

The Python exercises use NumPy and run in your browser.

1. Configuration space and collision checking

Warm-upHuman

A planar arm has two revolute joints, both able to turn round and round without limits. What is the shape of its configuration space?

Choose one

2. Configuration space and collision checking

Warm-upHuman

A quadrotor drone carries a small arm with two revolute joints underneath it, for picking fruit. Treat the drone's body as a rigid body free to move in space. What is the dimension of the configuration space of the drone with its arm?

3. Configuration space and collision checking

CoreHuman

A square robot with sides of 0.4 m slides without rotating, its reference point at its centre. A square obstacle with sides of 1.0 m sits with its sides parallel to the robot's. The configuration-space obstacle is a square. How long is its side, in centimetres?

4. Configuration space and collision checking

CoreHuman

In 2-D, the separating axis test for two oriented rectangles checks 4 axes (the edge normals of each). How many axes must be checked for two oriented boxes in 3-D?

5. Configuration space and collision checking

CoreHuman

A collision checker finds that the bounding spheres of two robot links overlap. What can it conclude?

Choose one

6. Configuration space and collision checking

CoreComputational

Write arm_collides(q1, q2, circles, l1=1.0, l2=0.8, link_r=0.05) for a planar two-link arm with its base at the origin. The elbow is at l1(cos⁡q1,sin⁡q1)l_1(\cos q_1, \sin q_1) and the hand at the elbow plus l2(cos⁡(q1+q2),sin⁡(q1+q2))l_2(\cos(q_1 + q_2), \sin(q_1 + q_2)). Each link is a segment thickened by link_r, and circles is a list of ((x, y), radius) obstacles. Return True if either link touches (or overlaps) any circle.

This is the test behind the configuration-space map in the lesson.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

7. Configuration space and collision checking

StretchComputational

Write obb_overlap(A, B) for two oriented rectangles, each given as (cx, cy, hx, hy, theta): the centre, the half-lengths along the rectangle's own xx and yy axes, and its angle in radians. Return True if they overlap (touching counts), using the separating axis test on the four axes (each rectangle's two axis directions).

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Further reading and resources