Path planning around offset obstacles

The example is a round robot of radius 0.3 crossing a room of 10 × 6 with four obstacles. For the robot’s center, the obstacles are larger, grown by the robot’s radius, which is their offset. The shortest path for the center then runs along straight lines that touch the grown obstacles and around their rounded corners. It is found in a visibility graph between points just outside them, searched with Dijkstra’s algorithm. Lengths are in m.

import heapq
import math
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML
import planex as px

The room

The obstacles are two walls, a triangle and a round column. The robot starts at the top left and drives to the bottom right:

room = px.QuickSketch().Rectangle(10, 6).Face()
obstacles = [
    px.QuickSketch((2.1, 0)).Rectangle(0.8, 3.4).Face(),
    px.QuickSketch((4.6, 3.0)).Rectangle(0.8, 3.0).Face(),
    px.QuickSketch((6.8, 0.8)).Line((8.6, 1.4)).Line((7.4, 3.0)).Close().Face(),
    px.QuickSketch().Circle(8.0, 4.4, 0.7).Face(),
]
robot = 0.3
start = (0.8, 5.2)
goal = (9.3, 0.6)

The robot’s view

Each obstacle grows by the robot’s radius, its offset, and the room’s walls move in by it. What is left is where the robot’s center may be (Figure 1).

grown = [o.Offset(robot, inplace=False) for o in obstacles]
blocked = px.Unify(grown + [room.outer.Offset(robot)])
free = room - blocked

Figure 1: The room as the robot sees it

The visibility graph

Candidate corners lie just outside the grown obstacles, 0.02 out, along their outlines. Two points see each other if the straight line between them stays out of the blocked area.

nodes = [start, goal]
for o in obstacles:
    ring = o.Offset(robot + 0.02, inplace=False)
    nodes += [(x, y) for x, y in ring.ToShapely(0.005).exterior.coords[:-1] if free.Contains((x, y))]


def sees(p, q):
    return px.Line(p, q).Distance(blocked) > 1e-9

edges = {k: [] for k in range(len(nodes))}
for a in range(len(nodes)):
    for b in range(a + 1, len(nodes)):
        if sees(nodes[a], nodes[b]):
            d = math.dist(nodes[a], nodes[b])
            edges[a].append((b, d))
            edges[b].append((a, d))
print(len(nodes), "points,", sum(len(e) for e in edges.values()) // 2, "lines of sight")

81 points, 470 lines of sight

The shortest way

Dijkstra’s algorithm runs from the start to the goal. Check: the robot’s center never comes closer than the robot’s radius to an obstacle or a wall.

best = {0: 0.0}
came = {}
queue = [(0.0, 0)]
while queue:
    d, k = heapq.heappop(queue)
    if k == 1:
        break
    if d > best.get(k, math.inf):
        continue
    for j, step in edges[k]:
        if d + step < best.get(j, math.inf):
            best[j], came[j] = d + step, k
            heapq.heappush(queue, (d + step, j))
route, k = [1], 1
while k != 0:
    k = came[k]
    route.append(k)
points = [nodes[k] for k in reversed(route)]
path = px.FromPolyline(points)
clearance = min([path.Distance(o) for o in obstacles] + [path.Distance(room.outer)])
print("path", round(path.mass, 3), "m against", round(math.dist(start, goal), 3), "m straight")
print("closest approach", round(clearance, 4), "m, robot radius", robot)

path 10.161 m against 9.665 m straight
closest approach 0.3014 m, robot radius 0.3

In motion

The robot on its way, with the grown obstacles shaded: