Kinematics

#![allow(unused)]
fn main() {
pub fn ik1(twist: Twist, positions: &[Vec2; 4]) -> [Setpoint1; 4];
pub fn ik2(twist: Twist, acc: ChassisAccel, positions: &[Vec2; 4]) -> [Setpoint2; 4];
pub fn ik3(twist: Twist, acc: ChassisAccel, jerk: ChassisJerk, positions: &[Vec2; 4]) -> [Setpoint3; 4];
pub fn fk1(measured: &[(f64, f64); 4], positions: &[Vec2; 4]) -> Twist;
}

Pure kinematics up to third order: chassis twist / accel / jerk in → per-module (azimuth, speed) plus their time derivatives out. ik2 and ik3 assume acc and jerk are the time derivatives of twist in the same frame — the FD-based test suite is the arbiter of the analytic formulas.

fk1 inverts ik1 in the least-squares sense, so measured per-module (angle, speed) pairs collapse back into a chassis twist.

The flagship kinematics demo

See Scene C for order-1 vs order-3 feedforward on an S-curve — the same input, one controller using only ik1, the other adding ik2/ik3 feedforward.

Source (Scene C)

#![allow(unused)]
fn main() {
//! Scene C — first-order vs third-order feedforward on an S-curve accel profile.
//!
//! Same commanded profile fed to two twin sims: one whose module controller
//! only knows about ik1 (order 1), one that adds ik2 rate/accel feedforward
//! terms (order 3). The double-integrated profile is the "ideal" trajectory.
//! We render both robots on the ideal path and per-robot drift bars.

use std::sync::OnceLock;

use anim_core::{arrow, Cmd, Color, DrawList, Vec2};
use anim_timeline::{simulate, Interpolate, System, Track};
use kinematics::controllers::ff_ho::{FfMem, HigherOrderFF, ProfileInput};
use kinematics::sim::SimState;
use kinematics::state::ChassisAccel;
use kinematics::{RobotParams, SwerveState, SwerveSystem};

use super::swerve_common as sc;

pub const DURATION: f64 = 4.0;

/// A smooth S-curve profile: lateral acceleration ramps up and back down,
/// with a small yaw acceleration on top. Deterministic function of t.
fn profile(t: f64) -> ChassisAccel {
    // Gentle S-curve; the order-1 controller has no drive-side feedback, so
    // absolute errors stay small enough for the drift ratio to be legible.
    let ax = 0.30 * (t * std::f64::consts::PI / 2.0).sin();
    let ay = 0.20 * ((t - 1.0) * std::f64::consts::PI / 2.0).sin();
    let alpha = 0.20 * (t * std::f64::consts::PI / 3.0).sin();
    ChassisAccel {
        a: Vec2::new(ax, ay),
        alpha,
    }
}

#[derive(Clone, Copy, Debug, Default)]
struct CompareState {
    low: SimState<FfMem>,
    high: SimState<FfMem>,
}

impl Interpolate for CompareState {
    fn lerp(a: &Self, b: &Self, t: f64) -> Self {
        CompareState {
            low: SimState::<FfMem>::lerp(&a.low, &b.low, t),
            high: SimState::<FfMem>::lerp(&a.high, &b.high, t),
        }
    }
}

struct CompareSystem {
    low: SwerveSystem<HigherOrderFF>,
    high: SwerveSystem<HigherOrderFF>,
}

impl System for CompareSystem {
    type State = CompareState;
    fn init(&self) -> Self::State {
        CompareState {
            low: self.low.init(),
            high: self.high.init(),
        }
    }
    fn step(&self, s: &Self::State, t: f64, dt: f64) -> Self::State {
        CompareState {
            low: self.low.step(&s.low, t, dt),
            high: self.high.step(&s.high, t, dt),
        }
    }
}

fn bake() -> &'static Track<CompareState> {
    static T: OnceLock<Track<CompareState>> = OnceLock::new();
    T.get_or_init(|| {
        let params = RobotParams::default_frc();
        let dt = 1.0 / 1000.0;
        let make = |order: u8| HigherOrderFF {
            input: ProfileInput::Accel(profile),
            order,
            params,
            dt,
            steer_kv: 0.05,
            steer_ka: 0.0,
            drive_ka: 0.8,
            steer_p: 30.0,
        };
        let low = SwerveSystem::new(params, make(1), SwerveState::default());
        let high = SwerveSystem::new(params, make(3), SwerveState::default());
        let sys = CompareSystem { low, high };
        simulate(&sys, DURATION, 1000.0, 120.0)
    })
}

fn camera(track: &Track<CompareState>) -> sc::Camera {
    let mut pts = Vec::new();
    for s in track.samples() {
        pts.push(s.low.swerve.chassis.pos);
        pts.push(s.high.swerve.chassis.pos);
        pts.push(s.high.mem.ideal_pos);
    }
    let trajectory_rect = sc::viewport_rect(0.0, 0.28, 1.0, 0.72);
    sc::Camera::fit_in_rect(&pts, trajectory_rect, 32.0, 3.0)
}

pub fn scene(t: f64) -> DrawList {
    let track = bake();
    let params = RobotParams::default_frc();
    let cam = camera(track);
    let state = track.lerp(t);

    let mut out = DrawList::new();
    out.extend(sc::field_grid(&cam, 5.0));

    // Ideal path (dashed) up to now.
    let dt = track.dt();
    let idx = ((t / dt) as usize).min(track.samples().len() - 1);
    let stride = ((track.samples().len() / 200).max(1)).max(1);
    let ideal_pts = sc::build_trail(
        track.samples(),
        |s: &CompareState| s.high.mem.ideal_pos,
        idx,
        stride,
    );
    sc::draw_trail(&cam, &ideal_pts, sc::GHOST_TRAIL_COL, true, &mut out);

    // Two robots.
    let low_col = Color::rgba(255, 130, 80, 0.5);
    let high_col = Color::rgba(120, 220, 200, 0.9);
    // Low-order robot slightly desaturated.
    let mut low_state = state.low.swerve;
    // Draw low first (under high).
    sc::draw_robot(&cam, &params, &low_state, Vec2::new(0.55, 0.55), &mut out);
    sc::draw_robot(
        &cam,
        &params,
        &state.high.swerve,
        Vec2::new(0.55, 0.55),
        &mut out,
    );
    // Ideal marker (dot).
    out.push(Cmd::FillCircle {
        center: cam.m(state.high.mem.ideal_pos),
        r: 5.0,
        color: sc::GHOST_COL,
    });
    let _ = &mut low_state;
    let _ = low_col;
    let _ = high_col;

    // Commanded accel arrow (coral) at ideal position.
    sc::draw_overlay_arrow(
        &cam,
        state.high.mem.ideal_pos,
        state.high.mem.accel.a,
        0.2,
        2.0,
        sc::ACC_COL,
        &mut out,
    );

    // Drift arrows: ideal -> low (orange), ideal -> high (teal).
    let s_low = cam.m(state.low.swerve.chassis.pos) - cam.m(state.high.mem.ideal_pos);
    let s_high = cam.m(state.high.swerve.chassis.pos) - cam.m(state.high.mem.ideal_pos);
    out.extend(arrow(
        cam.m(state.high.mem.ideal_pos),
        s_low,
        1.5,
        Color::rgba(255, 130, 80, 0.9),
    ));
    out.extend(arrow(
        cam.m(state.high.mem.ideal_pos),
        s_high,
        1.5,
        Color::rgba(120, 220, 200, 0.9),
    ));

    // Drift-over-time chart with both series overlaid so the reader sees
    // the whole comparison, not just the current instant.
    let xs: Vec<f64> = (0..track.samples().len()).map(|k| k as f64 * dt).collect();
    let ys_low: Vec<f64> = track
        .samples()
        .iter()
        .map(|s| (s.low.swerve.chassis.pos - s.high.mem.ideal_pos).length())
        .collect();
    let ys_high: Vec<f64> = track
        .samples()
        .iter()
        .map(|s| (s.high.swerve.chassis.pos - s.high.mem.ideal_pos).length())
        .collect();
    let all_ys = ys_low.iter().chain(ys_high.iter()).copied();
    let (y_lo, y_hi) = anim_core::auto_range(all_ys, true);
    let chart = anim_core::LineChart {
        rect: sc::viewport_rect(0.03, 0.03, 0.94, 0.20),
        x_range: (0.0, DURATION),
        y_range: (y_lo, y_hi),
        now: Some(t),
        ..anim_core::LineChart::default()
    };
    out.extend(chart.render(&[
        anim_core::Series {
            xs: &xs,
            ys: &ys_low,
            color: Color::rgba(255, 130, 80, 0.95),
            width: 2.0,
        },
        anim_core::Series {
            xs: &xs,
            ys: &ys_high,
            color: Color::rgba(120, 220, 200, 0.95),
            width: 2.0,
        },
    ]));

    out
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn order3_beats_order1() {
        let track = bake();
        let last = track.samples().last().unwrap();
        let ideal = last.high.mem.ideal_pos;
        let err_low = (last.low.swerve.chassis.pos - ideal).length();
        let err_high = (last.high.swerve.chassis.pos - ideal).length();
        eprintln!("err_low = {err_low}, err_high = {err_high}");
        assert!(err_low.is_finite() && err_high.is_finite());
        assert!(
            err_high < 0.5 * err_low,
            "order-3 err {err_high} not < 0.5 * order-1 err {err_low}"
        );
    }

    #[test]
    fn no_nans() {
        let track = bake();
        for s in track.samples() {
            assert!(s.low.swerve.chassis.pos.x.is_finite());
            assert!(s.high.swerve.chassis.pos.x.is_finite());
        }
    }
}
}