Skip to content

Commit c728b07

Browse files
committed
add 2d tr2jac2
1 parent 26568b0 commit c728b07

2 files changed

Lines changed: 34 additions & 0 deletions

File tree

‎spatialmath/base/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@
5454
'isrot2',
5555
'trlog2',
5656
'trexp2',
57+
'tr2jac2',
5758
'trinterp2',
5859
'trprint2',
5960
'trplot2',

‎spatialmath/base/transforms2d.py‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -495,6 +495,39 @@ def adjoint2(T):
495495
else:
496496
raise ValueError('bad argument')
497497

498+
def tr2jac2(T):
499+
r"""
500+
SE(2) Jacobian matrix
501+
502+
:param T: SE(2) matrix
503+
:type T: ndarray(3,3)
504+
:return: Jacobian matrix
505+
:rtype: ndarray(3,3)
506+
507+
Computes an Jacobian matrix that maps spatial velocity between two frames defined by
508+
an SE(2) matrix.
509+
510+
``tr2jac2(T)`` is a Jacobian matrix (3x3) that maps spatial velocity or
511+
differential motion from frame {B} to frame {A} where the pose of {B}
512+
elative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.
513+
514+
.. runblock:: pycon
515+
516+
>>> from spatialmath.base import *
517+
>>> T = trot2(0.3, t=[4,5])
518+
>>> tr2jac2(T)
519+
520+
:Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p65.
521+
:SymPy: supported
522+
"""
523+
524+
if not ishom2(T):
525+
raise ValueError("expecting an SE(2) matrix")
526+
527+
J = np.eye(3, dtype=T.dtype)
528+
J[:2,:2] = base.t2r(T)
529+
return J
530+
498531
def trinterp2(start, end, s=None):
499532
"""
500533
Interpolate SE(2) or SO(2) matrices

0 commit comments

Comments
 (0)