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..9093f5f6 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -666,6 +666,40 @@ def __imul__( """ return left.__mul__(right) + def __imatmul__( + left, right: Quaternion + ) -> Quaternion: # 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)``. 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 UnitQuaternion + >>> q = UnitQuaternion.Eul([0.1, 0.2, 0.3]) + >>> q @= UnitQuaternion.Eul([0.3, 0.4, 0.5]) + >>> print(q) + + + :seealso: :func:`__matmul__` + """ + # 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: """ Overloaded ``**`` operator @@ -1883,8 +1917,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. 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]),