fix(rne): don't crash when a joint's kinematic parent is a base-mounted static link

src/roboticstoolbox/robot/Robot.py

@@ -1911,18 +1911,23 @@ def rne(
                 Xup_int = SE3(joint.A(qk[jindex]))
                 Xup[j] = Xup_int.inv()  # type: ignore[union-attr]
 
-                if joint.parent is None:
+                # group_of_link_idx also covers a joint whose immediate
+                # .parent is a static link, or a run of them, rather than
+                # another joint directly -- returning None (rather than
+                # raising) when that walk reaches the root with no joint
+                # ancestor at all (e.g. a static base link, such as URDF
+                # Panda's panda_link0), which is kinematically equivalent
+                # to .parent being None.
+                group_idx = (
+                    group_of_link_idx.get(self.links.index(joint.parent))
+                    if joint.parent is not None
+                    else None
+                )
+
+                if group_idx is None:
                     v[j] = vJ
                     a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[jindex])
                 else:
-                    # The index of `joint`s parent within self.links
-                    parent_idx = self.links.index(joint.parent)
-
-                    # The index of the group that the parent link is in
-                    group_idx = [
-                        i for i, group in enumerate(link_groups) if parent_idx in group
-                    ][0]
-
                     v[j] = Xup[j] * v[group_idx] + vJ
                     a[j] = (
                         Xup[j] * a[group_idx]
@@ -1950,15 +1955,17 @@ def rne(
                     - joint.friction(qdk[jindex], coulomb=not symbolic)
                 )
 
-                if joint.parent is not None:
-                    # The index of `joint`s parent within self.links
-                    parent_idx = self.links.index(joint.parent)
-
-                    # The index of the group that the parent link is in
-                    group_idx = [
-                        i for i, group in enumerate(link_groups) if parent_idx in group
-                    ][0]
+                # See the forward recursion above: group_of_link_idx
+                # resolves through any static link(s) between this joint
+                # and its nearest joint ancestor, returning None if there
+                # isn't one (root).
+                group_idx = (
+                    group_of_link_idx.get(self.links.index(joint.parent))
+                    if joint.parent is not None
+                    else None
+                )
 
+                if group_idx is not None:
                     # Xup[j] is the child<-parent motion transform (v_child =
                     # Xup[j] * v_parent); propagating a force the other way,
                     # child->parent, needs the transform in the other

tests/test_ERobot.py

@@ -330,6 +330,32 @@ def test_invdyn_sandwiched_static_link_with_inertia(self):
                 expected_tau0 = -2.0 * 9.81 * 0.35 * cos(q0)
                 self.assertAlmostEqual(tau[0], expected_tau0, places=9)
 
+    def test_invdyn_base_mounted_static_link(self):
+        # Regression test: a static (fixed) link *before* the first joint,
+        # rigidly mounted directly on the immovable base -- e.g. URDF
+        # Panda's panda_link0 -- has no joint ancestor at all. Grouping it
+        # correctly (attaching it to nothing, since it contributes no joint
+        # torque) previously broke the *kinematic* parent lookup for the
+        # first joint itself: that joint's own .parent is this static base
+        # link, and resolving its upstream group via raw list-membership
+        # search found the link in no group at all (since it was
+        # deliberately dropped, not kinematically missing) and crashed with
+        # IndexError, rather than being treated as having no upstream joint
+        # (the same as .parent being None outright).
+        base = Link(ets=ETS(), m=5, r=[0.1, 0, 0], name="base")
+        joint1 = Link(ets=ETS(ET.Ry()), m=1, r=[0.5, 0, 0], parent=base, name="joint1")
+        robot = ERobot([base, joint1], name="base-mounted static link then joint")
+        self.assertEqual(robot.n, 1)
+
+        z = np.zeros(robot.n)
+        for q in (0.0, 0.5, -1.2, pi / 2):
+            tau = robot.rne(np.r_[q], z, z, gravity=[0, 0, -9.81])
+            # base is rigidly fixed to the world -- it never moves, so it
+            # contributes nothing to any joint torque regardless of its
+            # own mass/r
+            expected = -1.0 * 9.81 * 0.5 * cos(q)
+            self.assertAlmostEqual(tau[0], expected, places=9)
+
     def test_invdyn_trailing_static_link(self):
         # Regression test for #636: a run of static (fixed) links *after*
         # the last joint -- e.g. a tool-mount flange with no further joint