Пример #1
0
    def jacobe(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)

        ``jacobe(q)`` is the manipulator Jacobian matrix which maps joint
        velocity to end-effector spatial velocity.

        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.

        :seealso: :func:`jacob`, :func:`hessian0`
        """  # noqa

        if T is None:
            T = self.eval(q)

        return tr2jac(T.A) @ self.jacob0(q, T)
    def jacob(self):
        """
        Velocity transform for SE(3)

        :return: Jacobian matrix
        :rtype: numpy.ndarray, shape=(6,6)

        ``SE3.jacob()`` is the 6x6 Jacobian that maps spatial velocity or
        differential motion from frame {B} to frame {A} where the pose of {B}
        relative to {A} is represented by the homogeneous transform T =
        :math:`{}^A {\bf T}_B`.  
        
        .. note::
            - To map from frame {A} to frame {B} use the transpose of this matrix.
            - Use this method to map velocities between the robot end-effector frame
              and the base frames.

        .. warning:: Do not use this method to map velocities between two frames
            on the same rigid-body.

        :seealso: SE3.Ad, Twist.ad, :func:`~spatialmath.base.tr2jac`
        :Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p65.
        :SymPy: supported
        """
        return base.tr2jac(self.A)