fix(robot): the line of sight of vision_collision_damper is oriented correctly for parallel and antiparallel vectors

src/roboticstoolbox/robot/Robot.py

@@ -20,6 +20,7 @@
 from spatialmath import (
     SE2,
     SE3,
+    SO3,
     SpatialAcceleration,
     SpatialForce,
     SpatialInertia,
@@ -51,6 +52,25 @@
 _KEYWORDS_DEFAULT: list[str] = []
 
 
+def _rotation_between_vectors(a: ArrayLike, b: ArrayLike) -> SE3:
+    """
+    Rotation that takes the direction of one vector to the direction of another
+
+    :param a: the vector to rotate, which need not be a unit vector
+    :param b: the vector to rotate it to, which need not be a unit vector
+    :returns: a pure rotation, as an SE3
+
+    The vectors can be parallel, in which case the result is the identity, or
+    antiparallel, in which case it is a half turn about an axis perpendicular to
+    both.  Computing the angle with ``arccos`` and the axis with a cross product
+    fails for both: the axis is the zero vector, which is rejected by recent
+    SpatialMath, and for a dot product of 1.0000000000000002 the angle is NaN.
+    """
+    return SE3(
+        SO3.RotatedVector(np.asarray(a, dtype=float), np.asarray(b, dtype=float))
+    )
+
+
 # ==================================================================================== #
 # ================= Robot Class ====================================================== #
 # ==================================================================================== #
@@ -1644,15 +1664,6 @@ def vision_collision_damper(
         Ain = None
         bin = None
 
-        def rotation_between_vectors(a: Any, b: Any) -> Any:
-            a = a / np.linalg.norm(a)
-            b = b / np.linalg.norm(b)
-
-            angle = np.arccos(np.dot(a, b))
-            axis = np.cross(a, b)
-
-            return SE3.AngleAxis(angle, axis)
-
         if isinstance(camera, rtb.BaseRobot):
             wTcp = cast(NDArray, camera.fkine(camera.q).A)[:3, 3]
         elif isinstance(camera, SE3):
@@ -1664,7 +1675,7 @@ def rotation_between_vectors(a: Any, b: Any) -> Any:
 
         # Create line of sight object
         los_mid = SE3((wTcp + wTtp) / 2)
-        los_orientation = rotation_between_vectors(
+        los_orientation = _rotation_between_vectors(
             np.array([0.0, 0.0, 1.0]),
             wTcp - wTtp,
         )

tests/test_vision_collision_orientation.py

@@ -0,0 +1,84 @@
+"""The orientation of the line of sight of Robot.vision_collision_damper."""
+
+import numpy as np
+import numpy.testing as nt
+import pytest
+
+from roboticstoolbox.robot.Robot import _rotation_between_vectors
+
+Z = np.array([0.0, 0.0, 1.0])
+
+
+def check(a, b):
+    """The rotation is proper and takes the direction of a to the direction of b."""
+    a, b = np.asarray(a, dtype=float), np.asarray(b, dtype=float)
+    T = _rotation_between_vectors(a, b)
+    R = T.R
+
+    nt.assert_allclose(R.T @ R, np.eye(3), atol=1e-12)
+    assert np.linalg.det(R) == pytest.approx(1.0)
+    nt.assert_allclose(T.t, np.zeros(3), atol=1e-12)  # a pure rotation
+    nt.assert_allclose(R @ (a / np.linalg.norm(a)), b / np.linalg.norm(b), atol=1e-9)
+    return R
+
+
+@pytest.mark.parametrize("scale", [1.0, 2.0, 1e-3, 7.5])
+def test_parallel_is_the_identity(scale):
+    R = check(Z, scale * Z)
+    nt.assert_allclose(R, np.eye(3), atol=1e-12)
+
+
+@pytest.mark.parametrize("v", [[0.3, 0.4, 0.5], [1.0, -2.0, 3.0], [1e-3, 2e-3, 3e-3]])
+def test_parallel_up_to_rounding(v):
+    # a is twice b, but their normalized cross product need not be exactly zero
+    v = np.array(v)
+    R = check(2 * v, v)
+    nt.assert_allclose(R, np.eye(3), atol=1e-9)
+
+
+@pytest.mark.parametrize(
+    "b", [[0.0, 0.0, -1.0], [0.0, 0.0, -3.0], [0.0, 0.0, -1e-3]]
+)
+def test_antiparallel_is_a_half_turn(b):
+    R = check(Z, b)
+    # a half turn: it is its own inverse, and has trace -1
+    nt.assert_allclose(R @ R, np.eye(3), atol=1e-12)
+    assert np.trace(R) == pytest.approx(-1.0)
+
+
+@pytest.mark.parametrize(
+    "a", [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.3, 0.4, 0.5], [-1.0, 2.0, -3.0]]
+)
+def test_antiparallel_in_any_direction(a):
+    a = np.array(a)
+    R = check(a, -2.5 * a)
+    nt.assert_allclose(R @ R, np.eye(3), atol=1e-12)
+
+
+@pytest.mark.parametrize("b", [[1.0, 2.0, 3.0], [1.0, 0.0, 0.0], [-1.0, -1.0, 0.1]])
+def test_generic(b):
+    check(Z, b)
+
+
+def test_random_pairs():
+    rng = np.random.default_rng(0)
+    for _ in range(200):
+        check(rng.normal(size=3), rng.normal(size=3))
+
+
+def test_a_camera_directly_above_and_below_the_target():
+    # the line of sight is parallel, then antiparallel, to the cylinder's axis
+    target = np.array([0.5, 0.0, 0.2])
+    for camera in (np.array([0.5, 0.0, 1.0]), np.array([0.5, 0.0, -0.6])):
+        check(Z, camera - target)
+
+
+def test_no_warnings():
+    # arccos(1.0000000000000002) is NaN with a RuntimeWarning
+    import warnings
+
+    a = np.array([0.1, 0.2, 0.3])
+    with warnings.catch_warnings():
+        warnings.simplefilter("error")
+        check(a, a * (1 + 3e-16))
+        check(a, a)