diff --git a/spatialmath/base/__init__.py b/spatialmath/base/__init__.py index 9e9fbcbe..2f8e91e6 100644 --- a/spatialmath/base/__init__.py +++ b/spatialmath/base/__init__.py @@ -221,6 +221,7 @@ "tr2jac2", "trinterp2", "trprint2", + "tr2str2", "trplot2", "tranimate2", "xyt2tr", @@ -264,6 +265,7 @@ "exp2jac", "rot2jac", "trprint", + "tr2str", "trplot", "tranimate", "tr2x", diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 364a5ea8..a5031d2d 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -1156,10 +1156,10 @@ def qprint( q: Union[ArrayLike4, ArrayLike4], delim: Optional[Tuple[str, str]] = ("<", ">"), fmt: Optional[str] = "{: .4f}", - file: Optional[TextIO] = sys.stdout, -) -> None: + file: Optional[TextIO] = False, +) -> str: """ - Format a quaternion to a file + Compact single-line display of a quaternion :arg q: unit-quaternion :type q: array_like(4) @@ -1187,15 +1187,26 @@ def qprint( >>> q = qrand() # a unit quaternion >>> qprint(q, delim=('<<', '>>')) + .. deprecated:: 1.1.15 + ``file=None`` to get the string back without printing is + deprecated - call :func:`~q2str` directly instead. + :seealso: :meth:`q2str` """ q = smb.getvector(q, 4) + s = q2str(q, delim=delim, fmt=fmt) if file is None: warnings.warn( "Usage: qprint(..., file=None) -> str is deprecated, use q2str() instead", DeprecationWarning, ) - print(q2str(q, delim=delim, fmt=fmt), file=file) + else: + # file=False (the default) resolves to None here so print() looks + # up the *current* sys.stdout at call time, not whatever it was + # when this function was defined - that's what makes + # contextlib.redirect_stdout() work. + print(s, file=None if file is False else file) + return s if __name__ == "__main__": # pragma: no cover diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index ac0696cd..22f2dcfc 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -17,6 +17,7 @@ import sys import math import numpy as np +import warnings try: import matplotlib.pyplot as plt @@ -959,22 +960,19 @@ def trinterp2(start, end, s, shortest: bool = True): raise ValueError("Argument must be SO(2) or SE(2)") -def trprint2( +def tr2str2( T: Union[SO2Array, SE2Array], label: str = "", - file: TextIO = sys.stdout, fmt: str = "{:.3g}", unit: str = "deg", ) -> str: """ - Compact display of SE(2) or SO(2) matrices + Convert SO(2) or SE(3) matrices to compact single-line string :param T: matrix to format :type T: ndarray(3,3) or ndarray(2,2) :param label: text label to put at start of line :type label: str - :param file: file to write formatted string to - :type file: file object :param fmt: conversion format for each number :type fmt: str :param unit: angular units: 'rad' [default], or 'deg' @@ -985,12 +983,12 @@ def trprint2( The matrix is formatted and written to ``file`` and the string is returned. To suppress writing to a file, set ``file=None``. - - ``trprint2(R)`` displays the SO(2) rotation matrix in a compact + - ``tr2str2(R)`` displays the SO(2) rotation matrix in a compact single-line format and returns the string:: [LABEL:] θ UNIT - - ``trprint2(T)`` displays the SE(2) homogoneous transform in a compact + - ``tr2str2(T)`` displays the SE(2) homogoneous transform in a compact single-line format and returns the string:: [LABEL:] [t=X, Y;] θ UNIT @@ -999,9 +997,8 @@ def trprint2( >>> from spatialmath.base import * >>> T = transl2(1,2) @ trot2(0.3) - >>> trprint2(T, file=None, label='T') - >>> trprint2(T, file=None, label='T', fmt='{:8.4g}') - + >>> tr2str2(T, label='T') + >>> tr2str2(T, label='T', fmt='{:8.4g}') .. note:: @@ -1009,7 +1006,9 @@ def trprint2( - For tabular data set ``fmt`` to a fixed width format such as ``fmt='{:.3g}'`` - :seealso: trprint + .. versionadded:: 1.1.15 + + :seealso: :func:`~tr2str` """ s = "" @@ -1028,8 +1027,6 @@ def trprint2( else: s += " {} rad".format(_vec2s(fmt, [angle])) - if file: - print(s, file=file) return s @@ -1052,6 +1049,76 @@ def _vec2s(fmt: str, v: ArrayLikePure, tol: float = 20) -> str: return ", ".join([fmt.format(x) for x in v]) +def trprint2( + T: Union[SO2Array, SE2Array], + label: str = "", + file: TextIO = False, + **kwargs, +) -> str: + """ + Compact single-line display of SE(2) or SO(2) matrices + + :param T: matrix to format + :type T: ndarray(3,3) or ndarray(2,2) + :param label: text label to put at start of line + :type label: str + :param file: file to write formatted string to [default is stdout] + :type file: file object + :param fmt: conversion format for each number + :type fmt: str + :param unit: angular units: 'rad' [default], or 'deg' + :type unit: str + :return: formatted string + :rtype: str + + The matrix is formatted and written to ``file`` and the + string is returned. To suppress writing to a file, set ``file=None``. + + - ``trprint2(R)`` displays the SO(2) rotation matrix in a compact + single-line format and returns the string:: + + [LABEL:] θ UNIT + + - ``trprint2(T)`` displays the SE(2) homogoneous transform in a compact + single-line format and returns the string:: + + [LABEL:] [t=X, Y;] θ UNIT + + .. runblock:: pycon + + >>> from spatialmath.base import * + >>> T = transl2(1,2) @ trot2(0.3) + >>> trprint2(T, label='T') + >>> trprint2(T, label='T', fmt='{:8.4g}') + + + .. note:: + + - Default formatting is for compact display of data + - For tabular data set ``fmt`` to a fixed width format such as + ``fmt='{:.3g}'`` + + .. deprecated:: 1.1.15 + ``file=None`` to get the string back without printing is + deprecated - call :func:`~tr2str2` directly instead. + + :seealso: :func:`~tr2str2` :func:`~trprint` + """ + s = tr2str2(T, label=label, **kwargs) + if file is None: + warnings.warn( + "Usage: trprint2(..., file=None) -> str is deprecated, use tr2str2() instead", + DeprecationWarning, + ) + else: + # file=False (the default) resolves to None here so print() looks + # up the *current* sys.stdout at call time, not whatever it was + # when this function was defined - that's what makes + # contextlib.redirect_stdout() work. + print(s, file=None if file is False else file) + return s + + def points2tr2(p1: NDArray, p2: NDArray) -> SE2Array: """ SE(2) transform from corresponding points diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 04f9e1b8..dea30077 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -18,6 +18,8 @@ from collections.abc import Iterable import math import numpy as np +import warnings + from spatialmath.base.argcheck import getunit, getvector, isvector, isscalar, ismatrix from spatialmath.base.vectors import ( @@ -2779,50 +2781,43 @@ def rodrigues(w: ArrayLike3, theta: Optional[float] = None) -> SO3Array: ) -def trprint( +def tr2str( T: Union[SO3Array, SE3Array], orient: str = "rpy/zyx", label: str = "", - file: TextIO = sys.stdout, fmt: str = "{:.3g}", degsym: bool = True, unit: str = "deg", ) -> str: """ - Compact display of SO(3) or SE(3) matrices + Convert SO(3) or SE(3) matrices to compact single-line string - :param T: SE(3) or SO(3) matrix - :type T: ndarray(4,4) or ndarray(3,3) - :param label: text label to put at start of line - :type label: str - :param orient: 3-angle convention to use - :type orient: str - :param file: file to write formatted string to. [default, stdout] - :type file: file object - :param fmt: conversion format for each number in the format used with ``format`` - :type fmt: str - :param unit: angular units: 'rad' [default], or 'deg' - :type unit: str - :return: formatted string - :rtype: str - :raises ValueError: bad argument + :param T: SE(3) or SO(3) matrix + :type T: ndarray(4,4) or ndarray(3,3) + :param label: text label to put at start of line + :type label: str + :param orient: 3-angle convention to use + :type orient: str + :param fmt: conversion format for each number in the format used with ``format`` + :type fmt: str + :param unit: angular units: 'rad' [default], or 'deg' + :type unit: str + :return: formatted string + :rtype: str + :raises ValueError: bad argument - The matrix is formatted and written to ``file`` and the - string is returned. To suppress writing to a file, set ``file=None``. + The matrix is formatted and returned as a string. - - ``trprint(R)`` prints the SO(3) rotation matrix to stdout in a compact - single-line format: + - ``tr2str(R)`` converts the SO(3) rotation matrix to a compact + single-line string: [LABEL:] ORIENTATION UNIT - - ``trprint(T)`` prints the SE(3) homogoneous transform to stdout in a - compact single-line format: + - ``tr2str(T)`` prints the SE(3) homogoneous transform to a compact + single-line string: [LABEL:] [t=X, Y, Z;] ORIENTATION UNIT - - ``trprint(X, file=None)`` as above but returns the string rather than - printing to a file - Orientation is expressed in one of several formats: - 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order [default] @@ -2834,11 +2829,11 @@ def trprint( .. runblock:: pycon - >>> from spatialmath.base import transl, rpy2tr, trprint + >>> from spatialmath.base import * >>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg') - >>> trprint(T, file=None) - >>> trprint(T, file=None, label='T', orient='angvec') - >>> trprint(T, file=None, label='T', orient='angvec', fmt='{:8.4g}') + >>> tr2str(T) + >>> tr2str(T, label='T', orient='angvec') + >>> tr2str(T, label='T', orient='angvec', fmt='{:8.4g}') .. note:: @@ -2849,7 +2844,9 @@ def trprint( - For tabular data set ``fmt`` to a fixed width format such as ``fmt='{:.3g}'`` - :seealso: :func:`~spatialmath.base.transforms2d.trprint2` :func:`~tr2eul` :func:`~tr2rpy` :func:`~tr2angvec` + .. versionadded:: 1.1.15 + + :seealso: :func:`~trprint` :func:`~spatialmath.base.transforms2d.trprint2` :func:`~tr2eul` :func:`~tr2rpy` :func:`~tr2angvec` :SymPy: not supported """ @@ -2898,10 +2895,6 @@ def trprint( s += " angvec = ({} | {})".format(theta, _vec2s(fmt, v)) else: raise ValueError("bad orientation format") - - if file: - print(s, file=file) - return s @@ -2910,6 +2903,96 @@ def _vec2s(fmt, v): return ", ".join([fmt.format(x) for x in v]) +def trprint( + T: Union[SO3Array, SE3Array], + orient: str = "rpy/zyx", + label: str = "", + file: TextIO = False, + fmt: str = "{:.3g}", + degsym: bool = True, + unit: str = "deg", +) -> str: + """ + Compact single-line display of SO(3) or SE(3) matrices + + :param T: SE(3) or SO(3) matrix + :type T: ndarray(4,4) or ndarray(3,3) + :param label: text label to put at start of line + :type label: str + :param orient: 3-angle convention to use + :type orient: str + :param file: file to write formatted string to. [default, stdout] + :type file: file object + :param fmt: conversion format for each number in the format used with ``format`` + :type fmt: str + :param unit: angular units: 'rad' [default], or 'deg' + :type unit: str + :return: formatted string + :rtype: str + :raises ValueError: bad argument + + The matrix is formatted and written to ``file``. + + - ``trprint(R)`` prints the SO(3) rotation matrix to stdout in a compact + single-line format: + + [LABEL:] ORIENTATION UNIT + + - ``trprint(T)`` prints the SE(3) homogoneous transform to stdout in a + compact single-line format: + + [LABEL:] [t=X, Y, Z;] ORIENTATION UNIT + + Orientation is expressed in one of several formats: + + - 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order [default] + - 'rpy/yxz' roll-pitch-yaw angles in YXZ axis order + - 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order + - 'eul' Euler angles in ZYZ axis order + - 'angvec' angle and axis + + + .. runblock:: pycon + + >>> from spatialmath.base import * + >>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg') + >>> trprint(T) + >>> trprint(T, label='T', orient='angvec') + >>> trprint(T, label='T', orient='angvec', fmt='{:8.4g}') + + .. note:: + + - If the 'rpy' option is selected, then the particular angle sequence can be + specified with the options 'xyz' or 'yxz' which are passed through to ``tr2rpy``. + 'zyx' is the default. + - Default formatting is for compact display of data + - For tabular data set ``fmt`` to a fixed width format such as + ``fmt='{:.3g}'`` + + .. deprecated:: 1.1.15 + ``file=None`` to get the string back without printing is + deprecated - call :func:`~tr2str` directly instead. + + :seealso: :func:`~tr2str` :func:`~spatialmath.base.transforms2d.trprint2` :func:`~tr2eul` :func:`~tr2rpy` :func:`~tr2angvec` + :SymPy: not supported + """ + s = tr2str(T, orient=orient, label=label, fmt=fmt, degsym=degsym, unit=unit) + if file is None: + # deprecated: return the string without printing, matching the + # original `if file: print(...)` falsy-skip behaviour + warnings.warn( + "Usage: trprint(..., file=None) -> str is deprecated, use tr2str() instead", + DeprecationWarning, + ) + else: + # file=False (the default) resolves to None here so print() looks + # up the *current* sys.stdout at call time, not whatever it was + # when this function was defined - that's what makes + # contextlib.redirect_stdout() work. + print(s, file=None if file is False else file) + return s + + try: import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index 87071df3..e7987d7b 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -768,8 +768,8 @@ def strline(self, *args, **kwargs) -> str: >>> x.strline() >>> x = SE3.Rx([0.2, 0.3]) >>> x.strline() - >>> x.strline('angvec') - >>> x.strline(orient='angvec', fmt="{:.6f}") + >>> x.strline(orient="angvec") + >>> x.strline(orient="angvec", fmt="{:.6f}") >>> x = SE2(1, 2, 0.3) >>> x.strline() @@ -783,10 +783,10 @@ def strline(self, *args, **kwargs) -> str: s = "" if self.N == 2: for x in self.data: - s += smb.trprint2(x, *args, file=False, **kwargs) + s += smb.tr2str2(x, *args, **kwargs) else: for x in self.data: - s += smb.trprint(x, *args, file=False, **kwargs) + s += smb.tr2str(x, *args, **kwargs) return s def __repr__(self) -> str: diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 2994b6e6..30e32a2a 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -922,6 +922,24 @@ def __str__(self) -> str: delim = ("<", ">") return "\n".join([smb.q2str(q, delim=delim) for q in self.data]) + def printline(self): + """Print quaternion in compact single-line format (superclass method) + + Example: + + .. runblock:: pycon + + >>> from spatialmath import Quaternion, UnitQuaternion + >>> q = Quaternion([1,2,3,4]) + >>> q.printline() + >>> q = UnitQuaternion.Rx(0.3) + >>> q.printline() + >>> q = UnitQuaternion.Rx([0.1, 0.2, 0.3]) + >>> q.printline() + + """ + print(self) + # ========================================================================= #