devol.dev

Configuration space

A planner working directly on a robot’s shape asks the same question thousands of times: at this placement, does the body overlap anything? Every candidate motion is a shape test against every obstacle.

Configuration space answers all of those questions in advance, once, by moving the shape out of the robot and into the obstacles.

Motion planning / configuration space

The robot becomes a point

Solid shapes are the obstacles. The outlines around them are the placements where the robot would touch one, which is the obstacle grown by the robot's own shape. Change the robot and the outlines change with it. Drag the start or the goal.

ObstacleC-obstacleWhere the reference point may goRoute
Route
Detour
Turns
Corners

The solid shapes are the obstacles. The dashed outlines are the placements at which the robot would touch one, and they are what the planner actually works against. Grow the robot and the outlines grow with it. Grow it far enough and the gate near the left seals in the dashed picture while remaining perfectly visible in the solid one.

Configurations

A configuration is the smallest set of numbers that fixes where every part of the robot is. The configuration space CC is the set of all of them.

For the robot above, translating in the plane at a fixed orientation, a configuration is a point q=(x,y)q = (x, y) and C=R2C = \mathbb{R}^2. For a robot that can also turn, q=(x,y,θ)q = (x, y, \theta) and C=R2×S1C = \mathbb{R}^2 \times S^1. For a two-link arm, the configuration is the pair of joint angles and CC is a torus. For a six-axis industrial arm, CC is six-dimensional.

Write R(q)R(q) for the set of points the robot body occupies at configuration qq. The configurations that collide with an obstacle OO form a region of CC:

Cobs(O)={qC:R(q)O}C_{\text{obs}}(O) = \{\, q \in C : R(q) \cap O \neq \emptyset \,\}

and what is left is the free space CfreeC_{\text{free}}. The robot is a point in CC, and any path through CfreeC_{\text{free}} is a motion that never touches anything. Nothing has been approximated yet. This is just a change of question.

Building the region

For a robot that only translates, R(q)=R+qR(q) = R + q, and the region has a closed form. Work through the definition:

(R+q)O    rR, oO:q+r=o    r,o:q=or(R + q) \cap O \neq \emptyset \iff \exists\, r \in R,\ o \in O : q + r = o \iff \exists\, r, o : q = o - r

so the colliding configurations are exactly

Cobs(O)={or:oO, rR}=O(R)C_{\text{obs}}(O) = \{\, o - r : o \in O,\ r \in R \,\} = O \oplus (-R)

the Minkowski sum of the obstacle with the robot reflected through its reference point. The obstacle grown by the robot, turned around.

Both shapes here are convex, so the sum is convex too and is the convex hull of the pairwise sums of their vertices. There is nothing sampled about the dashed outlines: they are exact polygons, and a point is inside one exactly when the robot placed there overlaps the obstacle.

The walls close in for the same reason. The reference point has to stay far enough from a wall to keep the rest of the robot inside, which insets the arena by the robot’s reach in each direction.

The reference point is a choice

The reflection is the easy part of that to skip. Slide the reference point toward the front of the robot: the marker stays put, the body hangs back off it, and every dashed outline slides forward by the same amount.

That is the minus sign, and it is why the robot is reflected rather than merely grown. Growing an obstacle by the robot gives the right answer only when the robot is measured outward from the point the configuration names. Choose a different point on the body and you get a different, equally correct configuration space, shifted to match.

What this buys

Collision checking becomes a point test. No shape, no orientation, no overlap computation. Is this point inside one of these regions.

The shortest path has a known shape. In a plane with polygonal obstacles a shortest path is a polyline that bends only at obstacle corners, because any bend in open space can be cut across and any bend at a flat edge can be flattened. So the whole continuous problem collapses onto a graph whose nodes are those corners. Turn on the visibility graph and that is what you are looking at. The route it returns is not the best route at some resolution, it is the best route.

Every planner becomes reusable. A* over a grid, probabilistic roadmaps, rapidly exploring random trees: none of them care what the robot looks like, only what the free space is. That is why a planner written for one robot runs on the next one.

Narrow passages

Start and goal sit on the same side of a wall with two ways through: a gate 60 units wide near the left, and an 80 unit passage past the right end. A small robot takes the gate, and the route is a couple of percent longer than the straight line.

Now drag the size up. The wall does not move. What moves is the two grown regions, creeping toward each other until they meet at size 30, where the six-sided robot is 60 units wide and the gate is not one unit wider. The corridor is gone, the route swings to the far end of the arena, and the detour goes from a few percent to three hundred. Keep going and the right-hand passage closes the same way, the free space breaks into two pieces, and there is no route at all.

Reading that off the solid shapes takes a measurement and a comparison. Reading it off the dashed ones takes a glance, because the question of whether a gap is usable is not a question about the gap. It is a question about connectivity of the free space, and configuration space is where connectivity lives.

This is also the standard failure of sampling-based planners. A narrow passage occupies very little volume in CfreeC_{\text{free}}, so random samples rarely land in it, and the planner spends its time proving what it already knows about the open parts of the space.

Turning costs a dimension

The angle slider changes the robot’s orientation, and every dashed outline changes shape. Each one is a slice of the real thing.

For a robot that rotates, the configuration space is three-dimensional and the region belonging to a single obstacle is

Cobs(O)={(x,y,θ):(x,y)O(R(θ))}C_{\text{obs}}(O) = \{\, (x, y, \theta) : (x,y) \in O \oplus (-R(\theta)) \,\}

a solid whose horizontal cross sections are the outlines drawn here, twisting as θ\theta climbs. It is not convex even when the robot and the obstacle both are.

Set the robot to four sides at size 34 and turn it. Square on, the flats face the gate and it fits. By 3030^\circ the diagonal is what has to pass, the gate seals, and the route swings around the arena. A planner working one slice at a time sees those as two different worlds. A planner working in all three dimensions sees one world with a corridor that only exists at some angles, which is the ordinary problem of getting a sofa through a doorway.

Where the closed form runs out

The Minkowski sum works because the body translates rigidly. An articulated arm does not. Rotate the first joint of a two-link arm and the second link sweeps through the workspace along an arc, so the set of joint angles that put some part of the arm inside an obstacle has no elementary description. Its configuration space is a torus with regions on it that nobody writes down in closed form.

That is the general case, and it leaves two options. Discretise CC and search it, which is what a grid planner does and which costs a number of cells exponential in the number of joints. Or sample CC, check each sample with a collision test, and connect the ones that pass, which is what probabilistic roadmaps and RRTs do and which gives up completeness for tractability.

Both of them plan in configuration space. The change of question survives even where the closed form does not, and that is the part worth keeping: decide what a configuration is, work out which configurations are free, and the robot is a point again.

The code

The construction in C++, with the reference point passed in rather than assumed to be the centroid:

struct Point { double x, y; };

// Below this, two coordinates are the same coordinate.
constexpr double kEps = 1e-9;

double cross(const Point& o, const Point& a, const Point& b) {
    return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x);
}

// The robot measured outward from its reference point, then turned
// around: { ref - p : p in body }. The reference point is an argument
// because it is a choice, and choosing a different one slides every
// region below by the same vector.
std::vector<Point> reflectAbout(const std::vector<Point>& body, const Point& ref) {
    std::vector<Point> out;
    out.reserve(body.size());
    for (const Point& p : body) out.push_back({ref.x - p.x, ref.y - p.y});
    return out;
}

// Convex hull by monotone chain, counter-clockwise. Collinear points
// are dropped. A Minkowski sum of two polygons that share an edge
// direction produces plenty of them, and a vertex that cannot bend a
// path is a vertex the planner is better off never seeing.
std::vector<Point> convexHull(std::vector<Point> pts) {
    std::sort(pts.begin(), pts.end(), [](const Point& a, const Point& b) {
        return a.x < b.x || (a.x == b.x && a.y < b.y);
    });
    if (pts.size() < 3) return pts;

    auto half = [](const std::vector<Point>& src) {
        std::vector<Point> out;
        for (const Point& p : src) {
            while (out.size() >= 2 &&
                   cross(out[out.size() - 2], out.back(), p) <= kEps) {
                out.pop_back();
            }
            out.push_back(p);
        }
        out.pop_back();          // the shared endpoint, left to the other half
        return out;
    };

    const std::vector<Point> reversed(pts.rbegin(), pts.rend());
    std::vector<Point> hull = half(pts);
    const std::vector<Point> upper = half(reversed);
    hull.insert(hull.end(), upper.begin(), upper.end());
    return hull;
}

// Minkowski sum of two convex polygons, as the hull of every pairwise
// sum of vertices. There is a linear-time merge of the two edge lists
// that does the same thing; at the handful of vertices a robot
// footprint has, the hull is quicker to be sure of.
std::vector<Point> minkowskiSum(const std::vector<Point>& a,
                                const std::vector<Point>& b) {
    std::vector<Point> pts;
    pts.reserve(a.size() * b.size());
    for (const Point& p : a) {
        for (const Point& q : b) pts.push_back({p.x + q.x, p.y + q.y});
    }
    return convexHull(std::move(pts));
}

// The placements of the reference point at which the robot overlaps
// this obstacle: O (+) (-R). Both polygons convex, wound
// counter-clockwise, in world coordinates.
std::vector<Point> cObstacle(const std::vector<Point>& obstacle,
                             const std::vector<Point>& body,
                             const Point& reference) {
    return minkowskiSum(obstacle, reflectAbout(body, reference));
}