From 89648125ce80169210298a8ea2bcb87c1b7002eb Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 9 Aug 2026 13:59:36 +1000 Subject: [PATCH] fix: give SceneNode.__init__ a pose= kwarg, not just Shape SceneNode is the true base class for the scene graph, but pose= (with SE3 support) was only ever implemented on Shape.__init__, one level below it. SceneGroup subclasses SceneNode directly and skips Shape, so SceneGroup(pose=SE3(...)) raised TypeError -- only the raw T=ndarray kwarg worked. CollisionShapeGroup happened to accept pose= already, but only as a side effect of going through Shape/CollisionShape for unrelated reasons. Move pose (and its SE3->ndarray conversion) down to SceneNode.__init__ itself so every scene-graph node, groups included, gets it consistently. --- src/spatialgeometry/geom/SceneNode.py | 16 ++++++++++++++-- src/spatialgeometry/geom/Shape.py | 2 +- tests/test_Shape.py | 8 ++++++++ 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/spatialgeometry/geom/SceneNode.py b/src/spatialgeometry/geom/SceneNode.py index 0c71cf4..ad81e96 100644 --- a/src/spatialgeometry/geom/SceneNode.py +++ b/src/spatialgeometry/geom/SceneNode.py @@ -15,10 +15,22 @@ class SceneNode: def __init__( self, - T: ndarray = eye(4), + pose: ndarray | SE3 = eye(4), scene_parent: SceneNode | None = None, scene_children: list[SceneNode] | None = None, ) -> None: + """ + :param pose: Local reference frame of this node relative to its + parent in the scene graph (or the world frame if it has no + parent), defaults to the identity transform. + :param scene_parent: Parent node of this node in the scene graph. + :param scene_children: Child nodes of this node in the scene graph. + """ + if isinstance(pose, SE3): + T = pose.A + else: + T = pose + # These three are static attributes which can never be changed # If these are directly accessed and re-written, segmentation faults # will follow very soon after @@ -136,7 +148,7 @@ def __copy__(self): def __deepcopy__(self, memo): result = SceneNode( - T=self._T, + pose=self._T, ) result._scene_children = self.scene_children.copy() diff --git a/src/spatialgeometry/geom/Shape.py b/src/spatialgeometry/geom/Shape.py index b676989..e7e4e87 100644 --- a/src/spatialgeometry/geom/Shape.py +++ b/src/spatialgeometry/geom/Shape.py @@ -138,7 +138,7 @@ def __init__( self.color = color # Initialise the scene node - super().__init__(T=T, **kwargs) + super().__init__(pose=T, **kwargs) self.stype = stype self.v = zeros(6) diff --git a/tests/test_Shape.py b/tests/test_Shape.py index 9b38e1e..dc0b685 100644 --- a/tests/test_Shape.py +++ b/tests/test_Shape.py @@ -454,6 +454,14 @@ def test_scene_group_repr_and_str(self): self.assertEqual(repr(group), f"SceneGroup([{gm.Sphere(1.0)!r}])") self.assertTrue(str(group).startswith("SceneGroup at ")) + def test_scene_group_constructor_accepts_pose(self): + T = sm.SE3.Trans(1, 2, 3) + group = gm.SceneGroup(pose=T) + nt.assert_almost_equal(group._T, T.A) + + group2 = gm.SceneGroup(pose=T.A) + nt.assert_almost_equal(group2._T, T.A) + def test_scene_group_constructor_accepts_initial_elements(self): cube = gm.Cuboid([1, 1, 1]) sphere = gm.Sphere(1.0)