@@ -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
16481679if __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