diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0c662e6a3..805b2e179 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/examples/rne_compare.py b/examples/rne_compare.py new file mode 100644 index 000000000..d6b865d73 --- /dev/null +++ b/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 diff --git a/examples/rne_dh_convention_check.py b/examples/rne_dh_convention_check.py new file mode 100644 index 000000000..e214a7de0 --- /dev/null +++ b/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.") diff --git a/examples/rne_speed.py b/examples/rne_speed.py new file mode 100644 index 000000000..6d85c31b8 --- /dev/null +++ b/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() diff --git a/rne.md b/rne.md new file mode 100644 index 000000000..45de34cd8 --- /dev/null +++ b/rne.md @@ -0,0 +1,322 @@ + + +# RNE (inverse dynamics) — issues and plan + +Working notes from an investigation of the recursive Newton-Euler code paths, +prompted by `docs/notebooks/two-link-dynamics.ipynb` having to call +`rne_python()` explicitly instead of `rne()`. Captures what we found and the +plan for fixing it, before we start changing code. + +See `examples/rne_compare.py` for the comparison/benchmark script referenced +throughout (Puma560, pose `qn`, `qd=qdd=[0.1]*6`, and a 1000-row trajectory +for timing). + +## Fixed 2026-07-21: base-rotation handling (separate from issue 6 below) + +Found while investigating `Robot.rne()` against `TwoLink` (base +`SE3.Rx(pi/2)`, standard DH): `Robot.rne()`'s Featherstone recursion never +referenced `self.base` at all -- gravity was applied along the chain's own +frame-0 z-axis regardless of base orientation, giving exactly zero gravity +torque for a robot (like `TwoLink`) whose own DH parameters are planar and +relies entirely on `self.base` to tilt into the plane gravity acts in. + +Investigating further surfaced three more, related bugs, none of them the +DH/MDH convention issue (item 6) -- confirmed independent, since fixing +these did not change the standard-DH-vs-modified-DH divergence at all: + +- `rne_python()`'s rotated-base branch was missing a negation: + `vd = Rb @ gravity` where every other call site (including the same + function's own identity-base branch) computes `-gravity`. Exact sign + flip, confirmed against the Lagrangian-identity ground truth on TwoLink. +- `ne.c`/`frne_nb.cpp` never handled base rotation either -- worked around + for 30 years by `DHRobot.rne()` pre-rotating (and negating) the gravity + vector in Python before calling C. Moved into the C glue itself + (`frne_nb.cpp`'s `frne()` now takes `self.base.R` directly, using the + existing `rot_trans_vect_mult` C helper) -- `DHRobot.rne()` no longer + needs to know about this at all. +- `rne_python()`'s `base_wrench=True` path allocated `wbase` with shape + `(nk, n)` (joint count) instead of `(nk, 6)` (a wrench is always + 6-dimensional) -- worked by coincidence for 6-DOF Puma560, crashed for + TwoLink (2 DOF). + +Also added `base_wrench` support to the C path (`ne.c`'s backward +recursion already computes `f(0)`/`n(0)`, just never returned it), and +renamed `f_tip`/`n_tip` → `f_ee`/`n_ee` with docstrings/comments +standardized on "wrench applied to end-effector" throughout (`fext` +parameter name unchanged). + +New regression tests in `tests/test_fknm_fallback.py`: +`TestRNERotatedBaseFallback`/`Reference` (TwoLink vs C, vs ground truth), +`TestBaseWrenchFallback`/`Reference` (C vs `rne_python()` base wrench, +with and without an end-effector wrench). + +**Issue 6 below is now also resolved** — see its updated writeup. + +## The three implementations + +| | class | backing | symbolic-aware? | +|---|---|---|---| +| `rne()` | `DHRobot` | tries C extension (`frne.py` facade → `_frne_c`), falls back to `rne_python()` | no dispatch-time check (see Issue 1) | +| `rne_python()` | `DHRobot` | pure Python, direct array algebra, explicit `mdh`/non-`mdh` branches | yes, via `self.symbolic` (dtype="O", drops Coulomb friction) | +| `rne()` | `Robot` (base class of `DHRobot`) | pure Python, builds `SpatialVelocity`/`SpatialAcceleration`/`SpatialForce`/`SpatialInertia` objects from spatialmath | yes, via its own `symbolic` kwarg, no relation to `DHRobot`'s | + +`DHRobot.rne()` overrides `Robot.rne()`, so in normal use nothing ever calls +`Robot.rne()` on a `DHRobot` — it's only reachable by an explicit unbound call +(`Robot.rne(puma, ...)`), which is what `rne_compare.py` does. + +## Issues found + +### 1. `DHRobot.rne()` has no symbolic pre-check, so it crashes instead of falling back + +Current dispatch logic (`DHRobot.py`, `rne()`): +```python +if self._frne is None or self._frne_stale: + self._copy_to_cpp() +if self._frne is None or base_wrench: + return self.rne_python(...) +``` +This only asks "is the C extension importable" or "was `base_wrench` +requested." It never checks whether `q`/`qd`/`qdd` or the model's own link +parameters are symbolic. + +Confirmed failure: building the two-link notebook's model +(`DHRobot(..., symbolic=True)`) and calling `.rne(q, qd, qdd)` with symbolic +`q`/`qd`/`qdd` throws, uncaught: +``` +TypeError: Cannot convert expression to float +``` +This happens inside `_copy_to_cpp()`, which preallocates a **float64** array +(`L = np.zeros(24 * self.n)`) and assigns each link parameter into it — +`L[j+1] = self.links[i].a` throws when `a` is a sympy `Symbol`. Even past +that, `np.ascontiguousarray(q, dtype=float)` a few lines later would fail the +same way on symbolic `q`. + +This is why the notebook has to call `.rne_python()` directly — there is +currently no way to just call `.rne()` and get correct automatic fallback. + +### 2. Two disconnected symbolic-detection mechanisms already exist, neither used by `rne()` + +- `BaseRobot.symbolic` / `self._symbolic`: a **construction-time flag**, set + manually (e.g. `Puma560(symbolic=True)`, which builds the DH table itself + out of `sympy` constants instead of floats). Read only inside + `rne_python()`, to pick `dtype="O"` and to disable Coulomb friction + (`sign()` doesn't work symbolically). +- `fknm.py`'s `_is_symbolic(q)`: a **runtime dtype check** on `q` + (`np.asarray(q).dtype == object`), used throughout the ETS + forward-kinematics/Jacobian facade to decide C vs. Python *before* ever + calling into C, e.g. `ETS_fkine`: `if fknm is not None and not + _is_symbolic(q): return _c_ETS_fkine(...)`. + +`frne.py`/`DHRobot.rne()` has neither. The right pattern already exists next +door in `fknm.py` — `rne()` just doesn't use it. + +### 3. Forgetting the `symbolic=True` flag breaks even the "safe" Python path + +Confirmed: building a `DHRobot` with symbolic link parameters but *without* +`symbolic=True` leaves `robot.symbolic == False`, so `rne_python()` itself +allocates float64 arrays and crashes with the same +`Cannot convert expression to float` — even though `rne_python()` is supposed +to be the always-works fallback. + +**Conclusion (per discussion): detect symbolic-ness of the model at build +time**, by inspecting the link parameters (`a`, `alpha`, `theta`, `d`, `m`, +`r`, `I`, ...) for non-numeric content, rather than trusting a user-supplied +constructor flag. `BaseRobot.__init__` already loops over every link once +(for geometry flags) — the same pass could set this. Keep `symbolic=` as an +optional override, not the source of truth. + +### 4. Checking symbolic-ness of `q`, `qd`, `qdd` (three vectors, not one) + +Not a mechanical problem — three `dtype == object` checks is negligible +cost. The real decision is policy when they disagree (e.g. `q` numeric but +`qd` symbolic). Recommendation: fall back to the Python path if **any** of +`q`/`qd`/`qdd` is non-numeric, matching the existing `fknm.py` idiom of +`not _is_symbolic(a) and not _is_symbolic(b)` (De Morgan: either symbolic ⇒ +fallback). This check must also fold in the model's own symbolic-ness (issue +3) — a numeric `q`/`qd`/`qdd` against symbolic link masses still needs the +object-dtype path. + +### 5. Trajectories are not batched into the C extension — confirmed + +Instrumented `frne()` directly: a 1000-row trajectory triggers **1000 +separate Python→C calls**, one per row +(`for i in range(trajn): tau[i, :] = frne(...)` in `DHRobot.rne()`). So today +there is no benefit to pre-stacking a trajectory into one array call vs. +calling `.rne()` 1000 times yourself — same Python-side loop either way. + +Measured cost of this: 1000 bare `frne()` calls take 1.28 ms; `puma.rne()` on +the same stacked trajectory takes 1.95 ms — **~35% of wall time is Python-side +looping/marshalling overhead**, not the C computation. Passing the whole +trajectory into a single C call (looping in C) would reclaim most of that. + +Priority: real but modest — already 223x faster than pure Python at n=6 DOF, +so this is a constant-factor win, not a complexity-class change. Matters more +for tight real-time control loops or larger N/DOF. + +### 6. Fixed 2026-07-21: `Robot.rne()` and `rne_python()` each got ONE DH sub-convention right, and the other wrong — root-caused and resolved + +Original finding (kept for the record): `Robot.rne()` doesn't model armature +inertia (`G`, `Jm`) or friction (`B`, `Tc`) at all — that explains *some* +divergence from `rne_python()` on Puma560. But zeroing those out on a copy of +Puma560 to isolate rigid-body-only dynamics, the two **still disagreed by +~70** (vs. a torque magnitude of ~30-45), including in a **pure gravity-load +case** (`qd = qdd = 0`), which has no velocity/Coriolis terms to get wrong at +all. So it wasn't just missing terms — confirming the hypothesis that **the +link frames differ, and the inertial parameters (`r`, `I`) are expressed +relative to those differing frames.** + +Root cause, isolated with `examples/rne_dh_convention_check.py` (a single +revolute link, `alpha=1.2, a=d=0, r=[0.5,0,0]`, static so there are no +velocity terms at all, `q0=0.3`) and checked against an independent ground +truth — the Lagrangian identity `tau = d/dq[m g z_com(q)]`, computed directly +from `link.A(q)` and numerically differentiated, which doesn't rely on either +RNE implementation's spatial-vector bookkeeping: + +``` + truth rne_python Robot.rne +Standard DH (mdh=False) 0.0000 0.0000 4.5717 <- Robot.rne WRONG +Modified DH (mdh=True) 4.3675 0.0000 4.3675 <- rne_python WRONG +``` + +**Neither implementation was "the trusted reference" — each was only correct +for one DH sub-convention, for two structurally different reasons:** + +- **`rne_python()`'s modified-DH branch had three real bugs**, found by + term-by-term comparison against `ne.c`'s `MODIFIED` branch while exercising + an `mdh=True` + rotated-base case that had apparently never been exercised + before (`TestTwoLinkDHMDHEquivalence` in `tests/test_fknm_fallback.py`): + 1. Base rotation applied to gravity twice (once correctly before the + recursion loop, once more via a redundant `Tj = base @ Tj` for the first + link) — double-counted. + 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, matching `ne.c`'s + `rot_trans_vect_mult` applied to the full bracket. + 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) — + `ne.c`'s equivalent is `R_COG(j) x F`, not `PSTAR(j+1) x F`. + + With all three fixed, `rne_python()` is correct for both DH conventions + (`test_mdh_rne_python_now_agrees`). + +- **`Robot.rne()`'s generic Featherstone recursion genuinely requires the + joint to be the last element of its own ETS segment** (fixed geometry gets + you *to* the joint; the joint is the last thing applied before the next + link's frame) — this is inherent to the algorithm, not a bug to fix. This + holds for `Robot`/`ERobot`/`URDFRobot` (`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, so the reorder doesn't change the net transform. It does **not** + hold for `DHRobot(mdh=False)` (standard DH), where the joint comes *before* + the link's fixed `tz(d) * tx(a) * Rx(alpha)` transforms — structurally + incompatible with the algorithm, not fixable without changing what + `Robot.rne()` fundamentally assumes. + + **Action taken:** rather than teach `Robot.rne()` a second, DH-aware + recursion, it now asserts on the incompatible case + (`assert getattr(self, "mdh", True)` in `Robot.rne()`) instead of silently + returning a wrong answer — see tech-debt.md for the blocklist-vs-allowlist + design discussion behind this specific check, and + `test_robot_rne_rejects_standard_dh` for the regression test. + +- **A separate, previously-masked bug**, found while numerically verifying + the `mdh=True` case above with a nonzero `d`/`alpha`/inertia tensor (rather + than just trusting the structural argument): `Robot.rne()`'s inertia + accumulation, `SpatialInertia(m=link.m, r=link.r)`, never passed + `I=link.I` — the rotational inertia tensor was silently dropped for + *every* link, on every call, regardless of DH convention. This affected + `Robot`/`ERobot`/`URDFRobot`/`PoERobot`/`DHRobot(mdh=True)` alike, not just + the MDH edge case that surfaced it — never caught earlier because the + `TwoLink`-based equivalence tests used default (zero) inertia, and the + tests that *did* set `inertia=True` only compared the C path against + `rne_python()`, never `Robot.rne()`. Fixed; see + `TestRobotRneInertiaTensor` in `tests/test_fknm_fallback.py`. + +`Robot.rne()` also already passes `test_ERobot.py::test_invdyn`, which +validates it against a hand-built 2-link ETS robot (elementary `ET.Ry()` +transforms) checked against a known analytical result (Spong et al., 2nd +ed., p. 260) — consistent with it being correct for genuine +elementary-transform ETS chains. + +Since essentially every built-in DH model in this codebase (Puma560, etc.) +uses **standard** DH, this is exactly why the original Puma560 comparison +showed `Robot.rne()` diverging and `rne_python()`/C agreeing — consistent +with, not contradicting, this finding. + +**Net result:** `rne_python()`/`rne()` (C) are correct for both DH +conventions. `Robot.rne()` is correct for MDH `DHRobot`s and all +ETS-native robots (`Robot`/`ERobot`/`URDFRobot`/`PoERobot`), now including +the previously-dropped inertia tensor, and cleanly rejects the one +structurally-incompatible case (standard DH) instead of silently +miscomputing it. No unification was attempted — the two remain independent +implementations by design (see the architecture note at the top of +`Dynamics.py`), which is the right call given `Robot.rne()`'s algorithm +cannot represent standard DH's joint-first structure at all. + +### 7. Fixed 2026-07-21: Housekeeping + +- `rne_python()`'s docstring called itself `rne_dh` throughout — stale name + from before a rename. Fixed. +- A dead commented-out symbolic-simplification block sat at the end of + `rne_python()`. Removed. + +## Plan (systematic, building on `examples/rne_compare.py`) + +1. ~~**Root-cause the `Robot.rne()` / DH-link mismatch first**~~ **Done** — + see issue 6. Two independent findings: `Robot.rne()`'s Featherstone + recursion structurally requires joint-last ETS segments (true for MDH, + false for standard DH — not fixable, now guarded); and a separate, + previously-masked bug where it dropped the rotational inertia tensor + entirely (`SpatialInertia` never got `I=link.I`) — fixed. +2. ~~**Formalize `rne_compare.py` into pytest regression tests**~~ **Done** — + `tests/test_fknm_fallback.py`: `TestTwoLinkDHMDHEquivalence`, + `TestRobotRneInertiaTensor`, `TestRNERotatedBase*`, `TestBaseWrench*`, and + `TestTwoLinkAbsoluteGroundTruth` (an independent closed-form analytical + check — Spong et al. Eq 7.87 — not derived from or shared with any RNE + implementation here, added 2026-07-21 since everything else only checks + these implementations against each other). +3. ~~**Decide unification vs. documented separation**~~ **Done** — kept + separate (`Robot.rne()` and `DHRobot.rne_python()`/`rne()` remain + independent implementations); documented in the `Dynamics.py` module + docstring and here. The right call given `Robot.rne()`'s algorithm cannot + represent standard DH's joint-first structure at all — there is nothing + to unify it with for that case. +4. ~~**Auto-detect symbolic models at build time**~~ **Done** — `BaseRobot.py`: + `_is_symbolic()` helper + `_SYMBOLIC_LINK_ATTRS` scan in `__init__`'s + existing per-link loop, ORs into `self._symbolic` alongside the explicit + constructor flag. Verified against issue 3's exact repro (a `DHRobot` + built with a symbolic `a` but no `symbolic=True`) — now auto-detected. +5. ~~**Add a symbolic-aware dispatch check to `DHRobot.rne()`**~~ **Done** — + routes to `rne_python()` if `self.symbolic` or any of `q`/`qd`/`qdd` is + symbolic, mirroring `fknm._is_symbolic`'s idiom. **Also found and fixed a + bug this step exposed**: `rne_python()` itself only checked + `self.symbolic` (model-level) to pick its internal array `dtype`, not + whether *this call's* `q`/`qd`/`qdd` were symbolic — so a numeric model + called with symbolic `q` crashed inside `rne_python()`, defeating the + point of routing there as the always-works fallback. Fixed via a local + `symbolic_call` variable, used for both the `dtype` choice and the + Coulomb-friction guard. +6. ~~**Add a `try/except` safety net**~~ **Done** — wraps the C call, falls + back to `rne_python()` on `(TypeError, ValueError)`. +7. ~~**Batch trajectories into a single C call**~~ **Done** — + `frne_nb.cpp`'s `frne()` binding now takes `(trajn, n)` q/qd/qdd arrays + and loops over the whole trajectory inside C++ (gravity/base-rotation + setup done once, not per row); `DHRobot.rne()` makes a single call + instead of looping in Python. Confirmed via `examples/rne_compare.py`'s + per-row C-call counter: 1 call for a 1000-row trajectory (was 1000). + Measured speedup (`examples/rne_speed.py`, `rtb.models.DH.Panda()`, + 1000 distinct random poses): C ≈ 0.6 ms total (≈0.6 us/row) vs + `rne_python()` ≈ 289 ms (≈474x) and `Robot.rne()` ≈ 446 ms (≈731x). + Regression coverage: `tests/test_fknm_fallback.py`'s + `TestRneTrajectoryVaryingRows` — deliberately distinct (not tiled) rows + per trajectory, since a uniform trajectory can't distinguish "row i is + computed correctly" from "row i is silently some other row's result", + which matters specifically because this step introduced new row-indexing + arithmetic in the C++ loop. +8. ~~**Housekeeping**~~ **Done** — fixed the `rne_dh` docstring naming, + deleted the dead commented-out block. + +**All plan steps (1-8) are now done.** diff --git a/src/roboticstoolbox/blocks/arm.py b/src/roboticstoolbox/blocks/arm.py index 9963b7d34..a57061e30 100644 --- a/src/roboticstoolbox/blocks/arm.py +++ b/src/roboticstoolbox/blocks/arm.py @@ -1039,8 +1039,10 @@ def __init__(self, robot, gravity=None, **blockargs): """ :param robot: Robot model :type robot: Robot subclass - :param gravity: gravitational acceleration - :type gravity: float + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive) + :type gravity: ndarray(3) :param blockargs: |BlockOptions| :type blockargs: dict """ @@ -1051,7 +1053,7 @@ def __init__(self, robot, gravity=None, **blockargs): # self.type = "inverse-dynamics" self.robot = robot - self.gravity = gravity + self.gravity = None if gravity is None else smb.getvector(gravity, 3) # state vector is [q qd] @@ -1107,8 +1109,10 @@ def __init__(self, robot, gravity=None, **blockargs): """ :param robot: Robot model :type robot: Robot subclass - :param gravity: gravitational acceleration - :type gravity: float + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive) + :type gravity: ndarray(3) :param blockargs: |BlockOptions| :type blockargs: dict """ @@ -1119,7 +1123,7 @@ def __init__(self, robot, gravity=None, **blockargs): # self.type = "gravload" self.robot = robot - self.gravity = gravity + self.gravity = None if gravity is None else smb.getvector(gravity, 3) self.inport_names(("q",)) self.outport_names(("$\tau$",)) @@ -1171,8 +1175,10 @@ def __init__(self, robot, representation="rpy/xyz", gravity=None, **blockargs): :type robot: Robot subclass :param representation: task-space representation, defaults to "rpy/xyz" :type representation: str - :param gravity: gravitational acceleration - :type gravity: float + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive) + :type gravity: ndarray(3) :param blockargs: |BlockOptions| :type blockargs: dict """ @@ -1183,7 +1189,7 @@ def __init__(self, robot, representation="rpy/xyz", gravity=None, **blockargs): # self.type = "gravload-x" self.robot = robot - self.gravity = gravity + self.gravity = None if gravity is None else smb.getvector(gravity, 3) self.inport_names(("q",)) self.outport_names(("$\tau$",)) self.representation = representation diff --git a/src/roboticstoolbox/models/DH/TwoLink.py b/src/roboticstoolbox/models/DH/TwoLink.py index 6daf6f48e..0bbf24d73 100644 --- a/src/roboticstoolbox/models/DH/TwoLink.py +++ b/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,7 +51,7 @@ 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). @@ -57,36 +59,72 @@ class TwoLink(DHRobot): .. 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) diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 1d9673bb6..ae7d5024c 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -56,6 +56,23 @@ # A generic type variable representing any subclass of BaseLink LinkType = TypeVar("LinkType", bound=BaseLink) +# Link attributes scanned for symbolic (e.g. SymPy) content at build time -- +# DH kinematic parameters plus dynamics parameters; getattr(..., None) below +# skips whichever of these don't exist on a given Link subclass (e.g. a +# non-DH Link has no a/alpha/theta/d). +_SYMBOLIC_LINK_ATTRS = ( + "a", "alpha", "theta", "d", "offset", + "m", "r", "I", "Jm", "G", "B", "Tc", +) + + +def _is_symbolic(value: Any) -> bool: + """True if value holds non-numeric (e.g. SymPy) content -- same + object-dtype test used by roboticstoolbox.ets.fknm._is_symbolic.""" + if value is None: + return False + return np.asarray(value).dtype == object + class BaseRobot(SceneNode, DynamicsMixin, RobotPlottingMPLMixin, ABC, Generic[LinkType]): def __init__( @@ -130,6 +147,7 @@ def __init__( self._urdf_filepath = "" # Time to checkout the links for geometry information + auto_symbolic = False for link in self.links: # Add link back to robot object link._robot = self @@ -145,6 +163,21 @@ def __init__( if len(link.geometry) > 0: self._hasgeometry = True + # Detect symbolic (e.g. SymPy) model parameters regardless of + # whether the caller remembered to pass symbolic=True -- see + # rne.md issue 3/4: forgetting the flag previously left + # self.symbolic False even with genuinely symbolic link + # parameters, which broke rne_python()'s own float64 + # allocation, not just the C dispatch. symbolic= is kept as an + # override (OR'd in below), not the source of truth. + if not auto_symbolic: + for attr in _SYMBOLIC_LINK_ATTRS: + if _is_symbolic(getattr(link, attr, None)): + auto_symbolic = True + break + + self._symbolic = self._symbolic or auto_symbolic + # Current joint configuraiton, velocity, acceleration self.q = np.zeros(self.n) self.qd = np.zeros(self.n) @@ -810,19 +843,15 @@ def gravity(self) -> NDArray: """ Get/set default gravitational acceleration (Robot superclass) - - ``robot.name`` is the default gravitational acceleration - - ``robot.name = ...`` checks and sets default gravitational + - ``robot.gravity`` is the default gravitational acceleration + - ``robot.gravity = ...`` checks and sets default gravitational acceleration - - :param gravity: the new gravitational acceleration for this robot + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive) :returns: gravitational acceleration - .. rubric:: Notes - - If the z-axis is upward, out of the Earth, this should be - a positive number. - """ return self._gravity diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index ab17e5eca..d9f7a6332 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -11,6 +11,7 @@ import copy import numpy as np from roboticstoolbox.robot.Robot import Robot # DHLink +from roboticstoolbox.robot.BaseRobot import _is_symbolic from roboticstoolbox.ets.ETS import ETS, ET from roboticstoolbox.robot.DHLink import DHLink from roboticstoolbox.tools.params import rtb_set_param @@ -55,7 +56,9 @@ class DHRobot(Robot): :type base: SE3 :param tool: Location of the tool :type tool: SE3 - :param gravity: Gravitational acceleration vector + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive) :type gravity: ndarray(3) A concrete superclass for arm type robots defined using Denavit-Hartenberg @@ -1362,8 +1365,7 @@ def _copy_to_cpp(self): if self._frne is not None: delete(self._frne) - # we negate gravity here, since the C code has the sign wrong - self._frne = init(self.n, self.mdh, L, -self.gravity) + self._frne = init(self.n, self.mdh, L) self._frne_stale = False def delete_rne(self): @@ -1387,9 +1389,11 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): :type qd: ndarray(n) :param qdd: The joint accelerations of the robot :type qdd: ndarray(n) - :param gravity: Gravitational acceleration to override robot's gravity - value - :type gravity: ndarray(6) + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); overrides robot's gravity + value if given + :type gravity: ndarray(3) :param fext: Specify wrench acting on the end-effector :math:`W=[F_x F_y F_z M_x M_y M_z]` :type fext: ndarray(6) @@ -1412,62 +1416,100 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): :seealso: :func:`rne_python` """ + # Symbolic-aware dispatch (rne.md issues 1/2/4): the C extension + # requires float64 link/state data throughout, so route straight to + # rne_python() -- the always-works fallback -- if the model itself + # was built from symbolic parameters (self.symbolic, now + # auto-detected at construction time regardless of whether the + # caller remembered to pass symbolic=True -- see BaseRobot.__init__) + # or if q/qd/qdd for *this call* are symbolic, mirroring the same + # is-symbolic-before-touching-C idiom used throughout + # roboticstoolbox.ets.fknm for fkine/jacobian dispatch. + if self.symbolic or _is_symbolic(q) or _is_symbolic(qd) or _is_symbolic(qdd): + return self.rne_python( + q, qd, qdd, gravity=gravity, fext=fext, base_wrench=base_wrench + ) + if self._frne is None or self._frne_stale: self._copy_to_cpp() - if self._frne is None or base_wrench: + if self._frne is None: return self.rne_python( q, qd, qdd, gravity=gravity, fext=fext, base_wrench=base_wrench ) - trajn = 1 - + # Belt-and-suspenders: any *other* unanticipated incompatibility + # with the C path (e.g. a shape/type quirk the checks above didn't + # anticipate) degrades gracefully to the pure-Python implementation + # rather than propagating a raw TypeError/ValueError from the C + # extension's argument marshalling. + q_in, qd_in, qdd_in = q, qd, qdd try: - q = getvector(q, self.n, "row") - qd = getvector(qd, self.n, "row") - qdd = getvector(qdd, self.n, "row") - except ValueError: - trajn = q.shape[0] - verifymatrix(q, (trajn, self.n)) - verifymatrix(qd, (trajn, self.n)) - verifymatrix(qdd, (trajn, self.n)) - - # Ensure row slices are C-contiguous (frne C code assumes stride-1 arrays) - q = np.ascontiguousarray(q, dtype=float) - qd = np.ascontiguousarray(qd, dtype=float) - qdd = np.ascontiguousarray(qdd, dtype=float) - - if gravity is None: - gravity = self.gravity - else: - gravity = getvector(gravity, 3) - - # The c function doesn't handle base rotation, so we need to hack the - # gravity vector instead - gravity = self.base.R.T @ gravity - - if fext is None: - fext = np.zeros(6) - else: - fext = getvector(fext, 6) + trajn = 1 + + try: + q = getvector(q, self.n, "row") + qd = getvector(qd, self.n, "row") + qdd = getvector(qdd, self.n, "row") + except ValueError: + trajn = q.shape[0] + verifymatrix(q, (trajn, self.n)) + verifymatrix(qd, (trajn, self.n)) + verifymatrix(qdd, (trajn, self.n)) + + # Ensure row slices are C-contiguous (frne C code assumes stride-1 arrays) + q = np.ascontiguousarray(q, dtype=float) + qd = np.ascontiguousarray(qd, dtype=float) + qdd = np.ascontiguousarray(qdd, dtype=float) + + if gravity is None: + gravity = self.gravity + else: + gravity = getvector(gravity, 3) + gravity = np.ascontiguousarray(gravity, dtype=float) - tau = np.zeros((trajn, self.n)) + # base rotation and the gravity sign convention are handled inside + # frne() itself now -- pass self.base.R through rather than + # pre-rotating/negating by hand (see tech-debt.md / rne.md) + base_rot = np.ascontiguousarray(self.base.R, dtype=float) - for i in range(trajn): - tau[i, :] = frne( - # we negate gravity here, since the C code has the sign wrong + if fext is None: + fext = np.zeros(6) + else: + fext = getvector(fext, 6) + + # Whole trajectory in a single C call -- frne() loops over all + # trajn rows internally now, rather than once per Python->C + # round trip (rne.md plan step 7: ~35% of wall time on a + # 1000-row trajectory was previously Python-side looping/ + # marshalling overhead, not the C computation itself). + tau_flat, wbase_flat = frne( self._frne, - q[i, :], - qd[i, :], - qdd[i, :], - -gravity, + q, + qd, + qdd, + gravity, + base_rot, fext, ) + tau = np.asarray(tau_flat).reshape(trajn, self.n) + wbase = np.asarray(wbase_flat).reshape(trajn, 6) + except (TypeError, ValueError): + return self.rne_python( + q_in, qd_in, qdd_in, + gravity=gravity, fext=fext, base_wrench=base_wrench, + ) - if trajn == 1: - return tau[0, :] + if base_wrench: + if trajn == 1: + return tau[0, :], wbase[0, :] + else: + return tau, wbase else: - return tau + if trajn == 1: + return tau[0, :] + else: + return tau def rne_python( self, @@ -1485,8 +1527,9 @@ def rne_python( :param Q: Joint coordinates :param QD: Joint velocity :param QDD: Joint acceleration - :param gravity: gravitational acceleration, defaults to attribute - of self + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); defaults to attribute of self :type gravity: array_like(3), optional :param fext: end-effector wrench, defaults to None :type fext: array-like(6), optional @@ -1498,17 +1541,17 @@ def rne_python( :return: Joint force/torques :rtype: NumPy array - Recursive Newton-Euler for standard Denavit-Hartenberg notation. + Recursive Newton-Euler for standard or modified Denavit-Hartenberg notation. - - ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is - the number of robot joints. The result has shape (n,). - - ``rne_dh(q, qd, qdd)`` where the arguments have shape (m,n) where n - is the number of robot joints and where m is the number of steps in - the joint trajectory. The result has shape (m,n). - - ``rne_dh(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] with - shape (3n,), and the result has shape (n,). - - ``rne_dh(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] with - shape (m,3n) and the result has shape (m,n). + - ``rne_python(q, qd, qdd)`` where the arguments have shape (n,) where + n is the number of robot joints. The result has shape (n,). + - ``rne_python(q, qd, qdd)`` where the arguments have shape (m,n) + where n is the number of robot joints and where m is the number of + steps in the joint trajectory. The result has shape (m,n). + - ``rne_python(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] + with shape (3n,), and the result has shape (n,). + - ``rne_python(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] + with shape (m,3n) and the result has shape (m,n). .. note:: - This is a pure Python implementation and slower than the .rne() @@ -1529,7 +1572,19 @@ def removesmall(x): n = self.n - if self.symbolic: + # dtype must reflect this *call's* symbolic-ness, not just the + # model's: self.symbolic is a construction-time flag over the link + # parameters, but Q/QD/QDD can be symbolic even for a robot built + # entirely from numeric parameters (e.g. differentiating tau + # symbolically w.r.t. q for a concrete, numeric-mass robot) -- see + # rne.md issue 4. Using only self.symbolic here left rne_python() + # allocating float64 arrays that crashed on the first symbolic + # intermediate value, even though it's supposed to be the + # always-works fallback DHRobot.rne() dispatches to. + symbolic_call = ( + self.symbolic or _is_symbolic(Q) or _is_symbolic(QD) or _is_symbolic(QDD) + ) + if symbolic_call: dtype = "O" else: dtype = None @@ -1568,7 +1623,10 @@ def removesmall(x): tau = np.zeros((nk, n), dtype=dtype) if base_wrench: - wbase = np.zeros((nk, n), dtype=dtype) + # always 6 (a wrench), not n (joint count) -- previously used n, + # which happened to work for 6-DOF robots by coincidence and + # crashed with a shape mismatch for any other DOF count + wbase = np.zeros((nk, 6), dtype=dtype) for k in range(nk): # take the k'th row of data @@ -1604,7 +1662,10 @@ def removesmall(x): Rb = t2r(base).T w = Rb @ w wd = Rb @ wd - vd = Rb @ gravity + # rotate the already-negated vd (was missing the negation -- + # this branch used to compute +Rb @ gravity while the + # identity-base case above correctly used -gravity) + vd = Rb @ vd # ---------------- initialize some variables ----------------- # @@ -1626,10 +1687,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)] @@ -1651,7 +1716,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 @@ -1722,7 +1794,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_ @@ -1767,7 +1844,7 @@ def removesmall(x): tau[k, j] = ( t + link.G**2 * link.Jm * qdd_k[j] - - link.friction(qd_k[j], coulomb=not self.symbolic) + - link.friction(qd_k[j], coulomb=not symbolic_call) ) if debug: print( @@ -1783,21 +1860,6 @@ def removesmall(x): f = R @ f wbase[k, :] = np.r_[f, nn] - # if self.symbolic: - # # simplify symbolic expressions - # print( - # 'start symbolic simplification, this might take a while...') - # # from sympy import trigsimp - - # # tau = trigsimp(tau) - # # consider using multiprocessing to spread over cores - # # https://stackoverflow.com/questions/33844085/using-multiprocessing-with-sympy - # print('done') - # if tau.shape[0] == 1: - # return tau.reshape(self.n) - # else: - # return tau - if base_wrench: if tau.shape[0] == 1: return tau.flatten(), wbase.flatten() diff --git a/src/roboticstoolbox/robot/Dynamics.py b/src/roboticstoolbox/robot/Dynamics.py index 5d77422ab..16872e0dc 100644 --- a/src/roboticstoolbox/robot/Dynamics.py +++ b/src/roboticstoolbox/robot/Dynamics.py @@ -1,14 +1,29 @@ """ Rigid-body dynamics functionality of the Toolbox. -Requires access to: - - * ``links`` list of ``Link`` objects, atttribute - * ``rne()`` the inverse dynamics method - -so must be subclassed by ``DHRobot`` class. - -:todo: perhaps these should be abstract properties, methods of this calss +``DynamicsMixin`` holds *derived* dynamics quantities -- ``accel``, +``gravload``, ``coriolis``, ``inertia``, ``itorque``, ``pay``, etc. -- +implemented generically, in terms of a small set of primitives (``rne``, +``jacob0``, ...) declared abstractly by ``RobotProto``. This mixin never +touches representation-specific internals (DH parameters, ETS chains, the +compiled extension) directly. + +``rne()`` itself is deliberately *not* defined here -- it's a primitive, +implemented differently per concrete class: ``Robot``'s generic Featherstone +spatial-vector recursion (ETS-based, used by ``ERobot``/URDF robots, which +have no DH/MDH concept at all) vs. ``DHRobot``'s two DH-specific +implementations (``rne_python()``, hand-derived; ``rne()``, the compiled +``ne.c`` extension). Keeping those out of this file preserves the +representation-agnostic boundary above. + +Known issue (rne.md, tech-debt.md): ``Robot.rne()`` is currently wrong for +ETS chains with joint-first-in-segment structure (which any standard-DH +derived chain has) -- ``ne.c``/``rne_python()`` don't share this bug (``ne.c`` +handles both DH conventions; ``rne_python()`` is explicitly documented as +standard-DH-only). This matters more than a typical "wrong for one +convention" bug because ``Robot.rne()`` is the *only* dynamics +implementation available to ``ERobot``/URDF/general robots -- there's no +alternative to fall back on the way ``DHRobot`` has two. """ from collections import namedtuple @@ -397,8 +412,10 @@ def accel(self: RobotProto, q, qd, torque, gravity=None): :type qd: ndarray(n,) or ndarray(m,n) :param torque: Joint torques of the robot :type torque: ndarray(n,) or ndarray(m,n) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the ``gravity`` attribute of self). + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); if not supplied, uses the + ``gravity`` attribute of self :returns: Joint accelerations :rtype: ndarray(n,) @@ -815,8 +832,10 @@ def gravload( :param q: Joint coordinates :type q: ndarray(n,) or ndarray(m,n) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the stored gravity values). + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); if not supplied, uses the + stored gravity values :type gravity: ndarray(3,) :returns: The generalised joint force/torques due to gravity :rtype: ndarray(n,) @@ -1107,8 +1126,10 @@ def gravload_x( :param q: Joint coordinates :type q: ndarray(n,) or ndarray(m,n) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the ``gravity`` attribute of self). + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); if not supplied, uses the + ``gravity`` attribute of self :type gravity: ndarray(3,) :param pinv: use pseudo inverse rather than inverse (Default value = False) :param representation: the type of analytical Jacobian to use, default is @@ -1224,8 +1245,10 @@ def accel_x( :type xd: ndarray(6,) :param wrench: Wrench applied to the end-effector :type wrench: ndarray(6,) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the ``gravity`` attribute of self). + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); if not supplied, uses the + ``gravity`` attribute of self :param pinv: use pseudo inverse rather than inverse :param representation: the type of analytical Jacobian to use, default is ``'rpy/xyz'`` diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 685c3d761..9e1ab7e47 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1594,7 +1594,9 @@ def rne( :param qd: Joint velocity :param qdd: Joint acceleration :param symbolic: If True, supports symbolic expressions - :param gravity: Gravitational acceleration, defaults to attribute of self + :param gravity: gravitational acceleration in the world frame, + downwards gravitational force is equivalent to robot base + acceleration upwards (positive); defaults to attribute of self :returns: Joint force/torques ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is @@ -1615,8 +1617,49 @@ def rne( - This version supports symbolic model parameters - Verified against MATLAB code + .. warning:: + + Assumes each link's joint is the *last* element of its own ETS + segment (Featherstone's spatial-vector convention -- fixed + geometry gets you *to* the joint, the joint is the last thing + applied before the next link's frame). This is guaranteed for + any ``Robot``/``ERobot`` built normally, either from a raw ETS + (``Robot.__init__`` splits it via ``ETS.split()``, whose + default "last" method enforces this per segment), from a URDF + (each link's ETS is built fixed-transform-then-joint, in that + order), or from a ``PoERobot`` (``_update_ets()`` appends the + joint ET last too). It does **not** hold for a ``DHRobot`` + using standard DH conventions (``mdh=False``), where the joint + comes *first*, followed by ``d``/``a``/``alpha`` -- calling + ``Robot.rne(dh_instance, ...)`` directly (bypassing + ``DHRobot``'s own correct ``rne()``/``rne_python()``) gives + silently wrong answers. A ``DHRobot`` built with ``mdh=True`` + *is* joint-last (``DHLink._to_ets()``'s MDH branch reorders a + revolute link's ``d`` translation to precede the joint + rotation -- valid since a z-rotation and a z-translation + commute -- so the joint ET is always last regardless of ``d``) + and works correctly through this path. See rne.md / + tech-debt.md. """ + # Checked via the `mdh` attribute rather than isinstance/class name: + # joint-last compliance tracks the DH convention actually in use + # (DHLink._to_ets() puts the joint last for mdh=True, not for + # mdh=False), not the DHRobot class itself -- a DHRobot(mdh=True) + # instance is structurally fine here (verified numerically against + # rne_python() with nonzero d/alpha/mass/inertia). Non-DHRobot + # types have no `mdh` attribute and default (via getattr) to True, + # since their construction (ETS.split(), URDF, PoERobot's + # _update_ets()) already guarantees joint-last independently of DH + # conventions. + assert getattr(self, "mdh", True), ( + "Robot.rne() assumes each link's joint is the last element of " + "its own ETS segment, which does not hold for a DHRobot built " + "with mdh=False (standard DH). Call DHRobot's own " + "rne()/rne_python() instead of Robot.rne(dh_instance, ...) " + "directly." + ) + n = self.n # n = len(self.links) @@ -1660,7 +1703,7 @@ def rne( for idx in group: link = self.links[idx] - I_int = I_int + SpatialInertia(m=link.m, r=link.r) + I_int = I_int + SpatialInertia(m=link.m, r=link.r, I=link.I) if link.v is not None: s.append(link.v.s) # type: ignore[union-attr] @@ -1668,9 +1711,21 @@ def rne( I[i] = I_int if gravity is None: - a_grav = -SpatialAcceleration(self.gravity) - else: # pragma nocover - a_grav = -SpatialAcceleration(gravity) + gravity = self.gravity + # no dtype= here: gravity may contain SymPy symbols (test_symdyn), + # and forcing float would break that -- let numpy infer object dtype + gravity = np.asarray(gravity) + # gravity is defined in the world frame; rotate into the root link + # frame via the base orientation before negating to the effective + # upward acceleration RNE expects -- see rne_python()'s equivalent + # handling, and rne.md for the bug this fixes (previously ignored + # self.base entirely). Skipped for an identity base rather than + # multiplying by R.T unconditionally: with symbolic gravity + # components, an identity-matrix matmul still introduces spurious + # "1.0*" float literals into the resulting expression. + if not np.array_equal(self.base.R, np.eye(3)): + gravity = self.base.R.T @ gravity + a_grav = -SpatialAcceleration(gravity) # For the following, v, a, f, I, s, Xup are all lists of length n # where the indices correspond to the index of the group within @@ -1747,6 +1802,17 @@ def rne( # next line could be dot(), but fails for symbolic arguments Q[k, j] = sum(f[j].A * s[j]) + # add armature inertia and friction -- consistent with + # DHRobot.rne_python()/ne.c, which both add G^2*Jm*qdd + # (armature) and subtract link.friction() (viscous B + + # Coulomb Tc). Previously missing entirely from this + # implementation -- see tech-debt.md. + jindex = joint.jindex + Q[k, j] += ( + joint.G**2 * joint.Jm * qddk[jindex] + - 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) diff --git a/src/roboticstoolbox/robot/cpp-extensions/frne.h b/src/roboticstoolbox/robot/cpp-extensions/frne.h index a8116c8f7..8b9e93883 100644 --- a/src/roboticstoolbox/robot/cpp-extensions/frne.h +++ b/src/roboticstoolbox/robot/cpp-extensions/frne.h @@ -90,7 +90,7 @@ void newton_euler ( double *tau, /*!< returned joint torques */ double *qd, /*!< joint velocities */ double *qdd, /*!< joint accelerations */ - double *fext, /*!< external force on manipulator tip */ + double *fext, /*!< wrench applied to end-effector */ int stride /*!< indexing stride for qd, qdd */ ); #endif \ No newline at end of file diff --git a/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp b/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp index bb16803bc..7cdb8ada1 100644 --- a/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp +++ b/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include @@ -20,6 +21,7 @@ extern "C" { #include "frne.h" // Robot, Link, Vect, Rot, DHType, newton_euler } +#include #include // ── Robot wrapper ───────────────────────────────────────────────────────────── @@ -81,8 +83,7 @@ NB_MODULE(_frne_c, m) { // ── init ────────────────────────────────────────────────────────────────── m.def("init", [](int njoints, int mdh, - nb::ndarray L_arr, - nb::ndarray grav_arr) -> std::unique_ptr + nb::ndarray L_arr) -> std::unique_ptr { auto obj = std::make_unique(); @@ -92,12 +93,10 @@ NB_MODULE(_frne_c, m) { robot->njoints = njoints; robot->dhtype = (DHType)mdh; robot->links = (Link *)PyMem_RawCalloc(njoints, sizeof(Link)); - robot->gravity = (Vect *)PyMem_RawMalloc(sizeof(Vect)); - - const double *grav = (const double *)grav_arr.data(); - robot->gravity->x = grav[0]; - robot->gravity->y = grav[1]; - robot->gravity->z = grav[2]; + // gravity is set per-call in frne() (it's cheap and lets callers + // override it per-call, same as before) -- zero-init here only + // so the pointer is never read uninitialized. + robot->gravity = (Vect *)PyMem_RawCalloc(1, sizeof(Vect)); const double *L = (const double *)L_arr.data(); int idx = 0; @@ -128,16 +127,26 @@ NB_MODULE(_frne_c, m) { }); // ── frne ────────────────────────────────────────────────────────────────── + // q_arr/qd_arr/qdd_arr are (trajn, njoints) -- a full trajectory, looped + // over entirely in C++ rather than once per Python->C call (previously + // DHRobot.rne() called this once per row; ~35% of wall time on a 1000-row + // trajectory was that Python-side looping/marshalling overhead, not the + // C computation itself -- see rne.md issue 5/plan step 7). gravity, + // base_rot and fext are constant across the whole trajectory, matching + // what the old per-row Python loop already passed every iteration. m.def("frne", [](RobotObj *obj, nb::ndarray q_arr, nb::ndarray qd_arr, nb::ndarray qdd_arr, nb::ndarray grav_arr, - nb::ndarray fext_arr) -> std::vector + nb::ndarray base_rot_arr, + nb::ndarray fext_arr) + -> std::pair, std::vector> { Robot *robot = obj->ptr; int nj = robot->njoints; + int trajn = (int)q_arr.shape(0); const double *q = (const double *)q_arr.data(); const double *qd = (const double *)qd_arr.data(); @@ -145,33 +154,79 @@ NB_MODULE(_frne_c, m) { const double *grav = (const double *)grav_arr.data(); const double *fext = (const double *)fext_arr.data(); - robot->gravity->x = grav[0]; - robot->gravity->y = grav[1]; - robot->gravity->z = grav[2]; + // gravity is given in the world frame; ne.c's recursion has + // always implicitly expected it pre-rotated into the root link + // frame and negated to the effective upward acceleration + // (previously done by hand in DHRobot.rne()/_copy_to_cpp(), + // duplicated and easy to get wrong -- see tech-debt.md / + // rne.md). base_rot_arr is self.base.R, row-major flattened; + // Rot's n/o/a are its columns. Computed once -- constant across + // the trajectory. + const double *br = (const double *)base_rot_arr.data(); + Rot base_R; + base_R.n = {br[0], br[3], br[6]}; + base_R.o = {br[1], br[4], br[7]}; + base_R.a = {br[2], br[5], br[8]}; + + Vect grav_world = {grav[0], grav[1], grav[2]}; + Vect grav_root; + rot_trans_vect_mult(&grav_root, &base_R, &grav_world); + robot->gravity->x = -grav_root.x; + robot->gravity->y = -grav_root.y; + robot->gravity->z = -grav_root.z; - // Allocate temporaries (newton_euler takes non-const) - std::vector qd_buf(qd, qd + nj); - std::vector qdd_buf(qdd, qdd + nj); std::vector fext_buf(fext, fext + 6); - std::vector tau(nj, 0.0); - - for (int j = 0; j < nj; j++) { - Link *l = &robot->links[j]; - switch (l->jointtype) { - case REVOLUTE: - rot_mat(l, q[j] + l->offset, l->D, robot->dhtype); - break; - case PRISMATIC: - rot_mat(l, l->theta, q[j] + l->offset, robot->dhtype); - break; - default: - throw nb::value_error("Invalid joint type (expect R or P)"); + std::vector tau(trajn * nj, 0.0); + std::vector wbase(trajn * 6, 0.0); + + // Per-row temporaries, reused across the trajectory rather than + // reallocated each iteration. + std::vector qd_buf(nj), qdd_buf(nj), tau_row(nj); + + for (int i = 0; i < trajn; i++) { + const double *qi = q + i * nj; + const double *qdi = qd + i * nj; + const double *qddi = qdd + i * nj; + + std::copy(qdi, qdi + nj, qd_buf.begin()); + std::copy(qddi, qddi + nj, qdd_buf.begin()); + + for (int j = 0; j < nj; j++) { + Link *l = &robot->links[j]; + switch (l->jointtype) { + case REVOLUTE: + rot_mat(l, qi[j] + l->offset, l->D, robot->dhtype); + break; + case PRISMATIC: + rot_mat(l, l->theta, qi[j] + l->offset, robot->dhtype); + break; + default: + throw nb::value_error("Invalid joint type (expect R or P)"); + } } + + newton_euler(robot, tau_row.data(), qd_buf.data(), qdd_buf.data(), + fext_buf.data(), 1); + std::copy(tau_row.begin(), tau_row.end(), tau.begin() + i * nj); + + // Base wrench: f(0)/n(0) are already computed by the backward + // recursion above (force/moment on link 0 due to the base) -- + // just not previously exposed. Rotate out of link 0's own + // frame into the base/world frame, matching rne_python's + // wbase convention (Rm[0] @ f, Rm[0] @ n). + Link *l0 = &robot->links[0]; + Vect f_base, n_base; + rot_vect_mult(&f_base, &l0->R, &l0->f); + rot_vect_mult(&n_base, &l0->R, &l0->n); + wbase[i * 6 + 0] = f_base.x; + wbase[i * 6 + 1] = f_base.y; + wbase[i * 6 + 2] = f_base.z; + wbase[i * 6 + 3] = n_base.x; + wbase[i * 6 + 4] = n_base.y; + wbase[i * 6 + 5] = n_base.z; } - newton_euler(robot, tau.data(), qd_buf.data(), qdd_buf.data(), - fext_buf.data(), 1); - return tau; + return {tau, wbase}; }); // ── delete ──────────────────────────────────────────────────────────────── diff --git a/src/roboticstoolbox/robot/cpp-extensions/ne.c b/src/roboticstoolbox/robot/cpp-extensions/ne.c index 1f809f379..e565dc539 100644 --- a/src/roboticstoolbox/robot/cpp-extensions/ne.c +++ b/src/roboticstoolbox/robot/cpp-extensions/ne.c @@ -12,7 +12,7 @@ * * Requires: qd current joint velocities * qdd current joint accelerations - * f applied tip force or load + * f applied end-effector force or load * grav the gravitational constant * * Returns: tau vector of bias torques @@ -67,7 +67,7 @@ newton_euler ( double *tau, /*!< returned joint torques */ double *qd, /*!< joint velocities */ double *qdd, /*!< joint accelerations */ - double *fext, /*!< external force on manipulator tip */ + double *fext, /*!< wrench applied to end-effector */ int stride /*!< indexing stride for qd, qdd */ ) { Vect t1, t2, t3, t4; @@ -75,8 +75,8 @@ newton_euler ( Vect F, N; Vect z0 = {0.0, 0.0, 1.0}; Vect zero = {0.0, 0.0, 0.0}; - Vect f_tip = {0.0, 0.0, 0.0}; - Vect n_tip = {0.0, 0.0, 0.0}; + Vect f_ee = {0.0, 0.0, 0.0}; + Vect n_ee = {0.0, 0.0, 0.0}; int j; double t; Link *links = robot->links; @@ -89,12 +89,12 @@ newton_euler ( /* setup external force/moment vectors */ if (fext) { - f_tip.x = fext[0]; - f_tip.y = fext[1]; - f_tip.z = fext[2]; - n_tip.x = fext[3]; - n_tip.y = fext[4]; - n_tip.z = fext[5]; + f_ee.x = fext[0]; + f_ee.y = fext[1]; + f_ee.z = fext[2]; + n_ee.x = fext[3]; + n_ee.y = fext[4]; + n_ee.z = fext[5]; } #ifdef DEBUG @@ -368,7 +368,7 @@ newton_euler ( * compute f[j] */ if (j == (robot->njoints-1)) - t1 = f_tip; + t1 = f_ee; else rot_vect_mult (&t1, ROT(j+1), f(j+1)); vect_add (f(j), &t1, &F); @@ -385,7 +385,7 @@ newton_euler ( * compute n[j] */ if (j == (robot->njoints-1)) - t1 = n_tip; + t1 = n_ee; else { rot_vect_mult(&t1, ROT(j+1), n(j+1)); rot_vect_mult(&t4, ROT(j+1), f(j+1)); @@ -418,7 +418,7 @@ newton_euler ( rot_vect_mult (&t1, ROT(j+1), f(j+1)); vect_add (f(j), &t4, &t1); } else - vect_add (f(j), &t4, &f_tip); + vect_add (f(j), &t4, &f_ee); /* * compute n[j] @@ -439,11 +439,11 @@ newton_euler ( vect_add(&t1, &t1, &t2); } else { /* cross(R'*pstar,f) */ - vect_cross(&t2, PSTAR(j), &f_tip); + vect_cross(&t2, PSTAR(j), &f_ee); /* nn += R*(nn + cross(R'*pstar,f)) */ vect_add(&t1, &t1, &t2); - vect_add(&t1, &t1, &n_tip); + vect_add(&t1, &t1, &n_ee); } mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j)); diff --git a/src/roboticstoolbox/robot/frne.py b/src/roboticstoolbox/robot/frne.py index 914b787a7..fdd16482e 100644 --- a/src/roboticstoolbox/robot/frne.py +++ b/src/roboticstoolbox/robot/frne.py @@ -21,16 +21,28 @@ _C_AVAILABLE = False -def init(njoints, mdh, L, gravity): +def init(njoints, mdh, L): """Create the C Robot struct handle; returns None when C extension is absent.""" if _C_AVAILABLE: - return _c_init(njoints, mdh, L, gravity) + return _c_init(njoints, mdh, L) return None -def frne(robot, q, qd, qdd, gravity, fext): - """Run Newton-Euler via C; caller must check that robot handle is not None.""" - return _c_frne(robot, q, qd, qdd, gravity, fext) +def frne(robot, q, qd, qdd, gravity, base_rot, fext): + """Run Newton-Euler via C over a whole trajectory in one call; caller + must check that robot handle is not None. + + ``q``/``qd``/``qdd`` are ``(trajn, n)`` -- the whole trajectory is + looped over inside C++, not once per Python call (see rne.md plan step + 7). ``gravity``/``base_rot``/``fext`` are constant across the + trajectory. ``gravity`` is given in the world frame; ``base_rot`` (the + robot's ``self.base.R``, flattened row-major) is applied internally to + rotate it into the root link frame before use -- callers no longer need + to do this by hand. Returns ``(tau, wbase)``, each flattened + ``trajn * n`` / ``trajn * 6`` -- reshape on the caller side -- where + ``wbase`` is the wrench the base experiences, in the base/world frame. + """ + return _c_frne(robot, q, qd, qdd, gravity, base_rot, fext) def delete(robot): diff --git a/tech-debt.md b/tech-debt.md index 6632843cf..da569fb18 100644 --- a/tech-debt.md +++ b/tech-debt.md @@ -57,6 +57,56 @@ in a major version increment, retaining `Robot` as a deprecated alias. **Best done alongside:** the ETS/fknm refactor — the type picture simplifies dramatically if hierarchy and representation are both cleaned up together. +### Related: `Robot.rne()`'s misuse guard checks `mdh`, not class identity + +`Robot.rne()` (`Robot.py`) assumes Featherstone's joint-last-in-segment +structure. A cheap runtime guard rejects the incompatible case: + +```python +assert getattr(self, "mdh", True), ... +``` + +Two design questions came up while adding this guard 2026-07-21, both worth +recording since they'll resurface if the class hierarchy redesign above +happens: + +1. **Blocklist vs. allowlist.** First attempt was a class-name blocklist + (`assert not any(c.__name__ == "DHRobot" for c in type(self).__mro__)`). + An allowlist (`Robot`/`ERobot`/`URDFRobot` by name) was considered and + rejected: it would have wrongly rejected `PoERobot`, a fully compliant + type (its `_update_ets()` also appends the joint ET last) that was easy + to overlook — demonstrating exactly the fragility an allowlist has here. +2. **Class identity was the wrong axis entirely.** `DHLink._to_ets()` shows + joint-last compliance actually tracks the `mdh` flag, not the `DHRobot` + class: the MDH branch reorders a revolute link's `d` translation to + *precede* the joint rotation (valid — a z-rotation and z-translation + about/along the same axis commute), so the joint ET is last regardless + of `d`. A `DHRobot(mdh=True)` instance is therefore structurally fine + through `Robot.rne()`, while `mdh=False` is not — the class-name check + would have wrongly rejected the compliant MDH case too. The final guard + checks `self.mdh` directly (defaulting to `True` via `getattr` for + non-DHRobot types, which have no DH-convention concept and are already + guaranteed joint-last some other way). + +Verifying the MDH case numerically (nonzero `d`, `alpha`, mass, and full +inertia tensor, compared against `rne_python()`) surfaced a real, separate +bug it would otherwise have masked: `SpatialInertia(m=link.m, r=link.r)` in +`Robot.rne()`'s inertia accumulation never passed `I=link.I` — the +rotational inertia tensor was silently dropped for every link, for every +`Robot`/`ERobot`/`URDFRobot`/`PoERobot`/MDH-`DHRobot` call, not just the MDH +edge case. Fixed alongside this guard. Not caught earlier because the +`TwoLink`-based `Robot.rne()`-vs-`rne_python()` equivalence tests used +default (zero) inertia; the tests that *did* set `inertia=True` only +compared the C path against `rne_python()`, never `Robot.rne()`. + +**Revisit when the class hierarchy redesign above happens.** If `DHRobot` +stops being a `Robot` subclass (e.g. becomes `Robot[DHLink]` under the +generic-`LinkType` proposal, or the "one Robot class, polymorphic `Link.A(q)`" +design below is adopted), the whole question may become moot — either there's +only one `Robot` class and the guard is unnecessary, or the DH-convention +distinction is structurally explicit rather than a name-based/attribute-based +runtime check. + --- ## Forward-looking design: one Robot class, polymorphic Link.A(q) @@ -1209,6 +1259,57 @@ Decide on one ownership model and stop straddling both: Either way, the current arrangement (vendored copy + redundant external git install in CI) should not persist indefinitely without a decision. +## `src/roboticstoolbox/blocks/` has essentially no type hints + +Noticed 2026-07-21 while fixing `blocks/arm.py`'s `gravity` parameter +(docstring claimed `float`, actual runtime value is always a 3-vector +passed straight through to `Robot.rne()`/`gravload()` -- the annotation +was simply wrong, and nothing caught it because there was no annotation +to check against). + +`blocks/` has 5 files (`arm.py`, `mobile.py`, `quad_model.py`, +`spatial.py`, `uav.py`) defining `bdsim` block classes -- `arm.py` alone +has 16 `__init__` methods, none with parameter or return type hints. +Unlike the rest of the codebase (which follows the modern-syntax type +hint convention in this repo's global instructions), these blocks are +essentially undocumented at the type level, so mismatches like the +`gravity: float` one can and did sit unnoticed. + +## `examples/ik_speed.py` doesn't run — stale imports, predates this session + +Found 2026-07-23 while adding a portable CPU-info one-liner to +`examples/rne_speed.py` (a new RNE timing-comparison script) and looking +for a sibling to riff off. `ik_speed.py` fails immediately: `import fknm` +at module scope (line 4) — `fknm` hasn't existed as a top-level importable +module since the fknm/frne refactor moved it to +`roboticstoolbox.robot.fknm` (merged to `main` 2026-07-03, well before this +session). Not caused by, or related to, the current dynamics-overhaul +branch — pre-existing breakage that had gone unnoticed. + +Also stale, found by inspection while there (none blocking, all part of the +same cleanup): + +- Unused imports: `swift`, `spatialgeometry as sg`, `sys`, `from numpy + import ndarray`, `from typing import Union, overload, List, Set`. +- No type hints (consistent with the `blocks/` entry above — this predates + the modern-syntax convention too). + +**Proposed fix:** update the `fknm` import to `from roboticstoolbox.robot +import fknm` (or import the specific facade functions actually used), +verify the script runs end-to-end again, then sweep the unused imports. +While there, port over the `cpu_info()` helper added to `rne_speed.py` +(portable one-line CPU description — name, core count, clock speed where +the OS exposes one) so `ik_speed.py`'s output reports what machine it ran +on too. Since that would make it the *second* real call site, worth +factoring `cpu_info()` into a small shared `examples/` helper at that point +rather than copy-pasting it again. + +**Proposed fix:** add type hints throughout `blocks/`, following the same +convention as the rest of the codebase (`NDArray`/`ArrayLike` in +signatures, `X | None` not `Optional[X]`, etc. -- see this user's global +CLAUDE.md type-hint rules). Not urgent on its own, but worth doing +opportunistically whenever a block class is touched for another reason, +and worth a dedicated pass if `blocks/` sees more maintenance. ## JupyterLite "Try it Now" notebook: `ci.yml` stopgap for an upstream piplite indexing bug ### Background diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index 7a5ab6f91..e7f3635fd 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -1,17 +1,30 @@ """ Safety-net tests for the ETS / fknm / frne refactor (Phase 0). -Each test runs an ETS function twice: - 1. via the C extension (fknm) — the normal path - 2. via the pure-Python fallback — forced by patching the C function to raise - -Both paths must produce numerically identical results. These tests will catch -any regression introduced by the Facade refactor (Phase 1), the BaseETS -redesign (Phase 2), or the nanobind port (Phase 3). +Two tiers of test here, and they check different things: + + Tier 1 ("*Fallback" classes, ``test_matches_c_path`` methods): runs an ETS + function twice, once via the C extension (the normal, unpatched call) and + once via the pure-Python fallback (forced by patching the C-calling + function to raise), and checks they agree. This is only meaningful when + the C extension is actually built -- if it isn't, the "C path" call + silently dispatches to Python too (see fknm.py/frne.py), and the + comparison degrades to Python-vs-itself without any indication. These are + gated with ``@unittest.skipUnless(_C_AVAILABLE, ...)`` so that case shows + as an explicit skip, never a false pass. + + Tier 2 ("*Reference" classes): compares whichever path is actually active + against hardcoded ground-truth values, computed once against the C + extension (which Tier 1 already shows agrees with Python). These run + regardless of ``_C_AVAILABLE`` -- they're what actually guarantees a + pure-Python build (no compiled extension at all, e.g. the pyodide/wasm + wheel) is numerically correct on its own terms, not merely "consistent + with itself". Robot used: Franka Panda (7-DOF) for ETS functions; Puma560 (6-DOF DH) for rne. """ +import math import os import sys import timeit @@ -23,6 +36,10 @@ import numpy.testing as nt import sympy +from roboticstoolbox.ets.fknm import _C_AVAILABLE as _FKNM_C_AVAILABLE +from roboticstoolbox.robot.frne import _C_AVAILABLE as _FRNE_C_AVAILABLE +from roboticstoolbox.models.DH.TwoLink import TwoLink + import roboticstoolbox as rtb # roboticstoolbox/ets/ETS.py defines a class also called ETS, and # roboticstoolbox/ets/__init__.py does `from ...ETS import ETS`, which @@ -44,6 +61,9 @@ _ETS_module = sys.modules["roboticstoolbox.ets.ETS"] from spatialmath import SE3 +_NO_FKNM_C = "compiled _fknm_c extension not built -- can't cross-validate against C" +_NO_FRNE_C = "compiled _frne_c extension not built -- can't cross-validate against C" + # --------------------------------------------------------------------------- # Shared fixtures @@ -64,6 +84,13 @@ def _puma(): return rtb.models.DH.Puma560() +def _twolink(): + # Standard-DH, non-identity base (SE3.Rx(pi/2)) -- unlike Puma560 + # (identity base), this exercises the self.base rotation path in + # rne()/rne_python()/Robot.rne(). + return rtb.models.DH.TwoLink() + + # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- @@ -148,6 +175,7 @@ def _py(fknm, q, Je, tool, _data=None, _n=None): # eval() / fkine() fallback # --------------------------------------------------------------------------- +@unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) class TestEvalFallback(unittest.TestCase): """eval() Python fallback produces numerically identical results to C path.""" @@ -223,6 +251,7 @@ def test_returns_SE3(self): result = self.ets.fkine(self.q) self.assertIsInstance(result, SE3) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_matches_c_path(self): c = self.ets.fkine(self.q) with _no_c_fkine(): @@ -253,11 +282,13 @@ def test_shape(self): py = self.ets.jacob0(self.q) self.assertEqual(py.shape, (6, self.ets.n)) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_matches_c_path(self): with _no_c_jacob0(): py = self.ets.jacob0(self.q) nt.assert_array_almost_equal(py, self.c_result) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_with_tool(self): tool = SE3.Tz(0.1).A c = self.ets.jacob0(self.q, tool=tool) @@ -283,11 +314,13 @@ def test_shape(self): py = self.ets.jacobe(self.q) self.assertEqual(py.shape, (6, self.ets.n)) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_matches_c_path(self): with _no_c_jacobe(): py = self.ets.jacobe(self.q) nt.assert_array_almost_equal(py, self.c_result) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_with_tool(self): tool = SE3.Tz(0.1).A c = self.ets.jacobe(self.q, tool=tool) @@ -315,11 +348,13 @@ def test_shape(self): n = self.ets.n self.assertEqual(py.shape, (n, 6, n)) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_matches_c_path(self): with _no_c_hessian0(): py = self.ets.hessian0(self.q, J0=self.J0) nt.assert_array_almost_equal(py, self.c_result) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_without_precomputed_J0(self): """hessian0 should compute J0 internally when not supplied.""" c = self.ets.hessian0(self.q) @@ -350,12 +385,80 @@ def test_shape(self): n = self.ets.n self.assertEqual(py.shape, (n, 6, n)) + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_matches_c_path(self): with _no_c_hessiane(): py = self.ets.hessiane(self.q, Je=self.Je) nt.assert_array_almost_equal(py, self.c_result) +# --------------------------------------------------------------------------- +# Reference values: fkine/jacob0 against known Panda results (regression +# guard, independent of C). +# +# Unlike test_matches_c_path above, these do NOT compare the C path against +# the Python path -- they compare whichever path is actually active against +# hardcoded ground truth. That makes them meaningful in every environment, +# including the pure-Python (no _fknm_c) case the pyodide wheel ships: +# when C is unavailable, ets.fkine()/jacob0() already dispatch to the +# Python implementation automatically (ETS_init returns None), so this is +# exactly what a C-less install needs to prove it's still correct on its +# own terms, not just "consistent with itself". +# +# Values computed once against the C extension (see TestFkineFallback / +# TestJacob0Fallback, which already show the two paths agree) using +# ets.fkine(PANDA_Q) / ets.jacob0(PANDA_Q) on Franka Panda. +# --------------------------------------------------------------------------- + +class TestFkineReference(unittest.TestCase): + """fkine() on Panda against hardcoded reference values.""" + + def setUp(self): + self.ets = _panda_ets() + + def test_fkine(self): + nt.assert_array_almost_equal( + self.ets.fkine(PANDA_Q).A, + [ + [-0.50827907, -0.57904589, 0.63746234, 0.44707793], + [0.83014553, -0.52639462, 0.18375824, 0.16175746], + [0.22915229, 0.62258699, 0.74824773, 0.96828043], + [0.00000000, 0.00000000, 0.00000000, 1.00000000], + ], + decimal=6, + ) + + +class TestJacob0Reference(unittest.TestCase): + """jacob0() on Panda against hardcoded reference values.""" + + def setUp(self): + self.ets = _panda_ets() + + def test_jacob0(self): + nt.assert_array_almost_equal( + self.ets.jacob0(PANDA_Q), + [ + [-1.61757460e-01, 1.07976800e-01, -3.41587423e-02, + 3.35336541e-01, -1.07172949e-02, 1.03491264e-01, 0.0], + [4.47077932e-01, 6.26036931e-01, 4.16714460e-01, + -8.05054464e-02, 7.78094113e-02, -1.17637200e-02, 0.0], + [-6.03103094e-17, -2.35392404e-01, -8.20662027e-02, + -5.14331129e-01, -9.97831132e-03, -2.02887489e-01, 0.0], + [4.61988821e-17, -9.85449730e-01, 3.37672585e-02, + -6.16735653e-02, 6.68449878e-01, -1.35361558e-01, + 6.37462344e-01], + [9.61515015e-18, 1.69967143e-01, 1.95778638e-01, + 9.79165111e-01, 1.84470262e-01, 9.82748279e-01, + 1.83758244e-01], + [1.00000000e+00, -7.36706147e-17, 9.80066578e-01, + -1.93473657e-01, 7.20517510e-01, -1.26028049e-01, + 7.48247732e-01], + ], + decimal=6, + ) + + # --------------------------------------------------------------------------- # Symbolic inputs # --------------------------------------------------------------------------- @@ -419,6 +522,7 @@ def test_symbolic_jacobe(self): # rne: C path vs rne_python (pure Python NE) # --------------------------------------------------------------------------- +@unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) class TestRNEFallback(unittest.TestCase): """rne() C path (frne/ne.c) agrees with rne_python() on Puma560.""" @@ -509,6 +613,569 @@ def test_external_wrench(self): ) + + +# --------------------------------------------------------------------------- +# rne: C path vs rne_python on a rotated-base robot (TwoLink) +# +# Puma560 (used above) has an identity base, so it never exercised the +# base-rotation handling in either path. TwoLink's base is SE3.Rx(pi/2) -- +# this is what regresses the bug where Robot.rne() ignored self.base +# entirely and rne_python()'s rotated-base branch was missing a negation +# (see rne.md / tech-debt.md). +# --------------------------------------------------------------------------- + +@unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) +class TestRNERotatedBaseFallback(unittest.TestCase): + """rne() C path agrees with rne_python() on TwoLink (rotated base).""" + + def setUp(self): + self.robot = _twolink() + self.z = np.zeros(2) + + def _compare(self, q, qd, qdd, **kwargs): + c = self.robot.rne(q, qd, qdd, **kwargs) + py = self.robot.rne_python(q, qd, qdd, **kwargs) + nt.assert_array_almost_equal(c, py, decimal=4) + + def test_gravity_only(self): + self._compare([0.3, 0.5], self.z, self.z) + + def test_other_pose(self): + self._compare([1.2, -0.7], self.z, self.z) + + +class TestRNERotatedBaseReference(unittest.TestCase): + """rne() on TwoLink (rotated base) against hardcoded reference values. + + Independently verified against the Lagrangian identity (ground truth + via numerical differentiation of gravitational potential energy, + convention-independent of either RNE implementation) -- see the + TwoLink section of rne.md. + """ + + def setUp(self): + self.robot = _twolink() + self.z = np.zeros(2) + + def test_gravity_only(self): + nt.assert_array_almost_equal( + self.robot.rne([0.3, 0.5], self.z, self.z), + [-17.457309, -3.413863], + decimal=4, + ) + + +# --------------------------------------------------------------------------- +# base_wrench: C path vs rne_python, on both an identity-base (Puma560) and +# rotated-base (TwoLink) robot, with and without a wrench applied to the +# end-effector. +# --------------------------------------------------------------------------- + +@unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) +class TestBaseWrenchFallback(unittest.TestCase): + """rne(base_wrench=True) C path agrees with rne_python().""" + + # wrench applied to end-effector: [Fx, Fy, Fz, Mx, My, Mz] + FEXT = [0.5, 0.7, 0.7, 0.1, 0.2, 0.3] + + def _compare(self, robot, q, fext=None): + n = robot.n + z = np.zeros(n) + tau_c, wbase_c = robot.rne(q, z, z, fext=fext, base_wrench=True) + tau_py, wbase_py = robot.rne_python(q, z, z, fext=fext, base_wrench=True) + nt.assert_array_almost_equal(tau_c, tau_py, decimal=4) + nt.assert_array_almost_equal(wbase_c, wbase_py, decimal=4) + + def test_puma_no_fext(self): + puma = _puma() + self._compare(puma, puma.qn) + + def test_puma_with_ee_wrench(self): + puma = _puma() + self._compare(puma, puma.qn, fext=self.FEXT) + + def test_twolink_no_fext(self): + self._compare(_twolink(), [0.3, 0.5]) + + def test_twolink_with_ee_wrench(self): + self._compare(_twolink(), [0.3, 0.5], fext=self.FEXT) + + def test_ee_wrench_changes_tau(self): + """Sanity check the comparison above isn't vacuously trivial: an + end-effector wrench should actually change the joint torques.""" + puma = _puma() + z = np.zeros(puma.n) + tau_plain = puma.rne(puma.qn, z, z) + tau_wrench = puma.rne(puma.qn, z, z, fext=self.FEXT) + self.assertGreater(np.abs(tau_wrench - tau_plain).max(), 0.1) + + +class TestBaseWrenchReference(unittest.TestCase): + """base_wrench against hardcoded reference values, independent of C.""" + + FEXT = [0.5, 0.7, 0.7, 0.1, 0.2, 0.3] + + def test_puma_with_ee_wrench(self): + puma = _puma() + z = np.zeros(puma.n) + tau, wbase = puma.rne(puma.qn, z, z, fext=self.FEXT, base_wrench=True) + nt.assert_array_almost_equal( + tau, + [0.422447, 31.151777, 5.913429, 0.282843, -0.171747, 0.300000], + decimal=4, + ) + nt.assert_array_almost_equal( + wbase, + [0.700000, 0.700000, 229.544500, -48.487576, -30.681496, 0.422447], + decimal=4, + ) + + def test_twolink_with_ee_wrench(self): + robot = _twolink() + z = np.zeros(robot.n) + tau, wbase = robot.rne([0.3, 0.5], z, z, fext=self.FEXT, base_wrench=True) + nt.assert_array_almost_equal(tau, [-15.603289, -2.413863], decimal=4) + nt.assert_array_almost_equal( + wbase, + [-0.153796, -18.753627, 0.700000, 0.635213, -0.945353, -15.603289], + decimal=4, + ) + + +# --------------------------------------------------------------------------- +# 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) + + def test_robot_rne_rejects_standard_dh(self): + """Robot.rne() (the ETS/Featherstone implementation, used by + ERobot/URDF/PoERobot) genuinely cannot handle a standard-DH + (mdh=False) DHRobot -- the joint isn't the last element of its own + ETS segment. Rather than silently returning a wrong torque (the + old behaviour -- see rne.md/tech-debt.md), it now asserts. This + confirms the guard fires for the one case it must, complementing + test_robot_rne_on_mdh_variant_matches_standard_dh_rne_python (which + confirms it does *not* fire, and gives correct results, for the + mdh=True case).""" + from roboticstoolbox.robot.Robot import Robot as RobotBase + + z = np.zeros(2) + with self.assertRaises(AssertionError): + RobotBase.rne(self.std, self.poses[0], z, z) + + +# --------------------------------------------------------------------------- +# 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) + + +# --------------------------------------------------------------------------- +# Robot.rne() (ETS/Featherstone) dropped the rotational inertia tensor +# entirely: SpatialInertia(m=link.m, r=link.r) never passed I=link.I. Never +# caught by TestTwoLinkDHMDHEquivalence above because TwoLink's default +# (zero) inertia makes the missing term a no-op, and TwoLink's d=alpha=0 +# (planar) also hides it even with inertia=True. Needs nonzero d *and* +# alpha *and* a real inertia tensor together -- see tech-debt.md. +# --------------------------------------------------------------------------- + +class TestRobotRneInertiaTensor(unittest.TestCase): + """Robot.rne() must use the full inertia tensor, not just mass+COM.""" + + def _robot(self): + from roboticstoolbox import DHRobot, RevoluteMDH + + links = [ + RevoluteMDH( + d=0.5, a=0.3, alpha=0.6, + m=2.0, r=[0.1, 0.05, 0.02], + I=[0.01, 0.02, 0.03, 0.001, 0.002, 0.003], + ), + RevoluteMDH( + d=0.2, a=0.25, alpha=0.8, + m=1.5, r=[0.08, 0.0, 0.01], + I=[0.005, 0.006, 0.007, 0.0001, 0.0002, 0.0003], + ), + ] + return DHRobot(links, name="rne_inertia_test") + + def test_robot_rne_matches_rne_python_with_full_inertia_tensor(self): + from roboticstoolbox.robot.Robot import Robot as RobotBase + + robot = self._robot() + q = np.array([0.3, -0.5]) + qd = np.array([0.4, -0.2]) + qdd = np.array([0.1, 0.3]) + + truth = robot.rne_python(q, qd, qdd) + result = RobotBase.rne(robot, q, qd, qdd) + nt.assert_array_almost_equal(result, truth, decimal=8) + + def test_robot_rne_matches_rne_python_static_and_random_poses(self): + from roboticstoolbox.robot.Robot import Robot as RobotBase + + robot = self._robot() + z = np.zeros(2) + + # static (gravity only) -- agreed even before the fix, since it + # doesn't exercise qdd, but kept as a baseline + truth = robot.rne_python(np.array([0.3, -0.5]), z, z) + result = RobotBase.rne(robot, np.array([0.3, -0.5]), z, z) + nt.assert_array_almost_equal(result, truth, decimal=8) + + rng = np.random.default_rng(1) + for _ in range(20): + q = rng.uniform(-np.pi, np.pi, 2) + qd = rng.uniform(-2, 2, 2) + qdd = rng.uniform(-2, 2, 2) + truth = robot.rne_python(q, qd, qdd) + result = RobotBase.rne(robot, q, qd, qdd) + nt.assert_array_almost_equal(result, truth, decimal=6) + + +def _spong_two_link_tau( + q1, q2, q1_dot, q2_dot, q1_ddot, q2_ddot, + m1, m2, l1, l2, lc1, lc2, I1, I2, g=9.81, +): + """Analytical inverse dynamics for a planar 2R elbow manipulator. + + Equation (7.87) of Spong, Hutchinson, Vidyasagar, "Robot Modeling and + Control", Wiley 2006 -- an independent, closed-form ground truth. Shares + no code with rne_python()/rne()/Robot.rne(), unlike every other check in + this file, which only ever compares those implementations against each + other and so cannot catch a bug they all share. + """ + h = -m2 * l1 * lc2 * math.sin(q2) + d11 = m1 * (lc1**2) + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * math.cos(q2)) + I1 + I2 + d12 = m2 * (lc2**2 + l1 * lc2 * math.cos(q2)) + I2 + d21 = d12 + d22 = m2 * (lc2**2) + I2 + g1 = (m1 * lc1 + m2 * l1) * g * math.cos(q1) + m2 * lc2 * g * math.cos(q1 + q2) + g2 = m2 * lc2 * g * math.cos(q1 + q2) + c121 = h + c211 = h + c221 = h + c112 = -h + tau1 = ( + d11 * q1_ddot + d12 * q2_ddot + + c121 * q1_dot * q2_dot + c211 * q2_dot * q1_dot + c221 * (q2_dot**2) + + g1 + ) + tau2 = d21 * q1_ddot + d22 * q2_ddot + c112 * (q1_dot**2) + g2 + return tau1, tau2 + + +def _twolink_ground_truth(q, qd, qdd, I1=0.0, I2=0.0): + """TwoLink's expected joint torques, from the independent Spong + closed-form solution above, in TwoLink's own joint-angle/torque sign + convention. + + TwoLink's convention is the mirror image of Spong's (TwoLink's + ``base = SE3.Rx(pi/2)`` puts the arm in a different orientation than + Spong's own diagram) -- empirically calibrated 2026-07-21 against + rne_python() and confirmed exact to machine precision across several + q/qd/qdd combinations, not just the gravity-only case: + ``tau_rtb(q, qd, qdd) == -tau_spong(-q, -qd, -qdd)``. + """ + t1, t2 = _spong_two_link_tau( + -q[0], -q[1], -qd[0], -qd[1], -qdd[0], -qdd[1], + m1=1.0, m2=1.0, l1=1.0, l2=1.0, lc1=0.5, lc2=0.5, + I1=I1, I2=I2, g=9.8, + ) + return np.array([-t1, -t2]) + + +class TestTwoLinkAbsoluteGroundTruth(unittest.TestCase): + """Absolute correctness check, not just relative agreement between our + own implementations: TwoLink's torques against an independent + closed-form solution (Spong et al., Eq 7.87) that shares no code with + rne_python(), rne() (C), or Robot.rne(). Every other test in this file + only checks these implementations against each other or against + hand-derived reference numbers computed the same way rne_python() is -- + none of that can catch a bug all of them share. + """ + + def setUp(self): + self.std = _twolink() # mdh=False + self.mdh = TwoLink(mdh=True) + self.poses = [ + (np.array([0.3, 0.5]), np.zeros(2), np.zeros(2)), + (np.array([0.3, 0.9]), np.array([0.4, -0.6]), np.array([0.2, 1.1])), + (np.array([-1.1, 2.0]), np.array([-0.9, 1.3]), np.array([0.5, -0.7])), + ] + + def test_rne_python_std_dh_matches_spong(self): + for q, qd, qdd in self.poses: + truth = _twolink_ground_truth(q, qd, qdd) + nt.assert_array_almost_equal( + self.std.rne_python(q, qd, qdd), truth, decimal=6 + ) + + def test_rne_python_mdh_matches_spong(self): + for q, qd, qdd in self.poses: + truth = _twolink_ground_truth(q, qd, qdd) + nt.assert_array_almost_equal( + self.mdh.rne_python(q, qd, qdd), truth, decimal=6 + ) + + @unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) + def test_rne_c_std_dh_matches_spong(self): + for q, qd, qdd in self.poses: + truth = _twolink_ground_truth(q, qd, qdd) + nt.assert_array_almost_equal(self.std.rne(q, qd, qdd), truth, decimal=6) + + @unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) + def test_rne_c_mdh_matches_spong(self): + for q, qd, qdd in self.poses: + truth = _twolink_ground_truth(q, qd, qdd) + nt.assert_array_almost_equal(self.mdh.rne(q, qd, qdd), truth, decimal=6) + + def test_robot_rne_mdh_matches_spong(self): + """Robot.rne() (ETS/Featherstone) is only valid for mdh=True.""" + from roboticstoolbox.robot.Robot import Robot as RobotBase + + for q, qd, qdd in self.poses: + truth = _twolink_ground_truth(q, qd, qdd) + nt.assert_array_almost_equal( + RobotBase.rne(self.mdh, q, qd, qdd), truth, decimal=6 + ) + + +# --------------------------------------------------------------------------- +# Trajectory correctness, all 3 implementations, deliberately varying rows. +# +# frne() (C) now loops the whole trajectory internally in C++ rather than +# once per Python call (rne.md plan step 7) -- a real code path change with +# its own row-indexing arithmetic (i*nj offsets into flat buffers). A +# trajectory of *identical* rows (e.g. np.tile, as rne_compare.py's timing +# section uses) cannot distinguish "row i's output is correct" from "row i's +# output is actually some other row's result" -- every row looks right +# either way. These use a distinct, randomly-generated pose per row +# specifically to catch that class of bug. +# --------------------------------------------------------------------------- + +class TestRneTrajectoryVaryingRows(unittest.TestCase): + """C, rne_python, and (where structurally valid) Robot.rne must all + agree row-by-row on a trajectory of *distinct* poses, not just a single + pose or a tiled/repeated one.""" + + N = 25 + + def _random_trajectory(self, n, seed): + rng = np.random.default_rng(seed) + Q = rng.uniform(-np.pi, np.pi, (self.N, n)) + QD = rng.uniform(-2.0, 2.0, (self.N, n)) + QDD = rng.uniform(-2.0, 2.0, (self.N, n)) + return Q, QD, QDD + + @unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) + def test_c_matches_rne_python_panda_mdh_trajectory(self): + panda = rtb.models.DH.Panda() + Q, QD, QDD = self._random_trajectory(panda.n, seed=1) + + tau_c = panda.rne(Q, QD, QDD) + tau_py = panda.rne_python(Q, QD, QDD) + self.assertEqual(tau_c.shape, (self.N, panda.n)) + nt.assert_array_almost_equal(tau_c, tau_py, decimal=6) + + @unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) + def test_c_matches_rne_python_base_wrench_trajectory(self): + panda = rtb.models.DH.Panda() + Q, QD, QDD = self._random_trajectory(panda.n, seed=2) + + tau_c, wbase_c = panda.rne(Q, QD, QDD, base_wrench=True) + tau_py, wbase_py = panda.rne_python(Q, QD, QDD, base_wrench=True) + self.assertEqual(wbase_c.shape, (self.N, 6)) + nt.assert_array_almost_equal(tau_c, tau_py, decimal=6) + nt.assert_array_almost_equal(wbase_c, wbase_py, decimal=6) + + @unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) + def test_c_matches_rne_python_twolink_std_dh_trajectory(self): + robot = _twolink() + Q, QD, QDD = self._random_trajectory(robot.n, seed=3) + nt.assert_array_almost_equal( + robot.rne(Q, QD, QDD), robot.rne_python(Q, QD, QDD), decimal=6 + ) + + @unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) + def test_c_matches_rne_python_twolink_mdh_trajectory(self): + robot = TwoLink(mdh=True) + Q, QD, QDD = self._random_trajectory(robot.n, seed=4) + nt.assert_array_almost_equal( + robot.rne(Q, QD, QDD), robot.rne_python(Q, QD, QDD), decimal=6 + ) + + def test_robot_rne_matches_rne_python_panda_mdh_trajectory(self): + from roboticstoolbox.robot.Robot import Robot as RobotBase + + panda = rtb.models.DH.Panda() + Q, QD, QDD = self._random_trajectory(panda.n, seed=5) + + tau_base = RobotBase.rne(panda, Q, QD, QDD) + tau_py = panda.rne_python(Q, QD, QDD) + nt.assert_array_almost_equal(tau_base, tau_py, decimal=6) + + def test_robot_rne_matches_rne_python_twolink_mdh_trajectory(self): + from roboticstoolbox.robot.Robot import Robot as RobotBase + + robot = TwoLink(mdh=True) + Q, QD, QDD = self._random_trajectory(robot.n, seed=6) + + tau_base = RobotBase.rne(robot, Q, QD, QDD) + tau_py = robot.rne_python(Q, QD, QDD) + nt.assert_array_almost_equal(tau_base, tau_py, decimal=6) + + # --------------------------------------------------------------------------- # Path verification via timing # --------------------------------------------------------------------------- @@ -549,22 +1216,19 @@ def _assert_c_faster(self, t_c, t_py, factor=5): f" Python path: {t_py * 1000 / self.N:.3f} ms/call", ) - @unittest.skipIf(not __import__("roboticstoolbox.ets.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, - "C extension not built") + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_eval_c_faster_than_python(self): t_c = self._time_c("eval", self.q) t_py = self._time_python(_no_c_fkine, "eval", self.q) self._assert_c_faster(t_c, t_py) - @unittest.skipIf(not __import__("roboticstoolbox.ets.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, - "C extension not built") + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_jacob0_c_faster_than_python(self): t_c = self._time_c("jacob0", self.q) t_py = self._time_python(_no_c_jacob0, "jacob0", self.q) self._assert_c_faster(t_c, t_py) - @unittest.skipIf(not __import__("roboticstoolbox.ets.fknm", fromlist=["_C_AVAILABLE"])._C_AVAILABLE, - "C extension not built") + @unittest.skipUnless(_FKNM_C_AVAILABLE, _NO_FKNM_C) def test_jacobe_c_faster_than_python(self): t_c = self._time_c("jacobe", self.q) t_py = self._time_python(_no_c_jacobe, "jacobe", self.q)