diff --git a/pyproject.toml b/pyproject.toml index 8e052175..4b2398b7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "spatialmath-python" -version = "1.1.18" +version = "1.1.19" authors = [ { name="Peter Corke", email="rvc@petercorke.com" }, ] @@ -78,7 +78,7 @@ ros-humble = [ [build-system] -requires = ["setuptools", "oldest-supported-numpy"] +requires = ["setuptools"] build-backend = "setuptools.build_meta" [tool.setuptools] diff --git a/spatialmath/base/__init__.py b/spatialmath/base/__init__.py index 88f7bec2..02489283 100644 --- a/spatialmath/base/__init__.py +++ b/spatialmath/base/__init__.py @@ -10,6 +10,7 @@ from spatialmath.base.vectors import * # lgtm [py/polluting-import] from spatialmath.base.symbolic import * # lgtm [py/polluting-import] from spatialmath.base.numeric import * # lgtm [py/polluting-import] +from spatialmath.base._environment import * # lgtm [py/polluting-import] import importlib @@ -46,7 +47,6 @@ "plotvol2": "graphics", "plotvol3": "graphics", "expand_dims": "graphics", - "isnotebook": "graphics", } @@ -394,6 +394,7 @@ def __getattr__(name): "axes_logic", "set_window_title", "expand_dims", + "environment", "isnotebook", # spatial.base.numeric "numjac", diff --git a/spatialmath/base/_environment.py b/spatialmath/base/_environment.py new file mode 100644 index 00000000..7d5b017e --- /dev/null +++ b/spatialmath/base/_environment.py @@ -0,0 +1,78 @@ +# Part of Spatial Math Toolbox for Python +# Copyright (c) 2000 Peter Corke +# MIT Licence, see details in top-level file: LICENCE + +""" +Detect the interactive environment that Python is running in. + +This module deliberately does not import Matplotlib, so it is cheap to import +and works when Matplotlib is not installed. +""" + +import sys +from typing import Literal + +__all__ = ["environment", "isnotebook"] + +Environment = Literal["terminal", "ipython", "jupyter", "colab", "jupyterlite"] + + +def environment() -> Environment: + """ + Determine the environment that Python is running in + + :return: the environment + :rtype: str + + The result is one of: + + ======================== ==================================================== + ``"terminal"`` standard Python interpreter, or script + ``"ipython"`` IPython running in a terminal + ``"jupyter"`` Jupyter kernel, eg. JupyterLab, Notebook, VS Code or + qtconsole + ``"colab"`` Google Colab + ``"jupyterlite"`` JupyterLite, running in the browser (Pyodide) + ======================== ==================================================== + + .. note:: The different Jupyter front ends (JupyterLab, classic Notebook, + VS Code, Jupyter Desktop) cannot be reliably told apart from within the + kernel, so all are reported as ``"jupyter"``. How figures will be + rendered depends on the Matplotlib backend, not on the front end. + + :seealso: :func:`isnotebook` + """ + # Colab and JupyterLite both also provide an IPython kernel, so test for + # them first + if "google.colab" in sys.modules: + return "colab" + if sys.platform == "emscripten": + return "jupyterlite" + + try: + shell = get_ipython().__class__.__name__ # type: ignore[name-defined] + except NameError: + return "terminal" # probably standard Python interpreter + + if shell == "ZMQInteractiveShell": + return "jupyter" # Jupyter notebook or qtconsole + elif shell == "TerminalInteractiveShell": + return "ipython" + else: + return "terminal" # other type (?) + + +def isnotebook() -> bool: + """ + Determine if code is being run from a Jupyter notebook + + :return: True if running in a Jupyter kernel, Google Colab or JupyterLite + :rtype: bool + + :seealso: :func:`environment` + + :references: + + - https://stackoverflow.com/questions/15411967/how-can-i-check-if-code-is-executed-in-the-ipython-notebook/39662359#39662359 + """ + return environment() in ("jupyter", "colab", "jupyterlite") 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 2baf94b7..06e6ce85 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1513,7 +1513,10 @@ def _axes_dimensions(ax: plt.Axes) -> int: # handle the case of Animate objects pretending to be Axes classname = ax.__class__.__name__ base_classes = ax.__class__.__bases__ - if classname in ("Axes3DSubplot", "Animate") or any(base_class.__name__ in ("Axes3DSubplot", "Animate") for base_class in base_classes): + if classname in ("Axes3DSubplot", "Animate") or any( + base_class.__name__ in ("Axes3DSubplot", "Animate") + for base_class in base_classes + ): return 3 elif classname in ("AxesSubplot", "Animate2"): return 2 @@ -1580,6 +1583,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 +1601,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 +1615,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: @@ -1736,20 +1745,24 @@ def plotvol3( ax._plotvol = True return ax - def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: + def expand_dims(dim: ArrayLike | None = None, nd: int = 2) -> NDArray | None: """ Expand compact axis dimensions - :param dim: dimensions, defaults to None + :param dim: dimensions, or None if unspecified, defaults to None :type dim: scalar, array_like(2), array_like(4), array_like(6), optional - :param nd: number of axes dimensions, defaults to 2 + :param nd: number of axes dimensions (2 or 3), defaults to 2 :type nd: int, optional :raises ValueError: bad arguments - :return: 2d or 3d dimensions vector - :rtype: ndarray(4) or ndarray(6) + :return: 2d or 3d dimensions vector, or None if unspecified + :rtype: ndarray(4), ndarray(6), or None Compute bounding dimensions for plots from shorthand notation. + ``None`` is returned unchanged so the caller can choose whether to + autoscale or obtain dimensions from another source. No default extent + is inferred. + If ``nd==2``, [xmin, xmax, ymin, ymax]: * A -> [-A, A, -A, A] * [A,B] -> [A, B, A, B] @@ -1766,6 +1779,11 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: corresponding maximum, matplotlib will invert that axis when the limits are applied, which is a convenient way to flip an axis direction. """ + if nd not in (2, 3): + raise ValueError("nd is 2 or 3") + if dim is None: + return None + dim = smb.getvector(dim) if nd == 2: @@ -1788,8 +1806,6 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: return dim else: raise ValueError("bad dimension specified") - else: - raise ValueError("nd is 2 or 3") def set_window_title(fig: plt.Figure, title: str) -> bool: """ @@ -1822,26 +1838,6 @@ def set_window_title(fig: plt.Figure, title: str) -> bool: return True return False - def isnotebook() -> bool: - """ - Determine if code is being run from a Jupyter notebook - - :references: - - - https://stackoverflow.com/questions/15411967/how-can-i-check-if-code- - is-executed-in-the-ipython-notebook/39662359#39662359 - """ - try: - shell = get_ipython().__class__.__name__ - if shell == "ZMQInteractiveShell": - return True # Jupyter notebook or qtconsole - elif shell == "TerminalInteractiveShell": - return False # Terminal running IPython - else: - return False # Other type (?) - except NameError: - return False # Probably standard Python interpreter - if __name__ == "__main__": import pathlib diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 9041485d..6800ff53 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -16,11 +16,13 @@ import spatialmath.base as smb from spatialmath.base.argcheck import getunit from spatialmath.base.types import * -import scipy.interpolate as interpolate -from typing import Optional +from typing import TYPE_CHECKING, Optional from functools import lru_cache import warnings +if TYPE_CHECKING: + import scipy.interpolate as interpolate + _eps = np.finfo(np.float64).eps @@ -920,7 +922,7 @@ def _compute_cdf_sin_squared(theta: float): @lru_cache(maxsize=1) def _generate_inv_cdf_sin_squared_interp( num_interpolation_points: int = 256, -) -> interpolate.interp1d: +) -> "interpolate.interp1d": """ Computes an interpolation function for the inverse CDF of the distribution of angular magnitude. @@ -933,6 +935,8 @@ def _generate_inv_cdf_sin_squared_interp( This function returns interpolation function for the inverse of the integral of the pdf of angular magnitudes (2/pi * sin^2(theta/2)), which is not analytically defined. """ + import scipy.interpolate as interpolate # deferred, it is slow to import + cdf_sin_squared_interp_angles = np.linspace(0, np.pi, num_interpolation_points) cdf_sin_squared_interp_values = _compute_cdf_sin_squared( cdf_sin_squared_interp_angles diff --git a/spatialmath/base/symbolic.py b/spatialmath/base/symbolic.py index aebb827a..bdddf747 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 - .. note:: In Jupyter symbols are pretty printed. + >>> from spatialmath.base.symbolic import * + >>> theta = symbol('theta') + >>> theta + >>> theta, psi = symbol('theta psi') + >>> theta + >>> psi + >>> q = symbol('q_:6') + >>> q - - 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. + .. note:: In Jupyter symbols are pretty printed. - :seealso: :func:`sympy.symbols` - """ - return sympy.symbols(name, real=real) + - 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) 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,7 +288,7 @@ def negative_one() -> Symbol: :seealso: :func:`sympy.S.NegativeOne` """ - return sympy.S.NegativeOne + return _sympy().S.NegativeOne def pi() -> Symbol: @@ -302,7 +307,7 @@ def pi() -> Symbol: :seealso: :func:`sympy.S.Pi` """ - return sympy.S.Pi + return _sympy().S.Pi def simplify(x: Symbol) -> Symbol: @@ -324,10 +329,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 +358,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 c8a10e8f..ad9dc7f6 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] @@ -549,7 +545,7 @@ def trlog2( else: st = T[1, 0] ct = T[0, 0] - theta = math.atan(st / ct) + theta = math.atan2(st, ct) if abs(theta) < tol * _eps: tr = T[:2, 2].flatten() else: @@ -564,7 +560,7 @@ def trlog2( elif isrot2(T, check=check, tol=tol): # SO(2) rotation matrix - theta = math.atan(T[1, 0] / T[0, 0]) + theta = math.atan2(T[1, 0], T[0, 0]) if twist: return theta else: 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 299b2c32..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) diff --git a/spatialmath/baseposelist.py b/spatialmath/baseposelist.py index b102b4bb..1fdad6f3 100644 --- a/spatialmath/baseposelist.py +++ b/spatialmath/baseposelist.py @@ -154,6 +154,8 @@ def arghandler( #. None, an identity value is created #. a numpy.ndarray of the appropriate shape and value which is valid for the subclass + #. a stacked numpy.ndarray of shape (N, r, c), where (r, c) is the shape of one value + of the subclass, which is a sequence of N values #. a list whose elements all meet the criteria above #. an instance of the subclass #. a list whose elements are all singelton instances of the subclass @@ -187,6 +189,20 @@ def arghandler( elif isinstance(arg, np.ndarray): # it's a numpy array + shape = tuple(self.shape) + if len(shape) == 2 and arg.ndim == 3: + # a 3D array passed to a matrix pose class is a stack + if arg.shape[1:] != shape: + # a stack of matrices of the wrong size is rejected, + # whatever the value of check + return False + # a stacked (N, r, c) array is a sequence of N values + values = [self._import(x, check=check) for x in arg] + if any(x is None for x in values): + return False + self.data = values + return True + x = self._import(arg, check=check) if x is not None: self.data = [x] diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py old mode 100755 new mode 100644 index 84813a41..c1efbbb2 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -95,7 +95,7 @@ def LinePoint(cls, l: Line3, p: ArrayLike3) -> Self: :seealso: :meth:`PointNormal` :meth:`ThreePoints` """ - n = np.cross(l.w, p) + n = np.cross(l.w, p) - l.v d = np.dot(l.v, p) return cls(np.r_[n, d]) @@ -145,7 +145,7 @@ def intersection(pi1: Plane3, pi2: Plane3, pi3: Plane3) -> R3: """ A = np.vstack([pi1.n, pi2.n, pi3.n]) b = np.array([pi1.d, pi2.d, pi3.d]) - return np.linalg.det(A) @ b + return np.linalg.solve(A, -b) @property def n(self) -> R3: @@ -190,7 +190,7 @@ def contains(self, p: ArrayLike3, tol: float = 20) -> bool: :return: if the point is in the plane :rtype: bool """ - return abs(np.dot(self.n, p) - self.d) < tol * _eps + return abs(np.dot(self.n, p) + self.d) < tol * _eps def plot( self, @@ -830,15 +830,12 @@ def intersects( ``l1.intersects(l2)`` is the point of intersection of the two lines, or ``None`` if the lines do not intersect or are equivalent. - :seealso: :meth:`commonperp :meth:`eq` :meth:`__xor__` + :seealso: :meth:`commonperp` :meth:`isintersecting` :meth:`closest_to_line` """ if l1 ^ l2: - # lines do intersect - return -( - np.dot(l1.v, l2.w) * np.eye(3, 3) - + l1.w.reshape((3, 1)) @ l2.v.reshape((1, 3)) - - l2.w.reshape((3, 1)) @ l1.v.reshape((1, 3)) - ) * base.unitvec(np.cross(l1.w, l2.w)) + # For intersecting lines, the nearest point is their intersection. + point, _ = l1.closest_to_line(l2) + return point else: # lines don't intersect return None diff --git a/spatialmath/pose2d.py b/spatialmath/pose2d.py index f919ddff..2234d8bc 100644 --- a/spatialmath/pose2d.py +++ b/spatialmath/pose2d.py @@ -358,13 +358,13 @@ def Rand( Return an SE2 instance with random rotation and translation. - - ``SE2.Rand()`` is a random SE(2) rotation. + - ``SE2.Rand()`` is a random SE(2) pose. - ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random poses. Example, create random ten vehicles in the xy-plane:: - >>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) + >>> x = SE2.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) >>> len(x) 10 diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index dfc91a86..117f03a8 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -1096,6 +1096,9 @@ def __init__(self, x=None, y=None, z=None, *, check=True): - ``SE3([X1, X2, ... XN])`` has ``N`` values given by the elements ``Xi`` each of which is an SE3 instance. + - ``SE3(A)`` where ``A`` is an ``(N, 4, 4)`` NumPy array has ``N`` values, + equivalent to ``SE3(list(A))``. + :SymPy: supported """ if y is None and z is None: @@ -1115,7 +1118,10 @@ def __init__(self, x=None, y=None, z=None, *, check=True): self.data = [smb.transl(T) for T in x] else: - raise ValueError("bad argument to constructor") + raise ValueError( + "bad argument to constructor: expected an SE3, SO3 or SE2 instance, a 4x4 or " + "(N, 4, 4) array, a list of those, a 3-vector or an (N, 3) array of translations" + ) elif y is not None and z is not None: # SE3(x, y, z) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index a955241e..858522e8 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -16,6 +16,7 @@ # pylint: disable=invalid-name from __future__ import annotations import math +import warnings import numpy as np from typing import Any import spatialmath.base as smb @@ -2073,7 +2074,7 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern """ Interpolate a unit quaternion - :param shortest: Take the shortest path along the great circle + :param shortest: deprecated, has no effect :param s: interpolation coefficient, range 0 to 1, or number of steps :type s: array_like or int :return: interpolated unit quaternion @@ -2104,19 +2105,39 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern .. note:: values of ``s`` are silently clipped to the range [0, 1] + .. note:: ``shortest`` can never change the result here. Every + ``UnitQuaternion`` has a non-negative scalar part by construction + (see :func:`~spatialmath.base.quaternions.qunit`), and this method + always interpolates *from* the identity quaternion ``[1,0,0,0]`` + -- whose dot product with any unit quaternion is just that + quaternion's own (always non-negative) scalar part. A negative + dot product is what ``shortest`` checks for to detect "the long + way round", and that condition can never occur here, unlike + :meth:`interp`, where it can. + + .. deprecated:: 1.1.19 + ``shortest`` has no effect and will be removed in a future + release. + :seealso: :func:`~spatialmath.base.quaternions.qslerp` """ # TODO allow self to have len() > 1 + if shortest: + warnings.warn( + "shortest has no effect on interp1 and will be removed in a " + "future release: interpolation from the identity quaternion " + "is always the shortest path", + DeprecationWarning, + ) + if isinstance(s, int) and s > 1: s = np.linspace(0, 1, s) else: s = smb.getvector(s) s = np.clip(s, 0, 1) # enforce valid values - q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare( - smb.qeye(), self.vec, shortest=shortest - ) + q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare(smb.qeye(), self.vec) qi = [] for sk in s: if sk == 0: diff --git a/spatialmath/spline.py b/spatialmath/spline.py index 5fd66b39..c0a99cad 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -11,8 +11,6 @@ from typing import TYPE_CHECKING, List, Optional, Tuple import numpy as np -from scipy.interpolate import BSpline, CubicSpline -from scipy.spatial.transform import Rotation, RotationSpline from spatialmath import SE3, SO3, Twist3 from spatialmath.base.transforms3d import tranimate @@ -122,6 +120,9 @@ def __init__( self.timepoints = self.timepoints - self.timepoints[0] self.timepoints = self.timepoints / self.timepoints[-1] + from scipy.interpolate import CubicSpline # deferred, scipy is slow to import + from scipy.spatial.transform import Rotation, RotationSpline + self.spline_xyz = CubicSpline( self.timepoints, np.array([pose.t for pose in self.control_poses]), @@ -292,6 +293,8 @@ def __init__( ) # ensures the curve ends on the last control pose self.knots = knots + from scipy.interpolate import BSpline # deferred, scipy is slow to import + self.splines = [ BSpline(knots, self.control_pose_matrix[:, i], degree) for i in range(0, 6) # twists are length 6 diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 0645aef3..fa3c5db0 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -105,7 +105,7 @@ def isprismatic(self): if len(self) == 1: return smb.iszerovec(self.w) else: - return [smb.iszerovec(x.w) for x in self.data] + return [smb.iszerovec(x.w) for x in self] @property def isrevolute(self): @@ -131,7 +131,7 @@ def isrevolute(self): if len(self) == 1: return smb.iszerovec(self.v) else: - return [smb.iszerovec(x.v) for x in self.data] + return [smb.iszerovec(x.v) for x in self] @property def isunit(self): @@ -296,13 +296,30 @@ class Twist3(BaseTwist): 3D rigid body transformation which is the unique elements of the Lie algebra se(3) of the corresponding SE(3) matrix. + A twist can be used in two ways which share the same 6-vector, the same + :math:`[\vec{v}, \omega]` layout and the same algebra: + + - as a *displacement*, where ``exp(S)`` is the pose reached by following the + screw motion and ``exp(S * theta)`` scales it. ``S`` is often a unit + twist, see :meth:`unit`. This is the usage in Robotics, Vision & Control + and in Modern Robotics. + - as a *velocity*, where ``S`` has units of m/s and rad/s and ``exp(S * dt)`` + is the pose change over a time ``dt``. This is the meaning of ``Twist`` + in ROS. The typed counterpart, which also has acceleration, force and + momentum, is :class:`~spatialmath.spatialvector.SpatialVelocity`. + + Properties that describe the screw axis, such as :attr:`pitch`, + :attr:`pole` and :meth:`line`, are unchanged by scaling the twist so apply + to both. Those that depend on its magnitude, such as :attr:`theta`, are + an angle in the first usage and a rate in the second. + :References: - Robotics, Vision & Control for Python, Section 2.3.2.3, P. Corke, Springer 2023. - Modern Robotics, Lynch & Park, Cambridge 2017 .. note:: Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational - components, ie. :math:`[\omega, \vec{v}]`. + components, ie. :math:`[\vec{v}, \omega]`. """ @@ -364,8 +381,8 @@ def isvalid(v, check=True): """ Test if matrix is valid twist - :param x: array to test - :type x: ndarray + :param v: array to test + :type v: ndarray :return: Whether the value is a 6-vector or a valid 4x4 se(3) element :rtype: bool @@ -538,7 +555,7 @@ def Rx(cls, theta, unit="rad"): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Rx(θ)`` is an SE(3) rotation of θ radians about the x-axis + - ``Twist3.Rx(θ)`` is an se(3) rotation of θ radians about the x-axis - ``Twist3.Rx(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -569,7 +586,7 @@ def Ry(cls, theta, unit="rad", t=None): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis + - ``Twist3.Ry(θ)`` is an se(3) rotation of θ radians about the y-axis - ``Twist3.Ry(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -600,7 +617,7 @@ def Rz(cls, theta, unit="rad", t=None): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis + - ``Twist3.Rz(θ)`` is an se(3) rotation of θ radians about the z-axis - ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -765,14 +782,14 @@ def Rand( :type zrange: 2-element sequence, optional :param N: number of random transforms :type N: int - :return: SE(3) matrix - :rtype: SE3 instance + :return: 3D twist + :rtype: Twist3 instance - Return an SE3 instance with random rotation and translation. + Return a Twist3 instance with random rotation and translation. - - ``SE3.Rand()`` is a random SE(3) translation. - - ``SE3.Rand(N=N)`` is an SE3 object containing a sequence of N random - poses. + - ``Twist3.Rand()`` is a random se(3) twist. + - ``Twist3.Rand(N=N)`` is a Twist3 object containing a sequence of N random + twists. Example: @@ -781,7 +798,7 @@ def Rand( >>> from spatialmath import Twist3 >>> Twist3.Rand(N=2) - :seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand` + :seealso: :meth:`~spatialmath.pose3d.SE3.Rand` """ from spatialmath.pose3d import SO3 @@ -792,7 +809,7 @@ def Rand( low=yrange[0], high=yrange[1], size=N ) # random values in the range Z = np.random.uniform( - low=yrange[0], high=zrange[1], size=N + low=zrange[0], high=zrange[1], size=N ) # random values in the range R = SO3.Rand(N=N) @@ -886,12 +903,13 @@ def Ad(self): >>> S = Twist3.Rx(0.3) >>> S.Ad() - .. note:: This method computes the equivalent SE(3) matrix, then the adjoint - of that. + .. note:: Equivalent to, but faster than, ``self.SE3().Ad()`` -- computes + the adjoint directly from the twist's exponential without + constructing an intermediate ``SE3`` instance. :seealso: :func:`Twist3.ad`, :func:`Twist3.SE3`, :func:`Twist3.exp` """ - return self.SE3().Ad() + return smb.tr2adjoint(smb.trexp(self.S, check=False)) def skewa(self): """ @@ -1028,7 +1046,7 @@ def exp(self, theta=1, unit="rad"): """ Exponentiate a 3D twist - :param theta: rotation magnitude, defaults to None + :param theta: rotation magnitude, defaults to 1 :type theta: float, optional :param units: rotational units, defaults to 'rad' :type units: str, optional @@ -1037,7 +1055,7 @@ def exp(self, theta=1, unit="rad"): - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, + - ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` If ``len(X)==1`` and ``len(θ)==N`` then the resulting SE3 object has @@ -1107,6 +1125,7 @@ def __mul__( Twist3 scalar Twist3 element-wise product scalar Twist3 Twist3 element-wise product Twist3 SE3 Twist3 exponential x SE3 + Twist3 SpatialVector SpatialVector adjoint/coadjoint product ======== ==================== =================== ======================== .. note:: @@ -1114,7 +1133,10 @@ def __mul__( #. scalar x Twist is handled by ``__rmul__`` #. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix - #. Any other input combinations result in a ValueError. + #. ``Twist3 * SpatialVector`` (velocity, acceleration, force, momentum) + is handled by falling back to + :meth:`~spatialmath.spatialvector.SpatialVector.__rmul__`. + #. Any other input combination raises a ``TypeError``. For pose composition the ``left`` and ``right`` operands may be a sequence @@ -1147,7 +1169,11 @@ def __mul__( # return Twist(left.S * right) return Twist3(left.binop(right, lambda x, y: x * y)) else: - raise ValueError("twist *, incorrect right operand") + # not a type we handle directly -- return NotImplemented so Python + # falls back to right.__rmul__(left), which SpatialVector (velocity, + # acceleration, force, momentum) defines. If right has no __rmul__ + # either, Python raises its own TypeError. + return NotImplemented def __rmul__( right, left @@ -1165,7 +1191,7 @@ def __rmul__( - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` """ if smb.isscalar(left): - return Twist3(right.S * left) + return Twist3(right.binop(left, lambda x, y: x * y)) else: raise ValueError("Twist3 *, incorrect left operand") @@ -1180,10 +1206,10 @@ def __str__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist3 - >>> x = Twist3.R([1,2,3], [4,5,6]) + >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> print(x) """ return "\n".join( @@ -1204,13 +1230,11 @@ def __repr__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist3 - >>> x = Twist3.R([1,2,3], [4,5,6]) + >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> x - >>> a.append(a) - >>> a """ if len(self) == 0: @@ -1257,12 +1281,25 @@ def _repr_pretty_(self, p, cycle): class Twist2(BaseTwist): + r""" + 2D twist class + + A Twist2 object holds the parameters of a twist, a representation of a + 2D rigid body transformation which is the unique elements of the Lie + algebra se(2) of the corresponding SE(2) matrix. It is a 3-vector + :math:`[\vec{v}, \omega]`. + + As for :class:`Twist3` it can be used as a *displacement*, where ``exp(S)`` + is the pose reached by following the motion, or as a *velocity*, where + ``exp(S * dt)`` is the pose change over a time ``dt``. Properties that + describe the axis, such as :attr:`pole`, are unchanged by scaling the twist. + """ + def __init__(self, arg=None, w=None, check=True): r""" Construct a new 2D Twist object - :type a: 2-element array-like - :return: 2D prismatic twist + :return: 2D twist :rtype: Twist2 instance - ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as @@ -1272,7 +1309,7 @@ def __init__(self, arg=None, w=None, check=True): - ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation, and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion - ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the - moment vector (1 element) and direction vector (2 elements). + moment vector (2 elements) and direction vector (1 element). :References: - Robotics, Vision & Control for Python, Section 2.2.2.4, P. Corke, Springer 2023. @@ -1280,7 +1317,7 @@ def __init__(self, arg=None, w=None, check=True): .. note:: Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational - components, ie. :math:`[\omega, \vec{v}]`. + components, ie. :math:`[\vec{v}, \omega]`. """ from spatialmath.pose2d import SE2 @@ -1330,12 +1367,12 @@ def isvalid(v, check=True): """ Test if matrix is valid twist - :param x: array to test - :type x: ndarray + :param v: array to test + :type v: ndarray :return: Whether the value is a 3-vector or a valid 3x3 se(2) element :rtype: bool - A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix, + A twist can be represented by a 3-vector or a 3x3 augmented skew symmetric matrix, for example: .. runblock:: pycon @@ -1371,7 +1408,7 @@ def UnitRevolute(cls, q): :param q: Point on the line of action :type q: array_like(2) - :return: 2D prismatic twist + :return: 2D revolute twist :rtype: Twist2 instance - ``Twist2.UnitRevolute(q)`` is a 2D Twist object representing rotation about the 2D point ``q``. @@ -1515,7 +1552,7 @@ def SE2(self, theta=1, unit="rad"): Convert 2D twist to SE(2) matrix :return: an SE(2) representation - :rtype: SE3 instance + :rtype: SE2 instance ``S.SE2()`` is an SE2 object representing the homogeneous transformation equivalent to the Twist2. This is the exponentiation of the twist vector. @@ -1528,12 +1565,12 @@ def SE2(self, theta=1, unit="rad"): >>> S = Twist2.UnitPrismatic([1,2]) >>> S.SE2() - :seealso: :func:`Twist3.exp` + :seealso: :func:`Twist2.exp` """ from spatialmath.pose2d import SE2 if unit != "rad" and any(s.isprismatic for s in self): - print("Twist3.exp: using degree mode for a prismatic twist") + print("Twist2.SE2: using degree mode for a prismatic twist") theta = smb.getunit(theta, unit) @@ -1576,7 +1613,7 @@ def exp(self, theta=1, unit="rad"): r""" Exponentiate a 2D twist - :param theta: rotation magnitude, defaults to None + :param theta: rotation magnitude, defaults to 1 :type theta: float, optional :param unit: rotational units, defaults to 'rad' :type unit: str, optional @@ -1585,7 +1622,7 @@ def exp(self, theta=1, unit="rad"): - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, + - ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` Example: @@ -1720,7 +1757,7 @@ def Tx(cls, x): :return: 2D twist vector :rtype: Twist2 instance - `Twist2.Tx(x)` is an se(2) translation of ``x`` along the x-axis + ``Twist2.Tx(x)`` is an se(2) translation of ``x`` along the x-axis Example: @@ -1746,7 +1783,7 @@ def Ty(cls, y): :return: 2D twist vector :rtype: Twist2 instance - `Twist2.Ty(y) is an se(2) translation of ``y`` along the y-axis + ``Twist2.Ty(y)`` is an se(2) translation of ``y`` along the y-axis Example: @@ -1825,11 +1862,25 @@ def __mul__( else: raise ValueError("Twist2 *, incorrect right operand") - def __rmul(self, left): + def __rmul__( + right, left + ): # lgtm[py/not-named-self] pylint: disable=no-self-argument + """ + Overloaded ``*`` operator + + :arg right: right multiplicand + :arg left: left multiplicand + :return: product + :raises: ValueError + + Left-multiplication by a scalar + + - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` + """ if smb.isscalar(left): - return Twist2(self.S * left) + return Twist2(right.binop(left, lambda x, y: x * y)) else: - raise ValueError("twist *, incorrect left operand") + raise ValueError("Twist2 *, incorrect left operand") def __str__(self): """ @@ -1842,8 +1893,9 @@ def __str__(self): Example: - .. runblock: pycon + .. runblock:: pycon + >>> from spatialmath import Twist2 >>> x = Twist2([1,2,3]) >>> print(x) """ @@ -1858,13 +1910,11 @@ def __repr__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist2 >>> x = Twist2([1,2,3]) >>> x - >>> a.append(a) - >>> a """ 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_environment.py b/tests/base/test_environment.py new file mode 100644 index 00000000..a9911b08 --- /dev/null +++ b/tests/base/test_environment.py @@ -0,0 +1,64 @@ +import builtins +import sys + +import pytest + +from spatialmath.base import environment, isnotebook + +# These tests are mocked: a fake get_ipython() and sys.modules / sys.platform +# stand in for the real front ends, which cannot be run under CI. + + +def fake_shell(name): + shell = type(name, (), {})() + return lambda: shell + + +@pytest.fixture +def plain(monkeypatch): + """Standard interpreter: no IPython, not Colab, not Pyodide""" + monkeypatch.delattr(builtins, "get_ipython", raising=False) + monkeypatch.delitem(sys.modules, "google.colab", raising=False) + monkeypatch.setattr(sys, "platform", "linux") + return monkeypatch + + +def test_terminal(plain): + assert environment() == "terminal" + assert not isnotebook() + + +def test_ipython_terminal(plain): + plain.setattr( + builtins, "get_ipython", fake_shell("TerminalInteractiveShell"), raising=False + ) + assert environment() == "ipython" + assert not isnotebook() + + +def test_jupyter(plain): + plain.setattr( + builtins, "get_ipython", fake_shell("ZMQInteractiveShell"), raising=False + ) + assert environment() == "jupyter" + assert isnotebook() + + +def test_unknown_shell(plain): + plain.setattr(builtins, "get_ipython", fake_shell("SomethingElse"), raising=False) + assert environment() == "terminal" + assert not isnotebook() + + +def test_colab(plain): + # Colab's shell is not a ZMQInteractiveShell, so detect it by the module + plain.setattr(builtins, "get_ipython", fake_shell("Shell"), raising=False) + plain.setitem(sys.modules, "google.colab", object()) + assert environment() == "colab" + assert isnotebook() + + +def test_jupyterlite(plain): + plain.setattr(sys, "platform", "emscripten") + assert environment() == "jupyterlite" + assert isnotebook() diff --git a/tests/base/test_graphics.py b/tests/base/test_graphics.py index 17b88735..2d5223c4 100644 --- a/tests/base/test_graphics.py +++ b/tests/base/test_graphics.py @@ -46,6 +46,32 @@ class TestGraphics(unittest.TestCase): def teardown_method(self, method): plt.close("all") + def test_expand_dims_unspecified(self): + self.assertIsNone(expand_dims()) + for nd in (2, 3): + with self.subTest(nd=nd): + self.assertIsNone(expand_dims(nd=nd)) + self.assertIsNone(expand_dims(None, nd=nd)) + + def test_expand_dims_specified(self): + for nd in (2, 3): + with self.subTest(nd=nd): + # Zero is an explicit extent, not an unspecified one. + np.testing.assert_array_equal(expand_dims(0, nd), [0, 0] * nd) + np.testing.assert_array_equal(expand_dims(3, nd), [-3, 3] * nd) + np.testing.assert_array_equal(expand_dims([-2, 5], nd), [-2, 5] * nd) + limits = np.arange(2 * nd) + np.testing.assert_array_equal(expand_dims(limits, nd), limits) + with self.assertRaises(ValueError): + expand_dims([1, 2, 3], nd) + + def test_expand_dims_invalid_nd(self): + for nd in (1, 4): + for dim in (None, 1): + with self.subTest(nd=nd, dim=dim): + with self.assertRaisesRegex(ValueError, "nd is 2 or 3"): + expand_dims(dim, nd) + 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]) @@ -56,7 +82,9 @@ def test_expand_dims(self): 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]) + 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]) diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index 976b1047..31d3b6eb 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -12,7 +12,7 @@ import unittest from math import pi import math -from scipy.linalg import logm +from scipy.linalg import expm, logm import pytest import sys @@ -88,6 +88,25 @@ def test_trlog2(self): nt.assert_array_almost_equal(trlog2(T, twist=True), vexa(logm(T))) + def test_trlog2_quadrants(self): + for c, s in ( + (1, 0), + (math.sqrt(3) / 2, 0.5), + (-0.5, math.sqrt(3) / 2), + (-0.5, -math.sqrt(3) / 2), + (-1, 0), + (0, 1), + (0, -1), + ): + R = np.array([[c, -s], [s, c]]) + for T in (R, rt2tr(R, [1, 2])): + for twist in (False, True): + with self.subTest(c=c, s=s, shape=T.shape, twist=twist): + S = trlog2(T, twist=twist) + if twist: + S = skew(S) if T.shape == (2, 2) else skewa(S) + nt.assert_allclose(expm(S), T, atol=1e-12, rtol=0) + def test_trexp2(self): R = trexp2(skew(0.5)) nt.assert_array_almost_equal(R, rot2(0.5)) diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index 1fca4402..ca1675dd 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -47,6 +47,43 @@ def test_line_distance_parallel_tolerance(): assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) +@pytest.mark.parametrize( + "point, direction1, direction2", + [ + ([0, 0, 0], [1, 0, 0], [0, 1, 0]), + ([1, 2, 3], [1, 0, 0], [1, 1, 0]), + ([-3, 1, 2], [1, 2, 3], [-2, 1, 1]), + ], + ids=["origin", "oblique", "spatial"], +) +@pytest.mark.parametrize( + "scale1, scale2", [(1, 1), (2, 5), (-2, 0.25), (1e-8, 1e-8), (1e-8, 1e8)] +) +def test_line_intersection_point(point, direction1, direction2, scale1, scale2): + point = np.array(point) + direction1, direction2 = np.array(direction1), np.array(direction2) + line1 = Line3.PointDir(point + 2 * direction1, scale1 * direction1) + line2 = Line3.PointDir(point - 3 * direction2, scale2 * direction2) + + for first, second in [(line1, line2), (line2, line1)]: + intersection = first.intersects(second) + assert intersection is not None + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, rtol=0, atol=1e-12) + + +@pytest.mark.parametrize( + "point, direction", + [([0, 1, 0], [1, 0, 0]), ([2, 0, 0], [-2, 0, 0]), ([0, 0, 1], [0, 1, 0])], + ids=["parallel", "coincident", "skew"], +) +def test_line_intersection_none(point, direction): + line1 = Line3.PointDir([0, 0, 0], [1, 0, 0]) + line2 = Line3.PointDir(point, direction) + assert line1.intersects(line2) is None + assert line2.intersects(line1) is None + + class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): @@ -405,5 +442,33 @@ def test_methods(self): # py.intersect_plane(plane) +@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) +@pytest.mark.parametrize( + "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] +) +@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) +def test_three_plane_intersection(point, normals, scales): + normals = normals * np.array(scales)[:, None] + planes = [Plane3.PointNormal(point, normal) for normal in normals] + intersection = Plane3.intersection(*planes) + + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, atol=1e-12) + # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. + nt.assert_allclose( + normals @ intersection + np.array([plane.d for plane in planes]), + 0, + atol=1e-12, + ) + nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) + + +@pytest.mark.parametrize("offset", [0, 1]) +def test_three_plane_intersection_singular(offset): + planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] + with pytest.raises(np.linalg.LinAlgError): + Plane3.intersection(*planes) + + if __name__ == "__main__": unittest.main() diff --git a/tests/test_import.py b/tests/test_import.py new file mode 100644 index 00000000..e7e50fc4 --- /dev/null +++ b/tests/test_import.py @@ -0,0 +1,16 @@ +import subprocess +import sys +import unittest + + +class TestImport(unittest.TestCase): + def test_import_does_not_load_scipy(self): + # scipy is slow to import and only a few functions need it, so it is + # imported by those functions and not when spatialmath is imported + code = "import sys, spatialmath; sys.exit(any(m.split('.')[0] == 'scipy' for m in sys.modules))" + result = subprocess.run([sys.executable, "-c", code]) + self.assertEqual(result.returncode, 0, "import spatialmath loaded scipy") + + +if __name__ == "__main__": # pragma nocover + unittest.main() 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_plane_contains.py b/tests/test_plane_contains.py new file mode 100644 index 00000000..df2a6604 --- /dev/null +++ b/tests/test_plane_contains.py @@ -0,0 +1,39 @@ +import numpy as np +import pytest + +from spatialmath import Plane3 + + +@pytest.mark.parametrize("scale", [1, -1, 2.5]) +@pytest.mark.parametrize("factory", ["coefficients", "point-normal", "three-points"]) +def test_translated_plane_contains_points(factory, scale): + point = np.array([2.0, 3.0, 4.0]) + tangent1 = np.array([1.0, -1.0, 0.0]) + tangent2 = np.array([0.0, 1.0, -1.0]) + normal = np.cross(tangent1, tangent2) + if factory == "coefficients": + plane = Plane3(scale * np.r_[normal, -normal @ point]) + elif factory == "point-normal": + plane = Plane3.PointNormal(point, scale * normal) + else: + plane = Plane3.ThreePoints( + np.column_stack((point, point + scale * tangent1, point + tangent2)) + ) + + for candidate in (point, point + tangent1, point + tangent2): + assert plane.contains(candidate) + assert not plane.contains(point + normal) + # The old sign incorrectly accepted the reflected plane. + assert not plane.contains(-point) + + +def test_plane_contains_tolerance(): + plane = Plane3([1, 0, 0, -2]) + assert plane.contains([2 + 1e-10, 0, 0], tol=1e6) + assert not plane.contains([2 + 1e-10, 0, 0]) + + +def test_plane_through_origin(): + plane = Plane3([0, 0, 1, 0]) + assert plane.contains([2, 3, 0]) + assert not plane.contains([2, 3, 1]) diff --git a/tests/test_plane_line_point.py b/tests/test_plane_line_point.py new file mode 100644 index 00000000..ef880491 --- /dev/null +++ b/tests/test_plane_line_point.py @@ -0,0 +1,26 @@ +import numpy as np +import pytest + +from spatialmath import Line3, Plane3 + + +@pytest.mark.parametrize( + "anchor,direction,point", + [ + ([1, 2, 3], [1, 0, 0], [1, 4, 3]), + ([2, -1, 4], [1, 2, -1], [3, 2, 5]), + ([0, 0, 0], [0, 0, 1], [2, 3, 4]), + ], +) +@pytest.mark.parametrize("scale", [1, -2, 0.25]) +def test_line_point_plane_contains_translated_line(anchor, direction, point, scale): + anchor = np.asarray(anchor, dtype=float) + direction = np.asarray(direction, dtype=float) * scale + line = Line3.PointDir(anchor, direction) + plane = Plane3.LinePoint(line, np.asarray(point, dtype=float)) + + # Use the plane equation directly, independent of Plane3.contains. + for p in [point, anchor, anchor + direction, anchor - 3 * direction]: + np.testing.assert_allclose(plane.n @ p + plane.d, 0, atol=1e-12) + expected_normal = np.cross(direction, np.asarray(point) - anchor) + np.testing.assert_allclose(plane.n, expected_normal, atol=1e-12) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index d4b16668..ea1779cc 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -793,6 +793,26 @@ class TestSE3(unittest.TestCase): def tearDownClass(cls): plt.close("all") + def test_constructor_stacked_array(self): + # an (N, 4, 4) ndarray is a sequence of N poses, like a list of 4x4 arrays + poses = [SE3.Rx(0.3), SE3(1, 2, 3) * SE3.Ry(0.4), SE3.Rz(-0.2)] + stacked = np.array([T.A for T in poses]) + for check in (True, False): + T = SE3(stacked, check=check) + self.assertEqual(len(T), 3) + for k in range(3): + nt.assert_array_almost_equal(T[k].A, poses[k].A) + self.assertEqual(len(SE3(np.empty((0, 4, 4)))), 0) + self.assertEqual(len(SO3(np.array([SO3.Rx(0.1).A, SO3.Ry(0.2).A]))), 2) + + # stacked matrices that are not valid poses are still rejected by check=True + with self.assertRaises(ValueError): + SE3(2 * stacked) + # and a stack of matrices of the wrong size is rejected whatever the value of check + for check in (True, False): + with self.assertRaises(ValueError): + SE3(np.ones((3, 3, 3)), check=check) + def test_constructor(self): # null constructor R = SE3() diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 8e291573..40c5d10b 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -3,6 +3,7 @@ import numpy.testing as nt import unittest from unittest.mock import patch +import warnings from spatialmath import * from spatialmath.base import * @@ -783,6 +784,31 @@ def test_interp_prepares_slerp_once(self): self.assertEqual(len(interpolate()), 5) self.assertEqual(dot.call_count, 1) + def test_interp1_shortest_deprecated(self): + # shortest never had an effect on interp1: every UnitQuaternion has + # a non-negative scalar part by construction (qunit()), and interp1 + # always interpolates from the identity quaternion, whose dot + # product with any unit quaternion is just that quaternion's own + # (always non-negative) scalar part. So the "long way round" branch + # shortest exists to avoid is unreachable. Pin that True/False give + # identical results, that it warns, and that leaving it at its + # default doesn't. + q = UnitQuaternion.Rx(4.5) # would be "the long way" if reachable + + for s in (0, 0.25, 0.5, 0.75, 1): + qcompare(q.interp1(s, shortest=False), q.interp1(s, shortest=True)) + + for a, b in zip(q.interp1(11, shortest=False), q.interp1(11, shortest=True)): + qcompare(a, b) + + with self.assertWarns(DeprecationWarning): + q.interp1(0.5, shortest=True) + + with warnings.catch_warnings(): + warnings.simplefilter("error") + q.interp1(0.5) # default shortest=False must not warn + q.interp1(0.5, shortest=False) + def test_increment(self): q = UnitQuaternion() @@ -928,6 +954,9 @@ def test_constructor(self): # tc.verifyError( @() Quaternion(2), 'SMTB:Quaternion:badarg') # tc.verifyError( @() Quaternion([1, 2, 3]), 'SMTB:Quaternion:badarg') + # an array that is not a 4-vector must not give an empty Quaternion + self.assertNotEqual(len(Quaternion(np.random.rand(3, 4))), 0) + def test_string(self): u = Quaternion() diff --git a/tests/test_spatialvector.py b/tests/test_spatialvector.py index af853329..18c81c42 100644 --- a/tests/test_spatialvector.py +++ b/tests/test_spatialvector.py @@ -225,8 +225,23 @@ def test_products(self): # v x v = a *, v x F6 = a # a x I, I x a # v x I, I x v - # twist x v, twist x a, twist x F - pass + + # twist x v, twist x a, twist x F, twist x momentum + T = SE3(1, 2, 3) * SE3.Rx(0.3) + S = Twist3(T) + + v = SpatialVelocity([1, 2, 3, 4, 5, 6]) + a = SpatialAcceleration([1, 2, 3, 4, 5, 6]) + f = SpatialForce([1, 2, 3, 4, 5, 6]) + m = SpatialMomentum([1, 2, 3, 4, 5, 6]) + + # Twist3 * X must agree with the equivalent SE3 * X: same + # underlying transform, two ways of expressing it. + for x in (v, a, f, m): + result = S * x + expected = T * x + self.assertIsInstance(result, type(x)) + nt.assert_almost_equal(result.A, expected.A) def test_spatial_transform_duality(self): # A pose transform acts on motion (velocity/acceleration) by the diff --git a/tests/test_twist.py b/tests/test_twist.py index dcfb0fd1..894e852b 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -67,6 +67,12 @@ def test_conversion_SE3(self): T = SE3.Rx(0) * SE3(1, 2, 3) array_compare(Twist3(T).SE3(), T) + # from an SE(3) matrix, with and without checking + for check in (True, False): + tw = Twist3(T.A, check=check) + self.assertEqual(len(tw), 1) + array_compare(tw.SE3(), T) + def test_conversion_se3(self): s = [1, 2, 3, 4, 5, 6] x = Twist3(s) @@ -83,6 +89,12 @@ def test_conversion_se3(self): ), ) + # from an se(3) matrix, with and without checking + for check in (True, False): + tw = Twist3(x.skewa(), check=check) + self.assertEqual(len(tw), 1) + array_compare(tw, x) + def test_conversion_Plucker(self): pass @@ -190,6 +202,32 @@ def test_pitch(self): tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0], pitch=3) self.assertAlmostEqual(tw.pitch, 3.0) + def test_Ad(self): + # pure rotation: Ad is block-diagonal [[R,0],[0,R]], no translation + # coupling -- hand-verified ground truth, not derived from Ad() itself. + R = SE3.Rx(0.4).R + S = Twist3(SE3.Rx(0.4)) + expected = np.zeros((6, 6)) + expected[:3, :3] = R + expected[3:, 3:] = R + nt.assert_almost_equal(S.Ad(), expected) + + # pure translation: Ad couples translation into the top-right block + # via skew(t), identity rotation blocks -- also hand-verified. + t = np.r_[1, 2, 3] + S = Twist3(SE3(t)) + expected = np.eye(6) + expected[:3, 3:] = skew(t) + nt.assert_almost_equal(S.Ad(), expected) + + # general case: cross-check against SE3.Ad(), computed from the same + # twist's own exponential, for several random transforms. + for _ in range(20): + T = SE3.Rand() + S = Twist3(T) + nt.assert_almost_equal(S.Ad(), S.SE3().Ad()) + nt.assert_almost_equal(S.Ad(), T.Ad()) + def test_arith(self): # check overloaded * @@ -293,6 +331,12 @@ def test_conversion_SE2(self): self.assertIsInstance(tw.SE2(), SE2) self.assertEqual(len(tw.SE2()), 1) + # from an SE(2) matrix, with and without checking + for check in (True, False): + tw = Twist2(T.A, check=check) + self.assertEqual(len(tw), 1) + array_compare(tw.SE2(), T) + def test_conversion_se2(self): s = [1, 2, 3] x = Twist2(s) @@ -301,6 +345,12 @@ def test_conversion_se2(self): x.skewa(), np.array([[0.0, -3.0, 1.0], [3.0, 0.0, 2.0], [0.0, 0.0, 0.0]]) ) + # from an se(2) matrix, with and without checking + for check in (True, False): + tw = Twist2(x.skewa(), check=check) + self.assertEqual(len(tw), 1) + array_compare(tw, x) + def test_list_constuctor(self): x = Twist2([1, 0, 0]) diff --git a/tests/test_twist_sequence_predicates.py b/tests/test_twist_sequence_predicates.py new file mode 100644 index 00000000..8508a2a6 --- /dev/null +++ b/tests/test_twist_sequence_predicates.py @@ -0,0 +1,58 @@ +import numpy as np +import pytest + +from spatialmath import Twist2, Twist3 + + +def _sequence(cls): + """A prismatic, a revolute and a general twist, in that order.""" + if cls is Twist2: + vectors = [[1, 0, 0], [0, 0, 1], [1, 2, 3]] + else: + vectors = [[1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1], [1, 2, 3, 0.1, 0.2, 0.3]] + return cls([np.array(v, dtype=float) for v in vectors]) + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_isprismatic_sequence(cls): + """isprismatic gives one result per twist, as for a single twist.""" + twists = _sequence(cls) + assert list(twists.isprismatic) == [True, False, False] + assert [t.isprismatic for t in twists] == list(twists.isprismatic) + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_isrevolute_sequence(cls): + """isrevolute gives one result per twist, as for a single twist.""" + twists = _sequence(cls) + assert list(twists.isrevolute) == [False, True, False] + assert [t.isrevolute for t in twists] == list(twists.isrevolute) + + +def test_twist2_se2_degree_warning_covers_sequence(capsys): + """Degree mode warns when any twist in the sequence is prismatic.""" + _sequence(Twist2).SE2(30, unit="deg") + assert "Twist2.SE2" in capsys.readouterr().out + + Twist2([np.array([1.0, 2.0, 3.0])] * 2).SE2(30, unit="deg") + assert capsys.readouterr().out == "" + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_scalar_times_twist(cls): + """``s * X`` scales each twist, the same as ``X * s``.""" + twists = _sequence(cls) + for left in (2 * twists, 2.0 * twists): + right = twists * 2 + assert len(left) == len(twists) + for a, b, t in zip(left, right, twists): + np.testing.assert_allclose(a.S, 2 * t.S) + np.testing.assert_allclose(a.S, b.S) + + +def test_twist3_rand_honours_zrange(): + """The z lower bound comes from zrange, not yrange.""" + np.random.seed(0) + poses = Twist3.Rand(N=200, xrange=(0, 1), yrange=(-3, -2), zrange=(5, 6)).exp() + z = np.array([p.t[2] for p in poses]) + assert z.min() >= 5 and z.max() <= 6