Skip to content

Commit bebede0

Browse files
petercorkeclaude
andauthored
fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one (#693)
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 <noreply@anthropic.com>
1 parent a0ef01c commit bebede0

2 files changed

Lines changed: 149 additions & 106 deletions

File tree

‎src/roboticstoolbox/robot/Robot.py‎

Lines changed: 61 additions & 77 deletions
Original file line numberDiff line numberDiff line change
@@ -1792,46 +1792,51 @@ def rne(
17921792

17931793
link_groups: list[list[int]] = []
17941794

1795-
# Group links together based on whether they are joints or not
1796-
# Static links are grouped with the first joint encountered
1797-
current_group = []
1795+
# Group each joint together with every static (fixed) link rigidly
1796+
# attached to -- and moving with -- its own output frame: found by
1797+
# walking each static link's ancestry (via .parent, not flat
1798+
# self.links order, so this is correct under branching too) up to
1799+
# its nearest joint. A static link ends up in that joint's group
1800+
# regardless of where it sits relative to the *next* joint in the
1801+
# chain -- immediately after this joint, sandwiched anywhere before
1802+
# the next one, or trailing after the very last joint with nothing
1803+
# further downstream (e.g. a tool flange/mount link, such as URDF
1804+
# Panda's panda_link8). A static link with no joint ancestor at all
1805+
# (rigidly mounted on the immovable base) contributes no joint
1806+
# torque and is dropped.
1807+
#
1808+
# An earlier version grouped a static link with the first joint
1809+
# encountered scanning *forward* -- correct for a trailing run
1810+
# (#636), but wrong for a sandwiched static link (joint_A -> static
1811+
# -> joint_B): that attaches the static link's mass to joint_B's
1812+
# group, when it's actually rigidly welded to joint_A's output and
1813+
# physically independent of joint_B's angle, silently misattributing
1814+
# its torque contribution to the wrong joint (#483).
1815+
group_of_link_idx: dict[int, int] = {}
17981816
for i, link in enumerate(self.links):
1799-
current_group.append(i)
1800-
1801-
# Break after adding the first link
18021817
if link.isjoint:
1803-
link_groups.append(current_group)
1804-
current_group = []
1805-
1806-
# A trailing run of static links after the *last* joint (e.g. a tool
1807-
# flange/mount link with no further joint after it, such as URDF
1808-
# Panda's panda_link8) never triggers the isjoint branch above, so
1809-
# current_group is left non-empty and was previously dropped on the
1810-
# floor here -- silently excluding that link's mass/inertia from
1811-
# every torque in the chain rather than raising or warning (#636).
1812-
# Fold it into the last real group instead of discarding it; the
1813-
# joint lookups below no longer assume the joint is positionally
1814-
# last within its group, so this is safe regardless of group order.
1815-
if current_group:
1816-
link_groups[-1].extend(current_group)
1818+
link_groups.append([i])
1819+
group_of_link_idx[i] = len(link_groups) - 1
1820+
elif link.parent is not None:
1821+
group_idx = group_of_link_idx.get(self.links.index(link.parent))
1822+
if group_idx is not None:
1823+
link_groups[group_idx].append(i)
1824+
group_of_link_idx[i] = group_idx
18171825

18181826
# Make some intermediate variables
18191827
for i, group in enumerate(link_groups):
18201828
I_int = SpatialInertia()
18211829

1822-
# Links after the group's joint (trailing static links folded in
1823-
# above) are rigidly carried by that joint's own output frame,
1824-
# but link.r/link.I are each expressed in that *link's own*
1825-
# frame, not the joint's. Compose their fixed transforms
1826-
# relative to the joint and carry the CoM/inertia into the
1827-
# joint's frame before summing -- otherwise (as originally
1828-
# written) a trailing link's mass lands in I_int with the wrong
1829-
# moment arm relative to this joint (effectively r=0), which
1830-
# for a non-negligible offset silently drops its contribution
1831-
# to this joint's torque. Links at-or-before the joint are
1832-
# summed as before (unchanged, uses the group's own existing,
1833-
# separately-verified convention -- see #636 for the case this
1834-
# does and does not cover).
1830+
# group[0] is always the joint itself (see the grouping loop
1831+
# above); everything after it in the group is a static link
1832+
# rigidly carried by that joint's own output frame, but
1833+
# link.r/link.I are each expressed in that *link's own* frame,
1834+
# not the joint's. Compose their fixed transforms relative to
1835+
# the joint and carry the CoM/inertia into the joint's frame
1836+
# before summing -- otherwise a static link's mass lands in
1837+
# I_int with the wrong moment arm relative to this joint
1838+
# (effectively r=0), which for a non-negligible offset silently
1839+
# drops its contribution to this joint's torque (#636, #483).
18351840
past_joint = False
18361841
T_from_joint = None
18371842
for idx in group:
@@ -1876,8 +1881,8 @@ def rne(
18761881
# where the indices correspond to the index of the group within
18771882
# link_groups
18781883
# As always, q, qd, qdd are lists of length n, where indices correspond
1879-
# to the jindex of the joint, which will be the last link in the group
1880-
# within link_groups
1884+
# to the jindex of the joint, which is always the first link in the
1885+
# group within link_groups
18811886

18821887
for k in range(l):
18831888
qk = q[k, :]
@@ -1886,51 +1891,32 @@ def rne(
18861891

18871892
# forward recursion
18881893
for j, group in enumerate(link_groups):
1889-
# The joint may not be the last link in the group -- a
1890-
# trailing run of static links with no further joint after
1891-
# them (see above) is folded onto the end of the last
1892-
# group, after its joint.
1893-
joint = next(self.links[idx] for idx in group if self.links[idx].isjoint)
1894+
# group[0] is always the joint (see the grouping loop
1895+
# above); any static links after it in the group are
1896+
# rigidly carried by this joint's own output frame and are
1897+
# folded into I[j] directly instead, not into this
1898+
# kinematic frame -- v[j]/a[j] stay defined at the joint's
1899+
# own output, matching s[j]/vJ, which are themselves only
1900+
# ever expressed there.
1901+
joint = self.links[group[0]]
18941902
jindex = joint.jindex
18951903

18961904
vJ = SpatialVelocity(s[j] * qdk[jindex])
18971905

1898-
# transform from parent(j) to j -- stop at the joint itself.
1899-
# Any trailing static links after it (see above) are rigidly
1900-
# carried by the joint's own output frame and are folded
1901-
# into I[j] directly instead, not into this kinematic
1902-
# frame -- v[j]/a[j] stay defined at the joint's own output,
1903-
# matching s[j]/vJ, which are themselves only ever
1904-
# expressed there.
1905-
first_element = True
1906-
for idx in group:
1907-
link = self.links[idx]
1908-
1909-
if link.isjoint and link.jindex is not None:
1910-
if first_element:
1911-
Xup_int = SE3(link.A(qk[link.jindex]))
1912-
first_element = False
1913-
else:
1914-
Xup_int = Xup_int * SE3(link.A(qk[link.jindex]))
1915-
break
1916-
else:
1917-
if first_element:
1918-
Xup_int = SE3(link.A())
1919-
first_element = False
1920-
else:
1921-
Xup_int = Xup_int * SE3(link.A())
1922-
1906+
# transform from parent(j) to j: joint.A() already
1907+
# incorporates any fixed transform within the joint link's
1908+
# own ETS (the joint variable is guaranteed to be the last
1909+
# element of its segment -- see this method's docstring),
1910+
# so no further composition is needed.
1911+
Xup_int = SE3(joint.A(qk[jindex]))
19231912
Xup[j] = Xup_int.inv() # type: ignore[union-attr]
19241913

1925-
# The first link in the group
1926-
first_link = self.links[group[0]]
1927-
1928-
if first_link.parent is None:
1914+
if joint.parent is None:
19291915
v[j] = vJ
19301916
a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[jindex])
19311917
else:
1932-
# The index of `link`s parent within self.links
1933-
parent_idx = self.links.index(first_link.parent)
1918+
# The index of `joint`s parent within self.links
1919+
parent_idx = self.links.index(joint.parent)
19341920

19351921
# The index of the group that the parent link is in
19361922
group_idx = [
@@ -1949,9 +1935,7 @@ def rne(
19491935
# Backward recursion
19501936
for j in reversed(range(n)):
19511937
group = link_groups[j]
1952-
joint = next(self.links[idx] for idx in group if self.links[idx].isjoint)
1953-
first_link = self.links[group[0]]
1954-
# link = self.links[j]
1938+
joint = self.links[group[0]] # always the joint -- see above
19551939

19561940
# next line could be dot(), but fails for symbolic arguments
19571941
Q[k, j] = sum(f[j].A * s[j])
@@ -1966,9 +1950,9 @@ def rne(
19661950
- joint.friction(qdk[jindex], coulomb=not symbolic)
19671951
)
19681952

1969-
if first_link.parent is not None:
1970-
# The index of `link`s parent within self.links
1971-
parent_idx = self.links.index(first_link.parent)
1953+
if joint.parent is not None:
1954+
# The index of `joint`s parent within self.links
1955+
parent_idx = self.links.index(joint.parent)
19721956

19731957
# The index of the group that the parent link is in
19741958
group_idx = [

‎tests/test_ERobot.py‎

Lines changed: 88 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -214,62 +214,121 @@ def test_invdyn_static(self):
214214
nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22])
215215

216216
def test_invdyn_static2(self):
217-
# create a 2 link robot
218-
# Example from Spong etal. 2nd edition, p. 260
217+
# create a 3 link robot: joint1 (Ry) -> l2 (*fixed*, tx(1)) ->
218+
# joint2 (Ry, massless -- l3 carries no mass, and l2 is rigidly
219+
# welded to joint1's output with no rotational freedom of its own).
220+
#
221+
# Unlike test_invdyn (whose second link rotates with joint2), l2's
222+
# position here depends only on q0, never on q1: joint2 has nothing
223+
# downstream with mass, so its manipulator-inertia row/column and
224+
# every velocity (Coriolis) term must be exactly zero, and M11 is a
225+
# constant rather than a function of q1. Verified against an
226+
# independent virtual-work (finite-difference of CoM height vs. q)
227+
# ground truth and by hand (M11 = m1*r1^2 + m2*(offset+r2)^2 =
228+
# 1*0.5^2 + 1*1.5^2 = 2.5), not just against rne() itself -- see
229+
# #483, where a sandwiched static link's mass was misattributed to
230+
# the *following* joint's group instead of the *preceding* one it's
231+
# rigidly attached to, and this test's previous expected values
232+
# (copied from test_invdyn's different topology, where l2 *does*
233+
# rotate with joint2) matched that bug rather than catching it.
219234
l1 = Link(ets=ETS(ET.Ry()), m=1, r=[0.5, 0, 0], name="l1")
220235
l2 = Link(ets=ETS(ET.tx(1)), m=1, r=[0.5, 0, 0], parent=l1, name="l2")
221236
l3 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], parent=l2, name="l3")
222237
robot = ERobot([l1, l2, l3], name="simple 3 link")
223238
z = np.zeros(robot.n)
224239

225-
# check gravity load
240+
# check gravity load -- tau1 is exactly zero at every configuration
226241
tau = robot.rne(z, z, z) / 9.81
227-
nt.assert_array_almost_equal(tau, np.r_[-2, -0.5])
242+
nt.assert_array_almost_equal(tau, np.r_[-2, 0])
228243

229244
tau = robot.rne(np.array([0.0, -pi / 2.0]), z, z) / 9.81
230-
nt.assert_array_almost_equal(tau, np.r_[-1.5, 0])
245+
nt.assert_array_almost_equal(tau, np.r_[-2, 0])
231246

232247
tau = robot.rne(np.array([-pi / 2, pi / 2]), z, z) / 9.81
233-
nt.assert_array_almost_equal(tau, np.r_[-0.5, -0.5])
248+
nt.assert_array_almost_equal(tau, np.r_[0, 0])
234249

235250
tau = robot.rne(np.array([-pi / 2, 0]), z, z) / 9.81
236251
nt.assert_array_almost_equal(tau, np.r_[0, 0])
237252

238-
# check velocity terms
253+
# check velocity terms -- M(q) doesn't depend on q at all (l2 is
254+
# rigidly fixed, l3 is massless), so every Coriolis/centrifugal
255+
# term is zero regardless of qd
239256
robot.gravity = [0, 0, 0]
240257
q = np.array([0, -pi / 2])
241-
h = -0.5 * sin(q[1])
242-
243-
tau = robot.rne(q, np.array([0, 0]), z)
244-
nt.assert_array_almost_equal(tau, np.r_[0, 0] * h)
245258

246-
tau = robot.rne(q, np.array([1, 0]), z)
247-
nt.assert_array_almost_equal(tau, np.r_[0, -1] * h)
248-
249-
tau = robot.rne(q, np.array([0, 1]), z)
250-
nt.assert_array_almost_equal(tau, np.r_[1, 0] * h)
251-
252-
tau = robot.rne(q, np.array([1, 1]), z)
253-
nt.assert_array_almost_equal(tau, np.r_[3, -1] * h)
254-
255-
# check inertial terms
256-
257-
d11 = 1.5 + cos(q[1])
258-
d12 = 0.25 + 0.5 * cos(q[1])
259-
d21 = d12
260-
d22 = 0.25
259+
for qd in (np.r_[0, 0], np.r_[1, 0], np.r_[0, 1], np.r_[1, 1]):
260+
tau = robot.rne(q, qd, z)
261+
nt.assert_array_almost_equal(tau, np.r_[0, 0])
261262

263+
# check inertial terms -- M11=2.5 (constant), M12=M21=M22=0
262264
tau = robot.rne(q, z, np.array([0, 0]))
263265
nt.assert_array_almost_equal(tau, np.r_[0, 0])
264266

265267
tau = robot.rne(q, z, np.array([1, 0]))
266-
nt.assert_array_almost_equal(tau, np.r_[d11, d21])
268+
nt.assert_array_almost_equal(tau, np.r_[2.5, 0])
267269

268270
tau = robot.rne(q, z, np.array([0, 1]))
269-
nt.assert_array_almost_equal(tau, np.r_[d12, d22])
271+
nt.assert_array_almost_equal(tau, np.r_[0, 0])
270272

271273
tau = robot.rne(q, z, np.array([1, 1]))
272-
nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22])
274+
nt.assert_array_almost_equal(tau, np.r_[2.5, 0])
275+
276+
def test_invdyn_sandwiched_static_link(self):
277+
# Regression test for #483: a static (fixed) link *sandwiched*
278+
# between two joints -- rigidly welded to the *preceding* joint's
279+
# output, not the *following* one -- had its mass misattributed to
280+
# the wrong joint's torque. The grouping logic fixed by #636
281+
# attached a static link to whichever joint came next scanning
282+
# forward through the link list, which is correct for a trailing
283+
# run but wrong here.
284+
#
285+
# joint1 (Ry, massless) -> l2 (fixed, tx(1), m=1, r=[0.5,0,0]) ->
286+
# joint2 (Ry, massless downstream -- nothing after it has mass).
287+
# l2 has no rotational freedom of its own, so joint2's torque must
288+
# be exactly zero at *every* configuration, for *any* q1 -- not
289+
# just at a few sampled points.
290+
joint1 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], name="joint1")
291+
l2 = Link(ets=ETS(ET.tx(1)), m=1, r=[0.5, 0, 0], parent=joint1, name="l2")
292+
joint2 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], parent=l2, name="joint2")
293+
robot = ERobot([joint1, l2, joint2], name="joint, sandwiched static link, joint")
294+
self.assertEqual(robot.n, 2)
295+
296+
z = np.zeros(robot.n)
297+
for q0 in (0.0, 0.5, -1.2, pi / 2):
298+
for q1 in (0.0, 0.7, -2.1, pi):
299+
tau = robot.rne(np.r_[q0, q1], z, z, gravity=[0, 0, -9.81])
300+
self.assertAlmostEqual(tau[1], 0.0, places=9)
301+
# joint1 carries l2's full weight at its fixed 1.5 m offset,
302+
# independent of q1 -- the standard single-point-mass
303+
# pendulum formula
304+
expected_tau0 = -1.0 * 9.81 * 1.5 * cos(q0)
305+
self.assertAlmostEqual(tau[0], expected_tau0, places=9)
306+
307+
def test_invdyn_sandwiched_static_link_with_inertia(self):
308+
# Same as above, but the sandwiched static link also carries a
309+
# nonzero inertia tensor and an off-axis r, exercising the general
310+
# CoM/inertia transform into the joint's frame rather than just a
311+
# simple point mass on the rotation axis.
312+
joint1 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], name="joint1")
313+
l2 = Link(
314+
ets=ETS(ET.tx(0.30)),
315+
m=2,
316+
r=[0.05, 0, 0],
317+
I=np.diag([0.001, 0.002, 0.003]),
318+
parent=joint1,
319+
name="l2",
320+
)
321+
joint2 = Link(ets=ETS(ET.Ry()), m=0, r=[0, 0, 0], parent=l2, name="joint2")
322+
robot = ERobot([joint1, l2, joint2], name="joint, sandwiched static link 2, joint")
323+
self.assertEqual(robot.n, 2)
324+
325+
z = np.zeros(robot.n)
326+
for q0 in (0.0, 0.5, -1.2):
327+
for q1 in (0.0, 0.7, -2.1):
328+
tau = robot.rne(np.r_[q0, q1], z, z, gravity=[0, 0, -9.81])
329+
self.assertAlmostEqual(tau[1], 0.0, places=9)
330+
expected_tau0 = -2.0 * 9.81 * 0.35 * cos(q0)
331+
self.assertAlmostEqual(tau[0], expected_tau0, places=9)
273332

274333
def test_invdyn_trailing_static_link(self):
275334
# Regression test for #636: a run of static (fixed) links *after*

0 commit comments

Comments
 (0)