diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index ddfa64bd..77efe640 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -106,13 +106,13 @@ def qnorm(q: ArrayLike4) -> float: return np.linalg.norm(q) -def qunit(q: ArrayLike4, tol: float = 10) -> UnitQuaternionArray: +def qunit(q: ArrayLike4, tol: float = 20) -> UnitQuaternionArray: """ Create a unit quaternion :arg v: quaterion :type v: array_like(4) - :param tol: Tolerance in multiples of eps, defaults to 10 + :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: a pure quaternion :rtype: ndarray(4) @@ -146,13 +146,13 @@ def qunit(q: ArrayLike4, tol: float = 10) -> UnitQuaternionArray: return -q -def qisunit(q: ArrayLike4, tol: float = 100) -> bool: +def qisunit(q: ArrayLike4, tol: float = 20) -> bool: """ Test if quaternion has unit length :param v: quaternion :type v: array_like(4) - :param tol: tolerance in units of eps, defaults to 100 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether quaternion has unit length :rtype: bool @@ -174,7 +174,7 @@ def qisunit(q: ArrayLike4, tol: float = 100) -> bool: def qisequal( q1: ArrayLike4, q2: ArrayLike4, - tol: float = 100, + tol: float = 20, unitq: Optional[bool] = False, ) -> bool: ... @@ -184,13 +184,13 @@ def qisequal( def qisequal( q1: ArrayLike4, q2: ArrayLike4, - tol: float = 100, + tol: float = 20, unitq: Optional[bool] = True, ) -> bool: ... -def qisequal(q1, q2, tol: float = 100, unitq: Optional[bool] = False): +def qisequal(q1, q2, tol: float = 20, unitq: Optional[bool] = False): """ Test if quaternions are equal @@ -200,7 +200,7 @@ def qisequal(q1, q2, tol: 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, defaults to 100 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether quaternions are equal :rtype: bool @@ -547,7 +547,7 @@ def q2r( def r2q( R: SO3Array, check: Optional[bool] = False, - tol: float = 100, + tol: float = 20, order: Optional[str] = "sxyz", ) -> UnitQuaternionArray: """ @@ -557,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, defaults to 100 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :param order: the order of the returned quaternion elements. Must be 'sxyz' or 'xyzs'. Defaults to 'sxyz'. @@ -761,7 +761,11 @@ def r2q( def qslerp( - q0: ArrayLike4, q1: ArrayLike4, s: float, shortest: Optional[bool] = False, tol: float = 10 + q0: ArrayLike4, + q1: ArrayLike4, + s: float, + shortest: Optional[bool] = False, + tol: float = 20, ) -> UnitQuaternionArray: """ Quaternion conjugate @@ -774,7 +778,7 @@ 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 + :param tol: Tolerance when checking for identical quaternions, in multiples of eps, defaults to 20 :type tol: float, optional :return: interpolated unit-quaternion :rtype: ndarray(4) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 5dc396f8..669c8fdd 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -324,7 +324,7 @@ def pos2tr2(x, y=None): return T -def ishom2(T: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard(SE2): +def ishom2(T: Any, check: bool = False, tol: float = 20) -> bool: # TypeGuard(SE2): """ Test if matrix belongs to SE(2) @@ -332,7 +332,7 @@ def ishom2(T: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard( :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 + :param tol: Tolerance in units of eps for zero-rotation case, defaults to 20 :type: float :return: whether matrix is an SE(2) homogeneous transformation matrix :rtype: bool @@ -358,11 +358,14 @@ def ishom2(T: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard( return ( isinstance(T, np.ndarray) and T.shape == (3, 3) - and (not check or (smb.isR(T[:2, :2], tol=tol) 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, tol: float = 100) -> bool: # TypeGuard(SO2): +def isrot2(R: Any, check: bool = False, tol: float = 20) -> bool: # TypeGuard(SO2): """ Test if matrix belongs to SO(2) @@ -370,7 +373,7 @@ def isrot2(R: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard( :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 + :param tol: Tolerance in units of eps for zero-rotation case, defaults to 20 :type: float :return: whether matrix is an SO(2) rotation matrix :rtype: bool @@ -392,7 +395,11 @@ def isrot2(R: Any, check: bool = False, tol: float = 100) -> bool: # TypeGuard( :seealso: isR, ishom2, isrot """ - return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or smb.isR(R, tol=tol)) + return ( + isinstance(R, np.ndarray) + and R.shape == (2, 2) + and (not check or smb.isR(R, tol=tol)) + ) # ---------------------------------------------------------------------------------------# @@ -438,7 +445,7 @@ def trlog2( T: SO2Array, twist: bool = False, check: bool = True, - tol: float = 10, + tol: float = 20, ) -> so2Array: ... @@ -448,7 +455,7 @@ def trlog2( T: SE2Array, twist: bool = False, check: bool = True, - tol: float = 10, + tol: float = 20, ) -> se2Array: ... @@ -458,7 +465,7 @@ def trlog2( T: SO2Array, twist: bool = True, check: bool = True, - tol: float = 10, + tol: float = 20, ) -> float: ... @@ -468,7 +475,7 @@ def trlog2( T: SE2Array, twist: bool = True, check: bool = True, - tol: float = 10, + tol: float = 20, ) -> R3: ... @@ -477,7 +484,7 @@ def trlog2( T: Union[SO2Array, SE2Array], twist: bool = False, check: bool = True, - tol: float = 10, + tol: float = 20, ) -> Union[float, R3, so2Array, se2Array]: """ Logarithm of SO(2) or SE(2) matrix @@ -488,7 +495,7 @@ def trlog2( :type check: bool :param twist: return a twist vector instead of matrix [default] :type twist: bool - :param tol: Tolerance in units of eps for zero-rotation case, defaults to 10 + :param tol: Tolerance in units of eps for zero-rotation case, defaults to 20 :type: float :return: logarithm :rtype: ndarray(3,3) or ndarray(3); or ndarray(2,2) or ndarray(1) @@ -1016,13 +1023,13 @@ def trprint2( return s -def _vec2s(fmt: str, v: ArrayLikePure, tol: float = 100) -> str: +def _vec2s(fmt: str, v: ArrayLikePure, tol: float = 20) -> 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 + :param tol: Tolerance when checking for near-zero values, in multiples of eps, defaults to 20 :type tol: float, optional :return: string representation for the vector :rtype: str diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index f2f35a1d..376af9ff 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -349,14 +349,14 @@ def transl(x, y=None, z=None): return T -def ishom(T: Any, check: bool = False, tol: float = 100) -> bool: +def ishom(T: Any, check: bool = False, tol: float = 20) -> bool: """ Test if matrix belongs to SE(3) :param T: SE(3) matrix to test :type T: numpy(4,4) :param check: check validity of rotation submatrix - :param tol: Tolerance in units of eps for rotation submatrix check, defaults to 100 + :param tol: Tolerance in units of eps for rotation submatrix check, defaults to 20 :return: whether matrix is an SE(3) homogeneous transformation matrix - ``ishom(T)`` is True if the argument ``T`` is of dimension 4x4 @@ -387,14 +387,14 @@ def ishom(T: Any, check: bool = False, tol: float = 100) -> bool: ) -def isrot(R: Any, check: bool = False, tol: float = 100) -> bool: +def isrot(R: Any, check: bool = False, tol: float = 20) -> bool: """ Test if matrix belongs to SO(3) :param R: SO(3) matrix to test :type R: numpy(3,3) :param check: check validity of rotation submatrix - :param tol: Tolerance in units of eps for rotation matrix test, defaults to 100 + :param tol: Tolerance in units of eps for rotation matrix test, defaults to 20 :return: whether matrix is an SO(3) rotation matrix - ``isrot(R)`` is True if the argument ``R`` is of dimension 3x3 @@ -713,7 +713,7 @@ def eul2tr( # ---------------------------------------------------------------------------------------# -def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 10) -> SO3Array: +def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Array: """ Create an SO(3) rotation matrix from rotation angle and axis @@ -723,7 +723,7 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 10) -> SO3Arr :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 + :param tol: Tolerance in units of eps for zero-rotation case, defaults to 20 :type: float :return: SO(3) rotation matrix :rtype: ndarray(3,3) @@ -1046,7 +1046,7 @@ def tr2eul( unit: str = "rad", flip: bool = False, check: bool = False, - tol: float = 10, + tol: float = 20, ) -> R3: r""" Convert SO(3) or SE(3) to ZYX Euler angles @@ -1059,7 +1059,7 @@ 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 + :param tol: Tolerance in units of eps for near-zero checks, defaults to 20 :type: float :return: ZYZ Euler angles :rtype: ndarray(3) @@ -1129,7 +1129,7 @@ def tr2rpy( unit: str = "rad", order: str = "zyx", check: bool = False, - tol: float = 10, + tol: float = 20, ) -> R3: r""" Convert SO(3) or SE(3) to roll-pitch-yaw angles @@ -1142,7 +1142,7 @@ 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 + :param tol: Tolerance in units of eps, defaults to 20 :type: float :return: Roll-pitch-yaw angles :rtype: ndarray(3) @@ -1271,25 +1271,25 @@ def tr2rpy( # ---------------------------------------------------------------------------------------# @overload # pragma: no cover def trlog( - T: SO3Array, check: bool = True, twist: bool = False, tol: float = 10 + T: SO3Array, check: bool = True, twist: bool = False, tol: float = 20 ) -> so3Array: ... @overload # pragma: no cover def trlog( - T: SE3Array, check: bool = True, twist: bool = False, tol: float = 10 + T: SE3Array, check: bool = True, twist: bool = False, tol: float = 20 ) -> se3Array: ... @overload # pragma: no cover -def trlog(T: SO3Array, check: bool = True, twist: bool = True, tol: float = 10) -> R3: +def trlog(T: SO3Array, check: bool = True, twist: bool = True, tol: float = 20) -> R3: ... @overload # pragma: no cover -def trlog(T: SE3Array, check: bool = True, twist: bool = True, tol: float = 10) -> R6: +def trlog(T: SE3Array, check: bool = True, twist: bool = True, tol: float = 20) -> R6: ... @@ -1297,7 +1297,7 @@ def trlog( T: Union[SO3Array, SE3Array], check: bool = True, twist: bool = False, - tol: float = 10, + tol: float = 20, ) -> Union[R3, R6, so3Array, se3Array]: """ Logarithm of SO(3) or SE(3) matrix @@ -1308,7 +1308,7 @@ def trlog( :type check: bool :param twist: return a twist vector instead of matrix [default] :type twist: bool - :param tol: Tolerance in units of eps for zero-rotation case, defaults to 10 + :param tol: Tolerance in units of eps for zero-rotation case, defaults to 20 :type: float :return: logarithm :rtype: ndarray(4,4) or ndarray(3,3) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index d8a8b4bb..57476d05 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -295,7 +295,9 @@ def rt2tr(R, t, check=False): T[:3, :3] = R T[:3, 3] = t else: - raise ValueError(f"R must be an SO(2) or SO(3) rotation matrix, not {R.shape}") + raise ValueError( + f"R must be an SO(2) or SO(3) rotation matrix, not {R.shape}" + ) return T @@ -353,13 +355,13 @@ def Ab2M(A: np.ndarray, b: np.ndarray) -> np.ndarray: # ======================= predicates -def isR(R: NDArray, tol: float = 100) -> bool: # -> TypeGuard[SOnArray]: +def isR(R: NDArray, tol: float = 20) -> bool: # -> TypeGuard[SOnArray]: r""" Test if matrix belongs to SO(n) :param R: matrix to test :type R: ndarray(2,2) or ndarray(3,3) - :param tol: tolerance in units of eps, defaults to 100 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether matrix is a proper orthonormal rotation matrix :rtype: bool @@ -382,13 +384,13 @@ def isR(R: NDArray, tol: float = 100) -> bool: # -> TypeGuard[SOnArray]: ) -def isskew(S: NDArray, tol: float = 10) -> bool: # -> TypeGuard[sonArray]: +def isskew(S: NDArray, tol: float = 20) -> bool: # -> TypeGuard[sonArray]: r""" Test if matrix belongs to so(n) :param S: matrix to test :type S: ndarray(2,2) or ndarray(3,3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether matrix is a proper skew-symmetric matrix :rtype: bool @@ -409,13 +411,13 @@ def isskew(S: NDArray, tol: float = 10) -> bool: # -> TypeGuard[sonArray]: return bool(np.linalg.norm(S + S.T) < tol * _eps) -def isskewa(S: NDArray, tol: float = 10) -> bool: # -> TypeGuard[senArray]: +def isskewa(S: NDArray, tol: float = 20) -> bool: # -> TypeGuard[senArray]: r""" Test if matrix belongs to se(n) :param S: matrix to test :type S: ndarray(3,3) or ndarray(4,4) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether matrix is a proper skew-symmetric matrix :rtype: bool @@ -439,18 +441,18 @@ def isskewa(S: NDArray, tol: float = 10) -> bool: # -> TypeGuard[senArray]: ) -def iseye(S: NDArray, tol: float = 10) -> bool: +def iseye(S: NDArray, tol: float = 20) -> bool: """ Test if matrix is identity :param S: matrix to test :type S: ndarray(n,n) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether matrix is a proper skew-symmetric matrix :rtype: bool - Check if matrix is an identity matrix. + Check if matrix is an identity matrix. We check that the sum of the absolute value of the residual is less than ``tol * eps``. .. runblock:: pycon diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index 588c0a50..f29740a3 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -142,13 +142,13 @@ def colvec(v: ArrayLike) -> NDArray: return np.array(v).reshape((len(v), 1)) -def unitvec(v: ArrayLike, tol: float = 100) -> NDArray: +def unitvec(v: ArrayLike, tol: float = 20) -> 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 + :param tol: Tolerance in units of eps for zero-norm case, defaults to 20 :type: float :return: a unit-vector parallel to ``v``. :rtype: ndarray(n) @@ -174,13 +174,13 @@ def unitvec(v: ArrayLike, tol: float = 100) -> NDArray: raise ValueError("zero norm vector") -def unitvec_norm(v: ArrayLike, tol: float = 100) -> Tuple[NDArray, float]: +def unitvec_norm(v: ArrayLike, tol: float = 20) -> Tuple[NDArray, float]: """ 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 + :param tol: Tolerance in units of eps for zero-norm case, defaults to 20 :type: float :return: a unit-vector parallel to ``v`` and the norm :rtype: (ndarray(n), float) @@ -206,13 +206,13 @@ def unitvec_norm(v: ArrayLike, tol: float = 100) -> Tuple[NDArray, float]: raise ValueError("zero norm vector") -def isunitvec(v: ArrayLike, tol: float = 10) -> bool: +def isunitvec(v: ArrayLike, tol: float = 20) -> bool: """ Test if vector has unit length :param v: vector to test :type v: ndarray(n) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether vector has unit length :rtype: bool @@ -228,13 +228,13 @@ def isunitvec(v: ArrayLike, tol: float = 10) -> bool: return bool(abs(np.linalg.norm(v) - 1) < tol * _eps) -def iszerovec(v: ArrayLike, tol: float = 10) -> bool: +def iszerovec(v: ArrayLike, tol: float = 20) -> bool: """ Test if vector has zero length :param v: vector to test :type v: ndarray(n) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether vector has zero length :rtype: bool @@ -250,13 +250,13 @@ def iszerovec(v: ArrayLike, tol: float = 10) -> bool: return bool(np.linalg.norm(v) < tol * _eps) -def iszero(v: float, tol: float = 10) -> bool: +def iszero(v: float, tol: float = 20) -> bool: """ Test if scalar is zero :param v: value to test :type v: float - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether value is zero :rtype: bool @@ -272,13 +272,13 @@ def iszero(v: float, tol: float = 10) -> bool: return bool(abs(v) < tol * _eps) -def isunittwist(v: ArrayLike6, tol: float = 10) -> bool: +def isunittwist(v: ArrayLike6, tol: float = 20) -> bool: r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: twist vector to test :type v: array_like(6) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether twist has unit length :rtype: bool @@ -312,13 +312,13 @@ def isunittwist(v: ArrayLike6, tol: float = 10) -> bool: raise ValueError -def isunittwist2(v: ArrayLike3, tol: float = 10) -> bool: +def isunittwist2(v: ArrayLike3, tol: float = 20) -> bool: r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: twist vector to test :type v: array_like(3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether vector has unit length :rtype: bool @@ -351,13 +351,13 @@ def isunittwist2(v: ArrayLike3, tol: float = 10) -> bool: raise ValueError -def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]: +def unittwist(S: ArrayLike6, tol: float = 20) -> Union[R6, None]: """ Convert twist to unit twist :param S: twist vector :type S: array_like(6) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist :rtype: ndarray(6) @@ -392,13 +392,15 @@ def unittwist(S: ArrayLike6, tol: float = 10) -> Union[R6, None]: return S / th -def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[Union[R6, None], Union[float, None]]: +def unittwist_norm( + S: Union[R6, ArrayLike6], tol: float = 20 +) -> Tuple[Union[R6, None], Union[float, None]]: """ Convert twist to unit twist and norm :param S: twist vector :type S: array_like(6) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist and scalar motion :rtype: tuple (ndarray(6), float) @@ -437,13 +439,13 @@ def unittwist_norm(S: Union[R6, ArrayLike6], tol: float = 10) -> Tuple[Union[R6, return (S / th, th) -def unittwist2(S: ArrayLike3, tol: float = 10) -> Union[R3, None]: +def unittwist2(S: ArrayLike3, tol: float = 20) -> Union[R3, None]: """ Convert twist to unit twist :param S: twist vector :type S: array_like(3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist :rtype: ndarray(3) @@ -478,13 +480,15 @@ def unittwist2(S: ArrayLike3, tol: float = 10) -> Union[R3, None]: return S / th -def unittwist2_norm(S: ArrayLike3, tol: float = 10) -> Tuple[Union[R3, None], Union[float, None]]: +def unittwist2_norm( + S: ArrayLike3, tol: float = 20 +) -> Tuple[Union[R3, None], Union[float, None]]: """ Convert twist to unit twist :param S: twist vector :type S: array_like(3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist and scalar motion :rtype: tuple (ndarray(3), float) @@ -785,13 +789,13 @@ def vector_diff(v1: ArrayLike, v2: ArrayLike, mode: str) -> NDArray: return v -def removesmall(v: ArrayLike, tol: float = 100) -> NDArray: +def removesmall(v: ArrayLike, tol: float = 20) -> NDArray: """ Set small values to zero :param v: any vector :type v: array_like(n) or ndarray(n,m) - :param tol: Tolerance in units of eps, defaults to 100 + :param tol: Tolerance in units of eps, defaults to 20 :type tol: int, optional :return: vector with small values set to zero :rtype: ndarray(n) or ndarray(n,m) diff --git a/spatialmath/geom2d.py b/spatialmath/geom2d.py index 13c57895..55eccb2a 100755 --- a/spatialmath/geom2d.py +++ b/spatialmath/geom2d.py @@ -121,13 +121,13 @@ def plot(self, **kwargs) -> None: """ smb.plot_homline(self.line, **kwargs) - def intersect(self, other: Line2, tol: float = 10) -> R3: + def intersect(self, other: Line2, tol: float = 20) -> R3: """ Intersection with line :param other: another 2D line :type other: Line2 - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: intersection point in homogeneous form :rtype: ndarray(3) @@ -140,13 +140,13 @@ def intersect(self, other: Line2, tol: float = 10) -> R3: c = np.cross(self.line, other.line) return abs(c[2]) > tol * _eps - def contains(self, p: ArrayLike2, tol: float = 10) -> bool: + def contains(self, p: ArrayLike2, tol: float = 20) -> bool: """ Test if point is in line :param p1: point to test :type p1: array_like(2) or array_like(3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: True if point lies in the line :rtype: bool @@ -158,7 +158,9 @@ def contains(self, p: ArrayLike2, tol: float = 10) -> bool: # variant that gives lambda - def intersect_segment(self, p1: ArrayLike2, p2: ArrayLike2, tol: float = 10) -> bool: + def intersect_segment( + self, p1: ArrayLike2, p2: ArrayLike2, tol: float = 20 + ) -> bool: """ Test for line intersecting line segment @@ -166,7 +168,7 @@ def intersect_segment(self, p1: ArrayLike2, p2: ArrayLike2, tol: float = 10) -> :type p1: array_like(2) or array_like(3) :param p2: end of line segment :type p2: array_like(2) or array_like(3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: True if they intersect :rtype: bool @@ -1122,7 +1124,7 @@ def polygon(self, resolution=10) -> Polygon2: """ Approximate with a polygon - :param resolution: number of polygon vertices, defaults to 10 + :param resolution: number of polygon vertices, defaults to 20 :type resolution: int, optional :return: a polygon approximating the ellipse :rtype: :class:`Polygon2` instance diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index 5b3076ef..8b191ebd 100755 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -176,13 +176,13 @@ def d(self) -> float: """ return self.plane[3] - def contains(self, p: ArrayLike3, tol: float = 10) -> bool: + def contains(self, p: ArrayLike3, tol: float = 20) -> bool: """ Test if point in plane :param p: A 3D point :type p: array_like(3) - :param tol: tolerance in units of eps, defaults to 10 + :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: if the point is in the plane :rtype: bool @@ -618,14 +618,14 @@ def lam(self, point: ArrayLike3) -> float: # ------------------------------------------------------------------------- # def contains( - self, x: Union[R3, Points3], tol: float = 50 + self, x: Union[R3, Points3], tol: float = 20 ) -> Union[bool, List[bool]]: """ Test if points are on the line :param x: 3D point :type x: 3-element array_like, or ndarray(3,N) - :param tol: Tolerance in units of eps, defaults to 50 + :param tol: Tolerance in units of eps, defaults to 20 :type tol: float, optional :raises ValueError: Bad argument :return: Whether point is on the line @@ -649,14 +649,14 @@ def contains( raise ValueError("bad argument") def isequal( - l1, l2: Line3, tol: float = 10 # type: ignore + l1, l2: Line3, tol: float = 20 # type: ignore ) -> bool: # pylint: disable=no-self-argument """ Test if two lines are equivalent :param l2: Second line :type l2: ``Line3`` - :param tol: Tolerance in multiples of eps, defaults to 10 + :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: lines are equivalent :rtype: bool @@ -672,14 +672,14 @@ def isequal( ) def isparallel( - l1, l2: Line3, tol: float = 10 # type: ignore + l1, l2: Line3, tol: float = 20 # type: ignore ) -> bool: # pylint: disable=no-self-argument """ Test if lines are parallel :param l2: Second line :type l2: ``Line3`` - :param tol: Tolerance in multiples of eps, defaults to 10 + :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: lines are parallel :rtype: bool @@ -693,14 +693,14 @@ def isparallel( return bool(np.linalg.norm(np.cross(l1.w, l2.w)) < tol * _eps) def isintersecting( - l1, l2: Line3, tol: float = 10 # type: ignore + l1, l2: Line3, tol: float = 20 # type: ignore ) -> bool: # pylint: disable=no-self-argument """ Test if lines are intersecting :param l2: Second line :type l2: Line3 - :param tol: Tolerance in multiples of eps, defaults to 10 + :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: lines intersect :rtype: bool @@ -826,14 +826,14 @@ def intersects( return None def distance( - l1, l2: Line3, tol: float = 10 # type:ignore + l1, l2: Line3, tol: float = 20 # type:ignore ) -> float: # pylint: disable=no-self-argument """ Minimum distance between lines :param l2: Second line :type l2: ``Line3`` - :param tol: Tolerance in multiples of eps, defaults to 10 + :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: Closest distance between lines :rtype: float @@ -1075,14 +1075,14 @@ def __rmul__( # ------------------------------------------------------------------------- # def intersect_plane( - self, plane: Union[ArrayLike4, Plane3], tol: float = 100 + self, plane: Union[ArrayLike4, Plane3], tol: float = 20 ) -> Tuple[R3, float]: r""" Line intersection with a plane :param plane: A plane :type plane: array_like(4) or Plane3 - :param tol: Tolerance in multiples of eps, defaults to 10 + :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: Intersection point, λ :rtype: ndarray(3), float diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 981e9278..0b5ba7e9 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -396,11 +396,11 @@ def log(self) -> Quaternion: v = math.acos(self.s / norm) * smb.unitvec(self.v) return Quaternion(s=s, v=v) - def exp(self, tol: float=100) -> Quaternion: + def exp(self, tol: float = 20) -> Quaternion: r""" Exponential of quaternion - :param tol: Tolerance when checking for pure quaternion, in multiples of eps, defaults to 100 + :param tol: Tolerance when checking for pure quaternion, in multiples of eps, defaults to 20 :type tol: float, optional :rtype: Quaternion instance @@ -996,7 +996,9 @@ def __init__( # UnitQuaternion(R) R is 3x3 rotation matrix self.data = [smb.r2q(s)] else: - raise ValueError("invalid rotation matrix provided to UnitQuaternion constructor") + raise ValueError( + "invalid rotation matrix provided to UnitQuaternion constructor" + ) elif s.shape == (4,): # passed a 4-vector if norm: