symplex 0.42.0

//! Lagrangian dynamics for robotic systems.
//!
//! Derives the equations of motion from kinetic and potential energy:
//!
//! Given T(q, q̇) and V(q), the Euler-Lagrange equations are:
//!     d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ = τᵢ
//!
//! which yields the manipulator equation:
//!     M(q)q̈ + C(q, q̇)q̇ + G(q) = τ
//!
//! This module provides functions to extract M, C, and G symbolically.
//! Each coordinate travels with its velocity and acceleration as a
//! [`GeneralizedCoordinate`]; [`manipulator_equation`] returns the three
//! terms as a [`ManipulatorEquation`].

use crate::domains::matrix::Matrix;
use crate::prelude::*;

fn invalid(operation: &'static str, reason: String) -> SymplexError {
    SymplexError::invalid_argument(operation, reason)
}

/// Fresh symbols standing in for the coordinates, velocities and
/// accelerations that are not plain symbols, such as `q̇ = q.formal_diff(t)`.
///
/// The Lagrangian formalism differentiates with respect to `qᵢ`, `q̇ᵢ` and
/// `q̈ᵢ` as independent variables, and [`Ex::diff`] differentiates with
/// respect to symbols only (with respect to `Derivative(q, t)` it gives 0).
/// Every function of this module therefore works on the expressions with
/// such variables replaced by stand-ins ([`hide`](Self::hide)) and maps the
/// results back ([`show`](Self::show)).  Before 0.41 `euler_lagrange` of the
/// pendulum `T = ½ml²q̇²` with `q̇ = Derivative(q, t)` returned
/// `glm·sin q − ml²·q̇·Derivative(q̇, q)` (no `ml²q̈`) and its mass matrix
/// was `[[0]]`.
struct StandIns {
    /// `(variable, stand-in)` for the variables that are not symbols.
    pairs: Vec<(Ex, Ex)>,
}

impl StandIns {
    /// Stand-ins for the non-symbols among `vars`, with reserved-looking
    /// names (`__gc`, `__gc_1`, …) absent from `vars` and `exprs`.
    fn new(vars: &[&Ex], exprs: &[&Ex]) -> Self {
        let mut pairs: Vec<(Ex, Ex)> = Vec::new();
        let mut k = 0usize;
        for v in vars {
            if v.expr_type() == crate::api::expr::ExprType::Symbol
                || pairs.iter().any(|(o, _)| o == *v)
            {
                continue;
            }
            let ctx = v.context();
            let fresh = loop {
                let name = if k == 0 {
                    "__gc".to_string()
                } else {
                    format!("__gc_{k}")
                };
                k += 1;
                let sym = ctx.symbol(&name);
                if !vars.iter().chain(exprs).any(|e| e.contains(&sym)) {
                    break sym;
                }
            };
            pairs.push(((*v).clone(), fresh));
        }
        StandIns { pairs }
    }

    fn hide(&self, e: &Ex) -> Ex {
        if self.pairs.is_empty() {
            return e.clone();
        }
        let map: Vec<(&Ex, &Ex)> = self.pairs.iter().map(|(o, s)| (o, s)).collect();
        e.subs_map(&map)
    }

    fn show(&self, e: &Ex) -> Ex {
        if self.pairs.is_empty() {
            return e.clone();
        }
        let map: Vec<(&Ex, &Ex)> = self.pairs.iter().map(|(o, s)| (s, o)).collect();
        e.subs_map(&map)
    }

    /// The stand-in of `v` (`v` itself when it is a symbol).
    fn var(&self, v: &Ex) -> Ex {
        self.pairs
            .iter()
            .find(|(o, _)| o == v)
            .map_or_else(|| v.clone(), |(_, s)| s.clone())
    }

    fn vars(&self, vs: &[&Ex]) -> Vec<Ex> {
        vs.iter().map(|v| self.var(v)).collect()
    }
}

fn coord_vars<'a>(coords: &[GeneralizedCoordinate<'a>]) -> Vec<&'a Ex> {
    coords
        .iter()
        .flat_map(|c| [c.q, c.q_dot, c.q_ddot])
        .collect()
}

/// One generalized coordinate `qᵢ` together with its velocity `q̇ᵢ` and
/// acceleration `q̈ᵢ`.
///
/// The Lagrangian formalism treats the three as *independent* symbols for
/// partial differentiation; [`total_time_derivative`] reassembles `d/dt`
/// from them by the chain rule.  Keeping them in one value rules out a
/// transposed `(q̇, q)` pair or an acceleration list of the wrong length.
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::GeneralizedCoordinate;
///
/// let ctx = Context::new();
/// let q = ctx.symbol("q");
/// let qd = ctx.symbol("qd");
/// let qdd = ctx.symbol("qdd");
///
/// let coord = GeneralizedCoordinate { q: &q, q_dot: &qd, q_ddot: &qdd };
/// assert_eq!(format!("{}", coord.q_ddot), "qdd");
/// ```
#[derive(Clone, Copy, Debug)]
pub struct GeneralizedCoordinate<'a> {
    /// `qᵢ` — the coordinate (a joint angle, a displacement, …).
    pub q: &'a Ex,
    /// `q̇ᵢ` — its generalized velocity.
    pub q_dot: &'a Ex,
    /// `q̈ᵢ` — its generalized acceleration.
    pub q_ddot: &'a Ex,
}

/// Compute the total time derivative of an expression.
///
/// Given an expression that depends on generalized coordinates q(t) and
/// their velocities q̇(t), computes d/dt using the chain rule:
///
/// d/dt f(q, q̇) = Σᵢ (∂f/∂qᵢ)·q̇ᵢ + Σᵢ (∂f/∂q̇ᵢ)·q̈ᵢ
///
/// Since q and q̇ are treated as independent symbols for the purpose of
/// partial differentiation, we apply the chain rule manually.
///
/// # Parameters
/// - `expr`: The expression to differentiate with respect to time
/// - `coords`: The generalized coordinates, each with its velocity q̇ᵢ and
///   acceleration q̈ᵢ
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::{total_time_derivative, GeneralizedCoordinate};
///
/// let ctx = Context::new();
/// let q = ctx.symbol("q");
/// let qd = ctx.symbol("qd");
/// let qdd = ctx.symbol("qdd");
///
/// // d/dt(q) = qd
/// let coords = [GeneralizedCoordinate { q: &q, q_dot: &qd, q_ddot: &qdd }];
/// let result = total_time_derivative(&q, &coords);
/// let val = result.subs(&qd, &ctx.int(7)).eval().eval_f64().unwrap();
/// assert!((val - 7.0).abs() < 1e-12);
/// ```
pub fn total_time_derivative(expr: &Ex, coords: &[GeneralizedCoordinate<'_>]) -> Ex {
    let st = StandIns::new(&coord_vars(coords), &[expr]);
    if st.pairs.is_empty() {
        return total_time_derivative_symbols(expr, coords);
    }
    let hidden: Vec<[Ex; 3]> = coords
        .iter()
        .map(|c| [st.var(c.q), st.var(c.q_dot), st.var(c.q_ddot)])
        .collect();
    let hidden_coords: Vec<GeneralizedCoordinate<'_>> = hidden
        .iter()
        .map(|[q, q_dot, q_ddot]| GeneralizedCoordinate { q, q_dot, q_ddot })
        .collect();
    st.show(&total_time_derivative_symbols(
        &st.hide(expr),
        &hidden_coords,
    ))
}

/// [`total_time_derivative`] for coordinates that are all symbols.
fn total_time_derivative_symbols(expr: &Ex, coords: &[GeneralizedCoordinate<'_>]) -> Ex {
    // d/dt f = Σᵢ (∂f/∂qᵢ)·q̇ᵢ + Σᵢ (∂f/∂q̇ᵢ)·q̈ᵢ
    let mut result = expr.context().int(0);

    for coord in coords {
        // ∂f/∂qᵢ · q̇ᵢ
        let df_dqi = expr.diff(coord.q);
        result = &result + &(&df_dqi * coord.q_dot);

        // ∂f/∂q̇ᵢ · q̈ᵢ
        let df_dqi_dot = expr.diff(coord.q_dot);
        result = &result + &(&df_dqi_dot * coord.q_ddot);
    }

    result
}

/// Compute the Euler-Lagrange equations of motion.
///
/// Given kinetic energy T(q, q̇) and potential energy V(q), computes:
/// d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ = τᵢ
///
/// where L = T - V is the Lagrangian.
///
/// Returns a vector of expressions, one per generalized coordinate.
/// Each expression equals the generalized force τᵢ on the left-hand side
/// of the equation of motion.
///
/// # Parameters
/// - `kinetic_energy`: T(q, q̇), the kinetic energy
/// - `potential_energy`: V(q), the potential energy
/// - `coords`: The generalized coordinates, each with its velocity q̇ᵢ and
///   acceleration q̈ᵢ; one equation is returned per entry, in order
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::{euler_lagrange, GeneralizedCoordinate};
///
/// let ctx = Context::new();
/// let m = ctx.symbol("m");
/// let q = ctx.symbol("q");
/// let qd = ctx.symbol("qd");
/// let qdd = ctx.symbol("qdd");
///
/// // Free particle: T = ½m·q̇², V = 0
/// let half = ctx.rational(1, 2);
/// let ke = &half * &m * &qd.powi(2);
/// let pe = ctx.int(0);
/// let coords = [GeneralizedCoordinate { q: &q, q_dot: &qd, q_ddot: &qdd }];
/// let eqs = euler_lagrange(&ke, &pe, &coords);
/// // Should give m·q̈
/// assert_eq!(eqs.len(), 1);
/// ```
pub fn euler_lagrange(
    kinetic_energy: &Ex,
    potential_energy: &Ex,
    coords: &[GeneralizedCoordinate<'_>],
) -> Vec<Ex> {
    let st = StandIns::new(&coord_vars(coords), &[kinetic_energy, potential_energy]);
    if st.pairs.is_empty() {
        return euler_lagrange_symbols(kinetic_energy, potential_energy, coords);
    }
    let hidden: Vec<[Ex; 3]> = coords
        .iter()
        .map(|c| [st.var(c.q), st.var(c.q_dot), st.var(c.q_ddot)])
        .collect();
    let hidden_coords: Vec<GeneralizedCoordinate<'_>> = hidden
        .iter()
        .map(|[q, q_dot, q_ddot]| GeneralizedCoordinate { q, q_dot, q_ddot })
        .collect();
    euler_lagrange_symbols(
        &st.hide(kinetic_energy),
        &st.hide(potential_energy),
        &hidden_coords,
    )
    .iter()
    .map(|e| st.show(e))
    .collect()
}

/// [`euler_lagrange`] for coordinates that are all symbols.
fn euler_lagrange_symbols(
    kinetic_energy: &Ex,
    potential_energy: &Ex,
    coords: &[GeneralizedCoordinate<'_>],
) -> Vec<Ex> {
    let lagrangian = kinetic_energy - potential_energy;
    let mut equations = Vec::with_capacity(coords.len());

    for coord in coords {
        // ∂L/∂q̇ᵢ
        let dl_dqi_dot = lagrangian.diff(coord.q_dot);

        // d/dt(∂L/∂q̇ᵢ)
        let dt_dl_dqi_dot = total_time_derivative_symbols(&dl_dqi_dot, coords);

        // ∂L/∂qᵢ
        let dl_dqi = lagrangian.diff(coord.q);

        // Euler-Lagrange: d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ
        let eq_i = &dt_dl_dqi_dot - &dl_dqi;
        equations.push(eq_i.eval());
    }

    equations
}

/// Extract the mass (inertia) matrix M(q) from kinetic energy.
///
/// For a system where T = ½ q̇ᵀ M(q) q̇, the mass matrix entries are:
///
/// M_ij = ∂²T / (∂q̇ᵢ ∂q̇ⱼ)
///
/// The resulting matrix is symmetric for physical systems.
///
/// # Parameters
/// - `kinetic_energy`: T(q, q̇), the kinetic energy expression
/// - `qdot_vars`: The velocity variables [q̇₁, q̇₂, ...]
///
/// # Errors
///
/// Returns [`SymplexError::InvalidArgument`] if `qdot_vars` is empty.
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::mass_matrix;
///
/// let ctx = Context::new();
/// let m = ctx.symbol("m");
/// let qd = ctx.symbol("qd");
///
/// // T = ½m·q̇²  →  M = [[m]]
/// let half = ctx.rational(1, 2);
/// let ke = &half * &m * &qd.powi(2);
/// let mm = mass_matrix(&ke, &[&qd]).unwrap();
/// assert_eq!(mm.shape(), (1, 1));
/// ```
pub fn mass_matrix(kinetic_energy: &Ex, qdot_vars: &[&Ex]) -> Result<Matrix, SymplexError> {
    if qdot_vars.is_empty() {
        return Err(invalid(
            "dynamics::mass_matrix",
            "need at least one velocity variable".into(),
        ));
    }
    let st = StandIns::new(qdot_vars, &[kinetic_energy]);
    let t = st.hide(kinetic_energy);
    let qd = st.vars(qdot_vars);
    let n = qdot_vars.len();
    let mut rows = Vec::with_capacity(n);
    for i in 0..n {
        let mut row = Vec::with_capacity(n);
        for j in 0..n {
            // M_ij = ∂²T / ∂q̇ᵢ∂q̇ⱼ
            let m_ij = st.show(&t.diff(&qd[i]).diff(&qd[j]));
            row.push(m_ij);
        }
        rows.push(row);
    }
    // n ≥ 1 rows of n entries each.
    Ok(Matrix::from_rows_unchecked(rows).eval())
}

/// Compute Christoffel symbols of the first kind from the mass matrix.
///
/// The Christoffel symbols are defined as:
///
/// Γᵢⱼₖ = ½(∂Mᵢⱼ/∂qₖ + ∂Mᵢₖ/∂qⱼ - ∂Mⱼₖ/∂qᵢ)
///
/// Returns a 3D structure indexed as `christoffel[i][j][k]`.
///
/// # Parameters
/// - `mass_mat`: The mass matrix M(q), an n×n [`Matrix`]
/// - `q_vars`: The generalized coordinate variables [q₁, q₂, ...]
///
/// # Errors
///
/// Returns [`SymplexError::InvalidArgument`] if `mass_mat` is not
/// `n×n` for `n = q_vars.len()` (in particular if `q_vars` is empty).
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::{mass_matrix, christoffel_symbols};
///
/// let ctx = Context::new();
/// let m = ctx.symbol("m");
/// let qd = ctx.symbol("qd");
/// let q = ctx.symbol("q");
///
/// let half = ctx.rational(1, 2);
/// let ke = &half * &m * &qd.powi(2);
/// let mm = mass_matrix(&ke, &[&qd]).unwrap();
/// let cs = christoffel_symbols(&mm, &[&q]).unwrap();
/// assert_eq!(cs.len(), 1);
/// assert_eq!(cs[0].len(), 1);
/// assert_eq!(cs[0][0].len(), 1);
/// ```
pub fn christoffel_symbols(
    mass_mat: &Matrix,
    q_vars: &[&Ex],
) -> Result<Vec<Vec<Vec<Ex>>>, SymplexError> {
    let n = q_vars.len();
    if n == 0 || mass_mat.shape() != (n, n) {
        return Err(invalid(
            "dynamics::christoffel_symbols",
            format!(
                "mass matrix shape {:?} does not match {n} coordinates",
                mass_mat.shape()
            ),
        ));
    }

    let half = q_vars[0].context().rational(1, 2);
    let entries: Vec<&Ex> = mass_mat.iter().collect();
    let st = StandIns::new(q_vars, &entries);
    let q = st.vars(q_vars);
    let m = Matrix::from_fn_unchecked(n, n, |i, j| st.hide(mass_mat.get(i, j)));

    let mut result = Vec::with_capacity(n);
    for i in 0..n {
        let mut plane = Vec::with_capacity(n);
        for j in 0..n {
            let mut row = Vec::with_capacity(n);
            for k in 0..n {
                // Γᵢⱼₖ = ½(∂Mᵢⱼ/∂qₖ + ∂Mᵢₖ/∂qⱼ - ∂Mⱼₖ/∂qᵢ)
                let dm_ij_dqk = m.get(i, j).diff(&q[k]);
                let dm_ik_dqj = m.get(i, k).diff(&q[j]);
                let dm_jk_dqi = m.get(j, k).diff(&q[i]);

                let gamma = &half * &(&(&dm_ij_dqk + &dm_ik_dqj) - &dm_jk_dqi);
                row.push(st.show(&gamma));
            }
            plane.push(row);
        }
        result.push(plane);
    }

    Ok(result)
}

/// Compute the Coriolis matrix C(q, q̇).
///
/// The Coriolis matrix combines centrifugal and Coriolis effects:
///
/// Cᵢⱼ = Σₖ Γᵢⱼₖ · q̇ₖ
///
/// where Γᵢⱼₖ are the Christoffel symbols of the first kind.
///
/// # Parameters
/// - `mass_mat`: The mass matrix M(q)
/// - `q_vars`: Generalized coordinate variables [q₁, q₂, ...]
/// - `qdot_vars`: Generalized velocity variables [q̇₁, q̇₂, ...]
///
/// # Errors
///
/// Returns [`SymplexError::InvalidArgument`] if `q_vars` and `qdot_vars`
/// differ in length, or if `mass_mat` is not `n×n` for `n = q_vars.len()`
/// (in particular if `q_vars` is empty).
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::{mass_matrix, coriolis_matrix};
///
/// let ctx = Context::new();
/// let m = ctx.symbol("m");
/// let q = ctx.symbol("q");
/// let qd = ctx.symbol("qd");
///
/// let half = ctx.rational(1, 2);
/// let ke = &half * &m * &qd.powi(2);
/// let mm = mass_matrix(&ke, &[&qd]).unwrap();
/// let c = coriolis_matrix(&mm, &[&q], &[&qd]).unwrap();
/// assert_eq!(c.shape(), (1, 1));
/// ```
pub fn coriolis_matrix(
    mass_mat: &Matrix,
    q_vars: &[&Ex],
    qdot_vars: &[&Ex],
) -> Result<Matrix, SymplexError> {
    let n = q_vars.len();
    if qdot_vars.len() != n {
        return Err(invalid(
            "dynamics::coriolis_matrix",
            format!(
                "q_vars and qdot_vars must have the same length, got {n} and {}",
                qdot_vars.len()
            ),
        ));
    }

    // Also rejects an empty `q_vars`, so `christoffel` is n×n×n with n ≥ 1.
    let christoffel = christoffel_symbols(mass_mat, q_vars)?;

    let mut rows = Vec::with_capacity(n);
    for christoffel_i in christoffel.iter().take(n) {
        let mut row = Vec::with_capacity(n);
        for christoffel_ij in christoffel_i.iter().take(n) {
            // C_ij = Σₖ Γᵢⱼₖ · q̇ₖ
            let mut c_ij = qdot_vars[0].context().int(0);
            for k in 0..n {
                c_ij = &c_ij + &(&christoffel_ij[k] * qdot_vars[k]);
            }
            row.push(c_ij);
        }
        rows.push(row);
    }

    Ok(Matrix::from_rows_unchecked(rows).eval())
}

/// Compute the gravity vector g(q) = ∂V/∂q.
///
/// Each element gᵢ = ∂V/∂qᵢ is the generalized gravitational force
/// acting on the i-th coordinate.
///
/// # Parameters
/// - `potential_energy`: V(q), the potential energy expression
/// - `q_vars`: Generalized coordinate variables [q₁, q₂, ...]
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::gravity_vector;
///
/// let ctx = Context::new();
/// let m = ctx.symbol("m");
/// let g = ctx.symbol("g");
/// let l = ctx.symbol("L");
/// let q = ctx.symbol("q");
///
/// // V = m·g·L·cos(q)  →  g(q) = ∂V/∂q = -m·g·L·sin(q)
/// let pe = &m * &g * &l * &q.cos();
/// let gv = gravity_vector(&pe, &[&q]);
/// assert_eq!(gv.len(), 1);
/// ```
pub fn gravity_vector(potential_energy: &Ex, q_vars: &[&Ex]) -> Vec<Ex> {
    let st = StandIns::new(q_vars, &[potential_energy]);
    let v = st.hide(potential_energy);
    st.vars(q_vars)
        .iter()
        .map(|qi| st.show(&v.diff(qi)).eval())
        .collect()
}

/// The three terms of the manipulator equation
///
/// ```text
/// M(q)·q̈ + C(q, q̇)·q̇ + G(q) = τ
/// ```
///
/// as returned by [`manipulator_equation`], for `n` generalized coordinates
/// and a kinetic energy of the general natural form
/// `T = ½·q̇ᵀM(q)q̇ + b(q)ᵀq̇ + T₀(q)` (the last two terms appear in
/// rotating frames: gyroscopic and centrifugal-potential terms).  For the
/// same energies the left-hand side evaluates to exactly what
/// [`euler_lagrange`] returns.
#[derive(Clone, Debug)]
pub struct ManipulatorEquation {
    /// `M(q)` — the `n×n` mass (inertia) matrix, `Mᵢⱼ = ∂²T/∂q̇ᵢ∂q̇ⱼ`
    /// ([`mass_matrix`]).
    pub mass: Matrix,
    /// `C(q, q̇)` — the `n×n` matrix that multiplies `q̇`: the
    /// Coriolis/centrifugal matrix built from the Christoffel symbols of the
    /// first kind, `Σₖ Γᵢⱼₖ·q̇ₖ` ([`coriolis_matrix`]), plus the
    /// gyroscopic matrix `∂bᵢ/∂qⱼ − ∂bⱼ/∂qᵢ` of the part of `T` linear
    /// in `q̇` (zero for `T = ½·q̇ᵀMq̇`).  The gyroscopic matrix is
    /// skew-symmetric, so `Ṁ − 2C` stays skew-symmetric.
    pub coriolis: Matrix,
    /// `G(q)` — the `n` generalized conservative forces,
    /// `Gᵢ = ∂(V − T₀)/∂qᵢ`: the gradient of the potential
    /// ([`gravity_vector`]) less that of the velocity-independent part of
    /// `T` (zero for `T = ½·q̇ᵀMq̇`).
    pub gravity: Vec<Ex>,
}

/// Compute the full manipulator equation components: M(q), C(q, q̇), G(q).
///
/// Given kinetic energy T(q, q̇) and potential energy V(q), returns
/// the mass matrix, Coriolis matrix, and gravity vector such that:
///
/// M(q)q̈ + C(q, q̇)q̇ + G(q) = τ
///
/// with the left-hand side equal to the [`euler_lagrange`] equations.
/// For `T = ½·q̇ᵀM(q)q̇` this is [`mass_matrix`], [`coriolis_matrix`] and
/// [`gravity_vector`].  A kinetic energy with terms linear in the
/// velocities or free of them, `T = ½·q̇ᵀMq̇ + b(q)ᵀq̇ + T₀(q)`, adds the
/// gyroscopic matrix to `C` and `−∇T₀` to `G` (see
/// [`ManipulatorEquation`]); up to 0.29 those terms were silently dropped,
/// so that `M·q̈ + C·q̇ + G` disagreed with the Euler–Lagrange equations.
///
/// # Parameters
/// - `kinetic_energy`: T(q, q̇)
/// - `potential_energy`: V(q)
/// - `q_vars`: Generalized coordinate variables [q₁, q₂, ...]
/// - `qdot_vars`: Generalized velocity variables [q̇₁, q̇₂, ...]
///
/// # Returns
///
/// A [`ManipulatorEquation`] holding the `n×n` [`mass`](ManipulatorEquation::mass)
/// matrix, the `n×n` [`coriolis`](ManipulatorEquation::coriolis) matrix and the
/// `n`-element [`gravity`](ManipulatorEquation::gravity) vector.
///
/// # Errors
///
/// Returns [`SymplexError::InvalidArgument`] if `q_vars` and `qdot_vars`
/// differ in length or are empty, or if `T` is not of degree at most two
/// in the velocities (some `∂²T/∂q̇ᵢ∂q̇ⱼ` still depends on a velocity, as
/// for a relativistic `T`): the equations of motion are then not of the
/// form `M(q)·q̈ + C(q, q̇)·q̇ + G(q)`; [`euler_lagrange`] still applies.
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::dynamics::manipulator_equation;
///
/// let ctx = Context::new();
/// let m_val = ctx.symbol("m");
/// let q = ctx.symbol("q");
/// let qd = ctx.symbol("qd");
///
/// let half = ctx.rational(1, 2);
/// let ke = &half * &m_val * &qd.powi(2);
/// let pe = ctx.int(0);
/// let eq = manipulator_equation(&ke, &pe, &[&q], &[&qd]).unwrap();
/// assert_eq!(eq.mass.shape(), (1, 1));
/// assert_eq!(eq.coriolis.shape(), (1, 1));
/// assert_eq!(eq.gravity.len(), 1);
/// ```
pub fn manipulator_equation(
    kinetic_energy: &Ex,
    potential_energy: &Ex,
    q_vars: &[&Ex],
    qdot_vars: &[&Ex],
) -> Result<ManipulatorEquation, SymplexError> {
    const OP: &str = "dynamics::manipulator_equation";
    if q_vars.len() != qdot_vars.len() {
        return Err(invalid(
            OP,
            format!(
                "q_vars and qdot_vars must have the same length, got {} and {}",
                q_vars.len(),
                qdot_vars.len()
            ),
        ));
    }
    let all: Vec<&Ex> = q_vars.iter().chain(qdot_vars).copied().collect();
    let st = StandIns::new(&all, &[kinetic_energy, potential_energy]);
    if st.pairs.is_empty() {
        return manipulator_equation_symbols(kinetic_energy, potential_energy, q_vars, qdot_vars);
    }
    let q = st.vars(q_vars);
    let qd = st.vars(qdot_vars);
    let qr: Vec<&Ex> = q.iter().collect();
    let qdr: Vec<&Ex> = qd.iter().collect();
    let me = manipulator_equation_symbols(
        &st.hide(kinetic_energy),
        &st.hide(potential_energy),
        &qr,
        &qdr,
    )?;
    Ok(ManipulatorEquation {
        mass: me.mass.map(|e| st.show(e)),
        coriolis: me.coriolis.map(|e| st.show(e)),
        gravity: me.gravity.iter().map(|e| st.show(e)).collect(),
    })
}

/// [`manipulator_equation`] for coordinates and velocities that are all
/// symbols.
fn manipulator_equation_symbols(
    kinetic_energy: &Ex,
    potential_energy: &Ex,
    q_vars: &[&Ex],
    qdot_vars: &[&Ex],
) -> Result<ManipulatorEquation, SymplexError> {
    const OP: &str = "dynamics::manipulator_equation";
    let m = mass_matrix(kinetic_energy, qdot_vars)?;
    let n = qdot_vars.len();
    for i in 0..n {
        for j in 0..n {
            let entry = m.get(i, j);
            if let Some(v) = qdot_vars.iter().find(|v| entry.free_symbols().contains(v)) {
                return Err(invalid(
                    OP,
                    format!(
                        "the kinetic energy is not of degree at most two in the velocities: \
                         ∂²T/∂{}∂{} = {entry} depends on {v}",
                        qdot_vars[i], qdot_vars[j]
                    ),
                ));
            }
        }
    }
    let c = coriolis_matrix(&m, q_vars, qdot_vars)?;
    // T = ½ q̇ᵀMq̇ + bᵀq̇ + T₀ exactly, as its Hessian in q̇ is free of q̇:
    // bᵢ = ∂T/∂q̇ᵢ and T₀ = T at q̇ = 0.
    let ctx = kinetic_energy.context();
    let zero = ctx.int(0);
    let at_rest: Vec<(&Ex, &Ex)> = qdot_vars.iter().map(|v| (*v, &zero)).collect();
    let b: Vec<Ex> = qdot_vars
        .iter()
        .map(|v| kinetic_energy.diff(v).subs_map(&at_rest).eval())
        .collect();
    let t0 = kinetic_energy.subs_map(&at_rest).eval();
    let c = Matrix::from_fn_unchecked(n, n, |i, j| {
        let gyro = &b[i].diff(q_vars[j]) - &b[j].diff(q_vars[i]);
        (c.get(i, j) + &gyro).eval()
    });
    let g = q_vars
        .iter()
        .map(|qi| (&potential_energy.diff(qi) - &t0.diff(qi)).eval())
        .collect();
    Ok(ManipulatorEquation {
        mass: m.eval(),
        coriolis: c,
        gravity: g,
    })
}