From 67b280737b49857a250aeb05fdd39dd52e849a6e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 19 Oct 2026 14:40:38 +1000 Subject: [PATCH] fix(base): angvec2r rejects a zero axis for a non-zero angle angvec2r returned the identity whenever the axis norm was ~0, whatever the angle, so a non-zero rotation about no axis silently gave the identity (angvec2tr, SO3/SE3.AngleAxis and AngVec inherit this). Test the angle for zero instead, which is what the docstring already described for `tol`: a zero angle (within tol eps) returns the identity and ignores the axis; otherwise the axis is normalized with unitvec, which raises ValueError for a zero-length axis. Consistent with UnitQuaternion.AngVec in #266. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transforms3d.py | 21 +++++++++++++-------- tests/base/test_transforms3d.py | 28 ++++++++++++++++++++++++++++ tests/test_pose3d.py | 9 +++++++++ 3 files changed, 50 insertions(+), 8 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 95786b9c..e61e3dca 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -31,6 +31,7 @@ unitvec_norm, norm, isunitvec, + iszero, iszerovec, unittwist_norm, isunittwist, @@ -747,7 +748,7 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr :type: float :return: SO(3) rotation matrix :rtype: ndarray(3,3) - :raises ValueError: bad arguments + :raises ValueError: bad arguments, or a non-zero rotation about a zero-length axis ``angvec2r(θ, V)`` is an SO(3) orthonormal rotation matrix equivalent to a rotation of ``θ`` about the vector ``V``. @@ -760,8 +761,10 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr .. note:: - - If ``θ == 0`` then return identity matrix. - - If ``θ ~= 0`` then ``V`` must have a finite length. + - If ``θ`` is zero (within ``tol``) then return the identity matrix, and + ``V`` is ignored. + - Otherwise ``V`` must have a finite length, but need not be a unit + vector. A zero-length ``V`` raises ``ValueError``. :seealso: :func:`~angvec2tr` :func:`~tr2angvec` @@ -770,11 +773,11 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr if not isscalar(theta) or not isvector(v, 3): raise ValueError("Arguments must be angle and vector") - if np.linalg.norm(v) < tol * _eps: - return np.eye(3) - θ = getunit(theta, unit, vector=False) + if iszero(θ, tol): + return np.eye(3) + # Rodrigue's equation sk = skew(cast(ArrayLike3, unitvec(v))) @@ -806,8 +809,10 @@ def angvec2tr(theta: float, v: ArrayLike3, unit="rad") -> SE3Array: .. note:: - - If ``θ == 0`` then return identity matrix. - - If ``θ ~= 0`` then ``V`` must have a finite length. + - If ``θ`` is zero (within ``tol``) then return the identity matrix, and + ``V`` is ignored. + - Otherwise ``V`` must have a finite length, but need not be a unit + vector. A zero-length ``V`` raises ``ValueError``. - The translational part is zero. :seealso: :func:`~angvec2r` :func:`~tr2angvec` diff --git a/tests/base/test_transforms3d.py b/tests/base/test_transforms3d.py index e9b9af14..88ca96a1 100755 --- a/tests/base/test_transforms3d.py +++ b/tests/base/test_transforms3d.py @@ -275,6 +275,26 @@ def test_angvec2r(self): nt.assert_array_almost_equal(angvec2r(pi / 4, [0, 0, 1]), rotz(pi / 4)) nt.assert_array_almost_equal(angvec2r(-pi / 4, [0, 0, 1]), rotz(-pi / 4)) + def test_angvec2r_nonunit_axis(self): + # the axis is normalized, so its length is irrelevant + nt.assert_array_almost_equal( + angvec2r(0.5, [1, 2, 3]), angvec2r(0.5, np.array([1, 2, 3]) / np.sqrt(14)) + ) + nt.assert_array_almost_equal(angvec2r(pi / 4, [0, 0, 5]), rotz(pi / 4)) + + def test_angvec2r_zero_axis(self): + # zero rotation about a zero axis is the identity, any angle units + nt.assert_array_almost_equal(angvec2r(0, [0, 0, 0]), np.eye(3)) + nt.assert_array_almost_equal(angvec2r(0, [0, 0, 0], unit="deg"), np.eye(3)) + + # a non-zero rotation about a zero axis is meaningless + with self.assertRaises(ValueError): + angvec2r(0.5, [0, 0, 0]) + with self.assertRaises(ValueError): + angvec2r(pi, [0, 0, 0]) + with self.assertRaises(ValueError): + angvec2r(90, [0, 0, 0], unit="deg") + def test_angvec2tr(self): r2d = 180 / pi @@ -290,6 +310,14 @@ def test_angvec2tr(self): nt.assert_array_almost_equal(angvec2tr(pi / 4, [0, 0, 1]), trotz(pi / 4)) nt.assert_array_almost_equal(angvec2tr(-pi / 4, [0, 0, 1]), trotz(-pi / 4)) + # non-unit axis is normalized + nt.assert_array_almost_equal(angvec2tr(pi / 4, [0, 0, 5]), trotz(pi / 4)) + + # zero rotation about a zero axis is the identity, otherwise an error + nt.assert_array_almost_equal(angvec2tr(0, [0, 0, 0]), np.eye(4)) + with self.assertRaises(ValueError): + angvec2tr(0.5, [0, 0, 0]) + r2d = 180 / pi nt.assert_array_almost_equal(angvec2r(0, [1, 0, 0]), rotx(0)) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 589c0ec1..d4b16668 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -241,6 +241,15 @@ def test_constructor_TwoVec(self): # x axis should equal normalized x vector nt.assert_almost_equal(R.R[:, 0], v3 / np.linalg.norm(v3), 5) + def test_AngleAxis_zero_axis(self): + nt.assert_array_almost_equal(SO3.AngleAxis(0, [0, 0, 0]).A, np.eye(3)) + nt.assert_array_almost_equal(SO3.AngVec(0, [0, 0, 0]).A, np.eye(3)) + nt.assert_array_almost_equal(SE3.AngleAxis(0, [0, 0, 0]).A, np.eye(4)) + with self.assertRaises(ValueError): + SO3.AngleAxis(0.5, [0, 0, 0]) + with self.assertRaises(ValueError): + SE3.AngVec(0.5, [0, 0, 0]) + def test_conversion(self): R = SO3.AngleAxis(0.7, [1, 2, 3]) q = UnitQuaternion([11, 7, 3, -6])