From e4fb84abb7afc82fbd95fd549db41172665e389e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sat, 19 Sep 2026 14:27:08 +0200 Subject: [PATCH] fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one Robot.rne()'s link-grouping attached a static (fixed) link to the first joint encountered scanning *forward* through the link list -- correct for a trailing run of static links after the last joint (#636), but wrong for a static link sandwiched between two joints: it's rigidly welded to the *preceding* joint's output, not the following one, so its mass was being misattributed to the wrong joint's torque. Restructures the grouping to walk each static link's .parent chain up to its nearest joint ancestor (correct under branching too, not just a flat list scan), which also lets group[0] be unconditionally the joint -- simplifying the forward/backward recursion's joint lookups and the kinematic-transform construction that #636's fix had to special-case. test_invdyn_static2's previous expected values were themselves wrong, copied from test_invdyn's different topology (where the second link *does* rotate with the second joint) rather than derived for its own (where the fixed link's position is independent of the second joint's angle, so that joint's torque, Coriolis terms, and inertia-matrix row/column must all be exactly zero) -- so the bug was being actively protected by a passing test rather than caught by one. Rederived and verified against an independent virtual-work (finite-difference of CoM height vs. q) ground truth, not just against rne() itself. Also adds a dedicated regression test (test_invdyn_sandwiched_static_link, +inertia variant) isolating the exact scenario from #483. Fixes #483. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/Robot.py | 138 +++++++++++++---------------- tests/test_ERobot.py | 117 ++++++++++++++++++------ 2 files changed, 149 insertions(+), 106 deletions(-) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 7a9833a9f..79f34a14e 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1792,46 +1792,51 @@ def rne( link_groups: list[list[int]] = [] - # Group links together based on whether they are joints or not - # Static links are grouped with the first joint encountered - current_group = [] + # Group each joint together with every static (fixed) link rigidly + # attached to -- and moving with -- its own output frame: found by + # walking each static link's ancestry (via .parent, not flat + # self.links order, so this is correct under branching too) up to + # its nearest joint. A static link ends up in that joint's group + # regardless of where it sits relative to the *next* joint in the + # chain -- immediately after this joint, sandwiched anywhere before + # the next one, or trailing after the very last joint with nothing + # further downstream (e.g. a tool flange/mount link, such as URDF + # Panda's panda_link8). A static link with no joint ancestor at all + # (rigidly mounted on the immovable base) contributes no joint + # torque and is dropped. + # + # An earlier version grouped a static link with the first joint + # encountered scanning *forward* -- correct for a trailing run + # (#636), but wrong for a sandwiched static link (joint_A -> static + # -> joint_B): that attaches the static link's mass to joint_B's + # group, when it's actually rigidly welded to joint_A's output and + # physically independent of joint_B's angle, silently misattributing + # its torque contribution to the wrong joint (#483). + group_of_link_idx: dict[int, int] = {} for i, link in enumerate(self.links): - current_group.append(i) - - # Break after adding the first link if link.isjoint: - link_groups.append(current_group) - current_group = [] - - # A trailing run of static links after the *last* joint (e.g. a tool - # flange/mount link with no further joint after it, such as URDF - # Panda's panda_link8) never triggers the isjoint branch above, so - # current_group is left non-empty and was previously dropped on the - # floor here -- silently excluding that link's mass/inertia from - # every torque in the chain rather than raising or warning (#636). - # Fold it into the last real group instead of discarding it; the - # joint lookups below no longer assume the joint is positionally - # last within its group, so this is safe regardless of group order. - if current_group: - link_groups[-1].extend(current_group) + link_groups.append([i]) + group_of_link_idx[i] = len(link_groups) - 1 + elif link.parent is not None: + group_idx = group_of_link_idx.get(self.links.index(link.parent)) + if group_idx is not None: + link_groups[group_idx].append(i) + group_of_link_idx[i] = group_idx # Make some intermediate variables for i, group in enumerate(link_groups): I_int = SpatialInertia() - # Links after the group's joint (trailing static links folded in - # above) are rigidly carried by that joint's own output frame, - # but link.r/link.I are each expressed in that *link's own* - # frame, not the joint's. Compose their fixed transforms - # relative to the joint and carry the CoM/inertia into the - # joint's frame before summing -- otherwise (as originally - # written) a trailing link's mass lands in I_int with the wrong - # moment arm relative to this joint (effectively r=0), which - # for a non-negligible offset silently drops its contribution - # to this joint's torque. Links at-or-before the joint are - # summed as before (unchanged, uses the group's own existing, - # separately-verified convention -- see #636 for the case this - # does and does not cover). + # group[0] is always the joint itself (see the grouping loop + # above); everything after it in the group is a static link + # rigidly carried by that joint's own output frame, but + # link.r/link.I are each expressed in that *link's own* frame, + # not the joint's. Compose their fixed transforms relative to + # the joint and carry the CoM/inertia into the joint's frame + # before summing -- otherwise a static link's mass lands in + # I_int with the wrong moment arm relative to this joint + # (effectively r=0), which for a non-negligible offset silently + # drops its contribution to this joint's torque (#636, #483). past_joint = False T_from_joint = None for idx in group: @@ -1876,8 +1881,8 @@ def rne( # where the indices correspond to the index of the group within # link_groups # As always, q, qd, qdd are lists of length n, where indices correspond - # to the jindex of the joint, which will be the last link in the group - # within link_groups + # to the jindex of the joint, which is always the first link in the + # group within link_groups for k in range(l): qk = q[k, :] @@ -1886,51 +1891,32 @@ def rne( # forward recursion for j, group in enumerate(link_groups): - # The joint may not be the last link in the group -- a - # trailing run of static links with no further joint after - # them (see above) is folded onto the end of the last - # group, after its joint. - joint = next(self.links[idx] for idx in group if self.links[idx].isjoint) + # group[0] is always the joint (see the grouping loop + # above); any static links after it in the group are + # rigidly carried by this joint's own output frame and are + # folded into I[j] directly instead, not into this + # kinematic frame -- v[j]/a[j] stay defined at the joint's + # own output, matching s[j]/vJ, which are themselves only + # ever expressed there. + joint = self.links[group[0]] jindex = joint.jindex vJ = SpatialVelocity(s[j] * qdk[jindex]) - # transform from parent(j) to j -- stop at the joint itself. - # Any trailing static links after it (see above) are rigidly - # carried by the joint's own output frame and are folded - # into I[j] directly instead, not into this kinematic - # frame -- v[j]/a[j] stay defined at the joint's own output, - # matching s[j]/vJ, which are themselves only ever - # expressed there. - first_element = True - for idx in group: - link = self.links[idx] - - if link.isjoint and link.jindex is not None: - if first_element: - Xup_int = SE3(link.A(qk[link.jindex])) - first_element = False - else: - Xup_int = Xup_int * SE3(link.A(qk[link.jindex])) - break - else: - if first_element: - Xup_int = SE3(link.A()) - first_element = False - else: - Xup_int = Xup_int * SE3(link.A()) - + # transform from parent(j) to j: joint.A() already + # incorporates any fixed transform within the joint link's + # own ETS (the joint variable is guaranteed to be the last + # element of its segment -- see this method's docstring), + # so no further composition is needed. + Xup_int = SE3(joint.A(qk[jindex])) Xup[j] = Xup_int.inv() # type: ignore[union-attr] - # The first link in the group - first_link = self.links[group[0]] - - if first_link.parent is None: + if joint.parent is None: v[j] = vJ a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[jindex]) else: - # The index of `link`s parent within self.links - parent_idx = self.links.index(first_link.parent) + # 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 = [ @@ -1949,9 +1935,7 @@ def rne( # Backward recursion for j in reversed(range(n)): group = link_groups[j] - joint = next(self.links[idx] for idx in group if self.links[idx].isjoint) - first_link = self.links[group[0]] - # link = self.links[j] + joint = self.links[group[0]] # always the joint -- see above # next line could be dot(), but fails for symbolic arguments Q[k, j] = sum(f[j].A * s[j]) @@ -1966,9 +1950,9 @@ def rne( - joint.friction(qdk[jindex], coulomb=not symbolic) ) - if first_link.parent is not None: - # The index of `link`s parent within self.links - parent_idx = self.links.index(first_link.parent) + 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 = [ diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index 7f1e50f8e..544c41f17 100644 --- a/tests/test_ERobot.py +++ b/tests/test_ERobot.py @@ -214,62 +214,121 @@ def test_invdyn_static(self): nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22]) def test_invdyn_static2(self): - # create a 2 link robot - # Example from Spong etal. 2nd edition, p. 260 + # create a 3 link robot: joint1 (Ry) -> l2 (*fixed*, tx(1)) -> + # joint2 (Ry, massless -- l3 carries no mass, and l2 is rigidly + # welded to joint1's output with no rotational freedom of its own). + # + # Unlike test_invdyn (whose second link rotates with joint2), l2's + # position here depends only on q0, never on q1: joint2 has nothing + # downstream with mass, so its manipulator-inertia row/column and + # every velocity (Coriolis) term must be exactly zero, and M11 is a + # constant rather than a function of q1. Verified against an + # independent virtual-work (finite-difference of CoM height vs. q) + # ground truth and by hand (M11 = m1*r1^2 + m2*(offset+r2)^2 = + # 1*0.5^2 + 1*1.5^2 = 2.5), not just against rne() itself -- see + # #483, where a sandwiched static link's mass was misattributed to + # the *following* joint's group instead of the *preceding* one it's + # rigidly attached to, and this test's previous expected values + # (copied from test_invdyn's different topology, where l2 *does* + # rotate with joint2) matched that bug rather than catching it. l1 = Link(ets=ETS(ET.Ry()), m=1, r=[0.5, 0, 0], name="l1") l2 = Link(ets=ETS(ET.tx(1)), m=1, r=[0.5, 0, 0], parent=l1, name="l2") l3 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], parent=l2, name="l3") robot = ERobot([l1, l2, l3], name="simple 3 link") z = np.zeros(robot.n) - # check gravity load + # check gravity load -- tau1 is exactly zero at every configuration tau = robot.rne(z, z, z) / 9.81 - nt.assert_array_almost_equal(tau, np.r_[-2, -0.5]) + nt.assert_array_almost_equal(tau, np.r_[-2, 0]) tau = robot.rne(np.array([0.0, -pi / 2.0]), z, z) / 9.81 - nt.assert_array_almost_equal(tau, np.r_[-1.5, 0]) + nt.assert_array_almost_equal(tau, np.r_[-2, 0]) tau = robot.rne(np.array([-pi / 2, pi / 2]), z, z) / 9.81 - nt.assert_array_almost_equal(tau, np.r_[-0.5, -0.5]) + nt.assert_array_almost_equal(tau, np.r_[0, 0]) tau = robot.rne(np.array([-pi / 2, 0]), z, z) / 9.81 nt.assert_array_almost_equal(tau, np.r_[0, 0]) - # check velocity terms + # check velocity terms -- M(q) doesn't depend on q at all (l2 is + # rigidly fixed, l3 is massless), so every Coriolis/centrifugal + # term is zero regardless of qd robot.gravity = [0, 0, 0] q = np.array([0, -pi / 2]) - h = -0.5 * sin(q[1]) - - tau = robot.rne(q, np.array([0, 0]), z) - nt.assert_array_almost_equal(tau, np.r_[0, 0] * h) - tau = robot.rne(q, np.array([1, 0]), z) - nt.assert_array_almost_equal(tau, np.r_[0, -1] * h) - - tau = robot.rne(q, np.array([0, 1]), z) - nt.assert_array_almost_equal(tau, np.r_[1, 0] * h) - - tau = robot.rne(q, np.array([1, 1]), z) - nt.assert_array_almost_equal(tau, np.r_[3, -1] * h) - - # check inertial terms - - d11 = 1.5 + cos(q[1]) - d12 = 0.25 + 0.5 * cos(q[1]) - d21 = d12 - d22 = 0.25 + for qd in (np.r_[0, 0], np.r_[1, 0], np.r_[0, 1], np.r_[1, 1]): + tau = robot.rne(q, qd, z) + nt.assert_array_almost_equal(tau, np.r_[0, 0]) + # check inertial terms -- M11=2.5 (constant), M12=M21=M22=0 tau = robot.rne(q, z, np.array([0, 0])) nt.assert_array_almost_equal(tau, np.r_[0, 0]) tau = robot.rne(q, z, np.array([1, 0])) - nt.assert_array_almost_equal(tau, np.r_[d11, d21]) + nt.assert_array_almost_equal(tau, np.r_[2.5, 0]) tau = robot.rne(q, z, np.array([0, 1])) - nt.assert_array_almost_equal(tau, np.r_[d12, d22]) + nt.assert_array_almost_equal(tau, np.r_[0, 0]) tau = robot.rne(q, z, np.array([1, 1])) - nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22]) + nt.assert_array_almost_equal(tau, np.r_[2.5, 0]) + + def test_invdyn_sandwiched_static_link(self): + # Regression test for #483: a static (fixed) link *sandwiched* + # between two joints -- rigidly welded to the *preceding* joint's + # output, not the *following* one -- had its mass misattributed to + # the wrong joint's torque. The grouping logic fixed by #636 + # attached a static link to whichever joint came next scanning + # forward through the link list, which is correct for a trailing + # run but wrong here. + # + # joint1 (Ry, massless) -> l2 (fixed, tx(1), m=1, r=[0.5,0,0]) -> + # joint2 (Ry, massless downstream -- nothing after it has mass). + # l2 has no rotational freedom of its own, so joint2's torque must + # be exactly zero at *every* configuration, for *any* q1 -- not + # just at a few sampled points. + joint1 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], name="joint1") + l2 = Link(ets=ETS(ET.tx(1)), m=1, r=[0.5, 0, 0], parent=joint1, name="l2") + joint2 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], parent=l2, name="joint2") + robot = ERobot([joint1, l2, joint2], name="joint, sandwiched static link, joint") + self.assertEqual(robot.n, 2) + + z = np.zeros(robot.n) + for q0 in (0.0, 0.5, -1.2, pi / 2): + for q1 in (0.0, 0.7, -2.1, pi): + tau = robot.rne(np.r_[q0, q1], z, z, gravity=[0, 0, -9.81]) + self.assertAlmostEqual(tau[1], 0.0, places=9) + # joint1 carries l2's full weight at its fixed 1.5 m offset, + # independent of q1 -- the standard single-point-mass + # pendulum formula + expected_tau0 = -1.0 * 9.81 * 1.5 * cos(q0) + self.assertAlmostEqual(tau[0], expected_tau0, places=9) + + def test_invdyn_sandwiched_static_link_with_inertia(self): + # Same as above, but the sandwiched static link also carries a + # nonzero inertia tensor and an off-axis r, exercising the general + # CoM/inertia transform into the joint's frame rather than just a + # simple point mass on the rotation axis. + joint1 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], name="joint1") + l2 = Link( + ets=ETS(ET.tx(0.30)), + m=2, + r=[0.05, 0, 0], + I=np.diag([0.001, 0.002, 0.003]), + parent=joint1, + name="l2", + ) + joint2 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], parent=l2, name="joint2") + robot = ERobot([joint1, l2, joint2], name="joint, sandwiched static link 2, joint") + self.assertEqual(robot.n, 2) + + z = np.zeros(robot.n) + for q0 in (0.0, 0.5, -1.2): + for q1 in (0.0, 0.7, -2.1): + tau = robot.rne(np.r_[q0, q1], z, z, gravity=[0, 0, -9.81]) + self.assertAlmostEqual(tau[1], 0.0, places=9) + expected_tau0 = -2.0 * 9.81 * 0.35 * cos(q0) + self.assertAlmostEqual(tau[0], expected_tau0, places=9) def test_invdyn_trailing_static_link(self): # Regression test for #636: a run of static (fixed) links *after*