feat: wave 3 roadmap items (M1, M30, M8, M7, M29)

README.md

@@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g.
 
 ## Documentation
 
-| Category                                                     | Description                                                                                                                                                        |
-|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| [Kinematics](doc/kinematics/README.md)                       | Forward Kinematics, Inverse Kinematics (Damped Least Squares)                                                                                                      |
-| [Dynamics](doc/dynamics/README.md)                           | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration |
-| [Planning](doc/planning/README.md)                           | Primitive Distance Queries                                                                                                                                         |
-| [Trajectory](doc/trajectory/README.md)                       | Cubic Spline Trajectory                                                                                                                                            |
-| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation                                                                                                                               |
+| Category                                                     | Description                                                                                                                                                                                                  |
+|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [Kinematics](doc/kinematics/README.md)                       | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Inverse Kinematics (Damped Least Squares)                                                                      |
+| [Dynamics](doc/dynamics/README.md)                           | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration, Generic Joint Link |
+| [Planning](doc/planning/README.md)                           | Primitive Distance Queries                                                                                                                                                                                   |
+| [Trajectory](doc/trajectory/README.md)                       | Cubic Spline Trajectory                                                                                                                                                                                      |
+| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation                                                                                                                                                                         |
 
 Each category page lists its algorithms with a brief description and links to the detailed
 documentation.

ROADMAP.md

@@ -93,15 +93,17 @@ The library already has the core of manipulator **modelling**: forward/inverse d
 ([RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp), [ABA](robotics/dynamics/ArticulatedBodyAlgorithm.hpp),
 [Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), the joint-space mass matrix
 ([CRBA](robotics/dynamics/CompositeRigidBodyAlgorithm.hpp)), a fixed-step simulation step
-([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), position forward kinematics with base and
-tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). What is missing is
-the **full-pose kinematics, model plumbing, planning and control layer** that turns those models into a
-usable manipulator stack. Two current limitations gate many items below:
-
-> **Position-only, revolute-only.** [ForwardKinematics.hpp](robotics/kinematics/ForwardKinematics.hpp)
-> returns joint and tool *positions* (a 3×N Jacobian lives privately inside IK), and only
-> [RevoluteJointLink](robotics/dynamics/RevoluteJointLink.hpp) exists. Items **M1** (prismatic joints,
-> limits, armature), **M30** (full-pose kinematics) and **M8** (6×N Jacobian) lift these limits.
+([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), the manipulator-equation adapter
+([ChainDynamicsModel](robotics/dynamics/ChainDynamicsModel.hpp)), position forward kinematics with base and
+tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). Full-pose
+kinematics and the geometric Jacobian have shipped (see the note below); what is missing is the
+**planning and control layer** that turns those models into a usable manipulator stack.
+
+> **Full-pose kinematics shipped.** Links are generic ([JointLink](robotics/dynamics/JointLink.hpp):
+> revolute or prismatic, limits, armature); [ChainPoseKinematics](robotics/kinematics/ChainPoseKinematics.hpp)
+> and [DenavitHartenberg](robotics/kinematics/DenavitHartenberg.hpp) produce a frame chain, and
+> [GeometricJacobian](robotics/kinematics/GeometricJacobian.hpp) builds the 6×N Jacobian from it behind
+> the [JacobianProvider](robotics/kinematics/JacobianProvider.hpp) seam.
 
 **Conventions (normative for every item):** 6-vectors are ordered linear part first — twists `(v; ω)`,
 wrenches `(f; n)` — as defined by upstream `math::SE3Transform` (**NT53**); the geometric Jacobian
@@ -117,12 +119,7 @@ existing `dynamics/` convention.
 
 | #   | Component                                                       | Target module             | Depends on     | Difficulty |
 |-----|-----------------------------------------------------------------|---------------------------|----------------|------------|
-| M1  | Generic joint link (prismatic, limits, armature)                | `dynamics` + `kinematics` | —              | ★☆☆☆☆      |
 | M32 | Inverse dynamics with an external tool wrench                   | `dynamics`                | —              | ★☆☆☆☆      |
-| M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics)   | `dynamics`                | M28            | ★★☆☆☆      |
-| M30 | Chain pose kinematics (full-pose FK, frame chain)               | `kinematics`              | NT53           | ★★☆☆☆      |
-| M7  | Denavit-Hartenberg parameters (standard + modified)             | `kinematics`              | NT53, M30      | ★★☆☆☆      |
-| M8  | Geometric Jacobian (6×N), bias term, Jacobian provider          | `kinematics`              | NT53, M30      | ★★☆☆☆      |
 | M10 | Cartesian path + orientation (SLERP) interpolation              | `trajectory` (new)        | NT53, NT55     | ★★☆☆☆      |
 | M11 | Manipulability ellipsoid / Yoshikawa index                      | `kinematics`              | M8             | ★★☆☆☆      |
 | M14 | Redundancy resolution / null-space projection                   | `kinematics`              | M8, NT62       | ★★☆☆☆      |
@@ -149,26 +146,11 @@ retired here.
 
 ### Tier 1 — Trivial ★☆☆☆☆
 
-**M1. Generic joint link.** Evolve `RevoluteJointLink` (compatibly, via trailing defaulted fields) into a link with a joint type (revolute/prismatic), joint limits and armature, and add the prismatic branches to FK, RNEA, ABA, CRBA and the Jacobian.
-- *Algorithm / paper:* J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 and 6; Featherstone (2008), Ch. 4.
-
 **M32. Inverse dynamics with an external tool wrench.** RNEA overload taking the environment's wrench on the tool; satisfies `τ(w) − τ(0) = −Jᵀw`.
 - *Algorithm / paper:* Luh, Walker, Paul (1980).
 
 ### Tier 2 — Easy ★★☆☆☆
 
-**M29. Chain dynamics model.** Adapter implementing `EulerLagrangeDynamics` (M via CRBA, `Cq̇` and `g` via RNEA) and a one-call `InverseDynamicsModel` for the controllers.
-- *Reuses / builds on:* RNEA, M28; optional friction model from NT57.
-
-**M30. Chain pose kinematics.** Full tool pose and a model-independent frame chain (joint origins/axes in the base frame) for the link model.
-- *Algorithm / paper:* Siciliano et al. (2009), Ch. 2.
-
-**M7. Denavit-Hartenberg parameters.** Standard and modified `(a, α, d, θ)` descriptions with joint offsets, producing the M30 frame chain.
-- *Algorithm / paper:* Craig, *Introduction to Robotics*, Ch. 3.
-
-**M8. Geometric Jacobian (6×N).** Jacobian and `J̇q̇` from any frame chain, statics `τ = Jᵀw`, and the `JacobianProvider` seam used by IK and the task-space controllers; replaces IK's private 3×N Jacobian.
-- *Algorithm / paper:* Siciliano et al. (2009), Ch. 3; Lynch & Park (2017), Ch. 5.
-
 **M10. Cartesian path + orientation interpolation.** Straight-line position with SLERP orientation, one shared time law, twist feed-forward.
 - *Algorithm / paper:* Lynch & Park, Ch. 9; K. Shoemake, *SIGGRAPH* 1985.
 - *Reuses / builds on:* NT53 poses; any upstream motion profile (NT55, NT56, NT60) as the time law.

doc/dynamics/ArticulatedBodyAlgorithm.md

@@ -52,6 +52,11 @@ $$p^a_i = p^A_i + \hat{I}^A_i c_i + U_i \frac{u_i}{D_i}, \qquad p^A_{\lambda(i)}
 
 where $p^A_i$ starts as the rigid-body bias force $v_i \times^* I_i v_i$ (gyroscopic and centripetal terms).
 
+### Prismatic Joints and Armature
+
+With the motion subspace $S_i$ — $(\hat{z}_i; 0)$ for a revolute joint, $(0; \hat{z}_i)$ for a prismatic one — the projections are $U_i = I^A_i S_i$, $D_i = S_i^T U_i + J_{a,i}$ and $u_i = \tau_i - S_i^T p^A_i$. The velocity-product term is $c_i = v_i \times S_i \dot{q}_i$.
+Every transform to the parent uses the joint offset at the current displacement, and the armature $J_{a,i}$ (reflected rotor inertia) enters only through $D_i$.
+
 ## Complexity Analysis
 
 | Case | Time   | Space  | Notes                              |

doc/dynamics/ChainDynamicsModel.md

@@ -0,0 +1,101 @@
+# Chain Dynamics Model
+
+## Overview & Motivation
+
+Model-based manipulator controllers are written against the **manipulator equation**
+
+$$M(q)\,\ddot{q} + C(q, \dot{q})\,\dot{q} + g(q) = \tau$$
+
+Different controllers need different pieces of it:
+
+- PD control with gravity compensation needs only $g(q)$.
+- Operational-space control needs $M(q)$.
+- Computed-torque control needs the whole right-hand side for a commanded acceleration, ideally in a single pass.
+
+The recursive algorithms compute these pieces efficiently from a physical link description, but they speak in links, frames and spatial vectors rather than in matrices of the joint space.
+
+The chain dynamics model is the adapter between the two worlds. Given a link chain and the gravity vector, it answers every term of the manipulator equation through two small abstract interfaces. Controllers stay independent of how the dynamics are computed, can be tested against mock models, and can be bound to the concrete model on a hard real-time path.
+
+## Mathematical Theory
+
+### Terms from the Recursions
+
+With inverse dynamics written as $\mathrm{ID}(q, \dot{q}, \ddot{q}, a_g)$ (the recursive Newton–Euler algorithm, with gravity entering as a base acceleration $a_g = -g$):
+
+$$\begin{aligned}
+g(q) &= \mathrm{ID}(q, 0, 0, -g) \\
+C(q, \dot{q})\,\dot{q} &= \mathrm{ID}(q, \dot{q}, 0, 0) \\
+M(q)\,\ddot{q} + C\dot{q} + g &= \mathrm{ID}(q, \dot{q}, \ddot{q}, -g) \\
+M(q) &= \text{composite rigid body algorithm}
+\end{aligned}$$
+
+These identities hold because inverse dynamics is affine in $\ddot{q}$ and in the gravity vector, and quadratic in $\dot{q}$. Setting the other arguments to zero isolates each term. The mass matrix could also be assembled column by column, $M e_j = \mathrm{ID}(q, 0, e_j, 0)$, but the composite rigid body algorithm builds it directly at a lower cost.
+
+### Vector, Not Matrix
+
+The recursion returns the Coriolis and centrifugal **vector** $C(q, \dot{q})\dot{q}$, not the matrix $C(q, \dot{q})$. The vector is all that computed-torque control and simulation need. Passivity-based controllers and momentum observers need a particular factorization of the matrix itself: the Christoffel form, with $\dot{M} - 2C$ skew-symmetric. That form requires a modified recursion.
+
+### Consistency
+
+Because all four quantities come from the same link model, the identity $\mathrm{ID}(q, \dot{q}, \ddot{q}) = M\ddot{q} + C\dot{q} + g$ holds up to rounding. Forward dynamics by solving $M\ddot{q} = \tau - C\dot{q} - g$ reproduces the articulated-body result. Reflected actuator inertia (armature) appears consistently in $M$ and in the one-pass inverse dynamics.
+
+### Model Updates
+
+A changed payload or tool alters the last link's mass, centre of mass and inertia. Replacing the link description updates every term at once, since nothing is cached between calls.
+
+## Complexity Analysis
+
+| Term                        | Time     | Space    | Notes                                   |
+|-----------------------------|----------|----------|-----------------------------------------|
+| Gravity $g(q)$              | $O(n)$   | $O(n)$   | One inverse-dynamics pass               |
+| Coriolis vector $C\dot{q}$  | $O(n)$   | $O(n)$   | One inverse-dynamics pass               |
+| Inverse dynamics (one call) | $O(n)$   | $O(n)$   | One pass for $M\ddot{q} + C\dot{q} + g$ |
+| Mass matrix $M(q)$          | $O(n^2)$ | $O(n^2)$ | Composite rigid body algorithm          |
+
+Computing the three terms separately and combining them costs three passes plus an $O(n^2)$ product. The one-call inverse dynamics does it in a single $O(n)$ pass, which is why computed-torque control uses it.
+
+## Step-by-Step Walkthrough
+
+Two uniform rods rotate about horizontal parallel axes, with angles measured from the horizontal and gravity $9.81$ m/s² downwards. Rod 1 has $m_1 = 1.2$ kg and $l_1 = 0.6$ m; rod 2 has $m_2 = 0.7$ kg and $l_2 = 0.45$ m. The state is $q = (0.4, -0.9)$, $\dot{q} = (1.1, -0.6)$, $\ddot{q} = (0.8, -1.7)$.
+
+1. Gravity: $g = -9.81\,[\,m_1 \tfrac{l_1}{2}\cos q_1 + m_2(l_1 \cos q_1 + \tfrac{l_2}{2}\cos(q_1{+}q_2)),\ m_2 \tfrac{l_2}{2}\cos(q_1{+}q_2)\,]$.
+2. Coriolis: with $h = m_2 l_1 \tfrac{l_2}{2} \sin q_2$, $C\dot{q} = [-h(2\dot{q}_1\dot{q}_2 + \dot{q}_2^2),\ h\dot{q}_1^2]$.
+3. Mass matrix: $M_{11} = \tfrac{m_1 l_1^2}{3} + m_2(l_1^2 + \tfrac{l_2^2}{3} + l_1 l_2 \cos q_2)$, $M_{12} = m_2(\tfrac{l_2^2}{3} + \tfrac{l_1 l_2}{2}\cos q_2)$, $M_{22} = \tfrac{m_2 l_2^2}{3}$.
+4. Their sum at the given state is $\tau = (-8.2064, -1.4410)$ N·m, exactly the one-pass inverse dynamics.
+5. Doubling $m_2$ (a payload) changes only the $m_2$ terms of $g$, and the model reports the new holding torques immediately.
+
+## Pitfalls & Edge Cases
+
+- **Gravity sign**: gravity is the physical vector, e.g. $(0, 0, -9.81)$. The recursion internally applies it as a base acceleration of $+9.81$ upwards; passing $+g$ flips every holding torque.
+- **Coriolis matrix vs vector**: do not reconstruct $C(q, \dot{q})$ from the vector. Many matrices produce the same vector, and only the Christoffel one preserves the passivity property.
+- **Friction is not dynamics**: joint friction belongs in a separate model added to the torque command. Folding it into $C\dot{q}$ breaks the energy structure that controllers rely on.
+- **Virtual dispatch**: the abstract interfaces cost an indirect call per term. On hard real-time paths, bind the controller to the concrete model to devirtualize.
+- **Stale payloads**: the model is only as good as its link parameters. Update the last link whenever the gripper picks up a known load.
+
+## Variants & Generalizations
+
+- **Christoffel Coriolis matrix and regressor**: a modified recursion gives $C(q, \dot{q})\dot{q}_r$ with the skew-symmetric property and the inertial-parameter regressor $Y(q, \dot{q}, \dot{q}_r, \ddot{q}_r)$ for adaptive control.
+- **Friction and motor models**: Coulomb, viscous and Stribeck terms added beside the rigid-body model.
+- **Floating base**: the same interface over a floating-base chain yields the dynamics of legged and aerial manipulators.
+- **Identified models**: base-parameter identification replaces nominal link parameters with fitted ones while keeping the same interface.
+
+## Applications
+
+- PD control with gravity compensation (gravity term only).
+- Computed-torque and inverse-dynamics control (one-pass inverse dynamics).
+- Operational-space and impedance control with inertia shaping (mass matrix).
+- Forward-dynamics simulation through $M\ddot{q} = \tau - C\dot{q} - g$, and model-in-the-loop controller tests.
+
+## Connections to Other Algorithms
+
+- [Recursive Newton-Euler](RecursiveNewtonEuler.md): computes the gravity, Coriolis and one-pass inverse-dynamics terms.
+- [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md): computes the mass matrix.
+- [Euler-Lagrange](EulerLagrange.md): its forward and inverse dynamics run on any model that supplies $M$, $C\dot{q}$ and $g$, including this one.
+- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): forward dynamics without forming $M$; it must agree with solving the manipulator equation.
+- [PD Control with Gravity Compensation](../controllers/manipulator/PdGravityCompensation.md): consumes the gravity term.
+
+## References & Further Reading
+
+- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapters 5 and 6.
+- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 7.
+- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2020). *Robot Modeling and Control*. 2nd ed. Wiley. Chapters 7 and 9.

doc/dynamics/CompositeRigidBodyAlgorithm.md

@@ -49,6 +49,10 @@ $$f \leftarrow R_j f, \qquad n \leftarrow R_j n + r_j \times f$$
 
 projected on each ancestor axis gives $M_{ji} = M_{ij} = \hat{z}_j^T n$ for $j < i$. Each column costs one projection per ancestor, which gives the triangular operation count.
 
+### Prismatic Joints and Armature
+
+For a prismatic joint the unit motion is $(0; \hat{z}_i)$, so the column wrench is $I^c_i (0; \hat{z}_i)$ and each projection uses the force part. With the motion subspace $S_j$ both cases read $M_{ji} = S_j^T (n; f)$. Transforms use the joint offsets at the current displacements. Reflected actuator inertia $J_{a,i} = k_r^2 I_{\text{rotor}}$ adds to the diagonal entry $M_{ii}$ only.
+
 ### Properties
 
 - $M$ is symmetric, and positive definite whenever every composite body has non-zero inertia about its joint axis.
@@ -86,8 +90,6 @@ These are the textbook closed-form entries of the planar two-link arm.
 
 ## Variants & Generalizations
 
-- **Armature and rotor inertia**: add $k_r^2 I_{\text{rotor}}$ to each diagonal entry.
-- **Prismatic joints**: the unit motion is $(0; \hat{z}_i)$, so the column wrench becomes $I^c_i (0; \hat{z}_i)$ and the projection uses the force part.
 - **Floating base**: the base becomes a 6-DOF joint and $M$ gains a $6 \times 6$ block and coupling rows (Featherstone, Ch. 9).
 - **Branched trees**: the ancestor loop follows the parent array instead of $j - 1$. Sparsity can then be exploited in the factorization.
 - **Column-by-column RNEA**: $M e_j = \mathrm{ID}(q, 0, e_j)$ with gravity off. Simpler to derive and useful as a test oracle, but $n$ times the cost of one inverse-dynamics pass.