From bb1f6f73d31e235a10ece8b6ad4c16bc2009a50a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 2 Mar 2025 15:35:01 +1000 Subject: [PATCH 1/2] Consistent operation of `printline()` method for SO2, SE2, SO3, SE3, Quaternion, UnitQuaternion types. In `base` now have separate functions to convert type to a single line string or to print it. Fixed problem where stdout redirection failed using `contextlib.redirect_stdout`. --- spatialmath/base/__init__.py | 2 + spatialmath/base/quaternions.py | 13 ++- spatialmath/base/transforms2d.py | 86 ++++++++++++++++--- spatialmath/base/transforms3d.py | 140 +++++++++++++++++++++++-------- spatialmath/baseposematrix.py | 8 +- spatialmath/quaternion.py | 18 ++++ 6 files changed, 208 insertions(+), 59 deletions(-) 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..a788a742 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -1152,14 +1152,9 @@ def q2str( return template.format(q[0], delim[0], q[1], q[2], q[3], delim[1]) -def qprint( - q: Union[ArrayLike4, ArrayLike4], - delim: Optional[Tuple[str, str]] = ("<", ">"), - fmt: Optional[str] = "{: .4f}", - file: Optional[TextIO] = sys.stdout, -) -> None: +def qprint(q: Union[ArrayLike4, ArrayLike4], file=False, **kwargs) -> None: """ - Format a quaternion to a file + Compact single-line display of a quaternion :arg q: unit-quaternion :type q: array_like(4) @@ -1195,7 +1190,9 @@ def qprint( "Usage: qprint(..., file=None) -> str is deprecated, use q2str() instead", DeprecationWarning, ) - print(q2str(q, delim=delim, fmt=fmt), file=file) + if file is False: + file = None # defaults to stdout + print(q2str(q, **kwargs), file=file) if __name__ == "__main__": # pragma: no cover diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index ac0696cd..8daa9d4c 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,15 +960,15 @@ 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, + file: TextIO = None, 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) @@ -985,12 +986,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 +1000,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 +1009,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 +1030,6 @@ def trprint2( else: s += " {} rad".format(_vec2s(fmt, [angle])) - if file: - print(s, file=file) return s @@ -1052,6 +1052,70 @@ 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}'`` + + .. versionchanged:: 1.1.15 + To create a string use :func:`~tr2str2` instead of ``trprint2(...file=None)`` + + :seealso: :func:`~tr2str2` :func:`~trprint` + """ + if file is None: + warnings.warn( + "Usage: trprint2(..., file=None) -> str is deprecated, use tr2str2() instead", + DeprecationWarning, + ) + if file is False: + file = None # defaults to stdout + print(tr2str2(T, **kwargs), file=file) + + 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..24098d22 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,44 @@ 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, + file: TextIO = None, 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 +2830,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 +2845,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 +2896,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 +2904,80 @@ def _vec2s(fmt, v): return ", ".join([fmt.format(x) for x in v]) +def trprint(T: Union[SO3Array, SE3Array], file=False, **kwargs) -> 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}'`` + + .. versionchanged:: 1.1.15 + To create a string use :func:`~tr2str` instead of ``trprint(...file=None)`` + + :seealso: :func:`~tr2str` :func:`~spatialmath.base.transforms2d.trprint2` :func:`~tr2eul` :func:`~tr2rpy` :func:`~tr2angvec` + :SymPy: not supported + """ + if file is None: + warnings.warn( + "Usage: trprint(..., file=None) -> str is deprecated, use tr2str() instead", + DeprecationWarning, + ) + if file is False: + file = None # defaults to stdout + print(tr2str(T, **kwargs), file=file) + + 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) + # ========================================================================= # From 8d18e5d04f88504a11ceacb535fd6e4c484c8c50 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 28 Jul 2026 08:14:15 +1000 Subject: [PATCH 2/2] fix(base): fix bugs found in the printline/trprint/qprint rewrite Fixes discovered while packaging up old WIP work (commit 9dc38f9, "Consistent operation of printline()...") for review. Three separate bugs, all in that same unreleased WIP, none of it previously merged or published: 1. trprint()/qprint() silently shifted `file` into the 2nd positional argument slot (`def trprint(T, file=False, **kwargs)`), when the original, published, external-user-facing signatures had `orient`/ `delim` there instead. `trprint(T, 'angvec')` or the library's own documented `x.printline('angvec')` example put 'angvec' into `file` instead of `orient`, crashing with `AttributeError: 'str' object has no attribute 'write'`. Fixed by restoring the full original explicit parameter order for trprint/qprint (trprint2 was already fine here - its parameter order happened to be unaffected). 2. trprint2() captured `label` as an explicit parameter but its body never forwarded it to tr2str2() - `trprint2(T, label='T')` silently dropped the label from the output. 3. All three (trprint/trprint2/qprint) stopped returning the compact string entirely, only printing it - breaking any caller using the deprecated-but-still-supported `file=None` -> return string instead of printing usage, which several existing tests rely on (test_transforms3d.py::test_print, test_transforms2d.py::test_print2). While fixing (3): the `file=None` "give me a string instead" pattern is a wart worth phasing out rather than perpetuating silently - each of these now has a real *2str()/q2str() sibling that does that directly. So `file=None` is kept working (still returns the string, still doesn't print) but now consistently raises a DeprecationWarning and says so explicitly in the docstring via `.. deprecated::`, pointing at tr2str()/tr2str2()/q2str() as the replacement. All three now follow one consistent shape: always build the string via the *2str()/q2str() sibling, always return it, print only when file is not None (file=False, the default, resolves to None passed to print() so contextlib.redirect_stdout() sees the current sys.stdout at call time rather than whatever was captured when the function was defined - the actual bug this WIP originally set out to fix). Verified: file=None still returns the correct string with a DeprecationWarning and no printing; the default path both prints (confirmed via contextlib.redirect_stdout) and returns the same string; full test suite green (338 passed, 4 skipped); black --check clean at 23.10.0. --- spatialmath/base/quaternions.py | 22 ++++++++++++++++++---- spatialmath/base/transforms2d.py | 19 +++++++++++-------- spatialmath/base/transforms3d.py | 29 ++++++++++++++++++++++------- 3 files changed, 51 insertions(+), 19 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index a788a742..a5031d2d 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -1152,7 +1152,12 @@ def q2str( return template.format(q[0], delim[0], q[1], q[2], q[3], delim[1]) -def qprint(q: Union[ArrayLike4, ArrayLike4], file=False, **kwargs) -> None: +def qprint( + q: Union[ArrayLike4, ArrayLike4], + delim: Optional[Tuple[str, str]] = ("<", ">"), + fmt: Optional[str] = "{: .4f}", + file: Optional[TextIO] = False, +) -> str: """ Compact single-line display of a quaternion @@ -1182,17 +1187,26 @@ def qprint(q: Union[ArrayLike4, ArrayLike4], file=False, **kwargs) -> None: >>> 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, ) - if file is False: - file = None # defaults to stdout - print(q2str(q, **kwargs), 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 8daa9d4c..22f2dcfc 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -963,7 +963,6 @@ def trinterp2(start, end, s, shortest: bool = True): def tr2str2( T: Union[SO2Array, SE2Array], label: str = "", - file: TextIO = None, fmt: str = "{:.3g}", unit: str = "deg", ) -> str: @@ -974,8 +973,6 @@ def tr2str2( :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' @@ -1101,19 +1098,25 @@ def trprint2( - For tabular data set ``fmt`` to a fixed width format such as ``fmt='{:.3g}'`` - .. versionchanged:: 1.1.15 - To create a string use :func:`~tr2str2` instead of ``trprint2(...file=None)`` + .. 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, ) - if file is False: - file = None # defaults to stdout - print(tr2str2(T, **kwargs), 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 def points2tr2(p1: NDArray, p2: NDArray) -> SE2Array: diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 24098d22..dea30077 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -2785,7 +2785,6 @@ def tr2str( T: Union[SO3Array, SE3Array], orient: str = "rpy/zyx", label: str = "", - file: TextIO = None, fmt: str = "{:.3g}", degsym: bool = True, unit: str = "deg", @@ -2904,7 +2903,15 @@ def _vec2s(fmt, v): return ", ".join([fmt.format(x) for x in v]) -def trprint(T: Union[SO3Array, SE3Array], file=False, **kwargs) -> str: +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 @@ -2962,20 +2969,28 @@ def trprint(T: Union[SO3Array, SE3Array], file=False, **kwargs) -> str: - For tabular data set ``fmt`` to a fixed width format such as ``fmt='{:.3g}'`` - .. versionchanged:: 1.1.15 - To create a string use :func:`~tr2str` instead of ``trprint(...file=None)`` + .. 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, ) - if file is False: - file = None # defaults to stdout - print(tr2str(T, **kwargs), 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 try: