build_kdtree fails for an SE2 graph with a heading just below zero #85

With the "SE2" metric, build_kdtree() wraps each heading with np.mod(weight * theta, period) and passes the result to cKDTree(boxsize=[…, period]), which needs every value strictly below the box size. A heading of -2.4492935982947064e-16 (the rounding error of sin(2π)) wraps to 2π - 2.4e-16, which rounds to exactly 2π in floating point, and scipy raises:

ValueError: Some input data are greater than the size of the periodic box.

The metric itself accepts any heading, so any SE2 graph with a tiny negative angle can hit it, not only RTB's.

How it was found

RTB's LatticePlanner().plan(iterations=5): 209 vertices, 3 of which have the heading -2.449e-16, where an exact quarter turn would give 0. Its headings are all quarter turns to within 2.4e-16. plan(iterations=4), 97 vertices, has none, so it indexes fine.

import numpy as np
from roboticstoolbox import LatticePlanner

lat = LatticePlanner()
lat.plan(iterations=5)
lat.graph.build_kdtree()           # ValueError, see above
th = np.array([v.coord[2] for v in lat.graph])
print(np.mod(th, 2 * np.pi).max() == 2 * np.pi)   # True

Likely fix

After the wrap, map any value >= period to 0, both in build_kdtree (the data array) and in the query helper prepare, which wraps q[2] the same way. Floating point gives the same result for both sides of the wrap, so a heading at 2π - ε and one at 0 are then the same point, which is correct for a periodic axis.

A test: a 3-element SE2 graph with a vertex at heading -1e-17, built and queried, with and without a weight.

Seen from RTB, see petercorke/robotics-toolbox-python#889, where lattice indexing is deferred because of this.