feat: rigid-body dynamics for a serial chain (WP-04)

DynamicChain computes joint torques by recursive Newton-Euler and the
joint-space mass matrix by the composite-rigid-body algorithm. Both are O(n)
and O(n^2) respectively, allocation-free, non-throwing, and callable from the
cyclic task.

This closes a claim the repository has been making since its first commit. The
README headline and the CMake DESCRIPTION have both said "kinematics, dynamics,
trajectory generation and calibration" while src/core/ contained no dynamics.
That was defensible while the work sat behind a deadline; without one it is a
project describing itself inaccurately.

Everything stays in the base frame -- angular velocity, angular acceleration,
centre-of-mass acceleration, forces, moments and inertia tensors. The classical
formulation rotates each link's quantities into that link's own frame and saves
arithmetic doing it. The cost of that saving is that no intermediate value can
be read without first reconstructing which frame it lives in, and debugging
dynamics is mostly reading intermediate values. Here omega[3] is the angular
velocity of link 3 in the frame the machine is bolted to, which is the frame the
CAD model and the operator already use. It costs two 3x3 products per link to
rotate each inertia tensor, and the benchmark says that is not worth optimising.

Inertias are described the way joints already are, following ADR-0008: mass,
centre of mass and inertia tensor in the base frame at the zero configuration.
That is what a CAD package reports for an assembly. The alternative, a per-link
body frame, needs the link frames agreed first -- exactly what ADR-0008 declined
to require.

Gravity enters by initialising the base acceleration to -gravity, not by adding
a weight term to each link. This is the equivalence principle rather than a
trick: a base accelerating upward at 9.81 m/s^2 is indistinguishable from inside
from a base at rest in a field. One line at the top of the recursion replaces n
lines spread through the loop, and removes n chances to get a sign or a frame
wrong in a way that produces plausible torques in configurations nobody tested.
setGravity takes any vector, so an arm on a wall or hanging from a ceiling is
not a special case, and a test asserts that reversing the field reverses every
torque -- mounting orientation is normally discovered on site.

The mass matrix is computed by an algorithm that shares no derivation with the
recursion, and that is deliberate. It could have come out of inverseDynamics --
one call per joint with a unit acceleration, same complexity, about twenty lines
-- and it would have been correct. It would also have left the mass matrix with
no independent check: the property tests would confirm symmetry, which is then
guaranteed by construction rather than by correctness. Two derivations that
could not have made the same mistake give a figure instead of a tolerance:

    CRBA vs RNEA, worst element      8.882e-16 kg m^2

about one unit in the last place. The gravity torque is checked from a third
direction again -- it is the gradient of the potential energy, and the potential
energy is a one-line sum over link heights sharing no code with the recursion.
Numerical differentiation reproduces it to 9.05e-09 N m, the truncation floor of
a central difference on a 1e-6 step. A one-link pendulum pins one case to
arithmetic somebody can check on paper.

No Coriolis matrix. inverseDynamics evaluates M qdd + C qd + g in one pass
without forming C, which is not unique anyway -- many matrices satisfy the
equation -- and costs more to build than the answer it helps compute. It appears
in textbooks because the matrix form is how the equation is analysed, not how it
is evaluated.

build() refuses an inertia tensor that is asymmetric, has a non-positive
principal moment, or violates the triangle inequality on its principal moments.
The third needs eigenvalues, so there is a closed-form symmetric 3x3 eigensolver
that exists for this alone. It earns that because an implausible inertia
produces plausible torques: nothing downstream fails, the arm merely needs
numbers no real machine would need, in poses nobody tested, and the error is
attributed to the controller. The trade is that a genuine point mass is also
refused -- the validator cannot tell a deliberate idealisation from a forgotten
field, and only one of those is common.

Not provided, and named rather than left to be discovered: forward dynamics,
which needs the mass matrix factorised and an integrator and belongs to a
simulator; joint friction; and motor rotor inertia, which reflected through a
high gear ratio is often comparable to the link inertia itself. Anyone using
this to predict actual motor torque needs to add it.

SerialChain gains linkTransforms(), exposed rather than duplicated inside
dynamics because the asymmetry it encodes -- a joint is carried by everything
upstream of it but not by itself -- is worth stating once rather than
rediscovering per caller.

Measured on the six-axis example: inverse dynamics 446 ns, gravity torque
429 ns, mass matrix 427 ns, each under 0.05% of a 1 kHz cycle and allocating
nothing. The O(n^2) mass matrix costs the same as the O(n) recursion at six
joints, because at that size the constant factors decide.

Two findings during verification. GCC at -O3 raised -Wnull-dereference on a
std::vector subscript in a test, unable to prove the buffer non-null from a
runtime joint count; fixed with fixed-size storage rather than a suppression,
since the array carries its size in the type and the question does not arise.
clang-tidy caught an unused <ranges> include -- std::ranges::all_of lives in
<algorithm>.

18 new tests; 157 total, green under GCC and Clang in Debug and Release,
ASan/UBSan and clang-tidy, 144 under TSan, with the documentation gate and the
installed-package consumer both extended to cover the new header.

Author: Onwcan

.github/workflows/ci.yml

@@ -165,6 +165,7 @@ jobs:
           cat > /tmp/consumer/main.cpp <<'EOT'
           #include <array>
           #include <cmath>
+          #include "motionkit/core/dynamics.hpp"
           #include "motionkit/core/frame_graph.hpp"
           #include "motionkit/core/kinematics.hpp"
           #include "motionkit/core/trajectory.hpp"
@@ -227,6 +228,21 @@ jobs:
             if (!stop || std::abs(stop.value.stoppingDistance() - 0.3) > 1e-9) {
               return 11;
             }
+            // The dynamics header. Holding the example arm against gravity must
+            // take a non-zero torque somewhere -- an all-zero result would mean
+            // the installed inertias were lost rather than exported.
+            const DynamicChain loaded = DynamicChain::sixAxisExample();
+            std::array<Scalar, 6> torque{};
+            if (loaded.gravityTorque(joints, torque) != DynamicsError::None) {
+              return 12;
+            }
+            Scalar effort = 0.0;
+            for (const Scalar t : torque) {
+              effort += std::abs(t);
+            }
+            if (!(effort > 1e-6)) {
+              return 13;
+            }
             return 0;
           }
           EOT

CHANGELOG.md

@@ -31,6 +31,17 @@ bump as "something may have moved".
 
 ### Added
 
+- **Rigid-body dynamics** (WP-04). `DynamicChain` computes joint torques by
+  recursive Newton-Euler and the joint-space mass matrix by the
+  composite-rigid-body algorithm, both allocation-free and callable from a
+  cyclic task. Gravity is a settable field vector, so an arm on a wall or a
+  ceiling is not a special case. Inertia tensors that could not belong to a
+  real body are refused at construction rather than producing plausible wrong
+  torques. ([ADR-0010](docs/adr/0010-dynamics-in-the-base-frame.md))
+- **`SerialChain::linkTransforms`** — where each link has been carried at a
+  given configuration. Added because dynamics needs it, and exposed rather than
+  duplicated because the asymmetry it encodes (a joint is carried by everything
+  upstream of it but not by itself) is worth stating once.
 - **Forward and inverse kinematics for serial chains** (WP-03). `SerialChain`
   describes revolute joints by an axis and a point rather than DH parameters,
   computes forward kinematics as a product of exponentials, and solves the

CMakeLists.txt

@@ -46,6 +46,7 @@ add_library(motionkit_core
   src/core/so3.cpp
   src/core/se3.cpp
   src/core/frame_graph.cpp
+  src/core/dynamics.cpp
   src/core/kinematics.cpp
   src/core/trajectory.cpp)
 add_library(motionkit::core ALIAS motionkit_core)

CONTRIBUTING.md

@@ -20,7 +20,7 @@ Two caveats worth knowing before you spend an afternoon on them:
 
 - **TSan excludes the allocation tests.** They replace global `operator new`,
   which is exactly what TSan's runtime also does. `MOTIONKIT_BUILD_ALLOCATION_TESTS`
-  is off under that preset, so the count is 128 rather than 139.
+  is off under that preset, so the count is 144 rather than 157.
 - **`ScurveProfile` is rest-to-rest.** Planning to a position from a non-zero
   velocity is WP-12 and not implemented. `StopProfile` does start from an
   arbitrary state, which is a different problem.

README.md

@@ -19,16 +19,16 @@ Eigen, no KDL, no Pinocchio — the algorithms are the point.
 | WP-01 | Build system, CI, static analysis, install/export | **Done** |
 | WP-02 | SE(3) transforms, frame graph, tool modeling | **Done** |
 | WP-03 | Forward and inverse kinematics (6R) | **Done** |
-| WP-04 | Rigid-body dynamics (RNEA, CRBA) | Planned |
+| WP-04 | Rigid-body dynamics (RNEA, CRBA) | **Done** |
 | WP-05 | Trajectory planning (jerk-limited S-curve, multi-axis synchronisation) | **Done** |
 | WP-11 | Stopping from an arbitrary state, and the safety envelope it defines | **Done** |
 | WP-06 | Hand-eye, TCP and base-frame calibration | Planned |
 | WP-12 | Blending and TOPP (needs a position target from a non-zero state) | Planned |
 | WP-12 | CUDA batch IK and collision checking | Planned |
 | WP-15 | API reference, architecture docs, contribution process | **Done** |
 
-139 tests, all passing under GCC and Clang in Debug and Release. ASan and UBSan
-exercise the full suite. TSan exercises the 128 ordinary tests; the eleven
+157 tests, all passing under GCC and Clang in Debug and Release. ASan and UBSan
+exercise the full suite. TSan exercises the 144 ordinary tests; the thirteen
 allocator-interposition tests run in a dedicated executable and are excluded
 from TSan because both the tests and the sanitizer runtime replace the global
 allocation functions.
@@ -47,7 +47,7 @@ ctest --preset debug
 ```
 
 Other presets: `release`, `asan`, `tsan`, `tidy`. The `tsan` preset intentionally
-runs 128 tests: the eleven tests that instrument global allocation are a
+runs 144 tests: the thirteen tests that instrument global allocation are a
 test-harness incompatibility with TSan, not an exemption for production code.
 
 Before pushing, run the formatter -- CI enforces it:
@@ -236,6 +236,37 @@ allocates nothing, which is 0.23 % of a 1 kHz cycle, so it can run in the loop
 that needs the answer rather than on a thread with a queue in front of it. See
 [ADR-0008](docs/adr/0008-kinematics-by-screws-and-a-damped-inverse.md).
 
+**The mass matrix is computed twice, by two algorithms that share no derivation.**
+It could have come out of the recursive Newton-Euler code already written — one
+call per joint with a unit acceleration — for about twenty lines. The
+composite-rigid-body algorithm was written separately anyway, propagating the
+momentum of each frozen distal subtree. The two share the link frames and
+nothing else, so each is a test of the other, and the agreement is not a
+tolerance anybody chose: **8.882e-16 kg·m²**, about one unit in the last place.
+A hand-written expected matrix would only have proved that the author and the
+implementation multiplied the same way.
+
+**Gravity enters as an acceleration of the base, not as a weight on each link.**
+A base accelerating upward at 9.81 m/s² is indistinguishable from inside from a
+base at rest in a gravitational field, so one line at the top of the recursion
+replaces a term in every link's force balance — and removes `n` chances to get a
+sign wrong in configurations nobody tested. It also makes an arm on a wall or a
+ceiling free rather than special: `setGravity` takes any vector, and a test
+asserts that reversing the field reverses every torque. The gravity torque is
+checked against a third derivation entirely — it is the gradient of the
+potential energy, which is a one-line sum over link heights sharing no code with
+the recursion, and the two agree to **9.05e-09 N·m**. See
+[ADR-0010](docs/adr/0010-dynamics-in-the-base-frame.md).
+
+**An inertia tensor that could not belong to a real body is refused.** Symmetric
+and positive definite is not enough: the principal moments must also satisfy the
+triangle inequality, because no distribution of mass makes one axis harder to
+spin than the other two together. Checking that needs the eigenvalues, so there
+is a closed-form symmetric 3×3 eigensolver that exists for this alone. It earns
+its place because an implausible inertia produces *plausible* torques — nothing
+downstream fails, the arm just needs numbers no real machine would, and the
+error gets blamed on the controller.
+
 **Multi-axis moves are driven by one path parameter, not one profile per axis.**
 Planning each axis separately and stretching the quick ones does synchronise the
 endpoints, and no axis exceeds a limit — and the path is still bent, because
@@ -305,8 +336,8 @@ test wrong.
 |---|---|
 | GCC + Clang × Debug + Release | `-Wconversion` and `-Wold-style-cast` fire on different constructs per compiler |
 | `-Werror` with `-Wconversion -Wsign-conversion -Wold-style-cast -Wshadow` | Silent narrowing in a pose pipeline is a field failure, not a warning |
-| ASan + UBSan on all 139 tests, `-fno-sanitize-recover=all` | A UBSan finding fails the build rather than printing a note |
-| TSan on the 128 ordinary tests | Ahead of the threaded executor in WP-08; the eleven allocator-interposition tests are excluded because TSan defines the same global allocation hooks |
+| ASan + UBSan on all 157 tests, `-fno-sanitize-recover=all` | A UBSan finding fails the build rather than printing a note |
+| TSan on the 144 ordinary tests | Ahead of the threaded executor in WP-08; the thirteen allocator-interposition tests are excluded because TSan defines the same global allocation hooks |
 | clang-tidy, `--warnings-as-errors=*` | Rule set and exclusions justified in ADR-0002 |
 | `scripts/format.sh --check` with clang-format 18 | Formatting is not a review topic, and CI runs the same check developers run |
 | **install with repository tests off + downstream consumer compile and run** | Exercises only the installed package contract; it caught a real bug on first run when the exported target was `motionkit::motionkit_core` but consumers used `motionkit::core` |
@@ -319,7 +350,7 @@ test wrong.
 | Document | What it answers |
 |---|---|
 | [docs/architecture.md](docs/architecture.md) | C4 context, container and component views, and the rules that decide where new code goes |
-| [docs/adr/](docs/adr/) | Nine decisions, each with the alternatives that lost and why |
+| [docs/adr/](docs/adr/) | Ten decisions, each with the alternatives that lost and why |
 | [CONTRIBUTING.md](CONTRIBUTING.md) | How to build, what the gates are, and the conventions clang-format cannot express |
 | [docs/review-checklist.md](docs/review-checklist.md) | The questions that have actually caught something here |
 | [CHANGELOG.md](CHANGELOG.md) | What changed, and what `0.x` promises |
@@ -354,7 +385,7 @@ Unit tests assert known values; the interesting ones assert **properties** over
 thousands of uniformly sampled rotations from a fixed seed — a property test you
 cannot replay is a flake, not a test.
 
-Eleven allocation tests are instrumentation rather than ordinary unit tests. They
+Thirteen allocation tests are instrumentation rather than ordinary unit tests. They
 run in their own executable because their global `operator new`/`operator delete`
 replacements affect an entire process. That target alone suppresses GNU's
 `-Wmismatched-new-delete` diagnostic: the `malloc`/`free` pairing is deliberate
@@ -398,6 +429,9 @@ no real-time scheduling:
 | `SerialChain::forward`, 6R | 175.7 | 176.6 | 527.7 | 0.018 % |
 | `SerialChain::jacobian`, 6R | 417.4 | 427.2 | 1029.7 | 0.043 % |
 | `SerialChain::inverse`, 6R seeded | 2209.1 | 2319.8 | 7499.1 | 0.232 % |
+| `DynamicChain::inverseDynamics`, 6R | 415.6 | 445.6 | 4967.7 | 0.045 % |
+| `DynamicChain::gravityTorque`, 6R | 400.3 | 428.6 | 1038.0 | 0.043 % |
+| `DynamicChain::massMatrix`, 6R CRBA | 399.8 | 427.0 | 1050.9 | 0.043 % |
 
 The maximum column is dominated by whatever else the machine was doing, and is
 reported anyway: a control loop is sized by its worst cycle, not its median.

benchmarks/bench_core.cpp

@@ -17,6 +17,7 @@
 #include <string>
 #include <vector>
 
+#include "motionkit/core/dynamics.hpp"
 #include "motionkit/core/frame_graph.hpp"
 #include "motionkit/core/kinematics.hpp"
 #include "motionkit/core/trajectory.hpp"
@@ -181,6 +182,27 @@ int main() {
            g_sink = static_cast<Scalar>(solved.value.iterations);
          }));
 
+  const DynamicChain dyn6 = DynamicChain::sixAxisExample();
+  const std::array<Scalar, 6> rate{0.5, 0.2, -0.4, 0.9, -0.1, 0.3};
+  const std::array<Scalar, 6> accel{-0.2, 1.1, 0.6, -0.7, 0.4, 0.8};
+  std::array<Scalar, 6> torque{};
+  std::array<Scalar, 36> mass{};
+
+  report("DynamicChain::inverseDynamics (6R)", measure([&](std::size_t) {
+           (void)dyn6.inverseDynamics(pose, rate, accel, torque);
+           g_sink = torque[0];
+         }));
+
+  report("DynamicChain::gravityTorque (6R)", measure([&](std::size_t) {
+           (void)dyn6.gravityTorque(pose, torque);
+           g_sink = torque[0];
+         }));
+
+  report("DynamicChain::massMatrix (6R, CRBA)", measure([&](std::size_t) {
+           (void)dyn6.massMatrix(pose, mass);
+           g_sink = mass[0];
+         }));
+
   std::printf(
       "\n  Timings come from an ordinary desktop with no isolated cores and no\n"
       "  real-time scheduling, so the maximum column is dominated by whatever\n"