Skip to content

Commit 89f226b

Browse files
committed
tr2jac now returns transpose BREAKING CHANGE
samebody option gone, use tr2adjoint instead adjoint changed to tr2adjoint
1 parent 407651e commit 89f226b

1 file changed

Lines changed: 44 additions & 25 deletions

File tree

‎spatialmath/base/transforms3d.py‎

Lines changed: 44 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)