def inv(self):
r"""
Inverse of SE(3)
:return: inverse
:rtype: SE3 instance
Efficiently compute the inverse of each of the SE(3) values taking into
account the matrix structure.
.. math::
T = \left[ \begin{array}{cc} \mat{R} & \vec{t} \\ 0 & 1 \end{array} \right],
\mat{T}^{-1} = \left[ \begin{array}{cc} \mat{R}^T & -\mat{R}^T \vec{t} \\ 0 & 1 \end{array} \right]`
Example::
>>> x = SE3(1,2,3)
>>> x.inv()
SE3(array([[ 1., 0., 0., -1.],
[ 0., 1., 0., -2.],
[ 0., 0., 1., -3.],
[ 0., 0., 0., 1.]]))
:seealso: :func:`~spatialmath.base.transforms3d.trinv`
:SymPy: supported
"""
if len(self) == 1:
return SE3(base.trinv(self.A), check=False)
else:
return SE3([base.trinv(x) for x in self.A], check=False)
def inv(self):
r"""
Inverse of SE(3)
:return: inverse
:rtype: SE3
Returns the inverse taking into account its structure
:math:`T = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]`
:seealso: :func:`~spatialmath.base.transform3d.trinv`
"""
if len(self) == 1:
return SE3(tr.trinv(self.A))
else:
return SE3([tr.trinv(x) for x in self.A], check=False)
def inv(self):
r"""
Inverse of SE(3)
:param self: pose
:type self: SE3 instance
:return: inverse
:rtype: SE3
Returns the inverse taking into account its structure
:math:`T = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]`
"""
if len(self) == 1:
return SE3(tr.trinv(self.A))
else:
return SE3([tr.trinv(x) for x in self.A])
def inv(self):
r"""
Inverse of ETS
:return: [description]
:rtype: ETS instance
The inverse of a given ETS. It is computed as the inverse of the
individual ETs in the reverse order.
.. math::
(\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} )
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz(j=2) * ETS.tx(1) * ETS.rx(j=3,flip=True) * ETS.tx(1)
>>> print(e)
>>> print(e.inv())
>>> q = [1,2,3,4]
>>> print(e.eval(q) * e.inv().eval(q))
.. note:: It is essential to use explicit joint indices to account for
the reversed order of the transforms.
""" # noqa
inv = ETS()
for ns in reversed(self.data):
# get the namespace from the list
# clone it, and invert the elements to create an inverse
nsi = copy.copy(ns)
if nsi.joint:
nsi.flip ^= True # toggle flip status
elif nsi.axis[0] == 'C':
nsi.T = trinv(nsi.T)
elif nsi.eta is not None:
nsi.T = trinv(nsi.T)
nsi.eta = -nsi.eta
et = ETS() # create a new ETS instance
et.data = [nsi] # set it's data from the dict
inv *= et
return inv
def inv(self):
r"""
Inverse of ETS
:return: [description]
:rtype: ETS instance
The inverse of a given ETS. It is computed as the inverse of the
individual ETs in the reverse order.
.. math::
(\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} )
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz(j=2) * ETS.tx(1) * ETS.rx(j=3,flip=True) * ETS.tx(1)
>>> print(e)
>>> print(e.inv())
>>> q = [1,2,3,4]
>>> print(e.eval(q) * e.inv().eval(q))
.. note:: It is essential to use explicit joint indices to account for
the reversed order of the transforms.
"""
inv = ETS()
for e in reversed(self.data):
# get the named tuple from the list, and convert to a dict
etdict = e._asdict()
# update the dict to make this an inverse
if etdict['joint']:
etdict['flip'] ^= True # toggle flip status
elif etdict['axis'][0] == 'C':
etdict['T'] = trinv(etdict['T'])
elif etdict['eta'] is not None:
etdict['T'] = trinv(etdict['T'])
etdict['eta'] = -etdict['eta']
et = ETS() # create a new ETS instance
et.data = [self.ets_tuple(**etdict)] # set it's data from the dict
inv *= et
return inv
def cost(q, *args):
T, omega = args
E = (base.trinv(T) @ self.fkine(q).A - np.eye(4)) @ omega
return (E**2).sum() # quicker than np.sum(E**2)
def jacob0(self, q=None, T=None):
r"""
Jacobian in base frame
:param q: joint coordinates
:type q: array_like
:param T: ETS value as an SE(3) matrix if known
:type T: ndarray(4,4)
:return: Jacobian matrix
:rtype: ndarray(6,n)
``jacob0(q)`` is the ETS Jacobian matrix which maps joint
velocity to spatial velocity in the {0} frame.
End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x,
\omega_y, \omega_z)^T` is related to joint velocity by
:math:`{}^{e}\nu = {}^{e}\mathbf{J}_0(q) \dot{q}`.
If ``ets.eval(q)`` is already computed it can be passed in as ``T`` to
reduce computation time.
An ETS represents the relative pose from the {0} frame to the end frame
{e}. This is the composition of many relative poses, some constant and
some functions of the joint variables, which we can write as
:math:`\mathbf{E}(q)`.
.. math::
{}^0 T_e = \mathbf{E}(q) \in \mbox{SE}(3)
The temporal derivative of this is the spatial
velocity :math:`\nu` which is a 6-vector is related to the rate of
change of joint coordinates by the Jacobian matrix.
.. math::
{}^0 \nu = {}^0 \mathbf{J}(q) \dot{q} \in \mathbb{R}^6
This velocity can be expressed relative to the {0} frame or the {e}
frame.
:references:
- `Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke <https://arxiv.org/abs/2010.08696>`_
:seealso: :func:`jacobe`, :func:`hessian0`
""" # noqa
# TODO what is offset
# if offset is None:
# offset = SE3()
n = self.n # number of joints
q = getvector(q, n)
if T is None:
T = self.eval(q)
# we will work with NumPy arrays for maximum speed
T = T.A
U = np.eye(4) # SE(3) matrix
j = 0
J = np.zeros((6, n))
for et in self:
if et.isjoint:
# joint variable
# U = U @ link.A(q[j], fast=True)
U = U @ et.T(q[j])
# TODO???
# if link == to_link:
# U = U @ offset.A
Tu = trinv(U) @ T
n = U[:3, 0]
o = U[:3, 1]
a = U[:3, 2]
x = Tu[0, 3]
y = Tu[1, 3]
z = Tu[2, 3]
if et.axis == 'Rz':
J[:3, j] = (o * x) - (n * y)
J[3:, j] = a
elif et.axis == 'Ry':
J[:3, j] = (n * z) - (a * x)
J[3:, j] = o
elif et.axis == 'Rx':
J[:3, j] = (a * y) - (o * z)
J[3:, j] = n
elif et.axis == 'tx':
J[:3, j] = n
J[3:, j] = np.array([0, 0, 0])
elif et.axis == 'ty':
J[:3, j] = o
J[3:, j] = np.array([0, 0, 0])
elif et.axis == 'tz':
J[:3, j] = a
J[3:, j] = np.array([0, 0, 0])
j += 1
else:
# constant transform
U = U @ et.T()
return J
def _inverse(self, T):
return trinv(T)
