From 298fe80106c512a72033c6bec234d77a3969f0ea Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jul 2026 11:38:23 +1000 Subject: [PATCH 01/10] test(fknm): guarantee real C-vs-Python comparison, add CI build assertion Phase A of the pure-Python-wheel plan: harden the C/Python cross- validation tests so a missing compiled extension can never produce a false pass. Gates every test_matches_c_path-style test (and the whole TestRNEFallback/ TestEvalFallback classes) behind skipUnless(_C_AVAILABLE) -- previously these ran unconditionally, comparing the "C path" against a forced-Python path. When the extension isn't built (confirmed reproducible: this repo's own stale local dev env had neither _fknm_c nor _frne_c installed all session), the unpatched "C path" call silently dispatches to Python too, so the comparison degrades to Python-vs-itself and passes trivially, with no indication anything was skipped. Verified both regimes locally: with the extensions built, all 43 tests pass and genuinely exercise C; with them removed, the same 26 comparison tests show SKIPPED (not PASSED) and 17 remain passing. Adds a second, independent tier: TestFkineReference/TestJacob0Reference, hardcoded ground-truth values for Panda FK/jacob0 at a fixed pose, computed once against the (now-verified-correct) C extension. These run regardless of _C_AVAILABLE, mirroring the existing TestRNEReference pattern -- they're what actually proves a C-less build (e.g. the planned pyodide/wasm wheel) is correct on its own terms, not merely internally consistent. Adds a "Verify compiled extensions built" CI step (test-core and the full test matrix) that asserts _C_AVAILABLE for both fknm and frne right after install, so a silent build/import regression is a hard CI failure instead of something that only shows up as tests quietly skipping. Co-Authored-By: Claude Sonnet 5 --- .github/workflows/ci.yml | 26 ++++++++ tests/test_fknm_fallback.py | 118 ++++++++++++++++++++++++++++++++---- 2 files changed, 131 insertions(+), 13 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4f84c6c0..cf7a372c 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/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index 7a5ab6f9..148a56fa 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -1,13 +1,25 @@ """ 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. """ @@ -23,6 +35,9 @@ 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 + 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 +59,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 @@ -148,6 +166,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 +242,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 +273,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 +305,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 +339,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 +376,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 +513,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.""" @@ -549,22 +644,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) From 9a800e05e3d2431ccccc813cab80393be5308c65 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:17:49 +1000 Subject: [PATCH 02/10] fix(dynamics): rotate gravity by robot base in rne(), move handling into ne.c Robot.rne() (the ETS/Featherstone implementation) ignored self.base entirely for gravity -- fixed to rotate gravity into the root link frame via the base orientation before negating to the effective upward acceleration RNE expects, skipping the matmul for an identity base so symbolic gravity components don't pick up spurious "1.0*" literals. DHRobot.rne_python()'s rotated-base branch had a related bug: it computed vd = Rb @ gravity where every other call site (including this function's own identity-base branch) computes -gravity -- an exact sign flip. ne.c/frne_nb.cpp never handled base rotation at all -- 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 and applies it via the existing rot_trans_vect_mult C helper, so DHRobot.rne()/_copy_to_cpp() no longer need to know about this at all (init() no longer takes a gravity param). Also added base_wrench output to the C path (ne.c's backward recursion already computed f(0)/n(0), just never returned it) -- base_wrench=True no longer forces the slow Python fallback -- and fixed rne_python()'s base_wrench array being allocated with shape (nk, n) instead of (nk, 6) (a wrench is always 6-dimensional; happened to work by coincidence for 6-DOF Puma560, crashed for 2-DOF TwoLink). Standardized fext/tip-force terminology on "wrench applied to end-effector" throughout (f_tip/n_tip -> f_ee/n_ee in the C maths). New tests: TestRNERotatedBaseFallback/Reference (TwoLink vs C, vs hardcoded reference values verified against the Lagrangian identity) and TestBaseWrenchFallback/Reference (Puma560 + TwoLink, with and without an end-effector wrench). Co-authored-by: Claude Sonnet 5 --- src/roboticstoolbox/robot/DHRobot.py | 79 ++++++---- src/roboticstoolbox/robot/Robot.py | 22 ++- .../robot/cpp-extensions/frne.h | 2 +- .../robot/cpp-extensions/frne_nb.cpp | 56 +++++-- src/roboticstoolbox/robot/cpp-extensions/ne.c | 30 ++-- src/roboticstoolbox/robot/frne.py | 17 ++- tests/test_fknm_fallback.py | 138 ++++++++++++++++++ 7 files changed, 276 insertions(+), 68 deletions(-) diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index ab17e5ec..ead9f0e3 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -55,7 +55,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 +1364,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 +1388,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) @@ -1415,7 +1418,7 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): 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 ) @@ -1441,10 +1444,12 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): gravity = self.gravity else: gravity = getvector(gravity, 3) + gravity = np.ascontiguousarray(gravity, dtype=float) - # The c function doesn't handle base rotation, so we need to hack the - # gravity vector instead - gravity = self.base.R.T @ gravity + # 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) if fext is None: fext = np.zeros(6) @@ -1452,22 +1457,29 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): fext = getvector(fext, 6) tau = np.zeros((trajn, self.n)) + wbase = np.zeros((trajn, 6)) for i in range(trajn): - tau[i, :] = frne( - # we negate gravity here, since the C code has the sign wrong + tau[i, :], wbase[i, :] = frne( self._frne, q[i, :], qd[i, :], qdd[i, :], - -gravity, + gravity, + base_rot, fext, ) - 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 +1497,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 +1511,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() @@ -1568,7 +1581,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 +1620,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 ----------------- # diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 685c3d76..ce2ba81a 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 @@ -1668,9 +1670,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 diff --git a/src/roboticstoolbox/robot/cpp-extensions/frne.h b/src/roboticstoolbox/robot/cpp-extensions/frne.h index a8116c8f..8b9e9388 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 bb16803b..c102ea50 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 @@ -81,8 +82,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 +92,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; @@ -134,7 +132,9 @@ NB_MODULE(_frne_c, m) { 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; @@ -145,9 +145,25 @@ 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. + 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); @@ -171,7 +187,21 @@ NB_MODULE(_frne_c, m) { newton_euler(robot, tau.data(), qd_buf.data(), qdd_buf.data(), fext_buf.data(), 1); - return tau; + + // 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); + std::vector wbase = { + f_base.x, f_base.y, f_base.z, n_base.x, n_base.y, n_base.z, + }; + + return {tau, wbase}; }); // ── delete ──────────────────────────────────────────────────────────────── diff --git a/src/roboticstoolbox/robot/cpp-extensions/ne.c b/src/roboticstoolbox/robot/cpp-extensions/ne.c index 1f809f37..e565dc53 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 914b787a..2509385c 100644 --- a/src/roboticstoolbox/robot/frne.py +++ b/src/roboticstoolbox/robot/frne.py @@ -21,16 +21,23 @@ _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; caller must check that robot handle is not None. + + ``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)`` 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/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index 148a56fa..abd1c47f 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -37,6 +37,7 @@ 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 @@ -82,6 +83,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 # --------------------------------------------------------------------------- @@ -604,6 +612,136 @@ 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, + ) + + # --------------------------------------------------------------------------- # Path verification via timing # --------------------------------------------------------------------------- From 6b2889909fcd59b0d19a7f8d27adf881780b1fa7 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:23:23 +1000 Subject: [PATCH 03/10] fix(dynamics): fix three bugs in rne_python()'s modified-DH branch Found by term-by-term comparison against ne.c's MODIFIED branch, while exercising an mdh=True + rotated-base case (TwoLink) that had apparently never been exercised before: 1. Base rotation applied to gravity twice: once (correctly) before the recursion loop via vd = Rb @ vd, and again via a redundant `if j == 0: if base: Tj = base @ Tj; pstar = base @ pstar` block for the first link -- double-counted, not just wrong. Removed the block entirely; base rotation only needs applying once. 2. Missing parentheses in the MDH revolute case's linear acceleration formula: `Rt @ cross(wd, pstar) + cross(w, cross(w, pstar)) + vd` should distribute Rt over the whole sum, per ne.c's MODIFIED-DH branch (rot_trans_vect_mult applied to the full bracket) -- the prismatic case right below it already had this right. 3. The backward recursion's moment equation used pstar (the *next* link's offset) where it should use r (this link's own CoM offset) for the this-link-force-to-torque term -- ne.c's equivalent is R_COG(j) x F, not PSTAR(j+1) x F. With all three fixed, rne_python() agrees with the C extension for both DH conventions, not just standard DH. Also fixes TwoLink's mdh=True variant, which was copying the standard-DH a/alpha values directly onto RevoluteMDH links -- not how the DH<->MDH conversion works (it shifts a/alpha to the previous link index). Adds inertia=True support (real per-link inertia tensors via a tubular-link cylinder-inertia helper) so TwoLink can exercise non-trivial dynamics parameters, not just point masses. New tests: TestTwoLinkDHMDHEquivalence (TwoLink(mdh=False) and TwoLink(mdh=True) are the same physical robot -- fkine agreement, rne agreement between C and rne_python, and confirms rne_python's MDH branch now agrees with the standard-DH ground truth) and TestTwoLinkActuatorDynamics (C vs rne_python with non-zero Jm/G/B/Tc and real inertia tensors, for both DH conventions -- TwoLink is all-zero for these by default, so nothing before this exercised the actuator-dynamics terms through rne() at all). Co-authored-by: Claude Sonnet 5 --- src/roboticstoolbox/models/DH/TwoLink.py | 102 ++++++++++---- src/roboticstoolbox/robot/DHRobot.py | 28 +++- tests/test_fknm_fallback.py | 161 +++++++++++++++++++++++ 3 files changed, 256 insertions(+), 35 deletions(-) diff --git a/src/roboticstoolbox/models/DH/TwoLink.py b/src/roboticstoolbox/models/DH/TwoLink.py index 6daf6f48..0bbf24d7 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/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index ead9f0e3..77fc1c47 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -1645,10 +1645,14 @@ def removesmall(x): alpha = link.alpha if self.mdh: pstar = np.r_[link.a, -d * sym.sin(alpha), d * sym.cos(alpha)] - if j == 0: - if base: - Tj = base @ Tj - pstar = base @ pstar + # NOT baking base into Tj/pstar here (this block used to, + # before being removed): base rotation is already applied + # exactly once, to gravity/vd before this loop starts. + # Also folding it into Rm[0] here double-counted it -- + # confirmed by tracing TwoLink(mdh=True)'s gravity-only + # case, where vd ended up rotated by the base twice. + # ne.c matches this: it only ever rotates gravity by the + # base (in the nanobind glue), never any per-link R. else: pstar = np.r_[link.a, d * sym.sin(alpha), d * sym.cos(alpha)] @@ -1670,7 +1674,14 @@ def removesmall(x): # revolute axis w_ = Rt @ w + z0 * qd_k[j] wd_ = Rt @ wd + z0 * qdd_k[j] + _cross(Rt @ w, z0 * qd_k[j]) - vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd + # Rt must distribute over the whole bracket, not just + # the first term -- matches ne.c's MODIFIED-DH branch, + # which does rot_trans_vect_mult() (= Rt @ ...) on the + # full OMEGADOT(j-1)xPSTAR + OMEGA(j-1)x(OMEGA(j-1)xPSTAR) + # + ACC(j-1) sum. The prismatic case below already has + # this right; this revolute case was missing the + # parentheses (and therefore wrong for any MDH robot). + vd_ = Rt @ (_cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd) else: # prismatic axis w_ = Rt @ w @@ -1741,7 +1752,12 @@ def removesmall(x): nn_ = ( R @ nn + _cross(pstar, R @ f) - + _cross(pstar, Fm[:, j]) + # this link's own force acts through its own CoM + # offset r, not pstar (which is the offset to the + # *next* link's origin) -- matches ne.c's MODIFIED + # branch: vect_cross(&t2, R_COG(j), &F) uses R_COG(j) + # (this link's r), not PSTAR(j+1) + + _cross(r, Fm[:, j]) + Nm[:, j] ) f = f_ diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index abd1c47f..ddc12858 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -742,6 +742,167 @@ def test_twolink_with_ee_wrench(self): ) +# --------------------------------------------------------------------------- +# TwoLink(mdh=True) vs TwoLink(mdh=False): two different DH parameterizations +# of the *same physical robot* -- this is what actually caught the bug where +# TwoLink's mdh=True variant just copied the standard-DH `a`/`alpha` values +# onto RevoluteMDH links, which is not how the DH<->MDH conversion works (it +# shifts `a`/`alpha` to the previous link index). A single hand-checked pose +# (qn = [pi/6, -pi/6]) happened to agree by coincidence -- qn is symmetric +# (q2 = -q1) -- while every other pose diverged by up to ~0.7 in fkine. +# Random poses, not just qn, are the point of this test. +# +# This pairing also gives a genuinely independent cross-check of the DH/MDH +# convention finding (issue 6, rne.md). Originally: rne_python() trusted +# only for standard DH, Robot.rne() only for modified DH. Exercising +# rne_python() on this MDH+rotated-base pair (apparently never hit before) +# found and fixed three real bugs in its MDH branch (see +# test_mdh_rne_python_now_agrees) -- rne_python() is now correct for both +# conventions. Robot.rne() (the separate ETS/Featherstone implementation) is +# joint-last-compliant, and thus correct, for mdh=True DHRobot instances +# (test_robot_rne_on_mdh_variant_matches_standard_dh_rne_python); for +# mdh=False it now asserts rather than silently returning a wrong answer +# (test_robot_rne_rejects_standard_dh) -- see rne.md/tech-debt.md. +# --------------------------------------------------------------------------- + +class TestTwoLinkDHMDHEquivalence(unittest.TestCase): + """TwoLink(mdh=False) and TwoLink(mdh=True) are the same physical robot.""" + + def setUp(self): + self.std = _twolink() + self.mdh = TwoLink(mdh=True) + self.poses = [ + np.array([0.3, 0.5]), + np.array([1.2, -0.7]), + np.array([0.2, 0.3]), + ] + + def test_fkine_agrees(self): + for q in self.poses: + nt.assert_array_almost_equal( + self.std.fkine(q).A, self.mdh.fkine(q).A, decimal=10 + ) + + def test_fkine_agrees_random_poses(self): + rng = np.random.default_rng(0) + for _ in range(50): + q = rng.uniform(-np.pi, np.pi, 2) + nt.assert_array_almost_equal( + self.std.fkine(q).A, self.mdh.fkine(q).A, decimal=10 + ) + + def test_rne_agrees_between_parameterizations(self): + """rne() (the single ne.c implementation, dispatched per-robot via + its own dhtype: STANDARD vs MODIFIED) gives matching torques for + both parameterizations of the same physical robot -- evidence that + ne.c's two dhtype branches are mutually consistent, not that + there are two separate C implementations to compare.""" + z = np.zeros(2) + for q in self.poses: + tau_std_py = self.std.rne_python(q, z, z) + tau_std = self.std.rne(q, z, z) + tau_mdh = self.mdh.rne(q, z, z) + nt.assert_array_almost_equal(tau_std_py, tau_std, decimal=4) + nt.assert_array_almost_equal(tau_std, tau_mdh, decimal=4) + + def test_robot_rne_on_mdh_variant_matches_standard_dh_rne_python(self): + """The core rne.md finding, checked against a real matched pair + rather than the synthetic 1-link case: Robot.rne() (trusted for + MDH) applied to the MDH parameterization must agree with + rne_python() (trusted for standard DH) applied to the standard-DH + parameterization of the same physical robot.""" + from roboticstoolbox.robot.Robot import Robot as RobotBase + + z = np.zeros(2) + for q in self.poses: + tau_std_py = self.std.rne_python(q, z, z) + tau_mdh_base = RobotBase.rne(self.mdh, q, z, z) + nt.assert_array_almost_equal(tau_std_py, tau_mdh_base, decimal=4) + + def test_mdh_rne_python_now_agrees(self): + """rne_python() on the MDH parameterization -- exercising this + (mdh=True + a non-identity base) found three real bugs, none of + them issue 6: + + 1. Base rotation applied to gravity twice: once (correctly) + before the recursion loop, and again via Tj = base @ Tj for + the first link -- double-counted, not just wrong. + 2. Missing parentheses in the MDH revolute case's linear + acceleration formula: `Rt @ cross(wd, pstar) + cross(w, + cross(w, pstar)) + vd` should distribute Rt over the whole + sum, per ne.c's MODIFIED-DH branch (rot_trans_vect_mult + applied to the full bracket) -- the prismatic case right + below it already had this right. + 3. The backward recursion's moment equation used `pstar` (the + *next* link's offset) where it should use `r` (this link's + own CoM offset) for the this-link-force-to-torque term -- + ne.c's equivalent is `R_COG(j) x F`, not `PSTAR(j+1) x F`. + + With all three fixed, rne_python() is now correct for MDH too, + not just standard DH -- see test_robot_rne_rejects_standard_dh + for the one implementation (Robot.rne(), the ETS/Featherstone + one, unrelated code) that still can't handle standard DH (by + design, guarded, not silently wrong). + """ + z = np.zeros(2) + for q in self.poses: + truth = self.std.rne_python(q, z, z) + mdh_py = self.mdh.rne_python(q, z, z) + nt.assert_array_almost_equal(mdh_py, truth, decimal=4) + + +# --------------------------------------------------------------------------- +# Actuator dynamics (Jm, G, B, Tc) and non-zero link inertia: C vs +# rne_python() consistency on TwoLink, for both DH conventions. TwoLink is +# zero for all of these by default (per its own docstring: "Motor inertia +# is 0 ... Viscous and Coulomb friction is 0", link inertias also 0 unless +# inertia=True) -- so nothing before this exercised the actuator-dynamics +# terms (ne.c:485-491 / Link.friction()) or a non-zero inertia tensor +# through rne() at all. +# --------------------------------------------------------------------------- + +@unittest.skipUnless(_FRNE_C_AVAILABLE, _NO_FRNE_C) +class TestTwoLinkActuatorDynamics(unittest.TestCase): + """rne() (C) vs rne_python() with non-zero Jm/G/B/Tc and link inertia.""" + + def _with_actuator_dynamics(self, robot): + for i, link in enumerate(robot.links): + link.Jm = 0.05 * (i + 1) + link.G = 100.0 * (i + 1) + link.B = 0.01 * (i + 1) + link.Tc = [0.3 * (i + 1), -0.2 * (i + 1)] + return robot + + def setUp(self): + self.std = self._with_actuator_dynamics(TwoLink(mdh=False, inertia=True)) + self.mdh = self._with_actuator_dynamics(TwoLink(mdh=True, inertia=True)) + # non-zero qd/qdd: needed to actually exercise B/Tc (velocity- + # dependent) and Jm (acceleration-dependent) -- the gravity-only + # (qd=qdd=0) tests elsewhere in this file wouldn't touch them + self.q = np.array([0.3, 0.5]) + self.qd = np.array([0.4, -0.6]) + self.qdd = np.array([0.2, 0.7]) + + def test_std_dh_c_matches_python(self): + tau_c = self.std.rne(self.q, self.qd, self.qdd) + tau_py = self.std.rne_python(self.q, self.qd, self.qdd) + nt.assert_array_almost_equal(tau_c, tau_py, decimal=4) + + def test_mdh_c_matches_python(self): + tau_c = self.mdh.rne(self.q, self.qd, self.qdd) + tau_py = self.mdh.rne_python(self.q, self.qd, self.qdd) + nt.assert_array_almost_equal(tau_c, tau_py, decimal=4) + + def test_inertia_and_actuator_dynamics_actually_matter(self): + """Sanity check the comparisons above aren't vacuous: non-zero + inertia/Jm/G/B/Tc must actually change the result relative to the + all-zero-by-default TwoLink().""" + bare = TwoLink(mdh=False) + tau_bare = bare.rne(self.q, self.qd, self.qdd) + tau_with = self.std.rne(self.q, self.qd, self.qdd) + self.assertGreater(np.abs(tau_with - tau_bare).max(), 0.1) + + # --------------------------------------------------------------------------- # Path verification via timing # --------------------------------------------------------------------------- From 13e62a72680195369ba9c8ab88d7afb5f3bf1991 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:37:42 +1000 Subject: [PATCH 04/10] fix(dynamics): guard Robot.rne() against standard-DH DHRobot instances Root-caused why Robot.rne() (the separate ETS/Featherstone implementation) only ever worked for modified DH: its algorithm structurally requires the joint to be the last element of its own ETS segment (Featherstone's spatial-vector convention). This holds for Robot/ERobot/URDFRobot (built via ETS.split()), PoERobot (_update_ets() appends the joint last too), and -- checked carefully, since it wasn't obvious -- for DHRobot(mdh=True) too: DHLink._to_ets()'s MDH revolute branch reorders a nonzero d translation to precede the joint rotation specifically so the joint ET stays last; valid because a z-rotation and a z-translation about/along the same axis commute. It does not hold for DHRobot(mdh=False) (standard DH), where the joint comes before the link's fixed geometry -- structurally incompatible with the algorithm, not fixable without changing what Robot.rne() fundamentally assumes. Rather than teach Robot.rne() a second, DH-aware recursion, it now asserts on the incompatible case instead of silently returning a wrong answer: `assert getattr(self, "mdh", True)`. Checked via the mdh attribute rather than class identity -- an earlier class-name blocklist attempt, and a considered-and-rejected class-name allowlist, would each have gotten this wrong (the allowlist specifically would have rejected PoERobot, a fully compliant type). Design tradeoffs recorded in tech-debt.md, tied to the pending robot-class-hierarchy redesign since that may make the whole question moot. New diagnostic script examples/rne_dh_convention_check.py: a single-link revolute robot, checked against the Lagrangian-identity ground truth (independent of both RNE implementations), demonstrating rne_python() agrees for both DH conventions while Robot.rne() now cleanly rejects standard DH rather than silently mis-computing it. Co-authored-by: Claude Sonnet 5 --- examples/rne_dh_convention_check.py | 77 +++++++++++++++++++++++++++++ src/roboticstoolbox/robot/Robot.py | 41 +++++++++++++++ tech-debt.md | 50 +++++++++++++++++++ tests/test_fknm_fallback.py | 16 ++++++ 4 files changed, 184 insertions(+) create mode 100644 examples/rne_dh_convention_check.py diff --git a/examples/rne_dh_convention_check.py b/examples/rne_dh_convention_check.py new file mode 100644 index 00000000..e214a7de --- /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/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index ce2ba81a..07ede94f 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1617,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) diff --git a/tech-debt.md b/tech-debt.md index 3d5a37e8..8359f242 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) diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index ddc12858..d21e391a 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -850,6 +850,22 @@ def test_mdh_rne_python_now_agrees(self): 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 From 8434fc92ef542e201f707bae04d45363c3c61873 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:39:15 +1000 Subject: [PATCH 05/10] fix(dynamics): Robot.rne() dropped the rotational inertia tensor 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. Found while numerically verifying the previous commit's mdh-based guard (not just trusting the structural joint-last argument): a DHRobot(mdh=True) with nonzero d/alpha/mass and a full inertia tensor, compared against rne_python(), showed a genuine, non-trivial discrepancy (~0.005) that turned out to be entirely independent of d/alpha -- present even in the static (qd=qdd=0) case with G/Jm/B/Tc all zero, pinning it to the rigid- body inertia term rather than anything MDH-specific or actuator-related. Not caught earlier because the TwoLink-based Robot.rne()-vs-rne_python() 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(). New test: TestRobotRneInertiaTensor, using a synthetic MDH robot with nonzero d, alpha, mass, and a full asymmetric inertia tensor, checked against rne_python() at static, single, and 20 random poses. Co-authored-by: Claude Sonnet 5 --- src/roboticstoolbox/robot/Robot.py | 2 +- tests/test_fknm_fallback.py | 63 ++++++++++++++++++++++++++++++ 2 files changed, 64 insertions(+), 1 deletion(-) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 07ede94f..f2c26e3e 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1703,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] diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index d21e391a..a2f114f4 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -919,6 +919,69 @@ def test_inertia_and_actuator_dynamics_actually_matter(self): 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) + + # --------------------------------------------------------------------------- # Path verification via timing # --------------------------------------------------------------------------- From 1d55edfc7d3341c050600ef085f384906d755e1e Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:40:44 +1000 Subject: [PATCH 06/10] feat(dynamics): add armature inertia and friction to Robot.rne() Robot.rne() (the ETS/Featherstone implementation) modeled pure rigid-body dynamics only -- it never added armature inertia (G, Jm) or friction (B, Tc), unlike DHRobot.rne_python()/rne() (C), which both add G^2*Jm*qdd and subtract link.friction() in their backward recursion. Added the same term, so all three implementations now agree on robots with non-trivial actuator dynamics, not just rigid-body-only ones. No dedicated new test: exercised indirectly by every existing Robot.rne() comparison test on models with default (zero) G/Jm/B/Tc, where the added term is a no-op (confirmed: full suite unchanged) -- direct coverage with non-zero actuator parameters comes from examples/rne_speed.py's correctness check on rtb.models.DH.Panda(), which has real dynamics. Co-authored-by: Claude Sonnet 5 --- src/roboticstoolbox/robot/Robot.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index f2c26e3e..9e1ab7e4 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1802,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) From 11a461c41b45224edc92819d2fb5962033621f3b Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:45:56 +1000 Subject: [PATCH 07/10] test(dynamics): add absolute ground-truth check against Spong's closed-form dynamics Everything checking RNE correctness so far in this branch is relative: implementation A compared against implementation B, or against hardcoded reference numbers computed the same way one of the implementations works. None of that can catch a bug all three implementations share. Adds TestTwoLinkAbsoluteGroundTruth: TwoLink's torques checked against an independent closed-form solution for a planar 2R elbow manipulator (Spong, Hutchinson, Vidyasagar, "Robot Modeling and Control", Eq 7.87) -- typed in from the textbook, sharing no code with rne_python()/rne()/Robot.rne(). TwoLink's joint-angle/torque sign convention turns out to be the mirror image of Spong's own diagram (tau_rtb(q, qd, qdd) == -tau_spong(-q, -qd, -qdd)) -- empirically calibrated and confirmed exact to machine precision across several q/qd/qdd combinations, not just the gravity-only case. Covers rne_python() (both DH conventions), rne() (C, both conventions), and Robot.rne() (mdh=True only, per the guard added earlier). Co-authored-by: Claude Sonnet 5 --- tests/test_fknm_fallback.py | 109 ++++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index a2f114f4..314e14b7 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -24,6 +24,7 @@ Robot used: Franka Panda (7-DOF) for ETS functions; Puma560 (6-DOF DH) for rne. """ +import math import os import sys import timeit @@ -982,6 +983,114 @@ def test_robot_rne_matches_rne_python_static_and_random_poses(self): 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 + ) + + # --------------------------------------------------------------------------- # Path verification via timing # --------------------------------------------------------------------------- From c9db41370450e3ad2ba881d77c2a0620ab8a6838 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:48:23 +1000 Subject: [PATCH 08/10] fix(dynamics): auto-detect symbolic models, fix rne_python()'s per-call dtype selection Two related gaps in symbolic-model handling (rne.md issues 3-4): 1. self.symbolic was purely a construction-time flag the caller had to remember to pass (symbolic=True) -- forgetting it left self.symbolic False even when the link parameters genuinely contained SymPy content, which broke rne_python()'s own float64 array allocation (it's supposed to be the always-works fallback). Fixed: BaseRobot.__init__ now scans each link's DH/dynamics parameters (a, alpha, theta, d, m, r, I, Jm, G, B, Tc) for non-numeric content in its existing per-link geometry-flag loop, and ORs the result into self._symbolic. symbolic= is kept as an override, not the source of truth. 2. rne_python() only checked self.symbolic (model-level) to pick its internal dtype, not whether *this call's* Q/QD/QDD were symbolic -- a numeric model called with symbolic q (e.g. differentiating tau symbolically w.r.t. q for a concrete, numeric-mass robot) still crashed with float64 allocation. Fixed via a local symbolic_call variable (self.symbolic or any of Q/QD/QDD symbolic), used for both the dtype choice and the Coulomb-friction guard. Both verified directly: a DHRobot built with a symbolic `a` but no symbolic=True now correctly reports robot.symbolic == True, and a symbolic-q call against TwoLink() (a fully numeric model) now returns a symbolic expression instead of crashing. Co-authored-by: Claude Sonnet 5 --- src/roboticstoolbox/robot/BaseRobot.py | 33 ++++++++++++++++++++++++++ src/roboticstoolbox/robot/DHRobot.py | 17 +++++++++++-- 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 1d9673bb..5004e82c 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) diff --git a/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index 77fc1c47..3c106599 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 @@ -1542,7 +1543,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 @@ -1802,7 +1815,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( From c2bd405a7b07b2118a6f6cacb70055d7ff61a8cc Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:50:59 +1000 Subject: [PATCH 09/10] perf(dynamics): add symbolic-aware C dispatch check, safety net, batch trajectories into one C call Three closely-related changes to DHRobot.rne()'s dispatch logic, all touching the same method body: - Symbolic-aware dispatch: routes to rne_python() if self.symbolic or any of q/qd/qdd is symbolic, mirroring fknm._is_symbolic's idiom, instead of only discovering the C path can't handle it via a crash inside _copy_to_cpp()/frne() (rne.md issues 1/2). - try/except safety net around the C dispatch path: falls back to rne_python() on (TypeError, ValueError) so any *other* unanticipated incompatibility with the C extension's argument marshalling degrades gracefully instead of propagating a raw exception. - Batch trajectories into a single C call (rne.md issue 5/plan step 7): 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) instead of DHRobot.rne() looping in Python and calling frne() once per row. 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.6ms total (~0.6us/row) vs rne_python() ~289ms (~474x) and Robot.rne() ~446ms (~731x). New tests: TestRneTrajectoryVaryingRows uses deliberately distinct (not tiled/repeated) rows per trajectory -- 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 the C-side change introduces new row-indexing arithmetic (i*nj offsets into flat buffers). Co-authored-by: Claude Sonnet 5 --- examples/rne_compare.py | 107 +++++++++++ examples/rne_speed.py | 172 ++++++++++++++++++ src/roboticstoolbox/robot/DHRobot.py | 105 +++++++---- .../robot/cpp-extensions/frne_nb.cpp | 91 +++++---- src/roboticstoolbox/robot/frne.py | 19 +- tests/test_fknm_fallback.py | 85 +++++++++ 6 files changed, 501 insertions(+), 78 deletions(-) create mode 100644 examples/rne_compare.py create mode 100644 examples/rne_speed.py diff --git a/examples/rne_compare.py b/examples/rne_compare.py new file mode 100644 index 00000000..d6b865d7 --- /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_speed.py b/examples/rne_speed.py new file mode 100644 index 00000000..6d85c31b --- /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/src/roboticstoolbox/robot/DHRobot.py b/src/roboticstoolbox/robot/DHRobot.py index 3c106599..6ce3357f 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -1416,6 +1416,20 @@ 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() @@ -1424,52 +1438,67 @@ def rne(self, q, qd=None, qdd=None, gravity=None, fext=None, base_wrench=False): 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) - gravity = np.ascontiguousarray(gravity, dtype=float) - - # 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) - - 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)) - wbase = np.zeros((trajn, 6)) + # 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, :], wbase[i, :] = frne( + 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, :], + 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 base_wrench: if trajn == 1: diff --git a/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp b/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp index c102ea50..7cdb8ada 100644 --- a/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp +++ b/src/roboticstoolbox/robot/cpp-extensions/frne_nb.cpp @@ -21,6 +21,7 @@ extern "C" { #include "frne.h" // Robot, Link, Vect, Rot, DHType, newton_euler } +#include #include // ── Robot wrapper ───────────────────────────────────────────────────────────── @@ -126,6 +127,13 @@ 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, @@ -138,6 +146,7 @@ NB_MODULE(_frne_c, m) { { 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(); @@ -151,7 +160,8 @@ NB_MODULE(_frne_c, m) { // (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. + // 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]}; @@ -165,41 +175,56 @@ NB_MODULE(_frne_c, m) { 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.data(), qd_buf.data(), qdd_buf.data(), - fext_buf.data(), 1); - - // 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); - std::vector wbase = { - f_base.x, f_base.y, f_base.z, n_base.x, n_base.y, n_base.z, - }; + 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; + } return {tau, wbase}; }); diff --git a/src/roboticstoolbox/robot/frne.py b/src/roboticstoolbox/robot/frne.py index 2509385c..fdd16482 100644 --- a/src/roboticstoolbox/robot/frne.py +++ b/src/roboticstoolbox/robot/frne.py @@ -29,13 +29,18 @@ def init(njoints, mdh, L): def frne(robot, q, qd, qdd, gravity, base_rot, fext): - """Run Newton-Euler via C; caller must check that robot handle is not None. - - ``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)`` where ``wbase`` is the wrench - the base experiences, in the base/world frame. + """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) diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index 314e14b7..e7f3635f 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -1091,6 +1091,91 @@ def test_robot_rne_mdh_matches_spong(self): ) +# --------------------------------------------------------------------------- +# 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 # --------------------------------------------------------------------------- From c00af8ccc05f49a0c2a1b9257b58665424112dbd Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jul 2026 16:53:31 +1000 Subject: [PATCH 10/10] docs(dynamics): housekeeping and write up the full investigation in rne.md - Fixed rne_python()'s docstring calling itself rne_dh throughout (stale name from before a rename) and deleted a dead commented-out symbolic- simplification block at the end of the function. - Fixed BaseRobot.gravity's docstring: a robot.name -> robot.gravity copy-paste typo, and removed a factually wrong note ("if the z-axis is upward... this should be a positive number") that contradicted the actual [0,0,-9.81] convention -- standardized wording to match the gravity-parameter docstrings fixed elsewhere this branch. - Added a module docstring to Dynamics.py explaining the DynamicsMixin / RobotProto architecture: rne() is a per-class primitive (differently implemented by Robot vs DHRobot), and all derived dynamics quantities (accel, gravload, coriolis, etc.) are generic, built purely on top of primitives -- context for why Robot.rne()/DHRobot.rne_python() were deliberately kept as independent implementations rather than unified. - Fixed blocks/arm.py's gravity parameter: docstring claimed `float` but the actual runtime value is always a 3-vector passed straight through to Robot.rne()/gravload() -- fixed the type hint and 3 constructors that weren't calling getvector(gravity, 3) on it. - tech-debt.md: recorded two things noticed opportunistically while here but out of scope to fix now -- blocks/ has essentially no type hints, and examples/ik_speed.py doesn't run (stale `import fknm`, predates this branch entirely). - rne.md: the full investigation log for this branch, written up as it went -- root causes, fixes, and the plan for each of the issues fixed in the preceding commits. Co-authored-by: Claude Sonnet 5 --- rne.md | 322 +++++++++++++++++++++++++ src/roboticstoolbox/blocks/arm.py | 24 +- src/roboticstoolbox/robot/BaseRobot.py | 14 +- src/roboticstoolbox/robot/DHRobot.py | 15 -- src/roboticstoolbox/robot/Dynamics.py | 55 +++-- tech-debt.md | 52 ++++ 6 files changed, 433 insertions(+), 49 deletions(-) create mode 100644 rne.md diff --git a/rne.md b/rne.md new file mode 100644 index 00000000..45de34cd --- /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 9963b7d3..a57061e3 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/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 5004e82c..ae7d5024 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -843,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 6ce3357f..d9f7a633 100644 --- a/src/roboticstoolbox/robot/DHRobot.py +++ b/src/roboticstoolbox/robot/DHRobot.py @@ -1860,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 5d77422a..16872e0d 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/tech-debt.md b/tech-debt.md index 8359f242..ff30ae71 100644 --- a/tech-debt.md +++ b/tech-debt.md @@ -1275,3 +1275,55 @@ 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.