From 43dc52904fe491bc1edb97f43d29742febb525f1 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 21:59:22 +1000 Subject: [PATCH 1/4] feat(ets): fkine/jacob0/jacobe/hessian0/hessiane accept compact q ikine_LM/ik_LM and friends return a solution sized to just the joints on the requested sub-chain (e.g. YuMi's l_gripper, 7 elements) -- but fkine/jacob0/etc. only understood a full, global jindex-addressed q (14 elements for YuMi), silently misindexing when handed the shorter compact solution directly. This is the root cause of #379's remaining "fkine gives garbage" symptom, on top of the gripper-labelling bug already fixed in #649. Add BaseETS._resolve_q(), the single place that disambiguates the two shapes: q of length ets.n is compact and gets scattered into a global- length array via ets.jindices; q of length >= max(jindices)+1 is already global and passes through unchanged (this also preserves two pre-existing behaviours: accepting a q longer than strictly needed, and never reordering an already-global q even when jindices aren't in increasing order, e.g. after .inv()). Anything else raises ValueError naming both accepted lengths. Wired into eval/jacob0/jacobe/hessian0/hessiane. No C++ changes: the resolution happens once, in Python, before the facade decides between the C++ extension and the pure-Python fallback -- both keep receiving exactly the global-length q they always have. Verified numerically identical between the two paths (~1e-16) on a real branched robot. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/ets/ETS.py | 16 +++++++- src/roboticstoolbox/ets/_ETS.py | 72 +++++++++++++++++++++++++++++++++ tests/test_ETS.py | 35 ++++++++++++++++ 3 files changed, 122 insertions(+), 1 deletion(-) diff --git a/src/roboticstoolbox/ets/ETS.py b/src/roboticstoolbox/ets/ETS.py index aaa71e3fd..81e529b2f 100644 --- a/src/roboticstoolbox/ets/ETS.py +++ b/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( diff --git a/src/roboticstoolbox/ets/_ETS.py b/src/roboticstoolbox/ets/_ETS.py index 6e3acc3b5..e70d6c9a5 100644 --- a/src/roboticstoolbox/ets/_ETS.py +++ b/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""" diff --git a/tests/test_ETS.py b/tests/test_ETS.py index c860a4bd0..5f3920999 100644 --- a/tests/test_ETS.py +++ b/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() From 8e18221f0572659cf51f31b507e5d5719520e5c4 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 21:59:30 +1000 Subject: [PATCH 2/4] feat(ets2): extend compact-q support to the 2D/planar variant Same fix as the 3D ETS, for consistency -- ETS2's eval/jacob0/jacobe don't go through the C++ facade at all (pure Python, indexing q by jindex inline), but share the same BaseETS._resolve_q(). Guarded against ETS2's lazy jindex auto-assignment inside jacob0() (jindices can legitimately be unassigned until that runs): _resolve_q() no-ops when it can't cleanly determine jindices, preserving prior behaviour exactly in that case. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/ets/ETS2.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/roboticstoolbox/ets/ETS2.py b/src/roboticstoolbox/ets/ETS2.py index 2b7db3d5c..1e5de3e1f 100644 --- a/src/roboticstoolbox/ets/ETS2.py +++ b/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] From ba2e8e632b26b71aef64ce22ec0677bc83b07aed Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 21:59:36 +1000 Subject: [PATCH 3/4] docs(kinematics): explain fkine()'s compact-vs-global q on branched robots Adds a worked YuMi example alongside the existing single-chain one, covering the new dual-mode q accepted by RobotKinematics.fkine (see the ETS.eval()/compact-q commit). Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/RobotKinematics.py | 22 +++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/roboticstoolbox/robot/RobotKinematics.py b/src/roboticstoolbox/robot/RobotKinematics.py index 3fc793e9c..9a55afd22 100644 --- a/src/roboticstoolbox/robot/RobotKinematics.py +++ b/src/roboticstoolbox/robot/RobotKinematics.py @@ -35,10 +35,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 @@ -58,6 +66,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 From 5a7d7114147b045f16c31dcce9dec08640321ac2 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 21:59:43 +1000 Subject: [PATCH 4/4] test(robot): add end-to-end compact-q regression test on YuMi Solves ikine_LM for l_gripper and feeds the 7-element solution straight into fkine/jacob0 without any manual full-vector workaround -- this is the actual #379 usage pattern the compact-q fix targets. Co-Authored-By: Claude Sonnet 5 --- tests/test_Robot.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/tests/test_Robot.py b/tests/test_Robot.py index b8885a6b0..a51f3b093 100644 --- a/tests/test_Robot.py +++ b/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