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)