diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 79f34a14e..33878731c 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/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 diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index 544c41f17..4b7e2b0d9 100644 --- a/tests/test_ERobot.py +++ b/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