Project: plan a route and drive it

A plan computed on board, shortened, and then followed across a mat with two walls in it.

U9.7PlanningUniversity55 min

Do this lesson in the simulator

Everything so far ended at a printed number. This one ends with the robot in the green corner.

The mat has two walls that overlap in y, so there is no straight route. The robot has to go up over the first wall, down the corridor between them, under the second, and then up to the corner. It is about 320 cm of driving, and the plan for it has to be computed on board before a wheel turns.

The shape of the program

  1. Build the configuration space. Inflate the walls, and the mat edges too. Use more than the chassis radius: 10 cm is right here, because the next three steps all introduce error and the plan has to absorb it.
  2. Search. A* on the 40 by 40 grid, eight neighbours, octile heuristic. Straight out of U9.4.
  3. Shorten. The grid path is a staircase with sixty waypoints in it. Walk it and join the furthest pair of points that can still see each other through the inflated map, then repeat from there. Eight come out, and they are the corners of the route rather than the artefacts of the grid.
  4. Print the plan length. Before driving, so that if the drive goes wrong you already know whether the plan was sound.
  5. Follow it. Waypoint by waypoint, with the inverse kinematics from U2.

That order is the point of the whole module. Planning is a separate thing from driving, it happens first, and it produces an artefact you can inspect.

Why shortcut at all

A grid path is not a good thing to drive. It changes direction every cell or two, always by 45 or 90 degrees, and following it literally means the robot decelerates and accelerates sixty times for no reason. The shortcut pass is not cosmetic: it turns a discretisation artefact back into the straight lines the geometry actually wanted.

The line-of-sight test has to use the same inflated map the search used. Test the raw obstacles and the shortcut will happily cut a corner the search was careful to avoid.

def clear(a, b):
    d = math.hypot(b[0] - a[0], b[1] - a[1])
    n = max(2, int(d / 1.5))
    return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n + 1))

Sample the segment finer than the thinnest thing on the map. The walls here are 8 cm thick and inflated to 28, so 1.5 cm steps are ample.

Following waypoints

The robot is holonomic, so there is no need to turn towards anything. Hold the heading near zero, work out the world velocity you want, and convert:

vx_body = wx * math.cos(h) - wy * math.sin(h)
vy_body = wx * math.sin(h) + wy * math.cos(h)
drive(100 * vy_body / 20.0, 100 * vx_body / 15.0, spin_correction)

Aim at the current waypoint, take the next one when within a few centimetres, and ease off as the gap closes so the robot does not overshoot a corner. This is deliberately crude: a proper path follower, which looks ahead along the route rather than at one point, is U10.

position() is the lab's overhead camera and is allowed here. This module is about planning, not about working out where you are, and mixing the two problems is how projects become impossible to debug.

When it goes wrong

  • No route found. The inflation closed the corridor. Print the free-cell map and look at it.
  • Collision at a corner. The follower cut the corner. Either inflate more, slow down near waypoints, or tighten the arrival tolerance.
  • It drives past the last waypoint. The arrival test is on the distance to the waypoint, so a fast robot can pass through the tolerance between ticks. Ease the speed down with the gap.
  • The plan length is right and the robot is not where it should be. The follower is at fault, not the planner. That is why the plan is printed first.
from bugbot import *
import math
connect()

INFLATE = 10.0
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

# the free space the planner will be given, start S and goal G marked
for y in range(195, 0, -10):
    row = ""
    for x in range(0, 200, 5):
        if abs(x - 30) < 3 and abs(y - 30) < 6:
            row += "S"
        elif abs(x - 170) < 3 and abs(y - 170) < 6:
            row += "G"
        else:
            row += "." if free(x, y) else "#"
    print(row)

Run this in the simulator

Task: plan a route and drive it

Plan a route from (30, 30) to the green corner at (170, 170), print plan:, its length in centimetres, and then drive it. Do not touch either wall or the edge of the mat.

from bugbot import *
import heapq
import math
connect()

DT = 0.1
CELL = 5.0
N = 40
INFLATE = 10.0
V_MAX, V_LAT = 20.0, 15.0
START = (30.0, 30.0)
GOAL = (170.0, 170.0)
WALLS = [(70.0, 0.0, 8.0, 115.0), (125.0, 85.0, 8.0, 115.0)]

Challenges

  1. Plan with the cost map from U9.3 instead of plain distance and compare how close the robot comes to a wall on the way.
  2. Plan, then move a wall in your program's map by 15 cm without telling the planner, and watch what a wrong map does to a robot that trusts it.
  3. Replan every two seconds from where the robot actually is, rather than once at the start. What does that fix, and what does it cost?

What comes next

The plan in this project is a list of corners, and the robot drives at them one at a time, stopping and starting. That is not how anything moves well. U10 is about following a path properly: pure pursuit and its look-ahead distance, velocity profiles that respect what the machine can actually do, and turning a sequence of waypoints into a smooth trajectory a robot can track at speed.