feat(dynamics): add armature support, fix rne() calculation bugs, solid testing

.github/workflows/ci.yml

@@ -33,6 +33,21 @@ jobs:
       - name: Install package (core, no swift)
         run: pip install .[dev]
 
+      - name: Verify compiled extensions built
+        # Catches a silent build/import regression for _fknm_c/_frne_c --
+        # without this, tests/test_fknm_fallback.py's C-vs-Python
+        # cross-validation tests would just skip (they're gated on
+        # _C_AVAILABLE precisely so a missing extension can't masquerade as
+        # a passing comparison), and nothing else would notice.
+        shell: bash
+        run: |
+          python -c '
+          from roboticstoolbox.ets.fknm import _C_AVAILABLE as fknm_c
+          from roboticstoolbox.robot.frne import _C_AVAILABLE as frne_c
+          assert fknm_c and frne_c, f"compiled extension missing: fknm={fknm_c} frne={frne_c}"
+          print("OK: both _fknm_c and _frne_c built")
+          '
+
       - name: Test (core)
         run: pytest tests/ --ignore=tests/test_blocks.py --timeout=50 --timeout_method=thread -q
 
@@ -70,6 +85,17 @@ jobs:
       - name: Install package
         run: pip install .[dev]
 
+      - name: Verify compiled extensions built
+        # See the same step in test-core for why this matters.
+        shell: bash
+        run: |
+          python -c '
+          from roboticstoolbox.ets.fknm import _C_AVAILABLE as fknm_c
+          from roboticstoolbox.robot.frne import _C_AVAILABLE as frne_c
+          assert fknm_c and frne_c, f"compiled extension missing: fknm={fknm_c} frne={frne_c}"
+          print("OK: both _fknm_c and _frne_c built")
+          '
+
       - name: Test
         run: pytest tests/ --ignore=tests/test_blocks.py --timeout=50 --timeout_method=thread -q
 

examples/rne_compare.py

@@ -0,0 +1,107 @@
+#!/usr/bin/env python
+"""Compare the three RNE implementations on Puma560: C extension, rne_python,
+and the generic Robot.rne (spatialmath Spatial* object based).
+
+Robot.rne is invoked as the *unbound* base-class method so that DHRobot's
+override doesn't shadow it -- Puma560 (a DHRobot) never calls Robot.rne
+through normal dispatch.
+
+Historical note: this originally treated Robot.rne() as just "the third
+implementation" and compared it directly against Puma560. Since then (see
+rne.md issue 6), Robot.rne() turned out to structurally require joint-last
+ETS segments -- true for MDH DHRobots, false for Puma560 (standard DH) --
+and now asserts on the incompatible case instead of silently returning a
+wrong answer. So Robot.rne(puma, ...) below is *expected* to raise, and
+this script demonstrates that rather than crashing on it. For a 3-way
+comparison on a robot where all three genuinely apply, see rne_speed.py
+(uses rtb.models.DH.Panda(), which is mdh=True).
+"""
+
+import timeit
+
+import numpy as np
+import numpy.testing as nt
+
+import roboticstoolbox as rtb
+from roboticstoolbox.robot.Robot import Robot as RobotBase
+
+puma = rtb.models.DH.Puma560()
+qn = puma.qn
+qd1 = np.full(6, 0.1)
+qdd1 = np.full(6, 0.1)
+
+print("=" * 70)
+print("Single pose (q=qn, qd=qdd=[0.1]*6)")
+print("=" * 70)
+
+tau_c = puma.rne(qn, qd1, qdd1)
+tau_py = puma.rne_python(qn, qd1, qdd1)
+
+print("C extension (rne):        ", tau_c)
+print("pure Python (rne_python): ", tau_py)
+print("max|C - rne_python|   =", np.max(np.abs(tau_c - tau_py)))
+
+print()
+print("Robot.rne on Puma560 (standard DH) -- expected to be rejected:")
+try:
+    RobotBase.rne(puma, qn, qd1, qdd1)
+except AssertionError as e:
+    print(f"  correctly rejected: {e}")
+else:
+    print("  ERROR: expected AssertionError, got a result instead")
+
+print()
+print("=" * 70)
+print("Timing: trajectory of N=1000 identical rows (q=qn, qd=qdd=[0.1]*6)")
+print("=" * 70)
+
+N = 1000
+Q = np.tile(qn, (N, 1))
+QD = np.tile(qd1, (N, 1))
+QDD = np.tile(qdd1, (N, 1))
+
+
+def bench(label, fn, repeat=5):
+    t = min(timeit.repeat(fn, number=1, repeat=repeat))
+    print(f"{label:28s} {t*1e3:9.3f} ms   ({t/N*1e6:7.2f} us/row)")
+    return t
+
+
+t_c = bench("C extension (rne)", lambda: puma.rne(Q, QD, QDD))
+t_py = bench("pure Python (rne_python)", lambda: puma.rne_python(Q, QD, QDD))
+
+print()
+print(f"speedup rne_python -> C:  {t_py/t_c:6.1f}x")
+print("(Robot.rne skipped here -- Puma560 is standard DH, see note above."
+      " For a full 3-way timing comparison, see rne_speed.py.)")
+
+print()
+print("Per-row C-call count check (does rne() batch the trajectory into one")
+print("C call, or call frne() once per row from Python?)")
+print("Historical note: this used to report N -- frne() was called once per")
+print("trajectory row from a Python loop in DHRobot.rne(). Fixed (rne.md")
+print("plan step 7): the whole trajectory is now looped over inside a")
+print("single C++ call. Should report 1 below.")
+import sys
+
+# roboticstoolbox/robot/__init__.py does `from .DHRobot import DHRobot`, which
+# rebinds the `DHRobot` attribute of the `roboticstoolbox.robot` package to the
+# *class*, shadowing the submodule of the same name -- so
+# `import roboticstoolbox.robot.DHRobot as X` silently gives the class, not
+# the module, and patching X.frne is a no-op. sys.modules[...] is the only
+# reliable way to get the real module object (see test_fknm_fallback.py).
+DHRobot_module = sys.modules["roboticstoolbox.robot.DHRobot"]
+
+orig_frne = DHRobot_module.frne
+calls = {"n": 0}
+
+
+def counting_frne(*args, **kwargs):
+    calls["n"] += 1
+    return orig_frne(*args, **kwargs)
+
+
+DHRobot_module.frne = counting_frne
+puma.rne(Q, QD, QDD)
+print(f"frne() C calls for a {N}-row trajectory: {calls['n']}")
+DHRobot_module.frne = orig_frne

examples/rne_dh_convention_check.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+"""Root-cause diagnostic for the Robot.rne() vs DHRobot.rne_python() divergence
+originally seen in rne_compare.py -- see rne.md (issue 6) for the full writeup.
+
+Uses a single-link revolute robot with a pure axis twist (alpha != 0, a=d=0)
+to isolate the frame-convention question from friction/armature/multi-link
+recursion complexity. Ground truth is obtained *independently* of either RNE
+implementation via the Lagrangian identity: for a static pose (qd=qdd=0), the
+required joint torque equals dV/dq, where V(q) = m g z_com(q) is the
+gravitational potential energy of the link's centre of mass, computed
+directly from link.A(q) and numerically differentiated.
+
+Historical note: this originally showed rne_python() wrong for modified DH
+and Robot.rne() wrong for standard DH -- three real bugs in rne_python()'s
+MDH branch (see rne.md) have since been fixed, so rne_python() now agrees
+with ground truth for both conventions. Robot.rne()'s standard-DH case is
+not a bug to fix: its Featherstone recursion structurally requires the
+joint to be the last element of its own ETS segment, which standard DH
+never satisfies (the joint comes first). Robot.rne() now asserts on that
+case (see Robot.py) instead of silently returning a wrong answer.
+"""
+
+import numpy as np
+
+from roboticstoolbox import DHRobot, RevoluteDH, RevoluteMDH
+from roboticstoolbox.robot.Robot import Robot as RobotBase
+
+
+def gravity_torque_truth(robot, q0, g=9.81, h=1e-6):
+    """Numerically-differentiated ground truth, independent of any RNE code."""
+    link = robot.links[0]
+    p_local = np.array([*link.r, 1.0])
+
+    def com_z(q):
+        A = np.asarray(link.A(q))
+        return (A @ p_local)[2]
+
+    return g * (com_z(q0 + h) - com_z(q0 - h)) / (2 * h)
+
+
+def check(label, robot, q0=0.3, robot_rne_expected_to_work=True):
+    z = np.zeros(1)
+    q = np.array([q0])
+    truth = gravity_torque_truth(robot, q0)
+    tau_py = robot.rne_python(q, z, z)[0]
+    print(f"{label:28s} truth={truth:9.4f}  rne_python={tau_py:9.4f}")
+    print(f"{'':28s} |rne_python - truth| = {abs(tau_py - truth):.4f}")
+
+    if robot_rne_expected_to_work:
+        tau_base = RobotBase.rne(robot, q, z, z)[0]
+        print(f"{'':28s} Robot.rne={tau_base:9.4f}")
+        print(f"{'':28s} |Robot.rne - truth| = {abs(tau_base - truth):.4f}")
+    else:
+        try:
+            RobotBase.rne(robot, q, z, z)
+        except AssertionError:
+            print(f"{'':28s} Robot.rne: correctly rejected (AssertionError)")
+        else:
+            print(f"{'':28s} Robot.rne: ERROR -- expected AssertionError, got a result")
+
+
+print("Single-link revolute robot, alpha=1.2 rad, a=d=0, r=[0.5,0,0], static (qd=qdd=0)")
+print("Ground truth = d/dq[ m g z_com(q) ], independent of both RNE implementations.")
+print()
+
+std = DHRobot([RevoluteDH(a=0, alpha=1.2, d=0, m=1.0, r=[0.5, 0, 0])], gravity=[0, 0, -9.81])
+check("Standard DH (mdh=False)", std, robot_rne_expected_to_work=False)
+
+print()
+mdh = DHRobot([RevoluteMDH(a=0, alpha=1.2, d=0, m=1.0, r=[0.5, 0, 0])], gravity=[0, 0, -9.81])
+check("Modified DH (mdh=True)", mdh, robot_rne_expected_to_work=True)
+
+print()
+print("Conclusion: rne_python is correct for both DH conventions.")
+print("Robot.rne is correct for modified DH, and now cleanly rejects (rather")
+print("than silently mis-computing) standard DH, since its Featherstone")
+print("recursion cannot represent standard DH's joint-first structure.")

examples/rne_speed.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python
+"""Benchmark the three RNE implementations -- C extension (rne), pure Python
+(rne_python), and the generic ETS/Featherstone implementation (Robot.rne) --
+for both a single pose and a 1000-row trajectory.
+
+Uses rtb.models.DH.Panda() (7-DOF, mdh=True, real dynamics) rather than
+Puma560: Robot.rne()'s Featherstone recursion structurally requires
+joint-last ETS segments, true for MDH DHRobots, false for standard DH (see
+rne.md issue 6) -- it simply doesn't apply to Puma560 at all (asserts
+instead of silently miscomputing). Panda's DH variant is mdh=True, so all
+three implementations genuinely apply here and can be compared fairly, on
+the same robot, apples-to-apples.
+
+The trajectory is N *distinct*, randomly-generated poses, not a tiled/
+repeated single pose -- a uniform trajectory can't distinguish "this
+implementation correctly processes N different rows" from "it silently
+computes the same row N times" (relevant here since the C path now loops
+the whole trajectory inside a single C++ call rather than once per row --
+see rne.md plan step 7 and tests/test_fknm_fallback.py's
+TestRneTrajectoryVaryingRows for the correctness side of this).
+
+Riffs off ik_speed.py's ANSITable reporting and rne_compare.py's timing
+harness.
+"""
+
+import os
+import platform
+import subprocess
+import timeit
+
+import numpy as np
+from ansitable import ANSITable, Column
+
+import roboticstoolbox as rtb
+from roboticstoolbox.robot.Robot import Robot as RobotBase
+
+
+def cpu_info() -> str:
+    """Best-effort, portable one-line CPU description (name + core count,
+    clock speed when the OS actually exposes one). No new hard dependency:
+    psutil is used for clock speed only if already installed, and skipped
+    otherwise -- some platforms (e.g. Apple Silicon) don't expose a single
+    meaningful clock speed at all, so a missing/nonsensical reading is
+    silently omitted rather than shown.
+    """
+    system = platform.system()
+    name = None
+
+    if system == "Darwin":
+        try:
+            name = subprocess.check_output(
+                ["sysctl", "-n", "machdep.cpu.brand_string"], text=True
+            ).strip()
+        except Exception:
+            pass
+    elif system == "Linux":
+        try:
+            with open("/proc/cpuinfo") as f:
+                for line in f:
+                    if line.lower().startswith("model name"):
+                        name = line.split(":", 1)[1].strip()
+                        break
+        except Exception:
+            pass
+    elif system == "Windows":
+        name = platform.processor() or None
+
+    if not name:
+        name = platform.processor() or platform.machine() or "unknown CPU"
+
+    cores = os.cpu_count() or "?"
+    info = f"{name} ({cores} cores)"
+
+    try:
+        import psutil
+
+        freq = psutil.cpu_freq()
+        # Sanity floor: real clock speeds are in the hundreds-to-thousands
+        # of MHz; some platforms (Apple Silicon via psutil) report bogus
+        # single-digit values instead of raising.
+        if freq and freq.max and freq.max > 100:
+            info += f", {freq.max:.0f} MHz"
+    except Exception:
+        pass
+
+    return info
+
+
+robot = rtb.models.DH.Panda()
+n = robot.n
+rng = np.random.default_rng(0)
+
+print(f"CPU: {cpu_info()}")
+print(f"Robot: {robot.name}  (n={n}, mdh={bool(robot.mdh)})")
+print()
+
+# ---------------------------------------------------------------------------
+# Correctness check first -- a timing comparison between implementations
+# that don't actually agree with each other would be misleading.
+#
+# NB: this is only a *pairwise* (relative) check -- necessary but not
+# sufficient, since all three could share a bug and still agree with each
+# other. It's a sanity guard for this benchmark (catches a stale build or
+# wrong branch before trusting the timing numbers below), not a proof of
+# correctness. That proof lives in tests/test_fknm_fallback.py's
+# TestTwoLinkAbsoluteGroundTruth (Spong et al.'s closed-form solution) and
+# examples/rne_dh_convention_check.py's Lagrangian-identity check -- both
+# independent of all three RNE implementations here.
+# ---------------------------------------------------------------------------
+
+q1 = rng.uniform(-np.pi, np.pi, n)
+qd1 = rng.uniform(-2.0, 2.0, n)
+qdd1 = rng.uniform(-2.0, 2.0, n)
+
+tau_c = robot.rne(q1, qd1, qdd1)
+tau_py = robot.rne_python(q1, qd1, qdd1)
+tau_base = RobotBase.rne(robot, q1, qd1, qdd1)
+
+print("Correctness (single random pose):")
+print(f"  max|C - rne_python|         = {np.max(np.abs(tau_c - tau_py)):.3e}")
+print(f"  max|C - Robot.rne|          = {np.max(np.abs(tau_c - tau_base)):.3e}")
+print(f"  max|rne_python - Robot.rne| = {np.max(np.abs(tau_py - tau_base)):.3e}")
+print()
+
+# ---------------------------------------------------------------------------
+# Timing: single pose (1-off) and a 1000-row trajectory
+# ---------------------------------------------------------------------------
+
+N = 1000
+Q = rng.uniform(-np.pi, np.pi, (N, n))
+QD = rng.uniform(-2.0, 2.0, (N, n))
+QDD = rng.uniform(-2.0, 2.0, (N, n))
+
+
+def bench(fn, repeat=7):
+    return min(timeit.repeat(fn, number=1, repeat=repeat))
+
+
+implementations = [
+    ("C extension (rne)", lambda q, qd, qdd: robot.rne(q, qd, qdd)),
+    ("pure Python (rne_python)", lambda q, qd, qdd: robot.rne_python(q, qd, qdd)),
+    ("generic Robot.rne", lambda q, qd, qdd: RobotBase.rne(robot, q, qd, qdd)),
+]
+
+results = []
+for name, fn in implementations:
+    t_single = bench(lambda fn=fn: fn(q1, qd1, qdd1))
+    t_traj = bench(lambda fn=fn: fn(Q, QD, QDD))
+    results.append((name, t_single, t_traj))
+
+t_c_traj = results[0][2]
+
+table = ANSITable(
+    Column("Method", colalign="<", headalign="^"),
+    Column("1 pose (us)", fmt="{:.2f}", headalign="^"),
+    Column(f"{N}-row traj (ms)", fmt="{:.3f}", headalign="^"),
+    Column("us/row", fmt="{:.2f}", headalign="^"),
+    Column("speedup vs C (traj)", fmt="{:.1f}x", headalign="^"),
+    border="thin",
+)
+
+for name, t_single, t_traj in results:
+    table.row(
+        name,
+        t_single * 1e6,
+        t_traj * 1e3,
+        t_traj / N * 1e6,
+        t_traj / t_c_traj,
+    )
+
+print(f"Timing over a {N}-row trajectory (distinct random poses, not tiled):")
+table.print()