featherstone 0.1.0

//! Composite Rigid Body Algorithm (CRBA) — Mass Matrix Computation.
//!
//! Computes the joint-space mass matrix **H(q)** that appears in the equations of motion:
//!
//! ```text
//! H(q) * qdd + C(q, qd) = tau + J^T * f_contact
//! ```
//!
//! **H** is an n x n symmetric positive-definite matrix where n = total velocity DOF.
//! It encodes how each joint's acceleration couples to every other joint through
//! the kinematic tree.
//!
//! # Algorithm (Featherstone Ch.6)
//!
//! 1. **Backward pass**: compute *composite rigid body inertias* — the spatial inertia
//!    of each body plus all its descendants, as if they were welded into one rigid body:
//!    `I_c[i] = I[i] + sum(X_child^T * I_c[child] * X_child)`
//!
//! 2. **Fill H**: for each joint pair (i, j) where j is an ancestor of i, compute:
//!    `H_ij = S_i^T * I_c[i] * S_j`
//!    by transforming force columns up the tree.
//!
//! # When to use CRBA vs ABA
//!
//! - **ABA** (forward dynamics): computes qdd directly in O(n), without forming H.
//!   Use for simulation stepping.
//! - **CRBA**: explicitly builds H in O(n^2). Use when you need the mass matrix itself —
//!   model-based control (computed torque), eigenfrequency analysis, or operational-space
//!   formulations.
//!
//! # Complexity
//!
//! O(n^2) to fill the matrix, O(n^3) if you then invert it. For real-time simulation,
//! prefer ABA. For control, CRBA + inverse is the standard approach.

use nalgebra::DMatrix;

use super::body::ArticulatedBody;
use super::spatial::{SpatialInertia, SpatialVector};

/// Compute the joint-space mass matrix H(q) using CRBA.
///
/// H is an n x n symmetric positive-definite matrix where n = total velocity DOF.
///
/// # Algorithm
/// 1. Backward pass: compute composite rigid body inertia I_c for each body
///    (sum of all descendant inertias transformed to this body's frame)
/// 2. For each movable joint pair (i, j), compute H_ij = S_i^T * I_c_i * S_j
pub fn crba_mass_matrix(body: &ArticulatedBody) -> DMatrix<f32> {
    let n = body.dof_count();
    let nb = body.body_count();

    if n == 0 {
        return DMatrix::zeros(0, 0);
    }

    let mut h = DMatrix::zeros(n, n);

    // Step 1: Compute composite inertias (backward pass)
    let mut i_c: Vec<SpatialInertia> = body.bodies.iter().map(|b| b.inertia.clone()).collect();

    // Get transforms for current configuration
    let x_up: Vec<_> = (0..nb)
        .map(|i| {
            let bd = &body.bodies[i];
            let x_j = bd.joint_type.transform(body.joint_q(i));
            x_j.compose(&bd.x_tree)
        })
        .collect();

    // Backward pass: accumulate composite inertias from children to parents
    for i in body.iter_tip_to_base() {
        let bd = &body.bodies[i];
        if bd.parent >= 0 {
            let parent = bd.parent as usize;
            // Transform this body's composite inertia to parent frame and add
            let x_inv = x_up[i].inverse();
            let i_c_parent = x_inv.apply_inertia(&i_c[i]);
            i_c[parent] = i_c[parent].add(&i_c_parent);
        }
    }

    // Step 2: Compute mass matrix entries
    // Get motion subspaces
    let s_all: Vec<Vec<SpatialVector>> = (0..nb)
        .map(|i| body.bodies[i].joint_type.motion_subspace(body.joint_q(i)))
        .collect();

    for i in 0..nb {
        let dof_i = body.bodies[i].joint_type.dof();
        if dof_i == 0 {
            continue;
        }
        let v_idx_i = body.bodies[i].v_index;

        // Diagonal block: H_ii = S_i^T * I_c_i * S_i
        for r in 0..dof_i {
            let ic_sr = i_c[i].mul_vector(&s_all[i][r]);
            for c in r..dof_i {
                let val = s_all[i][c].dot(&ic_sr);
                h[(v_idx_i + r, v_idx_i + c)] = val;
                h[(v_idx_i + c, v_idx_i + r)] = val; // Symmetric
            }
        }

        // Off-diagonal: walk up the tree from i to root
        // F = I_c_i * S_i, then transform F through each ancestor
        let mut f_cols: Vec<SpatialVector> = s_all[i]
            .iter()
            .map(|s| i_c[i].mul_vector(s))
            .collect();

        let mut j = i;
        while body.bodies[j].parent >= 0 {
            // Transform F to parent frame
            let x_inv = x_up[j].inverse();
            f_cols = f_cols.iter().map(|f| x_inv.apply_force(f)).collect();

            j = body.bodies[j].parent as usize;
            let dof_j = body.bodies[j].joint_type.dof();
            if dof_j == 0 {
                continue;
            }
            let v_idx_j = body.bodies[j].v_index;

            // H_ji = S_j^T * F
            for r in 0..dof_j {
                for c in 0..dof_i {
                    let val = s_all[j][r].dot(&f_cols[c]);
                    h[(v_idx_j + r, v_idx_i + c)] = val;
                    h[(v_idx_i + c, v_idx_j + r)] = val; // Symmetric
                }
            }
        }
    }

    h
}

/// Compute the bias force vector C(q, qd) = RNEA(q, qd, 0).
///
/// C contains Coriolis, centrifugal, and gravity terms.
/// The full equations of motion are: H*qdd + C = tau + J^T*f_contact
///
/// This is computed via inverse dynamics (RNEA) with zero accelerations.
pub fn bias_forces(body: &mut ArticulatedBody) -> nalgebra::DVector<f32> {
    // Save and clear accelerations and torques
    let saved_qdd = body.qdd.clone();
    let saved_tau = body.tau.clone();
    body.qdd.fill(0.0);
    body.tau.fill(0.0);

    // Use RNEA-style computation (inline to avoid circular dependency)
    let n = body.dof_count();
    let nb = body.body_count();
    let mut c = nalgebra::DVector::zeros(n);

    if nb == 0 {
        return c;
    }

    // Forward pass: compute velocities
    let x_up: Vec<_> = (0..nb)
        .map(|i| {
            let bd = &body.bodies[i];
            let x_j = bd.joint_type.transform(body.joint_q(i));
            x_j.compose(&bd.x_tree)
        })
        .collect();

    let s_all: Vec<Vec<SpatialVector>> = (0..nb)
        .map(|i| body.bodies[i].joint_type.motion_subspace(body.joint_q(i)))
        .collect();

    let mut v = vec![SpatialVector::zero(); nb];
    let mut a = vec![SpatialVector::zero(); nb];

    for i in body.iter_base_to_tip() {
        let bd = &body.bodies[i];
        let qd_slice = body.joint_qd(i);
        let v_j = compute_joint_velocity(&s_all[i], qd_slice);

        if bd.parent < 0 {
            v[i] = v_j.clone();
            // Base acceleration = gravity (Featherstone convention)
            a[i] = x_up[i].apply_motion(&body.gravity);
            // Add Coriolis
            a[i] = a[i].add(&v[i].cross_motion(&v_j));
        } else {
            let parent = bd.parent as usize;
            let v_parent = x_up[i].apply_motion(&v[parent]);
            v[i] = v_parent.add(&v_j);
            let a_parent = x_up[i].apply_motion(&a[parent]);
            a[i] = a_parent.add(&v[i].cross_motion(&v_j));
        }
    }

    // Backward pass: compute forces
    let mut f = vec![SpatialVector::zero(); nb];

    for i in body.iter_tip_to_base() {
        let bd = &body.bodies[i];
        let iv = bd.inertia.mul_vector(&v[i]);
        let own_force = bd.inertia.mul_vector(&a[i]).add(&v[i].cross_force(&iv)).sub(&body.f_ext[i]);
        f[i] = f[i].add(&own_force);

        // Project onto joint axes to get bias torques
        let dof = bd.joint_type.dof();
        if dof > 0 {
            let v_idx = bd.v_index;
            for j in 0..dof {
                c[v_idx + j] = s_all[i][j].dot(&f[i]);
            }
        }

        // Propagate force to parent
        if bd.parent >= 0 {
            let parent = bd.parent as usize;
            let x_inv = x_up[i].inverse();
            f[parent] = f[parent].add(&x_inv.apply_force(&f[i]));
        }
    }

    // Restore saved state
    body.qdd = saved_qdd;
    body.tau = saved_tau;

    c
}

/// Compute joint velocity: v_j = sum S_k * qd_k
fn compute_joint_velocity(s: &[SpatialVector], qd: &[f32]) -> SpatialVector {
    let mut result = SpatialVector::zero();
    for (k, s_col) in s.iter().enumerate() {
        if k < qd.len() {
            result = result.add(&s_col.scale(qd[k]));
        }
    }
    result
}

// ============================================================================
// Tests
// ============================================================================

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::body::GenJointType;
    use super::super::spatial::{SpatialInertia, SpatialTransform};
    use approx::assert_relative_eq;
    use nalgebra::{Matrix3, Vector3};

    fn make_inertia(mass: f32) -> SpatialInertia {
        SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01 * mass)
    }

    #[test]
    fn test_crba_empty() {
        let body = ArticulatedBody::new();
        let h = crba_mass_matrix(&body);
        assert_eq!(h.nrows(), 0);
        assert_eq!(h.ncols(), 0);
    }

    #[test]
    fn test_crba_single_revolute() {
        let mut body = ArticulatedBody::new();
        let i_zz = 0.1_f32;
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia_at_com(
                1.0,
                Matrix3::from_diagonal(&Vector3::new(0.01, 0.01, i_zz)),
            ),
            SpatialTransform::identity(),
        );

        let h = crba_mass_matrix(&body);
        assert_eq!(h.nrows(), 1);
        assert_eq!(h.ncols(), 1);
        assert_relative_eq!(h[(0, 0)], i_zz, epsilon = 1e-6);
    }

    #[test]
    fn test_crba_symmetry() {
        let mut body = ArticulatedBody::new();
        for i in 0..4 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"),
                parent,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0),
                SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
            );
        }
        body.set_joint_q(0, &[0.3]);
        body.set_joint_q(1, &[-0.5]);
        body.set_joint_q(2, &[0.7]);

        let h = crba_mass_matrix(&body);

        for i in 0..h.nrows() {
            for j in 0..h.ncols() {
                assert_relative_eq!(h[(i, j)], h[(j, i)], epsilon = 1e-6);
            }
        }
    }

    #[test]
    fn test_crba_positive_definite() {
        let mut body = ArticulatedBody::new();
        for i in 0..3 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"),
                parent,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0),
                SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
            );
        }

        let h = crba_mass_matrix(&body);

        let eigenvalues = h.symmetric_eigenvalues();
        for &ev in eigenvalues.iter() {
            assert!(ev > 0.0, "Mass matrix eigenvalue should be positive: {ev}");
        }
    }

    #[test]
    fn test_crba_dimensions() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "base",
            -1,
            GenJointType::Floating,
            make_inertia(5.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "arm1",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
        );
        body.add_body(
            "arm2",
            1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
        );

        let h = crba_mass_matrix(&body);
        assert_eq!(h.nrows(), 8); // 6 + 1 + 1
        assert_eq!(h.ncols(), 8);
    }

    #[test]
    fn test_crba_aba_consistency() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        for i in 0..3 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"),
                parent,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0),
                SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
            );
        }

        body.set_joint_q(0, &[0.3]);
        body.set_joint_q(1, &[-0.2]);
        body.set_joint_tau(0, &[0.5]);
        body.set_joint_tau(1, &[-0.3]);

        // Compute via CRBA: qdd = H^{-1} * (tau - C)
        let h = crba_mass_matrix(&body);
        let c = bias_forces(&mut body);
        let tau_minus_c = &body.tau - &c;
        let h_inv = h.clone().try_inverse().expect("H should be invertible");
        let qdd_crba = h_inv * tau_minus_c;

        // Compute via ABA
        super::super::aba::aba_forward_dynamics(&mut body);
        let qdd_aba = body.qdd.clone();

        for i in 0..body.dof_count() {
            assert_relative_eq!(qdd_crba[i], qdd_aba[i], epsilon = 0.1);
        }
    }

    #[test]
    fn test_bias_forces_zero_velocity() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        let c = bias_forces(&mut body);

        for i in 0..c.len() {
            assert_relative_eq!(c[i], 0.0, epsilon = 1e-8);
        }
    }

    #[test]
    fn test_crba_heavier_body_larger_diagonal() {
        // Intent: heavier body should produce larger mass matrix diagonal entry
        let make_body = |mass: f32| {
            let mut body = ArticulatedBody::new();
            body.add_body(
                "link", -1,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(mass),
                SpatialTransform::identity(),
            );
            body
        };

        let mut light = make_body(1.0);
        let mut heavy = make_body(10.0);
        let m_light = crba_mass_matrix(&mut light);
        let m_heavy = crba_mass_matrix(&mut heavy);

        assert!(
            m_heavy[(0, 0)] > m_light[(0, 0)],
            "Heavier body should have larger mass matrix diagonal: light={}, heavy={}",
            m_light[(0, 0)], m_heavy[(0, 0)]
        );
    }

    #[test]
    fn test_crba_multi_link_finite() {
        // Intent: CRBA should produce finite mass matrix for complex tree
        let mut body = ArticulatedBody::new();
        for i in 0..4 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"), parent,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(0.5),
                SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
            );
        }

        let m = crba_mass_matrix(&mut body);
        for i in 0..m.nrows() {
            for j in 0..m.ncols() {
                assert!(m[(i, j)].is_finite(), "M[{i},{j}] should be finite: {}", m[(i, j)]);
            }
        }
    }

    // ── SLAM Cycle 2: CRBA intent tests ─────────────────────────────

    #[test]
    fn intent_crba_mass_matrix_symmetric() {
        // Physics intent: mass matrix MUST be symmetric (H^T = H)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(2.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));
        body.add_body("l3", 1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(0.5), SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));

        let m = crba_mass_matrix(&mut body);
        for i in 0..m.nrows() {
            for j in i+1..m.ncols() {
                assert!(
                    (m[(i,j)] - m[(j,i)]).abs() < 1e-5,
                    "mass matrix not symmetric: M[{},{}]={} vs M[{},{}]={}",
                    i, j, m[(i,j)], j, i, m[(j,i)]
                );
            }
        }
    }

    #[test]
    fn intent_crba_mass_matrix_positive_definite() {
        // Physics intent: mass matrix MUST have all positive eigenvalues
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::x() },
            make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));

        let m = crba_mass_matrix(&mut body);

        // Check diagonal dominance (necessary for PD in this case)
        for i in 0..m.nrows() {
            assert!(m[(i,i)] > 0.0, "diagonal M[{i},{i}] should be positive: {}", m[(i,i)]);
        }
    }

    #[test]
    fn intent_crba_heavier_body_larger_diagonal() {
        // Physics intent: heavier body → larger mass matrix diagonal
        let mut light = ArticulatedBody::new();
        light.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());

        let mut heavy = ArticulatedBody::new();
        heavy.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(10.0), SpatialTransform::identity());

        let m_light = crba_mass_matrix(&mut light);
        let m_heavy = crba_mass_matrix(&mut heavy);

        assert!(
            m_heavy[(0,0)] > m_light[(0,0)],
            "heavier body should have larger diagonal: light={}, heavy={}",
            m_light[(0,0)], m_heavy[(0,0)]
        );
    }

    // ── SLAM Cycle 10: Determinism test ──────────────────────────────

    #[test]
    fn determinism_crba_same_config_same_matrix() {
        let make_body = || {
            let mut body = ArticulatedBody::new();
            body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(2.0), SpatialTransform::identity());
            body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::x() },
                make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));
            body.q[0] = 0.7;
            body.q[1] = -0.3;
            body
        };

        let mut a = make_body();
        let mut b = make_body();
        let ma = crba_mass_matrix(&mut a);
        let mb = crba_mass_matrix(&mut b);

        for i in 0..ma.nrows() {
            for j in 0..ma.ncols() {
                assert_eq!(ma[(i,j)], mb[(i,j)],
                    "CRBA must be deterministic: M[{},{}] differs", i, j);
            }
        }
    }

    #[test]
    fn intent_crba_aba_consistency_m_qdd_equals_tau_minus_bias() {
        // Physics intent: M(q) * qdd = tau - C(q,qd)
        // Verify CRBA mass matrix is consistent with ABA forward dynamics
        use super::super::aba::aba_forward_dynamics;
        use super::super::rnea::rnea_inverse_dynamics;

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(2.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::x() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));

        body.q[0] = 0.5;
        body.q[1] = -0.3;
        body.qd[0] = 1.0;
        body.qd[1] = -0.5;
        body.tau[0] = 2.0;
        body.tau[1] = -1.0;

        // ABA gives qdd
        aba_forward_dynamics(&mut body);
        let qdd = body.qdd.clone();

        // CRBA gives M
        let m = crba_mass_matrix(&body);

        // RNEA gives bias forces (with qdd=0 it gives C(q,qd)*qd + G)
        let mut body_bias = body.clone();
        body_bias.qdd.fill(0.0);
        let (bias, _) = rnea_inverse_dynamics(&body_bias);

        // Check: M * qdd ≈ tau - bias
        let m_qdd = &m * &qdd;
        let tau_minus_bias = &body.tau - &bias;

        for i in 0..m_qdd.len() {
            assert!((m_qdd[i] - tau_minus_bias[i]).abs() < 0.5,
                "M*qdd should equal tau-bias at [{i}]: M*qdd={}, tau-bias={}",
                m_qdd[i], tau_minus_bias[i]);
        }
    }

    #[test]
    fn intent_crba_all_eigenvalues_positive() {
        // Physics intent: mass matrix must be positive definite
        // (all eigenvalues > 0, meaning energy E = 0.5*qd^T*M*qd > 0 for qd≠0)
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(2.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::x() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));
        body.q[0] = 1.2;
        body.q[1] = -0.7;

        let m = crba_mass_matrix(&body);
        let n = m.nrows();

        // Check positive-definiteness via Cholesky (succeeds iff PD)
        // Or check diagonal dominance / eigenvalues
        // Simple check: all diagonal elements positive
        for i in 0..n {
            assert!(m[(i, i)] > 0.0,
                "mass matrix diagonal must be positive: M[{i},{i}]={}", m[(i, i)]);
        }

        // Check via energy test: random qd should give positive kinetic energy
        let qd = nalgebra::DVector::from_vec(vec![1.0, -0.5]);
        let energy = qd.dot(&(&m * &qd));
        assert!(energy > 0.0,
            "kinetic energy must be positive for non-zero velocity: E={}", energy);
    }

    // ── SLAM Cycle 1: CRBA intent/property tests ─────────────────────

    #[test]
    fn property_crba_mass_matrix_positive_diagonal() {
        // Property: Mass matrix diagonal entries must always be strictly positive
        // (each joint has nonzero effective inertia about its own axis)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        for i in 0..3 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(&format!("l{i}"), parent,
                GenJointType::Revolute { axis: Vector3::z() },
                SpatialInertia::sphere(1.0, 0.1),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
        }

        let m = crba_mass_matrix(&body);
        for i in 0..m.nrows() {
            assert!(m[(i, i)] > 0.0,
                "M[{i},{i}]={} must be strictly positive", m[(i, i)]);
        }
        // Also verify positive definiteness via eigenvalues
        let eigenvalues = m.symmetric_eigenvalues();
        for &ev in eigenvalues.iter() {
            assert!(ev > 0.0, "All eigenvalues must be positive: {ev}");
        }
    }

    #[test]
    fn intent_crba_empty_body_zero_matrix() {
        // Intent: Empty body produces 0x0 mass matrix
        let body = ArticulatedBody::new();
        let m = crba_mass_matrix(&body);
        assert_eq!(m.nrows(), 0);
        assert_eq!(m.ncols(), 0);
    }

    #[test]
    fn intent_crba_floating_base_dimensions() {
        // Intent: Floating base (6 DOF) + 2 revolute (1 DOF each) = 8x8 mass matrix
        let mut body = ArticulatedBody::new();
        body.add_body("base", -1, GenJointType::Floating,
            SpatialInertia::sphere(5.0, 0.3), SpatialTransform::identity());
        body.add_body("arm1", 0, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::from_translation(Vector3::new(0.5, 0.0, 0.0)));
        body.add_body("arm2", 1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(0.5, 0.1),
            SpatialTransform::from_translation(Vector3::new(0.5, 0.0, 0.0)));

        let m = crba_mass_matrix(&body);
        assert_eq!(m.nrows(), 8, "6+1+1=8 DOF");
        assert_eq!(m.ncols(), 8);
    }

    // ── SLAM Cycle 7: CRBA proptests ──────────────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_crba_symmetric(
            q0 in -2.0f32..2.0,
            q1 in -2.0f32..2.0,
        ) {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("l1", -1,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0), SpatialTransform::identity());
            body.add_body("l2", 0,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(0.5),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
            body.q[0] = q0;
            body.q[1] = q1;

            let m = crba_mass_matrix(&body);
            for i in 0..m.nrows() {
                for j in i+1..m.ncols() {
                    prop_assert!((m[(i,j)] - m[(j,i)]).abs() < 1e-5,
                        "M must be symmetric: M[{i},{j}]={} vs M[{j},{i}]={}", m[(i,j)], m[(j,i)]);
                }
            }
        }

        #[test]
        fn prop_crba_positive_definite_energy(
            q in -2.0f32..2.0,
        ) {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("link", -1,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0), SpatialTransform::identity());
            body.q[0] = q;

            let m = crba_mass_matrix(&body);
            // E = 0.5 * qd^T * M * qd > 0 for qd != 0
            let qd = nalgebra::DVector::from_vec(vec![1.0]);
            let energy = qd.dot(&(&m * &qd));
            prop_assert!(energy > 0.0,
                "kinetic energy must be positive: E={energy} at q={q}");
        }
    }

    // ── SLAM Cycle 12: bias_forces direct tests ───────────────────────

    proptest! {
        #[test]
        fn prop_crba_floating_base_6x6_block_spd(
            q0 in -2.0f32..2.0,
        ) {
            let mut body = ArticulatedBody::new();
            body.add_body("base", -1, GenJointType::Floating,
                SpatialInertia::sphere(2.0, 0.2), SpatialTransform::identity());
            body.set_joint_q(0, &[q0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]);

            let m = crba_mass_matrix(&body);
            prop_assert_eq!(m.nrows(), 6);
            // Check positive diagonal
            for i in 0..6 {
                prop_assert!(m[(i, i)] > 0.0, "diagonal M[{i},{i}]={} must be positive", m[(i, i)]);
            }
        }
    }

    #[test]
    fn test_bias_forces_nonzero_with_velocity() {
        // bias_forces = C(q, qd)*qd + g(q) should be nonzero with gravity and velocity
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(0.5),
            SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
        body.q[0] = 0.5;
        body.qd[0] = 2.0;
        body.qd[1] = -1.0;

        let c = bias_forces(&mut body);
        let mag: f32 = c.iter().map(|v| v * v).sum::<f32>().sqrt();
        assert!(mag > 0.1, "bias forces should be nonzero with gravity + velocity: ||c||={mag}");
    }

    #[test]
    fn test_bias_forces_equals_gravity_at_zero_velocity() {
        // At qd=0, bias forces = gravity torques only (no Coriolis/centrifugal)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
        body.q[0] = 0.5;
        // qd is zero by default

        let c = bias_forces(&mut body);
        let tau_g = {
            use super::super::rnea::gravity_compensation;
            gravity_compensation(&body)
        };

        // bias_forces at zero velocity should equal gravity compensation
        for i in 0..c.len() {
            assert!((c[i] - tau_g[i]).abs() < 0.1,
                "bias[{i}]={} should match gravity_comp[{i}]={}", c[i], tau_g[i]);
        }
    }
}