Skip to content

fix: preserve joint types and limits in numerical IK - #669

Open
Doribelove wants to merge 1 commit into
petercorke:mainfrom
Doribelove:codex/ik-joint-aware-normalisation
Open

fix: preserve joint types and limits in numerical IK#669
Doribelove wants to merge 1 commit into
petercorke:mainfrom
Doribelove:codex/ik-joint-aware-normalisation

Conversation

@Doribelove

Copy link
Copy Markdown

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 4 becomes -2.283185...; forward kinematics of the returned solution is displaced by 2*pi.

Make post-convergence normalisation joint-aware in both IKSolver and 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++ fmod remainders. 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.

  • New regressions against the compiled original source: 71 failed, 6 passed; failures were result assertions.
  • Built a compiled wheel from the patch and installed it in a separate environment. Both _fknm_c and _frne_c were 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.
  • With C FK/Jacobian/angle-axis dispatch explicitly disabled: 40 passed, with the 37 C-only cases explicitly skipped.
  • Offline full-suite comparison, excluding test_blocks.py as 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 request robot_descriptions assets; the other skips were 9 optional Swift tests and 3 existing skips. That plugin is not part of the patch.
  • Independent enumeration of whole-turn equivalents: 10,000 random cases, 70 constructed endpoints, and 14 one-ULP out-of-limit cases passed, including forward-kinematics checks for a sparse mixed chain.
  • Ruff passes for the new test file; both Python files pass formatting and git diff --check passes. The existing IK.py file 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

  • PR title follows Conventional Commits.
  • Tests pass locally within the documented offline profile; the unrestricted suite is limited by external asset downloads.
  • Added tests for this change.
  • New code is type-hinted with modern syntax.
  • Updated IK documentation and added a reST helper docstring.
  • Changes are scoped to joint-coordinate normalisation and its limit check.
  • No private-project test files, data files or notebooks are included.

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.

Assumption of all revolute robot in IK.py IK.py unconditionally wraps q to +-pi, breaking prismatic joints

1 participant