angvec2r: reject a zero axis for a non-zero angle

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`

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

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