Skip to content

Commit edc303f

Browse files
committed
add conjugation method, allow posematrix to be premultiplied by conforming matrix
1 parent 1834e99 commit edc303f

1 file changed

Lines changed: 58 additions & 20 deletions

File tree

‎spatialmath/baseposematrix.py‎

Lines changed: 58 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -594,6 +594,34 @@ def stack(self) -> NDArray:
594594
"""
595595
return np.dstack(self.data)
596596

597+
def conjugation(self, A: NDArray) -> NDArray:
598+
"""
599+
Matrix conjugation
600+
601+
:param A: matrix to conjugate
602+
:type A: ndarray
603+
:return: conjugated matrix
604+
:rtype: ndarray
605+
606+
Compute the conjugation :math:`\mat{X} \mat{A} \mat{X}^{-1}` where :math:`\mat{X}`
607+
is the current object.
608+
609+
Example:
610+
611+
.. runblock:: pycon
612+
613+
>>> from spatialmath import SO2
614+
>>> import numpy as np
615+
>>> R = SO2(0.5)
616+
>>> A = np.array([[10, 0], [0, 1]])
617+
>>> print(R * A * R.inv())
618+
>>> print(R.conjugation(A))
619+
"""
620+
if self.isSO:
621+
return self.A @ A @ self.A.T
622+
else:
623+
return self.A @ A @ self.inv().A.T
624+
597625
# ----------------------- i/o stuff
598626

599627
def print(self, label: Optional[str] = None, file: Optional[TextIO] = None) -> None:
@@ -1263,23 +1291,26 @@ def __rmul__(right, left): # pylint: disable=no-self-argument
12631291
:rtype: Pose instance or NumPy array
12641292
:raises NotImplemented: for incompatible arguments
12651293
1266-
Left-multiplication by a scalar
1267-
1268-
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
1269-
1270-
Notes:
1294+
Left-multiplication
12711295
1272-
#. For other left-operands return ``NotImplemented``. Other classes
1273-
such as ``Plucker`` and ``Twist`` implement left-multiplication by
1274-
an ``SE3`` using their own ``__rmul__`` methods.
1296+
- ``s * X`` where ``s`` is a scalar, performs elementwise multiplication of the
1297+
elements of ``X`` by ``s`` and the result is a NumPy array.
1298+
- ``A * X`` where ``A`` is a conforming matrix, performs matrix multiplication
1299+
of ``A`` and ``X`` and the result is a NumPy array.
12751300
12761301
:seealso: :func:`__mul__`
12771302
"""
1278-
# if base.isscalar(left):
1279-
# return right.__mul__(left)
1280-
# else:
1281-
# return NotImplemented
1282-
return right.__mul__(left)
1303+
if isinstance(left, np.ndarray) and left.shape[-1] == right.A.shape[0]:
1304+
# left multiply by conforming matrix
1305+
return left @ right.A
1306+
elif smb.isscalar(left):
1307+
# left multiply by scalar
1308+
return right.__mul__(left)
1309+
else:
1310+
# For other left-operands return ``NotImplemented``. Other classes
1311+
# such as ``Plucker`` and ``Twist`` implement left-multiplication by
1312+
# an ``SE3`` using their own ``__rmul__`` methods.
1313+
return NotImplemented
12831314

12841315
def __imul__(left, right): # noqa
12851316
"""
@@ -1646,13 +1677,20 @@ def _op2(left, right: Self, op: Callable): # pylint: disable=no-self-argument
16461677

16471678

16481679
if __name__ == "__main__":
1649-
from spatialmath import SE3, SE2
1680+
from spatialmath import SE3, SE2, SO2
1681+
1682+
C = SO2(0.5)
1683+
A = np.array([[10, 0], [0, 1]])
1684+
1685+
print(C * A)
1686+
print(C * A * C.inv())
1687+
print(C.conjugation(A))
16501688

1651-
x = SE3.Rand(N=6)
1689+
# x = SE3.Rand(N=6)
16521690

1653-
x.printline(orient="rpy/xyz", fmt="{:8.3g}")
1691+
# x.printline(orient="rpy/xyz", fmt="{:8.3g}")
16541692

1655-
d = np.diag([0.25, 0.25, 1])
1656-
a = SE2()
1657-
print(a)
1658-
print(d * a)
1693+
# d = np.diag([0.25, 0.25, 1])
1694+
# a = SE2()
1695+
# print(a)
1696+
# print(d * a)

0 commit comments

Comments
 (0)