Skip to content

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

Merged
petercorke merged 5 commits into
mainfrom
feat/fkine-compact-q
Aug 26, 2026
Merged

feat(ets): fkine/jacob0/jacobe/hessian0/hessiane accept compact q#653
petercorke merged 5 commits into
mainfrom
feat/fkine-compact-q

Conversation

@petercorke

Copy link
Copy Markdown
Owner

Summary

  • 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/jacobe/hessian0/hessiane 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 Dual Arm robot (YuMi robot) gives different solutions, when check using forward kinematics, gives different trajectories each time... #379's remaining "fkine gives garbage" symptom, on top of the gripper-labelling bug already fixed in fix(models,ik): correct YuMi gripper parents and IK failure-path q compaction #649.
  • Added 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 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 (3D ETS) and eval/jacob0/jacobe (2D ETS2, extended for consistency).
  • No C++ changes needed at all -- 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 (YuMi).
  • Updated RobotKinematics.fkine()'s and ETS.eval()'s docstrings to explain the new dual-mode q explicitly, with a worked YuMi example.

Item 4 (final item) of the IK-solver cleanup plan (see claude-notes/ik-solver-cpp-python-divergence.md). Items 1-3 are PRs #649, #650, #652 (all merged).

Test plan

  • New regression tests: compact-vs-global equivalence on a synthetic branched sub-chain (eval/jacob0/jacobe/hessian0/hessiane), too-short q raises ValueError, trajectory support unaffected
  • New end-to-end test on YuMi: ikine_LM -> fkine/jacob0 with the raw 7-element solution, no manual full-vector workaround
  • Full test suite green (750 passed, 18 skipped) in a clean isolated venv, before and after merging in main's subsequent PRs
  • Manually re-ran the 379.py repro end-to-end -- fkine(sol.q, end=...) now matches the target directly
  • Sphinx docs build (-W --keep-going) clean of any new warnings, including the new runblock example in fkine's docstring
  • Single-chain robots (Panda, Puma560, UR5) and the ETS.inv() reversed-jindex edge case confirmed unaffected

🤖 Generated with Claude Code

petercorke and others added 5 commits August 26, 2026 21:59
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
…obots

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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
@codecov

codecov Bot commented Aug 26, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 0% with 25 lines in your changes missing coverage. Please review.
✅ Project coverage is 0.00%. Comparing base (07a60f4) to head (b520774).

Files with missing lines Patch % Lines
src/roboticstoolbox/ets/_ETS.py 0.00% 15 Missing ⚠️
src/roboticstoolbox/ets/ETS.py 0.00% 7 Missing ⚠️
src/roboticstoolbox/ets/ETS2.py 0.00% 3 Missing ⚠️
Additional details and impacted files
@@          Coverage Diff          @@
##            main    #653   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files        143     143           
  Lines      14027   14050   +23     
=====================================
- Misses     14027   14050   +23     

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

@petercorke
petercorke merged commit d5b6990 into main Aug 26, 2026
23 checks passed
@petercorke
petercorke deleted the feat/fkine-compact-q branch August 26, 2026 12:12
@github-actions github-actions Bot mentioned this pull request Aug 26, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant