feat(dynamics): add armature support, fix rne() calculation bugs, solid testing#560
Merged
Conversation
…tion 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 <noreply@anthropic.com>
…nto 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 <noreply@anthropic.com>
Found by term-by-term comparison against ne.c's MODIFIED branch, while exercising an mdh=True + rotated-base case (TwoLink) that had apparently never been exercised before: 1. Base rotation applied to gravity twice: once (correctly) before the recursion loop via vd = Rb @ vd, and again via a redundant `if j == 0: if base: Tj = base @ Tj; pstar = base @ pstar` block for the first link -- double-counted, not just wrong. Removed the block entirely; base rotation only needs applying once. 2. Missing parentheses in the MDH revolute case's linear acceleration formula: `Rt @ cross(wd, pstar) + cross(w, cross(w, pstar)) + vd` should distribute Rt over the whole sum, per ne.c's MODIFIED-DH branch (rot_trans_vect_mult applied to the full bracket) -- the prismatic case right below it already had this right. 3. The backward recursion's moment equation used pstar (the *next* link's offset) where it should use r (this link's own CoM offset) for the this-link-force-to-torque term -- ne.c's equivalent is R_COG(j) x F, not PSTAR(j+1) x F. With all three fixed, rne_python() agrees with the C extension for both DH conventions, not just standard DH. Also fixes TwoLink's mdh=True variant, which was copying the standard-DH a/alpha values directly onto RevoluteMDH links -- not how the DH<->MDH conversion works (it shifts a/alpha to the previous link index). Adds inertia=True support (real per-link inertia tensors via a tubular-link cylinder-inertia helper) so TwoLink can exercise non-trivial dynamics parameters, not just point masses. New tests: TestTwoLinkDHMDHEquivalence (TwoLink(mdh=False) and TwoLink(mdh=True) are the same physical robot -- fkine agreement, rne agreement between C and rne_python, and confirms rne_python's MDH branch now agrees with the standard-DH ground truth) and TestTwoLinkActuatorDynamics (C vs rne_python with non-zero Jm/G/B/Tc and real inertia tensors, for both DH conventions -- TwoLink is all-zero for these by default, so nothing before this exercised the actuator-dynamics terms through rne() at all). Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>
Root-caused why Robot.rne() (the separate ETS/Featherstone implementation) only ever worked for modified DH: its algorithm structurally requires the joint to be the last element of its own ETS segment (Featherstone's spatial-vector convention). This holds for Robot/ERobot/URDFRobot (built via ETS.split()), PoERobot (_update_ets() appends the joint last too), and -- checked carefully, since it wasn't obvious -- for DHRobot(mdh=True) too: DHLink._to_ets()'s MDH revolute branch reorders a nonzero d translation to precede the joint rotation specifically so the joint ET stays last; valid because a z-rotation and a z-translation about/along the same axis commute. It does not hold for DHRobot(mdh=False) (standard DH), where the joint comes before the link's fixed geometry -- structurally incompatible with the algorithm, not fixable without changing what Robot.rne() fundamentally assumes. Rather than teach Robot.rne() a second, DH-aware recursion, it now asserts on the incompatible case instead of silently returning a wrong answer: `assert getattr(self, "mdh", True)`. Checked via the mdh attribute rather than class identity -- an earlier class-name blocklist attempt, and a considered-and-rejected class-name allowlist, would each have gotten this wrong (the allowlist specifically would have rejected PoERobot, a fully compliant type). Design tradeoffs recorded in tech-debt.md, tied to the pending robot-class-hierarchy redesign since that may make the whole question moot. New diagnostic script examples/rne_dh_convention_check.py: a single-link revolute robot, checked against the Lagrangian-identity ground truth (independent of both RNE implementations), demonstrating rne_python() agrees for both DH conventions while Robot.rne() now cleanly rejects standard DH rather than silently mis-computing it. Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
…d-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 <noreply@anthropic.com>
…ll 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 <noreply@anthropic.com>
…h 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 <noreply@anthropic.com>
…ne.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 <noreply@anthropic.com>
petercorke
changed the title
Codecov Report❌ Patch coverage is Additional details and impacted files@@ Coverage Diff @@
## main #560 +/- ##
=====================================
Coverage 0.00% 0.00%
=====================================
Files 142 142
Lines 13729 13788 +59
=====================================
- Misses 13729 13788 +59 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
We thank you in advance for your pull request, and for your interest and support of the Robotics Toolbox for Python.
You could help us a lot by:
futurebranch. This is where we fix issues and merge pull requests prior to pushing out a new release.