From 87e5ed54257cd4a04d4f2cdb0f6512b32247e6b8 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 17 Aug 2026 17:28:44 +1000 Subject: [PATCH 01/33] fix: __version__ guard catches everything and leaves it unset on failure Bare except: pass means any exception during the importlib.metadata lookup gets silently swallowed (not just PackageNotFoundError for "not installed"), and __version__ never gets set at all in that case -- spatialmath.__version__ raises AttributeError instead of giving something to print. Narrow the except to PackageNotFoundError and fall back to an explicit "unknown" so the attribute always exists. --- spatialmath/__init__.py | 7 +++++-- tests/test_version.py | 22 ++++++++++++++++++++++ 2 files changed, 27 insertions(+), 2 deletions(-) create mode 100644 tests/test_version.py 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/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() From 8c9f35f3a00d54b574786e8e20ce5717ac22f4b6 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 20 Aug 2026 17:16:45 +1000 Subject: [PATCH 02/33] test: add regression coverage for trlog near-identity division trlog's general-case branch divides by sin(theta), computed from trace(R) via acos. The only existing test used R = np.eye(3) exactly, never a numerically near-identity matrix as produced by real computation -- the case #63 (unmerged) reported hitting a divide-by-zero on. Confirmed against the pre-a9fc08a code (rework code to be more robust to nearly identity rotation matrix) that this exact input raised FloatingPointError there; current code's crisp `st == 0` guard handles it cleanly. --- tests/base/test_transforms3d.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) 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)) From 457128f47a16fa408600a35f76ee8e9a66401b0f Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 08:44:53 +1000 Subject: [PATCH 03/33] refactor(base): collapse the three-way Python-version type shim into one file spatialmath/base/types.py dispatched on sys.version_info across _types_35.py, _types_39.py and _types_311.py, from when Python 3.5-3.10 support coexisted. pyproject.toml has required Python >=3.10 since #186, so _types_35.py could never be reached, and _types_39/_types_311 differed only in where Self is imported from. Collapse to a single file with a two-line version check; no change to the names or aliases it exports. --- spatialmath/base/_types_311.py | 157 ------------------------------ spatialmath/base/_types_35.py | 150 ---------------------------- spatialmath/base/_types_39.py | 158 ------------------------------ spatialmath/base/types.py | 173 +++++++++++++++++++++++++++++++-- 4 files changed, 166 insertions(+), 472 deletions(-) delete mode 100644 spatialmath/base/_types_311.py delete mode 100644 spatialmath/base/_types_35.py delete mode 100644 spatialmath/base/_types_39.py 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/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] From 8f1ab30b6a26a5ba0cd2b26212ef11234a028990 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 10:31:54 +1000 Subject: [PATCH 04/33] perf(base): speed up isR/ishom/ishom2 orthogonality checks np.linalg.det/norm and np.eye carry large dispatch overhead relative to the actual work for 2x2/3x3 matrices, dominating SE3/SO3 constructor cost when check=True. Replace with explicit cofactor- expansion determinants and a squared Frobenius residual for the 2x2 and 3x3 cases (falling back to the generic path otherwise), and replace the bottom-row `all(... == np.array(...))` checks in ishom/ishom2 with direct scalar comparisons. ~2x faster isR, ~30-40% faster SE3(T, check=True) construction. Co-Authored-By: Claude Sonnet 5 --- spatialmath/base/transforms2d.py | 7 ++++++- spatialmath/base/transforms3d.py | 8 +++++++- spatialmath/base/transformsNd.py | 32 ++++++++++++++++++++++++++++---- 3 files changed, 41 insertions(+), 6 deletions(-) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 50e35d02..475d45a9 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -361,7 +361,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 + ) ) ) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 1bc90600..94831c16 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -382,7 +382,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 + ) ) ) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..e88e80a1 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]: From af473dbcf1b96a89e684d31b763bec2b8e9aa9f0 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 11:26:00 +1000 Subject: [PATCH 05/33] perf(base): speed up trnorm and tr2adjoint Both functions built small, fixed-size (3x3/4x4/6x6) results out of generic NumPy helpers (np.cross/unitvec/np.stack in trnorm, np.block in tr2adjoint) whose dispatch overhead - built for arbitrary shapes and broadcasting - dominates cost at this size. Same pattern as the isR/ishom speedup in #213. Replaced with explicit scalar arithmetic (trnorm) and direct pre-allocated slice-assignment (tr2adjoint), dtype preserved via np.zeros(..., dtype=T.dtype) so tr2adjoint's documented SymPy support is unaffected. ~12x faster trnorm, ~2.7x faster tr2adjoint standalone. End to end: SE3 @ SE3 (which normalizes via trnorm) drops from ~30us to ~4us. Verified bit-for-bit numeric equivalence against the prior implementation over 200 random SO(3)/SE(3) trials, plus symbolic (SymPy dtype=object) equivalence for tr2adjoint. Co-Authored-By: Claude Sonnet 5 --- spatialmath/base/transforms3d.py | 52 ++++++++++++++++++++++---------- 1 file changed, 36 insertions(+), 16 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 1bc90600..1f60597e 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -1591,17 +1591,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 @@ -2718,7 +2737,6 @@ def tr2adjoint(T): :SymPy: supported """ - Z = np.zeros((3, 3), dtype=T.dtype) if T.shape == (3, 3): # SO(3) adjoint R = T @@ -2726,12 +2744,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") From 9f13356a31a713379ba6b0ea386551274d23d550 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 11:36:45 +1000 Subject: [PATCH 06/33] perf(base): speed up qqmul and qvmul Both used np.dot/np.cross on 3-vectors, whose generic dispatch overhead - built for arbitrary shapes/broadcasting - dominates cost at this size, same pattern as isR/trnorm/tr2adjoint (#213, #214). qqmul: replaced with explicit scalar Hamilton-product arithmetic, bit-identical to the prior result (verified to ~1 ULP over 2000 random trials). qvmul: replaced the q * pure(v) * conj(q) sandwich (two full Hamilton products, each wasting work on a zero scalar part, via qqmul/qpure/ qconj with their own getvector re-validation) with the closed-form rotation identity v' = v + 2s(w x v) + 2 w x (w x v) for q = (s, w). This is a different, well-known equivalent formula rather than a re-expression of the same one, so it is not bit-identical - verified to ~1e-14 absolute over 2000 random trials (v scaled to magnitude ~10), well within floating-point noise. qqmul ~10x faster (15.0us -> 1.4us isolated), qvmul ~24x faster (33.4us -> 1.4us isolated). End to end: Q1 * v drops from ~33us to ~4us (~9x), Q1 * Q2 from ~19us to ~10us (~2x - qqmul itself is no longer the bottleneck there; the remainder is UnitQuaternion construction overhead, in particular qunit()'s np.linalg.norm/np.r_ calls, which is a separate, un-addressed candidate for a future fix in the same spirit). Co-Authored-By: Claude Sonnet 5 --- spatialmath/base/quaternions.py | 37 ++++++++++++++++++++++++++------- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 24a9cadd..49fa54c0 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -326,12 +326,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 +405,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: From 764e7f5be876848a5da977b3e13b7fe19164622d Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 11:57:09 +1000 Subject: [PATCH 07/33] perf(base): speed up isskewa and cache identity matrix in trexp/trlog/rodrigues trexp/trlog don't have one dominant wasteful generic call like the isR/trnorm/tr2adjoint/qqmul/qvmul fixes did (#213/#214/#215) - they're deep call chains (trexp -> isskewa -> vexa -> iszerovec -> unittwist_norm -> rodrigues -> skew -> rt2tr -> ishom -> isR) where cost is spread thin across many small layers. Two targeted, verified wins pulled out of that chain: - isskewa (validity check trexp runs on se(3) input) had the same bug ishom had pre-#213: np.linalg.norm on a small fixed matrix, plus an all(S[-1,:] == 0) array-allocation-and-compare for the bottom row. Fixed the same way, ~1.9x faster standalone. - np.eye(3) was rebuilt from scratch on every call in rodrigues (1x), trexp's V-matrix construction (1x), and trlog (2x), despite only ever being used as a read-only operand in an addition/multiplication that produces a new array. Replaced with a module-level constant _EYE3, used only at call sites where it's provably never mutated or returned directly (an aliasing hazard if it were) - the four zero-motion/zero-rotation early-return `np.eye(N)` calls in rodrigues/trexp are untouched, left as fresh arrays, since they hand the object directly to the caller. isskew (the plain, non-augmented so(n) check) was also tried with the same explicit-arithmetic treatment as isskewa, but did NOT show a reliable win under min-of-repeats benchmarking - the only cost it avoids is np.linalg.norm on an already-cheap `S + S.T`, not enough margin to reliably beat by hand-unrolling. Reverted to the original implementation rather than keep an unproven "fix". Net effect, measured old-vs-new in the same process (min of 9 repeats each, to suppress system jitter after an earlier round of misleading single-run numbers): isskewa ~1.9x, rodrigues ~1.13x, trexp ~1.05x, trlog ~1.10x. Real but modest - see the PR description's "further speedup opportunities" section for what a bigger win here would actually require. Co-Authored-By: Claude Sonnet 5 --- spatialmath/base/transforms3d.py | 14 ++++++---- spatialmath/base/transformsNd.py | 44 +++++++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 1bc90600..f020364e 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -52,6 +52,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) + # ---------------------------------------------------------------------------------------# @@ -1355,7 +1360,7 @@ def trlog( else: # general case Ginv = ( - np.eye(3) + _EYE3 - S / 2 + (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S ) @@ -1374,8 +1379,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: @@ -1514,7 +1518,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 ) @@ -2774,7 +2778,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 ) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..3fc9b036 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -408,6 +408,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 +441,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: From e6ce47fb22c8186c1bc087a00e351710fca824c7 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 15 Sep 2026 21:14:43 +0200 Subject: [PATCH 08/33] chore: switch docs build to PyPI sphinx-pyrunblock instead of git+https sphinx-pyrunblock is Peter's own package (published to PyPI, hard fork of the original WhyNotHugo/sphinx-autorun), reachable at the same content today via git+https://github.com/petercorke/sphinx-autorun.git (an old, pre-rename URL that still redirects). Installing the PyPI wheel directly is ~2-4x faster in CI (no git clone/VCS resolution) and makes pyproject.toml's declared dependency honest -- previously "sphinx-autorun" there would, in isolation, resolve to the unrelated upstream PyPI package of that name rather than Peter's fork. No functional change: both paths resolve to the same sphinx-pyrunblock 1.1.0 release. Verified with a real sphinx-build against a clean venv. --- .github/workflows/sphinx.yml | 1 - docs/source/conf.py | 2 +- pyproject.toml | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/sphinx.yml b/.github/workflows/sphinx.yml index c0beb95a..2c04a57f 100644 --- a/.github/workflows/sphinx.yml +++ b/.github/workflows/sphinx.yml @@ -17,7 +17,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/docs/source/conf.py b/docs/source/conf.py index b5fcf84a..66262759 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -47,7 +47,7 @@ "sphinx.ext.inheritance_diagram", "matplotlib.sphinxext.plot_directive", "sphinx_autodoc_typehints", - "sphinx_autorun", + "sphinx_pyrunblock", "sphinx.ext.intersphinx", "sphinx_favicon", ] diff --git a/pyproject.toml b/pyproject.toml index 95919eb3..e519f963 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -61,7 +61,7 @@ dev = [ docs = [ "sphinx", "sphinx-rtd-theme", - "sphinx-autorun", + "sphinx-pyrunblock", "sphinxcontrib-jsmath", "sphinx-favicon", "sphinx-autodoc-typehints", From 890d07809472274c54c73117f9d4c9f4d0ee875f Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 16 Sep 2026 00:20:28 +0200 Subject: [PATCH 09/33] docs: add sphinx-copybutton and sphinx-codeautolink to docs deps --- pyproject.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pyproject.toml b/pyproject.toml index 95919eb3..2e72f80a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -65,6 +65,8 @@ docs = [ "sphinxcontrib-jsmath", "sphinx-favicon", "sphinx-autodoc-typehints", + "sphinx-copybutton", + "sphinx-codeautolink", ] ros-humble = [ From 0c8b16a53a32335e3772bbab526c0aa3114ac415 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 16 Sep 2026 00:21:01 +0200 Subject: [PATCH 10/33] docs: register sphinx_copybutton/sphinx_codeautolink extensions --- docs/source/conf.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/docs/source/conf.py b/docs/source/conf.py index b5fcf84a..d8647679 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 ----------------------------------------------------- @@ -50,6 +51,14 @@ "sphinx_autorun", "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 From 21ba463afff31f5c3036a3c30d724de2ae882dbf Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 16 Sep 2026 00:21:28 +0200 Subject: [PATCH 11/33] docs: configure sphinx-codeautolink and sphinx-copybutton options --- docs/source/conf.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/docs/source/conf.py b/docs/source/conf.py index d8647679..c9883189 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -193,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), From 563de344ad4abfa8f496790896201b9ccea7d57c Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 16 Sep 2026 02:24:00 +0200 Subject: [PATCH 12/33] docs: fix :linenos: indentation mismatch in intro.rst sphinx-codeautolink's own parsing pass over code blocks is stricter than plain docutils about this pre-existing 3-space/4-space mismatch between the :linenos: option and the code content, surfacing a new 'unexpected indent' warning that was previously silently tolerated. --- docs/source/intro.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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') From e5abf1244e1dc1555557b314550c065f76b79f6e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 16 Sep 2026 18:05:03 +0200 Subject: [PATCH 13/33] fix: broken docstring examples in Twist2/Twist3/DualQuaternion, and the root cause they shared Found via the sphinx-pyrunblock rollout (PR #221): several `.. runblock::` docstring examples were silently baking broken tracebacks into the published docs instead of demonstrating real output. ## Twist2.unit() / Twist3.unit() Branches were swapped (comments didn't match what the code under them did), `self.w` had a `S.w` typo (undefined name), and the "prismatic" branch passed a wrong-shape zero argument for Twist2 (3-element list where a scalar is required). Fixed the branch logic, and use `abs()` instead of `smb.norm()` for Twist2's scalar `w`. ## Twist2.pole The docstring's own example called `S.pole()`, but `pole` is a `@property`, not a method -- `TypeError: 'numpy.ndarray' object is not callable`. (Twist3.pole's own example was already correct; only its prose incorrectly said `X.pole()` too.) ## DualQuaternion: a shared root cause across three methods Fixing `DualQuaternion.SE3()`'s docstring (called `d.T` instead of the actual `d.SE3()` method) surfaced a real bug: `UnitQuaternion.conj()` silently re-canonicalized its result's sign (forces scalar part >= 0), which is correct for constructing a `UnitQuaternion` from arbitrary data (q and -q are the same rotation) but wrong for conjugation, an algebraic operation that must satisfy q*conj(q) == 1 for downstream algebra to be correct. Fixed `Quaternion.conj()` itself (spatialmath/quaternion.py) rather than routing around it at each call site -- this is what "conjugate" should mean regardless of caller. No existing test asserted the old (re-canonicalizing) behaviour, only the return type. This one fix resolved three separate, previously-untested DualQuaternion bugs at once: - `SE3()`: silently returned a transform with negated translation for any rotation with negative quaternion scalar part. - `norm()`: crashed outright (`math domain error`) for the same reason -- also added a small floating-point clamp, since the norm-squared terms are mathematically non-negative but rounding can leave e.g. -1e-17 instead of exactly 0. - Vector transformation (`dq * v`): found while re-verifying `norm()`'s fix -- unrelated to the conj() sign bug, this affected *every* case regardless of sign. The textbook q*P*conj(q) sandwich product's translation terms cancel to exactly zero under this class's own dual-part embedding convention (`__init__` builds `dual = 0.5*Pure(t)*real`, translation on the left) -- silently applying only the rotation and dropping translation entirely. Since `SE3()` already correctly extracts (R, t) from this same embedding, reused it instead of hand-deriving a second, convention-specific sandwich formula. ## Test coverage All of the above had zero prior test coverage (existing DualQuaternion tests used SE3.Rx(pi/4) -- no translation, positive quaternion scalar part -- which can't exercise any of these bugs). Added regression tests for conj()'s sign behaviour (including a mixed-sign multi-valued case), both branches of Twist2/Twist3.unit(), Twist2/Twist3.pole, and DualQuaternion's SE3()/norm()/vector-transform with both a negative-quaternion-scalar case and a general case. Full suite: 334 passed (up from 326), 0 regressions. Real sphinx-build verification (isolated venv, before/after): RUNBLOCK-ERROR count 6 -> 0, warning count unchanged at 14 (confirmed stable across repeat builds of both branches, ruling out build-to-build nondeterminism). --- spatialmath/DualQuaternion.py | 22 +++++++++++++--- spatialmath/quaternion.py | 42 +++++++++++++++++++++++++++--- spatialmath/twist.py | 28 ++++++++++---------- tests/test_dualquaternion.py | 47 ++++++++++++++++++++++++++++++++++ tests/test_quaternion.py | 36 ++++++++++++++++++++++++++ tests/test_twist.py | 48 +++++++++++++++++++++++++++++++++++ 6 files changed, 202 insertions(+), 21 deletions(-) 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/quaternion.py b/spatialmath/quaternion.py index a5759709..fe7d11ee 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -306,17 +306,53 @@ 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. + 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/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/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_quaternion.py b/tests/test_quaternion.py index 8a87a433..63a66946 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__": From 283f4adaf79f53f55e04c6c678d770ef40ef948f Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 16 Sep 2026 20:12:36 +0200 Subject: [PATCH 14/33] docs: note conj()'s sign fix as a versionchanged, bug present in 1.1.17 and earlier --- spatialmath/quaternion.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index fe7d11ee..28d44b51 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -322,6 +322,16 @@ def conj(self) -> Quaternion: 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 bde369027df4257a623a4acacc076eb3daa8314e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 18 Sep 2026 20:26:23 +0200 Subject: [PATCH 15/33] chore: declare Python 3.13 and 3.14 support, add 3.14 to CI matrix Co-Authored-By: Claude Sonnet 5 --- .github/workflows/master.yml | 2 +- pyproject.toml | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/master.yml b/.github/workflows/master.yml index 9e8a7fa7..d4bca5b0 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/pyproject.toml b/pyproject.toml index 10b74a4c..faf93b1b 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", From dbaaaf1a42d6d433947d9e602238e77e582ac120 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 12:29:48 +1000 Subject: [PATCH 16/33] chore: reorganize timing.py into per-category tables, collapse test boilerplate Reworks the ad hoc, partly-commented-out timing script (which had been manually re-enabled with a couple of tweaks, uncommitted, sitting in the working tree across several perf PRs this session) into something that's actually a useful reference: - Split into separate tables, one per category (SO(3)/SE(3) base functions, SE3 class, quaternion base functions, UnitQuaternion class, twist/exponential-map base functions, Twist3 class, NumPy baseline), printed with a plain header between each. Base-level (module functions on raw arrays) and class-level (constructors/ operators/properties on instances) are now consistently separated throughout, rather than mixed together - the old Twist3 section in particular had base functions (skew, trlog, trexp, rodrigues, ...) interleaved with class-level operations. - Fixed a corrupted docstring header left over from a bad find/replace on upstream (`# -*- coding", t)` / `@author", t)`), unrelated to any of this but noticed while rewriting the file. - Fixed mislabelled/ambiguous rows: `inner()` -> `np.inner(s, s).sum()`, bare `cross()` -> `base.cross(a, b)` (paired explicitly against `np.cross(a, b)`), and rows comparing decomposed-(R,t) vs full-4x4 composition that had identical labels for different code paths. - Added coverage that didn't exist before: SE3 <-> RPY, SE3 <-> Euler, SE3 -> UnitQuaternion, UnitQuaternion <-> rotation matrix, UnitQuaternion -> SE3, and SE3 @ SE3 (normalized compose) alongside SE3 * SE3. - Collapsed the `timeit.timeit(...); result(...)` two-line-per-test boilerplate into a single `timeit(stmt, label, setup)` call (module import aliased to `_timeit` to free up the name). Replaced the single `timeit.timeit(..., number=N)` run per test with `min(timeit.repeat(..., number=N, repeat=REPEATS))` - a single long run doesn't distinguish real cost from a GC pause or OS scheduling hiccup, min-of-repeats does. N dropped from 100_000 to 10_000 (an arbitrary starting point either way) since the repeats now do the work of suppressing noise that a bigger N was being used for. Co-Authored-By: Claude Sonnet 5 --- spatialmath/timing.py | 398 ++++++++++++++++++------------------------ 1 file changed, 170 insertions(+), 228 deletions(-) diff --git a/spatialmath/timing.py b/spatialmath/timing.py index ae169909..ffb35b6b 100755 --- a/spatialmath/timing.py +++ b/spatialmath/timing.py @@ -1,219 +1,186 @@ #!/usr/bin/env python3 -# -*- coding", t) +# -*- coding: utf-8 -*- """ Created on Fri Apr 10 14:22:36 2020 -@author", t) +@author: Peter Corke """ -import timeit +import timeit as _timeit from ansitable import ANSITable, Column -N = 100000 +N = 10_000 +REPEATS = 5 -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) +table = None -# 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) +def new_table(): + return ANSITable( + Column("Operation", headalign="^"), + Column("Time (μs)", headalign="^", fmt="{:.2f}"), + border="thick", + ) -# 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) +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) -# t = timeit.timeit(stmt='a = base.qvmul(q1,v)', setup=quat_setup, number=N) -# result("base.qvmul", t) +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() -# # ------------------------------------------------------------------------- # -# 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) +# ------------------------------------------------------------------------- # +transforms_setup = ''' +from spatialmath import SE3 +from spatialmath import base -# t = timeit.timeit(stmt='a = S1.exp(1)', setup=twist_setup, number=N) -# result("Twist3.Exp()", t) +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) -# t = timeit.timeit(stmt='a = base.skewa(v)', setup=twist_setup, number=N) -# result("skew", t) +# ------------------------------------------------------------------------- # +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) -# t = timeit.timeit(stmt='a = base.skewa(s)', setup=twist_setup, number=N) -# result("skewa", t) +# ------------------------------------------------------------------------- # +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) +''' -# t = timeit.timeit(stmt='a = base.vexa(se3)', setup=twist_setup, number=N) -# result("vexa", t) +section("Quaternion base functions") -# t = timeit.timeit(stmt='a = base.trlog(T1)', setup=twist_setup, number=N) -# result("trlog", t) +timeit('a = base.qqmul(q1,q2)', "base.qqmul(q1, q2)", quat_setup) +timeit('a = base.qvmul(q1,v)', "base.qvmul(q1, v)", quat_setup) -# t = timeit.timeit(stmt='a = base.trlog(T1, twist=True)', setup=twist_setup, number=N) -# result("trlog as twist", t) +# ------------------------------------------------------------------------- # +section("UnitQuaternion class") -# t = timeit.timeit(stmt='a = base.trexp(se3)', setup=twist_setup, number=N) -# result("trexp", t) +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) -# t = timeit.timeit(stmt='a = A1 @ s', setup=twist_setup, number=N) -# result("(6,6) * (6,)", t) +# ------------------------------------------------------------------------- # +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) -# t = timeit.timeit(stmt='a = base.rodrigues(v)', setup=twist_setup, number=N) -# result("rodrigues", t) +# ------------------------------------------------------------------------- # +section("Twist3 class") -# t = timeit.timeit(stmt='a = cos(0.3)', setup=twist_setup, number=N) -# result("math.cos", t) +# 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. -# t = timeit.timeit(stmt='a = np.cos(0.3)', setup=twist_setup, number=N) -# result("np.cos", t) +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 = """ @@ -228,47 +195,22 @@ def result(op, t): 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) +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() From 5f5a8939b5e9668327aa5d91c76157462ad9ca0a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 18 Sep 2026 20:48:50 +0200 Subject: [PATCH 17/33] chore: move timing.py to benchmarks/benchmark_smtb.py The script is a dev tool, not library API; move it out of the installed package so it no longer ships in the wheel. Co-Authored-By: Claude Sonnet 5 --- spatialmath/timing.py => benchmarks/benchmark_smtb.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename spatialmath/timing.py => benchmarks/benchmark_smtb.py (100%) diff --git a/spatialmath/timing.py b/benchmarks/benchmark_smtb.py similarity index 100% rename from spatialmath/timing.py rename to benchmarks/benchmark_smtb.py From a3f828f6bad4218329ef3d5eab0e4e8c5310d6cc Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 18 Sep 2026 20:50:02 +0200 Subject: [PATCH 18/33] feat(benchmarks): print machine and version summary before results CPU, OS, Python, numpy and spatialmath versions and the timing settings, so a pasted table is self-describing. Modeled on RTB's rne_speed.py. Co-Authored-By: Claude Sonnet 5 --- benchmarks/benchmark_smtb.py | 78 +++++++++++++++++++++++++++++++++++- 1 file changed, 77 insertions(+), 1 deletion(-) diff --git a/benchmarks/benchmark_smtb.py b/benchmarks/benchmark_smtb.py index ffb35b6b..abed7abb 100755 --- a/benchmarks/benchmark_smtb.py +++ b/benchmarks/benchmark_smtb.py @@ -1,21 +1,95 @@ #!/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="^"), @@ -41,6 +115,8 @@ def section(title): table = new_table() +print_machine_summary() + # ------------------------------------------------------------------------- # transforms_setup = ''' from spatialmath import SE3 From 2c46775a3ab3c5a3f6d8343c1748543e785d95d9 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sat, 26 Sep 2026 17:37:34 -0400 Subject: [PATCH 19/33] fix: trplot/trplot2 silently swallowed unknown keyword arguments Both trplot() and trplot2() ended with a bare **kwargs that was never read anywhere in their bodies, so a typo'd keyword (e.g. framelabel= instead of frame=) was silently accepted and ignored instead of raising -- it drew the frame with no error and no label at all. Removing the dead sink lets Python's own unexpected-keyword-argument check do its job. While rewriting the two places that used to lean on this catch-all: - trplot's anaglyph branch only forwarded a small fixed subset of parameters to each eye's recursive call, silently dropping textcolor, labels, originsize, origincolor, axislabel, axissubscript, width and projection; it also never returned ax and never actually called plt.show(block=...), so anaglyph mode ignored block entirely. - trplot's "T is an iterable of transforms" branch forwarded every parameter except axissubscript, and also never returned ax. - tranimate/tranimate2 were splatting the same unfiltered kwargs dict into Animate()/Animate2()'s constructor, the drawing call, and run()'s animation-control call, relying on each one's own dead **kwargs sink to drop what it didn't need. Once trplot/trplot2 validate strictly, run()-only parameters (movie, repeat, interval, nframes, wait) needed to be split off before the drawing call. Also fixes a long-standing typo in test_pose2d.py::test_graphics (T0=T2, which was never a real parameter -- animate()'s documented one is start=) that this same silent-swallow bug had been quietly masking. Adds tests/base/test_animate.py: constructing a FuncAnimation under a non-interactive backend doesn't actually run its per-frame update() callback (confirmed directly -- Animate2's frame counter never advances past its initial value), so existing animate() tests only proved construction didn't raise. These tests force every frame through the real callback via FuncAnimation.save() (PillowWriter, no external ffmpeg dependency) and assert trinterp/trinterp2 actually ran across a real s: 0->1 sweep. Co-Authored-By: Claude Sonnet 5 --- spatialmath/base/transforms2d.py | 12 +++- spatialmath/base/transforms3d.py | 34 ++++++++-- tests/base/test_animate.py | 97 ++++++++++++++++++++++++++++ tests/base/test_transforms2d.py | 8 +++ tests/base/test_transforms3d_plot.py | 40 ++++++++++++ tests/test_pose2d.py | 2 +- 6 files changed, 184 insertions(+), 9 deletions(-) create mode 100644 tests/base/test_animate.py diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index e9bc0e70..d09e820d 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -1336,7 +1336,6 @@ def trplot2( width: float = 1, d1: float = 0.1, d2: float = 1.15, - **kwargs, ): """ Plot a 2D coordinate frame @@ -1603,9 +1602,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..8f958e31 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -3029,7 +3029,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 +3216,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 +3227,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 +3257,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 +3313,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 +3529,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/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_transforms2d.py b/tests/base/test_transforms2d.py index 6a406006..61e3adab 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -296,6 +296,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_plot.py b/tests/base/test_transforms3d_plot.py index d18d9300..2a4ba18e 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -73,6 +73,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/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) # ---------------------------------------------------------------------------------------# From 3deefc8d9c0007f54372788a2617e1b71033c345 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Sun, 27 Sep 2026 21:01:46 +0800 Subject: [PATCH 20/33] Make Line3 relation tests invariant to Plucker scale MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Normalize line directions and moments for parallel and intersection checks. Construct common perpendiculars from the nearest point and unit direction cross product, with regression coverage across Plucker scalings. Assisted-by: OpenAI Codex (GPT-6 Sol) Signed-off-by: 李永祺 --- spatialmath/geom3d.py | 33 ++++++++++++++++++---------- tests/test_geom3d.py | 50 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+), 11 deletions(-) diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index edc4dc08..241c779d 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 """ @@ -1009,16 +1024,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/tests/test_geom3d.py b/tests/test_geom3d.py index 81a50fdb..4e4a874c 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -207,6 +207,24 @@ 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]) + almost_parallel = Line3.PointDir([0, 0, 1], [1, 1e-9, 0]) + scaled = Line3.PointDir([0, 0, 1], [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]) @@ -223,6 +241,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]) @@ -237,6 +271,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 From 149653a3ccad5499a8047260198613ba3d620a89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Mon, 28 Sep 2026 15:27:48 +0800 Subject: [PATCH 21/33] test: avoid round-off in scaled Line3 fixture MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Keep the near-parallel tolerance test focused on direction vectors by constructing both lines through the origin. This avoids an unrelated Plucker orthogonality check failure on NumPy 2. Assisted-by: OpenAI Codex (GPT-6 Sol) Signed-off-by: 李永祺 --- tests/test_geom3d.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index 4e4a874c..d8b6a4d0 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -220,8 +220,10 @@ def test_parallel_plucker_scale(self): # A loose angular tolerance must not shrink when Plucker coordinates grow. l1 = Line3.PointDir([0, 0, 0], [1, 0, 0]) - almost_parallel = Line3.PointDir([0, 0, 1], [1, 1e-9, 0]) - scaled = Line3.PointDir([0, 0, 1], [1e8, 0.1, 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)) From b91dcd8705bd09e734750de8a87cbeeb5008a2e7 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 8 Oct 2026 21:02:49 -0700 Subject: [PATCH 22/33] Fix double transform of animation start pose in Animate.trplot --- spatialmath/base/animate.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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) From c08331f68ca51284aaeaa6276a15447135aecea6 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 8 Oct 2026 21:03:16 -0700 Subject: [PATCH 23/33] Add regression test for animation start pose --- tests/base/test_transforms3d_plot.py | 29 ++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/tests/base/test_transforms3d_plot.py b/tests/base/test_transforms3d_plot.py index d18d9300..002598d1 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -22,9 +22,38 @@ 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): + 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") + for pose in (start, end): + anim._draw(pose) + for obj in anim.displaylist: + expected = pose @ obj.p if hasattr(obj, "p") else None + if obj.type == "line": + nt.assert_allclose( + np.asarray(obj.h.get_data_3d()), + expected[:3], + atol=1e-14, + ) + nt.assert_allclose(obj.p[:3, 0], np.zeros(3), atol=1e-14) + elif obj.type == "arrow": + nt.assert_allclose( + np.asarray(obj.h._segments3d), + expected[:3].T.reshape(3, 2, 3), + atol=1e-14, + ) + finally: + plt.close(fig) + @pytest.mark.skipif( os.environ.get("CI") == "true" or (sys.platform.startswith("darwin") and sys.version_info < (3, 11)), From c8d7188b41c4d93dec18fe0e68aee2a6f52483a2 Mon Sep 17 00:00:00 2001 From: mohith Date: Fri, 9 Oct 2026 10:54:26 +0530 Subject: [PATCH 24/33] Reject unsupported ndarray ranks in isvector --- spatialmath/base/argcheck.py | 4 ++-- tests/base/test_argcheck.py | 23 ++++++++++++++++++++++- 2 files changed, 24 insertions(+), 3 deletions(-) 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/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) From 56c5f217bf1634b932c0fd5cb146a0011736fa0b Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sat, 10 Oct 2026 16:15:35 +1000 Subject: [PATCH 25/33] test: add direct tests for e2h/h2e; fix docstring seealso e2h/h2e had one assertion each (N-vector only), with the matrix path and h2e scaling only exercised indirectly via the pose tests. Add tests for vector input types, matrix input, per-column scaling, round trips, bad types, and pin the current (1,N)-is-a-matrix interpretation. Docstrings: e2h's :seealso: pointed at itself (now h2e); document the (1,N) interpretation in both. No behavior change. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transformsNd.py | 9 ++++-- tests/base/test_transformsNd.py | 50 ++++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+), 2 deletions(-) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..90d60985 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -726,8 +726,10 @@ 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 treated as a matrix of column vectors, so a + row vector of shape (1,N) is interpreted as N homogeneous 1-vectors. - :seealso: e2h + :seealso: :func:`e2h` """ if isinstance(v, np.ndarray) and len(v.shape) == 2: # dealing with matrix @@ -765,8 +767,11 @@ 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 treated as a matrix of column vectors, so a + row vector of shape (1,N) is interpreted as N 1-vectors and the result + has shape (2,N). - :seealso: e2h + :seealso: :func:`h2e` """ if isinstance(v, np.ndarray) and len(v.shape) == 2: # dealing with matrix diff --git a/tests/base/test_transformsNd.py b/tests/base/test_transformsNd.py index 14c7fd42..47714541 100755 --- a/tests/base/test_transformsNd.py +++ b/tests/base/test_transformsNd.py @@ -261,6 +261,56 @@ 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_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]) From e7682ea41d6c080486de46a985aa739def343e78 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sat, 10 Oct 2026 16:20:44 +1000 Subject: [PATCH 26/33] fix: h2e rejects input with fewer than 2 rows; state e2h/h2e contract h2e of a (1,N) array, a length-1 vector or a scalar returned an empty (0,N) array, which is never useful (a homogeneous point has at least two elements). Raise ValueError instead. Docstrings for e2h and h2e now state the contract up front: columns are points, rows never are, with explicit shapes, the (1,N) consequence and :raises: entries. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transformsNd.py | 43 +++++++++++++++++++++----------- tests/base/test_transformsNd.py | 8 ++++++ 2 files changed, 36 insertions(+), 15 deletions(-) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 90d60985..d1af76d9 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -709,12 +709,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,18 +729,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 treated as a matrix of column vectors, so a - row vector of shape (1,N) is interpreted as N homogeneous 1-vectors. + .. 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: :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: @@ -751,12 +760,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 @@ -767,9 +779,10 @@ 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 treated as a matrix of column vectors, so a - row vector of shape (1,N) is interpreted as N 1-vectors and the result - has shape (2,N). + .. 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: :func:`h2e` """ diff --git a/tests/base/test_transformsNd.py b/tests/base/test_transformsNd.py index 47714541..7291eaa2 100755 --- a/tests/base/test_transformsNd.py +++ b/tests/base/test_transformsNd.py @@ -304,6 +304,14 @@ def test_homog_row_vector_is_matrix(self): 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,))): From f8298724fe15f100b822d0ddee67039d5f1dad81 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 01:50:22 -0700 Subject: [PATCH 27/33] Strengthen animation start-pose test against static trplot geometry Refactor test_animate_nonidentity_start to improve axis geometry validation and streamline assertions. --- tests/base/test_transforms3d_plot.py | 51 ++++++++++++++++++---------- 1 file changed, 34 insertions(+), 17 deletions(-) diff --git a/tests/base/test_transforms3d_plot.py b/tests/base/test_transforms3d_plot.py index 002598d1..cec724af 100755 --- a/tests/base/test_transforms3d_plot.py +++ b/tests/base/test_transforms3d_plot.py @@ -27,6 +27,30 @@ 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"): @@ -34,23 +58,16 @@ def test_animate_nonidentity_start(self): try: anim = Animate(dim=[-4, 4]) anim.trplot(end, start=start, style=style, frame="moving") - for pose in (start, end): - anim._draw(pose) - for obj in anim.displaylist: - expected = pose @ obj.p if hasattr(obj, "p") else None - if obj.type == "line": - nt.assert_allclose( - np.asarray(obj.h.get_data_3d()), - expected[:3], - atol=1e-14, - ) - nt.assert_allclose(obj.p[:3, 0], np.zeros(3), atol=1e-14) - elif obj.type == "arrow": - nt.assert_allclose( - np.asarray(obj.h._segments3d), - expected[:3].T.reshape(3, 2, 3), - atol=1e-14, - ) + 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) From 73222bf69553c3ffda085ce6832b12841f9d6a6d Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:49:52 -0700 Subject: [PATCH 28/33] Accept a stacked (N, r, c) array as a sequence of poses Added support for stacked numpy arrays in baseposelist.py. --- spatialmath/baseposelist.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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] From 1057abf2564d5dbad59ddd8723c12fd6815bea11 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:50:25 -0700 Subject: [PATCH 29/33] SE3: accept a stacked (N, 4, 4) array and say what is accepted in the error Added support for SE3 constructor with (N, 4, 4) NumPy array. --- spatialmath/pose3d.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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) From 3f06d5e5dc465ee936c8c7e8cb577fa502c2721a Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:50:48 -0700 Subject: [PATCH 30/33] Test SE3 and SO3 construction from a stacked array Add unit test for SE3 constructor with stacked array input. --- tests/test_pose3d.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) 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() From 08cb279003c1c9ddcb49fc6c5898862a93e5b04e Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 17:04:45 -0700 Subject: [PATCH 31/33] Defer scipy imports in spline.py --- spatialmath/spline.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 From a105dbb7794f236df2e38b020dbf8db46875258e Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 17:05:11 -0700 Subject: [PATCH 32/33] Defer scipy.interpolate import in quaternions --- spatialmath/base/quaternions.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 92111793..3de527f9 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 @@ -897,7 +899,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 +912,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 From e5802a51faae159369479c2bb1ddbd8ade79f59b Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 17:05:48 -0700 Subject: [PATCH 33/33] Add test that importing spatialmath does not load scipy Add a test to ensure scipy is not loaded when importing spatialmath. --- tests/test_import.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 tests/test_import.py 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()