From 354f4656d46466deabc7032c158c52c2dbe1831a Mon Sep 17 00:00:00 2001 From: paneendrakumar0 Date: Sun, 11 Oct 2026 22:49:57 +0530 Subject: [PATCH 1/2] fix(robot): improve jacob0_dot docstring, typing overloads, and representation error handling Fixes #734 - Correct method name reference from robot.jacob_dot to robot.jacob0_dot - Fix 'to to' typo in docstring - Clarify time derivative of manipulator Jacobian in frame {0} - Document q=None behavior and add type/shape annotations for q, qd, J0, and rtype - Convert See Also section to Sphinx .. seealso:: directive - Align continuation indent in References rubric to avoid blockquote rendering - Add runnable pycon example in docstring - Note that representation uses numerical Hessian via spatialmath numhess - Update typing overloads to restrict representation=None when q=None - Raise clear ValueError when representation is passed with q=None - Remove commented-out dead exploratory code - Add tests for precomputed J0 with q=None, all representations, and ValueError check --- src/roboticstoolbox/robot/Robot.py | 88 +++++++++++++++++------------- tests/test_jacob.py | 41 +++++++++----- 2 files changed, 75 insertions(+), 54 deletions(-) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 33878731c..0ad7cefdb 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1063,7 +1063,7 @@ def jacob0_dot( q: None, qd: ArrayLike, J0: NDArray = ..., - representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None, + representation: None = None, ) -> NDArray: # pragma no cover ... @@ -1075,16 +1075,25 @@ 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 ``None`` if ``J0`` is + provided and ``representation`` is ``None``) + :type q: ndarray(n) | None :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: ndarray(n) + :param J0: Jacobian in {0} frame. Ignored if ``representation`` is + specified. + :type J0: ndarray(6,n) | None + :param representation: Angular representation for analytical Jacobian + ('rpy/xyz', 'rpy/zyx', 'eul', or 'exp') + :type representation: str | None + :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:: @@ -1093,10 +1102,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: + + .. math:: - It is computed as the mode-3 product of the Hessian tensor and the - velocity vector. + \dmat{J}_0 = \mathcal{H}_0 \times_3 \dot{\vec{q}} The derivative of an analytical Jacobian can be obtained by setting ``representation`` as @@ -1115,15 +1129,30 @@ 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. + .. 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` + + .. rubric:: Examples - See Also - -------- - :func:`jacob0` - :func:`hessian0` + .. 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) """ @@ -1135,33 +1164,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 diff --git a/tests/test_jacob.py b/tests/test_jacob.py index 84b3ea092..f82d31fc3 100644 --- a/tests/test_jacob.py +++ b/tests/test_jacob.py @@ -80,21 +80,32 @@ 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) - - H = numhess( - lambda q: 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] - - print(np.round(j0, 2)) - print() - print(np.round(Jd, 2)) - nt.assert_array_almost_equal(j0, Jd, decimal=4) + 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_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 ------ # From 6add4c9f8c42f36a56a51f0742309ab308f54f62 Mon Sep 17 00:00:00 2001 From: paneendrakumar0 Date: Wed, 14 Oct 2026 10:05:01 +0530 Subject: [PATCH 2/2] fix(robot): address review comments on jacob0_dot docs, typing, and tests --- src/roboticstoolbox/robot/Robot.py | 49 +++++++++--- tests/test_jacob.py | 120 +++++++++++++++++++++++++++++ 2 files changed, 160 insertions(+), 9 deletions(-) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 0ad7cefdb..5b5360bc2 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1063,7 +1063,7 @@ def jacob0_dot( q: None, qd: ArrayLike, J0: NDArray = ..., - representation: None = None, + representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] | None = None, ) -> NDArray: # pragma no cover ... @@ -1077,18 +1077,19 @@ def jacob0_dot( r""" Time derivative of manipulator Jacobian in frame {0} - :param q: The joint configuration of the robot (must be provided if - ``representation`` is specified; may be ``None`` if ``J0`` is - provided and ``representation`` is ``None``) - :type q: ndarray(n) | None + :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 - :type qd: ndarray(n) + :type qd: ArrayLike(n) :param J0: Jacobian in {0} frame. Ignored if ``representation`` is - specified. - :type J0: ndarray(6,n) | None + 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 | None + :type representation: str, optional :returns: The time derivative of the manipulator Jacobian :rtype: ndarray(6,n) @@ -1135,6 +1136,30 @@ def jacob0_dot( 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 @@ -1153,6 +1178,12 @@ def jacob0_dot( >>> 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 """ diff --git a/tests/test_jacob.py b/tests/test_jacob.py index f82d31fc3..c6d7fd51f 100644 --- a/tests/test_jacob.py +++ b/tests/test_jacob.py @@ -86,6 +86,126 @@ def test_jacob_dot_precomputed_j0(self): 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], + ] + ) + + # 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)] + ) + + # 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)