fix: preserve joint types and limits in numerical IK - #669
Open
Doribelove wants to merge 1 commit into
Open
Conversation
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
Numerical IK can report success and a zero residual, then change the achieved pose when it wraps a prismatic joint as an angle. For a mixed 3P/3R chain with a valid initial solution, a translation of
4becomes-2.283185...; forward kinematics of the returned solution is displaced by2*pi.Make post-convergence normalisation joint-aware in both
IKSolverand the shared C++ IK loop. Prismatic coordinates stay unchanged. Revolute coordinates already within their limits are preserved, including multi-turn and offset ranges; otherwise, select a whole-turn equivalent towards the permitted interval and retain the existing rejection when no valid equivalent exists. Python checks the compact active coordinates against their corresponding limits, including subchains with sparse joint indices.The change also corrects negative C++
fmodremainders. It avoids reducing angles already in the principal interval, and recovers rounded limit endpoints only when adding a nonzero integer number of turns reconstructs the original coordinate exactly. This avoids adding a tolerance to the joint limits. The shared IK documentation describes the resulting behaviour.Related issue
Fixes #639.
Fixes #668.
Validation
Tested on Ubuntu 22.04 x86_64, Python 3.12.14, NumPy 2.5.3, SciPy 1.18.1, against main
02d2e1876f5e4cb38182fb83b1d217d5d3c08ff4._fknm_cand_frne_cwere verified available. All 77 regressions passed, covering Python LM/GN/NR/QP, C++ LM variants/GN/NR, mixed/flipped joints, actual iterations, limits on/off, multi-turn/end-point cases, and Python subchains/trajectories.test_blocks.pyas in the contribution guide: original 709 passed, patched wheel 786 passed; both had the same 31 skips, 4 warnings and 3 passing subtests. The unmodified full command first timed out fetching external robot assets. A local test-only plugin explicitly skipped 19 tests that requestrobot_descriptionsassets; the other skips were 9 optional Swift tests and 3 existing skips. That plugin is not part of the patch.git diff --checkpasses. The existingIK.pyfile has the same 16 Ruff findings as the base, with none added by this patch.This does not add global/sparse-index support to the C++ solvers or select a continuous trajectory branch. No hardware or application-level performance validation was performed.
AI assistance: the implementation, tests and this description were prepared with Codex. The validation above was executed locally.
Checklist