# Algorithms Guide
featherstone implements three core algorithms from Featherstone's "Rigid Body Dynamics Algorithms" (2008). All operate on the `ArticulatedBody` kinematic tree.
## ABA — Articulated Body Algorithm (Forward Dynamics)
**Purpose:** Given positions `q`, velocities `qd`, and applied torques `tau`, compute accelerations `qdd`.
**Complexity:** O(n) — one pass per body, three passes total.
**When to use:** Every simulation timestep. This is the main dynamics computation.
```rust
aba_forward_dynamics(&mut body);
// body.qdd now contains the accelerations
```
### How it works
The equations of motion are `H(q) * qdd + C(q, qd) = tau`. ABA solves for `qdd` without forming H (which would be O(n^3) to invert).
**Pass 1 (base to tip):** Compute each body's velocity and bias forces (Coriolis + centrifugal).
**Pass 2 (tip to base):** Compute *articulated body inertias* — the apparent inertia of each body plus all its descendants, accounting for joint freedom. This is the key insight: each joint "absorbs" some inertia, so the parent sees less than the full mass.
**Pass 3 (base to tip):** Compute accelerations using the articulated inertias and forces.
### Gravity convention
Gravity is modeled as a fictitious upward acceleration of the base (Featherstone convention):
```rust
body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
// Internally stored as a_0 = (0, +9.81, 0) — base accelerates upward
```
This avoids applying gravity as an external force to every body.
## RNEA — Recursive Newton-Euler (Inverse Dynamics)
**Purpose:** Given `q`, `qd`, and desired `qdd`, compute the required torques `tau`.
**Complexity:** O(n).
**When to use:** Control (computed torque, gravity compensation), not simulation.
```rust
let (tau, _data) = rnea_inverse_dynamics(&body);
```
### Derived computations
```rust
// Gravity compensation: torques to hold current pose
let tau_grav = gravity_compensation(&body);
// Bias forces: Coriolis + centrifugal + gravity
let c = bias_forces_rnea(&body);
// Pure Coriolis (no gravity)
let c_vel = coriolis_forces(&body);
```
## CRBA — Composite Rigid Body Algorithm (Mass Matrix)
**Purpose:** Compute the joint-space mass matrix `H(q)` explicitly.
**Complexity:** O(n^2) to build, O(n^3) to invert.
**When to use:** Model-based control, eigenanalysis, operational-space methods. NOT for simulation (use ABA instead).
```rust
let h = crba_mass_matrix(&body);
// h is n x n, symmetric, positive-definite
```
### The equations of motion
```
H(q) * qdd + C(q, qd) = tau + J^T * f_contact
```
- `H`: mass matrix (from CRBA)
- `C`: bias forces (from RNEA with qdd=0)
- `J^T * f_contact`: contact forces projected into joint space
## Algorithm Comparison
| Input | q, qd, tau | q, qd, qdd | q |
| Output | qdd | tau | H(q) |
| Complexity | O(n) | O(n) | O(n^2) |
| Use | Simulation | Control | Analysis |
| Benchmark (6-DOF) | 1.5 us | 530 ns | 880 ns |
## Duality
ABA and RNEA are mathematical inverses:
```rust
// Apply torques → get accelerations → recover torques
body.tau = original_tau.clone();
aba_forward_dynamics(&mut body); // qdd from tau
let (recovered_tau, _) = rnea_inverse_dynamics(&body); // tau from qdd
// recovered_tau ≈ original_tau (to f32 precision)
```