fix(dynamics): fix three bugs in rne_python()'s modified-DH branch

Found by term-by-term comparison against ne.c's MODIFIED branch, while
exercising an mdh=True + rotated-base case (TwoLink) that had apparently
never been exercised before:

1. Base rotation applied to gravity twice: once (correctly) before the
   recursion loop via vd = Rb @ vd, and again via a redundant
   `if j == 0: if base: Tj = base @ Tj; pstar = base @ pstar` block for
   the first link -- double-counted, not just wrong. Removed the block
   entirely; base rotation only needs applying once.
2. Missing parentheses in the MDH revolute case's linear acceleration
   formula: `Rt @ cross(wd, pstar) + cross(w, cross(w, pstar)) + vd`
   should distribute Rt over the whole sum, per ne.c's MODIFIED-DH branch
   (rot_trans_vect_mult applied to the full bracket) -- the prismatic case
   right below it already had this right.
3. The backward recursion's moment equation used pstar (the *next* link's
   offset) where it should use r (this link's own CoM offset) for the
   this-link-force-to-torque term -- ne.c's equivalent is R_COG(j) x F,
   not PSTAR(j+1) x F.

With all three fixed, rne_python() agrees with the C extension for both DH
conventions, not just standard DH.

Also fixes TwoLink's mdh=True variant, which was copying the standard-DH
a/alpha values directly onto RevoluteMDH links -- not how the DH<->MDH
conversion works (it shifts a/alpha to the previous link index). Adds
inertia=True support (real per-link inertia tensors via a tubular-link
cylinder-inertia helper) so TwoLink can exercise non-trivial dynamics
parameters, not just point masses.

New tests: TestTwoLinkDHMDHEquivalence (TwoLink(mdh=False) and
TwoLink(mdh=True) are the same physical robot -- fkine agreement, rne
agreement between C and rne_python, and confirms rne_python's MDH branch
now agrees with the standard-DH ground truth) and TestTwoLinkActuatorDynamics
(C vs rne_python with non-zero Jm/G/B/Tc and real inertia tensors, for both
DH conventions -- TwoLink is all-zero for these by default, so nothing
before this exercised the actuator-dynamics terms through rne() at all).

Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>

Author: petercorke

src/roboticstoolbox/models/DH/TwoLink.py

@@ -2,7 +2,7 @@
 @author: Peter Corke
 """
 
-from roboticstoolbox import DHRobot, RevoluteDH
+from roboticstoolbox import DHRobot, RevoluteDH, RevoluteMDH
 
 # from math import pi
 from spatialmath import SE3
@@ -14,7 +14,8 @@ class TwoLink(DHRobot):
     Class that models a 2-link robot moving in the vertical plane
 
     :param symbolic: use symbolic constants
-    :type symbolic: bool
+    :param mdh: create a model using modified DH parameters, otherwise standard DH parameters are used
+    :param inertia: include link inertias, otherwise they are set to zero
 
     ``TwoLink()`` is a class which models a 2-link planar robot and
     describes its kinematic and dynamic characteristics using standard DH
@@ -27,16 +28,17 @@ class TwoLink(DHRobot):
         >>> robot = rtb.models.DH.TwoLink()
         >>> print(robot)
 
-    The parameters values depend on the ``symbolic`` parameter
+    The parameters values depend on the ``symbolic`` and ``mdh`` parameters
 
-    =======================================  =================  ==============
-    Parameters                               Numeric values     Symbolic values
-    =======================================  =================  ==============
-    link lengths                             1, 1               a1, a2
-    link masses                              1, 1               m1, m2
-    link CoMs in the link frame x-direction  -0.5, -0.5         c1, c2
-    gravitational acceleration               9.8                g
-    =======================================  =================  ==============
+    ===========================================  =================  ==============
+    Parameters                                   Numeric values     Symbolic values
+    ===========================================  =================  ==============
+    link lengths                                 1, 1               a1, a2
+    link masses                                  1, 1               m1, m2
+    link CoMs in the link frame x-direction DH   -0.5, -0.5         c1, c2
+    link CoMs in the link frame x-direction MDH  0.5, 0.5           c1, c2
+    gravitational acceleration                   9.8                g
+    ===========================================  =================  ==============
 
     Defined joint configurations are:
 
@@ -49,44 +51,80 @@ class TwoLink(DHRobot):
 
         - Robot has only 2 DoF.
         - Motor inertia is 0.
-        - Link inertias are 0.
+        - Link inertias are 0 unless ``inertia`` is True.
         - Viscous and Coulomb friction is 0.
 
     :Reference: Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition).
 
     .. codeauthor:: Peter Corke
     """
 
-    def __init__(self, symbolic=False):
+    def __init__(self, symbolic:bool=False, mdh:bool=False, inertia:bool=False):
 
         if symbolic:
             import spatialmath.base.symbolic as sym
 
             zero = sym.zero()
             pi = sym.pi()
-            a1, a2 = sym.symbol("a1 a2")  # type: ignore
-            m1, m2 = sym.symbol("m1 m2")  # type: ignore
-            c1, c2 = sym.symbol("c1 c2")  # type: ignore
+            a1, a2 = sym.symbol("a1 a2")  # link lengths # type: ignore
+            m1, m2 = sym.symbol("m1 m2")  # link masses # type: ignore
+            c1, c2 = sym.symbol("c1 c2")  # link CoMs location relative to link frames# type: ignore
+            if inertia:
+                r = sym.symbol("r") # link radius, assumed tubular, for inertia calculation # type: ignore
+                I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2)  # moments of inertia about CoM # type: ignore
+            else:
+                I1, I2 = None, None
             g = sym.symbol("g")
         else:
             from math import pi
 
-            zero = 0.0
-            a1 = 1
-            a2 = 1
-            m1 = 1
-            m2 = 1
-            c1 = -0.5
-            c2 = -0.5
-            g = 9.8
 
-        links = [
-            RevoluteDH(a=a1, alpha=zero, m=m1, r=[c1, 0, 0]),
-            RevoluteDH(a=a2, alpha=zero, m=m2, r=[c2, 0, 0]),
-        ]
+        if mdh:
+            # create a modified DH model
+            if not symbolic:
+                zero = 0.0
+                a1 = 1 # length of first link
+                a2 = 1 # length of second link
+                m1 = 1 # mass of first link
+                m2 = 1 # mass of second link
+                c1 = 0.5 # CoM location of first link relative to first link frame
+                c2 = 0.5 # CoM location of second link relative to second link frame
+                r = 0.1 # radius of links, assumed tubular, for inertia calculation
+                if inertia:
+                    I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2)  # moments of inertia about CoM # type: ignore
+                else:
+                    I1, I2 = None, None
+                g = 9.8
+
+            links = [
+                RevoluteMDH(a=0, alpha=zero, m=m1, r=[c1, zero, zero], I=I1),
+                RevoluteMDH(a=a1, alpha=zero, m=m2, r=[c2, zero, zero], I=I2),
+            ]
+            tool = SE3.Tx(a2) # the last link is considered as a tool in this case, so the tool is a translation along x by a2
+        else:
+            # create a standard DH model
+            if not symbolic:
+                zero = 0.0
+                a1 = 1 # length of first link
+                a2 = 1 # length of second link
+                m1 = 1 # mass of first link
+                m2 = 1 # mass of second link
+                c1 = -0.5 # CoM location of first link relative to first link frame
+                c2 = -0.5 # CoM location of second link relative to second link frame
+                r = 0.1 # radius of links, assumed tubular, for inertia calculation
+                if inertia:
+                    I1, I2 = _cylinder_inertia_x(m1, r, a1), _cylinder_inertia_x(m2, r, a2)  # moments of inertia about CoM # type: ignore
+                else:
+                    I1, I2 = None, None
+                g = 9.8
+            links = [
+                RevoluteDH(a=a1, alpha=zero, m=m1, r=[c1, 0, 0], I=I1),
+                RevoluteDH(a=a2, alpha=zero, m=m2, r=[c2, 0, 0], I=I2),
+            ]
+            tool = None
 
         super().__init__(
-            links, symbolic=symbolic, name="2 link", keywords=("planar", "dynamics")
+            links, symbolic=symbolic, name="2 link", tool=tool, keywords=("planar", "dynamics")
         )
 
         self.qr = np.array([pi / 6, -pi / 6])
@@ -104,6 +142,12 @@ def __init__(self, symbolic=False):
         self.gravity = [0, 0, g]
 
 
+def _cylinder_inertia_x(m, r, L):
+    ixx = 0.5 * m * r**2
+    iyy = (1.0 / 12.0) * m * (3 * r**2 + L**2)
+    izz = iyy
+    return np.diag([ixx, iyy, izz])
+
 if __name__ == "__main__":  # pragma nocover
     robot = TwoLink(symbolic=True)
     print(robot)

src/roboticstoolbox/robot/DHRobot.py

@@ -1645,10 +1645,14 @@ def removesmall(x):
                 alpha = link.alpha
                 if self.mdh:
                     pstar = np.r_[link.a, -d * sym.sin(alpha), d * sym.cos(alpha)]
-                    if j == 0:
-                        if base:
-                            Tj = base @ Tj
-                            pstar = base @ pstar
+                    # NOT baking base into Tj/pstar here (this block used to,
+                    # before being removed): base rotation is already applied
+                    # exactly once, to gravity/vd before this loop starts.
+                    # Also folding it into Rm[0] here double-counted it --
+                    # confirmed by tracing TwoLink(mdh=True)'s gravity-only
+                    # case, where vd ended up rotated by the base twice.
+                    # ne.c matches this: it only ever rotates gravity by the
+                    # base (in the nanobind glue), never any per-link R.
                 else:
                     pstar = np.r_[link.a, d * sym.sin(alpha), d * sym.cos(alpha)]
 
@@ -1670,7 +1674,14 @@ def removesmall(x):
                         # revolute axis
                         w_ = Rt @ w + z0 * qd_k[j]
                         wd_ = Rt @ wd + z0 * qdd_k[j] + _cross(Rt @ w, z0 * qd_k[j])
-                        vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd
+                        # Rt must distribute over the whole bracket, not just
+                        # the first term -- matches ne.c's MODIFIED-DH branch,
+                        # which does rot_trans_vect_mult() (= Rt @ ...) on the
+                        # full OMEGADOT(j-1)xPSTAR + OMEGA(j-1)x(OMEGA(j-1)xPSTAR)
+                        # + ACC(j-1) sum. The prismatic case below already has
+                        # this right; this revolute case was missing the
+                        # parentheses (and therefore wrong for any MDH robot).
+                        vd_ = Rt @ (_cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd)
                     else:
                         # prismatic axis
                         w_ = Rt @ w
@@ -1741,7 +1752,12 @@ def removesmall(x):
                     nn_ = (
                         R @ nn
                         + _cross(pstar, R @ f)
-                        + _cross(pstar, Fm[:, j])
+                        # this link's own force acts through its own CoM
+                        # offset r, not pstar (which is the offset to the
+                        # *next* link's origin) -- matches ne.c's MODIFIED
+                        # branch: vect_cross(&t2, R_COG(j), &F) uses R_COG(j)
+                        # (this link's r), not PSTAR(j+1)
+                        + _cross(r, Fm[:, j])
                         + Nm[:, j]
                     )
                     f = f_

tests/test_fknm_fallback.py

@@ -742,6 +742,167 @@ def test_twolink_with_ee_wrench(self):
         )
 
 
+# ---------------------------------------------------------------------------
+# TwoLink(mdh=True) vs TwoLink(mdh=False): two different DH parameterizations
+# of the *same physical robot* -- this is what actually caught the bug where
+# TwoLink's mdh=True variant just copied the standard-DH `a`/`alpha` values
+# onto RevoluteMDH links, which is not how the DH<->MDH conversion works (it
+# shifts `a`/`alpha` to the previous link index). A single hand-checked pose
+# (qn = [pi/6, -pi/6]) happened to agree by coincidence -- qn is symmetric
+# (q2 = -q1) -- while every other pose diverged by up to ~0.7 in fkine.
+# Random poses, not just qn, are the point of this test.
+#
+# This pairing also gives a genuinely independent cross-check of the DH/MDH
+# convention finding (issue 6, rne.md). Originally: rne_python() trusted
+# only for standard DH, Robot.rne() only for modified DH. Exercising
+# rne_python() on this MDH+rotated-base pair (apparently never hit before)
+# found and fixed three real bugs in its MDH branch (see
+# test_mdh_rne_python_now_agrees) -- rne_python() is now correct for both
+# conventions. Robot.rne() (the separate ETS/Featherstone implementation) is
+# joint-last-compliant, and thus correct, for mdh=True DHRobot instances
+# (test_robot_rne_on_mdh_variant_matches_standard_dh_rne_python); for
+# mdh=False it now asserts rather than silently returning a wrong answer
+# (test_robot_rne_rejects_standard_dh) -- see rne.md/tech-debt.md.
+# ---------------------------------------------------------------------------
+
+class TestTwoLinkDHMDHEquivalence(unittest.TestCase):
+    """TwoLink(mdh=False) and TwoLink(mdh=True) are the same physical robot."""
+
+    def setUp(self):
+        self.std = _twolink()
+        self.mdh = TwoLink(mdh=True)
+        self.poses = [
+            np.array([0.3, 0.5]),
+            np.array([1.2, -0.7]),
+            np.array([0.2, 0.3]),
+        ]
+
+    def test_fkine_agrees(self):
+        for q in self.poses:
+            nt.assert_array_almost_equal(
+                self.std.fkine(q).A, self.mdh.fkine(q).A, decimal=10
+            )
+
+    def test_fkine_agrees_random_poses(self):
+        rng = np.random.default_rng(0)
+        for _ in range(50):
+            q = rng.uniform(-np.pi, np.pi, 2)
+            nt.assert_array_almost_equal(
+                self.std.fkine(q).A, self.mdh.fkine(q).A, decimal=10
+            )
+
+    def test_rne_agrees_between_parameterizations(self):
+        """rne() (the single ne.c implementation, dispatched per-robot via
+        its own dhtype: STANDARD vs MODIFIED) gives matching torques for
+        both parameterizations of the same physical robot -- evidence that
+        ne.c's two dhtype branches are mutually consistent, not that
+        there are two separate C implementations to compare."""
+        z = np.zeros(2)
+        for q in self.poses:
+            tau_std_py = self.std.rne_python(q, z, z)
+            tau_std = self.std.rne(q, z, z)
+            tau_mdh = self.mdh.rne(q, z, z)
+            nt.assert_array_almost_equal(tau_std_py, tau_std, decimal=4)
+            nt.assert_array_almost_equal(tau_std, tau_mdh, decimal=4)
+
+    def test_robot_rne_on_mdh_variant_matches_standard_dh_rne_python(self):
+        """The core rne.md finding, checked against a real matched pair
+        rather than the synthetic 1-link case: Robot.rne() (trusted for
+        MDH) applied to the MDH parameterization must agree with
+        rne_python() (trusted for standard DH) applied to the standard-DH
+        parameterization of the same physical robot."""
+        from roboticstoolbox.robot.Robot import Robot as RobotBase
+
+        z = np.zeros(2)
+        for q in self.poses:
+            tau_std_py = self.std.rne_python(q, z, z)
+            tau_mdh_base = RobotBase.rne(self.mdh, q, z, z)
+            nt.assert_array_almost_equal(tau_std_py, tau_mdh_base, decimal=4)
+
+    def test_mdh_rne_python_now_agrees(self):
+        """rne_python() on the MDH parameterization -- exercising this
+        (mdh=True + a non-identity base) found three real bugs, none of
+        them issue 6:
+
+        1. Base rotation applied to gravity twice: once (correctly)
+           before the recursion loop, and again via Tj = base @ Tj for
+           the first link -- double-counted, not just wrong.
+        2. Missing parentheses in the MDH revolute case's linear
+           acceleration formula: `Rt @ cross(wd, pstar) + cross(w,
+           cross(w, pstar)) + vd` should distribute Rt over the whole
+           sum, per ne.c's MODIFIED-DH branch (rot_trans_vect_mult
+           applied to the full bracket) -- the prismatic case right
+           below it already had this right.
+        3. The backward recursion's moment equation used `pstar` (the
+           *next* link's offset) where it should use `r` (this link's
+           own CoM offset) for the this-link-force-to-torque term --
+           ne.c's equivalent is `R_COG(j) x F`, not `PSTAR(j+1) x F`.
+
+        With all three fixed, rne_python() is now correct for MDH too,
+        not just standard DH -- see test_robot_rne_rejects_standard_dh
+        for the one implementation (Robot.rne(), the ETS/Featherstone
+        one, unrelated code) that still can't handle standard DH (by
+        design, guarded, not silently wrong).
+        """
+        z = np.zeros(2)
+        for q in self.poses:
+            truth = self.std.rne_python(q, z, z)
+            mdh_py = self.mdh.rne_python(q, z, z)
+            nt.assert_array_almost_equal(mdh_py, truth, decimal=4)
+
+
+# ---------------------------------------------------------------------------
+# Actuator dynamics (Jm, G, B, Tc) and non-zero link inertia: C vs
+# rne_python() consistency on TwoLink, for both DH conventions. TwoLink is
+# zero for all of these by default (per its own docstring: "Motor inertia
+# is 0 ... Viscous and Coulomb friction is 0", link inertias also 0 unless
+# inertia=True) -- so nothing before this exercised the actuator-dynamics
+# terms (ne.c:485-491 / Link.friction()) or a non-zero inertia tensor
+# through rne() at all.
+# ---------------------------------------------------------------------------
+
+@unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C)
+class TestTwoLinkActuatorDynamics(unittest.TestCase):
+    """rne() (C) vs rne_python() with non-zero Jm/G/B/Tc and link inertia."""
+
+    def _with_actuator_dynamics(self, robot):
+        for i, link in enumerate(robot.links):
+            link.Jm = 0.05 * (i + 1)
+            link.G = 100.0 * (i + 1)
+            link.B = 0.01 * (i + 1)
+            link.Tc = [0.3 * (i + 1), -0.2 * (i + 1)]
+        return robot
+
+    def setUp(self):
+        self.std = self._with_actuator_dynamics(TwoLink(mdh=False, inertia=True))
+        self.mdh = self._with_actuator_dynamics(TwoLink(mdh=True, inertia=True))
+        # non-zero qd/qdd: needed to actually exercise B/Tc (velocity-
+        # dependent) and Jm (acceleration-dependent) -- the gravity-only
+        # (qd=qdd=0) tests elsewhere in this file wouldn't touch them
+        self.q = np.array([0.3, 0.5])
+        self.qd = np.array([0.4, -0.6])
+        self.qdd = np.array([0.2, 0.7])
+
+    def test_std_dh_c_matches_python(self):
+        tau_c = self.std.rne(self.q, self.qd, self.qdd)
+        tau_py = self.std.rne_python(self.q, self.qd, self.qdd)
+        nt.assert_array_almost_equal(tau_c, tau_py, decimal=4)
+
+    def test_mdh_c_matches_python(self):
+        tau_c = self.mdh.rne(self.q, self.qd, self.qdd)
+        tau_py = self.mdh.rne_python(self.q, self.qd, self.qdd)
+        nt.assert_array_almost_equal(tau_c, tau_py, decimal=4)
+
+    def test_inertia_and_actuator_dynamics_actually_matter(self):
+        """Sanity check the comparisons above aren't vacuous: non-zero
+        inertia/Jm/G/B/Tc must actually change the result relative to the
+        all-zero-by-default TwoLink()."""
+        bare = TwoLink(mdh=False)
+        tau_bare = bare.rne(self.q, self.qd, self.qdd)
+        tau_with = self.std.rne(self.q, self.qd, self.qdd)
+        self.assertGreater(np.abs(tau_with - tau_bare).max(), 0.1)
+
+
 # ---------------------------------------------------------------------------
 # Path verification via timing
 # ---------------------------------------------------------------------------