Capstone: Polygonal Robot Route Planner

Capstone: Polygonal Robot Route Planner

A route planner is where clean geometric ideas meet inconvenient physical facts. A point can slip through an arbitrarily narrow gap; a robot with width cannot. Two edges that almost touch may be safely separated in a drawing yet overlap after rounding. A shortest polyline is useless if the robot collides while following it. In this capstone you will build an auditable planner for a rigid polygonal robot that translates among polygonal obstacles.

The central reduction comes from Part 21: replace the moving robot by a reference point and expand every obstacle into a configuration-space obstacle, or C-obstacle. Part 22 then supplies the global planner: connect mutually visible vertices, search the resulting visibility graph, and validate the returned motion in the original workspace. Earlier work on robust predicates, segment intersections, subdivisions, and polygon overlays supplies the machinery that keeps those steps consistent.

The baseline allows translation but fixes orientation. That restriction is deliberate. It produces a genuinely useful planner in two dimensions while keeping the configuration space planar. Rotation is an extension, not a hidden assumption.

Product brief and deliverables

Build a library and small command-line application that accepts a polygonal robot, polygonal obstacles, a start placement, and a goal placement. It returns either a collision-free route for the robot's reference point or a structured explanation of why no route was found.

Your submission should include:

1. a strict parser and normalization report;
2. collision and configuration-space modules;
3. C-obstacle union or an equivalent arrangement-aware representation;
4. a visibility-graph builder and Dijkstra or A* search;
5. workspace replay that validates every segment of the final route;
6. deterministic fixtures, property tests, and seeded stress tests;
7. benchmarks for preprocessing, graph construction, search, and validation;
8. SVG or GeoJSON diagnostics showing workspace and configuration space.

Input and output contracts

Use a documented Cartesian coordinate system and one length unit. Each polygon is a cyclic list of finite coordinate pairs. The robot must be simple and connected. If holes or multipolygons are supported, state their ring convention. Start and goal specify world coordinates of the chosen reference point. Fixed orientation is part of the request.

Pick the reference point once—often a marked body origin or the centroid—and express every robot vertex relative to it. Changing the reference point translates all C-obstacles and placements; doing so midway invalidates the plan.

Define boundary contact before writing predicates. A conservative physical planner normally treats touching an obstacle as collision. A demonstration mode may permit tangency, but then every module—point-in-polygon, visibility, C-obstacle membership, and replay—must use that same closed-versus-open convention.

Architecture: transform, search, certify

Keep stages independently testable. Normalization must not know about A*. Search must not make collision decisions. The final validator calls a collision API rather than trusting graph construction.

Phase 1: normalize geometry and establish policy

First remove consecutive duplicate vertices and a repeated closing vertex. Reject nonfinite coordinates, rings with fewer than three distinct vertices, self-intersections, and zero area. Normalize ring orientation. Preserve source IDs so every derived edge can be traced back to input.

Resolve obstacle overlaps under a declared policy: union obstacles before expansion, or union C-obstacles afterward. The latter is often simpler because expansions may overlap when workspace obstacles do not. Part 8's overlay is directly relevant: the planner needs the exposed forbidden boundary, not interior seams.

Use a filtered orientation predicate and robust segment intersection from the opening parts of the series. Scaling a global epsilon until tests pass is not a policy. Record exact, filtered, or approximate predicate mode in the build report; Part 23 explains why the choice belongs in the public engineering contract.

Phase 2: build configuration-space obstacles

Let $R$ be the robot polygon expressed relative to reference point $r=0$, and let $O_i$ be a workspace obstacle. A reference placement $q$ collides exactly when

\[ q \in C_i = O_i \oplus (-R), \]

where $-R=\{-x:x\in R\}$ is the reflection of the robot through its reference point. Intuitively, sweep the reflected robot around the obstacle boundary; the area it covers is the set of forbidden reference-point positions.

For convex polygons, compute the Minkowski sum in linear time after cyclically ordering edges by direction. Start from canonical lowest vertices, then merge the two edge-vector sequences as one would merge sorted lists. When directions tie, advance both and add their vectors. Remove collinear intermediate vertices under the same exact sign policy.

For nonconvex inputs, decompose robot and obstacles into convex pieces, compute pairwise convex sums, then union them. Pair counts grow quickly, so retain provenance and benchmark intermediate complexity. A production offset library is acceptable when its fill rule, precision model, and guarantees are documented and its output independently validated.

After constructing all $C_i$, compute their union boundary or maintain an arrangement that can answer whether a segment enters the forbidden union. Remove vertices buried strictly inside another C-obstacle; they must not become visibility-graph nodes. Build a spatial index over exposed boundary segments to accelerate line-of-sight and validation queries.

Phase 3: construct the visibility graph

Create graph nodes for start, goal, and every exposed C-obstacle vertex. Connect two nodes when the open segment between them lies in free space under the selected contact policy. Weight the edge by Euclidean distance. A shortest collision-free polyline among polygonal obstacles is taut: except at start and goal, its bends occur at visible obstacle vertices. That is the reason this finite graph can represent an optimal route.

The simplest builder tests all $O(V^2)$ node pairs against all boundary edges and is invaluable as a small-instance oracle. Then add rotational sweep or spatial-index pruning. Do not check only proper intersections: a candidate can graze a vertex, overlap an edge, or begin on the boundary. Classify these cases explicitly; finite sampling cannot replace continuous testing.

If tangency is forbidden, any boundary contact blocks visibility except a permitted endpoint occurrence with a departure direction that immediately enters free space. If tangency is allowed, use local incident-edge wedges to decide whether a segment merely touches or passes through obstacle interior.

Phase 4: search, simplify, and certify

Dijkstra's algorithm finds the shortest graph route for nonnegative Euclidean weights. A* usually explores fewer nodes with heuristic $h(v)=\lVert v-g\rVert$, which is admissible and consistent by the triangle inequality. Stable tie-breaking by node ID makes identical inputs reproducible.

Path smoothing comes after a correct graph route. Repeatedly replace a waypoint subsequence by its chord when the chord passes the same robust free-space test. This cannot increase length. More ambitious funnel-style smoothing is appropriate when a corridor or triangulated free space is available, but it must still be replayed.

from dataclasses import dataclass
from heapq import heappop, heappush

@dataclass(frozen=True)
class PlanResult:
    status: str
    path: tuple[tuple[float, float], ...]
    length: float | None
    witness: dict

def plan(scene, robot, start, goal, policy):
    clean = normalize(scene, robot, start, goal, policy)
    reflected = reflect_about_origin(clean.robot)
    pieces = []
    for obstacle in clean.obstacles:
        for op in convex_decompose(obstacle):
            for rp in convex_decompose(reflected):
                pieces.append(minkowski_sum_convex(op, rp))
    forbidden = polygon_union(pieces, policy.predicates)

    for name, q in (("start", start), ("goal", goal)):
        hit = forbidden.classify(q, policy.contact)
        if hit.blocked:
            return PlanResult(f"{name.upper()}_IN_COLLISION", (), None,
                              {"feature": hit.feature_id})

    graph = visibility_graph(forbidden, start, goal, policy)
    route = astar(graph, start, goal)
    if route is None:
        return PlanResult("NO_PATH", (), None, graph.stats())

    route = shortcut(route, forbidden.segment_is_free)
    certificate = replay_in_workspace(route, clean, policy)
    if not certificate.valid:
        return PlanResult("VALIDATION_FAILED", (), None,
                          certificate.witness)
    return PlanResult("OK", tuple(route), polyline_length(route),
                      certificate.as_dict())

Workspace replay translates every robot edge along every path segment and checks the resulting swept region against every relevant obstacle. It may reuse the configuration-space test as one certificate, but an independent swept-polygon or continuous collision implementation is a stronger differential oracle. Verify endpoints, every segment, and waypoint transitions. Return the first colliding robot feature and obstacle feature if certification fails.

Degeneracies and failure semantics

Geometric edge cases are ordinary inputs. Endpoints may lie on a C-obstacle boundary. Sums may generate collinear runs or repeated vertices. Expanded obstacles may share an edge, touch, or overlap. A visibility segment may coincide with a boundary. Narrow channels may vanish under a safety margin.

Handle them with named policies:

• normalize exact duplicates without merging merely close features;
• use robust signs for orientation, intersection, and point classification;
• preserve touching events during overlay until the union topology is resolved;
• apply clearance by expanding obstacles with an additional disk or polygon, rather than sprinkling epsilons through tests;
• return NUMERIC_INDETERMINACY if an approximate backend cannot certify a decision;
• distinguish NO_PATH from invalid input, colliding endpoints, and resource limits.

A planner may be conservative, but it should never be mysteriously conservative. When clearance removes a passage, report the clearance value and the features that close it.

Validation strategy

Begin with hand-checkable scenes: no obstacles; one rectangle; two rectangles forming a corridor; a concave L-shaped obstacle; overlapping obstacles; start or goal on a boundary; and a robot reduced to an increasingly small polygon. Include a case where the direct path is blocked but a two-corner detour is optimal.

High-value properties include:

• $q$ is inside $O\oplus(-R)$ exactly when the robot placed at $q$ intersects $O$, checked against a slow oracle;
• convex Minkowski sums are convex and their support values equal the sum of operand support values;
• union output has no exposed edge whose midpoint is strictly inside another forbidden piece;
• every visibility edge passes continuous free-space validation;
• every consecutive pair on a returned path is a graph edge or a certified shortcut;
• A* and Dijkstra return equal distances; on small graphs, both match exhaustive simple-path search;
• translating the entire workspace, start, and goal translates the route and preserves its length;
• rotating vertex-list starting positions or reversing normalized input rings does not change results;
• increasing robot size or safety clearance cannot create free space under nested-shape fixtures.

Differential-test the optimized visibility builder against the all-pairs builder. Run the entire pipeline under input permutations and fixed seeds. Store failure scenes as tiny regression files with the predicate mode and contact policy.

Benchmark what users wait for

Report stages separately so a fast search does not hide expensive preprocessing.

Use synthetic and realistic scenes: scattered obstacles, thin walls, overlapping expansions, comb-shaped concavities, and narrow passages. Record hardware, precision backend, seed, scale, and caching. For repeated queries, separate one-time map construction from per-query graph augmentation and search.

Suggested implementation milestones

Extensions after the baseline

Add a safety margin with a disk approximation. Support several fixed orientations using a layered graph with validated rotation transitions. Free rotation lifts configuration space to $(x,y,\theta)$; sampled orientations sacrifice completeness unless resolution assumptions are stated.

Other directions include dynamic obstacles and time-parameterized trajectories, kinodynamic constraints, weighted terrain costs, multi-robot prioritized planning, and roadmap methods such as probabilistic roadmaps or rapidly exploring random trees. A navigation mesh and funnel algorithm can outperform dense visibility graphs for repeated queries. Exact shortest paths remain a powerful oracle even when the production system eventually adopts an approximate planner.

Before implementation, revisit Part 1 on modeling and robust predicates, Part 2 on intersections and point classification, Part 8 on subdivision overlay, Part 21 on Minkowski sums and configuration spaces, Part 22 on visibility graphs, and Part 23 on engineering geometric algorithms. Their common lesson is the capstone's governing principle: transform geometry carefully, search a finite structure, then certify the physical motion you claim is safe.