Skip to content

fix(puma): self-contained ikine_a with trajectory and failure handling; deprecate ikine_6s - #715

Open
petercorke wants to merge 1 commit into
mainfrom
fix/ikine-a-puma
Open

petercorke wants to merge 1 commit into
mainfrom
fix/ikine-a-puma

Conversation

@petercorke

Copy link
Copy Markdown
Owner

Summary

Puma560.ikine_a was a hand-written Puma solution (Paul and Zhang 1986) wrapped in DHRobot.ikine_6s, a generic-looking helper from the MATLAB Toolbox. It is not generic: it only handles the wrist, and the ikfunc that solves joints 1-3 is specific to one geometry (it reads a2, a3, d1, d3, d4 and never a1, so it assumes a zero first link length and a lateral shoulder offset). Puma560.ikine_a was its only caller.

  • ikine_a is now self-contained. The wrist, base/tool and trajectory handling moved into it. The Paul and Zhang equations for joints 1-3 are unchanged.
  • DHRobot.ikine_6s is deprecated (DeprecationWarning, .. deprecated:: 1.5.0). It is frozen: it still runs but warns, and its known problems are noted in the deprecation text. The intent is to remove it in a later release.
  • Bugs fixed (all found by testing the old path):
    • a pose closer to the waist axis than the shoulder offset returned success=True with q all NaN (the arcsin(d3/r) NaN was never caught)
    • a trajectory returned a garbled IKSolution: the failure reasons were passed positionally as iterations, success was an array, and reason was empty
    • a trajectory containing an unreachable pose raised ValueError
    • an ndarray pose raised AttributeError
    • the docstring claimed a joint vector was returned and had typos

New ikine_a contract (matches the numerical solvers)

  • accepts an SE3 or a 4x4 / (N, 4, 4) array (bad shapes raise ValueError; the array handling works around spatialmath#236)
  • returns one IKSolution: q of shape (n,) or (N, n), a single bool success (True only if every pose succeeded), the maximum residual, the last failure reason, and iterations = searches = 0
  • joints of a pose that cannot be solved are NaN, so a trajectory's q stays rectangular
  • success is decided by checking the solution with forward kinematics against tol (default 1e-6, as in the numerical solvers), not by the absence of an exception, so a NaN can no longer be reported as a success
  • unchanged: config, a single pose still returns q with shape (n,), and Robot.jtraj still finds ikine_a by name
  • joint limits are not considered (documented)

Test plan

Follow-ups (separate PRs)

  • Robot.jtraj should take joint vectors only (pose arguments deprecated)
  • the new symbolic analytic IK can use this solver as an independent cross-check for the Puma

🤖 Generated with Claude Code

…dling; deprecate ikine_6s

Puma560.ikine_a relied on DHRobot.ikine_6s, which came from the MATLAB Toolbox
and is not general: it only handled the wrist, while the Paul and Zhang
first-three-joints solution (ik3) is specific to the Puma geometry (zero first
link length, lateral shoulder offset d3).  Move the wrist, base/tool and
trajectory handling into ikine_a so it is self-contained, and deprecate
ikine_6s (frozen, it still runs but warns).

Fix bugs in the old path:
- a pose closer to the waist axis than the shoulder offset returned
  success=True with q all NaN (the arcsin(d3/r) NaN was not caught)
- a trajectory returned a garbled IKSolution: the failure reasons were passed as
  the iterations argument, success was an array, and reason was empty
- a trajectory containing an unreachable pose raised ValueError
- an ndarray pose raised AttributeError
- the docstring said it returned a joint vector, and had typos

ikine_a now returns an IKSolution like the numerical solvers: q (n,) or (N, n),
one bool success, the maximum residual, and the last failure reason.  Joints of
a pose that cannot be solved are NaN.  Success is decided by checking the
solution by forward kinematics, not by the absence of an exception.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
@codacy-production

Copy link
Copy Markdown

Up to standards ✅

🟢 Issues 0 issues

Results:
0 new issues

View in Codacy

🟢 Metrics 29 complexity · 2 duplication

Metric Results
Complexity 29
Duplication 2

View in Codacy

NEW Get contextual insights on your PRs based on Codacy's metrics, along with PR and Jira context, without leaving GitHub. Enable AI reviewer
TIP This summary will be updated as you push new changes.

@codecov

codecov Bot commented Oct 4, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 0% with 49 lines in your changes missing coverage. Please review.
✅ Project coverage is 0.00%. Comparing base (acc6842) to head (718efff).
⚠️ Report is 1 commits behind head on main.

Files with missing lines Patch % Lines
src/roboticstoolbox/models/DH/Puma560.py 0.00% 48 Missing ⚠️
src/roboticstoolbox/robot/DHRobot.py 0.00% 1 Missing ⚠️
Additional details and impacted files
@@          Coverage Diff          @@
##            main    #715   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files        143     143           
  Lines      14269   14313   +44     
=====================================
- Misses     14269   14313   +44     

☔ 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.

This branch has not been deployed

No deployments
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