[DO NOT MERGE] Integration: high priority

.github/workflows/master.yml

@@ -77,6 +77,12 @@ jobs:
         file: ./coverage.xml
   sphinx:
     # If the above worked:
-    # Build docs and upload to GH Pages
+    # Build docs and upload to GH Pages -- only on push (merge to master/future),
+    # never on pull_request: a PR whose branch lives in a fork (including the
+    # author's own fork) gets a read-only GITHUB_TOKEN by GitHub's own design,
+    # so the gh-pages push step always fails there regardless of repo settings.
+    # Publishing unreviewed PR content to the live docs site isn't desired
+    # either way.
     needs: unittest
+    if: ${{ github.event_name != 'pull_request' }}
     uses: ./.github/workflows/sphinx.yml

.github/workflows/sphinx.yml

@@ -6,7 +6,6 @@ on:
 jobs:
   sphinx:
     runs-on: ubuntu-22.04
-    # if: ${{ github.event_name != 'pull_request' }}
     steps:
     - uses: actions/checkout@v4
     - name: Set up Python 3.12

pyproject.toml

@@ -37,7 +37,7 @@ dependencies = [
     "scipy",
     "matplotlib",
     "ansitable",
-    "typing_extensions",
+    "typing_extensions>=4.0.0",
     "pre-commit",
 ]
 

spatialmath/base/transforms2d.py

@@ -1312,7 +1312,11 @@ def _FindCorrespondences(
 
         num_iter += 1
 
-    return T
+    # T accumulates roundoff error through repeated composition
+    # (T = T @ new_T) over the iterations above, and can drift just
+    # outside ishom2()'s orthonormality tolerance -- restore exact
+    # orthonormality before returning.
+    return trnorm2(T)
 
 
 if _matplotlib_exists:

spatialmath/base/vectors.py

@@ -567,13 +567,10 @@ def wrap_mpi2_pi2(theta: ArrayLike) -> Union[float, NDArray]:
 
     """
     theta = getvector(theta)
-    n = theta / np.pi * 2
-    if isinstance(n, np.ndarray):
-        n = n.astype(int)
-    else:
-        n = np.fix(n).astype(int)
-
-    y = np.where(np.bitwise_and(n, 1) == 0, theta - n * np.pi, n * np.pi - theta)
+    theta = np.fmod(theta, 2 * np.pi)
+    theta = np.where(theta > np.pi, theta - 2 * np.pi, theta)
+    theta = np.where(theta < -np.pi, theta + 2 * np.pi, theta)
+    y = np.where(np.abs(theta) <= np.pi / 2, theta, np.copysign(np.pi, theta) - theta)
     if isinstance(y, np.ndarray) and len(y) == 1:
         return float(y[0])
     else:
@@ -832,7 +829,14 @@ def project(v1: ArrayLike3, v2: ArrayLike3) -> ArrayLike3:
     :return: vector projection of v1 onto v2 (parrallel to v2)
     :rtype: ndarray(n)
     """
-    return np.dot(v1, v2) * v2
+    v1 = getvector(v1)
+    v2 = getvector(v2)
+    scale = np.max(np.abs(v2))
+    if scale == 0:
+        return np.zeros_like(v2)
+    # Normalize the scale before dot products to avoid over/underflow.
+    v2 = v2 / scale
+    return (np.dot(v1, v2) / np.dot(v2, v2)) * v2
 
 
 def orthogonalize(v1: ArrayLike3, v2: ArrayLike3, normalize: bool = True) -> ArrayLike3:

spatialmath/geom3d.py

@@ -836,7 +836,7 @@ def distance(
 
         :param l2: Second line
         :type l2: ``Line3``
-        :param tol: Tolerance in multiples of eps, defaults to 20
+        :param tol: Parallel-direction tolerance in multiples of eps, defaults to 20
         :type tol: float, optional
         :return: Closest distance between lines
         :rtype: float
@@ -847,20 +847,16 @@ def distance(
 
         :seealso: :meth:`closest_to_line`
         """
-        if l1 | l2:
-            # lines are parallel
-            l = np.cross(
-                l1.w, l1.v - l2.v * np.dot(l1.w, l2.w) / dot(l2.w, l2.w)
-            ) / np.linalg.norm(l1.w)
-        else:
-            # lines are not parallel
-            if abs(l1 * l2) < tol * _eps:
-                # lines intersect at a point
-                l = 0
-            else:
-                # lines don't intersect, find closest distance
-                l = abs(l1 * l2) / np.linalg.norm(np.cross(l1.w, l2.w)) ** 2
-        return l
+        w1, w2 = l1.uw, l2.uw
+        normal = np.cross(w1, w2)
+        normal_length = np.linalg.norm(normal)
+        delta = l2.pp - l1.pp
+        if normal_length <= tol * _eps:
+            # Parallel lines: remove the component along the common direction.
+            return float(np.linalg.norm(np.cross(delta, w1)))
+        # The displacement along the common normal is the shortest distance.
+        # Unit directions make the result independent of Plucker scaling.
+        return float(abs(np.dot(delta, normal)) / normal_length)
 
     def closest_to_line(
         l1, l2: Line3  # type:ignore

spatialmath/quaternion.py

@@ -381,6 +381,9 @@ def log(self) -> Quaternion:
 
              \ln \| q \|,  \langle \frac{\vec{v}}{\| \vec{v} \|} \cos^{-1} \frac{s}{\| q \|} \rangle
 
+        The implementation uses the equivalent :math:`\operatorname{atan2}(\|\vec{v}\|, s)`
+        to avoid the loss of precision in ``acos`` for small rotation angles.
+
         For a ``UnitQuaternion`` the logarithm is a pure quaternion whose vector
         part :math:`\vec{v}` and :math:`\vec{v}/2` is a Euler vector: parallel
         to the axis of rotation and whose norm is the magnitude of rotation.
@@ -403,20 +406,17 @@ def log(self) -> Quaternion:
         norm = self.norm()
         s = np.log(norm)
         if len(self) == 1:
-            if smb.iszerovec(self._A[1:4]):
+            n = math.hypot(*self._A[1:4])
+            if n == 0:
                 v = np.zeros((3,))
             else:
-                v = math.acos(np.clip(self._A[0] / norm, -1, 1)) * smb.unitvec(
-                    self._A[1:4]
-                )
+                v = math.atan2(n, self._A[0]) * self._A[1:4] / n
             return Quaternion(s=s, v=v)
         else:
-            v = [
-                np.zeros((3,))
-                if smb.iszerovec(A[1:4])
-                else math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4])
-                for A, n in zip(self._A, norm)
-            ]
+            v = []
+            for A in self._A:
+                n = math.hypot(*A[1:4])
+                v.append(np.zeros((3,)) if n == 0 else math.atan2(n, A[0]) * A[1:4] / n)
             return Quaternion(s=s, v=np.array(v))
 
     def exp(self, tol: float = 20) -> Quaternion:

tests/base/test_transforms2d.py

@@ -174,6 +174,22 @@ def test_icp2d(self):
         T2 = ICP2d(p2, p1, T=xyt2tr([1, 2, 0.2]))
         nt.assert_almost_equal(T, T2)
 
+    def test_icp2d_returns_valid_se2(self):
+        # noisy correspondences plus a forced iteration count accumulate
+        # roundoff error in the T = T @ new_T composition -- the result
+        # must still pass ishom2(check=True), ie. be exactly orthonormal.
+        # seed fixed: this configuration reliably triggers the drift.
+        np.random.seed(0)
+        p1 = np.random.uniform(size=(2, 30))
+        T = xyt2tr([1, 2, 0.2])
+
+        p2 = homtrans(T, p1) + np.random.normal(scale=0.01, size=(2, 30))
+        k = np.random.permutation(p2.shape[1])
+        p2 = p2[:, k]
+
+        T2 = ICP2d(p2, p1, max_iter=100, min_delta_err=0)
+        self.assertTrue(ishom2(T2, check=True))
+
     def test_print2(self):
         T = transl2(1, 2) @ trot2(0.3)
 

tests/base/test_vectors.py

@@ -33,6 +33,23 @@ class TestVector(unittest.TestCase):
     def tearDownClass(cls):
         plt.close("all")
 
+    def test_project_nonunit_direction(self):
+        for scale in (1, 2, -3, 1e-200, 1e200):
+            direction = np.array([scale, scale, 0])
+            nt.assert_allclose(project([3, 1, 4], direction), [2, 2, 0])
+        nt.assert_allclose(project((3, 1, 4), (2, 2, 0)), [2, 2, 0])
+
+    def test_orthogonalize_nonunit_direction(self):
+        result = orthogonalize([3, 1, 4], [2, 2, 0], normalize=False)
+        nt.assert_allclose(result, [1, -1, 4])
+        self.assertAlmostEqual(np.dot(result, [2, 2, 0]), 0)
+        normalized = orthogonalize([3, 1, 4], [2, 2, 0])
+        self.assertAlmostEqual(np.linalg.norm(normalized), 1)
+        self.assertAlmostEqual(np.dot(normalized, [2, 2, 0]), 0)
+
+    def test_project_zero_direction(self):
+        nt.assert_array_equal(project([1, 2, 3], [0, 0, 0]), [0, 0, 0])
+
     def test_unit(self):
         nt.assert_array_almost_equal(unitvec([1, 0, 0]), np.r_[1, 0, 0])
         nt.assert_array_almost_equal(unitvec([0, 1, 0]), np.r_[0, 1, 0])
@@ -278,6 +295,30 @@ def test_angdiff(self):
         nt.assert_array_almost_equal(x, [0, 0, 0])
         self.assertIsInstance(x, np.ndarray)
 
+    def test_latitude_wrap_multiple_turns(self):
+        angles = pi * np.arange(-4, 4.5, 0.5)
+        expected = pi * np.array(
+            [0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0]
+        )
+        nt.assert_allclose(wrap_mpi2_pi2(angles), expected, atol=1e-14)
+        nt.assert_allclose(angle_wrap(angles, mode="-pi/2:pi/2"), expected, atol=1e-14)
+        for angle, folded in zip(angles, expected):
+            self.assertAlmostEqual(wrap_mpi2_pi2(angle), folded)
+
+    def test_latitude_wrap_boundaries_and_small_angles(self):
+        epsilon = 1e-12
+        angles = np.array(
+            [-pi / 2 - epsilon, -pi / 2 + epsilon, pi / 2 - epsilon, pi / 2 + epsilon]
+        )
+        expected = np.array(
+            [-pi / 2 + epsilon, -pi / 2 + epsilon, pi / 2 - epsilon, pi / 2 - epsilon]
+        )
+        nt.assert_allclose(wrap_mpi2_pi2(angles), expected, rtol=0, atol=1e-15)
+        nt.assert_allclose(
+            wrap_mpi2_pi2([1e-16, -1e-16]), [1e-16, -1e-16], rtol=0, atol=0
+        )
+        self.assertEqual(wrap_mpi2_pi2([]).size, 0)
+
     def test_wrap(self):
         self.assertAlmostEqual(wrap_0_2pi(0), 0)
         self.assertAlmostEqual(wrap_0_2pi(2 * pi), 0)

tests/test_geom3d.py

@@ -17,6 +17,36 @@
 import matplotlib.pyplot as plt
 
 
+@pytest.mark.parametrize(
+    "p1, w1, p2, w2, expected",
+    [
+        ([0, 0, 0], [1, 0, 0], [0, 3, 4], [1, 0, 0], 5),
+        ([1, 2, 3], [1, 0, 0], [4, 2, 3], [-1, 0, 0], 0),
+        ([1, 2, 3], [2, 0, 0], [1, 2, 3], [0, 5, 0], 0),
+        ([0, 0, 0], [1, 0, 0], [0, 0, 2], [0.6, 0.8, 0], 2),
+        ([1, 2, 3], [2, 0, 0], [1, 2, 5], [3, 4, 0], 2),
+    ],
+    ids=["parallel", "coincident", "intersecting", "skew-unit", "skew-scaled"],
+)
+@pytest.mark.parametrize("scale1, scale2", [(1, 1), (2, 5), (-2, 0.25), (1e-8, 1e-8)])
+def test_line_distance(p1, w1, p2, w2, expected, scale1, scale2):
+    # Scaling either set of Plucker coordinates does not change the line.
+    line1 = Line3.PointDir(p1, np.array(w1) * scale1)
+    line2 = Line3.PointDir(p2, np.array(w2) * scale2)
+    for first, second in [(line1, line2), (line2, line1)]:
+        distance = first.distance(second)
+        assert np.isscalar(distance)
+        assert distance == pytest.approx(expected, abs=1e-12)
+
+
+def test_line_distance_parallel_tolerance():
+    line1 = Line3.PointDir([0, 0, 0], [1, 0, 0])
+    line2 = Line3.PointDir([0, 1, 2], [1, 1e-8, 0])
+    assert line1.distance(line2) == pytest.approx(2)
+    # A larger angular tolerance treats the directions as parallel.
+    assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5))
+
+
 class Line3Test(unittest.TestCase):
     # Primitives
     def test_constructor1(self):

tests/test_quaternion.py

@@ -943,6 +943,60 @@ def test_log(self):
         self.assertIsInstance(qq, UnitQuaternion)
         nt.assert_array_almost_equal(qq.vec, np.r_[1, 0, 0, 0])
 
+    def test_log_small_rotation(self):
+        axis = np.array([1.0, -2.0, 3.0]) / math.sqrt(14)
+        for angle in (1e-12, -1e-9, 1e-7, 1e-4, 0.3):
+            for scale in (1.0, 3.0, 1e-6):
+                with self.subTest(angle=angle, scale=scale):
+                    values = scale * np.r_[cos(angle / 2), sin(angle / 2) * axis]
+                    q = UnitQuaternion(values) if scale == 1 else Quaternion(values)
+                    original = q.vec.copy()
+                    qlog = q.log()
+                    nt.assert_allclose(qlog.v, angle / 2 * axis, rtol=1e-13, atol=0)
+                    self.assertAlmostEqual(qlog.s, math.log(scale))
+                    nt.assert_allclose(qlog.exp().vec, q.vec, rtol=1e-13, atol=0)
+                    nt.assert_array_equal(q.vec, original)
+
+    def test_log_small_rotation_sequence(self):
+        angles = np.array([1e-12, -1e-9, 1e-7, 0.3])
+        axes = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0]])
+        values = np.column_stack(
+            (np.cos(angles / 2), np.sin(angles[:, None] / 2) * axes)
+        )
+        expected = angles[:, None] / 2 * axes
+        for scales in (
+            np.ones(4),
+            np.array([2.0, 3.0, 4.0, 5.0]),
+            1e-6 * np.ones(4),
+        ):
+            with self.subTest(scales=scales):
+                scaled = scales[:, None] * values
+                q = (
+                    UnitQuaternion(scaled)
+                    if np.all(scales == 1)
+                    else Quaternion(list(scaled))
+                )
+                original = q.vec.copy()
+                qlog = q.log()
+                self.assertEqual(len(qlog), len(q))
+                nt.assert_allclose(qlog.v, expected, rtol=1e-13, atol=0)
+                nt.assert_allclose(qlog.s, np.log(scales), atol=1e-15)
+                for i in range(len(q)):
+                    nt.assert_allclose(qlog[i].exp().vec, scaled[i], rtol=1e-13, atol=0)
+                    nt.assert_allclose(qlog[i].vec, q[i].log().vec, rtol=1e-13, atol=0)
+                nt.assert_array_equal(q.vec, original)
+
+    def test_log_near_negative_real(self):
+        # Keep the principal quaternion branch near pi, rather than folding
+        # a negative scalar part onto the small-angle branch.
+        angle = 1e-7
+        q = Quaternion([-cos(angle), sin(angle), 0, 0])
+        expected = np.r_[0, pi - angle, 0, 0]
+        nt.assert_allclose(q.log().vec, expected, rtol=0, atol=1e-15)
+        nt.assert_allclose(
+            Quaternion([q, q]).log().vec, [expected, expected], rtol=0, atol=1e-15
+        )
+
     def test_concat(self):
         u = Quaternion()
         uu = Quaternion([u, u, u, u])