@@ -1492,25 +1492,21 @@ def tr2delta(T0, T1=None):
14921492 return np .r_ [transl (Td ), base .vex (base .t2r (Td ) - np .eye (3 ))]
14931493
14941494
1495- def tr2jac (T , samebody = False ):
1495+ def tr2jac (T ):
14961496 r"""
1497- SE(3) adjoint
1497+ SE(3) Jacobian matrix
14981498
14991499 :param T: SE(3) matrix
15001500 :type T: ndarray(4,4)
1501- :return: adjoint matrix
1501+ :return: Jacobian matrix
15021502 :rtype: ndarray(6,6)
1503- :raises ValueError: bad arguments
1504-
1505- Computes an adjoint matrix that maps spatial velocity between two frames defined by
1506- an SE(3) matrix. It is a Jacobian matrix.
15071503
1508- - ``tr2jac(T)`` is a Jacobian matrix (6x6) that maps spatial velocity or
1509- differential motion from frame {A} to frame {B} where the pose of {B}
1510- relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.
1504+ Computes an Jacobian matrix that maps spatial velocity between two frames defined by
1505+ an SE(3) matrix.
15111506
1512- - ``tr2jac(T, True)`` as above but for the case when frame {A} to frame {B} are both
1513- attached to the same moving body. This is the adjoint matrix
1507+ ``tr2jac(T)`` is a Jacobian matrix (6x6) that maps spatial velocity or
1508+ differential motion from frame {B} to frame {A} where the pose of {B}
1509+ elative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.
15141510
15151511 .. runblock:: pycon
15161512
@@ -1524,31 +1520,54 @@ def tr2jac(T, samebody=False):
15241520
15251521 if not ishom (T ):
15261522 raise ValueError ("expecting an SE(3) matrix" )
1523+
15271524 Z = np .zeros ((3 , 3 ), dtype = T .dtype )
1525+ R = base .t2r (T )
1526+ return np .block ([[R , Z ], [Z , R ]])
15281527
1529- if samebody :
1530- # Adjoint case
1531- (R , t ) = base .tr2rt (T )
1532- return np .block ([[R .T , (base .skew (t )@R ).T ], [Z , R .T ]])
1533- else :
1534- R = base .t2r (T )
1535- return np .block ([[R .T , Z ], [Z , R .T ]])
1528+ def tr2adjoint (T ):
1529+ r"""
1530+ SE(3) adjoint matrix
1531+
1532+ :param T: SE(3) matrix
1533+ :type T: ndarray(4,4)
1534+ :return: adjoint matrix
1535+ :rtype: ndarray(6,6)
1536+
1537+ Computes an adjoint matrix that maps spatial velocity between two frames defined by
1538+ an SE(3) matrix.
1539+
1540+ ``tr2jac(T)`` is an adjoint matrix (6x6) that maps spatial velocity or
1541+ differential motion between frame {B} to frame {A} which are attached to the
1542+ same moving body. The pose of {B} relative to {A} is represented by the
1543+ homogeneous transform T = :math:`{}^A {\bf T}_B`.
15361544
1537- def adjoint (T ):
1538- # http://ethaneade.com/lie.pdf
1545+ .. runblock:: pycon
1546+
1547+ >>> from spatialmath.base import *
1548+ >>> T = trotx(0.3, t=[4,5,6])
1549+ >>> tr2adjoint(T)
1550+
1551+ :Reference:
1552+ - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p65.
1553+ - `Lie groups for 2D and 3D Transformations <http://ethaneade.com/lie.pdf>_
1554+
1555+ :SymPy: supported
1556+ """
1557+
15391558 Z = np .zeros ((3 , 3 ), dtype = T .dtype )
15401559 if T .shape == (3 ,3 ):
15411560 # SO(3) adjoint
15421561 return np .block ([
1543- [R , Z ],
1544- [Z , R ]
1562+ [R , Z ],
1563+ [Z , R ]
15451564 ])
15461565 elif T .shape == (4 ,4 ):
15471566 # SE(3) adjoint
15481567 (R , t ) = base .tr2rt (T )
15491568 return np .block ([
1550- [R , base .skew (t ) @ R ],
1551- [Z , R ]
1569+ [R , base .skew (t ) @ R ],
1570+ [Z , R ]
15521571 ])
15531572 else :
15541573 raise ValueError ('bad argument' )
0 commit comments