The configuration space

Grow the obstacles by the robot's radius and the robot becomes a point, which is the whole reason planning is tractable.

U9.1PlanningUniversity30 min

Planning starts with a change of frame that makes the whole problem easier, and it is worth doing slowly because everything after it depends on the trick.

The robot is not a point. It is a 7 cm square chassis, so "is this position free?" really means "does the whole body fit here without touching anything?", and that is a question about an area, not about a spot. Asking it a hundred thousand times, once per candidate position, is expensive and fiddly.

Grow the obstacles instead

So do the work once. Take every obstacle and grow it outwards by the robot's radius. In the grown world, the robot is a point, and "free" is a simple test: is this point inside anything?

That grown world is the configuration space, usually written C-space. A configuration is a complete description of where the robot is: for a robot on a flat mat that can slide and turn, it is (x, y, heading), so C-space is three dimensional. The obstacles in C-space are the configurations that would be a collision, and they are what growing the rectangles approximates.

For a holonomic robot with a circular footprint, that approximation is exact and the heading drops out entirely, because a circle looks the same whichever way it is facing. The BugBot is a 7 cm square. The circle that contains that square whichever way it faces has a radius of half its diagonal, 7 times the square root of 2 over 2, which is 4.95 cm, and planning with that circle is conservative: it will never plan a route that collides, and it will occasionally refuse a gap the robot could actually have squeezed through straight on. That trade is almost always the right one.

A 3.5 cm circle, half the side, fits inside the square rather than around it, so it is not conservative: a corner can reach an obstacle the circle says is clear. The simulator models the body as that 3.5 cm circle, which is why a route planned with it works here. A real robot needs the 4.95.

How much to grow by

At least the radius that contains the chassis, 4.95 cm for a 7 cm square. In practice add more:

Six centimetres is the smallest number worth using on this mat. Ten is more comfortable once the robot is actually driving the plan. The cost of inflating too much is that narrow gaps close up and the planner reports no route at all, which is a far more visible failure than a plan that scrapes a wall.

Seeing it

from bugbot import *
connect()

BLOCK = (80.0, 90.0, 40.0, 16.0)
MARGIN = 6.0
bx, by, bw, bh = BLOCK

print("the block on the mat :", bx, "to", bx + bw)
print("grown by the margin  :", bx - MARGIN, "to", bx + bw + MARGIN)

# the mat from above, one character per 10 cm, with the grown block marked
for gy in range(190, -1, -10):
    row = ""
    for gx in range(0, 200, 10):
        inside = (bx - MARGIN <= gx <= bx + bw + MARGIN) and (by - MARGIN <= gy <= by + bh + MARGIN)
        row += "#" if inside else "."
    print(str(gy).rjust(3), row)

Run this in the simulator

The hashes are where a point robot may not go. Everywhere else, it may, and no further thought about the chassis is needed anywhere in the rest of this module.

What the free space looks like

Two words that come up constantly, and are worth pinning down now.

Connectivity. The free space breaks into connected components. If the start and the goal are in different components there is no route, and no planner can invent one. A large part of what a planner actually computes is which component things are in.

Narrow passages. A gap that is only slightly wider than the inflated robot. These are where every planner struggles: a grid may miss the passage because no cell centre lands in it, and a sampling planner may take a very long time to throw a dart into it. When a planner mysteriously fails, look for the narrow passage first.

Task: grow the obstacle, shrink the robot

Standing still, print inflated:, the width of the block in centimetres after growing it by the margin on both sides, and free lanes:, how many of the twenty lanes at x = 0, 10, 20 ... 190 a point robot could drive straight up from y = 30 to y = 170 without entering the grown block.

from bugbot import *
connect()

BLOCK = (80.0, 90.0, 40.0, 16.0)      # x, y, width, height on the mat
MARGIN = 6.0                          # the 4.95 cm around the square, plus a little

Challenges

1. Work out the smallest margin at which one more lane becomes blocked, and the largest at which one more becomes free.
2. The block is 16 cm deep. How wide would a gap have to be for the inflated robot to fit through it with 2 cm to spare either side?
3. The BugBot is square, not round. Describe a gap it could drive through that the circular approximation refuses.