From 85575256043400baca82e6bc3904604af017a124 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 19 Oct 2026 14:53:34 +1000 Subject: [PATCH] fix(robot): the line of sight of vision_collision_damper has the right orientation for parallel and antiparallel vectors The orientation was computed from arccos and a cross product, which for parallel or antiparallel vectors is a rotation about a zero axis. That is the identity today, wrongly for antiparallel vectors, and a ValueError once SpatialMath rejects a zero axis. Use SO3.RotatedVector, which is in the released SpatialMath, and test the helper. Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/robot/Robot.py | 31 +++++--- tests/test_vision_collision_orientation.py | 84 ++++++++++++++++++++++ 2 files changed, 105 insertions(+), 10 deletions(-) create mode 100644 tests/test_vision_collision_orientation.py diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 7fc2f039c..aff082e7f 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/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, ) diff --git a/tests/test_vision_collision_orientation.py b/tests/test_vision_collision_orientation.py new file mode 100644 index 000000000..70fc04221 --- /dev/null +++ b/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)