From 84ba2550ab3d70a85f79cb403237b93ba5c6addc Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 07:01:34 +1000 Subject: [PATCH] feat: SE2Metric, a weight on the angle of the SE2 metric The built-in "SE2" metric added radians to metres with equal weight, and had no way to change it. SE2Metric(weight) is the same norm with the wrapped angle multiplied by a weight, the length that a radian is worth. "SE2" is SE2Metric() so existing results do not change. An instance is a metric or a heuristic, is pickled with its weight, and compares equal by weight. Unlike the function it replaces it does not write into its argument. Co-Authored-By: Claude Sonnet 5.5 --- docs/source/construction.rst | 30 ++++++++ docs/source/intro.rst | 2 +- src/pgraph/PGraph.py | 108 +++++++++++++++++++++++---- src/pgraph/__init__.py | 2 + tests/test_se2_metric.py | 139 +++++++++++++++++++++++++++++++++++ 5 files changed, 267 insertions(+), 14 deletions(-) create mode 100644 tests/test_se2_metric.py diff --git a/docs/source/construction.rst b/docs/source/construction.rst index 05178545..334f73cd 100644 --- a/docs/source/construction.rst +++ b/docs/source/construction.rst @@ -43,4 +43,34 @@ Copying a vertex .. automethod:: pgraph.PGraph.Vertex.copy +Distance metrics +---------------- + +The cost of an edge, the nearest vertex to a point and the heuristic of the A* +planner all come from a distance metric, a function of the difference between +two coordinates. It is chosen by name, ``"L2"`` (the default, Euclidean), +``"L1"`` (Manhattan) or ``"SE2"``, or you can supply your own function; see +:attr:`~pgraph.PGraph.Graph.metric` and :attr:`~pgraph.PGraph.Graph.heuristic`. + +The ``"SE2"`` metric is for poses :math:`(x, y, \theta)` and adds a position +in metres to an angle in radians, which are not the same kind of quantity. It +counts a radian as a metre. If a turn should count for more or less, use +:class:`~pgraph.PGraph.SE2Metric` with a weight, which is the length that a +radian is worth. Below, one vertex is 2 m ahead and facing the same way, and +another is at our position but turned through 1.5 radians. With a weight of 1 the +turn costs 1.5, so the turned vertex is the nearer; with a weight of 2 it costs 3, +and the vertex ahead is nearer + +.. runblock:: pycon + + >>> from pgraph import UGraph, SE2Metric + >>> for weight in (1.0, 2.0): + ... g = UGraph(dim=3, metric=SE2Metric(weight=weight)) + ... v = g.add_vertex([2, 0, 0], name="ahead") + ... v = g.add_vertex([0, 0, 1.5], name="turned") + ... vertex, distance = g.closest([0, 0, 0]) + ... print(weight, vertex.name, distance) + +.. autoclass:: pgraph.PGraph.SE2Metric + diff --git a/docs/source/intro.rst b/docs/source/intro.rst index f8171784..4b9aec8e 100644 --- a/docs/source/intro.rst +++ b/docs/source/intro.rst @@ -7,7 +7,7 @@ parallel edges are permitted. Vertices can optionally be *embedded*: given a coordinate in a space of any dimension. Edge costs are then computed from the vertex positions using a -distance metric (Euclidean, Manhattan, SE(2), or your own), the vertex nearest +distance metric (Euclidean, Manhattan, SE(2) with a weight on the angle, or your own), the vertex nearest a point can be found, ``A*`` uses the metric as its heuristic, and the graph can be plotted, or exported with its layout preserved. diff --git a/src/pgraph/PGraph.py b/src/pgraph/PGraph.py index ae0e02b7..9529f60d 100644 --- a/src/pgraph/PGraph.py +++ b/src/pgraph/PGraph.py @@ -1,5 +1,6 @@ from __future__ import annotations +import math import os import sys import warnings @@ -29,15 +30,75 @@ def _L2(v: NDArray) -> float: return float(np.linalg.norm(v)) -def _SE2(v: NDArray) -> float: - """Mixed norm for (x, y, theta), the built-in ``"SE2"`` metric""" - if len(v) != 3: - raise ValueError( - f"SE2 metric requires a 3-element (x, y, theta) vector, got length {len(v)}" - ) - # wrap angle to range [-pi, pi) - v[2] = (v[2] + np.pi) % (2 * np.pi) - np.pi - return float(np.linalg.norm(v)) +class SE2Metric: + r""" + Distance between planar poses, a weighted mixed norm + + :param weight: length that one radian of turn counts for, defaults to 1 + :raises ValueError: ``weight`` is negative or not finite + + The metric of a vector :math:`(x, y, \theta)`, the difference between two + poses, is + + .. math:: + + d = \sqrt{x^2 + y^2 + (w \bar{\theta})^2} + + where :math:`\bar{\theta}` is :math:`\theta` wrapped to the interval + :math:`[-\pi, \pi)` and :math:`w` is the ``weight``. + + Distance and angle have different units, so their relative importance is + a choice. The weight is the conversion: it is how much length a turn of one + radian is worth, so a turn of :math:`\theta` costs as much as moving + :math:`w \theta`. A weight of 1 treats a radian as a metre, which is what + the string ``"SE2"`` means. A characteristic length of the vehicle, such as + its turning radius, is a natural choice. With a weight of 0 the + orientation is ignored and this is the distance in the plane. + + An instance is a metric for a graph, and can be used as its heuristic too:: + + g = UGraph(dim=3, metric=SE2Metric(weight=0.5)) + + .. runblock:: pycon + + >>> from pgraph import SE2Metric + >>> import numpy as np + >>> v = np.r_[3, 4, np.pi / 2] # 3 east, 4 north, a quarter turn + >>> SE2Metric()(v) + >>> SE2Metric(weight=0.5)(v) + >>> SE2Metric(weight=0)(v) + + A metric with a weight is still a metric, so it is a consistent heuristic + for A* when the edge costs are computed with it. If edge costs come from + somewhere else, use a weight low enough, or zero, for the heuristic never to + exceed the cost to go. + + :seealso: :meth:`Graph.metric` :meth:`Graph.heuristic` + """ + + def __init__(self, weight: float = 1.0): + weight = float(weight) + if not math.isfinite(weight) or weight < 0: + raise ValueError(f"weight must be finite and not negative, got {weight!r}") + self.weight = weight + + def __call__(self, v: NDArray) -> float: + if len(v) != 3: + raise ValueError( + f"SE2 metric requires a 3-element (x, y, theta) vector, got length {len(v)}" + ) + # wrap the angle to the range [-pi, pi), without changing the argument + theta = (float(v[2]) + math.pi) % (2 * math.pi) - math.pi + return math.sqrt(float(v[0]) ** 2 + float(v[1]) ** 2 + (self.weight * theta) ** 2) + + def __repr__(self) -> str: + return f"SE2Metric(weight={self.weight:g})" + + def __eq__(self, other: object) -> bool: + return isinstance(other, SE2Metric) and other.weight == self.weight + + def __hash__(self) -> int: + return hash(("SE2Metric", self.weight)) #: the built-in metrics by name. They are module level, and pickled by name, @@ -45,7 +106,7 @@ def _SE2(v: NDArray) -> float: _METRICS: dict[str, Callable[[NDArray], float]] = { "L1": _L1, "L2": _L2, - "SE2": _SE2, + "SE2": SE2Metric(), } @@ -792,7 +853,8 @@ def copy(self) -> Graph: .. note:: - A metric or heuristic given by name (``"L1"``, ``"L2"``, - ``"SE2"``) is pickled by name. A function you supply must be + ``"SE2"``) is pickled by name, and an :class:`SE2Metric` with + its weight. A function you supply must be importable at module level to be pickled; a ``lambda`` or a function defined inside another function can be copied but not pickled. @@ -1552,7 +1614,10 @@ def metric(self, metric: Callable[[NDArray], float] | str) -> None: is :math:`\theta` wrapped to the interval :math:`[-\pi, \pi)`. Requires every coordinate involved -- vertex ``coord`` and any point passed to :meth:`closest`/:meth:`Vertex.distance` -- to be - exactly 3 elements; raises :exc:`ValueError` otherwise. + exactly 3 elements; raises :exc:`ValueError` otherwise. Radians + and metres are weighted equally; to change that pass + :class:`SE2Metric` with a ``weight``, as in + ``g.metric = SE2Metric(weight=0.5)``. The metric is used by :meth:`closest` and :meth:`distance` @@ -1563,6 +1628,16 @@ def metric(self, metric: Callable[[NDArray], float] | str) -> None: >>> g = UGraph() >>> g.metric = 'L1' >>> print(g.metric(np.r_[3, -4])) + + A pose metric whose orientation counts for less than its position + + .. runblock:: pycon + + >>> from pgraph import UGraph, SE2Metric + >>> import numpy as np + >>> g = UGraph(dim=3) + >>> g.metric = SE2Metric(weight=0.5) + >>> print(g.metric(np.r_[3, 4, np.pi / 2])) """ self._metric = self._metricfunc(metric) @@ -1598,7 +1673,10 @@ def heuristic(self, heuristic: Callable[[NDArray], float] | str) -> None: is :math:`\theta` wrapped to the interval :math:`[-\pi, \pi)`. Requires every coordinate involved -- vertex ``coord`` and any point passed to :meth:`closest`/:meth:`Vertex.distance` -- to be - exactly 3 elements; raises :exc:`ValueError` otherwise. + exactly 3 elements; raises :exc:`ValueError` otherwise. Radians + and metres are weighted equally; to change that pass + :class:`SE2Metric` with a ``weight``, as in + ``g.metric = SE2Metric(weight=0.5)``. The heuristic distance is only used by the A* planner :meth:`path_Astar`. @@ -1609,6 +1687,10 @@ def heuristic(self, heuristic: Callable[[NDArray], float] | str) -> None: >>> g = UGraph() >>> g.heuristic = 'L2' >>> print(g.heuristic(np.r_[3, 4])) + + The heuristic of A* must not exceed the true cost to go. For poses + whose costs are the planar distance travelled, ignore the orientation + with ``SE2Metric(weight=0)``. """ self._heuristic = self._metricfunc(heuristic) diff --git a/src/pgraph/__init__.py b/src/pgraph/__init__.py index 0c3444e3..3f2ac674 100644 --- a/src/pgraph/__init__.py +++ b/src/pgraph/__init__.py @@ -7,6 +7,7 @@ UGraph, DGraph, Edge, + SE2Metric, ) __all__ = [ @@ -15,6 +16,7 @@ "UGraph", "DGraph", "Edge", + "SE2Metric", ] diff --git a/tests/test_se2_metric.py b/tests/test_se2_metric.py new file mode 100644 index 00000000..3299aa88 --- /dev/null +++ b/tests/test_se2_metric.py @@ -0,0 +1,139 @@ +""" +The ``SE2Metric`` of a pose difference (x, y, theta) and its weight on the +angle, in graphs: edge costs, ``closest``, the A* heuristic, copying and +pickling. + +Expectations are worked out by hand. +""" + +import copy +import pickle # nosec B403 - only used to round-trip objects made in this file +import unittest + +import numpy as np + +from pgraph import DGraph, SE2Metric, UGraph + + +class TestMetric(unittest.TestCase): + def test_default_weight_is_the_string_se2(self): + v = np.r_[3.0, 4.0, 1.0] + self.assertEqual(SE2Metric()(v), UGraph(metric="SE2").metric(v)) + self.assertAlmostEqual(SE2Metric()(v), np.sqrt(26)) # 9 + 16 + 1 + + def test_weight_scales_the_angle_only(self): + v = np.r_[3.0, 4.0, np.pi / 2] + self.assertAlmostEqual(SE2Metric(1)(v), np.sqrt(25 + (np.pi / 2) ** 2)) + self.assertAlmostEqual(SE2Metric(2)(v), np.sqrt(25 + np.pi**2)) + self.assertAlmostEqual(SE2Metric(0.5)(v), np.sqrt(25 + (np.pi / 4) ** 2)) + self.assertAlmostEqual(SE2Metric(0)(v), 5.0) + + def test_angle_is_wrapped_before_weighting(self): + for turns in (-2, -1, 0, 1, 2): + with self.subTest(turns=turns): + v = np.r_[0.0, 0.0, 0.5 + turns * 2 * np.pi] + self.assertAlmostEqual(SE2Metric(3)(v), 1.5) + # just past pi is a turn the other way + self.assertAlmostEqual(SE2Metric(1)(np.r_[0.0, 0.0, np.pi + 0.1]), np.pi - 0.1) + + def test_a_pure_turn_is_worth_the_weight_times_the_angle(self): + self.assertAlmostEqual(SE2Metric(0.25)(np.r_[0.0, 0.0, 2.0]), 0.5) + + def test_the_argument_is_not_changed(self): + v = np.r_[1.0, 2.0, 7.0] # needs wrapping + before = v.copy() + SE2Metric(0.5)(v) + np.testing.assert_array_equal(v, before) + + def test_a_list_is_accepted(self): + self.assertAlmostEqual(SE2Metric(2)([0, 0, 1]), 2.0) + + def test_needs_three_elements(self): + for v in ([1.0, 2.0], [1.0, 2.0, 3.0, 4.0]): + with self.subTest(length=len(v)): + with self.assertRaises(ValueError): + SE2Metric()(np.array(v)) + + def test_bad_weights(self): + for weight in (-1, np.inf, np.nan): + with self.subTest(weight=weight): + with self.assertRaises(ValueError): + SE2Metric(weight) + + def test_repr_equality_and_hash(self): + self.assertEqual(repr(SE2Metric(0.5)), "SE2Metric(weight=0.5)") + self.assertEqual(SE2Metric(0.5), SE2Metric(0.5)) + self.assertNotEqual(SE2Metric(0.5), SE2Metric(2)) + self.assertEqual(len({SE2Metric(0.5), SE2Metric(0.5), SE2Metric(2)}), 2) + self.assertEqual(SE2Metric(), UGraph(metric="SE2").metric) + + +class TestInGraphs(unittest.TestCase): + cls = UGraph + + def test_edge_cost_uses_the_weight(self): + g = self.cls(dim=3, metric=SE2Metric(weight=2)) + a = g.add_vertex([0.0, 0.0, 0.0]) + b = g.add_vertex([0.0, 0.0, 0.5]) + self.assertAlmostEqual(g.add_edge(a, b).cost, 1.0) # 2 * 0.5 + + def test_closest_depends_on_the_weight(self): + # p is 1 away in the plane and no turn; q is at the same place but + # turned by 1 rad: nearer than p only if turning is cheap enough + def nearest(weight): + g = self.cls(dim=3, metric=SE2Metric(weight=weight)) + g.add_vertex([2.0, 0.0, 0.0], name="p") # 2 away, no turn + g.add_vertex([0.0, 0.0, 1.5], name="q") # no distance, 1.5 rad + return g.closest([0.0, 0.0, 0.0]) + + v, d = nearest(1.0) # q is 1.5 away, p is 2 away + self.assertEqual((v.name, round(d, 6)), ("q", 1.5)) + v, d = nearest(2.0) # q is 3 away, p is 2 away + self.assertEqual((v.name, round(d, 6)), ("p", 2.0)) + v, d = nearest(0.0) # q is 0 away + self.assertEqual((v.name, round(d, 6)), ("q", 0.0)) + + def test_metric_can_be_set_after(self): + g = self.cls(dim=3) + g.metric = SE2Metric(weight=3) + self.assertAlmostEqual(g.metric(np.r_[0.0, 0.0, 1.0]), 3.0) + + def test_astar_with_the_weighted_metric_is_optimal(self): + # heuristic defaults to the metric, which makes the edge costs and the + # heuristic consistent; compare with the uniform cost search + rng = np.random.default_rng(5) + for weight in (0.0, 0.3, 1.0, 4.0): + with self.subTest(weight=weight): + g = self.cls(dim=3, metric=SE2Metric(weight=weight), heuristic=SE2Metric(weight=weight)) + vs = [g.add_vertex([*rng.uniform(0, 10, 2), rng.uniform(-3, 3)]) for _ in range(30)] + for i in range(30): + for j in rng.choice(30, 3, replace=False): + if i != j: + g.add_edge(vs[i], vs[int(j)]) + for s, t in [(0, 29), (3, 17), (10, 25)]: + ucs = g.path_UCS(vs[s], vs[t]) + astar = g.path_Astar(vs[s], vs[t]) + self.assertEqual(ucs is None, astar is None) + if ucs is not None: + self.assertAlmostEqual(astar[1], ucs[1]) + + def test_pickle_and_copy_keep_the_weight(self): + g = self.cls(dim=3, metric=SE2Metric(weight=0.25), heuristic=SE2Metric(weight=0)) + g.add_vertex([0.0, 0.0, 0.0]) + for h in (pickle.loads(pickle.dumps(g)), g.copy(), copy.deepcopy(g)): # nosec B301 + self.assertEqual(h.metric, SE2Metric(0.25)) + self.assertEqual(h.heuristic, SE2Metric(0)) + h = pickle.loads(pickle.dumps(self.cls(metric="SE2"))) # nosec B301 + self.assertEqual(h.metric, SE2Metric()) + + def test_the_named_metric_is_still_pickled_by_name(self): + g = self.cls(metric="SE2") + self.assertIs(pickle.loads(pickle.dumps(g)).metric, g.metric) # nosec B301 + + +class TestInDirectedGraphs(TestInGraphs): + cls = DGraph + + +if __name__ == "__main__": + unittest.main()