From faa90dd0c481489839d450530bb1b2adb3b32518 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jan 2025 14:28:45 +1000 Subject: [PATCH 1/2] feat(pose,quaternion): add __imatmul__ (@=) operator `X @= Y` now compounds poses/quaternions in place with automatic normalization, mirroring the existing `X @ Y` (__matmul__) behaviour but writing the normalized result back into the left operand instead of returning a new one. Useful when a pose is updated incrementally over many cycles and you don't want to pay for a fresh object each time. Added to BasePoseMatrix (covers SO2/SE2/SO3/SE3) and Quaternion (covers Quaternion/UnitQuaternion). --- spatialmath/baseposematrix.py | 14 ++++++++++++++ spatialmath/quaternion.py | 28 ++++++++++++++++++++++++++-- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index 87071df3..9d4d49cc 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -1350,6 +1350,20 @@ def __imul__(left, right): # noqa """ return left.__mul__(right) + def __imatmul__(left, right): # noqa + """ + Overloaded ``@=`` operator (superclass method) + + :return: Product of two operands with normalization + :rtype: Pose instance or NumPy array + :raises ValueError: for incompatible arguments + + - ``X @= Y`` compounds the poses ``X`` and ``Y`` and places the normalized result in ``X`` + + :seealso: ``__imul__`` :meth:`__matmul__` + """ + return left.__matmul__(right) + def __truediv__(left, right): # pylint: disable=no-self-argument """ Overloaded ``/`` operator (superclass method) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 2994b6e6..e275cc7c 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -666,6 +666,32 @@ def __imul__( """ return left.__mul__(right) + def __imatmul__( + left, right: Quaternion + ) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument + """ + Overloaded ``@=`` operator + + :return: product + :rtype: Quaternion + :raises: ValueError + + ``q1 @= q2`` sets ``q1 := qnorm(q1 * q2`)` + + Example: + + .. runblock:: pycon + + >>> from spatialmath import Quaternion + >>> q = UnitQuaternion.Eul([0.1, 0.2, 0.3]) + >>> q @= Quaternion.Eul([0.3, 0.4, 0.5]) + >>> print(q) + + + :seealso: :func:`__mul__` + """ + return left.__mul__(right) + def __pow__(self, n: int) -> Quaternion: """ Overloaded ``**`` operator @@ -1883,8 +1909,6 @@ def __matmul__( - ``q1 @ q2`` is the Hamilton product of ``q1`` and ``q2``, both unit quaternions, followed by explicit normalization. - - `` q1 @= q2`` as above. - .. note:: This operator is functionally equivalent to ``*`` but is more costly. It is useful for cases where a pose is incrementally update over many cycles. From 766232c99152b48f2a9ca5356d2d605db5755ad5 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 27 Jul 2026 15:36:06 +1000 Subject: [PATCH 2/2] fix(quaternion): make __imatmul__ actually normalize Quaternion.__imatmul__'s own docstring claimed `q1 @= q2` sets `q1 := qnorm(q1 * q2)`, but the implementation just delegated to __mul__ - identical to plain *=, no normalization at all, contradicting both the docstring and the entire point of adding a separate @= operator. UnitQuaternion.__matmul__ (pre-existing, unchanged) already does this correctly via smb.qunit(smb.qqmul(x, y)) - qunit being the normalizer; qnorm just returns the scalar magnitude, so it was never actually the right function despite the docstring's wording. Fixed by having __imatmul__ delegate to `left @ right` instead of left.__mul__(right). Deliberately not left.__matmul__(right): plain Quaternion has no __matmul__ (only UnitQuaternion defines one, with normalization), and calling the dunder directly as a plain attribute bypasses Python's normal operator fallback, raising a confusing AttributeError instead of the same TypeError `q1 @ q2` already raises for plain Quaternion. `left @ right` matches @'s behaviour exactly in both cases: normalizes for UnitQuaternion, raises consistently for Quaternion. Also fixed the docstring's `-> bool` return type (should be `-> Quaternion`) and its example, which used Quaternion.Eul() - a method that only exists on UnitQuaternion. Tests: added @= coverage for UnitQuaternion (must match @, not *) and for plain Quaternion (must raise TypeError, matching @, not silently degrade to *=). --- spatialmath/quaternion.py | 20 ++++++++++++++------ tests/test_pose3d.py | 12 ++++++++++++ tests/test_quaternion.py | 13 +++++++++++++ 3 files changed, 39 insertions(+), 6 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index e275cc7c..9093f5f6 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -668,7 +668,7 @@ def __imul__( def __imatmul__( left, right: Quaternion - ) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument + ) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument """ Overloaded ``@=`` operator @@ -676,21 +676,29 @@ def __imatmul__( :rtype: Quaternion :raises: ValueError - ``q1 @= q2`` sets ``q1 := qnorm(q1 * q2`)` + ``q1 @= q2`` sets ``q1 := qnorm(q1 * q2)``. Only meaningful for + ``UnitQuaternion``, which is the only subclass defining ``__matmul__`` + (with normalization) that this delegates to; on a plain ``Quaternion`` + this raises the same ``TypeError`` that ``q1 @ q2`` would. Example: .. runblock:: pycon - >>> from spatialmath import Quaternion + >>> from spatialmath import UnitQuaternion >>> q = UnitQuaternion.Eul([0.1, 0.2, 0.3]) - >>> q @= Quaternion.Eul([0.3, 0.4, 0.5]) + >>> q @= UnitQuaternion.Eul([0.3, 0.4, 0.5]) >>> print(q) - :seealso: :func:`__mul__` + :seealso: :func:`__matmul__` """ - return left.__mul__(right) + # NOT left.__matmul__(right): Quaternion itself has no __matmul__ + # (only UnitQuaternion defines one), and calling the dunder + # directly as a plain attribute skips Python's normal operator + # fallback, raising a confusing AttributeError instead of the + # TypeError that `left @ right` raises consistently. + return left @ right def __pow__(self, n: int) -> Quaternion: """ diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index cdb80fbd..40b0a914 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -419,6 +419,11 @@ def test_arith(self): self.assertIsInstance(R, SO3) array_compare(R, rotx(pi / 2)) + R = SO3() + R @= SO3.Rx(pi / 2) + self.assertIsInstance(R, SO3) + array_compare(R, rotx(pi / 2)) + R = SO3() R *= 2 self.assertNotIsInstance(R, SO3) @@ -1078,6 +1083,13 @@ def test_arith(self): T, np.array([[0, 0, 1, 1], [0, 1, 0, 2], [-1, 0, 0, 3], [0, 0, 0, 1]]) ) + T = SE3(1, 2, 3) + T @= SE3.Ry(pi / 2) + self.assertIsInstance(T, SE3) + array_compare( + T, np.array([[0, 0, 1, 1], [0, 1, 0, 2], [-1, 0, 0, 3], [0, 0, 0, 1]]) + ) + T = SE3() T *= 2 self.assertNotIsInstance(T, SE3) diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 75d31b7c..24c264ef 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -507,6 +507,11 @@ def test_matmul(self): UnitQuaternion([ry * rx, rz * ry, rx * rz]), ) + # @= is @ as an augmented assignment, not *= + q = rx + q @= ry + qcompare(q, rx @ ry) + # def multiply_test_normalized(self): # vx = [1, 0, 0]; vy = [0, 1, 0]; vz = [0, 0, 1] @@ -949,6 +954,14 @@ def test_multiply(self): q *= q2 qcompare(q, [-12, 6, 24, 12]) + # plain Quaternion has no @ (only UnitQuaternion normalizes via @), + # so @= must fail the same way @ does, not silently fall back to *= + with self.assertRaises(TypeError): + q1 @ q2 + with self.assertRaises(TypeError): + q = q1 + q @= q2 + # vector x vector qcompare( Quaternion([q1, u, q2, u, q3, u]) * Quaternion([u, q1, u, q2, u, q3]),