fix(models,ik): correct YuMi gripper parents and IK failure-path q compaction - #649
Merged
Conversation
r_gripper and l_gripper were wired to the opposite arm's gripper base link, so ikine_LM(end='r_gripper') silently solved for the left arm and vice versa. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
IKSolver._solve() compacted q via ets.jindices on success but returned the raw zero-padded global-length vector on failure. Invisible for a sub-chain whose jindex happens to start at 0, but for one that doesn't (e.g. YuMi's l_gripper, jindex 7-13) the failed solution came back at the wrong length, breaking any caller expecting length == ets.n. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #649 +/- ##
=====================================
Coverage 0.00% 0.00%
=====================================
Files 143 143
Lines 14035 14035
=====================================
Misses 14035 14035 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
4 tasks
Merged
This was referenced Aug 26, 2026
petercorke
added a commit
that referenced
this pull request
Aug 26, 2026
* 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 <noreply@anthropic.com> * 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 <noreply@anthropic.com> * 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 <noreply@anthropic.com> * 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 <noreply@anthropic.com> --------- Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>
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
1 participant
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
YuMi.py'sr_gripper/l_gripperlinks were wired to the opposite arm's gripper base link, soend='r_gripper'silently solved for the left arm and vice versa (root-cause piece of Dual Arm robot (YuMi robot) gives different solutions, when check using forward kinematics, gives different trajectories each time... #379's "inconsistent results" report).#379repro, found a second, independent bug:IKSolver._solve()(shared byIK_LM/IK_NR/IK_GN/IK_QP) compacts the returnedqviaets.jindiceson success but not on failure. Invisible for a sub-chain whose jindex happens to start at 0, but for one that doesn't (e.g. YuMi'sl_gripper, jindex 7-13, only reachable once the parent-swap above is fixed) the failed solution came back at the wrong length — this is what actually crashed the#379repro once the swap was corrected.IK_LM_c/IK_NR_c/IK_GN_c) were checked and don't share this bug — they never operate in padded/global space to begin with, so there's nothing to mirror there.Part of a larger IK-solver investigation; this is Item 1 of a 4-item plan (see
claude-notes/ik-solver-cpp-python-divergence.md). Items 2-4 (brokenikine_LMSexamples,IKSolutionreturn-type consistency,fkine()compact-q support) are separate follow-up PRs.Test plan
r_gripper/l_gripperresolve to the correct arm'sgripper_r_base/gripper_l_base(fails on old code)IK_LM.solve()on a sub-chain with non-zero-based jindex returnsqof lengthets.n(fails on old code)#379repro script end-to-end; the length-mismatch crash it hit is gone (a separate, already-knownfkine()compact-q limitation remains, tracked as Item 4)🤖 Generated with Claude Code