Merge pull request #58 from petercorke/feat/se2-metric

feat: SE2Metric, a weight on the angle of the SE2 metric

Author: petercorke

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
+
 

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.
 

src/pgraph/PGraph.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import math
 import os
 import sys
 import warnings
@@ -31,23 +32,83 @@ 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,
 #: so that a graph using one can be pickled.
 _METRICS: dict[str, Callable[[NDArray], float]] = {
     "L1": _L1,
     "L2": _L2,
-    "SE2": _SE2,
+    "SE2": SE2Metric(),
 }
 
 
@@ -796,7 +857,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.
@@ -1556,7 +1618,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`
 
@@ -1567,6 +1632,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)
 
@@ -1602,7 +1677,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`.
 
@@ -1613,6 +1691,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)
 

src/pgraph/__init__.py

@@ -7,6 +7,7 @@
     UGraph,
     DGraph,
     Edge,
+    SE2Metric,
 )
 
 __all__ = [
@@ -15,6 +16,7 @@
     "UGraph",
     "DGraph",
     "Edge",
+    "SE2Metric",
 ]
 
 

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()