diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index eaa18f0a..ddfa64bd 100755 --- a/spatialmath/base/quaternions.py +++ b/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) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index a6e34813..5dc396f8 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -266,7 +266,7 @@ 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 @@ -274,7 +274,7 @@ def tr2pos2(T): >>> 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,9 +292,9 @@ 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. @@ -302,9 +302,9 @@ def pos2tr2(x, y=None): >>> 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]) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 66a3dad4..f2f35a1d 100644 --- a/spatialmath/base/transforms3d.py +++ b/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: diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index e38c4f58..588c0a50 100644 --- a/spatialmath/base/vectors.py +++ b/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") diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index 73c42586..5b3076ef 100755 --- a/spatialmath/geom3d.py +++ b/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 """ diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 3156a28d..981e9278 100644 --- a/spatialmath/quaternion.py +++ b/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: diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index 47b1156c..ff099930 100755 --- a/tests/base/test_transforms2d.py +++ b/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)) diff --git a/tests/base/test_transformsNd.py b/tests/base/test_transformsNd.py index ee8724c1..92d9e2a3 100755 --- a/tests/base/test_transformsNd.py +++ b/tests/base/test_transformsNd.py @@ -58,6 +58,10 @@ def test_r2t(self): with self.assertRaises(ValueError): r2t(np.eye(3, 4)) + + _ = r2t(np.ones((3, 3)), check=False) + with self.assertRaises(ValueError): + r2t(np.ones((3, 3)), check=True) @unittest.skipUnless(_symbolics, "sympy required") def test_r2t_sym(self): @@ -118,6 +122,13 @@ def test_rt2tr(self): with self.assertRaises(ValueError): rt2tr(np.eye(3, 4), [1, 2, 3, 4]) + with self.assertRaises(ValueError): + rt2tr(np.eye(4, 4), [1, 2, 3, 4]) + + _ = rt2tr(np.ones((3, 3)), [1, 2, 3], check=False) + with self.assertRaises(ValueError): + rt2tr(np.ones((3, 3)), [1, 2, 3], check=True) + @unittest.skipUnless(_symbolics, "sympy required") def test_rt2tr_sym(self): theta = symbol("theta") @@ -147,6 +158,32 @@ def test_tr2rt(self): with self.assertRaises(ValueError): R, t = tr2rt(np.eye(3, 4)) + def test_Ab2M(self): + # 3D + R = np.ones((3, 3)) + t = [3, 4, 5] + T = Ab2M(R, t) + nt.assert_array_almost_equal(T[:3, :3], R) + nt.assert_array_almost_equal(T[:3, 3], np.array(t)) + nt.assert_array_almost_equal(T[3, :], np.array([0, 0, 0, 0])) + + # 2D + R = np.ones((2, 2)) + t = [3, 4] + T = Ab2M(R, t) + nt.assert_array_almost_equal(T[:2, :2], R) + nt.assert_array_almost_equal(T[:2, 2], np.array(t)) + nt.assert_array_almost_equal(T[2, :], np.array([0, 0, 0])) + + with self.assertRaises(ValueError): + Ab2M(3, 4) + + with self.assertRaises(ValueError): + Ab2M(np.eye(3, 4), [1, 2, 3, 4]) + + with self.assertRaises(ValueError): + Ab2M(np.eye(4, 4), [1, 2, 3, 4]) + def test_checks(self): # 3D case, with rotation matrix R = np.eye(3) @@ -282,6 +319,13 @@ def test_vex(self): sk = skew(t) nt.assert_almost_equal(vex(sk), t) + _ = vex(np.ones((3, 3)), check=False) + with self.assertRaises(ValueError): + _ = vex(np.ones((3, 3)), check=True) + + with self.assertRaises(ValueError): + _ = vex(np.eye(4, 4)) + def test_isskew(self): t = [3, 4, 5] sk = skew(t) diff --git a/tests/base/test_vectors.py b/tests/base/test_vectors.py index 8507dc9c..592c2d16 100755 --- a/tests/base/test_vectors.py +++ b/tests/base/test_vectors.py @@ -88,6 +88,11 @@ def test_norm(self): self.assertAlmostEqual(norm([1, 2, 3]), math.sqrt(14)) self.assertAlmostEqual(norm(np.r_[1, 2, 3]), math.sqrt(14)) + def test_normsq(self): + self.assertAlmostEqual(normsq([0, 0, 0]), 0) + self.assertAlmostEqual(normsq([1, 2, 3]), 14) + self.assertAlmostEqual(normsq(np.r_[1, 2, 3]), 14) + @unittest.skipUnless(_symbolics, "sympy required") def test_norm_sym(self): x, y = symbol("x y") @@ -208,8 +213,46 @@ def test_unittwist_norm(self): nt.assert_array_almost_equal(a[0], np.r_[0, 0, -1, 0, 0, 0]) nt.assert_array_almost_equal(a[1], 2) - with self.assertRaises(ValueError): - unittwist_norm([0, 0, 0, 0, 0, 0]) + a = unittwist_norm([0, 0, 0, 0, 0, 0]) + self.assertIsNone(a[0]) + self.assertIsNone(a[1]) + + def test_unittwist2(self): + nt.assert_array_almost_equal( + unittwist2([1, 0, 0]), np.r_[1, 0, 0] + ) + nt.assert_array_almost_equal( + unittwist2([0, 2, 0]), np.r_[0, 1, 0] + ) + nt.assert_array_almost_equal( + unittwist2([0, 0, -3]), np.r_[0, 0, -1] + ) + nt.assert_array_almost_equal( + unittwist2([2, 0, -2]), np.r_[1, 0, -1] + ) + + self.assertIsNone(unittwist2([0, 0, 0])) + + def test_unittwist2_norm(self): + a = unittwist2_norm([1, 0, 0]) + nt.assert_array_almost_equal(a[0], np.r_[1, 0, 0]) + nt.assert_array_almost_equal(a[1], 1) + + a = unittwist2_norm([0, 2, 0]) + nt.assert_array_almost_equal(a[0], np.r_[0, 1, 0]) + nt.assert_array_almost_equal(a[1], 2) + + a = unittwist2_norm([0, 0, -3]) + nt.assert_array_almost_equal(a[0], np.r_[0, 0, -1]) + nt.assert_array_almost_equal(a[1], 3) + + a = unittwist2_norm([2, 0, -2]) + nt.assert_array_almost_equal(a[0], np.r_[1, 0, -1]) + nt.assert_array_almost_equal(a[1], 2) + + a = unittwist2_norm([0, 0, 0]) + self.assertIsNone(a[0]) + self.assertIsNone(a[1]) def test_iszerovec(self): self.assertTrue(iszerovec([0])) @@ -282,6 +325,21 @@ def test_wrap(self): [0, -0.5 * pi, 0.5 * pi, 0.4 * pi, -0.4 * pi], ) + for angle_factor in (0, 0.3, 0.5, 0.8, 1.0, 1.3, 1.5, 1.7, 2): + theta = angle_factor * pi + self.assertAlmostEqual(angle_wrap(theta), wrap_mpi_pi(theta)) + self.assertAlmostEqual(angle_wrap(-theta), wrap_mpi_pi(-theta)) + self.assertAlmostEqual(angle_wrap(theta=theta, mode="-pi:pi"), wrap_mpi_pi(theta)) + self.assertAlmostEqual(angle_wrap(theta=-theta, mode="-pi:pi"), wrap_mpi_pi(-theta)) + self.assertAlmostEqual(angle_wrap(theta=theta, mode="0:2pi"), wrap_0_2pi(theta)) + self.assertAlmostEqual(angle_wrap(theta=-theta, mode="0:2pi"), wrap_0_2pi(-theta)) + self.assertAlmostEqual(angle_wrap(theta=theta, mode="0:pi"), wrap_0_pi(theta)) + self.assertAlmostEqual(angle_wrap(theta=-theta, mode="0:pi"), wrap_0_pi(-theta)) + self.assertAlmostEqual(angle_wrap(theta=theta, mode="-pi/2:pi/2"), wrap_mpi2_pi2(theta)) + self.assertAlmostEqual(angle_wrap(theta=-theta, mode="-pi/2:pi/2"), wrap_mpi2_pi2(-theta)) + with self.assertRaises(ValueError): + angle_wrap(theta=theta, mode="foo") + def test_angle_stats(self): theta = np.linspace(3 * pi / 2, 5 * pi / 2, 50) self.assertAlmostEqual(angle_mean(theta), 0)