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)