feat: export textbook algorithm variants (mass_matrix_rnea, forward_dynamics_rnea)

Author: ferrolho

docs/src/lib/dynamics.md

@@ -10,8 +10,8 @@ Modules = [ModernRoboticsBook]
 Pages = ["ModernRoboticsBook.jl"]
 Filter = t -> nameof(t) in [
     :ad, :inverse_dynamics, :inverse_dynamics!, :mass_matrix, :mass_matrix!,
-    :velocity_quadratic_forces, :gravity_forces, :end_effector_forces,
-    :forward_dynamics, :euler_step,
-    :inverse_dynamics_trajectory, :forward_dynamics_trajectory,
+    :mass_matrix_rnea, :velocity_quadratic_forces, :gravity_forces,
+    :end_effector_forces, :forward_dynamics, :forward_dynamics_rnea,
+    :euler_step, :inverse_dynamics_trajectory, :forward_dynamics_trajectory,
 ]
 ```

docs/src/man/benchmarks.md

@@ -4,22 +4,16 @@ This page compares the performance of ModernRoboticsBook.jl against [Pinocchio](
 
 ## Results
 
-| Function | ModernRoboticsBook.jl | In-place (`!`) | Pinocchio 3.9 (C++) | vs Pinocchio |
+| Function | MRB.jl | In-place (`!`) | Pinocchio 3.9 (C++) | vs Pinocchio |
 |----------|----------------------:|---------------:|--------------------:|-------------:|
 | Forward kinematics | 0.31 μs | **0.30 μs** | 0.86 μs | **~3x faster** |
 | Jacobian | 0.39 μs | **0.36 μs** | 1.0 μs | **~3x faster** |
 | Inverse dynamics (RNEA) | 1.15 μs | **1.0 μs** | 0.64 μs | ~1.6x slower |
 | Mass matrix (CRBA) | 1.14 μs | **0.91 μs** | 0.65 μs | ~1.4x slower |
+| Mass matrix (`mass_matrix_rnea`) | 6.79 μs | — | — | — |
 | Gravity forces | 1.19 μs | — | 0.48 μs | ~2.5x slower |
 | Forward dynamics (CRBA + RNEA) | 2.71 μs | — | 2.82 μs | **~1x (on par)** |
-
-For reference, here are the timings of the textbook algorithms that were replaced by the optimized versions above:
-
-| Function (textbook algorithm) | ModernRoboticsBook.jl | Pinocchio 3.9 (C++) | vs Pinocchio |
-|-------------------------------|----------------------:|--------------------:|-------------:|
-| Inverse dynamics (RNEA, unoptimized) | 3.81 μs | 0.64 μs | ~6x slower |
-| Mass matrix (n × RNEA) | 22.2 μs | 0.65 μs | ~34x slower |
-| Forward dynamics (M⁻¹ × ...) | 34.2 μs | 2.82 μs | ~12x slower |
+| Forward dynamics (`forward_dynamics_rnea`) | 5.44 μs | — | — | — |
 
 *Measured on Apple M2 (16 GB), Julia 1.12, Python 3.13. Julia timings are median values from BenchmarkTools.jl.*
 
@@ -29,11 +23,13 @@ Forward kinematics and Jacobian computation are **~3x faster than Pinocchio** th
 
 Dynamics functions are now **within 1.4–2.5x of Pinocchio**, with forward dynamics on par. Both libraries use the same algorithms (RNEA, CRBA); the remaining gap is due to Pinocchio's hand-tuned C++/Eigen spatial algebra vs our general Julia `SMatrix` operations.
 
+The textbook algorithm variants (`mass_matrix_rnea`, `forward_dynamics_rnea`) are provided for educational purposes — they directly mirror the textbook equations but are slower than the optimized defaults.
+
 ### Algorithms
 
 - **Inverse dynamics**: Uses the **Recursive Newton-Euler Algorithm (RNEA)**, the same O(n) algorithm as Pinocchio. The in-place variant achieves zero allocations.
-- **Mass matrix**: Uses the **Composite Rigid Body Algorithm (CRBA)**, which computes the full matrix in a single backward pass over composite spatial inertias — the same algorithm as Pinocchio. The in-place variant achieves zero allocations.
-- **Forward dynamics**: Computes `M \ (τ - RNEA(q, dq, 0, g, F))` using CRBA for the mass matrix and a single RNEA call for the bias forces. Pinocchio uses the **Articulated Body Algorithm (ABA)**, which solves for joint accelerations in O(n) without forming the mass matrix.
+- **Mass matrix**: Uses the **Composite Rigid Body Algorithm (CRBA)**, which computes the full matrix in a single backward pass over composite spatial inertias — the same algorithm as Pinocchio. The in-place variant achieves zero allocations. The textbook variant `mass_matrix_rnea` calls `inverse_dynamics` n times with unit accelerations.
+- **Forward dynamics**: Computes `M \ (τ - RNEA(q, dq, 0, g, F))` using CRBA for the mass matrix and a single RNEA call for the bias forces. Pinocchio uses the **Articulated Body Algorithm (ABA)**, which solves for joint accelerations in O(n) without forming the mass matrix. The textbook variant `forward_dynamics_rnea` explicitly forms M⁻¹ and calls RNEA separately for each term.
 
 ### Allocations
 

src/ModernRoboticsBook.jl

@@ -40,10 +40,12 @@ export near_zero,
     inverse_dynamics!,
     mass_matrix,
     mass_matrix!,
+    mass_matrix_rnea,
     velocity_quadratic_forces,
     gravity_forces,
     end_effector_forces,
     forward_dynamics,
+    forward_dynamics_rnea,
     euler_step,
     inverse_dynamics_trajectory,
     forward_dynamics_trajectory,
@@ -1403,7 +1405,8 @@ Computes the mass matrix ``M(\\theta)`` of an open chain robot using the
     are needed for unit acceleration of joint ``i``?" Each call produces one column
     of ``M(\\theta)``. This builds intuition but requires ``n`` full Newton-Euler
     passes. CRBA computes the same result with one forward pass and one backward
-    pass, exploiting the symmetry of ``M``.
+    pass, exploiting the symmetry of ``M``. The textbook algorithm is preserved
+    See [`mass_matrix_rnea`](@ref) for this textbook algorithm.
 
 # Arguments
 - `joint_positions`: the ``n``-vector of joint variables.
@@ -1500,6 +1503,44 @@ function mass_matrix!(
     return M
 end
 
+"""
+    mass_matrix_rnea(joint_positions, link_frames, spatial_inertias, screw_axes)
+
+Computes the mass matrix using the textbook algorithm (Chapter 8.2): calls
+[`inverse_dynamics`](@ref) ``n`` times, each with a unit acceleration vector.
+This is slower than [`mass_matrix`](@ref) (which uses CRBA) but directly
+illustrates that column ``i`` of ``M(\\theta)`` is the torque needed for unit
+acceleration of joint ``i``.
+"""
+function mass_matrix_rnea(
+    joint_positions::AbstractVector,
+    link_frames::AbstractVector,
+    spatial_inertias::AbstractVector,
+    screw_axes::AbstractMatrix,
+)
+    n = length(joint_positions)
+    M = zeros(n, n)
+    ddq = zeros(n)
+    zero_dq = zeros(n)
+    zero_g = zeros(3)
+    zero_F = zeros(6)
+    for i = 1:n
+        ddq[i] = 1
+        M[:, i] = inverse_dynamics(
+            joint_positions,
+            zero_dq,
+            ddq,
+            zero_g,
+            zero_F,
+            link_frames,
+            spatial_inertias,
+            screw_axes,
+        )
+        ddq[i] = 0
+    end
+    return M
+end
+
 """
     velocity_quadratic_forces(joint_positions, joint_velocities, link_frames, spatial_inertias, screw_axes)
 
@@ -1765,7 +1806,7 @@ Computes forward dynamics in the space frame for an open chain robot.
     [`inverse_dynamics`](@ref) separately for Coriolis, gravity, and tip wrench terms.
     This implementation combines these into a single RNEA call and uses `M \\ b` (LU
     factorization) instead of explicit inversion, which is both faster and more
-    numerically stable.
+    numerically stable. See [`forward_dynamics_rnea`](@ref) for the textbook algorithm.
 
 # Arguments
 - `joint_positions`: the ``n``-vector of joint variables.
@@ -1815,6 +1856,48 @@ function forward_dynamics(
     return mass \ tau_rhs
 end
 
+"""
+    forward_dynamics_rnea(joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)
+
+Computes forward dynamics using the textbook algorithm (Chapter 8.3): explicitly
+forms ``M^{-1}`` and calls [`inverse_dynamics`](@ref) separately for Coriolis,
+gravity, and tip wrench terms. This is slower than [`forward_dynamics`](@ref)
+(which uses CRBA + single RNEA + backslash) but directly mirrors the textbook
+equation ``\\ddot{\\theta} = M^{-1}[\\tau - c - g - J^T F_{\\text{tip}}]``.
+"""
+function forward_dynamics_rnea(
+    joint_positions::AbstractVector,
+    joint_velocities::AbstractVector,
+    joint_torques::AbstractVector,
+    gravity::AbstractVector,
+    tip_wrench::AbstractVector,
+    link_frames::AbstractVector,
+    spatial_inertias::AbstractVector,
+    screw_axes::AbstractMatrix,
+)
+    LA.inv(mass_matrix(joint_positions, link_frames, spatial_inertias, screw_axes)) * (
+        joint_torques - velocity_quadratic_forces(
+            joint_positions,
+            joint_velocities,
+            link_frames,
+            spatial_inertias,
+            screw_axes,
+        ) - gravity_forces(
+            joint_positions,
+            gravity,
+            link_frames,
+            spatial_inertias,
+            screw_axes,
+        ) - end_effector_forces(
+            joint_positions,
+            tip_wrench,
+            link_frames,
+            spatial_inertias,
+            screw_axes,
+        )
+    )
+end
+
 """
     euler_step(joint_positions, joint_velocities, joint_accelerations, timestep)