Link._geometry/_collision SceneGroups aren't wired into the scene graph correctly

[petercorke]

Status

Confirmed, reproducible, not yet fixed. Found 2026-08-08 while confirming spatialgeometry's SceneGroup collision-checking works correctly post PyBullet→Coal migration (see spatialgeometry PRs #21/#24/#25/#27 for the related SceneGroup-side fixes made the same session). This issue is the RTB-side half of that investigation -- the bug lives here, not in SG.

The bug

Link.__init__ (src/roboticstoolbox/robot/Link.py:166-172):

self._geometry = SceneGroup(scene_children=geometry)
self._scene_children.append(self._geometry)   # <-- raw list append

self._collision = SceneGroup(scene_children=collision)
self._scene_children.append(self._collision)   # <-- raw list append

self._scene_children.append(...) mutates the link's own Python-level _scene_children list directly. It does two things wrong compared to going through the proper scene_parent/scene_children API (spatialgeometry's SceneNode):

1. It never sets self._geometry.scene_parent / self._collision.scene_parent back to self (the link). link.collision.scene_parent is link reads back False for every link, including on rtb.models.Panda().
2. It never triggers self.__update_c() (spatialgeometry's SceneNode, name-mangled private method) -- the step that syncs the current _scene_children list into the compiled C node that _propogate_scene_tree() actually walks. Since super().__init__() runs before these lines (at which point _geometry/_collision don't exist yet), the C node's children array can be permanently out of sync with the real Python-level children list, for the life of the Link.

Link.collision's setter has the same gap in its SceneGroup branch (Link.py:921-928):

@collision.setter
def collision(self, coll: SceneGroup | list[Shape] | Shape):
    if isinstance(coll, list):
        self.collision.scene_children = coll       # OK -- goes through the real setter
    elif isinstance(coll, Shape):
        self.collision.scene_children.append(coll)  # OK, same reason
    elif isinstance(coll, SceneGroup):
        self._collision = coll                       # <-- bare attribute swap, no wiring at all

(geometry's setter, Link.py:930-937, is the same shape.)

Demonstrated -- via the real public API only, no internals poked

import roboticstoolbox as rtb
import spatialgeometry as gm
import spatialmath as sm

col = gm.Cuboid([1, 1, 1])
link = rtb.Link(rtb.ETS(rtb.ET.Rz()), collision=[col])
robot = rtb.Robot([link])
robot.q = [0.0]

probe = gm.Cuboid([1, 1, 1], pose=sm.SE3(10, 0, 0))
link.T = sm.SE3(10, 0, 0)
link.closest_point(probe)   # calls _propogate_scene_tree() internally

col._wT[:3, 3]   # stays [0, 0, 0] -- never moved, despite link.T having moved
link.closest_point(probe)   # returns (None, None, None) both before AND after moving link.T

A Link built this way can never detect collision against anything, no matter where it's actually posed -- not "slightly wrong distance," a complete silent failure with no exception raised anywhere.

The unresolved part -- why doesn't this break rtb.models.Panda()?

It doesn't. Re-ran RTB's own existing test_ELink.py::test_dist/test_collided assertions fresh against a real Panda() link and they all pass, matching the committed expected values exactly. But:

p = rtb.models.Panda()
link = p.links[3]
link.collision.scene_parent is link   # False -- same signature as the bug above

So URDF-loaded robots have the exact same missing back-reference, yet collision detection works correctly for them in practice. Something in the URDF-loading/robot-assembly path must be triggering __update_c() on each link incidentally (most likely something that calls scene_parent=/scene_children= on the link itself for an unrelated reason, which as a side effect syncs whatever _scene_children currently holds at that point). Did NOT trace this to a specific line before stopping -- confirmed the symptom (assembled Robot([link]) from a bare rtb.Link(collision=[...]), i.e. not going through URDF loading, still shows scene_parent is link == False and still fails the closest_point() test above) but didn't isolate what URDF loading specifically does differently.

This matters beyond just "is Panda safe": if correctness currently depends on an undocumented incidental side effect of one specific construction path rather than something structurally guaranteed, that's fragile -- a future robot-construction path (a different URDF loader, programmatic construction, DHRobot, PoERobot, whatever) that doesn't happen to trigger the same incidental fix would silently reproduce the standalone-Link failure above, with no warning.

Proposed fix

Replace the raw self._scene_children.append(...) calls in Link.__init__ with the real API -- either self._geometry.scene_parent = self / self._collision.scene_parent = self (lets the existing scene_parent setter do the correct wiring both directions), or restructure so _geometry/_collision get attached after super().__init__() via self.attach(...). Fix the collision/geometry setters' SceneGroup branch the same way (coll.scene_parent = self instead of the bare self._collision = coll swap).

Once fixed, add a regression test exercising exactly the standalone-Link scenario above (not just an assembled URDF robot, which -- per the previous section -- may mask the bug) -- assert link.collision.scene_parent is link and that closest_point()/iscollided() actually track a moved link's pose.

Also worth deliberately tracing the Panda-works-anyway mechanism once the fix lands, to confirm the fix doesn't change Panda's already-correct behavior and to actually close out the "why does this work today" question rather than leaving it as an accepted mystery.

Note on test placement

Don't add tests for this to spatialgeometry's own test suite -- RTB depends on SG, not the other way around; a regression test for a bug in RTB's Link.py belongs in RTB's own test suite (tests/test_ELink.py/tests/test_Robot.py already have relevant iscollided/closest_point tests to extend).