perf: use CRBA + backslash for forward dynamics (34μs → 5.4μs)

Author: ferrolho

docs/src/man/benchmarks.md

@@ -8,33 +8,34 @@ This page compares the performance of ModernRoboticsBook.jl against [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 | 3.68 μs | — | 0.64 μs | ~6x slower |
-| Mass matrix | 22.2 μs | — | 0.65 μs | ~34x slower |
-| Gravity forces | 3.73 μs | — | 0.48 μs | ~8x slower |
-| Forward dynamics | 34.2 μs | — | 2.82 μs | ~12x slower |
+| Inverse dynamics | 3.81 μs | — | 0.64 μs | ~6x slower |
+| Mass matrix (CRBA) | 1.11 μs | **0.91 μs** | 0.65 μs | ~1.4x slower |
+| Gravity forces | 3.80 μs | — | 0.48 μs | ~8x slower |
+| Forward dynamics | 5.38 μs | — | 2.82 μs | ~2x slower |
 
 *Measured on Apple M2 (16 GB), Julia 1.12, Python 3.13. Julia timings are median values from BenchmarkTools.jl.*
 
 ## Analysis
 
 Forward kinematics and Jacobian computation are **~3x faster than Pinocchio** thanks to Julia's StaticArrays, which eliminate heap allocations for small fixed-size matrices (4×4, 6×6). The in-place variants (`forward_kinematics_space!`, `jacobian_space!`, etc.) achieve **zero allocations**.
 
-The remaining gap in dynamics functions comes from algorithmic differences:
+The remaining gap in dynamics functions comes from allocation overhead in the RNEA (inverse dynamics) implementation, which uses heap-allocated workspace arrays. The algorithmic gap has been largely closed:
 
 ### Algorithms
 
-- **Mass matrix**: ModernRoboticsBook.jl calls `inverse_dynamics` *n* times with unit accelerations (the textbook approach). Pinocchio uses the **Composite Rigid Body Algorithm (CRBA)**, which computes the full matrix in a single pass.
-- **Forward dynamics**: ModernRoboticsBook.jl explicitly forms and inverts the mass matrix. 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 and is within ~1.4x of Pinocchio.
+- **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.
+- **Inverse dynamics / gravity forces**: Uses the Recursive Newton-Euler Algorithm (RNEA), the same O(n) algorithm as Pinocchio. The ~6x gap is due to allocation overhead in the current implementation.
 
 ### Allocations
 
 | Function | Allocations | Memory | In-place |
 |----------|------------:|-------:|---------:|
 | Forward kinematics | 2 | 208 B | **0 (0 B)** |
 | Jacobian | 2 | 384 B | **0 (0 B)** |
-| Inverse dynamics | 186 | 13.4 KiB | — |
-| Mass matrix | 1,114 | 80.7 KiB | — |
-| Forward dynamics | 1,702 | 125.6 KiB | — |
+| Inverse dynamics | 188 | 13.6 KiB | — |
+| Mass matrix (CRBA) | 9 | 4.6 KiB | **0 (0 B)** |
+| Forward dynamics | 205 | 18.8 KiB | — |
 
 ### When does this matter?
 

src/ModernRoboticsBook.jl

@@ -1715,11 +1715,16 @@ Computes forward dynamics in the space frame for an open chain robot.
     \\ddot{\\theta} = M(\\theta)^{-1} \\left[ \\tau - c(\\theta,\\dot{\\theta}) - g(\\theta) - J^T(\\theta) \\mathcal{F}_{\\text{tip}} \\right]
     ```
 
-    where ``M`` is the [mass matrix](@ref mass_matrix), ``c`` captures
-    [velocity-dependent (Coriolis/centrifugal) forces](@ref velocity_quadratic_forces),
-    ``g`` captures [gravitational forces](@ref gravity_forces), and the last term
-    captures [end-effector contact forces](@ref end_effector_forces). Each of these
-    terms is computed via [`inverse_dynamics`](@ref) with appropriate inputs.
+    where ``M`` is the [mass matrix](@ref mass_matrix) (computed via CRBA), and the
+    right-hand side is computed via a single [`inverse_dynamics`](@ref) call with zero
+    accelerations that captures velocity-dependent, gravitational, and tip wrench forces.
+
+!!! details "Educational note"
+    The textbook computes forward dynamics by explicitly forming ``M^{-1}`` and calling
+    [`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.
 
 # Arguments
 - `joint_positions`: the ``n``-vector of joint variables.
@@ -1753,27 +1758,20 @@ function forward_dynamics(
     spatial_inertias::AbstractVector,
     screw_axes::AbstractMatrix,
 )
-    LA.inv(mass_matrix(joint_positions, link_frames, spatial_inertias, screw_axes)) * (
-        joint_torques - velocity_quadratic_forces(
+    # Use the explicit M⁻¹(τ - c - g - J^T F_tip) approach with CRBA for the mass matrix
+    mass = mass_matrix(joint_positions, link_frames, spatial_inertias, screw_axes)
+    tau_rhs =
+        joint_torques - inverse_dynamics(
             joint_positions,
             joint_velocities,
-            link_frames,
-            spatial_inertias,
-            screw_axes,
-        ) - gravity_forces(
-            joint_positions,
+            zeros(length(joint_positions)),
             gravity,
-            link_frames,
-            spatial_inertias,
-            screw_axes,
-        ) - end_effector_forces(
-            joint_positions,
             tip_wrench,
             link_frames,
             spatial_inertias,
             screw_axes,
         )
-    )
+    return mass \ tau_rhs
 end
 
 """