---
jupyter:
  description: 'The shortest path of a round robot between obstacles: grow them by
    its radius with offsets, then search a visibility graph with Dijkstra''s algorithm.'
---

# 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.

![A round robot driving the shortest way between obstacles](../_static/gallery/path-planning.gif){.result}

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

```python nbsphinx="hidden"
from docs_figures import INK, LIGHT, Row
```

## 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:

```python
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).

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

```python nbsphinx="hide-input"
Row((room, "The obstacles, and where the robot's center may be",
     {"fill": False, "fills": [(free, LIGHT)] + [(o, INK) for o in obstacles]}), caption="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.

```python
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")
```

## 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.

```python
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)
```

## In motion

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

```python nbsphinx="hide-input"
fig, ax = plt.subplots(figsize=(10, 6))
room.Plot(ax=ax, face_col="whitesmoke", legend=False)
for g in grown:
    g.Plot(ax=ax, face_col="mistyrose", legend=False)
for o in obstacles:
    o.Plot(ax=ax, face_col="dimgray", legend=False)
ax.plot(*zip(*points), color="tab:red", lw=1.5)
body = plt.Circle(start, robot, color="tab:blue")
ax.add_patch(body)
ax.set_aspect("equal")
ax.axis("off")

along = [0.0]
for p, q in zip(points, points[1:]):
    along.append(along[-1] + math.dist(p, q))


def frame(n):
    target = along[-1] * n / 79
    k = max(i for i in range(len(points) - 1) if along[i] <= target) if target < along[-1] else len(points) - 2
    u = (target - along[k]) / (along[k + 1] - along[k])
    (x0, y0), (x1, y1) = points[k], points[k + 1]
    body.center = (x0 + u * (x1 - x0), y0 + u * (y1 - y0))
    return (body,)


animation = FuncAnimation(fig, frame, frames=80, interval=60)
plt.close(fig)
HTML(animation.to_jshtml())
```

```python nbsphinx="hidden"
import sys
sys.path.insert(0, "..")
from gallery import save_scene, save_figure, save_animation, HIDDEN
save_animation(animation, "path-planning")
```