From 73222bf69553c3ffda085ce6832b12841f9d6a6d Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 15:49:52 -0700 Subject: [PATCH 01/31] 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 02/31] 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 03/31] 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 dfe0bb7df80ac2770704e5e16dca70baf1eeb501 Mon Sep 17 00:00:00 2001 From: vera-rubin <322284620+vera-rubin@users.noreply.github.com> Date: Wed, 14 Oct 2026 20:08:08 +0000 Subject: [PATCH 04/31] fix: preserve rotation quadrants in trlog2 --- spatialmath/base/transforms2d.py | 4 ++-- tests/base/test_transforms2d.py | 21 ++++++++++++++++++++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 6aad235c..664a9d6c 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -549,7 +549,7 @@ def trlog2( else: st = T[1, 0] ct = T[0, 0] - theta = math.atan(st / ct) + theta = math.atan2(st, ct) if abs(theta) < tol * _eps: tr = T[:2, 2].flatten() else: @@ -564,7 +564,7 @@ def trlog2( elif isrot2(T, check=check, tol=tol): # SO(2) rotation matrix - theta = math.atan(T[1, 0] / T[0, 0]) + theta = math.atan2(T[1, 0], T[0, 0]) if twist: return theta else: diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index bf578b22..fcd939bd 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -12,7 +12,7 @@ import unittest from math import pi import math -from scipy.linalg import logm +from scipy.linalg import expm, logm import pytest import sys @@ -88,6 +88,25 @@ def test_trlog2(self): nt.assert_array_almost_equal(trlog2(T, twist=True), vexa(logm(T))) + def test_trlog2_quadrants(self): + for c, s in ( + (1, 0), + (math.sqrt(3) / 2, 0.5), + (-0.5, math.sqrt(3) / 2), + (-0.5, -math.sqrt(3) / 2), + (-1, 0), + (0, 1), + (0, -1), + ): + R = np.array([[c, -s], [s, c]]) + for T in (R, rt2tr(R, [1, 2])): + for twist in (False, True): + with self.subTest(c=c, s=s, shape=T.shape, twist=twist): + S = trlog2(T, twist=twist) + if twist: + S = skew(S) if T.shape == (2, 2) else skewa(S) + nt.assert_allclose(expm(S), T, atol=1e-12, rtol=0) + def test_trexp2(self): R = trexp2(skew(0.5)) nt.assert_array_almost_equal(R, rot2(0.5)) From 5fb7852b7a87381e71f4f97648a364595c3c21fa Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:07:21 -0700 Subject: [PATCH 05/31] Defer optional SymPy loading until symbolic use --- spatialmath/base/symbolic.py | 139 ++++++++++++++++++----------------- 1 file changed, 73 insertions(+), 66 deletions(-) diff --git a/spatialmath/base/symbolic.py b/spatialmath/base/symbolic.py index aebb827a..2031b3c1 100644 --- a/spatialmath/base/symbolic.py +++ b/spatialmath/base/symbolic.py @@ -11,61 +11,67 @@ supported. """ +from __future__ import annotations + import math +from typing import TYPE_CHECKING from spatialmath.base.types import * -try: # pragma: no cover - # print('Using SymPy') - import sympy +import importlib as _importlib +import sys as _sys - _symbolics = True - symtype = (sympy.Expr,) +if TYPE_CHECKING: from sympy import Symbol -except ImportError: # pragma: no cover - # SymPy is not installed - _symbolics = False - symtype = () - Symbol = Any +def _sympy(): + """Load the optional dependency only for a symbolic operation.""" + return _importlib.import_module("sympy") -# ---------------------------------------------------------------------------------------# -if _symbolics: +def __getattr__(name): + """Keep explicit access to the SymPy type aliases lazy.""" + if name == "Symbol": + return _sympy().Symbol + if name == "symtype": + try: + return (_sympy().Expr,) + except ImportError: + return () + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") - def symbol( - name: str, real: Optional[bool] = True - ) -> Union[Symbol, Tuple[Symbol, ...]]: - """ - Create symbolic variables - :param name: symbol names - :type name: str - :param real: assume variable is real, defaults to True - :type real: bool, optional - :return: SymPy symbols - :rtype: sympy +def symbol(name: str, real: Optional[bool] = True) -> Union[Symbol, Tuple[Symbol, ...]]: + """ + Create symbolic variables - .. runblock:: pycon + :param name: symbol names + :type name: str + :param real: assume variable is real, defaults to True + :type real: bool, optional + :return: SymPy symbols + :rtype: sympy - >>> from spatialmath.base.symbolic import * - >>> theta = symbol('theta') - >>> theta - >>> theta, psi = symbol('theta psi') - >>> theta - >>> psi - >>> q = symbol('q_:6') - >>> q + .. runblock:: pycon + + >>> from spatialmath.base.symbolic import * + >>> theta = symbol('theta') + >>> theta + >>> theta, psi = symbol('theta psi') + >>> theta + >>> psi + >>> q = symbol('q_:6') + >>> q - .. note:: In Jupyter symbols are pretty printed. + .. note:: In Jupyter symbols are pretty printed. - - symbols named after greek letters will appear as greek letters - - underscore means subscript as it does in LaTex, so the symbols ``q`` - above will be subscripted. + - symbols named after greek letters will appear as greek letters + - underscore means subscript as it does in LaTex, so the symbols ``q`` + above will be subscripted. - :seealso: :func:`sympy.symbols` - """ - return sympy.symbols(name, real=real) + :seealso: :func:`sympy.symbols` + """ + return _sympy().symbols(name, real=real) def issymbol(var: Any) -> bool: @@ -84,13 +90,12 @@ def issymbol(var: Any) -> bool: >>> issymbol(3.4) """ - if _symbolics: - if isinstance(var, (list, tuple)): - return any([isinstance(x, symtype) for x in var]) - else: - return isinstance(var, symtype) - else: + sympy = _sys.modules.get("sympy") + if sympy is None: return False + if isinstance(var, (list, tuple)): + return any(isinstance(x, sympy.Expr) for x in var) + return isinstance(var, sympy.Expr) @overload @@ -122,7 +127,7 @@ def sin(theta): :seealso: :func:`sympy.sin` """ if issymbol(theta): - return sympy.sin(theta) + return _sympy().sin(theta) else: return math.sin(theta) @@ -156,7 +161,7 @@ def cos(theta): :seealso: :func:`sympy.cos` """ if issymbol(theta): - return sympy.cos(theta) + return _sympy().cos(theta) else: return math.cos(theta) @@ -190,7 +195,7 @@ def tan(theta): :seealso: :func:`sympy.cos` """ if issymbol(theta): - return sympy.tan(theta) + return _sympy().tan(theta) else: return math.tan(theta) @@ -224,7 +229,7 @@ def sqrt(v): :seealso: :func:`sympy.sqrt` """ if issymbol(v): - return sympy.sqrt(v) + return _sympy().sqrt(v) else: return math.sqrt(v) @@ -245,7 +250,7 @@ def zero() -> Symbol: :seealso: :func:`sympy.S.Zero` """ - return sympy.S.Zero + return _sympy().S.Zero def one() -> Symbol: @@ -264,7 +269,7 @@ def one() -> Symbol: :seealso: :func:`sympy.S.One` """ - return sympy.S.One + return _sympy().S.One def negative_one() -> Symbol: @@ -283,26 +288,27 @@ def negative_one() -> Symbol: :seealso: :func:`sympy.S.NegativeOne` """ - return sympy.S.NegativeOne + return _sympy().S.NegativeOne def pi() -> Symbol: """ - Symbolic constant: pi + Symbolic constant: pi - :return: π - :rtype: symbolic + :return: π + :rtype: symbolic - .. runblock:: pycon + .. runblock:: pycon - >>> from spatialmath.base.symbolic import * - >>> import math - >>> sin(pi()) - >>> sin(math.pi) + >>> from spatialmath.base.symbolic import * + >>> import math + from typing import TYPE_CHECKING + >>> sin(pi()) + >>> sin(math.pi) - :seealso: :func:`sympy.S.Pi` + :seealso: :func:`sympy.S.Pi` """ - return sympy.S.Pi + return _sympy().S.Pi def simplify(x: Symbol) -> Symbol: @@ -324,10 +330,11 @@ def simplify(x: Symbol) -> Symbol: :seealso: :func:`sympy.simplify` """ - if _symbolics: - return sympy.simplify(x) - else: + try: + sympy = _sympy() + except ImportError: return x + return sympy.simplify(x) def det(x): @@ -352,4 +359,4 @@ def det(x): .. note:: Converts to a SymPy ``Matrix`` and then back again. """ - return sympy.Matrix(x).det() + return _sympy().Matrix(x).det() From 2690a3792568496103841c1e473e5cf338a00a8e Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:07:49 -0700 Subject: [PATCH 06/31] Check symbolic scalar types dynamically --- spatialmath/base/argcheck.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/spatialmath/base/argcheck.py b/spatialmath/base/argcheck.py index 0db73497..8feb24d5 100644 --- a/spatialmath/base/argcheck.py +++ b/spatialmath/base/argcheck.py @@ -16,10 +16,10 @@ from collections.abc import Iterable # from spatialmath.base import symbolic as sym # HACK -from spatialmath.base.symbolic import issymbol, symtype +from spatialmath.base.symbolic import issymbol # valid scalar types -_scalartypes = (int, np.integer, float, np.floating) + symtype +_scalartypes = (int, np.integer, float, np.floating) # from typing import Union, List, Tuple, Any, Optional, Type, Callable # from numpy.typing import DTypeLike @@ -47,7 +47,9 @@ def isscalar(x: Any) -> bool: >>> isscalar([1]) """ - return isinstance(x, _scalartypes) + return isinstance(x, _scalartypes) or ( + not isinstance(x, (list, tuple)) and issymbol(x) + ) def isinteger(x: Any) -> bool: @@ -386,7 +388,7 @@ def getvector( """ dt = dtype - if isinstance(v, _scalartypes): # handle scalar case + if isscalar(v): # handle scalar case v = [v] # type: ignore if isinstance(v, (list, tuple)): # list or tuple was passed in @@ -501,7 +503,7 @@ def isvector(v: Any, dim: Optional[int] = None) -> bool: if ( isinstance(v, (list, tuple)) and (dim is None or len(v) == dim) - and all(map(lambda x: isinstance(x, _scalartypes), v)) + and all(map(isscalar, v)) ): return True # list or tuple @@ -516,7 +518,7 @@ def isvector(v: Any, dim: Optional[int] = None) -> bool: else: return s == (dim,) or s == (1, dim) or s == (dim, 1) - if (dim is None or dim == 1) and isinstance(v, _scalartypes): + if (dim is None or dim == 1) and isscalar(v): return True return False @@ -620,11 +622,7 @@ def isnumberlist(x: Any) -> bool: >>> isnumberlist(np.r_[1,2]) """ - return ( - isinstance(x, (list, tuple)) - and len(x) > 0 - and all(map(lambda x: isinstance(x, _scalartypes), x)) - ) + return isinstance(x, (list, tuple)) and len(x) > 0 and all(map(isscalar, x)) def isvectorlist(x: Any, n: int) -> bool: From 81c4ee46a2bcc800731cff167ca8c60cc275620b Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:08:37 -0700 Subject: [PATCH 07/31] Defer SymPy loading in transforms2d.py --- spatialmath/base/transforms2d.py | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index e9bc0e70..fe48fa6d 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -39,15 +39,6 @@ _eps = np.finfo(np.float64).eps -try: # pragma: no cover - # print('Using SymPy') - import sympy - - _symbolics = True - -except ImportError: # pragma: no cover - _symbolics = False - # ---------------------------------------------------------------------------------------# def rot2(theta: float, unit: str = "rad") -> SO2Array: @@ -170,8 +161,13 @@ def tr2xyt(T: SE2Array, unit: str = "rad") -> R3: :seealso: trot2 """ - if T.dtype == "O" and _symbolics: - angle = sympy.atan2(T[1, 0], T[0, 0]) + if T.dtype == "O": + try: + import sympy + except ImportError: + angle = math.atan2(T[1, 0], T[0, 0]) + else: + angle = sympy.atan2(T[1, 0], T[0, 0]) else: angle = math.atan2(T[1, 0], T[0, 0]) return np.r_[T[0, 2], T[1, 2], angle] @@ -367,7 +363,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 + ) ) ) @@ -1312,7 +1313,11 @@ def _FindCorrespondences( num_iter += 1 - return T + # T accumulates roundoff error through repeated composition + # (T = T @ new_T) over the iterations above, and can drift just + # outside ishom2()'s orthonormality tolerance -- restore exact + # orthonormality before returning. + return trnorm2(T) if _matplotlib_exists: From 82b2dd60d471ab1b824b2146d238ab0c76906180 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:08:48 -0700 Subject: [PATCH 08/31] Defer SymPy loading in transformsNd.py --- spatialmath/base/transformsNd.py | 95 +++++++++++++++++++++++++------- 1 file changed, 76 insertions(+), 19 deletions(-) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..0b23c078 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -22,14 +22,7 @@ # from spatialmath.base.transforms2d import transl2 -try: # pragma: no cover - # print('Using SymPy') - from sympy import Matrix - - _symbolics = True - -except ImportError: # pragma: no cover - _symbolics = False +from spatialmath.base import symbolic as sym _eps = np.finfo(np.float64).eps @@ -378,10 +371,34 @@ def isR(R: NDArray, tol: float = 20) -> bool: # -> TypeGuard[SOnArray]: :seealso: isrot2, isrot """ - return bool( - np.linalg.norm(R @ R.T - np.eye(R.shape[0])) < tol * _eps - and np.linalg.det(R) > 0 - ) + n = R.shape[0] + if n == 3: + # explicit cofactor expansion avoids the dispatch overhead of + # np.linalg.det/norm, which dominates cost for such a small matrix + det = ( + R[0, 0] * (R[1, 1] * R[2, 2] - R[1, 2] * R[2, 1]) + - R[0, 1] * (R[1, 0] * R[2, 2] - R[1, 2] * R[2, 0]) + + R[0, 2] * (R[1, 0] * R[2, 1] - R[1, 1] * R[2, 0]) + ) + if det <= 0: + return False + D = R @ R.T + D[0, 0] -= 1.0 + D[1, 1] -= 1.0 + D[2, 2] -= 1.0 + return bool(np.sum(D * D) < (tol * _eps) ** 2) + elif n == 2: + det = R[0, 0] * R[1, 1] - R[0, 1] * R[1, 0] + if det <= 0: + return False + D = R @ R.T + D[0, 0] -= 1.0 + D[1, 1] -= 1.0 + return bool(np.sum(D * D) < (tol * _eps) ** 2) + else: + return bool( + np.linalg.norm(R @ R.T - np.eye(n)) < tol * _eps and np.linalg.det(R) > 0 + ) def isskew(S: NDArray, tol: float = 20) -> bool: # -> TypeGuard[sonArray]: @@ -408,6 +425,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 +458,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: @@ -843,10 +898,12 @@ def det(m: np.ndarray) -> float: :SymPy: supported """ - if m.dtype.kind == "O" and _symbolics: - return Matrix(m).det() - else: - return np.linalg.det(m) + if m.dtype.kind == "O": + try: + return sym.det(m) + except ImportError: + pass + return np.linalg.det(m) if __name__ == "__main__": # pragma: no cover From 5df29aaa62e236628f543f9b581999760c798c2a Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:08:59 -0700 Subject: [PATCH 09/31] Defer SymPy loading in vectors.py --- spatialmath/base/vectors.py | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index b0055e9a..4f40acad 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -16,14 +16,7 @@ from spatialmath.base.argcheck import getvector from spatialmath.base.types import * -try: # pragma: no cover - # print('Using SymPy') - import sympy - - _symbolics = True - -except ImportError: # pragma: no cover - _symbolics = False +from spatialmath.base import symbolic as sym _eps = np.finfo(np.float64).eps @@ -55,8 +48,8 @@ def norm(v: ArrayLikePure) -> float: for x in v: sum += x * x - if _symbolics and isinstance(sum, sympy.Expr): - return sympy.sqrt(sum) + if sym.issymbol(sum): + return sym.sqrt(sum) else: return math.sqrt(sum) @@ -567,13 +560,10 @@ def wrap_mpi2_pi2(theta: ArrayLike) -> Union[float, NDArray]: """ theta = getvector(theta) - n = theta / np.pi * 2 - if isinstance(n, np.ndarray): - n = n.astype(int) - else: - n = np.fix(n).astype(int) - - y = np.where(np.bitwise_and(n, 1) == 0, theta - n * np.pi, n * np.pi - theta) + theta = np.fmod(theta, 2 * np.pi) + theta = np.where(theta > np.pi, theta - 2 * np.pi, theta) + theta = np.where(theta < -np.pi, theta + 2 * np.pi, theta) + y = np.where(np.abs(theta) <= np.pi / 2, theta, np.copysign(np.pi, theta) - theta) if isinstance(y, np.ndarray) and len(y) == 1: return float(y[0]) else: @@ -832,7 +822,14 @@ def project(v1: ArrayLike3, v2: ArrayLike3) -> ArrayLike3: :return: vector projection of v1 onto v2 (parrallel to v2) :rtype: ndarray(n) """ - return np.dot(v1, v2) * v2 + v1 = getvector(v1) + v2 = getvector(v2) + scale = np.max(np.abs(v2)) + if scale == 0: + return np.zeros_like(v2) + # Normalize the scale before dot products to avoid over/underflow. + v2 = v2 / scale + return (np.dot(v1, v2) / np.dot(v2, v2)) * v2 def orthogonalize(v1: ArrayLike3, v2: ArrayLike3, normalize: bool = True) -> ArrayLike3: From 514a615f8024ee425e993567f9640f8881c2b692 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:09:19 -0700 Subject: [PATCH 10/31] Test optional SymPy loading in fresh interpreters --- tests/test_lazy_sympy.py | 111 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 tests/test_lazy_sympy.py diff --git a/tests/test_lazy_sympy.py b/tests/test_lazy_sympy.py new file mode 100644 index 00000000..6b5c4c1b --- /dev/null +++ b/tests/test_lazy_sympy.py @@ -0,0 +1,111 @@ +"""Regression tests for optional SymPy loading in fresh interpreters.""" + +import subprocess +import sys +import textwrap + +import pytest + + +def run_python(source): + """Run an isolated check with the same interpreter as pytest.""" + subprocess.run([sys.executable, "-c", textwrap.dedent(source)], check=True) + + +def test_numeric_import_does_not_load_sympy(): + run_python( + """ + import sys + import numpy as np + import spatialmath + from spatialmath import base as smb + assert not any(n == 'sympy' or n.startswith('sympy.') for n in sys.modules) + assert smb.isscalar(1.0) + assert smb.isvector([1, 2]) + assert smb.isnumberlist([1, 2]) + assert smb.norm([3, 4]) == 5 + assert np.allclose(smb.rot2(0), np.eye(2)) + assert not any(n == 'sympy' or n.startswith('sympy.') for n in sys.modules) + """ + ) + + +@pytest.mark.parametrize("sympy_first", [False, True]) +def test_symbolic_operations_in_both_import_orders(sympy_first): + prefix = "import sympy as sp\n" if sympy_first else "" + run_python( + prefix + + """ +import numpy as np +from spatialmath import base as smb +import sympy as sp +x = sp.Symbol('x', real=True) +assert smb.issymbol(x) +assert smb.issymbol([1, x]) +assert smb.issymbol((x, 1)) +assert not smb.issymbol([1, 2]) +assert smb.isscalar(x) +assert not smb.isscalar([x]) +assert not smb.isscalar((x,)) +assert smb.isvector([x, 1]) +assert smb.isnumberlist([x, 1]) +assert smb.getvector(x)[0] == x +assert smb.getvector([x, 1])[0] == x +assert smb.sin(x) == sp.sin(x) +assert smb.cos(x) == sp.cos(x) +assert smb.tan(x) == sp.tan(x) +assert smb.sqrt(x) == sp.sqrt(x) +assert smb.norm([x, 1]) == sp.sqrt(x*x + 1) +assert smb.det(np.array([[x, 0], [0, 1]], dtype=object)) == x +T = smb.trot2(x) +assert sp.simplify(smb.tr2xyt(T)[2] - sp.atan2(sp.sin(x), sp.cos(x))) == 0 +assert smb.simplify(sp.sin(x)**2 + sp.cos(x)**2) == 1 +""" + ) + + +@pytest.mark.parametrize("helper", ["symbol", "zero", "one", "negative_one", "pi"]) +def test_explicit_symbolic_helpers_load_sympy(helper): + run_python( + textwrap.dedent(f"helper = {helper!r}\n") + + textwrap.dedent( + """ + import sys + from spatialmath.base import symbolic as sym + assert 'sympy' not in sys.modules + getattr(sym, helper)(*(['x'] if helper == 'symbol' else [])) + assert 'sympy' in sys.modules + x = sym.symbol('x') + import sympy as sp + assert isinstance(x, sp.Symbol) + from spatialmath.base.symbolic import Symbol, symtype + assert Symbol is sp.Symbol + assert symtype == (sp.Expr,) + assert sym.zero() == sp.S.Zero + assert sym.one() == sp.S.One + assert sym.negative_one() == sp.S.NegativeOne + assert sym.pi() == sp.pi + """ + ) + ) + + +def test_numeric_operations_without_sympy(): + run_python( + """ + import importlib.abc + import sys + class BlockSympy(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if fullname == 'sympy' or fullname.startswith('sympy.'): + raise ModuleNotFoundError('SymPy blocked for this test') + sys.meta_path.insert(0, BlockSympy()) + from spatialmath import base as smb + assert not smb.issymbol(1) + assert smb.norm([3, 4]) == 5 + assert smb.sin(0) == 0 + assert smb.simplify(2) == 2 + assert smb.isscalar(1) + assert smb.isvector([1, 2]) + """ + ) From 1fb1b5db4616e6d67902aec029818e5b7d71cf07 Mon Sep 17 00:00:00 2001 From: Tim Perkins Date: Fri, 16 Oct 2026 09:55:55 -0400 Subject: [PATCH 11/31] Using check=False seems to return empty types --- tests/test_quaternion.py | 3 +++ tests/test_twist.py | 24 ++++++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 8a87a433..73101d28 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -892,6 +892,9 @@ def test_constructor(self): # tc.verifyError( @() Quaternion(2), 'SMTB:Quaternion:badarg') # tc.verifyError( @() Quaternion([1, 2, 3]), 'SMTB:Quaternion:badarg') + # an array that is not a 4-vector must not give an empty Quaternion + self.assertNotEqual(len(Quaternion(np.random.rand(3, 4))), 0) + def test_string(self): u = Quaternion() diff --git a/tests/test_twist.py b/tests/test_twist.py index dfbd9f24..5ef53cc7 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -67,6 +67,12 @@ def test_conversion_SE3(self): T = SE3.Rx(0) * SE3(1, 2, 3) array_compare(Twist3(T).SE3(), T) + # from an SE(3) matrix, with and without checking + for check in (True, False): + tw = Twist3(T.A, check=check) + self.assertEqual(len(tw), 1) + array_compare(tw.SE3(), T) + def test_conversion_se3(self): s = [1, 2, 3, 4, 5, 6] x = Twist3(s) @@ -83,6 +89,12 @@ def test_conversion_se3(self): ), ) + # from an se(3) matrix, with and without checking + for check in (True, False): + tw = Twist3(x.skewa(), check=check) + self.assertEqual(len(tw), 1) + array_compare(tw, x) + def test_conversion_Plucker(self): pass @@ -271,6 +283,12 @@ def test_conversion_SE2(self): self.assertIsInstance(tw.SE2(), SE2) self.assertEqual(len(tw.SE2()), 1) + # from an SE(2) matrix, with and without checking + for check in (True, False): + tw = Twist2(T.A, check=check) + self.assertEqual(len(tw), 1) + array_compare(tw.SE2(), T) + def test_conversion_se2(self): s = [1, 2, 3] x = Twist2(s) @@ -279,6 +297,12 @@ def test_conversion_se2(self): x.skewa(), np.array([[0.0, -3.0, 1.0], [3.0, 0.0, 2.0], [0.0, 0.0, 0.0]]) ) + # from an se(2) matrix, with and without checking + for check in (True, False): + tw = Twist2(x.skewa(), check=check) + self.assertEqual(len(tw), 1) + array_compare(tw, x) + def test_list_constuctor(self): x = Twist2([1, 0, 0]) From fb39007af94c9772d00f91953f67cbad652db893 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sat, 17 Oct 2026 10:29:43 +1000 Subject: [PATCH 12/31] fix(graphics): axes_logic uses an empty current figure instead of creating a second one axes_logic reuses the current axes when the current figure has axes, but when the current figure had none, as after a bare plt.figure(), it called plt.figure() again. Every "new plot" idiom (plt.figure() followed by a plot_xxx call) therefore left an empty figure behind, most visible with the ipympl backend where figures stay open in notebooks. Put the axes in the empty current figure instead. new=True still always makes a new figure, a dimension mismatch still makes a new figure, and a current figure that has axes is reused as before. Add direct tests of axes_logic (there were none): the existing behaviour, and the empty-current-figure cases. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/graphics.py | 8 ++- tests/base/test_axes_logic.py | 102 ++++++++++++++++++++++++++++++++++ 2 files changed, 109 insertions(+), 1 deletion(-) create mode 100644 tests/base/test_axes_logic.py diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index 40503c41..15c87fee 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1580,6 +1580,7 @@ def axes_logic( """ # print(f"new axis logic ({dimensions}D): ", end='') + use_current_figure = False if ax is None: # no axes passed in, find out what's happening # need to be careful to not use gcf() or gca() since they @@ -1597,6 +1598,10 @@ def axes_logic( # print(f"ax has {_axes_dimensions(ax)} dimensions") if _axes_dimensions(ax) == dimensions and not new: return ax + elif not new: + # the current figure is empty, eg. from a bare plt.figure(): + # put the axes in it rather than leave it empty and make another + use_current_figure = True # otherwise it doesnt exist or dimension mismatch, create new axes else: # print("ax given", ax) @@ -1607,7 +1612,8 @@ def axes_logic( return ax # print("mismatch in dimensions, create new axes") # print("create new axes") - plt.figure() + if not use_current_figure: + plt.figure() if dimensions == 2: ax = plt.axes() if autoscale: diff --git a/tests/base/test_axes_logic.py b/tests/base/test_axes_logic.py new file mode 100644 index 00000000..a348f47d --- /dev/null +++ b/tests/base/test_axes_logic.py @@ -0,0 +1,102 @@ +"""Tests of ``axes_logic``, which decides which axes a plot_xxx() function draws into.""" + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import pytest + +from spatialmath.base.graphics import axes_logic + + +@pytest.fixture(autouse=True) +def close_figures(): + plt.close("all") + yield + plt.close("all") + + +def nfigs(): + return len(plt.get_fignums()) + + +# --- behaviour that must not change --------------------------------------------- + + +def test_no_figure_creates_one(): + ax = axes_logic(None, 2) + assert nfigs() == 1 + assert ax is plt.gca() + + +def test_current_axes_are_reused(): + fig, ax0 = plt.subplots() + ax = axes_logic(None, 2) + assert ax is ax0 + assert nfigs() == 1 + + +def test_new_forces_a_new_figure(): + fig, ax0 = plt.subplots() + ax = axes_logic(None, 2, new=True) + assert ax is not ax0 + assert nfigs() == 2 + assert ax is plt.gca() + + +def test_dimension_mismatch_makes_a_new_figure(): + fig = plt.figure() + ax3 = fig.add_subplot(projection="3d") + ax = axes_logic(None, 2) + assert ax is not ax3 + assert nfigs() == 2 + + +def test_explicit_axes_of_the_right_dimension_are_used(): + fig, ax0 = plt.subplots() + fig2, ax1 = plt.subplots() + assert axes_logic(ax0, 2) is ax0 + assert nfigs() == 2 + + +def test_explicit_axes_of_the_wrong_dimension_make_a_new_figure(): + fig, ax0 = plt.subplots() + ax = axes_logic(ax0, 3) + assert ax is not ax0 + assert nfigs() == 2 + + +# --- an empty current figure, eg. from a bare plt.figure() ----------------------- + + +def test_empty_current_figure_is_used_2d(): + fig = plt.figure() + ax = axes_logic(None, 2) + assert nfigs() == 1 + assert ax.figure is fig + assert ax is plt.gca() + + +def test_empty_current_figure_is_used_3d(): + fig = plt.figure() + ax = axes_logic(None, 3) + assert nfigs() == 1 + assert ax.figure is fig + assert ax.name == "3d" + + +def test_only_the_current_figure_matters(): + # an earlier figure has axes, but the current one is empty: use the current one + plt.subplots() + fig = plt.figure() + ax = axes_logic(None, 2) + assert nfigs() == 2 + assert ax.figure is fig + + +def test_new_with_an_empty_current_figure_still_makes_another(): + # new=True is an explicit request for a new figure + fig = plt.figure() + ax = axes_logic(None, 2, new=True) + assert nfigs() == 2 + assert ax.figure is not fig From 87ac1ea35a8168c975f9468964a7a7237725322f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Wed, 21 Oct 2026 19:25:45 +0800 Subject: [PATCH 13/31] fix(graphics): preserve unspecified dimensions in expand_dims MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 李永祺 --- spatialmath/base/graphics.py | 26 ++++++++++++++++++-------- tests/base/test_graphics.py | 26 ++++++++++++++++++++++++++ 2 files changed, 44 insertions(+), 8 deletions(-) diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index 40503c41..9989b9c9 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1513,7 +1513,10 @@ def _axes_dimensions(ax: plt.Axes) -> int: # handle the case of Animate objects pretending to be Axes classname = ax.__class__.__name__ base_classes = ax.__class__.__bases__ - if classname in ("Axes3DSubplot", "Animate") or any(base_class.__name__ in ("Axes3DSubplot", "Animate") for base_class in base_classes): + if classname in ("Axes3DSubplot", "Animate") or any( + base_class.__name__ in ("Axes3DSubplot", "Animate") + for base_class in base_classes + ): return 3 elif classname in ("AxesSubplot", "Animate2"): return 2 @@ -1736,20 +1739,24 @@ def plotvol3( ax._plotvol = True return ax - def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: + def expand_dims(dim: ArrayLike | None = None, nd: int = 2) -> NDArray | None: """ Expand compact axis dimensions - :param dim: dimensions, defaults to None + :param dim: dimensions, or None if unspecified, defaults to None :type dim: scalar, array_like(2), array_like(4), array_like(6), optional - :param nd: number of axes dimensions, defaults to 2 + :param nd: number of axes dimensions (2 or 3), defaults to 2 :type nd: int, optional :raises ValueError: bad arguments - :return: 2d or 3d dimensions vector - :rtype: ndarray(4) or ndarray(6) + :return: 2d or 3d dimensions vector, or None if unspecified + :rtype: ndarray(4), ndarray(6), or None Compute bounding dimensions for plots from shorthand notation. + ``None`` is returned unchanged so the caller can choose whether to + autoscale or obtain dimensions from another source. No default extent + is inferred. + If ``nd==2``, [xmin, xmax, ymin, ymax]: * A -> [-A, A, -A, A] * [A,B] -> [A, B, A, B] @@ -1760,6 +1767,11 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: * [A,B] -> [A, B, A, B, A, B] * [A,B,C,D,E,F] -> [A, B, C, D, E, F] """ + if nd not in (2, 3): + raise ValueError("nd is 2 or 3") + if dim is None: + return None + dim = smb.getvector(dim) if nd == 2: @@ -1780,8 +1792,6 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: return dim else: raise ValueError("bad dimension specified") - else: - raise ValueError("nd is 2 or 3") def isnotebook() -> bool: """ diff --git a/tests/base/test_graphics.py b/tests/base/test_graphics.py index 552ebdb0..aa0438a4 100644 --- a/tests/base/test_graphics.py +++ b/tests/base/test_graphics.py @@ -13,6 +13,32 @@ class TestGraphics(unittest.TestCase): def teardown_method(self, method): plt.close("all") + def test_expand_dims_unspecified(self): + self.assertIsNone(expand_dims()) + for nd in (2, 3): + with self.subTest(nd=nd): + self.assertIsNone(expand_dims(nd=nd)) + self.assertIsNone(expand_dims(None, nd=nd)) + + def test_expand_dims_specified(self): + for nd in (2, 3): + with self.subTest(nd=nd): + # Zero is an explicit extent, not an unspecified one. + np.testing.assert_array_equal(expand_dims(0, nd), [0, 0] * nd) + np.testing.assert_array_equal(expand_dims(3, nd), [-3, 3] * nd) + np.testing.assert_array_equal(expand_dims([-2, 5], nd), [-2, 5] * nd) + limits = np.arange(2 * nd) + np.testing.assert_array_equal(expand_dims(limits, nd), limits) + with self.assertRaises(ValueError): + expand_dims([1, 2, 3], nd) + + def test_expand_dims_invalid_nd(self): + for nd in (1, 4): + for dim in (None, 1): + with self.subTest(nd=nd, dim=dim): + with self.assertRaisesRegex(ValueError, "nd is 2 or 3"): + expand_dims(dim, nd) + @pytest.mark.skipif( sys.platform.startswith("darwin") and sys.version_info < (3, 11), reason="tkinter bug with mac", From 5c4a3d1caa06f70c5f88932856ceefd7d2a91655 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=B0=B8=E7=A5=BA?= Date: Thu, 22 Oct 2026 19:31:37 +0800 Subject: [PATCH 14/31] fix(geom3d): return a point from intersecting lines MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 李永祺 --- spatialmath/geom3d.py | 11 ++++------- tests/test_geom3d.py | 37 +++++++++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 7 deletions(-) diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index 84813a41..60402ec2 100755 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -830,15 +830,12 @@ def intersects( ``l1.intersects(l2)`` is the point of intersection of the two lines, or ``None`` if the lines do not intersect or are equivalent. - :seealso: :meth:`commonperp :meth:`eq` :meth:`__xor__` + :seealso: :meth:`commonperp` :meth:`isintersecting` :meth:`closest_to_line` """ if l1 ^ l2: - # lines do intersect - return -( - np.dot(l1.v, l2.w) * np.eye(3, 3) - + l1.w.reshape((3, 1)) @ l2.v.reshape((1, 3)) - - l2.w.reshape((3, 1)) @ l1.v.reshape((1, 3)) - ) * base.unitvec(np.cross(l1.w, l2.w)) + # For intersecting lines, the nearest point is their intersection. + point, _ = l1.closest_to_line(l2) + return point else: # lines don't intersect return None diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index 1fca4402..51e36971 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -47,6 +47,43 @@ def test_line_distance_parallel_tolerance(): assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) +@pytest.mark.parametrize( + "point, direction1, direction2", + [ + ([0, 0, 0], [1, 0, 0], [0, 1, 0]), + ([1, 2, 3], [1, 0, 0], [1, 1, 0]), + ([-3, 1, 2], [1, 2, 3], [-2, 1, 1]), + ], + ids=["origin", "oblique", "spatial"], +) +@pytest.mark.parametrize( + "scale1, scale2", [(1, 1), (2, 5), (-2, 0.25), (1e-8, 1e-8), (1e-8, 1e8)] +) +def test_line_intersection_point(point, direction1, direction2, scale1, scale2): + point = np.array(point) + direction1, direction2 = np.array(direction1), np.array(direction2) + line1 = Line3.PointDir(point + 2 * direction1, scale1 * direction1) + line2 = Line3.PointDir(point - 3 * direction2, scale2 * direction2) + + for first, second in [(line1, line2), (line2, line1)]: + intersection = first.intersects(second) + assert intersection is not None + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, rtol=0, atol=1e-12) + + +@pytest.mark.parametrize( + "point, direction", + [([0, 1, 0], [1, 0, 0]), ([2, 0, 0], [-2, 0, 0]), ([0, 0, 1], [0, 1, 0])], + ids=["parallel", "coincident", "skew"], +) +def test_line_intersection_none(point, direction): + line1 = Line3.PointDir([0, 0, 0], [1, 0, 0]) + line2 = Line3.PointDir(point, direction) + assert line1.intersects(line2) is None + assert line2.intersects(line1) is None + + class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): From 9c9dd3032b2b50b62e737d70194a20139458a72a Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Mon, 26 Oct 2026 04:55:28 -0700 Subject: [PATCH 15/31] Fix pi() docstring indentation and stray import in runblock The docstring was indented by an extra four spaces and a stray 'from typing import TYPE_CHECKING' line ended the runblock early, which would render incorrectly in the docs. Restore the docstring formatting to match the rest of the module; the lazy SymPy import is unchanged. --- spatialmath/base/symbolic.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/spatialmath/base/symbolic.py b/spatialmath/base/symbolic.py index 2031b3c1..bdddf747 100644 --- a/spatialmath/base/symbolic.py +++ b/spatialmath/base/symbolic.py @@ -293,20 +293,19 @@ def negative_one() -> Symbol: def pi() -> Symbol: """ - Symbolic constant: pi + Symbolic constant: pi - :return: π - :rtype: symbolic + :return: π + :rtype: symbolic - .. runblock:: pycon + .. runblock:: pycon - >>> from spatialmath.base.symbolic import * - >>> import math - from typing import TYPE_CHECKING - >>> sin(pi()) - >>> sin(math.pi) + >>> from spatialmath.base.symbolic import * + >>> import math + >>> sin(pi()) + >>> sin(math.pi) - :seealso: :func:`sympy.S.Pi` + :seealso: :func:`sympy.S.Pi` """ return _sympy().S.Pi From 67ebc01ac13335edb9cfd178c8fbbcc8e17f0a49 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Mon, 26 Oct 2026 06:12:50 -0700 Subject: [PATCH 16/31] Keep check=False lenient; reject only wrong-size stacked arrays The shape guard added with the stacked-array support rejected every array whose shape differed from the class's internal value shape when check=False. That broke accepted equivalent forms: Twist3 from a 4x4 SE(3)/se(3) matrix, Twist2 from a 3x3 SE(2)/se(2) matrix, and Quaternion from a non-4-vector array, all of which the class _import methods deliberately accept unchecked. Only reject when a matrix pose class (SE3, SO3, SE2, SO2) gets a 3D array whose per-matrix size is wrong: such an array can only be read as a stack, so the mismatch is never a valid pose, whatever check is. Single 2D arrays keep their historical check=False behavior. --- spatialmath/baseposelist.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/spatialmath/baseposelist.py b/spatialmath/baseposelist.py index bdf3ac64..1fdad6f3 100644 --- a/spatialmath/baseposelist.py +++ b/spatialmath/baseposelist.py @@ -190,7 +190,12 @@ def arghandler( # it's a numpy array shape = tuple(self.shape) - if len(shape) == 2 and arg.ndim == 3 and arg.shape[1:] == shape: + if len(shape) == 2 and arg.ndim == 3: + # a 3D array passed to a matrix pose class is a stack + if arg.shape[1:] != shape: + # a stack of matrices of the wrong size is rejected, + # whatever the value of check + return False # a stacked (N, r, c) array is a sequence of N values values = [self._import(x, check=check) for x in arg] if any(x is None for x in values): @@ -198,10 +203,6 @@ def arghandler( 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 fa014b40354a89775dcce8781229e10ebb67b2a0 Mon Sep 17 00:00:00 2001 From: ZHANG YU <182837830+xixihaha-ha@users.noreply.github.com> Date: Tue, 3 Nov 2026 10:22:01 +0800 Subject: [PATCH 17/31] Solve the plane equations for a three-plane intersection --- spatialmath/geom3d.py | 2 +- tests/test_geom3d.py | 28 ++++++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 1 deletion(-) mode change 100755 => 100644 spatialmath/geom3d.py mode change 100755 => 100644 tests/test_geom3d.py diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py old mode 100755 new mode 100644 index 84813a41..5808055c --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -145,7 +145,7 @@ def intersection(pi1: Plane3, pi2: Plane3, pi3: Plane3) -> R3: """ A = np.vstack([pi1.n, pi2.n, pi3.n]) b = np.array([pi1.d, pi2.d, pi3.d]) - return np.linalg.det(A) @ b + return np.linalg.solve(A, -b) @property def n(self) -> R3: diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py old mode 100755 new mode 100644 index 1fca4402..0cf87437 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -47,6 +47,34 @@ def test_line_distance_parallel_tolerance(): assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) +@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) +@pytest.mark.parametrize( + "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] +) +@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) +def test_three_plane_intersection(point, normals, scales): + normals = normals * np.array(scales)[:, None] + planes = [Plane3.PointNormal(point, normal) for normal in normals] + intersection = Plane3.intersection(*planes) + + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, atol=1e-12) + # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. + nt.assert_allclose( + normals @ intersection + np.array([plane.d for plane in planes]), + 0, + atol=1e-12, + ) + nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) + + +@pytest.mark.parametrize("offset", [0, 1]) +def test_three_plane_intersection_singular(offset): + planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] + with pytest.raises(np.linalg.LinAlgError): + Plane3.intersection(*planes) + + class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): From fa95c2753d5ea2d55c8c1c77b1b06f9fbc10ae1e Mon Sep 17 00:00:00 2001 From: ZHANG YU <182837830+xixihaha-ha@users.noreply.github.com> Date: Tue, 3 Nov 2026 11:08:55 +0800 Subject: [PATCH 18/31] Fix the constant coefficient sign in Plane3.contains --- spatialmath/geom3d.py | 2 +- tests/test_plane_contains.py | 39 ++++++++++++++++++++++++++++++++++++ 2 files changed, 40 insertions(+), 1 deletion(-) mode change 100755 => 100644 spatialmath/geom3d.py create mode 100644 tests/test_plane_contains.py diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py old mode 100755 new mode 100644 index 84813a41..1c3390c2 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -190,7 +190,7 @@ def contains(self, p: ArrayLike3, tol: float = 20) -> bool: :return: if the point is in the plane :rtype: bool """ - return abs(np.dot(self.n, p) - self.d) < tol * _eps + return abs(np.dot(self.n, p) + self.d) < tol * _eps def plot( self, diff --git a/tests/test_plane_contains.py b/tests/test_plane_contains.py new file mode 100644 index 00000000..df2a6604 --- /dev/null +++ b/tests/test_plane_contains.py @@ -0,0 +1,39 @@ +import numpy as np +import pytest + +from spatialmath import Plane3 + + +@pytest.mark.parametrize("scale", [1, -1, 2.5]) +@pytest.mark.parametrize("factory", ["coefficients", "point-normal", "three-points"]) +def test_translated_plane_contains_points(factory, scale): + point = np.array([2.0, 3.0, 4.0]) + tangent1 = np.array([1.0, -1.0, 0.0]) + tangent2 = np.array([0.0, 1.0, -1.0]) + normal = np.cross(tangent1, tangent2) + if factory == "coefficients": + plane = Plane3(scale * np.r_[normal, -normal @ point]) + elif factory == "point-normal": + plane = Plane3.PointNormal(point, scale * normal) + else: + plane = Plane3.ThreePoints( + np.column_stack((point, point + scale * tangent1, point + tangent2)) + ) + + for candidate in (point, point + tangent1, point + tangent2): + assert plane.contains(candidate) + assert not plane.contains(point + normal) + # The old sign incorrectly accepted the reflected plane. + assert not plane.contains(-point) + + +def test_plane_contains_tolerance(): + plane = Plane3([1, 0, 0, -2]) + assert plane.contains([2 + 1e-10, 0, 0], tol=1e6) + assert not plane.contains([2 + 1e-10, 0, 0]) + + +def test_plane_through_origin(): + plane = Plane3([0, 0, 1, 0]) + assert plane.contains([2, 3, 0]) + assert not plane.contains([2, 3, 1]) From 7be61bda794a66fb385c3428486c70971ee57ea1 Mon Sep 17 00:00:00 2001 From: ZHANG YU <182837830+xixihaha-ha@users.noreply.github.com> Date: Tue, 3 Nov 2026 11:38:34 +0800 Subject: [PATCH 19/31] Include line moment when constructing a plane from a line and point --- spatialmath/geom3d.py | 2 +- tests/test_plane_line_point.py | 26 ++++++++++++++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) mode change 100755 => 100644 spatialmath/geom3d.py create mode 100644 tests/test_plane_line_point.py diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py old mode 100755 new mode 100644 index 84813a41..4b6c9bc4 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -95,7 +95,7 @@ def LinePoint(cls, l: Line3, p: ArrayLike3) -> Self: :seealso: :meth:`PointNormal` :meth:`ThreePoints` """ - n = np.cross(l.w, p) + n = np.cross(l.w, p) - l.v d = np.dot(l.v, p) return cls(np.r_[n, d]) diff --git a/tests/test_plane_line_point.py b/tests/test_plane_line_point.py new file mode 100644 index 00000000..ef880491 --- /dev/null +++ b/tests/test_plane_line_point.py @@ -0,0 +1,26 @@ +import numpy as np +import pytest + +from spatialmath import Line3, Plane3 + + +@pytest.mark.parametrize( + "anchor,direction,point", + [ + ([1, 2, 3], [1, 0, 0], [1, 4, 3]), + ([2, -1, 4], [1, 2, -1], [3, 2, 5]), + ([0, 0, 0], [0, 0, 1], [2, 3, 4]), + ], +) +@pytest.mark.parametrize("scale", [1, -2, 0.25]) +def test_line_point_plane_contains_translated_line(anchor, direction, point, scale): + anchor = np.asarray(anchor, dtype=float) + direction = np.asarray(direction, dtype=float) * scale + line = Line3.PointDir(anchor, direction) + plane = Plane3.LinePoint(line, np.asarray(point, dtype=float)) + + # Use the plane equation directly, independent of Plane3.contains. + for p in [point, anchor, anchor + direction, anchor - 3 * direction]: + np.testing.assert_allclose(plane.n @ p + plane.d, 0, atol=1e-12) + expected_normal = np.cross(direction, np.asarray(point) - anchor) + np.testing.assert_allclose(plane.n, expected_normal, atol=1e-12) From babe57a7a8129b58bfdf3017f98a5ecbf126c9aa Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 20 Aug 2026 20:56:08 +1000 Subject: [PATCH 20/31] deprecate: UnitQuaternion.interp1's shortest parameter is a no-op Every UnitQuaternion has a non-negative scalar part by construction (qunit()), and interp1 always interpolates from the identity quaternion, whose dot product with any unit quaternion is just that quaternion's own scalar part. The negative-dot-product condition `shortest` checks for to detect "the long way round" can therefore never occur here -- confirmed empirically (2000 random compositions, plus explicit UnitQuaternion([-1,0,0,0]) construction, all canonicalize to non-negative scalar part). Drops the dead branch, warns on shortest=True, documents why in a docstring note, and pins the no-op behavior (identical results for True/False, warns only when True) with a regression test. --- spatialmath/quaternion.py | 29 +++++++++++++++++++++++++---- tests/test_quaternion.py | 26 ++++++++++++++++++++++++++ 2 files changed, 51 insertions(+), 4 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index a955241e..858522e8 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -16,6 +16,7 @@ # pylint: disable=invalid-name from __future__ import annotations import math +import warnings import numpy as np from typing import Any import spatialmath.base as smb @@ -2073,7 +2074,7 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern """ Interpolate a unit quaternion - :param shortest: Take the shortest path along the great circle + :param shortest: deprecated, has no effect :param s: interpolation coefficient, range 0 to 1, or number of steps :type s: array_like or int :return: interpolated unit quaternion @@ -2104,19 +2105,39 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern .. note:: values of ``s`` are silently clipped to the range [0, 1] + .. note:: ``shortest`` can never change the result here. Every + ``UnitQuaternion`` has a non-negative scalar part by construction + (see :func:`~spatialmath.base.quaternions.qunit`), and this method + always interpolates *from* the identity quaternion ``[1,0,0,0]`` + -- whose dot product with any unit quaternion is just that + quaternion's own (always non-negative) scalar part. A negative + dot product is what ``shortest`` checks for to detect "the long + way round", and that condition can never occur here, unlike + :meth:`interp`, where it can. + + .. deprecated:: 1.1.19 + ``shortest`` has no effect and will be removed in a future + release. + :seealso: :func:`~spatialmath.base.quaternions.qslerp` """ # TODO allow self to have len() > 1 + if shortest: + warnings.warn( + "shortest has no effect on interp1 and will be removed in a " + "future release: interpolation from the identity quaternion " + "is always the shortest path", + DeprecationWarning, + ) + if isinstance(s, int) and s > 1: s = np.linspace(0, 1, s) else: s = smb.getvector(s) s = np.clip(s, 0, 1) # enforce valid values - q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare( - smb.qeye(), self.vec, shortest=shortest - ) + q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare(smb.qeye(), self.vec) qi = [] for sk in s: if sk == 0: diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 8e291573..cc08e4e6 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -3,6 +3,7 @@ import numpy.testing as nt import unittest from unittest.mock import patch +import warnings from spatialmath import * from spatialmath.base import * @@ -783,6 +784,31 @@ def test_interp_prepares_slerp_once(self): self.assertEqual(len(interpolate()), 5) self.assertEqual(dot.call_count, 1) + def test_interp1_shortest_deprecated(self): + # shortest never had an effect on interp1: every UnitQuaternion has + # a non-negative scalar part by construction (qunit()), and interp1 + # always interpolates from the identity quaternion, whose dot + # product with any unit quaternion is just that quaternion's own + # (always non-negative) scalar part. So the "long way round" branch + # shortest exists to avoid is unreachable. Pin that True/False give + # identical results, that it warns, and that leaving it at its + # default doesn't. + q = UnitQuaternion.Rx(4.5) # would be "the long way" if reachable + + for s in (0, 0.25, 0.5, 0.75, 1): + qcompare(q.interp1(s, shortest=False), q.interp1(s, shortest=True)) + + for a, b in zip(q.interp1(11, shortest=False), q.interp1(11, shortest=True)): + qcompare(a, b) + + with self.assertWarns(DeprecationWarning): + q.interp1(0.5, shortest=True) + + with warnings.catch_warnings(): + warnings.simplefilter("error") + q.interp1(0.5) # default shortest=False must not warn + q.interp1(0.5, shortest=False) + def test_increment(self): q = UnitQuaternion() From 208fe9b8e170e96d86f6e8a63c6b091950073b0a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 07:57:35 +1000 Subject: [PATCH 21/31] fix: Twist3 * SpatialVector raised ValueError instead of dispatching Twist3.__mul__ raised a hard ValueError for any right operand it didn't directly recognize, instead of returning NotImplemented -- so Python never got the chance to fall back to right.__rmul__(left). This made Twist3 * SpatialVelocity/Acceleration/Force/Momentum unconditionally fail, even though SpatialVector.__rmul__ explicitly documents supporting Twist3 alongside SE3. Found while independently verifying #207 (SpatialForce/Momentum coadjoint fix): that PR's Twist3 code path is correct but was untestable through normal `*` syntax because of this bug. --- spatialmath/twist.py | 12 ++++++++++-- tests/test_spatialvector.py | 19 +++++++++++++++++-- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 0645aef3..0c2a9d14 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -1107,6 +1107,7 @@ def __mul__( Twist3 scalar Twist3 element-wise product scalar Twist3 Twist3 element-wise product Twist3 SE3 Twist3 exponential x SE3 + Twist3 SpatialVector SpatialVector adjoint/coadjoint product ======== ==================== =================== ======================== .. note:: @@ -1114,7 +1115,10 @@ def __mul__( #. scalar x Twist is handled by ``__rmul__`` #. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix - #. Any other input combinations result in a ValueError. + #. ``Twist3 * SpatialVector`` (velocity, acceleration, force, momentum) + is handled by falling back to + :meth:`~spatialmath.spatialvector.SpatialVector.__rmul__`. + #. Any other input combination raises a ``TypeError``. For pose composition the ``left`` and ``right`` operands may be a sequence @@ -1147,7 +1151,11 @@ def __mul__( # return Twist(left.S * right) return Twist3(left.binop(right, lambda x, y: x * y)) else: - raise ValueError("twist *, incorrect right operand") + # not a type we handle directly -- return NotImplemented so Python + # falls back to right.__rmul__(left), which SpatialVector (velocity, + # acceleration, force, momentum) defines. If right has no __rmul__ + # either, Python raises its own TypeError. + return NotImplemented def __rmul__( right, left diff --git a/tests/test_spatialvector.py b/tests/test_spatialvector.py index af853329..18c81c42 100644 --- a/tests/test_spatialvector.py +++ b/tests/test_spatialvector.py @@ -225,8 +225,23 @@ def test_products(self): # v x v = a *, v x F6 = a # a x I, I x a # v x I, I x v - # twist x v, twist x a, twist x F - pass + + # twist x v, twist x a, twist x F, twist x momentum + T = SE3(1, 2, 3) * SE3.Rx(0.3) + S = Twist3(T) + + v = SpatialVelocity([1, 2, 3, 4, 5, 6]) + a = SpatialAcceleration([1, 2, 3, 4, 5, 6]) + f = SpatialForce([1, 2, 3, 4, 5, 6]) + m = SpatialMomentum([1, 2, 3, 4, 5, 6]) + + # Twist3 * X must agree with the equivalent SE3 * X: same + # underlying transform, two ways of expressing it. + for x in (v, a, f, m): + result = S * x + expected = T * x + self.assertIsInstance(result, type(x)) + nt.assert_almost_equal(result.A, expected.A) def test_spatial_transform_duality(self): # A pose transform acts on motion (velocity/acceleration) by the From a660079d4dc8780bb58995745fb4be1e76649cbe Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 17:04:45 -0700 Subject: [PATCH 22/31] 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 fab7d5d2f54df0ee99a5c4e5aeeb712606bade45 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 17:05:11 -0700 Subject: [PATCH 23/31] 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 9041485d..6800ff53 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -16,11 +16,13 @@ import spatialmath.base as smb from spatialmath.base.argcheck import getunit from spatialmath.base.types import * -import scipy.interpolate as interpolate -from typing import Optional +from typing import TYPE_CHECKING, Optional from functools import lru_cache import warnings +if TYPE_CHECKING: + import scipy.interpolate as interpolate + _eps = np.finfo(np.float64).eps @@ -920,7 +922,7 @@ def _compute_cdf_sin_squared(theta: float): @lru_cache(maxsize=1) def _generate_inv_cdf_sin_squared_interp( num_interpolation_points: int = 256, -) -> interpolate.interp1d: +) -> "interpolate.interp1d": """ Computes an interpolation function for the inverse CDF of the distribution of angular magnitude. @@ -933,6 +935,8 @@ def _generate_inv_cdf_sin_squared_interp( This function returns interpolation function for the inverse of the integral of the pdf of angular magnitudes (2/pi * sin^2(theta/2)), which is not analytically defined. """ + import scipy.interpolate as interpolate # deferred, it is slow to import + cdf_sin_squared_interp_angles = np.linspace(0, np.pi, num_interpolation_points) cdf_sin_squared_interp_values = _compute_cdf_sin_squared( cdf_sin_squared_interp_angles From cf3c70183dcb8dab0aa62e52012cf7fc57cb6511 Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Sat, 10 Oct 2026 17:05:48 -0700 Subject: [PATCH 24/31] 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() From 778273113673a52f05d279d873d90f6d3e49cac6 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 08:02:21 +1000 Subject: [PATCH 25/31] perf: compute Twist3.Ad() directly instead of via a throwaway SE3 Ad() was self.SE3().Ad() -- constructs a full validated SE3 object just to immediately extract its array and discard the object. Computes the same result directly (trexp then tr2adjoint, skipping the SE3 constructor/validation overhead): bit-identical output, ~1.4x faster. No existing test covered Ad() at all; added one first (hand-verified pure-rotation and pure-translation cases, plus a cross-check against SE3.Ad() over 20 random transforms) and confirmed it passes unchanged against both the old and new implementation. --- spatialmath/twist.py | 7 ++++--- tests/test_twist.py | 26 ++++++++++++++++++++++++++ 2 files changed, 30 insertions(+), 3 deletions(-) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 0645aef3..be41c0ff 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -886,12 +886,13 @@ def Ad(self): >>> S = Twist3.Rx(0.3) >>> S.Ad() - .. note:: This method computes the equivalent SE(3) matrix, then the adjoint - of that. + .. note:: Equivalent to, but faster than, ``self.SE3().Ad()`` -- computes + the adjoint directly from the twist's exponential without + constructing an intermediate ``SE3`` instance. :seealso: :func:`Twist3.ad`, :func:`Twist3.SE3`, :func:`Twist3.exp` """ - return self.SE3().Ad() + return smb.tr2adjoint(smb.trexp(self.S, check=False)) def skewa(self): """ diff --git a/tests/test_twist.py b/tests/test_twist.py index dcfb0fd1..a8084d0f 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -190,6 +190,32 @@ def test_pitch(self): tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0], pitch=3) self.assertAlmostEqual(tw.pitch, 3.0) + def test_Ad(self): + # pure rotation: Ad is block-diagonal [[R,0],[0,R]], no translation + # coupling -- hand-verified ground truth, not derived from Ad() itself. + R = SE3.Rx(0.4).R + S = Twist3(SE3.Rx(0.4)) + expected = np.zeros((6, 6)) + expected[:3, :3] = R + expected[3:, 3:] = R + nt.assert_almost_equal(S.Ad(), expected) + + # pure translation: Ad couples translation into the top-right block + # via skew(t), identity rotation blocks -- also hand-verified. + t = np.r_[1, 2, 3] + S = Twist3(SE3(t)) + expected = np.eye(6) + expected[:3, 3:] = skew(t) + nt.assert_almost_equal(S.Ad(), expected) + + # general case: cross-check against SE3.Ad(), computed from the same + # twist's own exponential, for several random transforms. + for _ in range(20): + T = SE3.Rand() + S = Twist3(T) + nt.assert_almost_equal(S.Ad(), S.SE3().Ad()) + nt.assert_almost_equal(S.Ad(), T.Ad()) + def test_arith(self): # check overloaded * From bcc0d6b35c7a01b5511c174dca62c5a40c849b91 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:14:57 +1000 Subject: [PATCH 26/31] fix: twist sequence predicates, scalar left-multiply, and copied docstrings Follow-up to the twist-sequence broadcasting change. A search for cut/paste errors between Twist2 and Twist3 turned up some real bugs as well as text: - isprismatic/isrevolute raised AttributeError on a sequence because they read .w/.v from the raw ndarrays in .data instead of the Twist elements. - Twist2.__rmul had no trailing underscores so it was never called, and ``2 * Twist2`` silently fell through to UserList repetition. - Twist3.__rmul__ used right.S, which is a list for a sequence, so scalar * Twist3 failed for sequences. - Twist3.Rand used yrange[0] as the lower bound of the z range. Docstrings: Twist2.SE2 said it returned SE3 and pointed at Twist3.exp (and its degree-mode message named Twist3.exp); Twist2.__init__ said it built a prismatic twist and swapped the moment/direction sizes; Twist2.isvalid described a 6-vector/4x4; UnitRevolute said prismatic; Twist3.Rand described SE3.Rand; Rx/Ry/Rz called their results SE(3)/SO(3) rotations; the [omega, v] ordering note contradicted the [v, omega] data layout; exp() documented a default of None and left a literal unterminated; the ``runblock:`` directives in __str__/__repr__ had a single colon (and their examples would not have run once fixed). SE2.Rand's example used SE3. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/pose2d.py | 4 +- spatialmath/twist.py | 104 +++++++++++++----------- tests/test_twist_sequence_predicates.py | 58 +++++++++++++ 3 files changed, 117 insertions(+), 49 deletions(-) create mode 100644 tests/test_twist_sequence_predicates.py diff --git a/spatialmath/pose2d.py b/spatialmath/pose2d.py index f919ddff..2234d8bc 100644 --- a/spatialmath/pose2d.py +++ b/spatialmath/pose2d.py @@ -358,13 +358,13 @@ def Rand( Return an SE2 instance with random rotation and translation. - - ``SE2.Rand()`` is a random SE(2) rotation. + - ``SE2.Rand()`` is a random SE(2) pose. - ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random poses. Example, create random ten vehicles in the xy-plane:: - >>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) + >>> x = SE2.Rand(N=10, xrange=[-2,2], yrange=[-2,2]) >>> len(x) 10 diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 0645aef3..7ef2d382 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -105,7 +105,7 @@ def isprismatic(self): if len(self) == 1: return smb.iszerovec(self.w) else: - return [smb.iszerovec(x.w) for x in self.data] + return [smb.iszerovec(x.w) for x in self] @property def isrevolute(self): @@ -131,7 +131,7 @@ def isrevolute(self): if len(self) == 1: return smb.iszerovec(self.v) else: - return [smb.iszerovec(x.v) for x in self.data] + return [smb.iszerovec(x.v) for x in self] @property def isunit(self): @@ -302,7 +302,7 @@ class Twist3(BaseTwist): .. note:: Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational - components, ie. :math:`[\omega, \vec{v}]`. + components, ie. :math:`[\vec{v}, \omega]`. """ @@ -364,8 +364,8 @@ def isvalid(v, check=True): """ Test if matrix is valid twist - :param x: array to test - :type x: ndarray + :param v: array to test + :type v: ndarray :return: Whether the value is a 6-vector or a valid 4x4 se(3) element :rtype: bool @@ -538,7 +538,7 @@ def Rx(cls, theta, unit="rad"): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Rx(θ)`` is an SE(3) rotation of θ radians about the x-axis + - ``Twist3.Rx(θ)`` is an se(3) rotation of θ radians about the x-axis - ``Twist3.Rx(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -569,7 +569,7 @@ def Ry(cls, theta, unit="rad", t=None): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Ry(θ)`` is an SO(3) rotation of θ radians about the y-axis + - ``Twist3.Ry(θ)`` is an se(3) rotation of θ radians about the y-axis - ``Twist3.Ry(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -600,7 +600,7 @@ def Rz(cls, theta, unit="rad", t=None): :return: 3D twist vector :rtype: Twist3 instance - - ``Twist3.Rz(θ)`` is an SO(3) rotation of θ radians about the z-axis + - ``Twist3.Rz(θ)`` is an se(3) rotation of θ radians about the z-axis - ``Twist3.Rz(θ, "deg")`` as above but θ is in degrees If ``θ`` is an array then the result is a sequence of rotations defined @@ -765,14 +765,14 @@ def Rand( :type zrange: 2-element sequence, optional :param N: number of random transforms :type N: int - :return: SE(3) matrix - :rtype: SE3 instance + :return: 3D twist + :rtype: Twist3 instance - Return an SE3 instance with random rotation and translation. + Return a Twist3 instance with random rotation and translation. - - ``SE3.Rand()`` is a random SE(3) translation. - - ``SE3.Rand(N=N)`` is an SE3 object containing a sequence of N random - poses. + - ``Twist3.Rand()`` is a random se(3) twist. + - ``Twist3.Rand(N=N)`` is a Twist3 object containing a sequence of N random + twists. Example: @@ -781,7 +781,7 @@ def Rand( >>> from spatialmath import Twist3 >>> Twist3.Rand(N=2) - :seealso: :func:`~spatialmath.quaternions.UnitQuaternion.Rand` + :seealso: :meth:`~spatialmath.pose3d.SE3.Rand` """ from spatialmath.pose3d import SO3 @@ -792,7 +792,7 @@ def Rand( low=yrange[0], high=yrange[1], size=N ) # random values in the range Z = np.random.uniform( - low=yrange[0], high=zrange[1], size=N + low=zrange[0], high=zrange[1], size=N ) # random values in the range R = SO3.Rand(N=N) @@ -1028,7 +1028,7 @@ def exp(self, theta=1, unit="rad"): """ Exponentiate a 3D twist - :param theta: rotation magnitude, defaults to None + :param theta: rotation magnitude, defaults to 1 :type theta: float, optional :param units: rotational units, defaults to 'rad' :type units: str, optional @@ -1037,7 +1037,7 @@ def exp(self, theta=1, unit="rad"): - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, + - ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` If ``len(X)==1`` and ``len(θ)==N`` then the resulting SE3 object has @@ -1165,7 +1165,7 @@ def __rmul__( - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` """ if smb.isscalar(left): - return Twist3(right.S * left) + return Twist3(right.binop(left, lambda x, y: x * y)) else: raise ValueError("Twist3 *, incorrect left operand") @@ -1180,10 +1180,10 @@ def __str__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist3 - >>> x = Twist3.R([1,2,3], [4,5,6]) + >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> print(x) """ return "\n".join( @@ -1204,13 +1204,11 @@ def __repr__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist3 - >>> x = Twist3.R([1,2,3], [4,5,6]) + >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> x - >>> a.append(a) - >>> a """ if len(self) == 0: @@ -1261,8 +1259,7 @@ def __init__(self, arg=None, w=None, check=True): r""" Construct a new 2D Twist object - :type a: 2-element array-like - :return: 2D prismatic twist + :return: 2D twist :rtype: Twist2 instance - ``Twist2(R)`` is a 2D Twist object representing the SO(2) rotation expressed as @@ -1272,7 +1269,7 @@ def __init__(self, arg=None, w=None, check=True): - ``Twist2(X)`` if X is an SO2 instance then create a 2D Twist object representing the SO(2) rotation, and if X is an SE2 instance then create a 2D Twist object representing the SE(2) motion - ``Twist2(V)`` is a 2D Twist object specified directly by a 3-element array-like comprising the - moment vector (1 element) and direction vector (2 elements). + moment vector (2 elements) and direction vector (1 element). :References: - Robotics, Vision & Control for Python, Section 2.2.2.4, P. Corke, Springer 2023. @@ -1280,7 +1277,7 @@ def __init__(self, arg=None, w=None, check=True): .. note:: Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational - components, ie. :math:`[\omega, \vec{v}]`. + components, ie. :math:`[\vec{v}, \omega]`. """ from spatialmath.pose2d import SE2 @@ -1330,12 +1327,12 @@ def isvalid(v, check=True): """ Test if matrix is valid twist - :param x: array to test - :type x: ndarray + :param v: array to test + :type v: ndarray :return: Whether the value is a 3-vector or a valid 3x3 se(2) element :rtype: bool - A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix, + A twist can be represented by a 3-vector or a 3x3 augmented skew symmetric matrix, for example: .. runblock:: pycon @@ -1371,7 +1368,7 @@ def UnitRevolute(cls, q): :param q: Point on the line of action :type q: array_like(2) - :return: 2D prismatic twist + :return: 2D revolute twist :rtype: Twist2 instance - ``Twist2.UnitRevolute(q)`` is a 2D Twist object representing rotation about the 2D point ``q``. @@ -1515,7 +1512,7 @@ def SE2(self, theta=1, unit="rad"): Convert 2D twist to SE(2) matrix :return: an SE(2) representation - :rtype: SE3 instance + :rtype: SE2 instance ``S.SE2()`` is an SE2 object representing the homogeneous transformation equivalent to the Twist2. This is the exponentiation of the twist vector. @@ -1528,12 +1525,12 @@ def SE2(self, theta=1, unit="rad"): >>> S = Twist2.UnitPrismatic([1,2]) >>> S.SE2() - :seealso: :func:`Twist3.exp` + :seealso: :func:`Twist2.exp` """ from spatialmath.pose2d import SE2 if unit != "rad" and any(s.isprismatic for s in self): - print("Twist3.exp: using degree mode for a prismatic twist") + print("Twist2.SE2: using degree mode for a prismatic twist") theta = smb.getunit(theta, unit) @@ -1576,7 +1573,7 @@ def exp(self, theta=1, unit="rad"): r""" Exponentiate a 2D twist - :param theta: rotation magnitude, defaults to None + :param theta: rotation magnitude, defaults to 1 :type theta: float, optional :param unit: rotational units, defaults to 'rad' :type unit: str, optional @@ -1585,7 +1582,7 @@ def exp(self, theta=1, unit="rad"): - ``X.exp()`` is the homogeneous transformation equivalent to the twist, :math:`e^{[S]}` - - ``X.exp(θ) as above but with a rotation of ``θ`` about the twist axis, + - ``X.exp(θ)`` as above but with a rotation of ``θ`` about the twist axis, :math:`e^{\theta[S]}` Example: @@ -1720,7 +1717,7 @@ def Tx(cls, x): :return: 2D twist vector :rtype: Twist2 instance - `Twist2.Tx(x)` is an se(2) translation of ``x`` along the x-axis + ``Twist2.Tx(x)`` is an se(2) translation of ``x`` along the x-axis Example: @@ -1746,7 +1743,7 @@ def Ty(cls, y): :return: 2D twist vector :rtype: Twist2 instance - `Twist2.Ty(y) is an se(2) translation of ``y`` along the y-axis + ``Twist2.Ty(y)`` is an se(2) translation of ``y`` along the y-axis Example: @@ -1825,11 +1822,25 @@ def __mul__( else: raise ValueError("Twist2 *, incorrect right operand") - def __rmul(self, left): + def __rmul__( + right, left + ): # lgtm[py/not-named-self] pylint: disable=no-self-argument + """ + Overloaded ``*`` operator + + :arg right: right multiplicand + :arg left: left multiplicand + :return: product + :raises: ValueError + + Left-multiplication by a scalar + + - ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s`` + """ if smb.isscalar(left): - return Twist2(self.S * left) + return Twist2(right.binop(left, lambda x, y: x * y)) else: - raise ValueError("twist *, incorrect left operand") + raise ValueError("Twist2 *, incorrect left operand") def __str__(self): """ @@ -1842,8 +1853,9 @@ def __str__(self): Example: - .. runblock: pycon + .. runblock:: pycon + >>> from spatialmath import Twist2 >>> x = Twist2([1,2,3]) >>> print(x) """ @@ -1858,13 +1870,11 @@ def __repr__(self): Example: - .. runblock: pycon + .. runblock:: pycon >>> from spatialmath import Twist2 >>> x = Twist2([1,2,3]) >>> x - >>> a.append(a) - >>> a """ diff --git a/tests/test_twist_sequence_predicates.py b/tests/test_twist_sequence_predicates.py new file mode 100644 index 00000000..8508a2a6 --- /dev/null +++ b/tests/test_twist_sequence_predicates.py @@ -0,0 +1,58 @@ +import numpy as np +import pytest + +from spatialmath import Twist2, Twist3 + + +def _sequence(cls): + """A prismatic, a revolute and a general twist, in that order.""" + if cls is Twist2: + vectors = [[1, 0, 0], [0, 0, 1], [1, 2, 3]] + else: + vectors = [[1, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1], [1, 2, 3, 0.1, 0.2, 0.3]] + return cls([np.array(v, dtype=float) for v in vectors]) + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_isprismatic_sequence(cls): + """isprismatic gives one result per twist, as for a single twist.""" + twists = _sequence(cls) + assert list(twists.isprismatic) == [True, False, False] + assert [t.isprismatic for t in twists] == list(twists.isprismatic) + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_isrevolute_sequence(cls): + """isrevolute gives one result per twist, as for a single twist.""" + twists = _sequence(cls) + assert list(twists.isrevolute) == [False, True, False] + assert [t.isrevolute for t in twists] == list(twists.isrevolute) + + +def test_twist2_se2_degree_warning_covers_sequence(capsys): + """Degree mode warns when any twist in the sequence is prismatic.""" + _sequence(Twist2).SE2(30, unit="deg") + assert "Twist2.SE2" in capsys.readouterr().out + + Twist2([np.array([1.0, 2.0, 3.0])] * 2).SE2(30, unit="deg") + assert capsys.readouterr().out == "" + + +@pytest.mark.parametrize("cls", [Twist2, Twist3]) +def test_scalar_times_twist(cls): + """``s * X`` scales each twist, the same as ``X * s``.""" + twists = _sequence(cls) + for left in (2 * twists, 2.0 * twists): + right = twists * 2 + assert len(left) == len(twists) + for a, b, t in zip(left, right, twists): + np.testing.assert_allclose(a.S, 2 * t.S) + np.testing.assert_allclose(a.S, b.S) + + +def test_twist3_rand_honours_zrange(): + """The z lower bound comes from zrange, not yrange.""" + np.random.seed(0) + poses = Twist3.Rand(N=200, xrange=(0, 1), yrange=(-3, -2), zrange=(5, 6)).exp() + z = np.array([p.t[2] for p in poses]) + assert z.min() >= 5 and z.max() <= 6 From af4ea008a8dbb0a8a8fa18e9dcccad6eeacf6c80 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:31:06 +1000 Subject: [PATCH 27/31] docs: describe the displacement and velocity usages of Twist2/Twist3 A twist is used both as the log of a pose (RVC, Modern Robotics) and as a velocity (ROS Twist). They share the vector, the [v, w] layout and the algebra, but nothing in the docstrings said so. Say so, point to SpatialVelocity as the typed counterpart, and note which properties are invariant to scaling the twist and which are not. Twist2 had no class docstring, only __init__, so give it one. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/twist.py | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 7ef2d382..78fd99f6 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -296,6 +296,23 @@ class Twist3(BaseTwist): 3D rigid body transformation which is the unique elements of the Lie algebra se(3) of the corresponding SE(3) matrix. + A twist can be used in two ways which share the same 6-vector, the same + :math:`[\vec{v}, \omega]` layout and the same algebra: + + - as a *displacement*, where ``exp(S)`` is the pose reached by following the + screw motion and ``exp(S * theta)`` scales it. ``S`` is often a unit + twist, see :meth:`unit`. This is the usage in Robotics, Vision & Control + and in Modern Robotics. + - as a *velocity*, where ``S`` has units of m/s and rad/s and ``exp(S * dt)`` + is the pose change over a time ``dt``. This is the meaning of ``Twist`` + in ROS. The typed counterpart, which also has acceleration, force and + momentum, is :class:`~spatialmath.spatialvector.SpatialVelocity`. + + Properties that describe the screw axis, such as :attr:`pitch`, + :attr:`pole` and :meth:`line`, are unchanged by scaling the twist so apply + to both. Those that depend on its magnitude, such as :attr:`theta`, are + an angle in the first usage and a rate in the second. + :References: - Robotics, Vision & Control for Python, Section 2.3.2.3, P. Corke, Springer 2023. - Modern Robotics, Lynch & Park, Cambridge 2017 @@ -1255,6 +1272,20 @@ def _repr_pretty_(self, p, cycle): class Twist2(BaseTwist): + r""" + 2D twist class + + A Twist2 object holds the parameters of a twist, a representation of a + 2D rigid body transformation which is the unique elements of the Lie + algebra se(2) of the corresponding SE(2) matrix. It is a 3-vector + :math:`[\vec{v}, \omega]`. + + As for :class:`Twist3` it can be used as a *displacement*, where ``exp(S)`` + is the pose reached by following the motion, or as a *velocity*, where + ``exp(S * dt)`` is the pose change over a time ``dt``. Properties that + describe the axis, such as :attr:`pole`, are unchanged by scaling the twist. + """ + def __init__(self, arg=None, w=None, check=True): r""" Construct a new 2D Twist object From b02516c6eab658e38c894fca03baa6ef71f6d54b Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 22 Oct 2026 11:53:13 +1000 Subject: [PATCH 28/31] feat(base): add environment() and move isnotebook out of the matplotlib block isnotebook() lived inside graphics.py's matplotlib try-block, so it vanished when matplotlib was missing, and it reported False for Google Colab and JupyterLite. RTB, MVTB and bdsim each grew their own copy as a result. Add environment(), returning "terminal", "ipython", "jupyter", "colab" or "jupyterlite", in a new matplotlib-free module, and make isnotebook() a thin wrapper that is True for jupyter, colab and jupyterlite. smb.isnotebook is still exported. Tests use a faked get_ipython/sys.modules/sys.platform. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/__init__.py | 3 +- spatialmath/base/_environment.py | 78 ++++++++++++++++++++++++++++++++ spatialmath/base/graphics.py | 20 -------- tests/base/test_environment.py | 64 ++++++++++++++++++++++++++ 4 files changed, 144 insertions(+), 21 deletions(-) create mode 100644 spatialmath/base/_environment.py create mode 100644 tests/base/test_environment.py diff --git a/spatialmath/base/__init__.py b/spatialmath/base/__init__.py index 88f7bec2..02489283 100644 --- a/spatialmath/base/__init__.py +++ b/spatialmath/base/__init__.py @@ -10,6 +10,7 @@ from spatialmath.base.vectors import * # lgtm [py/polluting-import] from spatialmath.base.symbolic import * # lgtm [py/polluting-import] from spatialmath.base.numeric import * # lgtm [py/polluting-import] +from spatialmath.base._environment import * # lgtm [py/polluting-import] import importlib @@ -46,7 +47,6 @@ "plotvol2": "graphics", "plotvol3": "graphics", "expand_dims": "graphics", - "isnotebook": "graphics", } @@ -394,6 +394,7 @@ def __getattr__(name): "axes_logic", "set_window_title", "expand_dims", + "environment", "isnotebook", # spatial.base.numeric "numjac", diff --git a/spatialmath/base/_environment.py b/spatialmath/base/_environment.py new file mode 100644 index 00000000..7d5b017e --- /dev/null +++ b/spatialmath/base/_environment.py @@ -0,0 +1,78 @@ +# Part of Spatial Math Toolbox for Python +# Copyright (c) 2000 Peter Corke +# MIT Licence, see details in top-level file: LICENCE + +""" +Detect the interactive environment that Python is running in. + +This module deliberately does not import Matplotlib, so it is cheap to import +and works when Matplotlib is not installed. +""" + +import sys +from typing import Literal + +__all__ = ["environment", "isnotebook"] + +Environment = Literal["terminal", "ipython", "jupyter", "colab", "jupyterlite"] + + +def environment() -> Environment: + """ + Determine the environment that Python is running in + + :return: the environment + :rtype: str + + The result is one of: + + ======================== ==================================================== + ``"terminal"`` standard Python interpreter, or script + ``"ipython"`` IPython running in a terminal + ``"jupyter"`` Jupyter kernel, eg. JupyterLab, Notebook, VS Code or + qtconsole + ``"colab"`` Google Colab + ``"jupyterlite"`` JupyterLite, running in the browser (Pyodide) + ======================== ==================================================== + + .. note:: The different Jupyter front ends (JupyterLab, classic Notebook, + VS Code, Jupyter Desktop) cannot be reliably told apart from within the + kernel, so all are reported as ``"jupyter"``. How figures will be + rendered depends on the Matplotlib backend, not on the front end. + + :seealso: :func:`isnotebook` + """ + # Colab and JupyterLite both also provide an IPython kernel, so test for + # them first + if "google.colab" in sys.modules: + return "colab" + if sys.platform == "emscripten": + return "jupyterlite" + + try: + shell = get_ipython().__class__.__name__ # type: ignore[name-defined] + except NameError: + return "terminal" # probably standard Python interpreter + + if shell == "ZMQInteractiveShell": + return "jupyter" # Jupyter notebook or qtconsole + elif shell == "TerminalInteractiveShell": + return "ipython" + else: + return "terminal" # other type (?) + + +def isnotebook() -> bool: + """ + Determine if code is being run from a Jupyter notebook + + :return: True if running in a Jupyter kernel, Google Colab or JupyterLite + :rtype: bool + + :seealso: :func:`environment` + + :references: + + - https://stackoverflow.com/questions/15411967/how-can-i-check-if-code-is-executed-in-the-ipython-notebook/39662359#39662359 + """ + return environment() in ("jupyter", "colab", "jupyterlite") diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index 2baf94b7..e83332e2 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1822,26 +1822,6 @@ def set_window_title(fig: plt.Figure, title: str) -> bool: return True return False - def isnotebook() -> bool: - """ - Determine if code is being run from a Jupyter notebook - - :references: - - - https://stackoverflow.com/questions/15411967/how-can-i-check-if-code- - is-executed-in-the-ipython-notebook/39662359#39662359 - """ - try: - shell = get_ipython().__class__.__name__ - if shell == "ZMQInteractiveShell": - return True # Jupyter notebook or qtconsole - elif shell == "TerminalInteractiveShell": - return False # Terminal running IPython - else: - return False # Other type (?) - except NameError: - return False # Probably standard Python interpreter - if __name__ == "__main__": import pathlib diff --git a/tests/base/test_environment.py b/tests/base/test_environment.py new file mode 100644 index 00000000..a9911b08 --- /dev/null +++ b/tests/base/test_environment.py @@ -0,0 +1,64 @@ +import builtins +import sys + +import pytest + +from spatialmath.base import environment, isnotebook + +# These tests are mocked: a fake get_ipython() and sys.modules / sys.platform +# stand in for the real front ends, which cannot be run under CI. + + +def fake_shell(name): + shell = type(name, (), {})() + return lambda: shell + + +@pytest.fixture +def plain(monkeypatch): + """Standard interpreter: no IPython, not Colab, not Pyodide""" + monkeypatch.delattr(builtins, "get_ipython", raising=False) + monkeypatch.delitem(sys.modules, "google.colab", raising=False) + monkeypatch.setattr(sys, "platform", "linux") + return monkeypatch + + +def test_terminal(plain): + assert environment() == "terminal" + assert not isnotebook() + + +def test_ipython_terminal(plain): + plain.setattr( + builtins, "get_ipython", fake_shell("TerminalInteractiveShell"), raising=False + ) + assert environment() == "ipython" + assert not isnotebook() + + +def test_jupyter(plain): + plain.setattr( + builtins, "get_ipython", fake_shell("ZMQInteractiveShell"), raising=False + ) + assert environment() == "jupyter" + assert isnotebook() + + +def test_unknown_shell(plain): + plain.setattr(builtins, "get_ipython", fake_shell("SomethingElse"), raising=False) + assert environment() == "terminal" + assert not isnotebook() + + +def test_colab(plain): + # Colab's shell is not a ZMQInteractiveShell, so detect it by the module + plain.setattr(builtins, "get_ipython", fake_shell("Shell"), raising=False) + plain.setitem(sys.modules, "google.colab", object()) + assert environment() == "colab" + assert isnotebook() + + +def test_jupyterlite(plain): + plain.setattr(sys, "platform", "emscripten") + assert environment() == "jupyterlite" + assert isnotebook() From 60c634ccb950710b7664317110f36155e21ea0be Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Thu, 5 Nov 2026 08:59:22 -0500 Subject: [PATCH 29/31] chore: bump version to 1.1.19 --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 8e052175..a614f70d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "spatialmath-python" -version = "1.1.18" +version = "1.1.19" authors = [ { name="Peter Corke", email="rvc@petercorke.com" }, ] From 4ab65ff640de49f97f156c822750172f057f781e Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Thu, 5 Nov 2026 09:24:43 -0500 Subject: [PATCH 30/31] Move three-plane tests to the end of the test file Avoids a textual conflict with other PRs that add tests after the line distance tests, and restores the executable bit on the test file, which runs as a script. --- tests/test_geom3d.py | 56 ++++++++++++++++++++++---------------------- 1 file changed, 28 insertions(+), 28 deletions(-) mode change 100644 => 100755 tests/test_geom3d.py diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py old mode 100644 new mode 100755 index 0cf87437..ea2d6623 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -47,34 +47,6 @@ def test_line_distance_parallel_tolerance(): assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) -@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) -@pytest.mark.parametrize( - "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] -) -@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) -def test_three_plane_intersection(point, normals, scales): - normals = normals * np.array(scales)[:, None] - planes = [Plane3.PointNormal(point, normal) for normal in normals] - intersection = Plane3.intersection(*planes) - - assert intersection.shape == (3,) - nt.assert_allclose(intersection, point, atol=1e-12) - # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. - nt.assert_allclose( - normals @ intersection + np.array([plane.d for plane in planes]), - 0, - atol=1e-12, - ) - nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) - - -@pytest.mark.parametrize("offset", [0, 1]) -def test_three_plane_intersection_singular(offset): - planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] - with pytest.raises(np.linalg.LinAlgError): - Plane3.intersection(*planes) - - class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): @@ -433,5 +405,33 @@ def test_methods(self): # py.intersect_plane(plane) +@pytest.mark.parametrize("point", [[0, 0, 0], [1, 2, 3], [-1.5, 2, 3.25]]) +@pytest.mark.parametrize( + "normals", [np.eye(3), np.array([[1, 2, 0], [0, 1, 3], [2, 0, 1]])] +) +@pytest.mark.parametrize("scales", [[1, 1, 1], [-2, 0.5, 3]]) +def test_three_plane_intersection(point, normals, scales): + normals = normals * np.array(scales)[:, None] + planes = [Plane3.PointNormal(point, normal) for normal in normals] + intersection = Plane3.intersection(*planes) + + assert intersection.shape == (3,) + nt.assert_allclose(intersection, point, atol=1e-12) + # Each plane is defined by n @ x + d = 0, regardless of coefficient scale. + nt.assert_allclose( + normals @ intersection + np.array([plane.d for plane in planes]), + 0, + atol=1e-12, + ) + nt.assert_allclose(Plane3.intersection(*reversed(planes)), point, atol=1e-12) + + +@pytest.mark.parametrize("offset", [0, 1]) +def test_three_plane_intersection_singular(offset): + planes = [Plane3([1, 0, 0, 0]), Plane3([1, 0, 0, offset]), Plane3([0, 1, 0, 0])] + with pytest.raises(np.linalg.LinAlgError): + Plane3.intersection(*planes) + + if __name__ == "__main__": unittest.main() From 8eb0e3bd2649679c9a25462d4e135c51ed200204 Mon Sep 17 00:00:00 2001 From: Tim Perkins Date: Thu, 5 Nov 2026 10:04:17 -0500 Subject: [PATCH 31/31] build: drop oldest-supported-numpy from build requirements The package is pure Python, so numpy is not needed to build it, and numpy has deprecated oldest-supported-numpy. Build tools that do not install arbitrary build requirements (for example rules_py's uv sdist builder) fail on it. --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 8e052175..d3e35a1c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -78,7 +78,7 @@ ros-humble = [ [build-system] -requires = ["setuptools", "oldest-supported-numpy"] +requires = ["setuptools"] build-backend = "setuptools.build_meta" [tool.setuptools]