Skip to content

Commit 13e62a7

Browse files
petercorkeclaude
andcommitted
fix(dynamics): guard Robot.rne() against standard-DH DHRobot instances
Root-caused why Robot.rne() (the separate ETS/Featherstone implementation) only ever worked for modified DH: its algorithm structurally requires the joint to be the last element of its own ETS segment (Featherstone's spatial-vector convention). This holds for Robot/ERobot/URDFRobot (built via ETS.split()), PoERobot (_update_ets() appends the joint last too), and -- checked carefully, since it wasn't obvious -- for DHRobot(mdh=True) too: DHLink._to_ets()'s MDH revolute branch reorders a nonzero d translation to precede the joint rotation specifically so the joint ET stays last; valid because a z-rotation and a z-translation about/along the same axis commute. It does not hold for DHRobot(mdh=False) (standard DH), where the joint comes before the link's fixed geometry -- structurally incompatible with the algorithm, not fixable without changing what Robot.rne() fundamentally assumes. Rather than teach Robot.rne() a second, DH-aware recursion, it now asserts on the incompatible case instead of silently returning a wrong answer: `assert getattr(self, "mdh", True)`. Checked via the mdh attribute rather than class identity -- an earlier class-name blocklist attempt, and a considered-and-rejected class-name allowlist, would each have gotten this wrong (the allowlist specifically would have rejected PoERobot, a fully compliant type). Design tradeoffs recorded in tech-debt.md, tied to the pending robot-class-hierarchy redesign since that may make the whole question moot. New diagnostic script examples/rne_dh_convention_check.py: a single-link revolute robot, checked against the Lagrangian-identity ground truth (independent of both RNE implementations), demonstrating rne_python() agrees for both DH conventions while Robot.rne() now cleanly rejects standard DH rather than silently mis-computing it. Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>
1 parent 6b28899 commit 13e62a7

4 files changed

Lines changed: 184 additions & 0 deletions

File tree

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
#!/usr/bin/env python
2+
"""Root-cause diagnostic for the Robot.rne() vs DHRobot.rne_python() divergence
3+
originally seen in rne_compare.py -- see rne.md (issue 6) for the full writeup.
4+
5+
Uses a single-link revolute robot with a pure axis twist (alpha != 0, a=d=0)
6+
to isolate the frame-convention question from friction/armature/multi-link
7+
recursion complexity. Ground truth is obtained *independently* of either RNE
8+
implementation via the Lagrangian identity: for a static pose (qd=qdd=0), the
9+
required joint torque equals dV/dq, where V(q) = m g z_com(q) is the
10+
gravitational potential energy of the link's centre of mass, computed
11+
directly from link.A(q) and numerically differentiated.
12+
13+
Historical note: this originally showed rne_python() wrong for modified DH
14+
and Robot.rne() wrong for standard DH -- three real bugs in rne_python()'s
15+
MDH branch (see rne.md) have since been fixed, so rne_python() now agrees
16+
with ground truth for both conventions. Robot.rne()'s standard-DH case is
17+
not a bug to fix: its Featherstone recursion structurally requires the
18+
joint to be the last element of its own ETS segment, which standard DH
19+
never satisfies (the joint comes first). Robot.rne() now asserts on that
20+
case (see Robot.py) instead of silently returning a wrong answer.
21+
"""
22+
23+
import numpy as np
24+
25+
from roboticstoolbox import DHRobot, RevoluteDH, RevoluteMDH
26+
from roboticstoolbox.robot.Robot import Robot as RobotBase
27+
28+
29+
def gravity_torque_truth(robot, q0, g=9.81, h=1e-6):
30+
"""Numerically-differentiated ground truth, independent of any RNE code."""
31+
link = robot.links[0]
32+
p_local = np.array([*link.r, 1.0])
33+
34+
def com_z(q):
35+
A = np.asarray(link.A(q))
36+
return (A @ p_local)[2]
37+
38+
return g * (com_z(q0 + h) - com_z(q0 - h)) / (2 * h)
39+
40+
41+
def check(label, robot, q0=0.3, robot_rne_expected_to_work=True):
42+
z = np.zeros(1)
43+
q = np.array([q0])
44+
truth = gravity_torque_truth(robot, q0)
45+
tau_py = robot.rne_python(q, z, z)[0]
46+
print(f"{label:28s} truth={truth:9.4f} rne_python={tau_py:9.4f}")
47+
print(f"{'':28s} |rne_python - truth| = {abs(tau_py - truth):.4f}")
48+
49+
if robot_rne_expected_to_work:
50+
tau_base = RobotBase.rne(robot, q, z, z)[0]
51+
print(f"{'':28s} Robot.rne={tau_base:9.4f}")
52+
print(f"{'':28s} |Robot.rne - truth| = {abs(tau_base - truth):.4f}")
53+
else:
54+
try:
55+
RobotBase.rne(robot, q, z, z)
56+
except AssertionError:
57+
print(f"{'':28s} Robot.rne: correctly rejected (AssertionError)")
58+
else:
59+
print(f"{'':28s} Robot.rne: ERROR -- expected AssertionError, got a result")
60+
61+
62+
print("Single-link revolute robot, alpha=1.2 rad, a=d=0, r=[0.5,0,0], static (qd=qdd=0)")
63+
print("Ground truth = d/dq[ m g z_com(q) ], independent of both RNE implementations.")
64+
print()
65+
66+
std = DHRobot([RevoluteDH(a=0, alpha=1.2, d=0, m=1.0, r=[0.5, 0, 0])], gravity=[0, 0, -9.81])
67+
check("Standard DH (mdh=False)", std, robot_rne_expected_to_work=False)
68+
69+
print()
70+
mdh = DHRobot([RevoluteMDH(a=0, alpha=1.2, d=0, m=1.0, r=[0.5, 0, 0])], gravity=[0, 0, -9.81])
71+
check("Modified DH (mdh=True)", mdh, robot_rne_expected_to_work=True)
72+
73+
print()
74+
print("Conclusion: rne_python is correct for both DH conventions.")
75+
print("Robot.rne is correct for modified DH, and now cleanly rejects (rather")
76+
print("than silently mis-computing) standard DH, since its Featherstone")
77+
print("recursion cannot represent standard DH's joint-first structure.")

src/roboticstoolbox/robot/Robot.py

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1617,8 +1617,49 @@ def rne(
16171617
- This version supports symbolic model parameters
16181618
- Verified against MATLAB code
16191619
1620+
.. warning::
1621+
1622+
Assumes each link's joint is the *last* element of its own ETS
1623+
segment (Featherstone's spatial-vector convention -- fixed
1624+
geometry gets you *to* the joint, the joint is the last thing
1625+
applied before the next link's frame). This is guaranteed for
1626+
any ``Robot``/``ERobot`` built normally, either from a raw ETS
1627+
(``Robot.__init__`` splits it via ``ETS.split()``, whose
1628+
default "last" method enforces this per segment), from a URDF
1629+
(each link's ETS is built fixed-transform-then-joint, in that
1630+
order), or from a ``PoERobot`` (``_update_ets()`` appends the
1631+
joint ET last too). It does **not** hold for a ``DHRobot``
1632+
using standard DH conventions (``mdh=False``), where the joint
1633+
comes *first*, followed by ``d``/``a``/``alpha`` -- calling
1634+
``Robot.rne(dh_instance, ...)`` directly (bypassing
1635+
``DHRobot``'s own correct ``rne()``/``rne_python()``) gives
1636+
silently wrong answers. A ``DHRobot`` built with ``mdh=True``
1637+
*is* joint-last (``DHLink._to_ets()``'s MDH branch reorders a
1638+
revolute link's ``d`` translation to precede the joint
1639+
rotation -- valid since a z-rotation and a z-translation
1640+
commute -- so the joint ET is always last regardless of ``d``)
1641+
and works correctly through this path. See rne.md /
1642+
tech-debt.md.
16201643
"""
16211644

1645+
# Checked via the `mdh` attribute rather than isinstance/class name:
1646+
# joint-last compliance tracks the DH convention actually in use
1647+
# (DHLink._to_ets() puts the joint last for mdh=True, not for
1648+
# mdh=False), not the DHRobot class itself -- a DHRobot(mdh=True)
1649+
# instance is structurally fine here (verified numerically against
1650+
# rne_python() with nonzero d/alpha/mass/inertia). Non-DHRobot
1651+
# types have no `mdh` attribute and default (via getattr) to True,
1652+
# since their construction (ETS.split(), URDF, PoERobot's
1653+
# _update_ets()) already guarantees joint-last independently of DH
1654+
# conventions.
1655+
assert getattr(self, "mdh", True), (
1656+
"Robot.rne() assumes each link's joint is the last element of "
1657+
"its own ETS segment, which does not hold for a DHRobot built "
1658+
"with mdh=False (standard DH). Call DHRobot's own "
1659+
"rne()/rne_python() instead of Robot.rne(dh_instance, ...) "
1660+
"directly."
1661+
)
1662+
16221663
n = self.n
16231664
# n = len(self.links)
16241665

tech-debt.md

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,56 @@ in a major version increment, retaining `Robot` as a deprecated alias.
5757
**Best done alongside:** the ETS/fknm refactor — the type picture simplifies
5858
dramatically if hierarchy and representation are both cleaned up together.
5959

60+
### Related: `Robot.rne()`'s misuse guard checks `mdh`, not class identity
61+
62+
`Robot.rne()` (`Robot.py`) assumes Featherstone's joint-last-in-segment
63+
structure. A cheap runtime guard rejects the incompatible case:
64+
65+
```python
66+
assert getattr(self, "mdh", True), ...
67+
```
68+
69+
Two design questions came up while adding this guard 2026-07-21, both worth
70+
recording since they'll resurface if the class hierarchy redesign above
71+
happens:
72+
73+
1. **Blocklist vs. allowlist.** First attempt was a class-name blocklist
74+
(`assert not any(c.__name__ == "DHRobot" for c in type(self).__mro__)`).
75+
An allowlist (`Robot`/`ERobot`/`URDFRobot` by name) was considered and
76+
rejected: it would have wrongly rejected `PoERobot`, a fully compliant
77+
type (its `_update_ets()` also appends the joint ET last) that was easy
78+
to overlook — demonstrating exactly the fragility an allowlist has here.
79+
2. **Class identity was the wrong axis entirely.** `DHLink._to_ets()` shows
80+
joint-last compliance actually tracks the `mdh` flag, not the `DHRobot`
81+
class: the MDH branch reorders a revolute link's `d` translation to
82+
*precede* the joint rotation (valid — a z-rotation and z-translation
83+
about/along the same axis commute), so the joint ET is last regardless
84+
of `d`. A `DHRobot(mdh=True)` instance is therefore structurally fine
85+
through `Robot.rne()`, while `mdh=False` is not — the class-name check
86+
would have wrongly rejected the compliant MDH case too. The final guard
87+
checks `self.mdh` directly (defaulting to `True` via `getattr` for
88+
non-DHRobot types, which have no DH-convention concept and are already
89+
guaranteed joint-last some other way).
90+
91+
Verifying the MDH case numerically (nonzero `d`, `alpha`, mass, and full
92+
inertia tensor, compared against `rne_python()`) surfaced a real, separate
93+
bug it would otherwise have masked: `SpatialInertia(m=link.m, r=link.r)` in
94+
`Robot.rne()`'s inertia accumulation never passed `I=link.I` — the
95+
rotational inertia tensor was silently dropped for every link, for every
96+
`Robot`/`ERobot`/`URDFRobot`/`PoERobot`/MDH-`DHRobot` call, not just the MDH
97+
edge case. Fixed alongside this guard. Not caught earlier because the
98+
`TwoLink`-based `Robot.rne()`-vs-`rne_python()` equivalence tests used
99+
default (zero) inertia; the tests that *did* set `inertia=True` only
100+
compared the C path against `rne_python()`, never `Robot.rne()`.
101+
102+
**Revisit when the class hierarchy redesign above happens.** If `DHRobot`
103+
stops being a `Robot` subclass (e.g. becomes `Robot[DHLink]` under the
104+
generic-`LinkType` proposal, or the "one Robot class, polymorphic `Link.A(q)`"
105+
design below is adopted), the whole question may become moot — either there's
106+
only one `Robot` class and the guard is unnecessary, or the DH-convention
107+
distinction is structurally explicit rather than a name-based/attribute-based
108+
runtime check.
109+
60110
---
61111

62112
## Forward-looking design: one Robot class, polymorphic Link.A(q)

tests/test_fknm_fallback.py

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -850,6 +850,22 @@ def test_mdh_rne_python_now_agrees(self):
850850
mdh_py = self.mdh.rne_python(q, z, z)
851851
nt.assert_array_almost_equal(mdh_py, truth, decimal=4)
852852

853+
def test_robot_rne_rejects_standard_dh(self):
854+
"""Robot.rne() (the ETS/Featherstone implementation, used by
855+
ERobot/URDF/PoERobot) genuinely cannot handle a standard-DH
856+
(mdh=False) DHRobot -- the joint isn't the last element of its own
857+
ETS segment. Rather than silently returning a wrong torque (the
858+
old behaviour -- see rne.md/tech-debt.md), it now asserts. This
859+
confirms the guard fires for the one case it must, complementing
860+
test_robot_rne_on_mdh_variant_matches_standard_dh_rne_python (which
861+
confirms it does *not* fire, and gives correct results, for the
862+
mdh=True case)."""
863+
from roboticstoolbox.robot.Robot import Robot as RobotBase
864+
865+
z = np.zeros(2)
866+
with self.assertRaises(AssertionError):
867+
RobotBase.rne(self.std, self.poses[0], z, z)
868+
853869

854870
# ---------------------------------------------------------------------------
855871
# Actuator dynamics (Jm, G, B, Tc) and non-zero link inertia: C vs

0 commit comments

Comments
 (0)