Swerve

The full dynamic model:

#![allow(unused)]
fn main() {
pub struct RobotParams { pub mass: f64, pub inertia_z: f64, pub drive: MotorParams,
                         pub steer: MotorParams, pub tire: TireParams, pub modules: [ModuleConfig; 4] }
pub struct SwerveState { pub chassis: ChassisState, pub modules: [ModuleState; 4] }

pub struct SwerveSystem<C: Controller> { /* ... */ }
impl<C: Controller> System for SwerveSystem<C> { /* pure step */ }
}

The motor model exposes a single closed-form motor_torque(&MotorParams, v, omega) — same function used for both the drive and steer motors, since a DC motor doesn't care which shaft it's spinning. The stationary "do nothing" controller Zero is what you use for a rest-stays-at-rest test.

Every dynamic scene has a kinematic twin — a GhostState integrated by ghost_step on the same commanded twist. The gap between the ghost and the physically simulated chassis is the drift a real controller has to close.

Each step at 1 kHz: controller emits per-module ModuleCommands; steering dynamics integrate under motor torque and friction; per-module slip and tire forces are computed in the world frame; drive dynamics update wheel_omega; the chassis sums forces and torques and integrates its pose via semi-implicit Euler. Diagnostics (slip_vel, tire_force, cmd) land back on each ModuleState, so scenes never re-derive physics — they read the baked state directly.

The Controller trait folds any per-step memory the controller wants into SimState<Memory>, so every stored state is a valid resume point (verified by a checkpoint-resume test).

Scene A — first-order IK drift

The star scene: open-loop ik1 commands vs a kinematic ghost. The drift vector between them is the whole story.

Source (Scene A)

#![allow(unused)]
fn main() {
//! Scene A — open-loop first-order IK vs a kinematic ghost.
//!
//! The dynamic robot uses [`OpenLoopIk1`], a naive controller: `drive_v =
//! speed / kv_ground`, `steer_v = P * (target - actual)`. No slip
//! compensation, no accel FF — that's the whole point. The ghost integrates
//! the same commanded twist exactly. The drift vector between them is the
//! story of this scene.

use std::sync::OnceLock;

#[cfg(test)]
use anim_core::Cmd;
use anim_core::{arrow, auto_range, DrawList, LineChart, Series, Vec2};
use anim_timeline::{simulate, Track};
use kinematics::drift::{DriftFullState, DriftSystem};
use kinematics::{RobotParams, Twist};

use super::swerve_common as sc;

pub const DURATION: f64 = 6.0;

fn bake() -> &'static Track<DriftFullState> {
    static T: OnceLock<Track<DriftFullState>> = OnceLock::new();
    T.get_or_init(|| {
        let params = RobotParams::default_frc();
        let cmd = Twist {
            v: Vec2::new(1.5, 0.0),
            w: 1.8,
        };
        let sys = DriftSystem::new(params, cmd, 12.0);
        simulate(&sys, DURATION, 1000.0, 120.0)
    })
}

fn camera(track: &Track<DriftFullState>) -> sc::Camera {
    let mut pts: Vec<Vec2> = Vec::with_capacity(track.samples().len() * 2);
    for s in track.samples() {
        pts.push(s.swerve.chassis.pos);
        pts.push(s.ghost.pos);
    }
    // Reserve the top 70 % of the viewport for the trajectory; bottom for
    // the drift chart. `min_world_span` keeps things zoomed out enough to
    // show context around a small trajectory.
    let trajectory_rect = sc::viewport_rect(0.0, 0.28, 1.0, 0.72);
    sc::Camera::fit_in_rect(&pts, trajectory_rect, 32.0, 4.0)
}

fn drift_series(track: &Track<DriftFullState>) -> (Vec<f64>, Vec<f64>) {
    let xs: Vec<f64> = (0..track.samples().len())
        .map(|k| k as f64 * track.dt())
        .collect();
    let ys: Vec<f64> = track
        .samples()
        .iter()
        .map(|s| (s.swerve.chassis.pos - s.ghost.pos).length())
        .collect();
    (xs, ys)
}

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));

    // Trails up to current t.
    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 trail_robot = sc::build_trail(
        track.samples(),
        |s: &DriftFullState| s.swerve.chassis.pos,
        idx,
        stride,
    );
    let trail_ghost = sc::build_trail(
        track.samples(),
        |s: &DriftFullState| s.ghost.pos,
        idx,
        stride,
    );
    sc::draw_trail(&cam, &trail_ghost, sc::GHOST_TRAIL_COL, true, &mut out);
    sc::draw_trail(&cam, &trail_robot, sc::TRAIL_COL, false, &mut out);

    // Ghost + robot.
    let size = Vec2::new(0.7, 0.7);
    sc::draw_ghost(&cam, state.ghost.pos, state.ghost.heading, size, &mut out);
    sc::draw_robot(&cam, &params, &state.swerve, size, &mut out);

    // Overlays: velocity (teal), acceleration (coral), per-wheel slip (red).
    let vel_scale = 0.5;
    let acc_scale = 0.1;
    let slip_scale = 0.15;
    sc::draw_overlay_arrow(
        &cam,
        state.swerve.chassis.pos,
        state.swerve.chassis.twist.v,
        vel_scale,
        2.0,
        sc::VEL_COL,
        &mut out,
    );
    sc::draw_overlay_arrow(
        &cam,
        state.swerve.chassis.pos,
        state.swerve.chassis.accel.a,
        acc_scale,
        2.0,
        sc::ACC_COL,
        &mut out,
    );
    for (i, m) in state.swerve.modules.iter().enumerate() {
        let cfg = &params.modules[i];
        let hub_world = state.swerve.chassis.pos + cfg.pos.rotate(state.swerve.chassis.heading);
        sc::draw_overlay_arrow(
            &cam,
            hub_world,
            m.slip_vel,
            slip_scale,
            1.5,
            sc::SLIP_COL,
            &mut out,
        );
    }

    // Drift vector ghost -> robot.
    let drift_screen = cam.m(state.swerve.chassis.pos) - cam.m(state.ghost.pos);
    out.extend(arrow(
        cam.m(state.ghost.pos),
        drift_screen,
        2.0,
        sc::DRIFT_COL,
    ));

    // Drift-magnitude chart along the bottom of the viewport.
    let (xs, ys) = drift_series(track);
    let (y_lo, y_hi) = auto_range(ys.iter().copied(), true);
    let chart = 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),
        ..LineChart::default()
    };
    out.extend(chart.render(&[Series {
        xs: &xs,
        ys: &ys,
        color: sc::DRIFT_COL,
        width: 2.0,
    }]));

    out
}

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

    #[test]
    fn drift_grows() {
        let track = bake();
        let s0 = track.lerp(0.0);
        let sf = track.lerp(DURATION);
        let d0 = (s0.swerve.chassis.pos - s0.ghost.pos).length();
        let df = (sf.swerve.chassis.pos - sf.ghost.pos).length();
        assert!(d0 < 1e-9, "drift at t=0 = {d0}");
        assert!(df > 0.25, "drift at end = {df} m, expected > 0.25 m");
    }

    #[test]
    fn scene_produces_finite_coordinates() {
        for i in 0..=60 {
            let t = DURATION * i as f64 / 60.0;
            let cmds = scene(t);
            assert!(!cmds.is_empty());
            for c in &cmds {
                match c {
                    Cmd::Line { a, b, .. } => {
                        for p in [a, b] {
                            assert!(p.x.is_finite() && p.y.is_finite(), "t={t}");
                        }
                    }
                    Cmd::FillCircle { center, .. } => {
                        assert!(center.x.is_finite() && center.y.is_finite());
                    }
                    Cmd::Stroke { path, .. } => {
                        for p in [path.p0, path.p1, path.p2, path.p3] {
                            assert!(p.x.is_finite() && p.y.is_finite());
                        }
                    }
                    Cmd::Polyline { pts, .. } | Cmd::FillPolygon { pts, .. } => {
                        for p in pts {
                            assert!(p.x.is_finite() && p.y.is_finite());
                        }
                    }
                }
            }
        }
    }
}
}