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*