diff --git a/README.md b/README.md index ccd4747f..02421c30 100644 --- a/README.md +++ b/README.md @@ -137,6 +137,8 @@ git clone https://github.com/rai-opensource/spatialmath-python.git cd spatialmath-python pip install -e . # Optional: if you would like to contribute and commit code changes to the repository, +# install the development tools (including pre-commit) and set up the git hooks +# pip install -e .[dev] # pre-commit install ``` diff --git a/pyproject.toml b/pyproject.toml index d4e564ef..8e052175 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -40,7 +40,6 @@ dependencies = [ "matplotlib", "ansitable", "typing_extensions>=4.0.0", - "pre-commit", ] [project.urls] @@ -52,6 +51,7 @@ dependencies = [ [project.optional-dependencies] dev = [ + "pre-commit", "sympy", "pytest", "pytest-timeout", diff --git a/spatialmath/base/__init__.py b/spatialmath/base/__init__.py index b01e14ba..88f7bec2 100644 --- a/spatialmath/base/__init__.py +++ b/spatialmath/base/__init__.py @@ -42,6 +42,7 @@ "plot_cone": "graphics", "plot_cuboid": "graphics", "axes_logic": "graphics", + "set_window_title": "graphics", "plotvol2": "graphics", "plotvol3": "graphics", "expand_dims": "graphics", @@ -391,6 +392,7 @@ def __getattr__(name): "plot_cone", "plot_cuboid", "axes_logic", + "set_window_title", "expand_dims", "isnotebook", # spatial.base.numeric diff --git a/spatialmath/base/argcheck.py b/spatialmath/base/argcheck.py index 742598aa..01b20de0 100644 --- a/spatialmath/base/argcheck.py +++ b/spatialmath/base/argcheck.py @@ -16,10 +16,10 @@ from collections.abc import Iterable # from spatialmath.base import symbolic as sym # HACK -from spatialmath.base.symbolic import issymbol, symtype +from spatialmath.base.symbolic import issymbol # valid scalar types -_scalartypes = (int, np.integer, float, np.floating) + symtype +_scalartypes = (int, np.integer, float, np.floating) # from typing import Union, List, Tuple, Any, Optional, Type, Callable # from numpy.typing import DTypeLike @@ -47,7 +47,9 @@ def isscalar(x: Any) -> bool: >>> isscalar([1]) """ - return isinstance(x, _scalartypes) + return isinstance(x, _scalartypes) or ( + not isinstance(x, (list, tuple)) and issymbol(x) + ) def isinteger(x: Any) -> bool: @@ -386,7 +388,7 @@ def getvector( """ dt = dtype - if isinstance(v, _scalartypes): # handle scalar case + if isscalar(v): # handle scalar case v = [v] # type: ignore if isinstance(v, (list, tuple)): # list or tuple was passed in @@ -501,7 +503,7 @@ def isvector(v: Any, dim: Optional[int] = None) -> bool: if ( isinstance(v, (list, tuple)) and (dim is None or len(v) == dim) - and all(map(lambda x: isinstance(x, _scalartypes), v)) + and all(map(isscalar, v)) ): return True # list or tuple @@ -516,7 +518,7 @@ def isvector(v: Any, dim: Optional[int] = None) -> bool: else: return s == (dim,) or s == (1, dim) or s == (dim, 1) - if (dim is None or dim == 1) and isinstance(v, _scalartypes): + if (dim is None or dim == 1) and isscalar(v): return True return False @@ -620,11 +622,7 @@ def isnumberlist(x: Any) -> bool: >>> isnumberlist(np.r_[1,2]) """ - return ( - isinstance(x, (list, tuple)) - and len(x) > 0 - and all(map(lambda x: isinstance(x, _scalartypes), x)) - ) + return isinstance(x, (list, tuple)) and len(x) > 0 and all(map(isscalar, x)) def isvectorlist(x: Any, n: int) -> bool: diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index 40503c41..0d3344f9 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1580,6 +1580,7 @@ def axes_logic( """ # print(f"new axis logic ({dimensions}D): ", end='') + use_current_figure = False if ax is None: # no axes passed in, find out what's happening # need to be careful to not use gcf() or gca() since they @@ -1597,6 +1598,10 @@ def axes_logic( # print(f"ax has {_axes_dimensions(ax)} dimensions") if _axes_dimensions(ax) == dimensions and not new: return ax + elif not new: + # the current figure is empty, eg. from a bare plt.figure(): + # put the axes in it rather than leave it empty and make another + use_current_figure = True # otherwise it doesnt exist or dimension mismatch, create new axes else: # print("ax given", ax) @@ -1607,7 +1612,8 @@ def axes_logic( return ax # print("mismatch in dimensions, create new axes") # print("create new axes") - plt.figure() + if not use_current_figure: + plt.figure() if dimensions == 2: ax = plt.axes() if autoscale: @@ -1759,11 +1765,18 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: * A -> [-A, A, -A, A, -A, A] * [A,B] -> [A, B, A, B, A, B] * [A,B,C,D,E,F] -> [A, B, C, D, E, F] + + A scalar is symmetric about zero, so its sign is ignored: ``-A`` is the + same as ``A``. For the other forms the values are returned as given and + no ordering check is made. If a minimum is greater than the + corresponding maximum, matplotlib will invert that axis when the limits + are applied, which is a convenient way to flip an axis direction. """ dim = smb.getvector(dim) if nd == 2: if len(dim) == 1: + dim = abs(dim) return np.r_[-dim, dim, -dim, dim] elif len(dim) == 2: return np.r_[dim[0], dim[1], dim[0], dim[1]] @@ -1773,6 +1786,7 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: raise ValueError("bad dimension specified") elif nd == 3: if len(dim) == 1: + dim = abs(dim) return np.r_[-dim, dim, -dim, dim, -dim, dim] elif len(dim) == 2: return np.r_[dim[0], dim[1], dim[0], dim[1], dim[0], dim[1]] @@ -1783,6 +1797,37 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: else: raise ValueError("nd is 2 or 3") + def set_window_title(fig: plt.Figure, title: str) -> bool: + """ + Set the title of the window containing a figure, if it has one + + :param fig: the figure + :type fig: Figure + :param title: the window title + :type title: str + :return: True if the title was set + :rtype: bool + + A figure has a window only if it is managed by pyplot and the backend + provides one. A figure on a headless backend such as ``Agg``, or one + created directly with ``matplotlib.figure.Figure``, does not, and then + nothing is done and False is returned. + + Example:: + + >>> from spatialmath.base import plotvol2, set_window_title + >>> ax = plotvol2(5) + >>> set_window_title(ax.figure, "my plot") + + .. note:: The title of the window is not the title of the axes, see + :meth:`matplotlib.axes.Axes.set_title`. + """ + manager = getattr(fig.canvas, "manager", None) + if manager is not None and hasattr(manager, "set_window_title"): + manager.set_window_title(title) + return True + return False + def isnotebook() -> bool: """ Determine if code is being run from a Jupyter notebook @@ -1849,3 +1894,6 @@ def plot_cone(*args, **kwargs) -> None: def plot_cylinder(*args, **kwargs) -> None: raise NotImplementedError("Matplotlib is not installed: pip install matplotlib") + + def set_window_title(*args, **kwargs) -> None: + raise NotImplementedError("Matplotlib is not installed: pip install matplotlib") diff --git a/spatialmath/base/symbolic.py b/spatialmath/base/symbolic.py index aebb827a..2031b3c1 100644 --- a/spatialmath/base/symbolic.py +++ b/spatialmath/base/symbolic.py @@ -11,61 +11,67 @@ supported. """ +from __future__ import annotations + import math +from typing import TYPE_CHECKING from spatialmath.base.types import * -try: # pragma: no cover - # print('Using SymPy') - import sympy +import importlib as _importlib +import sys as _sys - _symbolics = True - symtype = (sympy.Expr,) +if TYPE_CHECKING: from sympy import Symbol -except ImportError: # pragma: no cover - # SymPy is not installed - _symbolics = False - symtype = () - Symbol = Any +def _sympy(): + """Load the optional dependency only for a symbolic operation.""" + return _importlib.import_module("sympy") -# ---------------------------------------------------------------------------------------# -if _symbolics: +def __getattr__(name): + """Keep explicit access to the SymPy type aliases lazy.""" + if name == "Symbol": + return _sympy().Symbol + if name == "symtype": + try: + return (_sympy().Expr,) + except ImportError: + return () + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") - def symbol( - name: str, real: Optional[bool] = True - ) -> Union[Symbol, Tuple[Symbol, ...]]: - """ - Create symbolic variables - :param name: symbol names - :type name: str - :param real: assume variable is real, defaults to True - :type real: bool, optional - :return: SymPy symbols - :rtype: sympy +def symbol(name: str, real: Optional[bool] = True) -> Union[Symbol, Tuple[Symbol, ...]]: + """ + Create symbolic variables - .. runblock:: pycon + :param name: symbol names + :type name: str + :param real: assume variable is real, defaults to True + :type real: bool, optional + :return: SymPy symbols + :rtype: sympy - >>> from spatialmath.base.symbolic import * - >>> theta = symbol('theta') - >>> theta - >>> theta, psi = symbol('theta psi') - >>> theta - >>> psi - >>> q = symbol('q_:6') - >>> q + .. runblock:: pycon + + >>> from spatialmath.base.symbolic import * + >>> theta = symbol('theta') + >>> theta + >>> theta, psi = symbol('theta psi') + >>> theta + >>> psi + >>> q = symbol('q_:6') + >>> q - .. note:: In Jupyter symbols are pretty printed. + .. note:: In Jupyter symbols are pretty printed. - - symbols named after greek letters will appear as greek letters - - underscore means subscript as it does in LaTex, so the symbols ``q`` - above will be subscripted. + - symbols named after greek letters will appear as greek letters + - underscore means subscript as it does in LaTex, so the symbols ``q`` + above will be subscripted. - :seealso: :func:`sympy.symbols` - """ - return sympy.symbols(name, real=real) + :seealso: :func:`sympy.symbols` + """ + return _sympy().symbols(name, real=real) def issymbol(var: Any) -> bool: @@ -84,13 +90,12 @@ def issymbol(var: Any) -> bool: >>> issymbol(3.4) """ - if _symbolics: - if isinstance(var, (list, tuple)): - return any([isinstance(x, symtype) for x in var]) - else: - return isinstance(var, symtype) - else: + sympy = _sys.modules.get("sympy") + if sympy is None: return False + if isinstance(var, (list, tuple)): + return any(isinstance(x, sympy.Expr) for x in var) + return isinstance(var, sympy.Expr) @overload @@ -122,7 +127,7 @@ def sin(theta): :seealso: :func:`sympy.sin` """ if issymbol(theta): - return sympy.sin(theta) + return _sympy().sin(theta) else: return math.sin(theta) @@ -156,7 +161,7 @@ def cos(theta): :seealso: :func:`sympy.cos` """ if issymbol(theta): - return sympy.cos(theta) + return _sympy().cos(theta) else: return math.cos(theta) @@ -190,7 +195,7 @@ def tan(theta): :seealso: :func:`sympy.cos` """ if issymbol(theta): - return sympy.tan(theta) + return _sympy().tan(theta) else: return math.tan(theta) @@ -224,7 +229,7 @@ def sqrt(v): :seealso: :func:`sympy.sqrt` """ if issymbol(v): - return sympy.sqrt(v) + return _sympy().sqrt(v) else: return math.sqrt(v) @@ -245,7 +250,7 @@ def zero() -> Symbol: :seealso: :func:`sympy.S.Zero` """ - return sympy.S.Zero + return _sympy().S.Zero def one() -> Symbol: @@ -264,7 +269,7 @@ def one() -> Symbol: :seealso: :func:`sympy.S.One` """ - return sympy.S.One + return _sympy().S.One def negative_one() -> Symbol: @@ -283,26 +288,27 @@ def negative_one() -> Symbol: :seealso: :func:`sympy.S.NegativeOne` """ - return sympy.S.NegativeOne + return _sympy().S.NegativeOne def pi() -> Symbol: """ - Symbolic constant: pi + Symbolic constant: pi - :return: π - :rtype: symbolic + :return: π + :rtype: symbolic - .. runblock:: pycon + .. runblock:: pycon - >>> from spatialmath.base.symbolic import * - >>> import math - >>> sin(pi()) - >>> sin(math.pi) + >>> from spatialmath.base.symbolic import * + >>> import math + from typing import TYPE_CHECKING + >>> sin(pi()) + >>> sin(math.pi) - :seealso: :func:`sympy.S.Pi` + :seealso: :func:`sympy.S.Pi` """ - return sympy.S.Pi + return _sympy().S.Pi def simplify(x: Symbol) -> Symbol: @@ -324,10 +330,11 @@ def simplify(x: Symbol) -> Symbol: :seealso: :func:`sympy.simplify` """ - if _symbolics: - return sympy.simplify(x) - else: + try: + sympy = _sympy() + except ImportError: return x + return sympy.simplify(x) def det(x): @@ -352,4 +359,4 @@ def det(x): .. note:: Converts to a SymPy ``Matrix`` and then back again. """ - return sympy.Matrix(x).det() + return _sympy().Matrix(x).det() diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index efe46a59..c36b9ffc 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -39,15 +39,6 @@ _eps = np.finfo(np.float64).eps -try: # pragma: no cover - # print('Using SymPy') - import sympy - - _symbolics = True - -except ImportError: # pragma: no cover - _symbolics = False - # ---------------------------------------------------------------------------------------# def rot2(theta: float, unit: str = "rad") -> SO2Array: @@ -170,8 +161,13 @@ def tr2xyt(T: SE2Array, unit: str = "rad") -> R3: :seealso: trot2 """ - if T.dtype == "O" and _symbolics: - angle = sympy.atan2(T[1, 0], T[0, 0]) + if T.dtype == "O": + try: + import sympy + except ImportError: + angle = math.atan2(T[1, 0], T[0, 0]) + else: + angle = sympy.atan2(T[1, 0], T[0, 0]) else: angle = math.atan2(T[1, 0], T[0, 0]) return np.r_[T[0, 2], T[1, 2], angle] @@ -790,15 +786,15 @@ def tr2adjoint2(T): Computes an adjoint matrix that maps the Lie algebra between frames. - .. math: + .. math:: - Ad(\mat{T}) \vec{X} X = \vee \left( \mat{T} \skew{\vec{X} \mat{T}^{-1} \right) + \mathrm{Ad}(\mat{T})\,\vec{X} = \vex{\mat{T}\,\sk{\vec{X}}\,\mat{T}^{-1}} - where :math:`\mat{T} \in \SE2`. + where :math:`\mat{T} \in \SE{2}`. - ``tr2jac2(T)`` is an adjoint matrix (6x6) that maps spatial velocity or - differential motion between frame {B} to frame {A} which are attached to the - same moving body. The pose of {B} relative to {A} is represented by the + ``tr2adjoint2(T)`` is an adjoint matrix (3x3 for SE(2), 1x1 for SO(2)) that + maps spatial velocity or differential motion between frame {B} to frame {A} + which are attached to the same moving body. The pose of {B} relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`. .. runblock:: pycon diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 95786b9c..5c90b636 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -31,6 +31,7 @@ unitvec_norm, norm, isunitvec, + iszero, iszerovec, unittwist_norm, isunittwist, @@ -747,7 +748,7 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr :type: float :return: SO(3) rotation matrix :rtype: ndarray(3,3) - :raises ValueError: bad arguments + :raises ValueError: bad arguments, or a non-zero rotation about a zero-length axis ``angvec2r(θ, V)`` is an SO(3) orthonormal rotation matrix equivalent to a rotation of ``θ`` about the vector ``V``. @@ -760,8 +761,10 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr .. note:: - - If ``θ == 0`` then return identity matrix. - - If ``θ ~= 0`` then ``V`` must have a finite length. + - If ``θ`` is zero (within ``tol``) then return the identity matrix, and + ``V`` is ignored. + - Otherwise ``V`` must have a finite length, but need not be a unit + vector. A zero-length ``V`` raises ``ValueError``. :seealso: :func:`~angvec2tr` :func:`~tr2angvec` @@ -770,11 +773,11 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr if not isscalar(theta) or not isvector(v, 3): raise ValueError("Arguments must be angle and vector") - if np.linalg.norm(v) < tol * _eps: - return np.eye(3) - θ = getunit(theta, unit, vector=False) + if iszero(θ, tol): + return np.eye(3) + # Rodrigue's equation sk = skew(cast(ArrayLike3, unitvec(v))) @@ -806,8 +809,10 @@ def angvec2tr(theta: float, v: ArrayLike3, unit="rad") -> SE3Array: .. note:: - - If ``θ == 0`` then return identity matrix. - - If ``θ ~= 0`` then ``V`` must have a finite length. + - If ``θ`` is zero (within ``tol``) then return the identity matrix, and + ``V`` is ignored. + - Otherwise ``V`` must have a finite length, but need not be a unit + vector. A zero-length ``V`` raises ``ValueError``. - The translational part is zero. :seealso: :func:`~angvec2r` :func:`~tr2angvec` @@ -2732,15 +2737,15 @@ def tr2adjoint(T): Computes an adjoint matrix that maps the Lie algebra between frames. - .. math: + .. math:: - Ad(\mat{T}) \vec{X} X = \vee \left( \mat{T} \skew{\vec{X} \mat{T}^{-1} \right) + \mathrm{Ad}(\mat{T})\,\vec{X} = \vex{\mat{T}\,\sk{\vec{X}}\,\mat{T}^{-1}} - where :math:`\mat{T} \in \SE3`. + where :math:`\mat{T} \in \SE{3}`. - ``tr2jac(T)`` is an adjoint matrix (6x6) that maps spatial velocity or - differential motion between frame {B} to frame {A} which are attached to the - same moving body. The pose of {B} relative to {A} is represented by the + ``tr2adjoint(T)`` is an adjoint matrix (6x6 for SE(3), 3x3 for SO(3)) that + maps spatial velocity or differential motion between frame {B} to frame {A} + which are attached to the same moving body. The pose of {B} relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`. .. runblock:: pycon diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 8dbe7596..c632e3e6 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -22,14 +22,7 @@ # from spatialmath.base.transforms2d import transl2 -try: # pragma: no cover - # print('Using SymPy') - from sympy import Matrix - - _symbolics = True - -except ImportError: # pragma: no cover - _symbolics = False +from spatialmath.base import symbolic as sym _eps = np.finfo(np.float64).eps @@ -923,10 +916,12 @@ def det(m: np.ndarray) -> float: :SymPy: supported """ - if m.dtype.kind == "O" and _symbolics: - return Matrix(m).det() - else: - return np.linalg.det(m) + if m.dtype.kind == "O": + try: + return sym.det(m) + except ImportError: + pass + return np.linalg.det(m) if __name__ == "__main__": # pragma: no cover diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index 59c61797..d8267737 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -16,14 +16,7 @@ from spatialmath.base.argcheck import getvector from spatialmath.base.types import * -try: # pragma: no cover - # print('Using SymPy') - import sympy - - _symbolics = True - -except ImportError: # pragma: no cover - _symbolics = False +from spatialmath.base import symbolic as sym _eps = np.finfo(np.float64).eps @@ -55,8 +48,8 @@ def norm(v: ArrayLikePure) -> float: for x in v: sum += x * x - if _symbolics and isinstance(sum, sympy.Expr): - return sympy.sqrt(sum) + if sym.issymbol(sum): + return sym.sqrt(sum) else: return math.sqrt(sum) @@ -683,14 +676,28 @@ def angle_std(theta: ArrayLike) -> float: r""" Standard deviation of angular values - :param theta: angular values + :param theta: angular values, in radians :type theta: array_like - :return: circular standard deviation + :return: circular standard deviation, in radians :rtype: float + The circular standard deviation is + .. math:: - \sigma_{\theta} = \sqrt{-2 \log \| \left[ \frac{\sum \sin \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \|} \in [0, \infty) + \sigma_{\theta} = \sqrt{-2 \log R} \in [0, \infty), \quad + R = \left\| \left[ \frac{\sum \cos \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \right\| + + where :math:`R` is the length of the mean of the unit vectors at angles + :math:`\theta_i`. :math:`R = 1` if all the angles are equal, giving a + standard deviation of zero, and :math:`R \to 0` as the angles spread evenly + around the circle, when the standard deviation grows without bound. + + This is the same quantity as ``scipy.stats.circstd`` with its default + arguments. + + .. note:: If :math:`R` is exactly zero the result is ``inf``, with a NumPy + divide-by-zero warning. :seealso: :func:`angle_mean` """ @@ -698,23 +705,33 @@ def angle_std(theta: ArrayLike) -> float: Y = np.sin(theta).mean() R = np.sqrt(X**2 + Y**2) - return np.sqrt(-2 * np.log(R)) + # R is the length of an average of unit vectors so is at most 1, but + # rounding can make it 1 + eps, which would give the square root of a + # (tiny) negative number and nan + return np.sqrt(max(0.0, -2 * np.log(R))) def angle_mean(theta: ArrayLike) -> float: r""" Mean of angular values - :param theta: angular values - :type v: array_like - :return: circular mean + :param theta: angular values, in radians + :type theta: array_like + :return: circular mean, in radians :rtype: float - The circular mean is given by + The circular mean is the angle of the mean of the unit vectors at angles + :math:`\theta_i` .. math:: - \bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi)] + \bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi] + + computed with the two-argument arctangent so that the quadrant is correct. + + .. note:: If the unit vectors cancel, for example the angles :math:`0` and + :math:`\pi`, the mean direction is undefined and the result is decided by + rounding error. :seealso: :func:`angle_std` """ @@ -727,8 +744,8 @@ def angle_wrap(theta: ArrayLike, mode: str = "-pi:pi") -> Union[float, NDArray]: """ Generalized angle-wrapping - :param v: angles to wrap - :type v: array_like + :param theta: angles to wrap + :type theta: array_like :param mode: wrapping mode, one of: ``"0:2pi"``, ``"0:pi"``, ``"-pi/2:pi/2"`` or ``"-pi:pi"`` [default] :type mode: str, optional :return: wrapped angles diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index ceb1fc91..f663c1aa 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -618,7 +618,7 @@ def stack(self) -> NDArray: return np.dstack(self.data) def conjugation(self, A: NDArray) -> NDArray: - """ + r""" Matrix conjugation :param A: matrix to conjugate diff --git a/spatialmath/pose2d.py b/spatialmath/pose2d.py index 62ac8510..f919ddff 100644 --- a/spatialmath/pose2d.py +++ b/spatialmath/pose2d.py @@ -609,6 +609,32 @@ def inv(self): else: return SE2([smb.rt2tr(x.R.T, -x.R.T @ x.t) for x in self], check=False) + def Ad(self): + r""" + Adjoint of SE(2) + + :return: adjoint matrix + :rtype: ndarray(3,3) + + ``SE2.Ad`` is the 3x3 adjoint matrix + + If the twist or spatial velocity :math:`\nu = (v_x, v_y, \omega)^T` + and the SE(2) represents the pose of {B} relative to {A}, + ie. :math:`{}^A {\bf T}_B`, and the adjoint is :math:`\mathbf{A}` then + :math:`{}^{A}\!\nu = \mathbf{A} {}^{B}\!\nu`. + + Example: + + .. runblock:: pycon + + >>> from spatialmath import SE2 + >>> SE2(1, 2, 0.3).Ad() + + :seealso: :meth:`spatialmath.pose3d.SE3.Ad`, :meth:`spatialmath.twist.Twist2.ad`, + :func:`~spatialmath.base.transforms2d.tr2adjoint2` + """ + return smb.tr2adjoint2(self.A) + def SE3(self, z=0): """ Create SE(3) from SE(2) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 29d68de6..a955241e 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1483,7 +1483,7 @@ def RPY( @classmethod def OA(cls, o: ArrayLike3, a: ArrayLike3) -> UnitQuaternion: - """ + r""" Construct a new unit quaternion from two vectors :param o: 3-vector parallel to Y- axis @@ -1544,13 +1544,17 @@ def AngVec( >>> print(UQ.AngVec(0, [1,0,0])) >>> print(UQ.AngVec(90, [1,0,0], unit='deg')) - .. note:: :math:`\theta = 0` the result in an identity quaternion, otherwise - ``V`` must have a finite length, ie. :math:`|V| > 0`. + .. note:: If :math:`\theta` is zero within the default tolerance of + :func:`~spatialmath.base.vectors.iszero`, the result is an identity + quaternion. Otherwise, ``v`` must have a nonzero norm. :seealso: :meth:`UnitQuaternion.angvec` :meth:`UnitQuaternion.exp` :func:`~spatialmath.base.transforms3d.angvec2r` """ v = smb.getvector(v, 3) theta = smb.getunit(theta, unit, vector=False) + if smb.iszero(theta): + return cls() + v = smb.unitvec(v) return cls( s=math.cos(theta / 2), v=math.sin(theta / 2) * v, norm=False, check=False ) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 4579ec29..0645aef3 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -986,7 +986,7 @@ def pole(self): >>> S = Twist3(T) >>> S.pole """ - return np.cross(self.w, self.v) / self.theta + return np.cross(self.w, self.v) / np.dot(self.w, self.w) def SE3(self, theta=1, unit="rad"): """ @@ -1014,7 +1014,7 @@ def SE3(self, theta=1, unit="rad"): if len(theta) == 1: # theta is a scalar - return SE3(smb.trexp(self.S * theta)) + return SE3([smb.trexp(S * theta[0]) for S in self.data]) else: # theta is a vector if len(self) == 1: @@ -1072,6 +1072,8 @@ def exp(self, theta=1, unit="rad"): if len(self) == 1: return SE3([smb.trexp(self.S * t) for t in theta], check=False) + elif len(theta) == 1: + return SE3([smb.trexp(s * theta[0]) for s in self.data], check=False) elif len(self) == len(theta): return SE3([smb.trexp(s * t) for s, t in zip(self.S, theta)], check=False) else: @@ -1500,7 +1502,7 @@ def pole(self): >>> S.pole """ - p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta + p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.w**2 return p[:2] # ------------------------- methods -------------------------------# @@ -1530,15 +1532,19 @@ def SE2(self, theta=1, unit="rad"): """ from spatialmath.pose2d import SE2 - if unit != "rad" and self.isprismatic: + if unit != "rad" and any(s.isprismatic for s in self): print("Twist3.exp: using degree mode for a prismatic twist") theta = smb.getunit(theta, unit) if len(theta) == 1: - return SE2(smb.trexp2(self.S * theta)) - else: + return SE2([smb.trexp2(S * theta[0]) for S in self.data]) + elif len(self) == 1: return SE2([smb.trexp2(self.S * t) for t in theta]) + elif len(self) == len(theta): + return SE2([smb.trexp2(S * t) for S, t in zip(self.data, theta)]) + else: + raise ValueError("length of twist and theta not consistent") def skewa(self): """ @@ -1605,6 +1611,8 @@ def exp(self, theta=1, unit="rad"): if len(self) == 1: return SE2([smb.trexp2(self.S * t) for t in theta], check=False) + elif len(theta) == 1: + return SE2([smb.trexp2(s * theta[0]) for s in self.data], check=False) elif len(self) == len(theta): return SE2([smb.trexp2(s * t) for s, t in zip(self.S, theta)], check=False) else: @@ -1633,13 +1641,26 @@ def unit(self): # general twist: normalize so |w| == 1 (w is a scalar for Twist2) return Twist2(self.S / abs(self.w)) - @property def ad(self): - """ - Twist2.ad Logarithm of adjoint + r""" + Logarithm of adjoint of 2D twist + + :return: logarithm of adjoint matrix + :rtype: ndarray(3,3), or list of ndarray if the twist is a sequence + + ``S.ad()`` is the 3x3 matrix representation of the Lie bracket with + the twist ``S``, such that for twists ``S`` and ``T`` + + .. math:: + + \mathrm{ad}(S) \vec{T} = \vee \left( [S] [T] - [T] [S] \right) - - ``S.ad()`` is the logarithm of the adjoint matrix of the corresponding - homogeneous transformation. + where :math:`[S]` is the augmented skew-symmetric matrix of the twist. + For :math:`S = (v_x, v_y, \omega)` it is + + .. math:: + + \begin{pmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{pmatrix} Example: @@ -1648,16 +1669,46 @@ def ad(self): >>> from spatialmath import SE2, Twist2 >>> T = SE2(1, 2, 0.3) >>> S = Twist2(T) - >>> S.unit() + >>> S.ad() + + .. note:: An alternative approach to computing the adjoint is to + exponentiate this 3x3 matrix. - :seealso: SE3.Ad. + :seealso: :func:`Twist2.Ad` """ - return np.array( - [ - [smb.skew(self.w), smb.skew(self.v)], - [np.zeros((3, 3)), smb.skew(self.w)], - ] - ) + if len(self) == 1: + v, w = self.v, self.w + return np.array([[0, -w, v[1]], [w, 0, -v[0]], [0, 0, 0]]) + else: + return [x.ad() for x in self] + + def Ad(self): + """ + Adjoint of 2D twist + + :return: adjoint matrix + :rtype: ndarray(3,3) + + ``S.Ad()`` is the 3x3 adjoint matrix of the corresponding + homogeneous transformation. + + For a twist representing motion from frame {B} to {A}, the adjoint will + transform a twist relative to frame {A} to one relative to frame {B}. + + Example: + + .. runblock:: pycon + + >>> from spatialmath import Twist2 + >>> S = Twist2([1, 2, 0.3]) + >>> S.Ad() + + .. note:: This method computes the equivalent SE(2) matrix, then the adjoint + of that. + + :seealso: :func:`Twist2.ad`, :func:`Twist2.SE2`, :func:`Twist2.exp` + """ + return self.SE2().Ad() @classmethod def Tx(cls, x): diff --git a/tests/base/test_axes_logic.py b/tests/base/test_axes_logic.py new file mode 100644 index 00000000..a348f47d --- /dev/null +++ b/tests/base/test_axes_logic.py @@ -0,0 +1,102 @@ +"""Tests of ``axes_logic``, which decides which axes a plot_xxx() function draws into.""" + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import pytest + +from spatialmath.base.graphics import axes_logic + + +@pytest.fixture(autouse=True) +def close_figures(): + plt.close("all") + yield + plt.close("all") + + +def nfigs(): + return len(plt.get_fignums()) + + +# --- behaviour that must not change --------------------------------------------- + + +def test_no_figure_creates_one(): + ax = axes_logic(None, 2) + assert nfigs() == 1 + assert ax is plt.gca() + + +def test_current_axes_are_reused(): + fig, ax0 = plt.subplots() + ax = axes_logic(None, 2) + assert ax is ax0 + assert nfigs() == 1 + + +def test_new_forces_a_new_figure(): + fig, ax0 = plt.subplots() + ax = axes_logic(None, 2, new=True) + assert ax is not ax0 + assert nfigs() == 2 + assert ax is plt.gca() + + +def test_dimension_mismatch_makes_a_new_figure(): + fig = plt.figure() + ax3 = fig.add_subplot(projection="3d") + ax = axes_logic(None, 2) + assert ax is not ax3 + assert nfigs() == 2 + + +def test_explicit_axes_of_the_right_dimension_are_used(): + fig, ax0 = plt.subplots() + fig2, ax1 = plt.subplots() + assert axes_logic(ax0, 2) is ax0 + assert nfigs() == 2 + + +def test_explicit_axes_of_the_wrong_dimension_make_a_new_figure(): + fig, ax0 = plt.subplots() + ax = axes_logic(ax0, 3) + assert ax is not ax0 + assert nfigs() == 2 + + +# --- an empty current figure, eg. from a bare plt.figure() ----------------------- + + +def test_empty_current_figure_is_used_2d(): + fig = plt.figure() + ax = axes_logic(None, 2) + assert nfigs() == 1 + assert ax.figure is fig + assert ax is plt.gca() + + +def test_empty_current_figure_is_used_3d(): + fig = plt.figure() + ax = axes_logic(None, 3) + assert nfigs() == 1 + assert ax.figure is fig + assert ax.name == "3d" + + +def test_only_the_current_figure_matters(): + # an earlier figure has axes, but the current one is empty: use the current one + plt.subplots() + fig = plt.figure() + ax = axes_logic(None, 2) + assert nfigs() == 2 + assert ax.figure is fig + + +def test_new_with_an_empty_current_figure_still_makes_another(): + # new=True is an explicit request for a new figure + fig = plt.figure() + ax = axes_logic(None, 2, new=True) + assert nfigs() == 2 + assert ax.figure is not fig diff --git a/tests/base/test_graphics.py b/tests/base/test_graphics.py index 552ebdb0..17b88735 100644 --- a/tests/base/test_graphics.py +++ b/tests/base/test_graphics.py @@ -9,10 +9,60 @@ # TODO check they actually create artists +class TestWindowTitle(unittest.TestCase): + def teardown_method(self, method): + plt.close("all") + + def test_sets_the_title_of_a_managed_figure(self): + from types import SimpleNamespace + from unittest import mock + + manager = mock.Mock() + fig = SimpleNamespace(canvas=SimpleNamespace(manager=manager)) + self.assertTrue(set_window_title(fig, "a title")) + manager.set_window_title.assert_called_once_with("a title") + + def test_pyplot_figure(self): + # the Agg manager has no window, but does accept a title + fig = plt.figure() + self.assertIsInstance(set_window_title(fig, "a title"), bool) + + def test_figure_without_a_manager(self): + from matplotlib.figure import Figure + + self.assertFalse(set_window_title(Figure(), "a title")) + + def test_manager_without_the_method(self): + from types import SimpleNamespace + + class Manager: + pass + + fig = SimpleNamespace(canvas=SimpleNamespace(manager=Manager())) + self.assertFalse(set_window_title(fig, "a title")) + + class TestGraphics(unittest.TestCase): def teardown_method(self, method): plt.close("all") + def test_expand_dims(self): + np.testing.assert_array_equal(expand_dims(3), [-3, 3, -3, 3]) + np.testing.assert_array_equal(expand_dims(-3), [-3, 3, -3, 3]) + np.testing.assert_array_equal(expand_dims([1, 2]), [1, 2, 1, 2]) + np.testing.assert_array_equal(expand_dims([1, 2, 3, 4]), [1, 2, 3, 4]) + # reversed limits are passed through, matplotlib inverts the axis + np.testing.assert_array_equal(expand_dims([2, 1]), [2, 1, 2, 1]) + + np.testing.assert_array_equal(expand_dims(-3, nd=3), [-3, 3, -3, 3, -3, 3]) + np.testing.assert_array_equal(expand_dims([1, 2], nd=3), [1, 2] * 3) + np.testing.assert_array_equal(expand_dims([1, 2, 3, 4, 5, 6], nd=3), [1, 2, 3, 4, 5, 6]) + + with self.assertRaises(ValueError): + expand_dims([1, 2, 3]) + with self.assertRaises(ValueError): + expand_dims([1, 2, 3, 4], nd=3) + @pytest.mark.skipif( sys.platform.startswith("darwin") and sys.version_info < (3, 11), reason="tkinter bug with mac", diff --git a/tests/base/test_transforms3d.py b/tests/base/test_transforms3d.py index e9b9af14..88ca96a1 100755 --- a/tests/base/test_transforms3d.py +++ b/tests/base/test_transforms3d.py @@ -275,6 +275,26 @@ def test_angvec2r(self): nt.assert_array_almost_equal(angvec2r(pi / 4, [0, 0, 1]), rotz(pi / 4)) nt.assert_array_almost_equal(angvec2r(-pi / 4, [0, 0, 1]), rotz(-pi / 4)) + def test_angvec2r_nonunit_axis(self): + # the axis is normalized, so its length is irrelevant + nt.assert_array_almost_equal( + angvec2r(0.5, [1, 2, 3]), angvec2r(0.5, np.array([1, 2, 3]) / np.sqrt(14)) + ) + nt.assert_array_almost_equal(angvec2r(pi / 4, [0, 0, 5]), rotz(pi / 4)) + + def test_angvec2r_zero_axis(self): + # zero rotation about a zero axis is the identity, any angle units + nt.assert_array_almost_equal(angvec2r(0, [0, 0, 0]), np.eye(3)) + nt.assert_array_almost_equal(angvec2r(0, [0, 0, 0], unit="deg"), np.eye(3)) + + # a non-zero rotation about a zero axis is meaningless + with self.assertRaises(ValueError): + angvec2r(0.5, [0, 0, 0]) + with self.assertRaises(ValueError): + angvec2r(pi, [0, 0, 0]) + with self.assertRaises(ValueError): + angvec2r(90, [0, 0, 0], unit="deg") + def test_angvec2tr(self): r2d = 180 / pi @@ -290,6 +310,14 @@ def test_angvec2tr(self): nt.assert_array_almost_equal(angvec2tr(pi / 4, [0, 0, 1]), trotz(pi / 4)) nt.assert_array_almost_equal(angvec2tr(-pi / 4, [0, 0, 1]), trotz(-pi / 4)) + # non-unit axis is normalized + nt.assert_array_almost_equal(angvec2tr(pi / 4, [0, 0, 5]), trotz(pi / 4)) + + # zero rotation about a zero axis is the identity, otherwise an error + nt.assert_array_almost_equal(angvec2tr(0, [0, 0, 0]), np.eye(4)) + with self.assertRaises(ValueError): + angvec2tr(0.5, [0, 0, 0]) + r2d = 180 / pi nt.assert_array_almost_equal(angvec2r(0, [1, 0, 0]), rotx(0)) diff --git a/tests/base/test_vectors.py b/tests/base/test_vectors.py index 3110eb56..e07db802 100755 --- a/tests/base/test_vectors.py +++ b/tests/base/test_vectors.py @@ -7,6 +7,7 @@ """ +import warnings import numpy as np import numpy.testing as nt import unittest @@ -397,6 +398,17 @@ def test_angle_stats(self): self.assertAlmostEqual(angle_mean(theta), pi) self.assertAlmostEqual(angle_std(theta), 0.9717284050981313) + def test_angle_std_identical_angles(self): + # the length of the mean unit vector can come out as 1 + eps, which + # used to give the square root of a negative number + for angle in np.linspace(-3.1, 3.1, 63): + for n in (1, 2, 3, 7, 10, 100): + with warnings.catch_warnings(): + warnings.simplefilter("error") + std = angle_std(np.full(n, angle)) + self.assertTrue(np.isfinite(std)) + self.assertLess(std, 1e-6) + def test_removesmall(self): v = np.r_[1, 2, 3] nt.assert_array_almost_equal(removesmall(v), v) diff --git a/tests/test_angvec_nonunit_axis.py b/tests/test_angvec_nonunit_axis.py new file mode 100644 index 00000000..22c748df --- /dev/null +++ b/tests/test_angvec_nonunit_axis.py @@ -0,0 +1,82 @@ +"""Angle-axis construction preserves unit-quaternion and zero-angle contracts.""" + +import math +import unittest + +import numpy as np +from numpy.testing import assert_allclose + +from spatialmath import UnitQuaternion +from spatialmath.base import angvec2r + + +class TestAngVecNonunitAxis(unittest.TestCase): + def test_unit_norm(self): + assert_allclose(np.linalg.norm(UnitQuaternion.AngVec(0.5, [1, 2, 3]).vec), 1) + + def test_angle_round_trip(self): + theta, axis = UnitQuaternion.AngVec(0.5, [1, 2, 3]).angvec() + assert_allclose(theta, 0.5) + assert_allclose(axis, np.array([1, 2, 3]) / np.sqrt(14)) + + def test_rotation_matrix(self): + assert_allclose( + UnitQuaternion.AngVec(0.5, [1, 2, 3]).R, + angvec2r(0.5, [1, 2, 3]), + atol=1e-12, + ) + + def test_degrees(self): + assert_allclose( + UnitQuaternion.AngVec(90, [0, 0, 4], unit="deg").R, + angvec2r(math.pi / 2, [0, 0, 1]), + atol=1e-12, + ) + + def test_unit_axis_control(self): + assert_allclose( + UnitQuaternion.AngVec(0.5, [1, 0, 0]).vec, + [math.cos(0.25), math.sin(0.25), 0, 0], + ) + + def test_zero_angle_zero_axis(self): + assert_allclose(UnitQuaternion.AngVec(0, [0, 0, 0]).vec, [1, 0, 0, 0]) + + def test_zero_degrees_zero_axis(self): + assert_allclose( + UnitQuaternion.AngVec(0, [0, 0, 0], unit="deg").vec, [1, 0, 0, 0] + ) + + def test_nonzero_angle_requires_nonzero_axis(self): + with self.assertRaises(ValueError): + UnitQuaternion.AngVec(0.5, [0, 0, 0]) + + def test_near_zero_angle_zero_axis(self): + for theta in [1e-20, -1e-20]: + with self.subTest(theta=theta): + assert_allclose( + UnitQuaternion.AngVec(theta, [0, 0, 0]).vec, [1, 0, 0, 0] + ) + + def test_near_zero_degrees_zero_axis(self): + for theta in [1e-20, -1e-20]: + with self.subTest(theta=theta): + assert_allclose( + UnitQuaternion.AngVec(theta, [0, 0, 0], unit="deg").vec, + [1, 0, 0, 0], + ) + + def test_nonzero_small_angle_requires_nonzero_axis(self): + for theta in [1e-10, -1e-10]: + with self.subTest(theta=theta): + with self.assertRaises(ValueError): + UnitQuaternion.AngVec(theta, [0, 0, 0]) + + def test_nonzero_small_angle_preserves_rotation(self): + for theta in [1e-10, -1e-10]: + with self.subTest(theta=theta): + assert_allclose( + UnitQuaternion.AngVec(theta, [2, 0, 0]).vec, + [math.cos(theta / 2), math.sin(theta / 2), 0, 0], + atol=0, + ) diff --git a/tests/test_lazy_sympy.py b/tests/test_lazy_sympy.py new file mode 100644 index 00000000..6b5c4c1b --- /dev/null +++ b/tests/test_lazy_sympy.py @@ -0,0 +1,111 @@ +"""Regression tests for optional SymPy loading in fresh interpreters.""" + +import subprocess +import sys +import textwrap + +import pytest + + +def run_python(source): + """Run an isolated check with the same interpreter as pytest.""" + subprocess.run([sys.executable, "-c", textwrap.dedent(source)], check=True) + + +def test_numeric_import_does_not_load_sympy(): + run_python( + """ + import sys + import numpy as np + import spatialmath + from spatialmath import base as smb + assert not any(n == 'sympy' or n.startswith('sympy.') for n in sys.modules) + assert smb.isscalar(1.0) + assert smb.isvector([1, 2]) + assert smb.isnumberlist([1, 2]) + assert smb.norm([3, 4]) == 5 + assert np.allclose(smb.rot2(0), np.eye(2)) + assert not any(n == 'sympy' or n.startswith('sympy.') for n in sys.modules) + """ + ) + + +@pytest.mark.parametrize("sympy_first", [False, True]) +def test_symbolic_operations_in_both_import_orders(sympy_first): + prefix = "import sympy as sp\n" if sympy_first else "" + run_python( + prefix + + """ +import numpy as np +from spatialmath import base as smb +import sympy as sp +x = sp.Symbol('x', real=True) +assert smb.issymbol(x) +assert smb.issymbol([1, x]) +assert smb.issymbol((x, 1)) +assert not smb.issymbol([1, 2]) +assert smb.isscalar(x) +assert not smb.isscalar([x]) +assert not smb.isscalar((x,)) +assert smb.isvector([x, 1]) +assert smb.isnumberlist([x, 1]) +assert smb.getvector(x)[0] == x +assert smb.getvector([x, 1])[0] == x +assert smb.sin(x) == sp.sin(x) +assert smb.cos(x) == sp.cos(x) +assert smb.tan(x) == sp.tan(x) +assert smb.sqrt(x) == sp.sqrt(x) +assert smb.norm([x, 1]) == sp.sqrt(x*x + 1) +assert smb.det(np.array([[x, 0], [0, 1]], dtype=object)) == x +T = smb.trot2(x) +assert sp.simplify(smb.tr2xyt(T)[2] - sp.atan2(sp.sin(x), sp.cos(x))) == 0 +assert smb.simplify(sp.sin(x)**2 + sp.cos(x)**2) == 1 +""" + ) + + +@pytest.mark.parametrize("helper", ["symbol", "zero", "one", "negative_one", "pi"]) +def test_explicit_symbolic_helpers_load_sympy(helper): + run_python( + textwrap.dedent(f"helper = {helper!r}\n") + + textwrap.dedent( + """ + import sys + from spatialmath.base import symbolic as sym + assert 'sympy' not in sys.modules + getattr(sym, helper)(*(['x'] if helper == 'symbol' else [])) + assert 'sympy' in sys.modules + x = sym.symbol('x') + import sympy as sp + assert isinstance(x, sp.Symbol) + from spatialmath.base.symbolic import Symbol, symtype + assert Symbol is sp.Symbol + assert symtype == (sp.Expr,) + assert sym.zero() == sp.S.Zero + assert sym.one() == sp.S.One + assert sym.negative_one() == sp.S.NegativeOne + assert sym.pi() == sp.pi + """ + ) + ) + + +def test_numeric_operations_without_sympy(): + run_python( + """ + import importlib.abc + import sys + class BlockSympy(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if fullname == 'sympy' or fullname.startswith('sympy.'): + raise ModuleNotFoundError('SymPy blocked for this test') + sys.meta_path.insert(0, BlockSympy()) + from spatialmath import base as smb + assert not smb.issymbol(1) + assert smb.norm([3, 4]) == 5 + assert smb.sin(0) == 0 + assert smb.simplify(2) == 2 + assert smb.isscalar(1) + assert smb.isvector([1, 2]) + """ + ) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 589c0ec1..d4b16668 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -241,6 +241,15 @@ def test_constructor_TwoVec(self): # x axis should equal normalized x vector nt.assert_almost_equal(R.R[:, 0], v3 / np.linalg.norm(v3), 5) + def test_AngleAxis_zero_axis(self): + nt.assert_array_almost_equal(SO3.AngleAxis(0, [0, 0, 0]).A, np.eye(3)) + nt.assert_array_almost_equal(SO3.AngVec(0, [0, 0, 0]).A, np.eye(3)) + nt.assert_array_almost_equal(SE3.AngleAxis(0, [0, 0, 0]).A, np.eye(4)) + with self.assertRaises(ValueError): + SO3.AngleAxis(0.5, [0, 0, 0]) + with self.assertRaises(ValueError): + SE3.AngVec(0.5, [0, 0, 0]) + def test_conversion(self): R = SO3.AngleAxis(0.7, [1, 2, 3]) q = UnitQuaternion([11, 7, 3, -6]) diff --git a/tests/test_source_warnings.py b/tests/test_source_warnings.py new file mode 100644 index 00000000..a580164d --- /dev/null +++ b/tests/test_source_warnings.py @@ -0,0 +1,33 @@ +"""Compiling the package must not raise SyntaxWarning or DeprecationWarning. + +A non-raw docstring containing LaTeX such as ``\\mat`` or ``\\vec`` gives an +"invalid escape sequence" warning, and ``\\t`` or ``\\v`` (as in ``\\times`` and +``\\vec``) are valid escapes that silently put a tab or vertical tab into the +documentation. Use ``r\"\"\"`` for docstrings that contain backslashes. +""" + +import pathlib +import unittest +import warnings + +import spatialmath + + +class TestSourceWarnings(unittest.TestCase): + def test_package_compiles_without_warnings(self): + root = pathlib.Path(spatialmath.__file__).parent + problems = [] + for path in sorted(root.rglob("*.py")): + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + compile(path.read_text(encoding="utf-8"), str(path), "exec") + problems += [ + f"{path.relative_to(root)}:{w.lineno}: {w.message}" + for w in caught + if issubclass(w.category, (SyntaxWarning, DeprecationWarning)) + ] + self.assertEqual(problems, []) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_twist2_adjoint.py b/tests/test_twist2_adjoint.py new file mode 100644 index 00000000..23413c44 --- /dev/null +++ b/tests/test_twist2_adjoint.py @@ -0,0 +1,66 @@ +import numpy as np +import numpy.testing as nt +import pytest +from scipy.linalg import expm + +from spatialmath import SE2, Twist2 +from spatialmath.base import skewa, vexa + + +def _bracket(a, b): + """Lie bracket of two se(2) vectors, via their matrix commutator.""" + A, B = skewa(a), skewa(b) + return vexa(A @ B - B @ A) + + +@pytest.fixture +def twists(): + rng = np.random.default_rng(7) + return rng.normal(size=3), rng.normal(size=3) + + +def test_ad_is_lie_bracket(twists): + """ad(S) @ T is the Lie bracket [S, T].""" + a, b = twists + ad = Twist2(a).ad() + assert ad.shape == (3, 3) + nt.assert_allclose(ad @ b, _bracket(a, b), atol=1e-14) + + +def test_ad_antisymmetry(twists): + """[S, S] = 0, and ad(S) T = -ad(T) S.""" + a, b = twists + nt.assert_allclose(Twist2(a).ad() @ a, 0, atol=1e-14) + nt.assert_allclose(Twist2(a).ad() @ b, -Twist2(b).ad() @ a, atol=1e-14) + + +def test_exp_of_ad_is_Ad(twists): + """The adjoint of exp(S) is the exponential of ad(S).""" + a, _ = twists + S = Twist2(a) + nt.assert_allclose(S.Ad(), expm(S.ad()), atol=1e-12) + nt.assert_allclose(S.Ad(), S.SE2().Ad(), atol=1e-14) + + +def test_SE2_Ad_maps_twists(twists): + """Ad(T) S is vee(T [S] T^-1).""" + a, _ = twists + T = SE2(1.5, -0.7, 0.9) + expected = vexa(T.A @ skewa(a) @ T.inv().A) + nt.assert_allclose(T.Ad() @ a, expected, atol=1e-14) + + +def test_SE2_Ad_identity_and_composition(): + """Ad is a homomorphism: Ad(T1 T2) = Ad(T1) Ad(T2).""" + nt.assert_allclose(SE2().Ad(), np.eye(3), atol=1e-14) + T1, T2 = SE2(1, 2, 0.3), SE2(-0.5, 0.25, -1.1) + nt.assert_allclose((T1 * T2).Ad(), T1.Ad() @ T2.Ad(), atol=1e-14) + + +def test_ad_sequence(): + """A sequence of twists gives a list of matrices, as for skewa().""" + seq = Twist2([np.array([1.0, 2.0, 0.3]), np.array([0.0, 1.0, -0.4])]) + ads = seq.ad() + assert isinstance(ads, list) and len(ads) == 2 + for m, t in zip(ads, seq): + nt.assert_allclose(m, t.ad()) diff --git a/tests/test_twist_pole.py b/tests/test_twist_pole.py new file mode 100644 index 00000000..8acc2e34 --- /dev/null +++ b/tests/test_twist_pole.py @@ -0,0 +1,35 @@ +import numpy as np +import pytest +from scipy.linalg import expm + +from spatialmath import Twist2, Twist3 + + +@pytest.mark.parametrize("speed", [-2.0, -0.3, 0.3, 1.0, 2.0]) +def test_twist2_pole_is_rotation_center(speed): + """A change in angular speed must not move the center of rotation.""" + center = np.array([3.0, -2.0]) + twist = Twist2(speed * np.r_[center[1], -center[0], 1]) + np.testing.assert_allclose(twist.pole, center) + transform = expm(twist.skewa() * 0.7) + np.testing.assert_allclose( + transform @ np.r_[twist.pole, 1], np.r_[center, 1], atol=1e-14 + ) + + +@pytest.mark.parametrize("speed", [-2.0, -0.3, 0.3, 1.0, 2.0]) +@pytest.mark.parametrize("pitch", [0, 0.4]) +def test_twist3_pole_is_on_screw_axis(speed, pitch): + """The closest axis point is fixed except for translation along the axis.""" + axis = np.array([1.0, 2.0, -2.0]) / 3 + center = np.array([3.0, -2.0, 1.0]) + center -= (center @ axis) * axis + angular = speed * axis + linear = -np.cross(angular, center) + pitch * angular + twist = Twist3(linear, angular) + np.testing.assert_allclose(twist.pole, center) + transform = expm(twist.skewa() * 0.7) + expected = center + pitch * angular * 0.7 + np.testing.assert_allclose( + (transform @ np.r_[twist.pole, 1])[:3], expected, atol=1e-14 + ) diff --git a/tests/test_twist_sequence.py b/tests/test_twist_sequence.py new file mode 100644 index 00000000..7dcbee9a --- /dev/null +++ b/tests/test_twist_sequence.py @@ -0,0 +1,49 @@ +import numpy as np +import pytest +from scipy.linalg import expm + +from spatialmath import SE2, SE3, Twist2, Twist3 + + +@pytest.mark.parametrize("twist,pose", [(Twist2, SE2), (Twist3, SE3)]) +@pytest.mark.parametrize("method", ["exp", "pose"]) +@pytest.mark.parametrize("theta", [None, 0, -0.4, [0.2, -0.7]]) +def test_twist_sequence_exponential(twist, pose, method, theta): + """A shared step and individual steps both integrate every twist.""" + vectors = ( + [[1, -2, 0.3], [2, 1, -0.4]] + if twist is Twist2 + else [[1, -2, 3, 0.2, 0.3, -0.4], [2, 1, -1, -0.1, 0.5, 0.2]] + ) + sequence = twist([np.array(v) for v in vectors]) + convert = getattr(sequence, "exp" if method == "exp" else pose.__name__) + result = convert() if theta is None else convert(theta) + steps = np.broadcast_to(1 if theta is None else theta, (2,)) + assert isinstance(result, pose) + assert len(result) == 2 + for value, source, step in zip(result, sequence, steps): + np.testing.assert_allclose(value.A, expm(source.skewa() * step), atol=1e-14) + + +@pytest.mark.parametrize("twist,pose", [(Twist2, SE2), (Twist3, SE3)]) +@pytest.mark.parametrize("method", ["exp", "pose"]) +def test_twist_sequence_degrees_and_singleton(twist, pose, method): + """Degrees, singleton expansion and mismatched lengths retain their rules.""" + source = twist([1, 2, 0.3] if twist is Twist2 else [1, 2, 3, 0.2, 0.3, 0.4]) + name = "exp" if method == "exp" else pose.__name__ + angles = [15, -30] + expanded = getattr(source, name)(angles, unit="deg") + assert len(expanded) == 2 + for value, angle in zip(expanded, angles): + np.testing.assert_allclose( + value.A, expm(source.skewa() * np.deg2rad(angle)), atol=1e-14 + ) + sequence = twist([source, source]) + shared = getattr(sequence, name)(30, unit="deg") + assert len(shared) == 2 + for value in shared: + np.testing.assert_allclose( + value.A, expm(source.skewa() * np.deg2rad(30)), atol=1e-14 + ) + with pytest.raises(ValueError): + getattr(sequence, name)([0.1, 0.2, 0.3])