feat(ets): fkine/jacob0/jacobe/hessian0/hessiane accept compact q

src/roboticstoolbox/ets/ETS.py

@@ -325,10 +325,17 @@ def eval(
         """
         Forward kinematics (returns raw ndarray)
 
-        :param q: Joint coordinates
+        :param q: Joint coordinates -- either *global* (length
+            ``max(self.jindices) + 1``, addressed by each joint's own
+            ``jindex``) or *compact* (length ``self.n``, positionally
+            ordered to match :meth:`joints`). These only differ when this
+            ETS is one branch of a larger, branched robot -- e.g. it's what
+            ``ikine_LM``/``ik_LM`` return when solving for just this ETS.
+            For a whole, unbranched robot the two coincide.
         :param base: a base transform applied before the ETS
         :param tool: tool transform, optional
         :param include_base: set to True if the base transform should be considered
+        :raises ValueError: if ``q``'s length matches neither interpretation
         :returns: the transformation matrix representing the pose of the end-effector
         :rtype: ndarray(4,4) or ndarray(m,4,4)
 
@@ -360,6 +367,7 @@ def eval(
 
         """
 
+        q = self._resolve_q(q, allow_trajectory=True)
         return ETS_fkine(self._fknm, q, base, tool, include_base, _data=self.data)
 
     def jacob0(
@@ -407,6 +415,7 @@ def jacob0(
 
         """
 
+        q = self._resolve_q(q)
         return ETS_jacob0(self._fknm, q, tool, _data=self.data, _n=self.n)
 
     def jacobe(
@@ -454,6 +463,7 @@ def jacobe(
 
         """
 
+        q = self._resolve_q(q)
         return ETS_jacobe(self._fknm, q, tool, _data=self.data, _n=self.n)
 
     def hessian0(
@@ -523,6 +533,8 @@ def hessian0(
 
         """
 
+        if q is not None:
+            q = self._resolve_q(q)
         return ETS_hessian0(self._fknm, q, J0, tool, _data=self.data, _n=self.n)
 
     def hessiane(
@@ -592,6 +604,8 @@ def hessiane(
 
         """
 
+        if q is not None:
+            q = self._resolve_q(q)
         return ETS_hessiane(self._fknm, q, Je, tool, _data=self.data, _n=self.n)
 
     def jacob0_analytical(

src/roboticstoolbox/ets/ETS2.py

@@ -309,7 +309,7 @@ def eval(
         - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke
         """
 
-        q = getmatrix(q, (None, None))
+        q = self._resolve_q(q, allow_trajectory=True)
         l, _ = q.shape  # type: ignore
         end = self[-1]
 
@@ -375,8 +375,6 @@ def jacob0(
     ) -> NDArray:
         # very inefficient implementation, just put a 1 in last row
         # if its a rotation joint
-        q = getvector(q)
-
         j = 0
         J = np.zeros((3, self.n))
         etjoints = self.joint_idx()
@@ -386,6 +384,9 @@ def jacob0(
             for j in range(self.n):
                 i = etjoints[j]
                 self[i].jindex = j
+            self.__dict__.pop("jindices", None)  # invalidate cached_property
+
+        q = self._resolve_q(q)
 
         for j in range(self.n):
             i = etjoints[j]

src/roboticstoolbox/ets/_ETS.py

@@ -399,6 +399,78 @@ def jindices(self) -> NDArray:
 
         return np.array([j.jindex for j in self.joints()])  # type: ignore
 
+    def _resolve_q(self, q: ArrayLike, allow_trajectory: bool = False) -> NDArray:
+        """
+        Resolve q to a full jindex-addressed vector
+
+        :param q: joint coordinates, either *compact* (length :attr:`n`,
+            positionally ordered to match this ETS's own :meth:`joints`) or
+            *global* (length ``max(jindices) + 1``, addressed by each
+            joint's global ``jindex`` -- e.g. the whole robot's ``q`` on a
+            branched robot, when this ETS is only one branch of it)
+        :param allow_trajectory: if True, ``q`` is normalised with
+            :func:`getmatrix` (preserving a genuine ``(m, n)`` trajectory's
+            row count, matching :meth:`eval`'s own convention); if False,
+            ``q`` is flattened with :func:`getvector` first, matching
+            :meth:`jacob0`/:meth:`jacobe`/:meth:`hessian0`/:meth:`hessiane`,
+            none of which accept a trajectory
+        :raises TypeError: if ``q`` isn't numeric (from :func:`getvector`)
+        :raises ValueError: if ``q``'s length matches neither interpretation
+        :returns: a global jindex-addressed array; unchanged if ``q`` was
+            already global, scattered via :attr:`jindices` if ``q`` was
+            compact
+
+        This is the single place that disambiguates the two ``q`` shapes
+        accepted by :meth:`eval`, :meth:`jacob0`, :meth:`jacobe`,
+        :meth:`hessian0` and :meth:`hessiane` -- everything downstream of
+        this (the C++ extension and the pure-Python fallback) only ever
+        sees a global, jindex-addressed vector, exactly as before this
+        method existed.
+
+        Checking the compact length first means the common case -- a
+        single-chain robot, or any ETS whose own joints already happen to
+        carry global jindex 0..n-1 in order -- is unaffected: scattering
+        via :attr:`jindices` is then just the identity.
+        """
+        n = self.n
+        jindices = self.jindices
+
+        q = getmatrix(q, (None, None)) if allow_trajectory else getvector(q, None)
+
+        if n == 0 or jindices.dtype == object:
+            # no joints, or at least one joint has no jindex assigned yet
+            # (e.g. an ETS2 instance before its lazy auto-assignment runs)
+            # -- nothing to safely disambiguate, leave q exactly as given
+            return q
+
+        global_len = int(jindices.max()) + 1
+        length = q.shape[-1]
+
+        if length == n and n < global_len:
+            # a genuine sub-chain (this ETS's own joints don't span the
+            # full global jindex range) -- scatter positionally
+            full = np.zeros(q.shape[:-1] + (global_len,), dtype=q.dtype)
+            full[..., jindices] = q
+            return full
+
+        if length >= global_len:
+            # already global-addressed; longer-than-needed is accepted
+            # (matches the pre-existing convention of indexing q by
+            # jindex and ignoring unused trailing entries). This also
+            # covers n == global_len, where compact and global lengths
+            # coincide -- always treating that as global (not scattering)
+            # matters when this ETS's own joints don't carry jindex in
+            # increasing order (e.g. a reversed chain from .inv()), where
+            # scattering would silently reorder q instead of leaving it
+            # alone.
+            return q
+
+        raise ValueError(
+            f"q has length {length}, expected {n} (compact, positionally "
+            f"matching this ETS's own joint order) or at least {global_len} "
+            f"(global, addressed by jindex -- e.g. the whole robot's q)"
+        )
+
     @property
     def qlim(self):
         r"""

src/roboticstoolbox/robot/RobotKinematics.py

@@ -36,10 +36,18 @@ def fkine(
         """
         Forward kinematics
 
-        :param q: Joint coordinates
+        :param q: Joint coordinates -- either *global* (length ``robot.n``,
+            addressed by each joint's ``jindex``, e.g. the whole robot's
+            current ``q``) or, when ``end``/``start`` select just one branch
+            of a branched robot, *compact* (length equal to the number of
+            joints on that branch, positionally ordered from ``start`` to
+            ``end`` -- exactly what ``ikine_LM``/``ik_LM`` etc. return when
+            solving for that same ``end``)
         :param end: end-effector or gripper to compute forward kinematics to
         :param start: the link to compute forward kinematics from
         :param tool: tool transform, optional
+        :raises ValueError: if ``q``'s length matches neither the compact
+            nor the global interpretation for the selected ``start``/``end``
         :returns: The transformation matrix representing the pose of the end-effector
 
         ``T = robot.fkine(q)`` evaluates forward kinematics for the robot at
@@ -59,6 +67,18 @@ def fkine(
         >>> panda = rtb.models.Panda()
         >>> panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
 
+        On a branched robot, ``end`` selects one branch, and ``q`` may be
+        *either* the whole robot's global ``q`` *or* just the solution for
+        that branch on its own -- both give the same result:
+
+        .. runblock:: pycon
+        >>> import roboticstoolbox as rtb
+        >>> from spatialmath import SE3
+        >>> yumi = rtb.models.YuMi()
+        >>> sol = yumi.ikine_LM(SE3(0.4, 0.2, 0.3) * SE3.Rx(0.2), end="l_gripper", method="chan", k=0.1, seed=0)
+        >>> sol.q.shape
+        >>> yumi.fkine(sol.q, end="l_gripper")
+
         .. rubric:: Notes
 
         - For a robot with a single end-effector there is no need to

tests/test_ETS.py

@@ -4473,6 +4473,41 @@ def test_manip_fail2(self):
         with self.assertRaises(ValueError):
             ets.manipulability(q, axes="abcdef")  # type: ignore
 
+    def test_resolve_q_compact_and_global(self):
+        # a sub-chain whose own jindex range doesn't start at 0 -- e.g. a
+        # second arm on a branched robot, jindex 3-5 while some other
+        # branch owns 0-2
+        e = Rz(jindex=3) * tx(1) * Rz(jindex=4) * tx(1) * Rz(jindex=5)
+        ets = rtb.ETS(e)
+
+        self.assertEqual(ets.n, 3)
+        nt.assert_array_equal(ets.jindices, [3, 4, 5])
+
+        q_compact = [0.1, 0.2, 0.3]
+        q_global = np.zeros(6)
+        q_global[3:6] = q_compact
+
+        nt.assert_almost_equal(ets.eval(q_compact), ets.eval(q_global))
+        nt.assert_almost_equal(ets.jacob0(q_compact), ets.jacob0(q_global))
+        nt.assert_almost_equal(ets.jacobe(q_compact), ets.jacobe(q_global))
+        nt.assert_almost_equal(ets.hessian0(q_compact), ets.hessian0(q_global))
+        nt.assert_almost_equal(ets.hessiane(q_compact), ets.hessiane(q_global))
+
+    def test_resolve_q_too_short_raises(self):
+        e = Rz(jindex=3) * tx(1) * Rz(jindex=4) * tx(1) * Rz(jindex=5)
+        ets = rtb.ETS(e)
+
+        # neither compact (3) nor global (>= 6)
+        with self.assertRaises(ValueError):
+            ets.eval([0.1, 0.2, 0.3, 0.4])
+
+    def test_resolve_q_trajectory_still_works(self):
+        panda = rtb.models.Panda().ets()
+        qt = np.tile(panda.random_q(), (5, 1))
+
+        T = panda.eval(qt)
+        self.assertEqual(T.shape, (5, 4, 4))
+
 
 if __name__ == "__main__":
     unittest.main()

tests/test_Robot.py

@@ -796,6 +796,26 @@ def test_fkine_all2(self):
 
         r.fkine_all(r.q)
 
+    def test_fkine_compact_q_branched_robot(self):
+        # regression (issue #379): fkine()/jacob0() on a branched robot
+        # must accept the compact, per-arm q that ikine_LM/ik_LM return for
+        # a sub-chain (e.g. YuMi's l_gripper, global jindex 7-13) directly,
+        # not just a full-robot, jindex-addressed q.
+        from spatialmath import SE3
+
+        robot = rtb.models.YuMi()
+        Tep = SE3(0.4, 0.2, 0.3) * SE3.Rx(0.2)
+
+        sol = robot.ikine_LM(Tep, end="l_gripper", method="chan", k=0.1, seed=0)
+        self.assertTrue(sol.success)
+        self.assertEqual(sol.q.shape[0], 7)
+
+        T = robot.fkine(sol.q, end="l_gripper")
+        nt.assert_almost_equal(T.A, Tep.A, decimal=4)
+
+        J = robot.jacob0(sol.q, end="l_gripper")
+        self.assertEqual(J.shape, (6, 7))
+
     def test_fkine_all_past_ee_link(self):
         # fkine_all() used to stop recursing the instant it reached a
         # link registered in self.ee_links, even when that link has real