Skip to content

fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one #693

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
petercorke merged 1 commit into from
Sep 19, 2026
+149 −106
Merged

fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one #693

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
138 changes: 61 additions & 77 deletions src/roboticstoolbox/robot/Robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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, :]
Expand All @@ -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 = [
Expand All @@ -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])
Expand All @@ -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 = [
Expand Down
117 changes: 88 additions & 29 deletions tests/test_ERobot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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*
Expand Down
Loading