fix(robot): improve jacob0_dot docstring, typing overloads, and representation error handling

src/roboticstoolbox/robot/Robot.py

@@ -1092,16 +1092,26 @@ def jacob0_dot(
         representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None,
     ):
         r"""
-        Derivative of Jacobian
+        Time derivative of manipulator Jacobian in frame {0}
 
-        :param q: The joint configuration of the robot
+        :param q: The joint configuration of the robot. Must be provided if
+            ``representation`` is specified; may be omitted if ``J0`` is
+            provided and ``representation`` is not.
+        :type q: ArrayLike(n), optional
         :param qd: The joint velocity of the robot
-        :param J0: Jacobian in {0} frame
-        :param representation: angular representation
-        :returns: The derivative of the manipulator Jacobian
-
-        ``robot.jacob_dot(q, qd)`` computes the rate of change of the
-        Jacobian elements
+        :type qd: ArrayLike(n)
+        :param J0: Jacobian in {0} frame. Ignored if ``representation`` is
+            specified; may be provided to save computation if
+            ``representation`` is not specified.
+        :type J0: ndarray(6,n), optional
+        :param representation: Angular representation for analytical Jacobian
+            ('rpy/xyz', 'rpy/zyx', 'eul', or 'exp')
+        :type representation: str, optional
+        :returns: The time derivative of the manipulator Jacobian
+        :rtype: ndarray(6,n)
+
+        ``robot.jacob0_dot(q, qd)`` computes the rate of change of the
+        manipulator Jacobian elements
 
         .. math::
 
@@ -1110,10 +1120,15 @@ def jacob0_dot(
         where the first term is the rank-3 Hessian.
 
         If ``J0`` is already calculated for the joint
-        coordinates ``q`` it can be passed in to to save computation time.
+        coordinates ``q`` it can be passed in to save computation time.
+
+        For the geometric Jacobian (``representation=None``), it is computed
+        analytically as the mode-3 product of the Hessian tensor and the joint
+        velocity vector:
 
-        It is computed as the mode-3 product of the Hessian tensor and the
-        velocity vector.
+        .. math::
+
+            \dmat{J}_0 = \mathcal{H}_0 \times_3 \dot{\vec{q}}
 
         The derivative of an analytical Jacobian can be obtained by setting
         ``representation`` as
@@ -1132,15 +1147,60 @@ def jacob0_dot(
            * - ``'exp'``
              - exponential coordinate rates
 
+        When ``representation`` is specified, the Hessian of the analytical
+        Jacobian is evaluated numerically using finite differences via
+        :func:`spatialmath.base.numhess` and contracted with ``qd``. This is
+        an approximate, numerical calculation and is slower than the closed-form
+        geometric Jacobian derivative.
+
+        .. list-table:: Parameter interaction between ``representation``, ``q``, and ``J0``
+           :header-rows: 1
+
+           * - ``representation``
+             - ``q``
+             - ``J0``
+             - Behavior
+           * - Omitted / ``None``
+             - Provided
+             - Omitted / ``None``
+             - Geometric Jacobian: computes :math:`\mat{J}_0` and Hessian :math:`\mathcal{H}_0` from ``q``, then :math:`\dmat{J}_0 = \mathcal{H}_0 \times_3 \dot{\vec{q}}`.
+           * - Omitted / ``None``
+             - Omitted (or ``None``)
+             - Provided
+             - Geometric Jacobian: reuses precomputed ``J0`` to compute Hessian :math:`\mathcal{H}_0`, then :math:`\dmat{J}_0 = \mathcal{H}_0 \times_3 \dot{\vec{q}}`.
+           * - Specified (e.g. ``'rpy/xyz'``)
+             - Provided
+             - Ignored
+             - Analytical Jacobian: evaluates numerical Hessian via :func:`spatialmath.base.numhess` at ``q`` and contracts with ``qd``.
+           * - Specified (e.g. ``'rpy/xyz'``)
+             - Omitted (or ``None``)
+             - Any
+             - Raises :class:`ValueError` (``q`` is required to evaluate the analytical Jacobian).
+
         .. rubric:: References
 
         - Kinematic Derivatives using the Elementary Transform
-            Sequence, J. Haviland and P. Corke
+          Sequence, J. Haviland and P. Corke
+
+        .. seealso:: :func:`jacob0`, :func:`hessian0`
 
-        See Also
-        --------
-        :func:`jacob0`
-        :func:`hessian0`
+        .. rubric:: Examples
+
+        .. runblock:: pycon
+
+            >>> import roboticstoolbox as rtb
+            >>> robot = rtb.models.DH.Puma560()
+            >>> q = robot.qn
+            >>> qd = [0.1, -0.2, 0.3, -0.1, 0.2, -0.3]
+            >>> j0_dot = robot.jacob0_dot(q, qd)
+            >>> j0_dot.shape
+            (6, 6)
+            >>> # If J0 is already computed, it can be passed in directly:
+            >>> J0 = robot.jacob0(q)
+            >>> j0_dot_fast = robot.jacob0_dot(None, qd, J0=J0)
+            >>> import numpy as np
+            >>> np.allclose(j0_dot, j0_dot_fast)
+            True
 
         """
 
@@ -1152,33 +1212,14 @@ def jacob0_dot(
             H = self.hessian0(q, J0=J0)
 
         else:
-            # # determine analytic rotation
-            # T = self.fkine(q).A
-            # gamma = smb.r2x(smb.t2r(T), representation=representation)
-
-            # # get transformation angular velocity to analytic velocity
-            # Ai = smb.rotvelxform(
-            #     gamma, representation=representation, inverse=True, full=True
-            # )
-
-            # # get analytic rate from joint rates
-            # omega = J0[3:, :] @ qd
-            # gamma_dot = Ai[3:, 3:] @ omega
-            # Ai_dot = smb.rotvelxform_inv_dot(gamma, gamma_dot, full=True)
-            # Ai_dot = sp.linalg.block_diag(np.zeros((3, 3)), Ai_dot)
-
-            # Jd = Ai_dot @ J0 + Ai @ Jd
-
-            # not actually sure this can be written in closed form
+            if q is None:
+                raise ValueError("q must be provided when representation is specified")
 
+            # Numerical Hessian of the analytical Jacobian
             H = smb.numhess(
                 lambda q: self.jacob0_analytical(q, representation=representation), q
             )
 
-            # Jd = Ai @ Jd
-
-            # return Jd
-
         return np.tensordot(H, qd, (0, 0))
 
     @overload

tests/test_jacob.py

@@ -80,21 +80,152 @@ def test_jacob_dot(self):
 
         nt.assert_array_almost_equal(j0, Jd, decimal=4)
 
-    def test_jacob_dot_analytical_eul(self):
-        rep = "eul"
-        j0 = self.robot.jacob0_dot(self.q, self.qd, representation=rep)
+    def test_jacob_dot_precomputed_j0(self):
+        J0 = self.robot.jacob0(self.q)
+        j0_direct = self.robot.jacob0_dot(self.q, self.qd)
+        j0_precomputed = self.robot.jacob0_dot(None, self.qd, J0=J0)
+        nt.assert_array_almost_equal(j0_direct, j0_precomputed)
+
+    def test_jacob0_dot_planar2_handworked_and_sympy(self):
+        """
+        Verify jacob0, hessian0, and jacob0_dot against both handworked
+        analytical ground truth and independently derived SymPy symbolic equations
+        for a 2-link planar RR manipulator.
+        """
+        import sympy as sp
+
+        robot = rtb.models.DH.Planar2()
+
+        # Symbolic derivation from end-effector forward kinematics in {0}
+        q1_s, q2_s, qd1_s, qd2_s = sp.symbols("q1 q2 qd1 qd2")
+        a1_s, a2_s = 1.0, 1.0
+        px_s = a1_s * sp.cos(q1_s) + a2_s * sp.cos(q1_s + q2_s)
+        py_s = a1_s * sp.sin(q1_s) + a2_s * sp.sin(q1_s + q2_s)
+
+        # Geometric Jacobian matrix (6x2): linear velocity (px, py, pz=0) and angular velocity (wz)
+        J_sym = sp.Matrix(
+            [
+                [sp.diff(px_s, q1_s), sp.diff(px_s, q2_s)],
+                [sp.diff(py_s, q1_s), sp.diff(py_s, q2_s)],
+                [0, 0],
+                [0, 0],
+                [0, 0],
+                [1, 1],
+            ]
+        )
 
-        H = numhess(
-            lambda q: self.robot.jacob0_analytical(q, representation=rep), self.q
+        # Symbolic Hessian tensor slices dJ/dq1 and dJ/dq2
+        H_sym_0 = sp.Matrix(
+            [[sp.diff(J_sym[r, c], q1_s) for c in range(2)] for r in range(6)]
+        )
+        H_sym_1 = sp.Matrix(
+            [[sp.diff(J_sym[r, c], q2_s) for c in range(2)] for r in range(6)]
         )
-        Jd = np.zeros((6, self.robot.n))
-        for i in range(self.robot.n):
-            Jd += H[i, :, :] * self.qd[i]
 
-        print(np.round(j0, 2))
-        print()
-        print(np.round(Jd, 2))
-        nt.assert_array_almost_equal(j0, Jd, decimal=4)
+        # Symbolic time derivative of Jacobian: dJ/dt = (dJ/dq1)*qd1 + (dJ/dq2)*qd2
+        Jdot_sym = H_sym_0 * qd1_s + H_sym_1 * qd2_s
+
+        test_cases = [
+            (np.array([0.0, 0.0]), np.array([1.0, 0.5])),
+            (np.array([np.pi / 4, -np.pi / 3]), np.array([-0.5, 1.2])),
+            (np.array([0.3, 0.7]), np.array([1.5, -2.0])),
+            (np.array([-1.2, 2.1]), np.array([0.8, -0.4])),
+            (np.array([np.pi / 2, np.pi / 2]), np.array([0.0, 2.5])),
+        ]
+
+        a1 = 1.0
+        a2 = 1.0
+
+        for q, qd in test_cases:
+            q1, q2 = q
+            qd1, qd2 = qd
+            s1, c1 = np.sin(q1), np.cos(q1)
+            s12, c12 = np.sin(q1 + q2), np.cos(q1 + q2)
+
+            # --- Handworked Analytical Equations ---
+            # Geometric Jacobian J0 (6x2)
+            J_hand = np.zeros((6, 2))
+            J_hand[0, 0] = -a1 * s1 - a2 * s12
+            J_hand[0, 1] = -a2 * s12
+            J_hand[1, 0] = a1 * c1 + a2 * c12
+            J_hand[1, 1] = a2 * c12
+            J_hand[5, 0] = 1.0
+            J_hand[5, 1] = 1.0
+
+            # Geometric Hessian H0 (2x6x2)
+            H_hand = np.zeros((2, 6, 2))
+            # dJ/dq1
+            H_hand[0, 0, 0] = -a1 * c1 - a2 * c12
+            H_hand[0, 0, 1] = -a2 * c12
+            H_hand[0, 1, 0] = -a1 * s1 - a2 * s12
+            H_hand[0, 1, 1] = -a2 * s12
+            # dJ/dq2
+            H_hand[1, 0, 0] = -a2 * c12
+            H_hand[1, 0, 1] = -a2 * c12
+            H_hand[1, 1, 0] = -a2 * s12
+            H_hand[1, 1, 1] = -a2 * s12
+
+            # Time derivative of Jacobian: j0_dot (6x2)
+            j0_dot_hand = np.zeros((6, 2))
+            j0_dot_hand[0, 0] = -a1 * c1 * qd1 - a2 * c12 * (qd1 + qd2)
+            j0_dot_hand[0, 1] = -a2 * c12 * (qd1 + qd2)
+            j0_dot_hand[1, 0] = -a1 * s1 * qd1 - a2 * s12 * (qd1 + qd2)
+            j0_dot_hand[1, 1] = -a2 * s12 * (qd1 + qd2)
+
+            # --- SymPy Numerical Evaluation ---
+            subs = {q1_s: q1, q2_s: q2, qd1_s: qd1, qd2_s: qd2}
+            J_sp = np.array(J_sym.evalf(subs=subs), dtype=float)
+            H_sp = np.stack(
+                [
+                    np.array(H_sym_0.evalf(subs=subs), dtype=float),
+                    np.array(H_sym_1.evalf(subs=subs), dtype=float),
+                ],
+                axis=0,
+            )
+            j0_dot_sp = np.array(Jdot_sym.evalf(subs=subs), dtype=float)
+
+            # --- RTB Computed Quantities ---
+            J_rtb = robot.jacob0(q)
+            H_rtb = robot.hessian0(q)
+            j0_dot_rtb = robot.jacob0_dot(q, qd)
+            j0_dot_rtb_precomputed = robot.jacob0_dot(None, qd, J0=J_rtb)
+
+            # --- Ground-Truth Assertions ---
+            # 1. Jacobian matches handworked & sympy
+            nt.assert_allclose(J_rtb, J_hand, atol=1e-12)
+            nt.assert_allclose(J_rtb, J_sp, atol=1e-12)
+
+            # 2. Hessian matches handworked & sympy
+            nt.assert_allclose(H_rtb, H_hand, atol=1e-12)
+            nt.assert_allclose(H_rtb, H_sp, atol=1e-12)
+
+            # 3. Time derivative of Jacobian matches handworked & sympy
+            nt.assert_allclose(j0_dot_rtb, j0_dot_hand, atol=1e-12)
+            nt.assert_allclose(j0_dot_rtb, j0_dot_sp, atol=1e-12)
+
+            # 4. Precomputed J0 path gives identical result
+            nt.assert_allclose(j0_dot_rtb_precomputed, j0_dot_hand, atol=1e-12)
+
+    def test_jacob_dot_analytical_all_representations(self):
+        for rep in ["rpy/xyz", "rpy/zyx", "eul", "exp"]:
+            j0 = self.robot.jacob0_dot(self.q, self.qd, representation=rep)
+
+            H = numhess(
+                lambda q, rep=rep: self.robot.jacob0_analytical(q, representation=rep),
+                self.q,
+            )
+            Jd = np.zeros((6, self.robot.n))
+            for i in range(self.robot.n):
+                Jd += H[i, :, :] * self.qd[i]
+
+            nt.assert_array_almost_equal(j0, Jd, decimal=4)
+
+    def test_jacob_dot_representation_q_none_raises(self):
+        J0 = self.robot.jacob0(self.q)
+        with pytest.raises(
+            ValueError, match="q must be provided when representation is specified"
+        ):
+            self.robot.jacob0_dot(None, self.qd, J0=J0, representation="eul")
 
     # ------ This section tests various ets' with flipped joints ------ #