Update more tolerance usages, add a few docstring and unit test (#106)

Author: jcao-bdai

spatialmath/base/quaternions.py

@@ -761,7 +761,7 @@ def r2q(
 
 
 def qslerp(
-    q0: ArrayLike4, q1: ArrayLike4, s: float, shortest: Optional[bool] = False
+    q0: ArrayLike4, q1: ArrayLike4, s: float, shortest: Optional[bool] = False, tol: float = 10
 ) -> UnitQuaternionArray:
     """
     Quaternion conjugate
@@ -774,6 +774,8 @@ def qslerp(
     :type s: float
     :arg shortest: choose shortest distance [default False]
     :type shortest: bool
+    :param tol: Tolerance when checking for identical quaternions, in multiples of eps, defaults to 10
+    :type tol: float, optional
     :return: interpolated unit-quaternion
     :rtype: ndarray(4)
     :raises ValueError: s is outside interval [0, 1]
@@ -822,7 +824,7 @@ def qslerp(
 
     dotprod = np.clip(dotprod, -1, 1)  # Clip within domain of acos()
     theta = math.acos(dotprod)  # theta is the angle between rotation vectors
-    if abs(theta) > 10 * _eps:
+    if abs(theta) > tol * _eps:
         s0 = math.sin((1 - s) * theta)
         s1 = math.sin(s * theta)
         return ((q0 * s0) + (q1 * s1)) / math.sin(theta)

spatialmath/base/transforms2d.py

@@ -266,15 +266,15 @@ def tr2pos2(T):
     :return: translation elements of SE(2) matrix
     :rtype: ndarray(2)
 
-    - ``t = transl2(T)`` is the translational part of the SE(3) matrix ``T`` as a
+    - ``t = tr2pos2(T)`` is the translational part of the SE(3) matrix ``T`` as a
       2-element NumPy array.
 
     .. runblock:: pycon
 
         >>> from spatialmath.base import *
         >>> import numpy as np
         >>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
-        >>> transl2(T)
+        >>> tr2pos2(T)
 
     :seealso: :func:`pos2tr2` :func:`transl2`
     """
@@ -292,19 +292,19 @@ def pos2tr2(x, y=None):
     :return: SE(2) matrix
     :rtype: ndarray(3,3)
 
-    - ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3)
+    - ``T = pos2tr2([X, Y])`` is an SE(2) homogeneous transform (3x3)
       representing a pure translation.
-    - ``T = transl2( V )`` as above but the translation is given by a 2-element
+    - ``T = pos2tr2( V )`` as above but the translation is given by a 2-element
       list, dict, or a numpy array, row or column vector.
 
 
     .. runblock:: pycon
 
         >>> from spatialmath.base import *
         >>> import numpy as np
-        >>> transl2(3, 4)
-        >>> transl2([3, 4])
-        >>> transl2(np.array([3, 4]))
+        >>> pos2tr2(3, 4)
+        >>> pos2tr2([3, 4])
+        >>> pos2tr2(np.array([3, 4]))
 
     :seealso: :func:`tr2pos2` :func:`transl2`
     """
@@ -1016,8 +1016,22 @@ def trprint2(
     return s
 
 
-def _vec2s(fmt: str, v: ArrayLikePure):
-    v = [x if np.abs(x) > 100 * _eps else 0.0 for x in v]
+def _vec2s(fmt: str, v: ArrayLikePure, tol: float = 100) -> str:
+    """
+    Return a string representation for vector using the provided fmt.
+
+    :param fmt: format string for each value in v
+    :type fmt: str
+    :param tol: Tolerance when checking for near-zero values, in multiples of eps, defaults to 100
+    :type tol: float, optional
+    :return: string representation for the vector
+    :rtype: str
+
+    Return a string representation for vector using the provided fmt, where
+    near-zero values are rounded to 0.
+    """
+
+    v = [x if np.abs(x) > tol * _eps else 0.0 for x in v]
     return ", ".join([fmt.format(x) for x in v])
 
 

spatialmath/base/transforms3d.py

@@ -713,7 +713,7 @@ def eul2tr(
 # ---------------------------------------------------------------------------------------#
 
 
-def angvec2r(theta: float, v: ArrayLike3, unit="rad") -> SO3Array:
+def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 10) -> SO3Array:
     """
     Create an SO(3) rotation matrix from rotation angle and axis
 
@@ -723,6 +723,8 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad") -> SO3Array:
     :type unit: str
     :param v: 3D rotation axis
     :type v: array_like(3)
+    :param tol: Tolerance in units of eps for zero-rotation case, defaults to 10
+    :type: float
     :return: SO(3) rotation matrix
     :rtype: ndarray(3,3)
     :raises ValueError: bad arguments
@@ -748,7 +750,7 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad") -> SO3Array:
     if not isscalar(theta) or not isvector(v, 3):
         raise ValueError("Arguments must be angle and vector")
 
-    if np.linalg.norm(v) < 10 * _eps:
+    if np.linalg.norm(v) < tol * _eps:
         return np.eye(3)
 
     θ = getunit(theta, unit)
@@ -1044,6 +1046,7 @@ def tr2eul(
     unit: str = "rad",
     flip: bool = False,
     check: bool = False,
+    tol: float = 10,
 ) -> R3:
     r"""
     Convert SO(3) or SE(3) to ZYX Euler angles
@@ -1056,6 +1059,8 @@ def tr2eul(
     :type flip: bool
     :param check: check that rotation matrix is valid
     :type check: bool
+    :param tol: Tolerance in units of eps for near-zero checks, defaults to 10
+    :type: float
     :return: ZYZ Euler angles
     :rtype: ndarray(3)
 
@@ -1090,11 +1095,11 @@ def tr2eul(
         R = t2r(T)
     else:
         R = T
-    if not isrot(R, check=check):
+    if not isrot(R, check=check, tol=tol):
         raise ValueError("argument is not SO(3)")
 
     eul = np.zeros((3,))
-    if abs(R[0, 2]) < 10 * _eps and abs(R[1, 2]) < 10 * _eps:
+    if abs(R[0, 2]) < tol * _eps and abs(R[1, 2]) < tol * _eps:
         eul[0] = 0
         sp = 0
         cp = 1
@@ -1124,6 +1129,7 @@ def tr2rpy(
     unit: str = "rad",
     order: str = "zyx",
     check: bool = False,
+    tol: float = 10,
 ) -> R3:
     r"""
     Convert SO(3) or SE(3) to roll-pitch-yaw angles
@@ -1136,6 +1142,8 @@ def tr2rpy(
     :type order: str
     :param check: check that rotation matrix is valid
     :type check: bool
+    :param tol: Tolerance in units of eps, defaults to 10
+    :type: float
     :return: Roll-pitch-yaw angles
     :rtype: ndarray(3)
     :raises ValueError: bad arguments
@@ -1176,13 +1184,13 @@ def tr2rpy(
         R = t2r(T)
     else:
         R = T
-    if not isrot(R, check=check):
+    if not isrot(R, check=check, tol=tol):
         raise ValueError("not a valid SO(3) matrix")
 
     rpy = np.zeros((3,))
     if order in ("xyz", "arm"):
         # XYZ order
-        if abs(abs(R[0, 2]) - 1) < 10 * _eps:  # when |R13| == 1
+        if abs(abs(R[0, 2]) - 1) < tol * _eps:  # when |R13| == 1
             # singularity
             rpy[0] = 0  # roll is zero
             if R[0, 2] > 0:
@@ -1206,7 +1214,7 @@ def tr2rpy(
 
     elif order in ("zyx", "vehicle"):
         # old ZYX order (as per Paul book)
-        if abs(abs(R[2, 0]) - 1) < 10 * _eps:  # when |R31| == 1
+        if abs(abs(R[2, 0]) - 1) < tol * _eps:  # when |R31| == 1
             # singularity
             rpy[0] = 0  # roll is zero
             if R[2, 0] < 0:
@@ -1229,7 +1237,7 @@ def tr2rpy(
                 rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[0]) / R[2, 2])
 
     elif order in ("yxz", "camera"):
-        if abs(abs(R[1, 2]) - 1) < 10 * _eps:  # when |R23| == 1
+        if abs(abs(R[1, 2]) - 1) < tol * _eps:  # when |R23| == 1
             # singularity
             rpy[0] = 0
             if R[1, 2] < 0:

spatialmath/base/vectors.py

@@ -392,7 +392,7 @@ def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]:
     return S / th
 
 
-def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float]:
+def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[Union[R6, None], Union[float, None]]:
     """
     Convert twist to unit twist and norm
 
@@ -424,7 +424,7 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float
     S = getvector(S, 6)
 
     if iszerovec(S, tol=tol):
-        raise ValueError("zero norm")
+        return (None, None)  # according to "note" in docstring.
 
     v = S[0:3]
     w = S[3:6]
@@ -437,7 +437,7 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float
     return (S / th, th)
 
 
-def unittwist2(S: ArrayLike3, tol: float = 10) -> R3:
+def unittwist2(S: ArrayLike3, tol: float = 10) -> Union[R3, None]:
     """
     Convert twist to unit twist
 
@@ -459,9 +459,14 @@ def unittwist2(S: ArrayLike3, tol: float = 10) -> R3:
         >>> unittwist2([2, 4, 2)
         >>> unittwist2([2, 0, 0])
 
+    .. note:: Returns None if the twist has zero magnitude
     """
 
     S = getvector(S, 3)
+
+    if iszerovec(S, tol=tol):
+        return None
+
     v = S[0:2]
     w = S[2]
 
@@ -473,7 +478,7 @@ def unittwist2(S: ArrayLike3, tol: float = 10) -> R3:
     return S / th
 
 
-def unittwist2_norm(S: ArrayLike3, tol: float = 10) -> Tuple[R3, float]:
+def unittwist2_norm(S: ArrayLike3, tol: float = 10) -> Tuple[Union[R3, None], Union[float, None]]:
     """
     Convert twist to unit twist
 
@@ -495,9 +500,14 @@ def unittwist2_norm(S: ArrayLike3, tol: float = 10) -> Tuple[R3, float]:
         >>> unittwist2([2, 4, 2)
         >>> unittwist2([2, 0, 0])
 
+    .. note:: Returns (None, None) if the twist has zero magnitude
     """
 
     S = getvector(S, 3)
+
+    if iszerovec(S, tol=tol):
+        return (None, None)
+
     v = S[0:2]
     w = S[2]
 
@@ -728,7 +738,7 @@ def angle_wrap(theta: ArrayLike, mode: str = "-pi:pi") -> Union[float, NDArray]:
         return wrap_mpi_pi(theta)
     elif mode == "0:pi":
         return wrap_0_pi(theta)
-    elif mode == "0:pi":
+    elif mode == "-pi/2:pi/2":
         return wrap_mpi2_pi2(theta)
     else:
         raise ValueError("bad method specified")

spatialmath/geom3d.py

@@ -712,7 +712,7 @@ def isintersecting(
 
         :seealso: :meth:`__xor__` :meth:`intersects` :meth:`isparallel`
         """
-        return not l1.isparallel(l2, tol=tol) and bool(abs(l1 * l2) < 10 * _eps)
+        return not l1.isparallel(l2, tol=tol) and bool(abs(l1 * l2) < tol * _eps)
 
     def __eq__(l1, l2: Line3) -> bool:  # type: ignore pylint: disable=no-self-argument
         """

spatialmath/quaternion.py

@@ -396,10 +396,12 @@ def log(self) -> Quaternion:
         v = math.acos(self.s / norm) * smb.unitvec(self.v)
         return Quaternion(s=s, v=v)
 
-    def exp(self) -> Quaternion:
+    def exp(self, tol: float=100) -> Quaternion:
         r"""
         Exponential of quaternion
 
+        :param tol: Tolerance when checking for pure quaternion, in multiples of eps, defaults to 100
+        :type tol: float, optional
         :rtype: Quaternion instance
 
         ``q.exp()`` is the exponential of the quaternion ``q``, ie.
@@ -433,7 +435,7 @@ def exp(self) -> Quaternion:
         norm_v = smb.norm(self.v)
         s = exp_s * math.cos(norm_v)
         v = exp_s * self.v / norm_v * math.sin(norm_v)
-        if abs(self.s) < 100 * _eps:
+        if abs(self.s) < tol * _eps:
             # result will be a unit quaternion
             return UnitQuaternion(s=s, v=v)
         else:

tests/base/test_transforms2d.py

@@ -118,6 +118,30 @@ def test_transl2(self):
             transl2([1, 2]), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]])
         )
 
+    def test_pos2tr2(self):
+        nt.assert_array_almost_equal(
+            pos2tr2(1, 2), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]])
+        )
+        nt.assert_array_almost_equal(
+            transl2([1, 2]), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]])
+        )
+        nt.assert_array_almost_equal(
+            tr2pos2(pos2tr2(1, 2)), np.array([1, 2])
+        )
+
+    def test_tr2jac2(self):
+        T = trot2(0.3, t=[4, 5])
+        jac2 = tr2jac2(T)
+        nt.assert_array_almost_equal(
+            jac2[:2, :2], smb.t2r(T)
+        )
+        nt.assert_array_almost_equal(
+            jac2[:3, 2], np.array([0, 0, 1])
+        )
+        nt.assert_array_almost_equal(
+            jac2[2, :3], np.array([0, 0, 1])
+        )
+
     def test_xyt2tr(self):
         T = xyt2tr([1, 2, 0])
         nt.assert_array_almost_equal(T, transl2(1, 2))