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

Start by reading the current LatticePlanner implementation, its query and collision-checking paths, and desiderata.md; the issue gives no file paths for these entry points. Draft a design covering integer states, primitive input and costs, snapping, collision checks, .mprim support, and compatibility tests. Done means the design answers all six questions; wait for agreement before sending a PR.

Help wanted. Please comment with a design first, and do not send a PR before it is agreed.

Today

LatticePlanner plans on a lattice of poses reached by three moves from each pose: straight (SE2(1, 0, 0)), left and right (SE2(1, ±1, ±π/2)). The turn is always a quarter turn and the moves are fixed in the code; the constructor takes only the costs and the root.

• A vertex stores a float (x, y, theta) from SE2 multiplication. In practice every x and y is within about 4e-16 of an integer and every heading is a multiple of π/2, so the state space is exactly (integer x, integer y, one of four headings). The integer quarter-turn count only appears in the debug strings (icoord()).
• A duplicate is found with graph.closest(xyt) and a tolerance of 0.01, inside the loop over every candidate move. That scans every vertex, so plan() is roughly quadratic: 97, 372 and 780 vertices take about 0.00, 0.07 and 0.38 s.
• The floats drift, and some headings come out as -2.449e-16 instead of 0. That makes pgraph's build_kdtree raise (petercorke/pgraph-python#85), which is why #889 indexes PRM roadmaps but not lattices.

Idea

1. Store the state as integers (ix, iy, k), with k a heading index modulo N (N = 4 today), and derive coord exactly as theta = 2πk/N wrapped to (-π, π]. Duplicates become a dictionary lookup, so planning is linear and needs no spatial index, and the drift disappears. query() then snaps an arbitrary start or goal with an explicit rule (round x and y to the nearest cell, the heading to the nearest k), which could raise when a pose is far from the lattice. Today it silently snaps to the nearest vertex.
2. Allow finer turn steps with a table of motion primitives, one list for each start heading, each entry an end point (dx, dy, dk) in cells and heading steps, with a cost and, optionally, the intermediate poses used for collision checking. Symmetry means the primitives for one octant give the rest by integer rotation. With N = 4 and the current three moves, the table reproduces today's planner, and the default behavior must not change.

Prior art is SBPL (Likhachev), whose lattices work this way: typically 16 headings, and a node's successors turn by a small range of headings, not a full circle. Its .mprim text files hold a header (resolution_m, numberofangles, totalnumberofprimitives) and then, for each primitive, primID, startangle_c, endpose_c: dx dy dtheta, additionalactioncostmult, intermediateposes: n followed by n lines of x y theta. For example matlab/mprim/pr2.mprim has 112 primitives for 16 angles. The project itself has had no commit on its default branch since 2018, and its Python wrapper pysbpl none since 2021, so do not depend on either. The file format is simple enough to read directly, and a LatticePlanner.from_mprim(file) would give people a way to use the existing files. Check the licence of any file before copying one into RTB.

What a design should cover

1. The state representation, and how coord and the pgraph vertices are kept consistent.
2. How primitives are given: for example an argument that takes a list of lists of end-point tuples, one list per heading, and how costs and intermediate poses are supplied or generated.
3. The rule for snapping a start or goal in query(), and what happens for a pose far from the lattice.
4. How plan(iterations=) and the occupancy-grid collision check work with primitives other than the three arcs.
5. Testing: N = 4 with the default moves must give the same vertices, edges and paths as now, and the plan time should be shown.
6. Whether reading .mprim files is included, and a small example file.

Things to know

• #889 (PRM indexing) and petercorke/pgraph-python#85 are related, but neither is needed. The aim is that the lattice does not need pgraph's spatial index at all.
• The aim of stateless robots and planners, in desiderata.md, is a reason to prefer an immutable table of primitives.
• If an AI assistant wrote or helped with the change, say so in the PR, as AGENTS.md asks.
• One change per PR.