feat(ets): fkine/jacob0/jacobe/hessian0/hessiane accept compact q - #653
Merged
Conversation
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 Report❌ Patch coverage is
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. 🚀 New features to boost your workflow:
|
Merged
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
ikine_LM/ik_LMand friends return a solution sized to just the joints on the requested sub-chain (e.g. YuMi'sl_gripper, 7 elements) -- butfkine/jacob0/jacobe/hessian0/hessianeonly understood a full, global jindex-addressedq(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.BaseETS._resolve_q(), the single place that disambiguates the two shapes:qof lengthets.nis compact and gets scattered into a global-length array viaets.jindicesqof length>= max(jindices) + 1is global and passes through unchanged (this also preserves two pre-existing behaviours: accepting aqlonger than strictly needed, and never reordering an already-globalqeven when jindices aren't in increasing order, e.g. after.inv())ValueErrornaming both accepted lengthseval/jacob0/jacobe/hessian0/hessiane(3DETS) andeval/jacob0/jacobe(2DETS2, extended for consistency).qthey always have. Verified numerically identical between the two paths (~1e-16) on a real branched robot (YuMi).RobotKinematics.fkine()'s andETS.eval()'s docstrings to explain the new dual-modeqexplicitly, 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
eval/jacob0/jacobe/hessian0/hessiane), too-shortqraisesValueError, trajectory support unaffectedikine_LM->fkine/jacob0with the raw 7-element solution, no manual full-vector workaroundmain's subsequent PRs379.pyrepro end-to-end --fkine(sol.q, end=...)now matches the target directly-W --keep-going) clean of any new warnings, including the new runblock example infkine's docstringETS.inv()reversed-jindex edge case confirmed unaffected🤖 Generated with Claude Code