From 874accebd95250586fb8fec0aa5b7feafdd7a284 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 13 Oct 2026 11:33:31 +1000 Subject: [PATCH 01/20] fix: pre-commit is a development tool, not a runtime dependency pre-commit was added to [project].dependencies in #98 along with the black hook, so every 'pip install spatialmath-python' also installs pre-commit and its dependency tree (virtualenv, identify, cfgv, nodeenv, ...). spatialmath never imports it. This is not just bloat. It makes the install fail anywhere a transitive pin conflicts: virtualenv 21.14.7 requires packaging>=26.3, and Pyodide (JupyterLite, in-browser) ships packaging 24.2, so micropip.install('spatialmath-python') raises ValueError. Downstream JupyterLite notebooks of the Robotics, Machine Vision and bdsim toolboxes are affected. Move it to the dev extra, which the CI workflows already install, and make the README's contributor instructions install it. Co-Authored-By: Claude Sonnet 5.5 --- README.md | 2 ++ pyproject.toml | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ccd4747f..02421c30 100644 --- a/README.md +++ b/README.md @@ -137,6 +137,8 @@ git clone https://github.com/rai-opensource/spatialmath-python.git cd spatialmath-python pip install -e . # Optional: if you would like to contribute and commit code changes to the repository, +# install the development tools (including pre-commit) and set up the git hooks +# pip install -e .[dev] # pre-commit install ``` diff --git a/pyproject.toml b/pyproject.toml index 10b74a4c..01ddc21e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -38,7 +38,6 @@ dependencies = [ "matplotlib", "ansitable", "typing_extensions", - "pre-commit", ] [project.urls] @@ -50,6 +49,7 @@ dependencies = [ [project.optional-dependencies] dev = [ + "pre-commit", "sympy", "pytest", "pytest-timeout", From de57cf3ea59f6b727b19241b90a6489c931c34e7 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 14 Oct 2026 08:07:35 +1000 Subject: [PATCH 02/20] fix: make the OA and conjugation docstrings raw strings Two docstrings containing LaTeX (\mat, \vec, \times) were not raw strings, so compiling the package gives 'invalid escape sequence' SyntaxWarnings, and in UnitQuaternion.OA the \t of \times and the \v of \vec are valid escapes that put a tab and a vertical tab into the rendered documentation. The OA docstring was rewritten with these in #195 and the conjugation one predates it. Add a test that compiles every module with warnings recorded so a non-raw docstring with a backslash fails CI. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/baseposematrix.py | 2 +- spatialmath/quaternion.py | 2 +- tests/test_source_warnings.py | 33 +++++++++++++++++++++++++++++++++ 3 files changed, 35 insertions(+), 2 deletions(-) create mode 100644 tests/test_source_warnings.py diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index ceb1fc91..f663c1aa 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -618,7 +618,7 @@ def stack(self) -> NDArray: return np.dstack(self.data) def conjugation(self, A: NDArray) -> NDArray: - """ + r""" Matrix conjugation :param A: matrix to conjugate diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 6fad5597..0468a615 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1437,7 +1437,7 @@ def RPY( @classmethod def OA(cls, o: ArrayLike3, a: ArrayLike3) -> UnitQuaternion: - """ + r""" Construct a new unit quaternion from two vectors :param o: 3-vector parallel to Y- axis diff --git a/tests/test_source_warnings.py b/tests/test_source_warnings.py new file mode 100644 index 00000000..a580164d --- /dev/null +++ b/tests/test_source_warnings.py @@ -0,0 +1,33 @@ +"""Compiling the package must not raise SyntaxWarning or DeprecationWarning. + +A non-raw docstring containing LaTeX such as ``\\mat`` or ``\\vec`` gives an +"invalid escape sequence" warning, and ``\\t`` or ``\\v`` (as in ``\\times`` and +``\\vec``) are valid escapes that silently put a tab or vertical tab into the +documentation. Use ``r\"\"\"`` for docstrings that contain backslashes. +""" + +import pathlib +import unittest +import warnings + +import spatialmath + + +class TestSourceWarnings(unittest.TestCase): + def test_package_compiles_without_warnings(self): + root = pathlib.Path(spatialmath.__file__).parent + problems = [] + for path in sorted(root.rglob("*.py")): + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + compile(path.read_text(encoding="utf-8"), str(path), "exec") + problems += [ + f"{path.relative_to(root)}:{w.lineno}: {w.message}" + for w in caught + if issubclass(w.category, (SyntaxWarning, DeprecationWarning)) + ] + self.assertEqual(problems, []) + + +if __name__ == "__main__": + unittest.main() From d9eab2e430ac33d4c40229198d6281870645ef52 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 14 Oct 2026 19:05:02 +1000 Subject: [PATCH 03/20] docs(base): fix the angle_std formula and the angle_mean and angle_wrap parameter docs - angle_std: the formula had sin in both components of the mean vector, it is cos and sin; say what R is, that the result is the same as scipy.stats.circstd, and what happens as R tends to zero - angle_mean: the parameter was documented as v but is theta, and the range had a stray bracket; the result is in [-pi, pi]; say that a cancelling set of angles has no defined mean - angle_wrap: the parameter was documented as v but is theta Documentation only. --- spatialmath/base/vectors.py | 41 ++++++++++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index 59c61797..22ac77f5 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -683,14 +683,28 @@ def angle_std(theta: ArrayLike) -> float: r""" Standard deviation of angular values - :param theta: angular values + :param theta: angular values, in radians :type theta: array_like - :return: circular standard deviation + :return: circular standard deviation, in radians :rtype: float + The circular standard deviation is + .. math:: - \sigma_{\theta} = \sqrt{-2 \log \| \left[ \frac{\sum \sin \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \|} \in [0, \infty) + \sigma_{\theta} = \sqrt{-2 \log R} \in [0, \infty), \quad + R = \left\| \left[ \frac{\sum \cos \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \right\| + + where :math:`R` is the length of the mean of the unit vectors at angles + :math:`\theta_i`. :math:`R = 1` if all the angles are equal, giving a + standard deviation of zero, and :math:`R \to 0` as the angles spread evenly + around the circle, when the standard deviation grows without bound. + + This is the same quantity as ``scipy.stats.circstd`` with its default + arguments. + + .. note:: If :math:`R` is exactly zero the result is ``inf``, with a NumPy + divide-by-zero warning. :seealso: :func:`angle_mean` """ @@ -705,16 +719,23 @@ def angle_mean(theta: ArrayLike) -> float: r""" Mean of angular values - :param theta: angular values - :type v: array_like - :return: circular mean + :param theta: angular values, in radians + :type theta: array_like + :return: circular mean, in radians :rtype: float - The circular mean is given by + The circular mean is the angle of the mean of the unit vectors at angles + :math:`\theta_i` .. math:: - \bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi)] + \bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi] + + computed with the two-argument arctangent so that the quadrant is correct. + + .. note:: If the unit vectors cancel, for example the angles :math:`0` and + :math:`\pi`, the mean direction is undefined and the result is decided by + rounding error. :seealso: :func:`angle_std` """ @@ -727,8 +748,8 @@ def angle_wrap(theta: ArrayLike, mode: str = "-pi:pi") -> Union[float, NDArray]: """ Generalized angle-wrapping - :param v: angles to wrap - :type v: array_like + :param theta: angles to wrap + :type theta: array_like :param mode: wrapping mode, one of: ``"0:2pi"``, ``"0:pi"``, ``"-pi/2:pi/2"`` or ``"-pi:pi"`` [default] :type mode: str, optional :return: wrapped angles From 6e02bb89509ab51a4fdcd04c1fa952bd5c2c3541 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 14 Oct 2026 19:17:05 +1000 Subject: [PATCH 04/20] fix(base): angle_std returned nan for identical angles The length of the mean of the unit vectors, R, is at most 1 but rounding can make it 1 + eps, so -2 * log(R) was a tiny negative number and its square root nan, with 'invalid value encountered in sqrt'. For example angle_std of 100 copies of -2.9. 33 of 378 combinations of angle and sample count failed. Clamp at zero. --- spatialmath/base/vectors.py | 5 ++++- tests/base/test_vectors.py | 12 ++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index 59c61797..d6ca4647 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -698,7 +698,10 @@ def angle_std(theta: ArrayLike) -> float: Y = np.sin(theta).mean() R = np.sqrt(X**2 + Y**2) - return np.sqrt(-2 * np.log(R)) + # R is the length of an average of unit vectors so is at most 1, but + # rounding can make it 1 + eps, which would give the square root of a + # (tiny) negative number and nan + return np.sqrt(max(0.0, -2 * np.log(R))) def angle_mean(theta: ArrayLike) -> float: diff --git a/tests/base/test_vectors.py b/tests/base/test_vectors.py index 3110eb56..e07db802 100755 --- a/tests/base/test_vectors.py +++ b/tests/base/test_vectors.py @@ -7,6 +7,7 @@ """ +import warnings import numpy as np import numpy.testing as nt import unittest @@ -397,6 +398,17 @@ def test_angle_stats(self): self.assertAlmostEqual(angle_mean(theta), pi) self.assertAlmostEqual(angle_std(theta), 0.9717284050981313) + def test_angle_std_identical_angles(self): + # the length of the mean unit vector can come out as 1 + eps, which + # used to give the square root of a negative number + for angle in np.linspace(-3.1, 3.1, 63): + for n in (1, 2, 3, 7, 10, 100): + with warnings.catch_warnings(): + warnings.simplefilter("error") + std = angle_std(np.full(n, angle)) + self.assertTrue(np.isfinite(std)) + self.assertLess(std, 1e-6) + def test_removesmall(self): v = np.r_[1, 2, 3] nt.assert_array_almost_equal(removesmall(v), v) From 2e48076f11fd28b22cf7f9d86dbac313437b6751 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:39:16 +1000 Subject: [PATCH 05/20] fix: Twist2.ad raised ValueError for every twist; add Twist2.Ad and SE2.Ad Twist2.ad was a copy of Twist3.ad that was never adapted to 2D: it called skew() on the scalar w and built 6x6 blocks, so it raised ValueError for any Twist2. It was also a property while Twist3.ad is a method, and its docstring example called unit(). Rewrite it as a method returning the 3x3 matrix of the Lie bracket, [[0, -w, vy], [w, 0, -vx], [0, 0, 0]] and return a list for a sequence, as skewa() does. Add Twist2.Ad() and SE2.Ad() (tr2adjoint2 existed but only as a base function) to match Twist3.Ad and SE3.Ad. Also correct the 2D-specific text in tr2adjoint2's docstring, which named tr2jac2 and said 6x6. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transforms2d.py | 6 +-- spatialmath/pose2d.py | 26 ++++++++++++ spatialmath/twist.py | 69 ++++++++++++++++++++++++++------ tests/test_twist2_adjoint.py | 66 ++++++++++++++++++++++++++++++ 4 files changed, 151 insertions(+), 16 deletions(-) create mode 100644 tests/test_twist2_adjoint.py diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 6aad235c..f431df2e 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -796,9 +796,9 @@ def tr2adjoint2(T): where :math:`\mat{T} \in \SE2`. - ``tr2jac2(T)`` is an adjoint matrix (6x6) that maps spatial velocity or - differential motion between frame {B} to frame {A} which are attached to the - same moving body. The pose of {B} relative to {A} is represented by the + ``tr2adjoint2(T)`` is an adjoint matrix (3x3 for SE(2), 1x1 for SO(2)) that + maps spatial velocity or differential motion between frame {B} to frame {A} + which are attached to the same moving body. The pose of {B} relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`. .. runblock:: pycon diff --git a/spatialmath/pose2d.py b/spatialmath/pose2d.py index 62ac8510..f919ddff 100644 --- a/spatialmath/pose2d.py +++ b/spatialmath/pose2d.py @@ -609,6 +609,32 @@ def inv(self): else: return SE2([smb.rt2tr(x.R.T, -x.R.T @ x.t) for x in self], check=False) + def Ad(self): + r""" + Adjoint of SE(2) + + :return: adjoint matrix + :rtype: ndarray(3,3) + + ``SE2.Ad`` is the 3x3 adjoint matrix + + If the twist or spatial velocity :math:`\nu = (v_x, v_y, \omega)^T` + and the SE(2) represents the pose of {B} relative to {A}, + ie. :math:`{}^A {\bf T}_B`, and the adjoint is :math:`\mathbf{A}` then + :math:`{}^{A}\!\nu = \mathbf{A} {}^{B}\!\nu`. + + Example: + + .. runblock:: pycon + + >>> from spatialmath import SE2 + >>> SE2(1, 2, 0.3).Ad() + + :seealso: :meth:`spatialmath.pose3d.SE3.Ad`, :meth:`spatialmath.twist.Twist2.ad`, + :func:`~spatialmath.base.transforms2d.tr2adjoint2` + """ + return smb.tr2adjoint2(self.A) + def SE3(self, z=0): """ Create SE(3) from SE(2) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 928235e4..d3433869 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -1633,13 +1633,26 @@ def unit(self): # prismatic twist return Twist2(smb.unitvec(self.v), [0, 0, 0]) - @property def ad(self): - """ - Twist2.ad Logarithm of adjoint + r""" + Logarithm of adjoint of 2D twist + + :return: logarithm of adjoint matrix + :rtype: ndarray(3,3), or list of ndarray if the twist is a sequence + + ``S.ad()`` is the 3x3 matrix representation of the Lie bracket with + the twist ``S``, such that for twists ``S`` and ``T`` - - ``S.ad()`` is the logarithm of the adjoint matrix of the corresponding - homogeneous transformation. + .. math:: + + \mathrm{ad}(S) \vec{T} = \vee \left( [S] [T] - [T] [S] \right) + + where :math:`[S]` is the augmented skew-symmetric matrix of the twist. + For :math:`S = (v_x, v_y, \omega)` it is + + .. math:: + + \begin{pmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{pmatrix} Example: @@ -1648,16 +1661,46 @@ def ad(self): >>> from spatialmath import SE2, Twist2 >>> T = SE2(1, 2, 0.3) >>> S = Twist2(T) - >>> S.unit() + >>> S.ad() + + .. note:: An alternative approach to computing the adjoint is to + exponentiate this 3x3 matrix. - :seealso: SE3.Ad. + :seealso: :func:`Twist2.Ad` """ - return np.array( - [ - [smb.skew(self.w), smb.skew(self.v)], - [np.zeros((3, 3)), smb.skew(self.w)], - ] - ) + if len(self) == 1: + v, w = self.v, self.w + return np.array([[0, -w, v[1]], [w, 0, -v[0]], [0, 0, 0]]) + else: + return [x.ad() for x in self] + + def Ad(self): + """ + Adjoint of 2D twist + + :return: adjoint matrix + :rtype: ndarray(3,3) + + ``S.Ad()`` is the 3x3 adjoint matrix of the corresponding + homogeneous transformation. + + For a twist representing motion from frame {B} to {A}, the adjoint will + transform a twist relative to frame {A} to one relative to frame {B}. + + Example: + + .. runblock:: pycon + + >>> from spatialmath import Twist2 + >>> S = Twist2([1, 2, 0.3]) + >>> S.Ad() + + .. note:: This method computes the equivalent SE(2) matrix, then the adjoint + of that. + + :seealso: :func:`Twist2.ad`, :func:`Twist2.SE2`, :func:`Twist2.exp` + """ + return self.SE2().Ad() @classmethod def Tx(cls, x): diff --git a/tests/test_twist2_adjoint.py b/tests/test_twist2_adjoint.py new file mode 100644 index 00000000..23413c44 --- /dev/null +++ b/tests/test_twist2_adjoint.py @@ -0,0 +1,66 @@ +import numpy as np +import numpy.testing as nt +import pytest +from scipy.linalg import expm + +from spatialmath import SE2, Twist2 +from spatialmath.base import skewa, vexa + + +def _bracket(a, b): + """Lie bracket of two se(2) vectors, via their matrix commutator.""" + A, B = skewa(a), skewa(b) + return vexa(A @ B - B @ A) + + +@pytest.fixture +def twists(): + rng = np.random.default_rng(7) + return rng.normal(size=3), rng.normal(size=3) + + +def test_ad_is_lie_bracket(twists): + """ad(S) @ T is the Lie bracket [S, T].""" + a, b = twists + ad = Twist2(a).ad() + assert ad.shape == (3, 3) + nt.assert_allclose(ad @ b, _bracket(a, b), atol=1e-14) + + +def test_ad_antisymmetry(twists): + """[S, S] = 0, and ad(S) T = -ad(T) S.""" + a, b = twists + nt.assert_allclose(Twist2(a).ad() @ a, 0, atol=1e-14) + nt.assert_allclose(Twist2(a).ad() @ b, -Twist2(b).ad() @ a, atol=1e-14) + + +def test_exp_of_ad_is_Ad(twists): + """The adjoint of exp(S) is the exponential of ad(S).""" + a, _ = twists + S = Twist2(a) + nt.assert_allclose(S.Ad(), expm(S.ad()), atol=1e-12) + nt.assert_allclose(S.Ad(), S.SE2().Ad(), atol=1e-14) + + +def test_SE2_Ad_maps_twists(twists): + """Ad(T) S is vee(T [S] T^-1).""" + a, _ = twists + T = SE2(1.5, -0.7, 0.9) + expected = vexa(T.A @ skewa(a) @ T.inv().A) + nt.assert_allclose(T.Ad() @ a, expected, atol=1e-14) + + +def test_SE2_Ad_identity_and_composition(): + """Ad is a homomorphism: Ad(T1 T2) = Ad(T1) Ad(T2).""" + nt.assert_allclose(SE2().Ad(), np.eye(3), atol=1e-14) + T1, T2 = SE2(1, 2, 0.3), SE2(-0.5, 0.25, -1.1) + nt.assert_allclose((T1 * T2).Ad(), T1.Ad() @ T2.Ad(), atol=1e-14) + + +def test_ad_sequence(): + """A sequence of twists gives a list of matrices, as for skewa().""" + seq = Twist2([np.array([1.0, 2.0, 0.3]), np.array([0.0, 1.0, -0.4])]) + ads = seq.ad() + assert isinstance(ads, list) and len(ads) == 2 + for m, t in zip(ads, seq): + nt.assert_allclose(m, t.ad()) From 162b05d2ca273f0e3d2fd9694d316fcf869cd5fd Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:42:10 +1000 Subject: [PATCH 06/20] docs: fix the math block in the tr2adjoint and tr2adjoint2 docstrings The formula never rendered and would not have rendered correctly: - ``.. math:`` has a single colon, so reST treated the block as a comment. - \skew is not a macro defined in docs/source/conf.py (\sk and \vex are). - the braces of \skew{...} were unbalanced. - there was a stray trailing X, and Ad was set in italic. Write it with the project's macros, and use \SE{n} rather than \SE3, which only worked because \SE takes one argument. The 3D docstring also named tr2jac and its sizes; name tr2adjoint and give 6x6 for SE(3), 3x3 for SO(3). Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transforms2d.py | 6 +++--- spatialmath/base/transforms3d.py | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 6aad235c..059d6845 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -790,11 +790,11 @@ def tr2adjoint2(T): Computes an adjoint matrix that maps the Lie algebra between frames. - .. math: + .. math:: - Ad(\mat{T}) \vec{X} X = \vee \left( \mat{T} \skew{\vec{X} \mat{T}^{-1} \right) + \mathrm{Ad}(\mat{T})\,\vec{X} = \vex{\mat{T}\,\sk{\vec{X}}\,\mat{T}^{-1}} - where :math:`\mat{T} \in \SE2`. + where :math:`\mat{T} \in \SE{2}`. ``tr2jac2(T)`` is an adjoint matrix (6x6) that maps spatial velocity or differential motion between frame {B} to frame {A} which are attached to the diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index c552c797..6a792f6a 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -2732,15 +2732,15 @@ def tr2adjoint(T): Computes an adjoint matrix that maps the Lie algebra between frames. - .. math: + .. math:: - Ad(\mat{T}) \vec{X} X = \vee \left( \mat{T} \skew{\vec{X} \mat{T}^{-1} \right) + \mathrm{Ad}(\mat{T})\,\vec{X} = \vex{\mat{T}\,\sk{\vec{X}}\,\mat{T}^{-1}} - where :math:`\mat{T} \in \SE3`. + where :math:`\mat{T} \in \SE{3}`. - ``tr2jac(T)`` is an adjoint matrix (6x6) that maps spatial velocity or - differential motion between frame {B} to frame {A} which are attached to the - same moving body. The pose of {B} relative to {A} is represented by the + ``tr2adjoint(T)`` is an adjoint matrix (6x6 for SE(3), 3x3 for SO(3)) that + maps spatial velocity or differential motion between frame {B} to frame {A} + which are attached to the same moving body. The pose of {B} relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`. .. runblock:: pycon From 102c91d64a7d9c4c7b53368edaa112a6cb5b1993 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 11:19:47 +1000 Subject: [PATCH 07/20] fix(graphics): ignore sign of scalar in expand_dims, document reversed limits A negative scalar previously produced reversed limits (-3 -> [3,-3,...]), silently inverting the axes. A scalar is symmetric about zero, so use abs(). Document that min > max in the 2/4/6 element forms is passed through and matplotlib inverts the axis. Add test_expand_dims. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/graphics.py | 8 ++++++++ tests/base/test_graphics.py | 17 +++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index c51d7f94..1dd63b14 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1708,11 +1708,18 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: * A -> [-A, A, -A, A, -A, A] * [A,B] -> [A, B, A, B, A, B] * [A,B,C,D,E,F] -> [A, B, C, D, E, F] + + A scalar is symmetric about zero, so its sign is ignored: ``-A`` is the + same as ``A``. For the other forms the values are returned as given and + no ordering check is made. If a minimum is greater than the + corresponding maximum, matplotlib will invert that axis when the limits + are applied, which is a convenient way to flip an axis direction. """ dim = smb.getvector(dim) if nd == 2: if len(dim) == 1: + dim = abs(dim) return np.r_[-dim, dim, -dim, dim] elif len(dim) == 2: return np.r_[dim[0], dim[1], dim[0], dim[1]] @@ -1722,6 +1729,7 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: raise ValueError("bad dimension specified") elif nd == 3: if len(dim) == 1: + dim = abs(dim) return np.r_[-dim, dim, -dim, dim, -dim, dim] elif len(dim) == 2: return np.r_[dim[0], dim[1], dim[0], dim[1], dim[0], dim[1]] diff --git a/tests/base/test_graphics.py b/tests/base/test_graphics.py index 552ebdb0..d19bb6bf 100644 --- a/tests/base/test_graphics.py +++ b/tests/base/test_graphics.py @@ -13,6 +13,23 @@ class TestGraphics(unittest.TestCase): def teardown_method(self, method): plt.close("all") + def test_expand_dims(self): + np.testing.assert_array_equal(expand_dims(3), [-3, 3, -3, 3]) + np.testing.assert_array_equal(expand_dims(-3), [-3, 3, -3, 3]) + np.testing.assert_array_equal(expand_dims([1, 2]), [1, 2, 1, 2]) + np.testing.assert_array_equal(expand_dims([1, 2, 3, 4]), [1, 2, 3, 4]) + # reversed limits are passed through, matplotlib inverts the axis + np.testing.assert_array_equal(expand_dims([2, 1]), [2, 1, 2, 1]) + + np.testing.assert_array_equal(expand_dims(-3, nd=3), [-3, 3, -3, 3, -3, 3]) + np.testing.assert_array_equal(expand_dims([1, 2], nd=3), [1, 2] * 3) + np.testing.assert_array_equal(expand_dims([1, 2, 3, 4, 5, 6], nd=3), [1, 2, 3, 4, 5, 6]) + + with self.assertRaises(ValueError): + expand_dims([1, 2, 3]) + with self.assertRaises(ValueError): + expand_dims([1, 2, 3, 4], nd=3) + @pytest.mark.skipif( sys.platform.startswith("darwin") and sys.version_info < (3, 11), reason="tkinter bug with mac", From 37b588637ee975e1484502501c611593c8d60f97 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 17:29:40 +1000 Subject: [PATCH 08/20] feat(base): set_window_title, set the title of a figure's window if it has one A figure on a headless backend, or one not managed by pyplot, has no window; the function then does nothing and returns False. RTB and MVTB each had their own copy with different guards. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/__init__.py | 2 ++ spatialmath/base/graphics.py | 34 ++++++++++++++++++++++++++++++++++ tests/base/test_graphics.py | 33 +++++++++++++++++++++++++++++++++ 3 files changed, 69 insertions(+) diff --git a/spatialmath/base/__init__.py b/spatialmath/base/__init__.py index b01e14ba..88f7bec2 100644 --- a/spatialmath/base/__init__.py +++ b/spatialmath/base/__init__.py @@ -42,6 +42,7 @@ "plot_cone": "graphics", "plot_cuboid": "graphics", "axes_logic": "graphics", + "set_window_title": "graphics", "plotvol2": "graphics", "plotvol3": "graphics", "expand_dims": "graphics", @@ -391,6 +392,7 @@ def __getattr__(name): "plot_cone", "plot_cuboid", "axes_logic", + "set_window_title", "expand_dims", "isnotebook", # spatial.base.numeric diff --git a/spatialmath/base/graphics.py b/spatialmath/base/graphics.py index 40503c41..034fd824 100644 --- a/spatialmath/base/graphics.py +++ b/spatialmath/base/graphics.py @@ -1783,6 +1783,37 @@ def expand_dims(dim: ArrayLike = None, nd: int = 2) -> NDArray: else: raise ValueError("nd is 2 or 3") + def set_window_title(fig: plt.Figure, title: str) -> bool: + """ + Set the title of the window containing a figure, if it has one + + :param fig: the figure + :type fig: Figure + :param title: the window title + :type title: str + :return: True if the title was set + :rtype: bool + + A figure has a window only if it is managed by pyplot and the backend + provides one. A figure on a headless backend such as ``Agg``, or one + created directly with ``matplotlib.figure.Figure``, does not, and then + nothing is done and False is returned. + + Example:: + + >>> from spatialmath.base import plotvol2, set_window_title + >>> ax = plotvol2(5) + >>> set_window_title(ax.figure, "my plot") + + .. note:: The title of the window is not the title of the axes, see + :meth:`matplotlib.axes.Axes.set_title`. + """ + manager = getattr(fig.canvas, "manager", None) + if manager is not None and hasattr(manager, "set_window_title"): + manager.set_window_title(title) + return True + return False + def isnotebook() -> bool: """ Determine if code is being run from a Jupyter notebook @@ -1849,3 +1880,6 @@ def plot_cone(*args, **kwargs) -> None: def plot_cylinder(*args, **kwargs) -> None: raise NotImplementedError("Matplotlib is not installed: pip install matplotlib") + + def set_window_title(*args, **kwargs) -> None: + raise NotImplementedError("Matplotlib is not installed: pip install matplotlib") diff --git a/tests/base/test_graphics.py b/tests/base/test_graphics.py index 552ebdb0..2397ff9c 100644 --- a/tests/base/test_graphics.py +++ b/tests/base/test_graphics.py @@ -9,6 +9,39 @@ # TODO check they actually create artists +class TestWindowTitle(unittest.TestCase): + def teardown_method(self, method): + plt.close("all") + + def test_sets_the_title_of_a_managed_figure(self): + from types import SimpleNamespace + from unittest import mock + + manager = mock.Mock() + fig = SimpleNamespace(canvas=SimpleNamespace(manager=manager)) + self.assertTrue(set_window_title(fig, "a title")) + manager.set_window_title.assert_called_once_with("a title") + + def test_pyplot_figure(self): + # the Agg manager has no window, but does accept a title + fig = plt.figure() + self.assertIsInstance(set_window_title(fig, "a title"), bool) + + def test_figure_without_a_manager(self): + from matplotlib.figure import Figure + + self.assertFalse(set_window_title(Figure(), "a title")) + + def test_manager_without_the_method(self): + from types import SimpleNamespace + + class Manager: + pass + + fig = SimpleNamespace(canvas=SimpleNamespace(manager=Manager())) + self.assertFalse(set_window_title(fig, "a title")) + + class TestGraphics(unittest.TestCase): def teardown_method(self, method): plt.close("all") From 5fb7852b7a87381e71f4f97648a364595c3c21fa Mon Sep 17 00:00:00 2001 From: Prajval Arora Date: Thu, 15 Oct 2026 04:07:21 -0700 Subject: [PATCH 09/20] 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 10/20] 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 11/20] 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 12/20] 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 13/20] 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 14/20] 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 fb39007af94c9772d00f91953f67cbad652db893 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sat, 17 Oct 2026 10:29:43 +1000 Subject: [PATCH 15/20] 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 54fb3c895f039041e84f65cb76c044c03f47571e Mon Sep 17 00:00:00 2001 From: Huashuo Lei <139048865+wpyaoxueros@users.noreply.github.com> Date: Sun, 18 Oct 2026 03:51:59 +0800 Subject: [PATCH 16/20] Normalize the axis in UnitQuaternion.AngVec --- spatialmath/quaternion.py | 3 ++ tests/test_angvec_nonunit_axis.py | 52 +++++++++++++++++++++++++++++++ 2 files changed, 55 insertions(+) create mode 100644 tests/test_angvec_nonunit_axis.py diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 29d68de6..98bffcaa 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1551,6 +1551,9 @@ def AngVec( """ v = smb.getvector(v, 3) theta = smb.getunit(theta, unit, vector=False) + if theta == 0: + return cls() + v = smb.unitvec(v) return cls( s=math.cos(theta / 2), v=math.sin(theta / 2) * v, norm=False, check=False ) diff --git a/tests/test_angvec_nonunit_axis.py b/tests/test_angvec_nonunit_axis.py new file mode 100644 index 00000000..8eb3926b --- /dev/null +++ b/tests/test_angvec_nonunit_axis.py @@ -0,0 +1,52 @@ +"""Angle-axis construction preserves unit-quaternion and zero-angle contracts.""" + +import math +import unittest + +import numpy as np +from numpy.testing import assert_allclose + +from spatialmath import UnitQuaternion +from spatialmath.base import angvec2r + + +class TestAngVecNonunitAxis(unittest.TestCase): + def test_unit_norm(self): + assert_allclose(np.linalg.norm(UnitQuaternion.AngVec(0.5, [1, 2, 3]).vec), 1) + + def test_angle_round_trip(self): + theta, axis = UnitQuaternion.AngVec(0.5, [1, 2, 3]).angvec() + assert_allclose(theta, 0.5) + assert_allclose(axis, np.array([1, 2, 3]) / np.sqrt(14)) + + def test_rotation_matrix(self): + assert_allclose( + UnitQuaternion.AngVec(0.5, [1, 2, 3]).R, + angvec2r(0.5, [1, 2, 3]), + atol=1e-12, + ) + + def test_degrees(self): + assert_allclose( + UnitQuaternion.AngVec(90, [0, 0, 4], unit="deg").R, + angvec2r(math.pi / 2, [0, 0, 1]), + atol=1e-12, + ) + + def test_unit_axis_control(self): + assert_allclose( + UnitQuaternion.AngVec(0.5, [1, 0, 0]).vec, + [math.cos(0.25), math.sin(0.25), 0, 0], + ) + + def test_zero_angle_zero_axis(self): + assert_allclose(UnitQuaternion.AngVec(0, [0, 0, 0]).vec, [1, 0, 0, 0]) + + def test_zero_degrees_zero_axis(self): + assert_allclose( + UnitQuaternion.AngVec(0, [0, 0, 0], unit="deg").vec, [1, 0, 0, 0] + ) + + def test_nonzero_angle_requires_nonzero_axis(self): + with self.assertRaises(ValueError): + UnitQuaternion.AngVec(0.5, [0, 0, 0]) From 6940542e7c68dbe310cfecf80de182db8b220cd4 Mon Sep 17 00:00:00 2001 From: Huashuo Lei <139048865+wpyaoxueros@users.noreply.github.com> Date: Mon, 19 Oct 2026 12:07:14 +0800 Subject: [PATCH 17/20] Use the scalar zero tolerance for angle-axis quaternions --- spatialmath/quaternion.py | 7 ++++--- tests/test_angvec_nonunit_axis.py | 30 ++++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 98bffcaa..f788d738 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1544,14 +1544,15 @@ def AngVec( >>> print(UQ.AngVec(0, [1,0,0])) >>> print(UQ.AngVec(90, [1,0,0], unit='deg')) - .. note:: :math:`\theta = 0` the result in an identity quaternion, otherwise - ``V`` must have a finite length, ie. :math:`|V| > 0`. + .. note:: If :math:`\theta` is zero within the default tolerance of + :func:`~spatialmath.base.vectors.iszero`, the result is an identity + quaternion. Otherwise, ``v`` must have a nonzero norm. :seealso: :meth:`UnitQuaternion.angvec` :meth:`UnitQuaternion.exp` :func:`~spatialmath.base.transforms3d.angvec2r` """ v = smb.getvector(v, 3) theta = smb.getunit(theta, unit, vector=False) - if theta == 0: + if smb.iszero(theta): return cls() v = smb.unitvec(v) return cls( diff --git a/tests/test_angvec_nonunit_axis.py b/tests/test_angvec_nonunit_axis.py index 8eb3926b..22c748df 100644 --- a/tests/test_angvec_nonunit_axis.py +++ b/tests/test_angvec_nonunit_axis.py @@ -50,3 +50,33 @@ def test_zero_degrees_zero_axis(self): def test_nonzero_angle_requires_nonzero_axis(self): with self.assertRaises(ValueError): UnitQuaternion.AngVec(0.5, [0, 0, 0]) + + def test_near_zero_angle_zero_axis(self): + for theta in [1e-20, -1e-20]: + with self.subTest(theta=theta): + assert_allclose( + UnitQuaternion.AngVec(theta, [0, 0, 0]).vec, [1, 0, 0, 0] + ) + + def test_near_zero_degrees_zero_axis(self): + for theta in [1e-20, -1e-20]: + with self.subTest(theta=theta): + assert_allclose( + UnitQuaternion.AngVec(theta, [0, 0, 0], unit="deg").vec, + [1, 0, 0, 0], + ) + + def test_nonzero_small_angle_requires_nonzero_axis(self): + for theta in [1e-10, -1e-10]: + with self.subTest(theta=theta): + with self.assertRaises(ValueError): + UnitQuaternion.AngVec(theta, [0, 0, 0]) + + def test_nonzero_small_angle_preserves_rotation(self): + for theta in [1e-10, -1e-10]: + with self.subTest(theta=theta): + assert_allclose( + UnitQuaternion.AngVec(theta, [2, 0, 0]).vec, + [math.cos(theta / 2), math.sin(theta / 2), 0, 0], + atol=0, + ) From 67b280737b49857a250aeb05fdd39dd52e849a6e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 19 Oct 2026 14:40:38 +1000 Subject: [PATCH 18/20] fix(base): angvec2r rejects a zero axis for a non-zero angle angvec2r returned the identity whenever the axis norm was ~0, whatever the angle, so a non-zero rotation about no axis silently gave the identity (angvec2tr, SO3/SE3.AngleAxis and AngVec inherit this). Test the angle for zero instead, which is what the docstring already described for `tol`: a zero angle (within tol eps) returns the identity and ignores the axis; otherwise the axis is normalized with unitvec, which raises ValueError for a zero-length axis. Consistent with UnitQuaternion.AngVec in #266. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transforms3d.py | 21 +++++++++++++-------- tests/base/test_transforms3d.py | 28 ++++++++++++++++++++++++++++ tests/test_pose3d.py | 9 +++++++++ 3 files changed, 50 insertions(+), 8 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 95786b9c..e61e3dca 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -31,6 +31,7 @@ unitvec_norm, norm, isunitvec, + iszero, iszerovec, unittwist_norm, isunittwist, @@ -747,7 +748,7 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr :type: float :return: SO(3) rotation matrix :rtype: ndarray(3,3) - :raises ValueError: bad arguments + :raises ValueError: bad arguments, or a non-zero rotation about a zero-length axis ``angvec2r(θ, V)`` is an SO(3) orthonormal rotation matrix equivalent to a rotation of ``θ`` about the vector ``V``. @@ -760,8 +761,10 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr .. note:: - - If ``θ == 0`` then return identity matrix. - - If ``θ ~= 0`` then ``V`` must have a finite length. + - If ``θ`` is zero (within ``tol``) then return the identity matrix, and + ``V`` is ignored. + - Otherwise ``V`` must have a finite length, but need not be a unit + vector. A zero-length ``V`` raises ``ValueError``. :seealso: :func:`~angvec2tr` :func:`~tr2angvec` @@ -770,11 +773,11 @@ def angvec2r(theta: float, v: ArrayLike3, unit="rad", tol: float = 20) -> SO3Arr if not isscalar(theta) or not isvector(v, 3): raise ValueError("Arguments must be angle and vector") - if np.linalg.norm(v) < tol * _eps: - return np.eye(3) - θ = getunit(theta, unit, vector=False) + if iszero(θ, tol): + return np.eye(3) + # Rodrigue's equation sk = skew(cast(ArrayLike3, unitvec(v))) @@ -806,8 +809,10 @@ def angvec2tr(theta: float, v: ArrayLike3, unit="rad") -> SE3Array: .. note:: - - If ``θ == 0`` then return identity matrix. - - If ``θ ~= 0`` then ``V`` must have a finite length. + - If ``θ`` is zero (within ``tol``) then return the identity matrix, and + ``V`` is ignored. + - Otherwise ``V`` must have a finite length, but need not be a unit + vector. A zero-length ``V`` raises ``ValueError``. - The translational part is zero. :seealso: :func:`~angvec2r` :func:`~tr2angvec` diff --git a/tests/base/test_transforms3d.py b/tests/base/test_transforms3d.py index e9b9af14..88ca96a1 100755 --- a/tests/base/test_transforms3d.py +++ b/tests/base/test_transforms3d.py @@ -275,6 +275,26 @@ def test_angvec2r(self): nt.assert_array_almost_equal(angvec2r(pi / 4, [0, 0, 1]), rotz(pi / 4)) nt.assert_array_almost_equal(angvec2r(-pi / 4, [0, 0, 1]), rotz(-pi / 4)) + def test_angvec2r_nonunit_axis(self): + # the axis is normalized, so its length is irrelevant + nt.assert_array_almost_equal( + angvec2r(0.5, [1, 2, 3]), angvec2r(0.5, np.array([1, 2, 3]) / np.sqrt(14)) + ) + nt.assert_array_almost_equal(angvec2r(pi / 4, [0, 0, 5]), rotz(pi / 4)) + + def test_angvec2r_zero_axis(self): + # zero rotation about a zero axis is the identity, any angle units + nt.assert_array_almost_equal(angvec2r(0, [0, 0, 0]), np.eye(3)) + nt.assert_array_almost_equal(angvec2r(0, [0, 0, 0], unit="deg"), np.eye(3)) + + # a non-zero rotation about a zero axis is meaningless + with self.assertRaises(ValueError): + angvec2r(0.5, [0, 0, 0]) + with self.assertRaises(ValueError): + angvec2r(pi, [0, 0, 0]) + with self.assertRaises(ValueError): + angvec2r(90, [0, 0, 0], unit="deg") + def test_angvec2tr(self): r2d = 180 / pi @@ -290,6 +310,14 @@ def test_angvec2tr(self): nt.assert_array_almost_equal(angvec2tr(pi / 4, [0, 0, 1]), trotz(pi / 4)) nt.assert_array_almost_equal(angvec2tr(-pi / 4, [0, 0, 1]), trotz(-pi / 4)) + # non-unit axis is normalized + nt.assert_array_almost_equal(angvec2tr(pi / 4, [0, 0, 5]), trotz(pi / 4)) + + # zero rotation about a zero axis is the identity, otherwise an error + nt.assert_array_almost_equal(angvec2tr(0, [0, 0, 0]), np.eye(4)) + with self.assertRaises(ValueError): + angvec2tr(0.5, [0, 0, 0]) + r2d = 180 / pi nt.assert_array_almost_equal(angvec2r(0, [1, 0, 0]), rotx(0)) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 589c0ec1..d4b16668 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -241,6 +241,15 @@ def test_constructor_TwoVec(self): # x axis should equal normalized x vector nt.assert_almost_equal(R.R[:, 0], v3 / np.linalg.norm(v3), 5) + def test_AngleAxis_zero_axis(self): + nt.assert_array_almost_equal(SO3.AngleAxis(0, [0, 0, 0]).A, np.eye(3)) + nt.assert_array_almost_equal(SO3.AngVec(0, [0, 0, 0]).A, np.eye(3)) + nt.assert_array_almost_equal(SE3.AngleAxis(0, [0, 0, 0]).A, np.eye(4)) + with self.assertRaises(ValueError): + SO3.AngleAxis(0.5, [0, 0, 0]) + with self.assertRaises(ValueError): + SE3.AngVec(0.5, [0, 0, 0]) + def test_conversion(self): R = SO3.AngleAxis(0.7, [1, 2, 3]) q = UnitQuaternion([11, 7, 3, -6]) From d7e69da39407211641ec8de231745360b5a78441 Mon Sep 17 00:00:00 2001 From: Anton Soloviev Date: Mon, 12 Oct 2026 01:50:53 -0700 Subject: [PATCH 19/20] Broadcast scalar integration steps over twist sequences --- spatialmath/twist.py | 16 +++++++++--- tests/test_twist_sequence.py | 49 ++++++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+), 4 deletions(-) create mode 100644 tests/test_twist_sequence.py diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 4579ec29..c8862e4e 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -1014,7 +1014,7 @@ def SE3(self, theta=1, unit="rad"): if len(theta) == 1: # theta is a scalar - return SE3(smb.trexp(self.S * theta)) + return SE3([smb.trexp(S * theta[0]) for S in self.data]) else: # theta is a vector if len(self) == 1: @@ -1072,6 +1072,8 @@ def exp(self, theta=1, unit="rad"): if len(self) == 1: return SE3([smb.trexp(self.S * t) for t in theta], check=False) + elif len(theta) == 1: + return SE3([smb.trexp(s * theta[0]) for s in self.data], check=False) elif len(self) == len(theta): return SE3([smb.trexp(s * t) for s, t in zip(self.S, theta)], check=False) else: @@ -1530,15 +1532,19 @@ def SE2(self, theta=1, unit="rad"): """ from spatialmath.pose2d import SE2 - if unit != "rad" and self.isprismatic: + if unit != "rad" and any(s.isprismatic for s in self): print("Twist3.exp: using degree mode for a prismatic twist") theta = smb.getunit(theta, unit) if len(theta) == 1: - return SE2(smb.trexp2(self.S * theta)) - else: + return SE2([smb.trexp2(S * theta[0]) for S in self.data]) + elif len(self) == 1: return SE2([smb.trexp2(self.S * t) for t in theta]) + elif len(self) == len(theta): + return SE2([smb.trexp2(S * t) for S, t in zip(self.data, theta)]) + else: + raise ValueError("length of twist and theta not consistent") def skewa(self): """ @@ -1605,6 +1611,8 @@ def exp(self, theta=1, unit="rad"): if len(self) == 1: return SE2([smb.trexp2(self.S * t) for t in theta], check=False) + elif len(theta) == 1: + return SE2([smb.trexp2(s * theta[0]) for s in self.data], check=False) elif len(self) == len(theta): return SE2([smb.trexp2(s * t) for s, t in zip(self.S, theta)], check=False) else: diff --git a/tests/test_twist_sequence.py b/tests/test_twist_sequence.py new file mode 100644 index 00000000..7dcbee9a --- /dev/null +++ b/tests/test_twist_sequence.py @@ -0,0 +1,49 @@ +import numpy as np +import pytest +from scipy.linalg import expm + +from spatialmath import SE2, SE3, Twist2, Twist3 + + +@pytest.mark.parametrize("twist,pose", [(Twist2, SE2), (Twist3, SE3)]) +@pytest.mark.parametrize("method", ["exp", "pose"]) +@pytest.mark.parametrize("theta", [None, 0, -0.4, [0.2, -0.7]]) +def test_twist_sequence_exponential(twist, pose, method, theta): + """A shared step and individual steps both integrate every twist.""" + vectors = ( + [[1, -2, 0.3], [2, 1, -0.4]] + if twist is Twist2 + else [[1, -2, 3, 0.2, 0.3, -0.4], [2, 1, -1, -0.1, 0.5, 0.2]] + ) + sequence = twist([np.array(v) for v in vectors]) + convert = getattr(sequence, "exp" if method == "exp" else pose.__name__) + result = convert() if theta is None else convert(theta) + steps = np.broadcast_to(1 if theta is None else theta, (2,)) + assert isinstance(result, pose) + assert len(result) == 2 + for value, source, step in zip(result, sequence, steps): + np.testing.assert_allclose(value.A, expm(source.skewa() * step), atol=1e-14) + + +@pytest.mark.parametrize("twist,pose", [(Twist2, SE2), (Twist3, SE3)]) +@pytest.mark.parametrize("method", ["exp", "pose"]) +def test_twist_sequence_degrees_and_singleton(twist, pose, method): + """Degrees, singleton expansion and mismatched lengths retain their rules.""" + source = twist([1, 2, 0.3] if twist is Twist2 else [1, 2, 3, 0.2, 0.3, 0.4]) + name = "exp" if method == "exp" else pose.__name__ + angles = [15, -30] + expanded = getattr(source, name)(angles, unit="deg") + assert len(expanded) == 2 + for value, angle in zip(expanded, angles): + np.testing.assert_allclose( + value.A, expm(source.skewa() * np.deg2rad(angle)), atol=1e-14 + ) + sequence = twist([source, source]) + shared = getattr(sequence, name)(30, unit="deg") + assert len(shared) == 2 + for value in shared: + np.testing.assert_allclose( + value.A, expm(source.skewa() * np.deg2rad(30)), atol=1e-14 + ) + with pytest.raises(ValueError): + getattr(sequence, name)([0.1, 0.2, 0.3]) From 1e23a46547e1015fb6d27d6d40886a4b2a167d93 Mon Sep 17 00:00:00 2001 From: Anton Soloviev Date: Mon, 12 Oct 2026 01:50:53 -0700 Subject: [PATCH 20/20] Normalize twist poles by squared angular magnitude --- spatialmath/twist.py | 4 ++-- tests/test_twist_pole.py | 35 +++++++++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 2 deletions(-) create mode 100644 tests/test_twist_pole.py diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 4579ec29..c42965e6 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -986,7 +986,7 @@ def pole(self): >>> S = Twist3(T) >>> S.pole """ - return np.cross(self.w, self.v) / self.theta + return np.cross(self.w, self.v) / np.dot(self.w, self.w) def SE3(self, theta=1, unit="rad"): """ @@ -1500,7 +1500,7 @@ def pole(self): >>> S.pole """ - p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta + p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.w**2 return p[:2] # ------------------------- methods -------------------------------# diff --git a/tests/test_twist_pole.py b/tests/test_twist_pole.py new file mode 100644 index 00000000..8acc2e34 --- /dev/null +++ b/tests/test_twist_pole.py @@ -0,0 +1,35 @@ +import numpy as np +import pytest +from scipy.linalg import expm + +from spatialmath import Twist2, Twist3 + + +@pytest.mark.parametrize("speed", [-2.0, -0.3, 0.3, 1.0, 2.0]) +def test_twist2_pole_is_rotation_center(speed): + """A change in angular speed must not move the center of rotation.""" + center = np.array([3.0, -2.0]) + twist = Twist2(speed * np.r_[center[1], -center[0], 1]) + np.testing.assert_allclose(twist.pole, center) + transform = expm(twist.skewa() * 0.7) + np.testing.assert_allclose( + transform @ np.r_[twist.pole, 1], np.r_[center, 1], atol=1e-14 + ) + + +@pytest.mark.parametrize("speed", [-2.0, -0.3, 0.3, 1.0, 2.0]) +@pytest.mark.parametrize("pitch", [0, 0.4]) +def test_twist3_pole_is_on_screw_axis(speed, pitch): + """The closest axis point is fixed except for translation along the axis.""" + axis = np.array([1.0, 2.0, -2.0]) / 3 + center = np.array([3.0, -2.0, 1.0]) + center -= (center @ axis) * axis + angular = speed * axis + linear = -np.cross(angular, center) + pitch * angular + twist = Twist3(linear, angular) + np.testing.assert_allclose(twist.pole, center) + transform = expm(twist.skewa() * 0.7) + expected = center + pitch * angular * 0.7 + np.testing.assert_allclose( + (transform @ np.r_[twist.pole, 1])[:3], expected, atol=1e-14 + )