LatticePlanner: integer headings, and motion primitives with finer turn steps (design first) #895

Activity

1. petercorke

   added

   enhancement

   help wantedExtra attention is neededExtra attention is needed

   on Oct 23, 2026
2. kVinsom commented on Oct 23, 2026

   

   kVinsom

   on Oct 23, 2026

   Contributor

   More actions

   I'd like to work on this issue. I'll review the current LatticePlanner and post a design proposal here first, then wait for agreement before starting the implementation or opening a PR.
3. kVinsom commented on Oct 25, 2026

   

   kVinsom

   on Oct 25, 2026

   Contributor

   More actions

   Thanks for setting out the requirements. I've reviewed LatticePlanner against current main (9f282d8f) and propose the design below. I'll wait for agreement before implementing it or opening a PR.

   I suggest separating the integer-state change from the new motion-primitive API, so each PR has one reviewable purpose.

   1. Integer states and graph consistency

   Use an immutable (ix, iy, k) key and a planner-owned dictionary from keys to LatticeVertex objects. Each plan() creates a new graph and dictionary. Duplicate detection uses exact lookup; no spatial index is built.

   Keep the integer grid in a frame anchored at root, preserving translated/rotated roots. Snapshot that transform for each plan. Root has state (0, 0, 0); with resolution r, derive every world pose independently as SE2(root) * SE2(r*ix, r*iy, 2*pi*k/N). This avoids accumulated transform drift, without claiming exact floating-point world coordinates for arbitrary rotations. Wrap world headings to (-pi, pi].

   State is authoritative. For planner-created vertices, coord and pose derive from the same state/frame. Reject coordinate/pose/state reassignment and return detached data, including a detached SE2, so in-place edits cannot corrupt lookup. Generic pgraph vertices remain unchanged. Direct structural mutation of the planner-owned graph would be documented as unsupported; plan() replaces the graph and state map together. This restriction needs agreement because graph is publicly accessible today.

   Preserve the S/L/R expansion order, first-discovery names, edge order and costs. Expected duplicate-lookup work becomes O(E) with a fixed primitive table; this is not a claim that collision checks or A* are constant-time.

   2. Query behaviour

   One compatibility detail: current main already raises ValueError when the nearest start or goal is more than 0.001 away. test_start_not_in_lattice explicitly expects (0.5, 0, 0) to be rejected.

   For the initial change, transform the requested pose into the root frame, round position and heading to a candidate integer key, require that vertex to exist, then retain the existing SE2-distance acceptance threshold. Use deterministic half-to-even rounding and modular heading arithmetic. A missing candidate must not silently select a different reachable vertex. Exact halfway cases are rejected by the default threshold. Existing NoPathError behaviour remains.

   For custom resolutions/headings, document that quantisation selects a grid state, not the nearest reachable vertex. Broader snapping should be an explicit later option with separate position/angle limits, not a default change hidden in this optimisation.

   3. Motion primitives, as a follow-up PR

   Proposed keyword-only API: resolution=1.0, primitives=None; the number of headings is the length of a supplied table. None selects four headings and the existing three moves/costs. At non-unit resolution, scale geometric default costs by resolution; explicit costs= values remain caller-supplied edge weights. An explicit table supplies its own costs; combining it with costs= is rejected as ambiguous.

   Use a frozen primitive record containing an identifier, integer (dx, dy, dk), finite positive cost, and optional immutable intermediate (x, y, theta) poses. Group by starting heading and normalise input into tuples. Validate positive finite resolution, integer endpoints, identifiers unique within each heading, and finite samples. Successors are (ix+dx, iy+dy, (k+dk) % N). Displacements are already oriented for their start heading and expressed in lattice axes. Intermediate positions use physical units relative to the source cell; headings are relative to the lattice frame, not the source heading. Validate start/end samples with a documented tolerance and wrapped angles. Plotting and collision checking use the same geometry.

   Do not obtain arbitrary headings by rotating integer endpoints through 22.5 or 45 degrees and rounding: that distorts the geometry. Supply per-heading primitives; exact quarter-turn rotations preserve the square grid. Other symmetries, including reflection, should be explicit and used only when the motion model permits them.

   Different primitives can reach the same state along different paths. Collision-check each candidate before retaining the cheapest feasible edge for a source/target pair, breaking equal-cost ties by table order. Store that primitive's identifier and geometry on the edge so returned segments and plotting agree with the chosen path. This avoids depending on parallel-edge behaviour across pgraph versions.

   Custom primitive costs need a compatible A* heuristic. Unless a lower bound is established, set the graph's heuristic to zero. Keep the existing heuristic in the compatibility-only change; do not silently change default route selection as part of duplicate lookup.

   4. Expansion, collisions and plotting

   Keep iterations as the number of frontier-expansion rounds. Each new state enters a frontier once. Without an iteration limit, require the existing bounded occupancy grid and stop when the frontier is empty.

   The initial integer-state PR preserves today's endpoint-only collision check and root handling. For custom primitives with an occupancy grid, require an explicit path and check every cell intersected by its piecewise-linear segments, including out-of-bounds cells. Checking sample points alone can miss obstacles. Use the occupancy grid's world-coordinate mapping, which may have a different resolution from the lattice, and define/test conservative cell-boundary handling. A missing path must not silently fall back to endpoint checking. This checks a point robot along the supplied polyline, not an exact swept curved path or a vehicle footprint.

   For default primitives retain the existing arcs for plotting. The generalised version should keep geometry with the immutable primitive table rather than the module-global arcs dictionary. Path highlighting can resolve vertices by state keys as well.

   5. Tests and measurements

   Compare the default planner with the current implementation: vertex keys/names, directed edges, costs, paths and move sequences, including translated/rotated roots, replanning, occupancy boundaries, heading wrap, near/far inputs, missing states and disconnected paths. Existing growth checks include (4, 3), (13, 12) and (97, 114) vertices/edges at 1, 2 and 4 iterations. Test that planning/query/path highlighting do not call closest() or build an index, and that coordinate/pose mutation cannot corrupt state lookup. Preserve discovery order and characterise equal-cost path ties; report any route differences rather than treating equal cost alone as compatibility.

   Report before/after median planning times, vertex/edge counts and Python/dependency versions over repeated runs at several depths, with plotting disabled. Keep timing assertions out of CI. The primitive follow-up adds finer heading tables, invalid inputs, same-endpoint alternative paths, obstacles between clear endpoints, and path-cost comparison against zero-heuristic search. I reran the existing local lattice tests: 9 tests and 3 subtests passed. This is baseline validation, not evidence of an implemented speed-up.

   6. .mprim support

   Defer the reader to a separate PR after the primitive API is agreed. Parse the uniform-angle format directly without an SBPL/pysbpl dependency; reject unsupported variants clearly. SBPL's endpoint heading is an absolute target index, so convert it to a delta relative to startangle_c. Validate counts, headings and sample endpoints. Its action-cost multiplier is not a complete edge cost: SBPL combines it with motion/time parameters. I propose requiring an explicit cost-conversion callable for import rather than claiming equivalent SBPL costs from the file alone.

   Include a small original fixture: resolution 1, four headings, one unit straight primitive per heading, two samples per primitive and multiplier 1. This tests parsing and heading conversion without copying third-party data; a finer-heading fixture would separately test integration.

   Reference: SBPL reader and endpoint validation.

   Would this staged approach fit your expectations: integer-state compatibility first, then custom primitives/collision semantics, then .mprim import? In particular, I'd appreciate agreement on the root-relative frame and graph-mutation restriction, the primitive/path contract, and deferring the reader until the table API is settled.

   Prepared with AI assistance; this is a design proposal, not a claim of implemented or benchmarked changes.