Skip to content

feat(robot): add self-collision checks for non-adjacent links - #816

Merged
petercorke merged 6 commits into
petercorke:mainfrom
kVinsom:feat/723-self-collision
Oct 22, 2026
Merged

petercorke merged 6 commits into
petercorke:mainfrom
kVinsom:feat/723-self-collision

Conversation

@kVinsom

@kVinsom kVinsom commented Oct 16, 2026 •

Copy link
Copy Markdown
Contributor

Summary

Adds robot.isselfcollided(q=None, skip=False) to check collision geometry between non-adjacent links, including grippers and nested groups. Passing the robot itself to iscollided(q, robot) (or the deprecated collided alias) now performs this check instead of raising an attribute error or silently returning False for a model without geometry.

Shapes on the same link, neighbouring collision-bearing links, and links within the same gripper are excluded. Links without collision geometry are transparent when determining adjacency. Siblings outside a shared gripper, separate branches and different grippers are still checked. Touching counts as collision for the pairs tested. This changes public behaviour by removing false positives across empty spacer links and normal finger contact within a gripper. Models without collision geometry raise ValueError. Pose updates preserve local geometry transforms and stored joint configurations, and work without the optional RTB C extension. Documentation explains partial geometry coverage and skip=True.

Related issue

Fixes #723.

Validation

  • Review follow-up: all 44 self-collision tests pass with real Coal on Linux/Python 3.14, without skips, using the RTB Python fallback. Includes Panda/Frankie ready poses, chains of empty spacer links, same/different grippers, arm-gripper collisions, nested/rotated geometry and repeated public scene updates.
  • Five regression cases fail before these review fixes and pass afterwards (empty spacer chains, same-gripper contact, Frankie ready pose and scene-update consistency).
  • Broader Robot/ERobot/BaseRobot/Gripper/collision suite: 273 passed, 4 skipped, 4 failed. All four failures reproduce on the pre-fix code after the same merge of main; this environment lacks the RTB native extensions. The broader local suite is not fully green.
  • mypy --platform linux: no issues in 165 source files. Default Windows mypy reports six pre-existing POSIX-signal attribute errors in symbolic_ik.py.
  • New test lint/format checks and git diff --check pass. The changed method adds no Ruff findings; existing unrelated module findings/formatting were left unchanged.
  • Updated the branch with main and resolved the deprecated-alias docstring conflict. Remote CI and other platforms still need to validate the updated branch.

Checklist

  • PR title follows Conventional Commits
  • Tests pass locally (pytest) — focused feature tests pass; full-suite baseline failures detailed above
  • Added/updated tests for this change
  • New/changed code uses modern type hints
  • Docstrings updated in reST style
  • PR is focused on one feature
  • No personal-project test data or notebooks included

Implementation, documentation, tests and review fixes were developed with OpenAI Codex (GPT-6) assistance.

Include gripper and nested collision geometry, reject models without collision shapes, and support iscollided(q, robot). Add documentation and 34 regression tests for issue petercorke#723.
@codacy-production

codacy-production Bot commented Oct 16, 2026 •

Copy link
Copy Markdown

Not up to standards ⛔

🔴 Issues 24 high

Alerts:
⚠ 24 issues (≤ 0 issues of at least minor severity)

Results:
24 new issues

Category Results
Security 24 high

View in Codacy

🟢 Metrics 66 complexity · 0 duplication

Metric Results
Complexity 66
Duplication 0

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 16, 2026 •

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 0% with 47 lines in your changes missing coverage. Please review.
✅ Project coverage is 0.00%. Comparing base (093a760) to head (15cae90).
⚠️ Report is 1 commits behind head on main.

Files with missing lines Patch % Lines
src/roboticstoolbox/robot/Robot.py 0.00% 47 Missing ⚠️
Additional details and impacted files
@@          Coverage Diff          @@
##            main    #816   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files        146     146           
  Lines      17168   17212   +44     
=====================================
- Misses     17168   17212   +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.

@petercorke

Copy link
Copy Markdown
Owner

Thank you for the self-collision check, which is a useful feature and well tested. Two requests before it goes in.

  1. In Robot.py (around lines 1503–1504) the code writes node._wT and node._wq, which are private attributes of spatialgeometry. Is there a reason the public way of updating a node's pose cannot be used? If it's needed for speed, could you say so in a comment, and which spatialgeometry version it relies on?

  2. Only direct parent and child links are excluded, so a link with no collision geometry in between breaks the exclusion. For the Panda hand the tree is panda_link7 → panda_link8 → panda_hand, and panda_link8 has no geometry, so panda_link7 and panda_hand are tested against each other and reported as colliding. This makes Frankie().isselfcollided(qr) return True, and Panda too at some poses. Could links without collision geometry be treated as transparent when deciding adjacency, so that panda_link7 counts as the neighbour of panda_hand? The two fingers touch too, and since touching counts as a collision they are reported. Please either exclude pairs within the same gripper or say in the docstring that touching fingers are reported.

There is also a one-line docstring conflict with main in Robot.py. If you update the branch it will go away, or I can resolve it.

kVinsom and others added 2 commits October 21, 2026 21:58
Co-Authored-By: Codex (GPT-6) <noreply@openai.com>
Co-Authored-By: Codex (GPT-6) <noreply@openai.com>
@kVinsom

kVinsom commented Oct 21, 2026

Copy link
Copy Markdown
Contributor Author

Thank you, Peter. Both points are now addressed, and I have merged main and resolved the docstring conflict.

  • Pose updates now use the public link.T setter and robot.update(). There are no direct writes to spatialgeometry's _wT or _wq. FK still works without the optional RTB C extension, while local geometry offsets and stored robot/gripper configurations are preserved.
  • Adjacency now follows the nearest ancestor with actual collision geometry, looking through empty spacer links and nested empty groups.
  • Pairs within the same gripper are excluded. Arm-gripper and different-gripper collisions remain checked; the docstring and user guide state this explicitly.

All 44 self-collision tests pass with real Coal, including Panda().isselfcollided(qr) and Frankie().isselfcollided(qr), both False. Five regression cases fail before the fixes and pass afterwards. The broader related suite has 273 passing tests, 4 skips and 4 failures that also reproduce before these fixes; details are in the updated PR description. The Linux-targeted mypy check passes for all 165 modules.

Thank you for the concrete examples; they helped make the adjacency rules and scene updates much clearer.

@petercorke

Copy link
Copy Markdown
Owner

Thank you for these changes to isselfcollided. Both requests are addressed: it now uses the public link.T with robot.update() instead of _wT/_wq, and adjacency looks through links that have no geometry, with the fingers of one gripper excluded. I re-ran the 44 self-collision tests and the full suite (2286 passed), mypy is clean, and Panda and Frankie at qr are now correctly not in self-collision. I'll update the branch with main and merge it once CI is green.

@petercorke
petercorke enabled auto-merge (squash) October 22, 2026 01:35
@petercorke
petercorke merged commit 8255224 into petercorke:main Oct 22, 2026
22 of 23 checks passed
@github-actions github-actions Bot mentioned this pull request Oct 22, 2026
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.

Self-collision checking for a robot's own links

2 participants