diff --git a/.github/workflows/master.yml b/.github/workflows/master.yml index 210d1237..2406182d 100644 --- a/.github/workflows/master.yml +++ b/.github/workflows/master.yml @@ -19,7 +19,7 @@ jobs: matrix: os: [windows-latest, ubuntu-22.04, macos-latest] # https://devguide.python.org/versions/ - python-version: ["3.10", "3.11", "3.12", "3.13"] + python-version: ["3.10", "3.11", "3.12", "3.13", "3.14"] exclude: - os: windows-latest python-version: "3.11" diff --git a/.github/workflows/sphinx.yml b/.github/workflows/sphinx.yml index 445d7181..13c0b836 100644 --- a/.github/workflows/sphinx.yml +++ b/.github/workflows/sphinx.yml @@ -16,7 +16,6 @@ jobs: run: | python -m pip install --upgrade pip pip install .[dev,docs] - pip install git+https://github.com/petercorke/sphinx-autorun.git pip install sympy sudo apt-get install graphviz - name: Build docs diff --git a/benchmarks/benchmark_smtb.py b/benchmarks/benchmark_smtb.py new file mode 100755 index 00000000..abed7abb --- /dev/null +++ b/benchmarks/benchmark_smtb.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Micro-benchmarks for spatialmath: base functions and classes for SO(3), +SE(3), quaternions and twists, plus a NumPy baseline. Run from a checkout:: + + python benchmarks/benchmark_smtb.py + +The output starts with a summary of the machine and package versions, so a +pasted table is self-describing. + +Created on Fri Apr 10 14:22:36 2020 + +@author: Peter Corke +""" + +import os +import platform +import subprocess +import timeit as _timeit + +import numpy as np +from ansitable import ANSITable, Column + +import spatialmath + +N = 10_000 +REPEATS = 5 + +table = None + + +def cpu_info() -> str: + """Best-effort, portable one-line CPU description + + :return: CPU name and core count, plus clock speed if available + :rtype: str + + No new hard dependency: psutil is used for clock speed only if already + installed. Some platforms (e.g. Apple Silicon) don't expose a single + meaningful clock speed, so a missing or nonsensical reading is omitted. + """ + system = platform.system() + name = None + + if system == "Darwin": + try: + name = subprocess.check_output( + ["sysctl", "-n", "machdep.cpu.brand_string"], text=True + ).strip() + except Exception: + pass + elif system == "Linux": + try: + with open("/proc/cpuinfo") as f: + for line in f: + if line.lower().startswith("model name"): + name = line.split(":", 1)[1].strip() + break + except Exception: + pass + elif system == "Windows": + name = platform.processor() or None + + if not name: + name = platform.processor() or platform.machine() or "unknown CPU" + + info = f"{name} ({os.cpu_count() or '?'} cores)" + + try: + import psutil + + freq = psutil.cpu_freq() + # real clock speeds are hundreds to thousands of MHz; some platforms + # report bogus single-digit values instead of raising + if freq and freq.max and freq.max > 100: + info += f", {freq.max:.0f} MHz" + except Exception: + pass + + return info + + +def print_machine_summary() -> None: + print(f"CPU: {cpu_info()}") + print(f"OS: {platform.platform()}") + print(f"Python: {platform.python_version()}") + print(f"numpy: {np.__version__}") + print(f"spatialmath: {spatialmath.__version__}") + print(f"Timing: min of {REPEATS} repeats x {N} calls") + + +def new_table(): + return ANSITable( + Column("Operation", headalign="^"), + Column("Time (μs)", headalign="^", fmt="{:.2f}"), + border="thick", + ) + + +def timeit(stmt, label, setup): + # min-of-repeats suppresses system jitter (GC pauses, OS scheduling) + # much better than a single long run of the same total work + t = min(_timeit.repeat(stmt=stmt, setup=setup, number=N, repeat=REPEATS)) + table.row(label, t / N * 1e6) + + +def section(title): + # print the previous table (if any), then start a fresh one under a + # plain header naming the category of test that follows + global table + if table is not None: + table.print() + print(f"\n{title}\n") + table = new_table() + + +print_machine_summary() + +# ------------------------------------------------------------------------- # +transforms_setup = ''' +from spatialmath import SE3 +from spatialmath import base + +import numpy as np +from collections import namedtuple +Rt = namedtuple('Rt', 'R t') +X1 = SE3.Rand() +X2 = SE3.Rand() +T1 = X1.A +T2 = X2.A +R1 = base.t2r(T1) +R2 = base.t2r(T2) +t1 = base.transl(T1) +t2 = base.transl(T2) +Rt1 = Rt(R1, t1) +Rt2 = Rt(R2, t2) +v = np.r_[1,2,3] +v2 = np.r_[1,2,3, 1] +''' + +section("SO(3) / SE(3) base functions") + +timeit('base.getvector(0.2)', "base.getvector(x)", transforms_setup) +timeit('base.rotx(0.2, unit="rad")', "base.rotx(x)", transforms_setup) +timeit('base.trotx(0.2, unit="rad")', "base.trotx(x)", transforms_setup) +timeit('base.t2r(T1)', "base.t2r(T1)", transforms_setup) +timeit('base.r2t(R1)', "base.r2t(R1)", transforms_setup) +timeit('T1 @ T2', "T1 @ T2 (4x4)", transforms_setup) +timeit( + 'T1[:3,:3] @ T2[:3,:3] + T1[:3,:3] @ T2[:3,3]', + "T1 @ T2 decomposed (slice)", + transforms_setup, +) +timeit( + '(Rt1.R @ Rt2.R, Rt1.R @ Rt2.t)', + "T1 @ T2 decomposed (namedtuple)", + transforms_setup, +) +timeit('base.trinv(T1)', "base.trinv(T1)", transforms_setup) +timeit( + '(Rt1.R.T, -Rt1.R.T @ Rt1.t)', "T1 inverse decomposed (R,t)", transforms_setup +) +timeit('np.linalg.inv(T1)', "np.linalg.inv(T1)", transforms_setup) +timeit('T1 @ v2', "T1 @ v2 (4,4)*(4,)", transforms_setup) + +# ------------------------------------------------------------------------- # +section("SE3 class") + +timeit('SE3()', "SE3()", transforms_setup) +timeit('SE3.Rx(0.2)', "SE3.Rx(x)", transforms_setup) +timeit('SE3(T1)', "SE3(T1)", transforms_setup) +timeit('SE3(T1, check=False)', "SE3(T1, check=False)", transforms_setup) +timeit('SE3([T1], check=False)', "SE3([T1], check=False)", transforms_setup) +timeit('T1[:3,:3]', "T1[:3,:3] (raw slice)", transforms_setup) +timeit('X1.A', "X1.A (property)", transforms_setup) +timeit('X1 * X2', "X1 * X2", transforms_setup) +timeit('X1 @ X2', "X1 @ X2 (normalized)", transforms_setup) +timeit('X1.inv()', "X1.inv()", transforms_setup) +timeit('X1 * v', "X1 * v", transforms_setup) +timeit('a = X1.log()', "X1.log()", transforms_setup) +timeit('SE3.RPY([0.1, 0.2, 0.3])', "SE3.RPY(rpy)", transforms_setup) +timeit('X1.rpy()', "X1.rpy()", transforms_setup) +timeit('SE3.Eul([0.1, 0.2, 0.3])', "SE3.Eul(eul)", transforms_setup) +timeit('X1.eul()', "X1.eul()", transforms_setup) +timeit('X1.UnitQuaternion()', "X1.UnitQuaternion()", transforms_setup) + +# ------------------------------------------------------------------------- # +quat_setup = ''' +from spatialmath import base +from spatialmath import UnitQuaternion, SO3 +import numpy as np +q1 = base.qrand() +q2 = base.qrand() +v = np.r_[1,2,3] +R1 = SO3.Rand().R +Q1 = UnitQuaternion.Rx(0.2) +Q2 = UnitQuaternion.Ry(0.3) +''' + +section("Quaternion base functions") + +timeit('a = base.qqmul(q1,q2)', "base.qqmul(q1, q2)", quat_setup) +timeit('a = base.qvmul(q1,v)', "base.qvmul(q1, v)", quat_setup) + +# ------------------------------------------------------------------------- # +section("UnitQuaternion class") + +timeit('a = UnitQuaternion()', "UnitQuaternion()", quat_setup) +timeit('a = UnitQuaternion.Rx(0.2)', "UnitQuaternion.Rx(x)", quat_setup) +timeit('a = UnitQuaternion(R1)', "UnitQuaternion(R1)", quat_setup) +timeit('a = Q1.R', "Q1.R (property)", quat_setup) +timeit('a = Q1 * Q2', "Q1 * Q2", quat_setup) +timeit('a = Q1 * v', "Q1 * v", quat_setup) +timeit('a = Q1.SE3()', "Q1.SE3()", quat_setup) + +# ------------------------------------------------------------------------- # +twist_setup = ''' +from spatialmath import SE3, Twist3 +from spatialmath import base +import numpy as np +from math import cos +S1 = Twist3(SE3.Rand()) +S2 = Twist3(SE3.Rand()) +X1 = SE3.Rand() +T1 = X1.A +A1 = X1.Ad() +se3 = S1.skewa() +s = np.r_[1,2,3,4,5,6] +v = np.r_[1,2,3] +''' + +section("Twist / exponential-map base functions") + +timeit('a = base.skew(v)', "base.skew(v)", twist_setup) +timeit('a = base.skewa(s)', "base.skewa(s)", twist_setup) +timeit('a = base.vexa(se3)', "base.vexa(se3)", twist_setup) +timeit('a = base.trlog(T1)', "base.trlog(T1)", twist_setup) +timeit('a = base.trlog(T1, twist=True)', "base.trlog(T1, twist=True)", twist_setup) +timeit('a = base.trexp(se3)', "base.trexp(se3)", twist_setup) +timeit('a = base.rodrigues(v)', "base.rodrigues(v)", twist_setup) +timeit('a = A1 @ s', "A1 @ s (6,6)*(6,)", twist_setup) +timeit('a = cos(0.3)', "math.cos(x)", twist_setup) +timeit('a = np.cos(0.3)', "np.cos(x)", twist_setup) + +# ------------------------------------------------------------------------- # +section("Twist3 class") + +# NB: Twist3 * Twist3 and Twist3.Ad() both round-trip through the SE3 +# exponential/logarithm maps (trexp/trlog above), which is why they cost +# several times more than a single trexp or trlog call - see +# claude-notes/twist3-timing-investigation.md for the profiling detail. + +timeit('a = Twist3()', "Twist3()", twist_setup) +timeit('a = Twist3(X1)', "Twist3(X1) (via log)", twist_setup) +timeit('a = S1.inv()', "S1.inv()", twist_setup) +timeit('a = S1 * S2', "S1 * S2 (product of exponentials)", twist_setup) +timeit('a = S1.Ad()', "S1.Ad() (via SE3 + tr2adjoint)", twist_setup) +timeit('a = S1.exp(1)', "S1.exp() -> SE3", twist_setup) + +# ------------------------------------------------------------------------- # +misc_setup = """ +from spatialmath import base +import numpy as np +s = np.r_[1.0,2,3,4,5,6] +s3 = np.r_[1.0,2,3] +a = np.r_[1.0, 2.0, 3.0] +b = np.r_[-5.0, 4.0, 3.0] + +A = np.random.randn(6,6) +As = (A + A.T) / 2 +bb = np.random.randn(6) +""" + +section("NumPy linear-algebra baseline") + +timeit("c = np.linalg.inv(As)", "np.linalg.inv(As)", misc_setup) +timeit("c = np.linalg.pinv(As)", "np.linalg.pinv(As)", misc_setup) +timeit("c = np.linalg.solve(As, bb)", "np.linalg.solve(As, b)", misc_setup) +timeit("c = np.cross(a,b)", "np.cross(a, b)", misc_setup) +timeit("c = base.cross(a,b)", "base.cross(a, b)", misc_setup) +timeit("a = np.inner(s,s).sum()", "np.inner(s, s).sum()", misc_setup) +timeit("a = np.linalg.norm(s) ** 2", "np.linalg.norm(s) ** 2", misc_setup) +timeit("a = base.normsq(s)", "base.normsq(s)", misc_setup) +timeit("a = (s ** 2).sum()", "(s ** 2).sum()", misc_setup) +timeit("a = np.sum(s ** 2)", "np.sum(s ** 2)", misc_setup) +timeit("a = np.linalg.norm(s)", "np.linalg.norm(s) [R6]", misc_setup) +timeit("a = base.norm(s)", "base.norm(s) [R6]", misc_setup) +timeit("a = np.linalg.norm(s3)", "np.linalg.norm(s3) [R3]", misc_setup) +timeit("a = base.norm(s3)", "base.norm(s3) [R3]", misc_setup) + +table.print() diff --git a/docs/source/conf.py b/docs/source/conf.py index b5fcf84a..d7ba0447 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -15,6 +15,7 @@ # sys.path.insert(0, os.path.abspath('.')) # sys.path.insert(0, os.path.abspath('..')) +from sphinx_codeautolink import clean_ipython, clean_pycon # -- Project information ----------------------------------------------------- @@ -47,9 +48,17 @@ "sphinx.ext.inheritance_diagram", "matplotlib.sphinxext.plot_directive", "sphinx_autodoc_typehints", - "sphinx_autorun", + "sphinx_pyrunblock", "sphinx.ext.intersphinx", "sphinx_favicon", + "sphinx_copybutton", + "sphinx_codeautolink", +] + +suppress_warnings = [ + "codeautolink.match_block", + "codeautolink.match_name", + "config.cache", # codeautolink_custom_blocks holds function refs, not picklable ] #'sphinx.ext.autosummary', # typehints_use_signature_return = True @@ -184,6 +193,24 @@ np.set_printoptions(precision=4, suppress=True) """ +# -------- sphinx-codeautolink options --------------------------------------------# + +codeautolink_custom_blocks = { + "pycon": clean_pycon, + "ipython": clean_ipython, + "ipython3": clean_ipython, +} +# Ensure pycon (Python console) blocks are included in the autolink search. +codeautolink_search_css_classes = ["highlight-python", "highlight-pycon"] + +# -------- sphinx-copybutton options ----------------------------------------------# +# Strip interactive prompts (Python and shell) when users copy code snippets. + +copybutton_prompt_text = r">>> |\.\.\. |\$ " +copybutton_prompt_is_regexp = True +copybutton_only_copy_prompt_lines = False +copybutton_remove_prompts = True + intersphinx_mapping = { "numpy": ("http://docs.scipy.org/doc/numpy/", None), "scipy": ("http://docs.scipy.org/doc/scipy/reference/", None), diff --git a/docs/source/intro.rst b/docs/source/intro.rst index 14672bd7..e1e9e93f 100644 --- a/docs/source/intro.rst +++ b/docs/source/intro.rst @@ -629,7 +629,7 @@ Graphics If ``matplotlib`` is installed then we can add 2D coordinate frames to a figure in a variety of styles: .. code-block:: python - :linenos: + :linenos: >>> trplot2( transl2(1,2), frame='A', rviz=True, width=1) >>> trplot2( transl2(3,1), color='red', arrow=True, width=3, frame='B') @@ -646,7 +646,7 @@ If a figure does not yet exist one is added. If a figure exists but there is no Similarly, we can plot 3D coordinate frames in a variety of styles: .. code-block:: python - :linenos: + :linenos: >>> trplot( transl(1,2,3), frame='A', rviz=True, width=1, dims=[0, 10, 0, 10, 0, 10]) >>> trplot( transl(3,1, 2), color='red', width=3, frame='B') diff --git a/pyproject.toml b/pyproject.toml index 1597ad25..d4e564ef 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -15,6 +15,8 @@ classifiers = [ "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", + "Programming Language :: Python :: 3.13", + "Programming Language :: Python :: 3.14", "License :: OSI Approved :: MIT License", "Operating System :: OS Independent", @@ -61,10 +63,12 @@ dev = [ docs = [ "sphinx", "sphinx-rtd-theme", - "sphinx-autorun", + "sphinx-pyrunblock", "sphinxcontrib-jsmath", "sphinx-favicon", "sphinx-autodoc-typehints", + "sphinx-copybutton", + "sphinx-codeautolink", ] ros-humble = [ diff --git a/spatialmath/DualQuaternion.py b/spatialmath/DualQuaternion.py index 5e606dcd..6e657192 100644 --- a/spatialmath/DualQuaternion.py +++ b/spatialmath/DualQuaternion.py @@ -117,7 +117,11 @@ def norm(self) -> Tuple[float, float]: """ a = self.real * self.real.conj() b = self.real * self.dual.conj() + self.dual * self.real.conj() - return (base.sqrt(a.s), base.sqrt(b.s)) + # a.s/b.s are mathematically guaranteed non-negative (they're the + # scalar part of q*conj(q)-like products), but floating-point + # rounding can leave a value like -1e-17 instead of exactly 0, + # which sqrt() rejects outright. Clamp away that noise. + return (base.sqrt(max(0.0, a.s)), base.sqrt(max(0.0, b.s))) def conj(self) -> Self: r""" @@ -208,8 +212,18 @@ def __mul__(left, right: Self) -> Self: # pylint: disable=no-self-argument return DualQuaternion(real, dual) elif isinstance(left, UnitDualQuaternion) and base.isvector(right, 3): v = base.getvector(right, 3) - vp = left * DualQuaternion.Pure(v) * left.conj() - return vp.dual.v + # NB: not the textbook q*P*conj(q) sandwich product. With this + # class's own dual-part embedding convention (__init__ builds + # dual = 0.5*Pure(t)*real, translation quaternion on the left), + # that sandwich's translation terms cancel exactly to zero: + # qr*conj(qd) + qd*conj(qr) == 0 for this embedding, leaving + # only the rotated point with no translation applied at all. + # SE3() already correctly extracts (R, t) from this same + # embedding (see its own derivation), so reuse it here rather + # than hand-deriving a second, convention-specific formula. + # Flatten to match this method's original flat-vector return + # convention (SE3.__mul__ returns a (3,1) column instead). + return (left.SE3() * v).flatten() def matrix(self) -> R8x8: """ @@ -343,7 +357,7 @@ def SE3(self) -> SE3: >>> print(T) >>> d = UnitDualQuaternion(T) >>> print(d) - >>> print(d.T) + >>> print(d.SE3()) """ R = base.q2r(self.real.A) t = 2 * self.dual * self.real.conj() diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py index 551481e1..97922905 100644 --- a/spatialmath/__init__.py +++ b/spatialmath/__init__.py @@ -51,5 +51,8 @@ import importlib.metadata __version__ = importlib.metadata.version("spatialmath-python") -except: - pass +except importlib.metadata.PackageNotFoundError: + # running from a source checkout without an installed/editable + # spatialmath-python distribution -- e.g. importing straight from + # the repo root + __version__ = "unknown" diff --git a/spatialmath/base/_types_311.py b/spatialmath/base/_types_311.py deleted file mode 100644 index bd1d64b9..00000000 --- a/spatialmath/base/_types_311.py +++ /dev/null @@ -1,157 +0,0 @@ -# for Python >= 3.9 - -from typing import ( - overload, - cast, - Union, - List, - Tuple, - Type, - TextIO, - Any, - Callable, - Optional, - Iterator, - Self, -) -from typing import Literal as L - -from numpy import ndarray, dtype, floating -from numpy.typing import NDArray, DTypeLike - -# array like - -# these are input to many functions in spatialmath.base, and can be a list, tuple or -# ndarray. The elements are generally float, but some functions accept symbolic -# arguments as well, which leads to a NumPy array with dtype=object. For now -# symbolics will throw a lint error. Possibly create variants ArrayLikeSym that -# admits symbols and can be used for those functions that accept symbols. -# -# The variants like ArrayLike2 indicate that a list, tuple or ndarray of -# length 2 is expected. Static checking of tuple length is possible, but not for lists. -# This might be possible in future versions of Python, but for now it is a hint to the -# coder about what is expected - -# cannot be a scalar -ArrayLikePure = Union[List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] - -ArrayLike = Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] -ArrayLike2 = Union[ - List[float], - Tuple[float, float], - ndarray[ - Tuple[L[2]], - dtype[floating], - ], -] -ArrayLike3 = Union[ - List[float], - Tuple[float, float, float], - ndarray[ - Tuple[L[3]], - dtype[floating], - ], -] -ArrayLike4 = Union[ - List[float], - Tuple[float, float, float, float], - ndarray[ - Tuple[L[4]], - dtype[floating], - ], -] -ArrayLike6 = Union[ - List[float], - Tuple[float, float, float, float, float, float], - ndarray[ - Tuple[L[6]], - dtype[floating], - ], -] - -# real vectors -R1 = ndarray[ - Tuple[L[1]], - dtype[floating], -] # R^1 -R2 = ndarray[ - Tuple[L[2]], - dtype[floating], -] # R^2 -R3 = ndarray[ - Tuple[L[3]], - dtype[floating], -] # R^3 -R4 = ndarray[ - Tuple[L[4]], - dtype[floating], -] # R^4 -R6 = ndarray[ - Tuple[L[6]], - dtype[floating], -] # R^6 - -R8 = ndarray[ - Tuple[L[8,]], - dtype[floating], -] # R^8 - -# real matrices -R1x1 = ndarray[Tuple[L[1], L[1]], dtype[floating]] # R^{1x1} matrix -R2x2 = ndarray[Tuple[L[2], L[2]], dtype[floating]] # R^{2x2} matrix -R3x3 = ndarray[Tuple[L[3], L[3]], dtype[floating]] # R^{3x3} matrix -R4x4 = ndarray[Tuple[L[4], L[4]], dtype[floating]] # R^{4x4} matrix -R6x6 = ndarray[Tuple[L[6], L[6]], dtype[floating]] # R^{6x6} matrix -R8x8 = ndarray[Tuple[L[8], L[8]], dtype[floating]] # R^{8x8} matrix -R1x3 = ndarray[Tuple[L[1], L[3]], dtype[floating]] # R^{1x3} row vector -R3x1 = ndarray[Tuple[L[3], L[1]], dtype[floating]] # R^{3x1} column vector -R1x2 = ndarray[Tuple[L[1], L[2]], dtype[floating]] # R^{1x2} row vector -R2x1 = ndarray[Tuple[L[2], L[1]], dtype[floating]] # R^{2x1} column vector - -# Points2 = ndarray[Tuple[L[2, Any]], dtype[floating]] # R^{2xN} matrix -# Points3 = ndarray[Tuple[L[3, Any]], dtype[floating]] # R^{2xN} matrix -Points2 = NDArray # R^{2xN} matrix -Points3 = NDArray # R^{2xN} matrix - -# RNx3 = ndarray[(Any, 3), dtype[floating]] # R^{Nx3} matrix -RNx3 = NDArray - -# Lie group elements -SO2Array = ndarray[Tuple[L[2], L[2]], dtype[floating]] # SO(2) rotation matrix -SE2Array = ndarray[Tuple[L[3], L[3]], dtype[floating]] # SE(2) rigid-body transform -SO3Array = ndarray[Tuple[L[3], L[3]], dtype[floating]] # SO(3) rotation matrix -SE3Array = ndarray[Tuple[L[4], L[4]], dtype[floating]] # SE(3) rigid-body transform - -# Lie algebra elements -so2Array = ndarray[ - Tuple[L[2, 2]], dtype[floating] -] # so(2) Lie algebra of SO(2), skew-symmetrix matrix -se2Array = ndarray[ - Tuple[L[3, 3]], dtype[floating] -] # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix -so3Array = ndarray[ - Tuple[L[3, 3]], dtype[floating] -] # so(3) Lie algebra of SO(3), skew-symmetrix matrix -se3Array = ndarray[ - Tuple[L[4, 4]], dtype[floating] -] # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix - -# quaternion arrays -QuaternionArray = ndarray[ - Tuple[L[4,]], - dtype[floating], -] -UnitQuaternionArray = ndarray[ - Tuple[L[4,]], - dtype[floating], -] - -Rn = Union[R2, R3] - -SOnArray = Union[SO2Array, SO3Array] -SEnArray = Union[SE2Array, SE3Array] - -sonArray = Union[so2Array, so3Array] -senArray = Union[se2Array, se3Array] - -Color = Union[str, ArrayLike3] diff --git a/spatialmath/base/_types_35.py b/spatialmath/base/_types_35.py deleted file mode 100644 index d74f63ac..00000000 --- a/spatialmath/base/_types_35.py +++ /dev/null @@ -1,150 +0,0 @@ -# for Python <= 3.8 - -from typing import ( - overload, - Union, - List, - Tuple, - Type, - TextIO, - Any, - Callable, - Optional, - Iterator, -) -from typing_extensions import Literal as L -from typing_extensions import Self - -# array like - -# these are input to many functions in spatialmath.base, and can be a list, tuple or -# ndarray. The elements are generally float, but some functions accept symbolic -# arguments as well, which leads to a NumPy array with dtype=object -# -# The variants like ArrayLike2 indicate that a list, tuple or ndarray of length 2 is -# expected. Static checking of tuple length is possible but not a lists. This might be -# possible in future versions of Python, but for now it is a hint to the coder about -# what is expected - -from numpy.typing import DTypeLike, NDArray # , ArrayLike - -from typing import cast - -# from typing import TypeVar -# NDArray = TypeVar('NDArray') -import numpy as np - - -ArrayLike = Union[float, List[float], Tuple[float, ...], NDArray] -ArrayLikePure = Union[List[float], Tuple[float, ...], NDArray] -ArrayLike2 = Union[List, Tuple[float, float], NDArray] -ArrayLike3 = Union[List, Tuple[float, float, float], NDArray] -ArrayLike4 = Union[List, Tuple[float, float, float, float], NDArray] -ArrayLike6 = Union[List, Tuple[float, float, float, float, float, float], NDArray] - -# real vectors -R1 = NDArray[np.floating] # R^1 -R2 = NDArray[np.floating] # R^2 -R3 = NDArray[np.floating] # R^3 -R4 = NDArray[np.floating] # R^4 -R6 = NDArray[np.floating] # R^6 -R8 = NDArray[np.floating] # R^8 - -# real matrices -R1x1 = NDArray # R^{1x1} matrix -R2x2 = NDArray # R^{3x3} matrix -R3x3 = NDArray # R^{3x3} matrix -R4x4 = NDArray # R^{4x4} matrix -R6x6 = NDArray # R^{6x6} matrix -R8x8 = NDArray # R^{8x8} matrix - -R1x3 = NDArray # R^{1x3} row vector -R3x1 = NDArray # R^{3x1} column vector -R1x2 = NDArray # R^{1x2} row vector -R2x1 = NDArray # R^{2x1} column vector - -Points2 = NDArray # R^{2xN} matrix -Points3 = NDArray # R^{2xN} matrix - -RNx3 = NDArray # R^{Nx3} matrix - - -# Lie group elements -SO2Array = NDArray # SO(2) rotation matrix -SE2Array = NDArray # SE(2) rigid-body transform -SO3Array = NDArray # SO(3) rotation matrix -SE3Array = NDArray # SE(3) rigid-body transform - -# Lie algebra elements -so2Array = NDArray # so(2) Lie algebra of SO(2), skew-symmetrix matrix -se2Array = NDArray # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix -so3Array = NDArray # so(3) Lie algebra of SO(3), skew-symmetrix matrix -se3Array = NDArray # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix - -# quaternion arrays -QuaternionArray = NDArray -UnitQuaternionArray = NDArray - -Rn = Union[R2, R3] - -SOnArray = Union[SO2Array, SO3Array] -SEnArray = Union[SE2Array, SE3Array] - -sonArray = Union[so2Array, so3Array] -senArray = Union[se2Array, se3Array] - -# __all__ = [ -# overload, -# Union, -# List, -# Tuple, -# Type, -# TextIO, -# Any, -# Callable, -# Optional, -# Iterator, -# ArrayLike, -# ArrayLike2, -# ArrayLike3, -# ArrayLike4, -# ArrayLike6, -# # real vectors -# R2, -# R3, -# R4, -# R6, -# R8, -# # real matrices -# R2x2, -# R3x3, -# R4x4, -# R6x6, -# R8x8, -# R1x3, -# R3x1, -# R1x2, -# R2x1, -# Points2, -# Points3, -# RNx3, -# # Lie group elements -# SO2Array, -# SE2Array, -# SO3Array, -# SE3Array, -# # Lie algebra elements -# so2Array, -# se2Array, -# so3Array, -# se3Array, -# # quaternion arrays -# QuaternionArray, -# UnitQuaternionArray, -# Rn, -# SOnArray, -# SEnArray, -# sonArray, -# senArray, -# ] -Color = Union[str, ArrayLike3] diff --git a/spatialmath/base/_types_39.py b/spatialmath/base/_types_39.py deleted file mode 100644 index 350210f5..00000000 --- a/spatialmath/base/_types_39.py +++ /dev/null @@ -1,158 +0,0 @@ -# for Python >= 3.9 - -from typing import ( - overload, - cast, - Union, - List, - Tuple, - Type, - TextIO, - Any, - Callable, - Optional, - Iterator, -) -from typing import Literal as L -from typing_extensions import Self - -import numpy as np -from numpy import ndarray, dtype, floating -from numpy.typing import NDArray, DTypeLike - -# array like - -# these are input to many functions in spatialmath.base, and can be a list, tuple or -# ndarray. The elements are generally float, but some functions accept symbolic -# arguments as well, which leads to a NumPy array with dtype=object. For now -# symbolics will throw a lint error. Possibly create variants ArrayLikeSym that -# admits symbols and can be used for those functions that accept symbols. -# -# The variants like ArrayLike2 indicate that a list, tuple or ndarray of -# length 2 is expected. Static checking of tuple length is possible, but not for lists. -# This might be possible in future versions of Python, but for now it is a hint to the -# coder about what is expected - - -ArrayLike = Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] -ArrayLikePure = Union[List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] -ArrayLike2 = Union[ - List[float], - Tuple[float, float], - ndarray[ - Tuple[L[2,]], - dtype[floating], - ], -] -ArrayLike3 = Union[ - List[float], - Tuple[float, float, float], - ndarray[ - Tuple[L[3,]], - dtype[floating], - ], -] -ArrayLike4 = Union[ - List[float], - Tuple[float, float, float, float], - ndarray[ - Tuple[L[4,]], - dtype[floating], - ], -] -ArrayLike6 = Union[ - List[float], - Tuple[float, float, float, float, float, float], - ndarray[ - Tuple[L[6,]], - dtype[floating], - ], -] - -# real vectors -R1 = ndarray[ - Tuple[L[1]], - dtype[floating], -] # R^1 -R2 = ndarray[ - Tuple[L[2]], - dtype[floating], -] # R^2 -R3 = ndarray[ - Tuple[L[3]], - dtype[floating], -] # R^3 -R4 = ndarray[ - Tuple[L[4]], - dtype[floating], -] # R^4 -R6 = ndarray[ - Tuple[L[6]], - dtype[floating], -] # R^6 -R8 = ndarray[ - Tuple[L[8]], - dtype[floating], -] # R^8 - -# real matrices -R1x1 = ndarray[Tuple[L[1], L[1]], dtype[floating]] # R^{1x1} matrix -R2x2 = ndarray[Tuple[L[2], L[2]], dtype[floating]] # R^{2x2} matrix -R3x3 = ndarray[Tuple[L[3], L[3]], dtype[floating]] # R^{3x3} matrix -R4x4 = ndarray[Tuple[L[4], L[4]], dtype[floating]] # R^{4x4} matrix -R6x6 = ndarray[Tuple[L[6], L[6]], dtype[floating]] # R^{6x6} matrix -R8x8 = ndarray[Tuple[L[8], L[8]], dtype[floating]] # R^{8x8} matrix -R1x3 = ndarray[Tuple[L[1], L[3]], dtype[floating]] # R^{1x3} row vector -R3x1 = ndarray[Tuple[L[3], L[1]], dtype[floating]] # R^{3x1} column vector -R1x2 = ndarray[Tuple[L[1], L[2]], dtype[floating]] # R^{1x2} row vector -R2x1 = ndarray[Tuple[L[2], L[1]], dtype[floating]] # R^{2x1} column vector - -# Points2 = ndarray[Tuple[L[2, Any]], dtype[floating]] # R^{2xN} matrix -# Points3 = ndarray[Tuple[L[3, Any]], dtype[floating]] # R^{2xN} matrix -Points2 = NDArray # R^{2xN} matrix -Points3 = NDArray # R^{2xN} matrix - -# RNx3 = ndarray[(Any, 3), dtype[floating]] # R^{Nx3} matrix -RNx3 = NDArray - -# Lie group elements -SO2Array = ndarray[Tuple[L[2, 2]], dtype[floating]] # SO(2) rotation matrix -SE2Array = ndarray[Tuple[L[3, 3]], dtype[floating]] # SE(2) rigid-body transform -# SO3Array = ndarray[Tuple[L[3, 3]], dtype[floating]] -SO3Array = np.ndarray[Tuple[L[3], L[3]], dtype[floating]] # SO(3) rotation matrix -SE3Array = ndarray[Tuple[L[4], L[4]], dtype[floating]] # SE(3) rigid-body transform - - -# Lie algebra elements -so2Array = ndarray[ - Tuple[L[2, 2]], dtype[floating] -] # so(2) Lie algebra of SO(2), skew-symmetrix matrix -se2Array = ndarray[ - Tuple[L[3, 3]], dtype[floating] -] # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix -so3Array = ndarray[ - Tuple[L[3, 3]], dtype[floating] -] # so(3) Lie algebra of SO(3), skew-symmetrix matrix -se3Array = ndarray[ - Tuple[L[4, 4]], dtype[floating] -] # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix - -# quaternion arrays -QuaternionArray = ndarray[ - Tuple[L[4,]], - dtype[floating], -] -UnitQuaternionArray = ndarray[ - Tuple[L[4,]], - dtype[floating], -] - -Rn = Union[R2, R3] - -SOnArray = Union[SO2Array, SO3Array] -SEnArray = Union[SE2Array, SE3Array] - -sonArray = Union[so2Array, so3Array] -senArray = Union[se2Array, se3Array] - -Color = Union[str, ArrayLike3] diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py index 45ff705e..0e26ed49 100755 --- a/spatialmath/base/animate.py +++ b/spatialmath/base/animate.py @@ -163,8 +163,9 @@ def trplot( else: self.start = start - # draw axes at the origin - smb.trplot(self.start, ax=self, **kwargs) + # Store frame geometry at the origin so each absolute pose is applied once. + smb.trplot(np.identity(4), ax=self, **kwargs) + self._draw(self.start) def set_proj_type(self, proj_type: str): self.ax.set_proj_type(proj_type) diff --git a/spatialmath/base/argcheck.py b/spatialmath/base/argcheck.py index 0db73497..742598aa 100644 --- a/spatialmath/base/argcheck.py +++ b/spatialmath/base/argcheck.py @@ -510,8 +510,8 @@ def isvector(v: Any, dim: Optional[int] = None) -> bool: if dim is None: return ( (len(s) == 1 and s[0] > 0) - or (s[0] == 1 and s[1] > 0) - or (s[0] > 0 and s[1] == 1) + or (len(s) == 2 and s[0] == 1 and s[1] > 0) + or (len(s) == 2 and s[0] > 0 and s[1] == 1) ) else: return s == (dim,) or s == (1, dim) or s == (dim, 1) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 92111793..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 @@ -326,12 +328,19 @@ def qqmul(q1: ArrayLike4, q2: ArrayLike4) -> QuaternionArray: """ q1 = smb.getvector(q1, 4) q2 = smb.getvector(q2, 4) - s1 = q1[0] - v1 = q1[1:4] - s2 = q2[0] - v2 = q2[1:4] + s1, x1, y1, z1 = q1 + s2, x2, y2, z2 = q2 - return np.r_[s1 * s2 - np.dot(v1, v2), s1 * v2 + s2 * v1 + np.cross(v1, v2)] + # explicit scalar arithmetic avoids the generic-dispatch overhead of + # np.dot/np.cross on a 3-vector, which dominates cost at this size + return np.array( + [ + s1 * s2 - x1 * x2 - y1 * y2 - z1 * z2, + s1 * x2 + x1 * s2 + y1 * z2 - z1 * y2, + s1 * y2 - x1 * z2 + y1 * s2 + z1 * x2, + s1 * z2 + x1 * y2 - y1 * x2 + z1 * s2, + ] + ) def qinner(q1: ArrayLike4, q2: ArrayLike4) -> float: @@ -398,8 +407,24 @@ def qvmul(q: ArrayLike4, v: ArrayLike3) -> R3: """ q = smb.getvector(q, 4) v = smb.getvector(v, 3) - qv = qqmul(q, qqmul(qpure(v), qconj(q))) - return qv[1:4] + s, x, y, z = q + vx, vy, vz = v + + # closed-form v' = v + 2s(w x v) + 2 w x (w x v), for q = (s, w) unit; + # mathematically equivalent to q * pure(v) * conj(q) but avoids two full + # Hamilton products (each wasting work on a zero scalar part) and the + # np.cross/np.dot dispatch overhead within them + tx = 2 * (y * vz - z * vy) + ty = 2 * (z * vx - x * vz) + tz = 2 * (x * vy - y * vx) + + return np.array( + [ + vx + s * tx + (y * tz - z * ty), + vy + s * ty + (z * tx - x * tz), + vz + s * tz + (x * ty - y * tx), + ] + ) def vvmul(qa: ArrayLike3, qb: ArrayLike3) -> R3: @@ -897,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. @@ -910,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/transforms2d.py b/spatialmath/base/transforms2d.py index b33590ae..efe46a59 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -367,7 +367,12 @@ def ishom2(T: Any, check: bool = False, tol: float = 20) -> bool: # TypeGuard(S and T.shape == (3, 3) and ( not check - or (smb.isR(T[:2, :2], tol=tol) and all(T[2, :] == np.array([0, 0, 1]))) + or ( + smb.isR(T[:2, :2], tol=tol) + and T[2, 0] == 0 + and T[2, 1] == 0 + and T[2, 2] == 1 + ) ) ) @@ -1340,7 +1345,6 @@ def trplot2( width: float = 1, d1: float = 0.1, d2: float = 1.15, - **kwargs, ): """ Plot a 2D coordinate frame @@ -1607,9 +1611,18 @@ def tranimate2(T: Union[SO2Array, SE2Array], **kwargs): """ dims = kwargs.pop("dims", None) ax = kwargs.pop("ax", None) + + # Animate2.run()'s own parameters must not also be forwarded to + # trplot2's drawing call, which now validates its keywords strictly + # (see Animate2.run's signature for this parameter set) + run_kwargs = { + k: kwargs.pop(k) + for k in ("movie", "repeat", "interval", "nframes", "wait") + if k in kwargs + } anim = smb.animate.Animate2(dims=dims, axes=ax, **kwargs) anim.trplot2(T, **kwargs) - return anim.run(**kwargs) + return anim.run(**run_kwargs) if __name__ == "__main__": # pragma: no cover diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 11aba217..95786b9c 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -61,6 +61,11 @@ _eps = np.finfo(np.float64).eps +# read-only constant: safe to use directly as an operand in expressions that +# produce a new array (e.g. `_EYE3 + ...`), never in a context that could +# mutate it in place or return it directly to a caller +_EYE3 = np.eye(3) + # ---------------------------------------------------------------------------------------# @@ -391,7 +396,13 @@ def ishom(T: Any, check: bool = False, tol: float = 20) -> bool: and T.shape == (4, 4) and ( not check - or (isR(T[:3, :3], tol=tol) and all(T[3, :] == np.array([0, 0, 0, 1]))) + or ( + isR(T[:3, :3], tol=tol) + and T[3, 0] == 0 + and T[3, 1] == 0 + and T[3, 2] == 0 + and T[3, 3] == 1 + ) ) ) @@ -1364,7 +1375,7 @@ def trlog( else: # general case Ginv = ( - np.eye(3) + _EYE3 - S / 2 + (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S ) @@ -1383,8 +1394,7 @@ def trlog( diagonal = R.diagonal() k = diagonal.argmax() mx = diagonal[k] - I = np.eye(3) - col = R[:, k] + I[:, k] + col = R[:, k] + _EYE3[:, k] w = col / np.sqrt(2 * (1 + mx)) theta = math.pi if twist: @@ -1523,7 +1533,7 @@ def trexp(S, theta=None, check=True): skw = skew(w) V = ( - np.eye(3) * theta + _EYE3 * theta + (1.0 - math.cos(theta)) * skw + (theta - math.sin(theta)) * skw @ skw ) @@ -1600,17 +1610,36 @@ def trnorm(T: SE3Array) -> SE3Array: if not ishom(T) and not isrot(T): raise ValueError("expecting SO(3) or SE(3)") - o = T[:3, 1] - a = T[:3, 2] + # explicit scalar arithmetic avoids the generic-dispatch overhead of + # np.cross/unitvec/np.stack (built for arbitrary shapes/broadcasting), + # which dominates cost for a fixed 3x3 rotation submatrix + o0, o1, o2 = T[0, 1], T[1, 1], T[2, 1] + a0, a1, a2 = T[0, 2], T[1, 2], T[2, 2] - n = np.cross(o, a) # N = O x A - o = np.cross(a, n) # (a)]; - R = np.stack((unitvec(n), unitvec(o), unitvec(a)), axis=1) + # n = o x a + n0 = o1 * a2 - o2 * a1 + n1 = o2 * a0 - o0 * a2 + n2 = o0 * a1 - o1 * a0 - if ishom(T): - return rt2tr(cast(SO3Array, R), T[:3, 3]) - else: - return R + # o = a x n, to re-orthogonalize + o0, o1, o2 = a1 * n2 - a2 * n1, a2 * n0 - a0 * n2, a0 * n1 - a1 * n0 + + n_norm = (n0 * n0 + n1 * n1 + n2 * n2) ** 0.5 + o_norm = (o0 * o0 + o1 * o1 + o2 * o2) ** 0.5 + a_norm = (a0 * a0 + a1 * a1 + a2 * a2) ** 0.5 + + is_hom = ishom(T) + R = np.empty((4, 4) if is_hom else (3, 3)) + R[0, 0], R[1, 0], R[2, 0] = n0 / n_norm, n1 / n_norm, n2 / n_norm + R[0, 1], R[1, 1], R[2, 1] = o0 / o_norm, o1 / o_norm, o2 / o_norm + R[0, 2], R[1, 2], R[2, 2] = a0 / a_norm, a1 / a_norm, a2 / a_norm + + if is_hom: + R[3, :3] = 0.0 + R[3, 3] = 1.0 + R[:3, 3] = T[:3, 3] + + return cast(SE3Array, R) @overload @@ -2727,7 +2756,6 @@ def tr2adjoint(T): :SymPy: supported """ - Z = np.zeros((3, 3), dtype=T.dtype) if T.shape == (3, 3): # SO(3) adjoint R = T @@ -2735,12 +2763,14 @@ def tr2adjoint(T): elif T.shape == (4, 4): # SE(3) adjoint (R, t) = tr2rt(T) - # fmt: off - return np.block([ - [R, skew(t) @ R], - [Z, R] - ]) - # fmt: on + # direct block assignment avoids np.block's generic nested-list + # assembly overhead, which dominates cost for this fixed 6x6-from- + # four-3x3 layout; dtype preserved for SymPy support + A = np.zeros((6, 6), dtype=T.dtype) + A[:3, :3] = R + A[:3, 3:] = skew(t) @ R + A[3:, 3:] = R + return A else: raise ValueError("bad argument") @@ -2783,7 +2813,7 @@ def rodrigues(w: ArrayLike3, theta: Optional[float] = None) -> SO3Array: skw = skew(cast(ArrayLike3, w)) return ( - np.eye(skw.shape[0]) + _EYE3 + math.sin(theta) * skw + (1.0 - math.cos(theta)) * skw @ skw ) @@ -3029,7 +3059,6 @@ def trplot( dims: Optional[ArrayLikePure] = None, d2: float = 1.15, flo: Tuple[float, float, float] = (-0.05, -0.05, -0.05), - **kwargs, ): """ Plot a 3D coordinate frame @@ -3217,6 +3246,9 @@ def trplot( ax.set_proj_type("persp") # collect all the arguments to use for left and right views + # note: dims is deliberately excluded, it was already applied to + # ax above; block is deliberately excluded and handled once + # below, after both eyes are drawn, rather than per-eye args = { "ax": ax, "frame": frame, @@ -3225,8 +3257,15 @@ def trplot( "wtl": wtl, "flo": flo, "d2": d2, + "textcolor": textcolor, + "labels": labels, + "originsize": originsize, + "origincolor": origincolor, + "axislabel": axislabel, + "axissubscript": axissubscript, + "width": width, + "projection": projection, } - args = {**args, **kwargs} # unpack the anaglyph parameters shift = 0.1 @@ -3248,7 +3287,11 @@ def trplot( T = r2t(cast(SO3Array, T)) trplot(transl(shift, 0, 0) @ T, color=colors[1], **args) - return + if block is not None: + import matplotlib.pyplot as plt + + plt.show(block=block) + return ax if style == "rviz": if originsize is None: @@ -3300,9 +3343,9 @@ def trplot( flo=flo, anaglyph=anaglyph, axislabel=axislabel, - **kwargs, + axissubscript=axissubscript, ) - return + return ax if dims is not None: dims = tuple(dims) @@ -3516,9 +3559,18 @@ def tranimate(T: Union[SO3Array, SE3Array], **kwargs) -> str: # alias for backward compatibility with the previous docstring/API dim = kwargs.pop("dim", kwargs.pop("dims", None)) ax = kwargs.pop("ax", None) + + # Animate.run()'s own parameters must not also be forwarded to + # trplot's drawing call, which now validates its keywords strictly + # (see Animate.run's signature for this parameter set) + run_kwargs = { + k: kwargs.pop(k) + for k in ("movie", "repeat", "interval", "nframes", "wait") + if k in kwargs + } anim = Animate(dim=dim, ax=ax, **kwargs) anim.trplot(T, **kwargs) - return anim.run(**kwargs) + return anim.run(**run_kwargs) if __name__ == "__main__": # pragma: no cover diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..8dbe7596 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -378,10 +378,34 @@ def isR(R: NDArray, tol: float = 20) -> bool: # -> TypeGuard[SOnArray]: :seealso: isrot2, isrot """ - return bool( - np.linalg.norm(R @ R.T - np.eye(R.shape[0])) < tol * _eps - and np.linalg.det(R) > 0 - ) + n = R.shape[0] + if n == 3: + # explicit cofactor expansion avoids the dispatch overhead of + # np.linalg.det/norm, which dominates cost for such a small matrix + det = ( + R[0, 0] * (R[1, 1] * R[2, 2] - R[1, 2] * R[2, 1]) + - R[0, 1] * (R[1, 0] * R[2, 2] - R[1, 2] * R[2, 0]) + + R[0, 2] * (R[1, 0] * R[2, 1] - R[1, 1] * R[2, 0]) + ) + if det <= 0: + return False + D = R @ R.T + D[0, 0] -= 1.0 + D[1, 1] -= 1.0 + D[2, 2] -= 1.0 + return bool(np.sum(D * D) < (tol * _eps) ** 2) + elif n == 2: + det = R[0, 0] * R[1, 1] - R[0, 1] * R[1, 0] + if det <= 0: + return False + D = R @ R.T + D[0, 0] -= 1.0 + D[1, 1] -= 1.0 + return bool(np.sum(D * D) < (tol * _eps) ** 2) + else: + return bool( + np.linalg.norm(R @ R.T - np.eye(n)) < tol * _eps and np.linalg.det(R) > 0 + ) def isskew(S: NDArray, tol: float = 20) -> bool: # -> TypeGuard[sonArray]: @@ -408,6 +432,11 @@ def isskew(S: NDArray, tol: float = 20) -> bool: # -> TypeGuard[sonArray]: :seealso: isskewa """ + # NB: unlike isR/isskewa, an explicit-arithmetic fast path here did not + # show a reliable win under careful (min-of-repeats) benchmarking - the + # only overhead being avoided is np.linalg.norm on an already-cheap + # `S + S.T`, not enough to reliably beat the scalar-indexing cost of + # unrolling it by hand. Left as the original implementation. return bool(np.linalg.norm(S + S.T) < tol * _eps) @@ -436,9 +465,42 @@ def isskewa(S: NDArray, tol: float = 20) -> bool: # -> TypeGuard[senArray]: :seealso: isskew """ - return bool(np.linalg.norm(S[0:-1, 0:-1] + S[0:-1, 0:-1].T) < tol * _eps) and all( - S[-1, :] == 0 - ) + n = S.shape[0] + if n == 4: + # explicit sum-of-squares + scalar bottom-row check avoids the + # generic-dispatch overhead of np.linalg.norm and the array + # allocation + all() of the bottom-row comparison, same as isR/ishom + r00 = S[0, 0] + S[0, 0] + r01 = S[0, 1] + S[1, 0] + r02 = S[0, 2] + S[2, 0] + r11 = S[1, 1] + S[1, 1] + r12 = S[1, 2] + S[2, 1] + r22 = S[2, 2] + S[2, 2] + resid = r00 * r00 + r11 * r11 + r22 * r22 + 2.0 * ( + r01 * r01 + r02 * r02 + r12 * r12 + ) + return bool( + resid < (tol * _eps) ** 2 + and S[3, 0] == 0 + and S[3, 1] == 0 + and S[3, 2] == 0 + and S[3, 3] == 0 + ) + elif n == 3: + r00 = S[0, 0] + S[0, 0] + r01 = S[0, 1] + S[1, 0] + r11 = S[1, 1] + S[1, 1] + resid = r00 * r00 + r11 * r11 + 2.0 * r01 * r01 + return bool( + resid < (tol * _eps) ** 2 + and S[2, 0] == 0 + and S[2, 1] == 0 + and S[2, 2] == 0 + ) + else: + return bool( + np.linalg.norm(S[0:-1, 0:-1] + S[0:-1, 0:-1].T) < tol * _eps + ) and all(S[-1, :] == 0) def iseye(S: NDArray, tol: float = 20) -> bool: @@ -709,12 +771,15 @@ def h2e(v: NDArray) -> NDArray: :param v: homogeneous vector or matrix :type v: array_like(n), ndarray(n,m) :return: Euclidean vector - :rtype: ndarray(n-1), ndarray(n-1,m) + :rtype: ndarray(n-1,1), ndarray(n-1,m) + :raises ValueError: if ``v`` has fewer than two rows, or is not a vector or + matrix - - If ``v`` is an N-vector, return an (N-1)-column vector where the elements have - all been scaled by the last element of ``v``. - - If ``v`` is a matrix (NxM), return a matrix (N-1xM), where each column has - been scaled by its last element. + Each *column* of ``v`` is a homogeneous point, and is scaled by its last element: + + - If ``v`` is an N-vector, shape (N,) or (N,1), or a list or tuple, return an + (N-1,1) column vector. + - If ``v`` is a matrix (NxM), return a matrix (N-1xM). .. runblock:: pycon @@ -726,16 +791,24 @@ def h2e(v: NDArray) -> NDArray: >>> h2e(h) .. note:: The result is always a 2D array, a 1D input results in a column vector. + .. note:: A 2D array is always a matrix of column vectors, rows are never + points. To convert a row vector of shape (1,N) pass its transpose. A + homogeneous point needs at least two elements, so an input with a single + row, such as shape (1,N), raises a ``ValueError``. - :seealso: e2h + :seealso: :func:`e2h` """ if isinstance(v, np.ndarray) and len(v.shape) == 2: # dealing with matrix + if v.shape[0] < 2: + raise ValueError("homogeneous input must have at least 2 rows") return v[:-1, :] / v[-1, :][np.newaxis, :] elif isvector(v): # dealing with shape (N,) array v = getvector(v, out="col") + if v.shape[0] < 2: + raise ValueError("homogeneous input must have at least 2 elements") return v[0:-1] / v[-1] else: @@ -749,12 +822,15 @@ def e2h(v: NDArray) -> NDArray: :param v: Euclidean vector or matrix :type v: array_like(n), ndarray(n,m) :return: homogeneous vector - :rtype: ndarray(n+1,m) + :rtype: ndarray(n+1,1), ndarray(n+1,m) + :raises ValueError: if ``v`` is not a vector or matrix + + Each *column* of ``v`` is a Euclidean point, and has a 1 appended: - - If ``v`` is an N-vector, return an (N+1)-column vector where a value of 1 has - been appended as the last element. - - If ``v`` is a matrix (NxM), return a matrix (N+1xM), where each column has - been appended with a value of 1, ie. a row of ones has been appended to the matrix. + - If ``v`` is an N-vector, shape (N,) or (N,1), or a list or tuple, return an + (N+1,1) column vector. + - If ``v`` is a matrix (NxM), return a matrix (N+1xM), ie. a row of ones has + been appended to the matrix. .. runblock:: pycon @@ -765,8 +841,12 @@ def e2h(v: NDArray) -> NDArray: >>> e2h(e) .. note:: The result is always a 2D array, a 1D input results in a column vector. + .. note:: A 2D array is always a matrix of column vectors, rows are never + points. A row vector of shape (1,N) is therefore N one-dimensional + points and the result has shape (2,N). To convert a row vector to a + single point pass its transpose. - :seealso: e2h + :seealso: :func:`h2e` """ if isinstance(v, np.ndarray) and len(v.shape) == 2: # dealing with matrix diff --git a/spatialmath/base/types.py b/spatialmath/base/types.py index eb35e9d2..18e110c3 100644 --- a/spatialmath/base/types.py +++ b/spatialmath/base/types.py @@ -1,11 +1,170 @@ -import sys +# Type aliases used throughout spatialmath and spatialmath.base. +# +# This used to dispatch across three per-Python-version files +# (_types_35.py, _types_39.py, _types_311.py) via sys.version_info, from +# when Python 3.5-3.10 support coexisted. pyproject.toml has required +# Python >=3.10 since #186, so the only genuinely version-dependent piece +# left is where `Self` lives (typing_extensions before 3.11, typing from +# 3.11 on). -_version = sys.version_info.minor +import sys +from typing import ( + overload, + cast, + Union, + List, + Tuple, + Type, + TextIO, + Any, + Callable, + Optional, + Iterator, +) +from typing import Literal as L -if _version >= 11: - from spatialmath.base._types_311 import * -elif _version >= 9: - from spatialmath.base._types_39 import * +if sys.version_info >= (3, 11): + from typing import Self else: - from spatialmath.base._types_35 import * + from typing_extensions import Self + +from numpy import ndarray, dtype, floating +from numpy.typing import NDArray, DTypeLike + +# array like + +# these are input to many functions in spatialmath.base, and can be a list, tuple or +# ndarray. The elements are generally float, but some functions accept symbolic +# arguments as well, which leads to a NumPy array with dtype=object. For now +# symbolics will throw a lint error. Possibly create variants ArrayLikeSym that +# admits symbols and can be used for those functions that accept symbols. +# +# The variants like ArrayLike2 indicate that a list, tuple or ndarray of +# length 2 is expected. Static checking of tuple length is possible, but not for lists. +# This might be possible in future versions of Python, but for now it is a hint to the +# coder about what is expected + +# cannot be a scalar +ArrayLikePure = Union[List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] + +ArrayLike = Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] +ArrayLike2 = Union[ + List[float], + Tuple[float, float], + ndarray[ + Tuple[L[2]], + dtype[floating], + ], +] +ArrayLike3 = Union[ + List[float], + Tuple[float, float, float], + ndarray[ + Tuple[L[3]], + dtype[floating], + ], +] +ArrayLike4 = Union[ + List[float], + Tuple[float, float, float, float], + ndarray[ + Tuple[L[4]], + dtype[floating], + ], +] +ArrayLike6 = Union[ + List[float], + Tuple[float, float, float, float, float, float], + ndarray[ + Tuple[L[6]], + dtype[floating], + ], +] + +# real vectors +R1 = ndarray[ + Tuple[L[1]], + dtype[floating], +] # R^1 +R2 = ndarray[ + Tuple[L[2]], + dtype[floating], +] # R^2 +R3 = ndarray[ + Tuple[L[3]], + dtype[floating], +] # R^3 +R4 = ndarray[ + Tuple[L[4]], + dtype[floating], +] # R^4 +R6 = ndarray[ + Tuple[L[6]], + dtype[floating], +] # R^6 + +R8 = ndarray[ + Tuple[L[8,]], + dtype[floating], +] # R^8 + +# real matrices +R1x1 = ndarray[Tuple[L[1], L[1]], dtype[floating]] # R^{1x1} matrix +R2x2 = ndarray[Tuple[L[2], L[2]], dtype[floating]] # R^{2x2} matrix +R3x3 = ndarray[Tuple[L[3], L[3]], dtype[floating]] # R^{3x3} matrix +R4x4 = ndarray[Tuple[L[4], L[4]], dtype[floating]] # R^{4x4} matrix +R6x6 = ndarray[Tuple[L[6], L[6]], dtype[floating]] # R^{6x6} matrix +R8x8 = ndarray[Tuple[L[8], L[8]], dtype[floating]] # R^{8x8} matrix +R1x3 = ndarray[Tuple[L[1], L[3]], dtype[floating]] # R^{1x3} row vector +R3x1 = ndarray[Tuple[L[3], L[1]], dtype[floating]] # R^{3x1} column vector +R1x2 = ndarray[Tuple[L[1], L[2]], dtype[floating]] # R^{1x2} row vector +R2x1 = ndarray[Tuple[L[2], L[1]], dtype[floating]] # R^{2x1} column vector + +# Points2 = ndarray[Tuple[L[2, Any]], dtype[floating]] # R^{2xN} matrix +# Points3 = ndarray[Tuple[L[3, Any]], dtype[floating]] # R^{2xN} matrix +Points2 = NDArray # R^{2xN} matrix +Points3 = NDArray # R^{2xN} matrix + +# RNx3 = ndarray[(Any, 3), dtype[floating]] # R^{Nx3} matrix +RNx3 = NDArray + +# Lie group elements +SO2Array = ndarray[Tuple[L[2], L[2]], dtype[floating]] # SO(2) rotation matrix +SE2Array = ndarray[Tuple[L[3], L[3]], dtype[floating]] # SE(2) rigid-body transform +SO3Array = ndarray[Tuple[L[3], L[3]], dtype[floating]] # SO(3) rotation matrix +SE3Array = ndarray[Tuple[L[4], L[4]], dtype[floating]] # SE(3) rigid-body transform + +# Lie algebra elements +so2Array = ndarray[ + Tuple[L[2, 2]], dtype[floating] +] # so(2) Lie algebra of SO(2), skew-symmetrix matrix +se2Array = ndarray[ + Tuple[L[3, 3]], dtype[floating] +] # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix +so3Array = ndarray[ + Tuple[L[3, 3]], dtype[floating] +] # so(3) Lie algebra of SO(3), skew-symmetrix matrix +se3Array = ndarray[ + Tuple[L[4, 4]], dtype[floating] +] # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix + +# quaternion arrays +QuaternionArray = ndarray[ + Tuple[L[4,]], + dtype[floating], +] +UnitQuaternionArray = ndarray[ + Tuple[L[4,]], + dtype[floating], +] + +Rn = Union[R2, R3] + +SOnArray = Union[SO2Array, SO3Array] +SEnArray = Union[SE2Array, SE3Array] + +sonArray = Union[so2Array, so3Array] +senArray = Union[se2Array, se3Array] + +Color = Union[str, ArrayLike3] diff --git a/spatialmath/baseposelist.py b/spatialmath/baseposelist.py index b102b4bb..bdf3ac64 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,19 @@ def arghandler( elif isinstance(arg, np.ndarray): # it's a numpy array + shape = tuple(self.shape) + if len(shape) == 2 and arg.ndim == 3 and arg.shape[1:] == shape: + # 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 + + if not check and arg.shape != shape: + # check=False skips the value checks, but not the shape check + return False + x = self._import(arg, check=check) if x is not None: self.data = [x] diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index 17f54a76..84813a41 100755 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -693,7 +693,14 @@ def isparallel( :seealso: :meth:`__or__` :meth:`intersects` """ - return bool(np.linalg.norm(np.cross(l1.w, l2.w)) < tol * _eps) + n1 = np.linalg.norm(l1.w) + n2 = np.linalg.norm(l2.w) + if n1 == 0 or n2 == 0: + return True + + # Plucker coordinates are homogeneous: compare the angle, not the + # magnitude of the direction vectors. + return bool(np.linalg.norm(np.cross(l1.w / n1, l2.w / n2)) < tol * _eps) def isintersecting( l1, l2: Line3, tol: float = 20 # type: ignore @@ -715,7 +722,15 @@ def isintersecting( :seealso: :meth:`__xor__` :meth:`intersects` :meth:`isparallel` """ - return not l1.isparallel(l2, tol=tol) and bool(abs(l1 * l2) < tol * _eps) + if l1.isparallel(l2, tol=tol): + return False + + n1 = np.linalg.norm(l1.w) + n2 = np.linalg.norm(l2.w) + # Normalize both moments as well as directions before applying the + # intersection tolerance; each pair (v, w) may be rescaled freely. + reciprocal = np.dot(l1.w / n1, l2.v / n2) + np.dot(l2.w / n2, l1.v / n1) + return bool(abs(reciprocal) < tol * _eps) def __eq__(l1, l2: Line3) -> bool: # type: ignore pylint: disable=no-self-argument """ @@ -1005,16 +1020,12 @@ def commonperp( if l1 | l2: # no common perpendicular if lines are parallel return None - else: - # lines are skew or intersecting - w = np.cross(l1.w, l2.w) - v = ( - np.cross(l1.v, l2.w) - - np.cross(l2.v, l1.w) - + (l1 * l2) * np.dot(l1.w, l2.w) * base.unitvec(np.cross(l1.w, l2.w)) - ) - return l1.__class__(v, w) + # The nearest point lies on both the common perpendicular and l1. + # Unit directions keep its direction independent of Plucker scaling. + point, _ = l1.closest_to_line(l2) + w = np.cross(l1.w / np.linalg.norm(l1.w), l2.w / np.linalg.norm(l2.w)) + return l1.__class__.PointDir(point, w) def __mul__( left, right: Line3 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 6fad5597..29d68de6 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -306,17 +306,63 @@ def conj(self) -> Quaternion: ``q.conj()`` is the quaternion ``q`` with the vector part negated, ie. :math:`q = s \langle -v_x, -v_y, -v_z \rangle` + .. note:: For a ``UnitQuaternion`` this deliberately does **not** + canonicalize the result's scalar part to be non-negative, unlike + normal ``UnitQuaternion`` construction. ``UnitQuaternion`` + construction canonicalizes because :math:`q` and :math:`-q` + represent the same rotation, which is the right behaviour when + building a unit quaternion from arbitrary data. But conjugation + is an algebraic operation, not a re-representation of a + rotation: it must satisfy :math:`q \cdot \bar{q} = 1` for any + downstream algebra to be correct (e.g. dual-quaternion + translation extraction, which relies on exactly this identity). + Re-canonicalizing the conjugate would silently return + :math:`-\bar{q}` whenever ``q`` has negative scalar part, + breaking that identity. See the example below: the scalar part + of the result matches the input's sign, it is not forced + positive. + + .. versionchanged:: 1.1.18 + Fixes a bug present in 1.1.17 and earlier: ``UnitQuaternion.conj()`` + re-canonicalized its result's sign, silently returning + :math:`-\bar{q}` instead of the true conjugate whenever ``q`` had + negative scalar part. This broke any algebra relying on + :math:`q \cdot \bar{q} = 1`, including :class:`DualQuaternion` + translation extraction (``SE3()``), ``norm()`` (which could raise + ``ValueError`` from taking the square root of a small negative + float), and point transformation via ``dq * v``. + Example: .. runblock:: pycon - >>> from spatialmath import Quaternion + >>> from spatialmath import Quaternion, UnitQuaternion + >>> import numpy as np >>> print(Quaternion.Pure([1,2,3]).conj()) + >>> q = UnitQuaternion(np.array([[-0.5, 0.5, 0.5, 0.5]]), norm=False) + >>> print(q) + >>> print(q.conj()) :seealso: :func:`~spatialmath.base.quaternions.qconj` """ - - return self.__class__([smb.qconj(q._A) for q in self]) + # NB: iterate self.data directly, not `for q in self` -- indexing + # a UnitQuaternion (which iteration uses under the hood) goes + # through BasePoseList.__getitem__, which reconstructs each + # element via self.__class__(self.data[i], check=False) with no + # norm=False override, silently re-canonicalizing sign on every + # single access. Working from self.data sidesteps that entirely. + if isinstance(self, UnitQuaternion): + # Pass a 2D (N,4) array with norm=False so construction stores + # the conjugated array as-is, bypassing qunit()'s scalar-sign + # canonicalization -- see the note above for why that + # canonicalization must not apply here. A 1D (4,) array would + # instead be caught by the generic arghandler() path first, + # which normalizes/canonicalizes unconditionally regardless of + # norm -- the 2D-array path is what actually honours norm=False. + return self.__class__( + np.array([smb.qconj(d) for d in self.data]), norm=False + ) + return self.__class__([smb.qconj(d) for d in self.data]) def norm(self) -> float: r""" 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/timing.py b/spatialmath/timing.py deleted file mode 100755 index ae169909..00000000 --- a/spatialmath/timing.py +++ /dev/null @@ -1,274 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding", t) -""" -Created on Fri Apr 10 14:22:36 2020 - -@author", t) -""" - - -import timeit -from ansitable import ANSITable, Column - -N = 100000 - -table = ANSITable( - Column("Operation", headalign="^"), - Column("Time (μs)", headalign="^", fmt="{:.2f}"), - border="thick", -) - - -def result(op, t): - global table - - table.row(op, t / N * 1e6) - - -# ------------------------------------------------------------------------- # - -# transforms_setup = ''' -# from spatialmath import SE3 -# from spatialmath import base - -# import numpy as np -# from collections import namedtuple -# Rt = namedtuple('Rt', 'R t') -# X1 = SE3.Rand() -# X2 = SE3.Rand() -# T1 = X1.A -# T2 = X2.A -# R1 = base.t2r(T1) -# R2 = base.t2r(T2) -# t1 = base.transl(T1) -# t2 = base.transl(T2) -# Rt1 = Rt(R1, t1) -# Rt2 = Rt(R2, t2) -# v = np.r_[1,2,3] -# v2 = np.r_[1,2,3, 1] -# ''' -# t = timeit.timeit(stmt='base.getvector(0.2)', setup=transforms_setup, number=N) -# result("getvector(x)", t) - -# t = timeit.timeit(stmt='base.rotx(0.2, unit="rad")', setup=transforms_setup, number=N) -# result("base.rotx", t) - -# t = timeit.timeit(stmt='base.trotx(0.2, unit="rad")', setup=transforms_setup, number=N) -# result("base.trotx", t) - -# t = timeit.timeit(stmt='base.t2r(T1)', setup=transforms_setup, number=N) -# result("base.t2r", t) - -# t = timeit.timeit(stmt='base.r2t(R1)', setup=transforms_setup, number=N) -# result("base.r2t", t) - -# t = timeit.timeit(stmt='T1 @ T2', setup=transforms_setup, number=N) -# result("4x4 @", t) - -# t = timeit.timeit(stmt='T1[:3,:3] @ T2[:3,:3] + T1[:3,:3] @ T2[:3,3]', setup=transforms_setup, number=N) -# result("R1*R2, R1*t", t) - -# t = timeit.timeit(stmt='(Rt1.R @ Rt2.R, Rt1.R @ Rt2.t)', setup=transforms_setup, number=N) -# result("T1 * T2 (R, t)", t) - -# t = timeit.timeit(stmt='base.trinv(T1)', setup=transforms_setup, number=N) -# result("base.trinv", t) - -# t = timeit.timeit(stmt='(Rt1.R.T, -Rt1.R.T @ Rt1.t)', setup=transforms_setup, number=N) -# result("base.trinv (R,t)", t) - -# t = timeit.timeit(stmt='np.linalg.inv(T1)', setup=transforms_setup, number=N) -# result("np.linalg.inv", t) - -# t = timeit.timeit(stmt='T1 @ v2', setup=transforms_setup, number=N) -# result("(4,4) * (4,)", t) - -# # ------------------------------------------------------------------------- # -# table.rule() - -# t = timeit.timeit(stmt='SE3()', setup=transforms_setup, number=N) -# result("SE3()", t) - -# t = timeit.timeit(stmt='SE3.Rx(0.2)', setup=transforms_setup, number=N) -# result("SE3.Rx()", t) - -# t = timeit.timeit(stmt='T1[:3,:3]', setup=transforms_setup, number=N) -# result("T1[:3,:3]", t) - -# t = timeit.timeit(stmt='X1.A', setup=transforms_setup, number=N) -# result("SE3.A", t) - -# t = timeit.timeit(stmt='SE3(T1)', setup=transforms_setup, number=N) -# result("SE3(T1)", t) - -# t = timeit.timeit(stmt='SE3(T1, check=False)', setup=transforms_setup, number=N) -# result("SE3(T1 check=False)", t) - -# t = timeit.timeit(stmt='SE3([T1], check=False)', setup=transforms_setup, number=N) -# result("SE3([T1])", t) - -# t = timeit.timeit(stmt='X1 * X2', setup=transforms_setup, number=N) -# result("SE3 * SE3", t) - -# t = timeit.timeit(stmt='X1.inv()', setup=transforms_setup, number=N) -# result("SE3.inv", t) - -# t = timeit.timeit(stmt='X1 * v', setup=transforms_setup, number=N) -# result("SE3 * v", t) - -# t = timeit.timeit(stmt='a = X1.log()', setup=transforms_setup, number=N) -# result("SE3.log()", t) - -# # ------------------------------------------------------------------------- # -# quat_setup = ''' -# from spatialmath import base -# from spatialmath import UnitQuaternion -# import numpy as np -# q1 = base.rand() -# q2 = base.rand() -# v = np.r_[1,2,3] -# Q1 = UnitQuaternion.Rx(0.2) -# Q2 = UnitQuaternion.Ry(0.3) -# ''' -# table.rule() - -# t = timeit.timeit(stmt='a = UnitQuaternion()', setup=quat_setup, number=N) -# result("UnitQuaternion() ", t) - -# t = timeit.timeit(stmt='a = UnitQuaternion.Rx(0.2)', setup=quat_setup, number=N) -# result("UnitQuaternion.Rx ", t) - -# t = timeit.timeit(stmt='a = Q1 * Q2', setup=quat_setup, number=N) -# result("UnitQuaternion * UnitQuaternion", t) - -# t = timeit.timeit(stmt='a = Q1 * v', setup=quat_setup, number=N) -# result("UnitQuaternion * v", t) - -# t = timeit.timeit(stmt='a = base.qqmul(q1,q2)', setup=quat_setup, number=N) -# result("base.qqmul", t) - -# t = timeit.timeit(stmt='a = base.qvmul(q1,v)', setup=quat_setup, number=N) -# result("base.qvmul", t) - - -# # ------------------------------------------------------------------------- # -# twist_setup = ''' -# from spatialmath import SE3, Twist3 -# from spatialmath import base -# import numpy as np -# from math import cos -# S1 = SE3.Rand().Twist3() -# S2 = SE3.Rand().Twist3() -# X1 = SE3.Rand() -# T1 = X1.A -# A1 = X1.Ad() -# se3 = S1.se3() -# s = np.r_[1,2,3,4,5,6] -# v = np.r_[1,2,3] -# ''' -# table.rule() -# t = timeit.timeit(stmt='a = Twist3()', setup=twist_setup, number=N) -# result("Twist3()", t) - -# t = timeit.timeit(stmt='a = X1.Twist3()', setup=twist_setup, number=N) -# result("SE3.Twist3()", t) - -# t = timeit.timeit(stmt='a = S1 * S2', setup=twist_setup, number=N) -# result("Twist3 * Twist3", t) - -# t = timeit.timeit(stmt='a = S1.inv()', setup=twist_setup, number=N) -# result("Twist3.inv()", t) - -# t = timeit.timeit(stmt='a = S1.Ad()', setup=twist_setup, number=N) -# result("Twist3.Ad()", t) - -# t = timeit.timeit(stmt='a = S1.exp(1)', setup=twist_setup, number=N) -# result("Twist3.Exp()", t) - -# t = timeit.timeit(stmt='a = base.skewa(v)', setup=twist_setup, number=N) -# result("skew", t) - -# t = timeit.timeit(stmt='a = base.skewa(s)', setup=twist_setup, number=N) -# result("skewa", t) - -# t = timeit.timeit(stmt='a = base.vexa(se3)', setup=twist_setup, number=N) -# result("vexa", t) - -# t = timeit.timeit(stmt='a = base.trlog(T1)', setup=twist_setup, number=N) -# result("trlog", t) - -# t = timeit.timeit(stmt='a = base.trlog(T1, twist=True)', setup=twist_setup, number=N) -# result("trlog as twist", t) - -# t = timeit.timeit(stmt='a = base.trexp(se3)', setup=twist_setup, number=N) -# result("trexp", t) - -# t = timeit.timeit(stmt='a = A1 @ s', setup=twist_setup, number=N) -# result("(6,6) * (6,)", t) - -# t = timeit.timeit(stmt='a = base.rodrigues(v)', setup=twist_setup, number=N) -# result("rodrigues", t) - -# t = timeit.timeit(stmt='a = cos(0.3)', setup=twist_setup, number=N) -# result("math.cos", t) - -# t = timeit.timeit(stmt='a = np.cos(0.3)', setup=twist_setup, number=N) -# result("np.cos", t) - -# ------------------------------------------------------------------------- # -misc_setup = """ -from spatialmath import base -import numpy as np -s = np.r_[1.0,2,3,4,5,6] -s3 = np.r_[1.0,2,3] -a = np.r_[1.0, 2.0, 3.0] -b = np.r_[-5.0, 4.0, 3.0] - -A = np.random.randn(6,6) -As = (A + A.T) / 2 -bb = np.random.randn(6) -""" -table.rule() - -t = timeit.timeit(stmt="c = np.linalg.inv(As)", setup=misc_setup, number=N) -result("np.inv(As)", t) - -t = timeit.timeit(stmt="c = np.linalg.pinv(As)", setup=misc_setup, number=N) -result("np.pinv(As)", t) - -t = timeit.timeit(stmt="c = np.linalg.solve(As, bb)", setup=misc_setup, number=N) -result("np.solve(As, b)", t) - -t = timeit.timeit(stmt="c = np.cross(a,b)", setup=misc_setup, number=N) -result("np.cross()", t) - -t = timeit.timeit(stmt="c = base.cross(a,b)", setup=misc_setup, number=N) -result("cross()", t) - -t = timeit.timeit(stmt="a = np.inner(s,s).sum()", setup=misc_setup, number=N) -result("inner()", t) - -t = timeit.timeit(stmt="a = np.linalg.norm(s) ** 2", setup=misc_setup, number=N) -result("np.norm**2", t) - -t = timeit.timeit(stmt="a = base.normsq(s)", setup=misc_setup, number=N) -result("base.normsq", t) - -t = timeit.timeit(stmt="a = (s ** 2).sum()", setup=misc_setup, number=N) -result("s**2.sum()", t) - -t = timeit.timeit(stmt="a = np.sum(s ** 2)", setup=misc_setup, number=N) -result("np.sum(s ** 2)", t) - -t = timeit.timeit(stmt="a = np.linalg.norm(s)", setup=misc_setup, number=N) -result("np.norm(R6)", t) -t = timeit.timeit(stmt="a = base.norm(s)", setup=misc_setup, number=N) -result("base.norm(R6)", t) - -t = timeit.timeit(stmt="a = np.linalg.norm(s3)", setup=misc_setup, number=N) -result("np.norm(R3)", t) -t = timeit.timeit(stmt="a = base.norm(s3)", setup=misc_setup, number=N) -result("base.norm(R3)", t) - - -table.print() diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 928235e4..4579ec29 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -813,7 +813,7 @@ def unit(self): """ Unit twist - - ``S.unit()`` is a Twist2 objec3 representing a unit twist aligned with the + - ``S.unit()`` is a Twist3 object representing a unit twist aligned with the Twist ``S``. Example: @@ -825,12 +825,12 @@ def unit(self): >>> S = Twist3(T) >>> S.unit() """ - if smb.iszerovec(self.w): - # rotational twist - return Twist3(self.S / smb.norm(S.w)) - else: - # prismatic twist + if self.isprismatic: + # prismatic twist (zero rotation): normalize the direction vector return Twist3(smb.unitvec(self.v), [0, 0, 0]) + else: + # general twist: normalize so |w| == 1 + return Twist3(self.S / smb.norm(self.w)) def ad(self): """ @@ -974,7 +974,7 @@ def pole(self): :return: the pole of the twist :rtype: ndarray(3) - ``X.pole()`` is a point on the twist axis. For a pure translation + ``X.pole`` is a point on the twist axis. For a pure translation this point is at infinity. Example: @@ -1487,7 +1487,7 @@ def pole(self): :return: the pole of the twist :rtype: ndarray(2) - ``X.pole()`` is a point on the twist axis. For a pure translation + ``X.pole`` is a point on the twist axis. For a pure translation this point is at infinity. Example: @@ -1497,7 +1497,7 @@ def pole(self): >>> from spatialmath import SE2, Twist2 >>> T = SE2(1, 2, 0.3) >>> S = Twist2(T) - >>> S.pole() + >>> S.pole """ p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta @@ -1626,12 +1626,12 @@ def unit(self): >>> S = Twist2(T) >>> S.unit() """ - if smb.iszerovec(self.w): - # rotational twist - return Twist2(self.S / smb.norm(S.w)) + if self.isprismatic: + # prismatic twist (zero rotation): normalize the direction vector + return Twist2(smb.unitvec(self.v), 0) else: - # prismatic twist - return Twist2(smb.unitvec(self.v), [0, 0, 0]) + # general twist: normalize so |w| == 1 (w is a scalar for Twist2) + return Twist2(self.S / abs(self.w)) @property def ad(self): diff --git a/tests/base/test_animate.py b/tests/base/test_animate.py new file mode 100644 index 00000000..147a09ad --- /dev/null +++ b/tests/base/test_animate.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Regression coverage for spatialmath.base.animate. + +animate()/tranimate()/tranimate2() build a matplotlib FuncAnimation, but +under a non-interactive backend with nothing pumping a real event loop +(exactly the CI/test environment -- see conftest.py, which forces the Agg +backend for the whole session) its per-frame update() callback never +actually fires. Merely constructing the FuncAnimation, or calling +.animate() and letting the result be garbage collected, proves nothing +about whether the interpolation code (trinterp/trinterp2) runs or is +correct -- it only proves construction didn't raise. + +FuncAnimation.save() is different: it synchronously steps through every +frame and invokes the real update() callback for each one, regardless of +backend -- the same thing a real playback or movie export would do. We +drive that here with PillowWriter (pure Python, ships with +matplotlib+Pillow, no external ffmpeg binary) purely to force every frame +through the real callback, and record what it actually computed, rather +than analyzing the saved output itself. +""" + +import os +import tempfile +import unittest +from unittest.mock import patch + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +from matplotlib.animation import PillowWriter + +import spatialmath.base as smb +from spatialmath import SE2, SE3 + + +def _drive_all_frames(fa): + tmp = tempfile.mktemp(suffix=".gif") + try: + fa.save(tmp, writer=PillowWriter(fps=5)) + finally: + if os.path.exists(tmp): + os.remove(tmp) + + +class TestAnimate2(unittest.TestCase): + def test_drives_real_frames(self): + plt.close("all") + end = SE2.Rand() + start = SE2.Rand() + nframes = 5 + + real_trinterp2 = smb.trinterp2 + calls = [] + + def recording_trinterp2(*args, **kwargs): + calls.append(kwargs["s"]) + return real_trinterp2(*args, **kwargs) + + with patch("spatialmath.base.trinterp2", side_effect=recording_trinterp2): + fa = end.animate(start=start, dims=[-2, 2], nframes=nframes, repeat=False) + _drive_all_frames(fa) + + self.assertEqual(len(calls), nframes) + self.assertAlmostEqual(min(calls), 0.0) + self.assertAlmostEqual(max(calls), 1.0) + plt.close("all") + + +class TestAnimate3(unittest.TestCase): + def test_drives_real_frames(self): + plt.close("all") + end = SE3.Rand() + start = SE3.Rand() + nframes = 5 + + real_trinterp = smb.trinterp + calls = [] + + def recording_trinterp(*args, **kwargs): + calls.append(kwargs["s"]) + return real_trinterp(*args, **kwargs) + + with patch("spatialmath.base.trinterp", side_effect=recording_trinterp): + fa = end.animate(start=start, dims=[-2, 2], nframes=nframes, repeat=False) + _drive_all_frames(fa) + + self.assertEqual(len(calls), nframes) + self.assertAlmostEqual(min(calls), 0.0) + self.assertAlmostEqual(max(calls), 1.0) + plt.close("all") + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/base/test_argcheck.py b/tests/base/test_argcheck.py index 39c943d1..bdf603e2 100755 --- a/tests/base/test_argcheck.py +++ b/tests/base/test_argcheck.py @@ -11,6 +11,7 @@ import numpy.testing as nt from spatialmath.base.argcheck import * +from spatialmath.base.transformsNd import e2h, h2e class Test_check(unittest.TestCase): @@ -212,10 +213,30 @@ def test_isvector(self): self.assertFalse(isvector(np.array([[1, 2, 3]]), 4)) self.assertFalse(isvector(np.array([[1], [2], [3]]), 4)) - def test_isvector(self): + def test_assertvector(self): l = [1, 2, 3] nt.assert_raises(ValueError, assertvector, l, 4) + def test_isvector_unsupported_array_shapes(self): + for shape in [(), (2, 3), (1, 3, 2), (3, 1, 2), (1, 1, 1), (1, 3, 0)]: + with self.subTest(shape=shape): + value = np.ones(shape) + self.assertFalse(isvector(value)) + with self.assertRaises(ValueError): + assertvector(value) + + def test_isvector_empty_arrays(self): + for shape in [(0,), (1, 0), (0, 1), (0, 0)]: + with self.subTest(shape=shape): + self.assertFalse(isvector(np.empty(shape))) + + def test_homogeneous_conversion_unsupported_array_shapes(self): + for convert in [e2h, h2e]: + for shape in [(), (1, 3, 2), (3, 1, 2), (1, 1, 1)]: + with self.subTest(convert=convert.__name__, shape=shape): + with self.assertRaises(ValueError): + convert(np.ones(shape)) + def test_getvector(self): l = [1, 2, 3] t = (1, 2, 3) diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index bf578b22..976b1047 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -312,6 +312,14 @@ def test_plot(self): ) plt.close("all") + def test_plot_rejects_unknown_kwarg(self): + # trplot2() used to silently swallow an unrecognized keyword via a + # bare **kwargs sink instead of raising -- e.g. a typo'd + # framelabel= (the real parameter is frame=) drew the frame with + # no label and no error at all. + with self.assertRaises(TypeError): + trplot2(transl2(1, 2), framelabel="A", block=False) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": diff --git a/tests/base/test_transforms3d.py b/tests/base/test_transforms3d.py index 8b2fb080..e9b9af14 100755 --- a/tests/base/test_transforms3d.py +++ b/tests/base/test_transforms3d.py @@ -319,6 +319,34 @@ def test_trlog(self): T = transl(1, 2, 3) @ rpy2tr(0.1, 0.2, 0.3) nt.assert_array_almost_equal(logm(T), trlog(T)) + def test_trlog_near_identity(self): + # Regression test for #63: trlog's general-case branch divides by + # sin(theta), computed from trace(R) via acos. A near-identity R + # that isn't *exactly* np.eye(3) (as produced by real computation, + # not hand-constructed) must not raise or blow up. + + # theta small enough that cos(theta) underflows to exactly 1.0 in + # float64, so trace(R) rounds to exactly 3 and acos(1.0) == 0.0 + # exactly: this is the case that used to divide by sin(theta) == 0 + # with no guard. R itself is not bitwise np.eye(3) (it still has + # sin(theta) noise off the diagonal), so this also exercises the + # "is this actually the identity" branch on a matrix that isn't one. + R = rotx(1e-9) + assert not np.array_equal(R, np.eye(3)) + nt.assert_array_almost_equal(trlog(R, twist=True), [0, 0, 0]) + nt.assert_array_almost_equal(trlog(R), skew([0, 0, 0])) + + # theta small enough to be well within the near-identity regime, but + # not small enough to clamp: the general-case formula must stay + # accurate here rather than falling back to a coarse zero. This is + # the case a fuzzy identity-tolerance (checking R against I before + # computing theta, as originally proposed in #63) risks getting + # wrong in the other direction, by discarding a real small rotation. + theta = 1e-7 + for R in (rotx(theta), roty(theta), rotz(theta)): + v = trlog(R, twist=True) + nt.assert_almost_equal(np.linalg.norm(v), theta, decimal=12) + def test_trexp(self): R = trexp(skew([0.5, 0, 0])) nt.assert_array_almost_equal(R, rotx(0.5)) diff --git a/tests/base/test_transforms3d_plot.py b/tests/base/test_transforms3d_plot.py index d18d9300..aed1160d 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -22,9 +22,55 @@ from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr import matplotlib.pyplot as plt +from spatialmath.base.animate import Animate class Test3D(unittest.TestCase): + def test_animate_nonidentity_start(self): + # the axis artists of an Animate must sit where a static trplot puts them, both straight after + # trplot(end, start=start) and after _draw(end); losing either half of the fix breaks one of these + def axis_geometry(artists): + geometry = [] + for h in artists[:3]: # the three axes of the frame + # arrow style: compare the shaft only, the head wings differ + if hasattr(h, "_segments3d"): + geometry.append(np.asarray(h._segments3d[0]).T) + else: + geometry.append(np.asarray(h.get_data_3d())) + return geometry + + def static_geometry(pose, style): + fig = plt.figure() + try: + ax = fig.add_subplot(projection="3d") + trplot(pose, ax=ax, style=style, frame="moving") + kinds = ("Line3DCollection",) if style == "arrow" else ("Line3D",) + return axis_geometry( + [a for a in ax.get_children() if type(a).__name__ in kinds] + ) + finally: + plt.close(fig) + + start = transl(-1, 0, 2) @ trotz(0.4) + end = transl(1, 2, 1) @ trotx(0.3) + for style in ("line", "arrow", "rviz"): + fig = plt.figure() + try: + anim = Animate(dim=[-4, 4]) + anim.trplot(end, start=start, style=style, frame="moving") + artists = [x.h for x in anim.displaylist] + for actual, expected in zip( + axis_geometry(artists), static_geometry(start, style) + ): + nt.assert_allclose(actual, expected, atol=1e-12) + anim._draw(end) + for actual, expected in zip( + axis_geometry(artists), static_geometry(end, style) + ): + nt.assert_allclose(actual, expected, atol=1e-12) + finally: + plt.close(fig) + @pytest.mark.skipif( os.environ.get("CI") == "true" or (sys.platform.startswith("darwin") and sys.version_info < (3, 11)), @@ -73,6 +119,46 @@ def test_plot(self): plt.close("all") + def test_plot_rejects_unknown_kwarg(self): + # trplot() used to silently swallow an unrecognized keyword via a + # bare **kwargs sink instead of raising -- e.g. a typo'd + # framelabel= (the real parameter is frame=) drew the frame with + # no label and no error at all. + with self.assertRaises(TypeError): + trplot(transl(1, 2, 3), framelabel="A", block=False) + + def test_plot_anaglyph_forwards_params(self): + # the anaglyph branch recurses into trplot() for each eye; it used + # to forward only a small fixed subset of parameters (ax, frame, + # length, style, wtl, flo, d2), silently dropping textcolor, + # labels, originsize, origincolor, axislabel, axissubscript, width + # and projection for both eyes. + plt.figure() + ax = trplot( + transl(1, 2, 3), + frame="A", + anaglyph=True, + block=False, + textcolor="k", + originsize=0, + ) + # frame label + 3 axis labels, per eye + self.assertEqual(len(ax.texts), 8) + self.assertTrue(all(t.get_color() == "k" for t in ax.texts)) + plt.close("all") + + def test_plot_iterable_forwards_axissubscript(self): + # the "T is an iterable of transforms" branch used to forward + # every named parameter except axissubscript when recursing per + # transform, so axissubscript=False on the outer call was ignored. + plt.figure() + T = [transl(1, 2, 3), transl(2, 3, 4)] + ax = trplot(T, frame="F", axissubscript=False, block=False) + axis_labels = [t.get_text() for t in ax.texts if t.get_text() != r"$\{F\}$"] + self.assertTrue(axis_labels) + self.assertTrue(all("_{" not in text for text in axis_labels)) + plt.close("all") + @pytest.mark.skipif( plt.get_backend().lower() == "agg" or os.environ.get("CI") == "true" diff --git a/tests/base/test_transformsNd.py b/tests/base/test_transformsNd.py index 14c7fd42..7291eaa2 100755 --- a/tests/base/test_transformsNd.py +++ b/tests/base/test_transformsNd.py @@ -261,6 +261,64 @@ def test_homog(self): nt.assert_almost_equal(h2e([2, 4, 6, 2]), np.c_[1, 2, 3].T) + def test_e2h_vector(self): + # list, tuple, 1D array, column vector all give an (N+1,1) column + expected = np.c_[1, 2, 3, 1].T + for v in ([1, 2, 3], (1, 2, 3), np.r_[1, 2, 3], np.c_[1, 2, 3].T): + with self.subTest(type=type(v).__name__): + out = e2h(v) + self.assertEqual(out.shape, (4, 1)) + nt.assert_almost_equal(out, expected) + + def test_e2h_matrix(self): + # each column gains a trailing 1 + e = np.c_[[1, 2], [3, 4], [5, 6]] + nt.assert_almost_equal(e2h(e), np.array([[1, 3, 5], [2, 4, 6], [1, 1, 1]])) + + def test_h2e_vector(self): + # the result is scaled by the last element + expected = np.c_[1, 2, 3].T + for v in ([2, 4, 6, 2], (2, 4, 6, 2), np.r_[2, 4, 6, 2], np.c_[2, 4, 6, 2].T): + with self.subTest(type=type(v).__name__): + out = h2e(v) + self.assertEqual(out.shape, (3, 1)) + nt.assert_almost_equal(out, expected) + + def test_h2e_matrix(self): + # each column is scaled by its own last element + h = np.c_[[1, 2, 1], [3, 4, 2], [5, 6, 1]] + nt.assert_almost_equal(h2e(h), np.array([[1, 1.5, 5], [2, 2, 6]])) + + def test_homog_roundtrip(self): + v = np.c_[[1, 2, 3], [-4, 5, 0.5]] + nt.assert_almost_equal(h2e(e2h(v)), v) + + h = np.c_[[2, 4, 6, 2], [3, 6, 9, 3]] + nt.assert_almost_equal(e2h(h2e(h)), np.c_[[1, 2, 3, 1], [1, 2, 3, 1]]) + + def test_homog_row_vector_is_matrix(self): + # Documents (rather than endorses) current behavior: a 2D array is + # always a matrix of column vectors, so a (1,N) row is N 1-vectors. + # Changing this would silently break callers passing 1xN matrices. + self.assertEqual(e2h(np.ones((1, 3))).shape, (2, 3)) + nt.assert_almost_equal(e2h(np.c_[[1, 2, 3]].T), np.array([[1, 2, 3], [1, 1, 1]])) + nt.assert_almost_equal(h2e(np.array([[1.0, 2, 4], [1, 2, 2]])), np.array([[1, 1, 2]])) + + def test_h2e_too_few_rows(self): + # a homogeneous point needs at least 2 elements, h2e of 1 row would be + # an empty (0,N) array + for bad in (np.ones((1, 3)), np.ones((1, 1)), [5], 5, np.ones((1,))): + with self.subTest(value=repr(bad)): + with self.assertRaises(ValueError): + h2e(bad) + + def test_homog_bad_type(self): + for convert in (e2h, h2e): + for bad in ("hello", None, np.empty((0,))): + with self.subTest(convert=convert.__name__, value=repr(bad)): + with self.assertRaises(ValueError): + convert(bad) + def test_homtrans(self): # 3D T = trotx(pi / 2, t=[1, 2, 3]) diff --git a/tests/test_dualquaternion.py b/tests/test_dualquaternion.py index ed785313..cb9b5af9 100644 --- a/tests/test_dualquaternion.py +++ b/tests/test_dualquaternion.py @@ -84,11 +84,36 @@ def test_init(self): dq = UnitDualQuaternion(T) nt.assert_array_almost_equal(dq.SE3().A, T.A) + def test_init_negative_scalar(self): + # Rx(pi/4) above has no translation and a positive quaternion + # scalar part, so it can't exercise either bug that used to live + # here: SE3() used self.real.conj(), which silently returned + # -conj(real) whenever real had negative scalar part (flipping + # the sign of the recovered translation). This seed's first draw + # is confirmed to produce a UnitQuaternion with negative scalar + # part, so it's kept as a fixed regression case rather than + # relying on randomness at test time. + np.random.seed(0) + T = SE3.Rand() + dq = UnitDualQuaternion(T) + self.assertLess(dq.real.A[0], 0) + nt.assert_array_almost_equal(dq.SE3().A, T.A) + def test_norm(self): T = SE3.Rx(pi / 4) dq = UnitDualQuaternion(T) nt.assert_array_almost_equal(dq.norm(), (1, 0)) + def test_norm_negative_scalar(self): + # see test_init_negative_scalar: norm() used the same broken + # conj() and would crash with "math domain error" (sqrt of a + # small negative float) for this case before the fix. + np.random.seed(0) + T = SE3.Rand() + dq = UnitDualQuaternion(T) + self.assertLess(dq.real.A[0], 0) + nt.assert_array_almost_equal(dq.norm(), (1, 0)) + def test_multiply(self): T1 = SE3.Rx(pi / 4) T2 = SE3.Rz(-pi / 3) @@ -101,6 +126,28 @@ def test_multiply(self): d = d1 * d2 nt.assert_array_almost_equal(d.SE3().A, T.A) + def test_vector_transform(self): + # previously untested and broken: the q*P*conj(q) sandwich + # product's translation terms cancel exactly to zero under this + # class's own dual-part embedding convention (dual = + # 0.5*Pure(t)*real), so the old code silently applied only the + # rotation and dropped the translation entirely. + T = SE3(1, 2, 3) * SE3.Rx(0.3) + dq = UnitDualQuaternion(T) + v = np.array([4.0, 5.0, 6.0]) + vp = dq * v + expected = (T * v).flatten() + nt.assert_array_almost_equal(vp, expected) + + # also check a second, independent transform for good measure + np.random.seed(2) + SE3.Rand() + T = SE3.Rand() + dq = UnitDualQuaternion(T) + vp = dq * v + expected = (T * v).flatten() + nt.assert_array_almost_equal(vp, expected) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": # pragma: no cover diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index bc6f2e63..1fca4402 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -237,6 +237,26 @@ def test_parallel(self): self.assertFalse(L1.isparallel(L3)) self.assertFalse(L1 | L3) + def test_parallel_plucker_scale(self): + for scale1, scale2 in ((1, 1), (1e-8, 1e-8), (-1e-8, 1e-8)): + l1 = Line3.PointDir([0, 0, 0], [scale1, 0, 0]) + perpendicular = Line3.PointDir([0, 0, 1], [0, scale2, 0]) + parallel = Line3.PointDir([0, 0, 1], [scale2, 0, 0]) + + self.assertFalse(l1.isparallel(perpendicular)) + self.assertFalse(l1 | perpendicular) + self.assertTrue(l1.isparallel(parallel)) + self.assertTrue(l1 | parallel) + + # A loose angular tolerance must not shrink when Plucker coordinates grow. + l1 = Line3.PointDir([0, 0, 0], [1, 0, 0]) + # Only the directions matter here. A zero moment also avoids + # round-off in the constructor's Plucker orthogonality check. + almost_parallel = Line3.PointDir([0, 0, 0], [1, 1e-9, 0]) + scaled = Line3.PointDir([0, 0, 0], [1e8, 0.1, 0]) + self.assertTrue(l1.isparallel(almost_parallel, tol=1e8)) + self.assertTrue(l1.isparallel(scaled, tol=1e8)) + def test_intersect(self): L1 = Line3.PointDir([4, 5, 6], [1, 2, 3]) L2 = Line3.PointDir([5, 5, 6], [1, 2, 3]) @@ -253,6 +273,22 @@ def test_intersect(self): L3 ^ L4, ) + def test_intersect_plucker_scale(self): + for scale in (1, 1e-8, 1e8): + l1 = Line3.PointDir([0, 0, 0], [scale, 0, 0]) + crossing = Line3.PointDir([0, 0, 0], [0, scale, 0]) + skew = Line3.PointDir([0, 0, 1e-7], [0, scale, 0]) + + self.assertTrue(l1.isintersecting(crossing)) + self.assertTrue(l1 ^ crossing) + self.assertFalse(l1.isintersecting(skew)) + self.assertFalse(l1 ^ skew) + + # Tolerance on the miss distance is independent of direction magnitude. + l1 = Line3.PointDir([0, 0, 0], [1e8, 0, 0]) + near = Line3.PointDir([0, 0, 1e-8], [0, 1e8, 0]) + self.assertTrue(l1.isintersecting(near, tol=1e9)) + def test_commonperp(self): L1 = Line3.PointDir([4, 5, 6], [0, 0, 1]) L2 = Line3.PointDir([6, 5, 6], [0, 1, 0]) @@ -267,6 +303,22 @@ def test_commonperp(self): self.assertTrue(L ^ L1) self.assertTrue(L ^ L2) + def test_commonperp_plucker_scale(self): + for scale1, scale2 in ((1, 1), (1e-8, 1e-8), (-1e-8, 1e8)): + l1 = Line3.PointDir([1, 2, 3], [scale1, 0, 0]) + l2 = Line3.PointDir([0, 2, 4], [scale2, scale2, 0]) + + common = l1.commonperp(l2) + self.assertIsNotNone(common) + nt.assert_allclose(common.pp, [0, 2, 0], atol=1e-12) + self.assertAlmostEqual(abs(common.uw[2]), 1) + self.assertTrue(common.isintersecting(l1)) + self.assertTrue(common.isintersecting(l2)) + + nearest, distance = l1.closest_to_line(l2) + nt.assert_allclose(nearest, [0, 2, 3], atol=1e-12) + self.assertAlmostEqual(distance, 1) + def test_line(self): # mindist # intersect 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_pose2d.py b/tests/test_pose2d.py index fc3734d6..4e4f7151 100755 --- a/tests/test_pose2d.py +++ b/tests/test_pose2d.py @@ -509,7 +509,7 @@ def test_graphics(self): T1.plot(block=False, dims=[-2, 2]) T1.animate(repeat=False, dims=[-2, 2], nframes=10) - T1.animate(T0=T2, repeat=False, dims=[-2, 2], nframes=10) + T1.animate(start=T2, repeat=False, dims=[-2, 2], nframes=10) # ---------------------------------------------------------------------------------------# diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 589c0ec1..8e401920 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -784,6 +784,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 683fd315..8e291573 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -380,6 +380,42 @@ def test_canonic(self): R = rotz(-pi) qcompare(UnitQuaternion(R), np.r_[cos(pi / 2), sin(pi / 2) * np.r_[0, 0, 1]]) + def test_conj(self): + # plain Quaternion: conj negates the vector part only + q = Quaternion([1, 2, 3, 4]) + qcompare(q.conj(), [1, -2, -3, -4]) + self.assertIsInstance(q.conj(), Quaternion) + + # UnitQuaternion, positive scalar part: matches plain conjugate + u = UnitQuaternion(rotx(0.3)) + self.assertGreater(u.A[0], 0) + qcompare(u.conj(), qconj(u.A)) + self.assertIsInstance(u.conj(), UnitQuaternion) + + # UnitQuaternion, negative scalar part (reachable via norm=False, + # e.g. a >180 deg rotation before its own construction-time + # canonicalization -- constructed directly here to isolate conj()): + # conj() must NOT re-canonicalize the result's sign to be + # positive, unlike normal UnitQuaternion construction. If it did, + # this would silently return -conj(u) instead of the true + # conjugate, breaking the identity u * conj(u) == 1 that + # downstream algebra (e.g. DualQuaternion) depends on. + u = UnitQuaternion(np.array([[-0.5, 0.5, 0.5, 0.5]]), norm=False) + self.assertLess(u.A[0], 0) + qcompare(u.conj(), [-0.5, -0.5, -0.5, -0.5]) + self.assertIsInstance(u.conj(), UnitQuaternion) + + # the algebraic identity that must hold regardless of sign + qcompare(u * u.conj(), [1, 0, 0, 0]) + + # multi-valued UnitQuaternion, mixed signs + us = UnitQuaternion( + np.array([[-0.5, 0.5, 0.5, 0.5], [0.5, -0.5, -0.5, -0.5]]), norm=False + ) + conjs = us.conj() + qcompare(conjs.data[0], [-0.5, -0.5, -0.5, -0.5]) + qcompare(conjs.data[1], [0.5, 0.5, 0.5, 0.5]) + def test_convert(self): # test conversion from rotn matrix to u.quaternion and back R = rotx(0) diff --git a/tests/test_twist.py b/tests/test_twist.py index dfbd9f24..dcfb0fd1 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -213,6 +213,28 @@ def test_prod(self): x = Twist3([x1, x2]) array_compare(x.prod().SE3(), T1 * T2) + def test_unit(self): + # general (rotational) twist: normalized so |w| == 1 + T = SE3(1, 2, 3) * SE3.Rx(0.3) + S = Twist3(T) + u = S.unit() + self.assertAlmostEqual(np.linalg.norm(u.w), 1.0) + nt.assert_array_almost_equal(u.S, S.S / np.linalg.norm(S.w)) + + # prismatic twist (zero rotation): normalized direction vector, + # previously untested -- this branch raised ValueError before the + # fix (wrong-shape zero argument, S.w typo) + S = Twist3(np.r_[3, 4, 0, 0, 0, 0]) + u = S.unit() + self.assertAlmostEqual(np.linalg.norm(u.v), 1.0) + nt.assert_array_almost_equal(u.w, [0, 0, 0]) + + def test_pole(self): + T = SE3(1, 2, 3) * SE3.Rx(0.3) + S = Twist3(T) + p = S.pole + self.assertEqual(len(p), 3) + class Twist2dTest(unittest.TestCase): def test_constructor(self): @@ -375,6 +397,32 @@ def test_prod(self): x = Twist2([x1, x2]) array_compare(x.prod().SE2(), T1 * T2) + def test_unit(self): + # general (rotational) twist: normalized so |w| == 1. Previously + # broken: branches were swapped (this case fell into the "zero + # rotation" branch and tried to construct Twist2 with a 3-element + # zero argument instead of scalar 0, raising ValueError). + T = SE2(1, 2, 0.3) + S = Twist2(T) + u = S.unit() + self.assertAlmostEqual(abs(u.w), 1.0) + nt.assert_array_almost_equal(u.S, S.S / abs(S.w)) + + # prismatic twist (zero rotation): normalized direction vector + S = Twist2([3, 4], 0) + u = S.unit() + self.assertAlmostEqual(np.linalg.norm(u.v), 1.0) + self.assertEqual(u.w, 0) + + def test_pole(self): + # previously broken: docstring example called S.pole() but pole + # is a @property, not a method -- TypeError: 'numpy.ndarray' + # object is not callable + T = SE2(1, 2, 0.3) + S = Twist2(T) + p = S.pole + self.assertEqual(len(p), 2) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": diff --git a/tests/test_version.py b/tests/test_version.py new file mode 100644 index 00000000..e8590bb6 --- /dev/null +++ b/tests/test_version.py @@ -0,0 +1,22 @@ +import re +import unittest +from pathlib import Path + +import spatialmath + + +class TestVersion(unittest.TestCase): + def test_version_is_a_non_empty_string(self): + self.assertIsInstance(spatialmath.__version__, str) + self.assertTrue(spatialmath.__version__) + + def test_version_matches_pyproject(self): + pyproject = Path(__file__).parent.parent / "pyproject.toml" + match = re.search(r'^version\s*=\s*"([^"]+)"', pyproject.read_text(), re.MULTILINE) + self.assertIsNotNone(match, f"couldn't find a version in {pyproject}") + self.assertEqual(spatialmath.__version__, match.group(1)) + + +# ---------------------------------------------------------------------------------------# +if __name__ == "__main__": + unittest.main()