Standardizing tolerance uses (#104)

Author: jcao-bdai

spatialmath/base/quaternions.py

@@ -106,12 +106,14 @@ def qnorm(q: ArrayLike4) -> float:
     return np.linalg.norm(q)
 
 
-def qunit(q: ArrayLike4, tol: Optional[float] = 10) -> UnitQuaternionArray:
+def qunit(q: ArrayLike4, tol: float = 10) -> UnitQuaternionArray:
     """
     Create a unit quaternion
 
     :arg v: quaterion
     :type v: array_like(4)
+    :param tol: Tolerance in multiples of eps, defaults to 10
+    :type tol: float, optional
     :return: a pure quaternion
     :rtype: ndarray(4)
     :raises ValueError: quaternion has (near) zero norm
@@ -144,13 +146,13 @@ def qunit(q: ArrayLike4, tol: Optional[float] = 10) -> UnitQuaternionArray:
         return -q
 
 
-def qisunit(q: ArrayLike4, tol: Optional[float] = 100) -> bool:
+def qisunit(q: ArrayLike4, tol: float = 100) -> bool:
     """
     Test if quaternion has unit length
 
     :param v: quaternion
     :type v: array_like(4)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 100
     :type tol: float
     :return: whether quaternion has unit length
     :rtype: bool
@@ -172,7 +174,7 @@ def qisunit(q: ArrayLike4, tol: Optional[float] = 100) -> bool:
 def qisequal(
     q1: ArrayLike4,
     q2: ArrayLike4,
-    tol: Optional[float] = 100,
+    tol: float = 100,
     unitq: Optional[bool] = False,
 ) -> bool:
     ...
@@ -182,13 +184,13 @@ def qisequal(
 def qisequal(
     q1: ArrayLike4,
     q2: ArrayLike4,
-    tol: Optional[float] = 100,
+    tol: float = 100,
     unitq: Optional[bool] = True,
 ) -> bool:
     ...
 
 
-def qisequal(q1, q2, tol: Optional[float] = 100, unitq: Optional[bool] = False):
+def qisequal(q1, q2, tol: float = 100, unitq: Optional[bool] = False):
     """
     Test if quaternions are equal
 
@@ -198,7 +200,7 @@ def qisequal(q1, q2, tol: Optional[float] = 100, unitq: Optional[bool] = False):
     :type q2: array_like(4)
     :param unitq: quaternions are unit quaternions
     :type unitq: bool
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 100
     :type tol: float
     :return: whether quaternions are equal
     :rtype: bool
@@ -545,7 +547,7 @@ def q2r(
 def r2q(
     R: SO3Array,
     check: Optional[bool] = False,
-    tol: Optional[float] = 100,
+    tol: float = 100,
     order: Optional[str] = "sxyz",
 ) -> UnitQuaternionArray:
     """
@@ -555,7 +557,7 @@ def r2q(
     :type R: ndarray(3,3)
     :param check: check validity of rotation matrix, default False
     :type check: bool
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 100
     :type tol: float
     :param order: the order of the returned quaternion elements. Must be 'sxyz' or
         'xyzs'. Defaults to 'sxyz'.

spatialmath/base/transforms2d.py

@@ -324,14 +324,16 @@ def pos2tr2(x, y=None):
     return T
 
 
-def ishom2(T: Any, check: bool = False) -> bool:  # TypeGuard(SE2):
+def ishom2(T: Any, check: bool = False, tol: float = 100) -> bool:  # TypeGuard(SE2):
     """
     Test if matrix belongs to SE(2)
 
     :param T: SE(2) matrix to test
     :type T: ndarray(3,3)
     :param check: check validity of rotation submatrix
     :type check: bool
+    :param tol: Tolerance in units of eps for zero-rotation case, defaults to 100
+    :type: float
     :return: whether matrix is an SE(2) homogeneous transformation matrix
     :rtype: bool
 
@@ -356,18 +358,20 @@ def ishom2(T: Any, check: bool = False) -> bool:  # TypeGuard(SE2):
     return (
         isinstance(T, np.ndarray)
         and T.shape == (3, 3)
-        and (not check or (smb.isR(T[:2, :2]) and all(T[2, :] == np.array([0, 0, 1]))))
+        and (not check or (smb.isR(T[:2, :2], tol=tol) and all(T[2, :] == np.array([0, 0, 1]))))
     )
 
 
-def isrot2(R: Any, check: bool = False) -> bool:  # TypeGuard(SO2):
+def isrot2(R: Any, check: bool = False, tol: float = 100) -> bool:  # TypeGuard(SO2):
     """
     Test if matrix belongs to SO(2)
 
     :param R: SO(2) matrix to test
     :type R: ndarray(3,3)
     :param check: check validity of rotation submatrix
     :type check: bool
+    :param tol: Tolerance in units of eps for zero-rotation case, defaults to 100
+    :type: float
     :return: whether matrix is an SO(2) rotation matrix
     :rtype: bool
 
@@ -388,7 +392,7 @@ def isrot2(R: Any, check: bool = False) -> bool:  # TypeGuard(SO2):
 
     :seealso: isR, ishom2, isrot
     """
-    return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or smb.isR(R))
+    return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or smb.isR(R, tol=tol))
 
 
 # ---------------------------------------------------------------------------------------#
@@ -514,10 +518,10 @@ def trlog2(
               :func:`~spatialmath.base.transformsNd.vexa`
     """
 
-    if ishom2(T, check=check):
+    if ishom2(T, check=check, tol=tol):
         # SE(2) matrix
 
-        if smb.iseye(T, tol):
+        if smb.iseye(T, tol=tol):
             # is identity matrix
             if twist:
                 return np.zeros((3,))
@@ -539,7 +543,7 @@ def trlog2(
                     [[smb.skew(theta), tr[:, np.newaxis]], [np.zeros((1, 3))]]
                 )
 
-    elif isrot2(T, check=check):
+    elif isrot2(T, check=check, tol=tol):
         # SO(2) rotation matrix
         theta = math.atan(T[1, 0] / T[0, 0])
         if twist:

spatialmath/base/transforms3d.py

@@ -1335,7 +1335,7 @@ def trlog(
         [R, t] = tr2rt(T)
 
         # S = trlog(R, check=False)  # recurse
-        S = trlog(cast(SO3Array, R), check=False)  # recurse
+        S = trlog(cast(SO3Array, R), check=False, tol=tol)  # recurse
         w = vex(S)
         theta = norm(w)
         if theta == 0:

spatialmath/base/transformsNd.py

@@ -359,7 +359,7 @@ def isR(R: NDArray, tol: float = 100) -> bool:  # -> TypeGuard[SOnArray]:
 
     :param R: matrix to test
     :type R: ndarray(2,2) or ndarray(3,3)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 100
     :type tol: float
     :return: whether matrix is a proper orthonormal rotation matrix
     :rtype: bool
@@ -388,7 +388,7 @@ def isskew(S: NDArray, tol: float = 10) -> bool:  # -> TypeGuard[sonArray]:
 
     :param S: matrix to test
     :type S: ndarray(2,2) or ndarray(3,3)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether matrix is a proper skew-symmetric matrix
     :rtype: bool
@@ -415,7 +415,7 @@ def isskewa(S: NDArray, tol: float = 10) -> bool:  # -> TypeGuard[senArray]:
 
     :param S: matrix to test
     :type S: ndarray(3,3) or ndarray(4,4)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether matrix is a proper skew-symmetric matrix
     :rtype: bool
@@ -445,7 +445,7 @@ def iseye(S: NDArray, tol: float = 10) -> bool:
 
     :param S: matrix to test
     :type S: ndarray(n,n)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether matrix is a proper skew-symmetric matrix
     :rtype: bool

spatialmath/base/vectors.py

@@ -142,12 +142,14 @@ def colvec(v: ArrayLike) -> NDArray:
     return np.array(v).reshape((len(v), 1))
 
 
-def unitvec(v: ArrayLike) -> NDArray:
+def unitvec(v: ArrayLike, tol: float = 100) -> NDArray:
     """
     Create a unit vector
 
     :param v: any vector
     :type v: array_like(n)
+    :param tol: Tolerance in units of eps for zero-norm case, defaults to 100
+    :type: float
     :return: a unit-vector parallel to ``v``.
     :rtype: ndarray(n)
     :raises ValueError: for zero length vector
@@ -166,7 +168,7 @@ def unitvec(v: ArrayLike) -> NDArray:
     v = getvector(v)
     n = norm(v)
 
-    if n > 100 * _eps:  # if greater than eps
+    if n > tol * _eps:  # if greater than eps
         return v / n
     else:
         raise ValueError("zero norm vector")
@@ -178,6 +180,8 @@ def unitvec_norm(v: ArrayLike, tol: float = 100) -> Tuple[NDArray, float]:
 
     :param v: any vector
     :type v: array_like(n)
+    :param tol: Tolerance in units of eps for zero-norm case, defaults to 100
+    :type: float
     :return: a unit-vector parallel to ``v`` and the norm
     :rtype: (ndarray(n), float)
     :raises ValueError: for zero length vector
@@ -208,7 +212,7 @@ def isunitvec(v: ArrayLike, tol: float = 10) -> bool:
 
     :param v: vector to test
     :type v: ndarray(n)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether vector has unit length
     :rtype: bool
@@ -230,7 +234,7 @@ def iszerovec(v: ArrayLike, tol: float = 10) -> bool:
 
     :param v: vector to test
     :type v: ndarray(n)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether vector has zero length
     :rtype: bool
@@ -252,7 +256,7 @@ def iszero(v: float, tol: float = 10) -> bool:
 
     :param v: value to test
     :type v: float
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether value is zero
     :rtype: bool
@@ -274,7 +278,7 @@ def isunittwist(v: ArrayLike6, tol: float = 10) -> bool:
 
     :param v: twist vector to test
     :type v: array_like(6)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether twist has unit length
     :rtype: bool
@@ -302,7 +306,7 @@ def isunittwist(v: ArrayLike6, tol: float = 10) -> bool:
     if len(v) == 6:
         # test for SE(3) twist
         return isunitvec(v[3:6], tol=tol) or (
-            bool(np.linalg.norm(v[3:6]) < tol * _eps) and isunitvec(v[0:3], tol=tol)
+            iszerovec(v[3:6], tol=tol) and isunitvec(v[0:3], tol=tol)
         )
     else:
         raise ValueError
@@ -314,7 +318,7 @@ def isunittwist2(v: ArrayLike3, tol: float = 10) -> bool:
 
     :param v: twist vector to test
     :type v: array_like(3)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: whether vector has unit length
     :rtype: bool
@@ -341,7 +345,7 @@ def isunittwist2(v: ArrayLike3, tol: float = 10) -> bool:
     if len(v) == 3:
         # test for SE(2) twist
         return isunitvec(v[2], tol=tol) or (
-            np.abs(v[2]) < tol * _eps and isunitvec(v[0:2], tol=tol)
+            iszero(v[2], tol=tol) and isunitvec(v[0:2], tol=tol)
         )
     else:
         raise ValueError
@@ -353,7 +357,7 @@ def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]:
 
     :param S: twist vector
     :type S: array_like(6)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: unit twist
     :rtype: ndarray(6)
@@ -380,7 +384,7 @@ def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]:
     v = S[0:3]
     w = S[3:6]
 
-    if iszerovec(w):
+    if iszerovec(w, tol=tol):
         th = norm(v)
     else:
         th = norm(w)
@@ -394,7 +398,7 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float
 
     :param S: twist vector
     :type S: array_like(6)
-    :param tol: tolerance in units of eps
+    :param tol: tolerance in units of eps, defaults to 10
     :type tol: float
     :return: unit twist and scalar motion
     :rtype: tuple (ndarray(6), float)
@@ -425,20 +429,22 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[R6, float
     v = S[0:3]
     w = S[3:6]
 
-    if iszerovec(w):
+    if iszerovec(w, tol=tol):
         th = norm(v)
     else:
         th = norm(w)
 
     return (S / th, th)
 
 
-def unittwist2(S: ArrayLike3) -> R3:
+def unittwist2(S: ArrayLike3, tol: float = 10) -> R3:
     """
     Convert twist to unit twist
 
     :param S: twist vector
     :type S: array_like(3)
+    :param tol: tolerance in units of eps, defaults to 10
+    :type tol: float
     :return: unit twist
     :rtype: ndarray(3)
 
@@ -459,20 +465,22 @@ def unittwist2(S: ArrayLike3) -> R3:
     v = S[0:2]
     w = S[2]
 
-    if iszero(w):
+    if iszero(w, tol=tol):
         th = norm(v)
     else:
         th = abs(w)
 
     return S / th
 
 
-def unittwist2_norm(S: ArrayLike3) -> Tuple[R3, float]:
+def unittwist2_norm(S: ArrayLike3, tol: float = 10) -> Tuple[R3, float]:
     """
     Convert twist to unit twist
 
     :param S: twist vector
     :type S: array_like(3)
+    :param tol: tolerance in units of eps, defaults to 10
+    :type tol: float
     :return: unit twist and scalar motion
     :rtype: tuple (ndarray(3), float)
 
@@ -493,7 +501,7 @@ def unittwist2_norm(S: ArrayLike3) -> Tuple[R3, float]:
     v = S[0:2]
     w = S[2]
 
-    if iszero(w):
+    if iszero(w, tol=tol):
         th = norm(v)
     else:
         th = abs(w)

spatialmath/baseposematrix.py

@@ -882,8 +882,6 @@ def _string_color(self, color: Optional[bool] = False) -> str:
 
         :param color: colorise the output, defaults to False
         :type color: bool, optional
-        :param tol: zero values smaller than tol*eps, defaults to 10
-        :type tol: float, optional
         :return: multiline matrix representation
         :rtype: str