extern crate alloc;
extern crate std;
use alloc::format;
use alloc::string::{String, ToString};
use alloc::vec;
use alloc::vec::Vec;
use crate::datasets::{
aloha_static, aloha_static_pingpong_test, aloha_static_screw_driver, aloha_static_tape,
anymal_parkour, cheetah3, cwru, dlr_justin, droid, femto_st, icub3_sorrentino,
icub_pushrecovery, ims, kuka_lwr, mobile_aloha, openx, panda_gaz, so100, unitree_g1,
ur10_kufieta, DatasetFamily, DatasetId,
};
use crate::engine::DsfbRoboticsEngine;
use crate::envelope::AdmissibilityEnvelope;
use crate::platform::RobotContext;
use crate::Episode;
pub const PAPER_LOCK_W: usize = 8;
pub const PAPER_LOCK_K: usize = 4;
pub const PAPER_LOCK_BOUNDARY_FRAC: f64 = 0.5;
pub const PAPER_LOCK_DELTA_S: f64 = 0.05;
pub const PAPER_LOCK_VERSION: &str = "0.1.0";
pub const CRATE_VERSION: &str = env!("CARGO_PKG_VERSION");
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum Mode {
FixtureSmokeTest,
RealData,
}
impl Mode {
#[inline]
#[must_use]
pub const fn label(self) -> &'static str {
match self {
Self::FixtureSmokeTest => "fixture-smoke-test",
Self::RealData => "real-data",
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RunConfiguration {
#[cfg_attr(feature = "serde", serde(rename = "W"))]
pub w: usize,
#[cfg_attr(feature = "serde", serde(rename = "K"))]
pub k: usize,
pub boundary_frac: f64,
pub delta_s: f64,
}
impl RunConfiguration {
#[inline]
#[must_use]
pub const fn paper_lock() -> Self {
Self {
w: PAPER_LOCK_W,
k: PAPER_LOCK_K,
boundary_frac: PAPER_LOCK_BOUNDARY_FRAC,
delta_s: PAPER_LOCK_DELTA_S,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Aggregate {
pub total_samples: usize,
pub admissible: usize,
pub boundary: usize,
pub violation: usize,
pub compression_ratio: f64,
pub max_residual_norm_sq: f64,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize))]
pub struct ExplainEntry {
pub index: usize,
pub grammar: &'static str,
pub residual_norm_sq: f64,
pub drift: f64,
pub narrative: String,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize))]
pub struct PaperLockReport {
pub paper_lock_version: String,
pub crate_version: String,
pub dataset: String,
pub family: String,
pub mode: String,
pub run_configuration: RunConfiguration,
pub aggregate: Aggregate,
#[cfg_attr(feature = "serde", serde(skip_serializing_if = "Option::is_none"))]
pub trace: Option<Vec<Episode>>,
#[cfg_attr(feature = "serde", serde(skip_serializing_if = "Option::is_none"))]
pub explain: Option<Vec<ExplainEntry>>,
}
fn build_report(id: DatasetId, mode: Mode, residuals: &[f64], include_trace: bool) -> PaperLockReport {
debug_assert!(residuals.len() <= usize::MAX / 2, "residuals stream unreasonably large");
debug_assert!(matches!(mode, Mode::FixtureSmokeTest | Mode::RealData));
let envelope = calibrated_envelope(residuals);
let mut eng = DsfbRoboticsEngine::<PAPER_LOCK_W, PAPER_LOCK_K>::from_envelope(envelope);
let mut episodes = vec![Episode::empty(); residuals.len()];
let n = eng.observe(residuals, &mut episodes, RobotContext::ArmOperating);
debug_assert!(n <= episodes.len(), "engine wrote past output capacity");
let aggregate = aggregate_from_episodes(&episodes[..n]);
let trace = if include_trace { Some(episodes[..n].to_vec()) } else { None };
PaperLockReport {
paper_lock_version: PAPER_LOCK_VERSION.to_string(),
crate_version: CRATE_VERSION.to_string(),
dataset: id.slug().to_string(),
family: family_label(id.family()),
mode: mode.label().to_string(),
run_configuration: RunConfiguration::paper_lock(),
aggregate,
trace,
explain: None,
}
}
#[must_use]
pub fn build_explain(report: &PaperLockReport) -> Vec<ExplainEntry> {
let Some(trace) = &report.trace else {
return Vec::new();
};
let mut out = Vec::with_capacity(trace.len());
let n = trace.len();
let mut i = 0_usize;
while i < n {
let ep = &trace[i];
i += 1;
if ep.grammar == "Admissible" {
continue;
}
out.push(explain_entry_from_episode(ep));
}
out
}
fn explain_entry_from_episode(ep: &Episode) -> ExplainEntry {
let narrative = format!(
"Committed `{grammar}` at sample index {idx}: \u{2016}r\u{2016}\u{b2} = {nrm:.4}, \
drift = {drift:+.4}. Boundary triggered when the residual \
entered the (\u{3b2}\u{b7}\u{3c1}, \u{3c1}] band with sustained-outward-drift or \
abrupt-slew or recurrent-grazing structure; Violation \
triggered when the residual exceeded \u{3c1}. See companion paper \u{a7}4 to read \
the full grammar evaluator semantics.",
grammar = ep.grammar,
idx = ep.index,
nrm = ep.residual_norm_sq,
drift = ep.drift,
);
ExplainEntry {
index: ep.index,
grammar: ep.grammar,
residual_norm_sq: ep.residual_norm_sq,
drift: ep.drift,
narrative,
}
}
pub fn emit_review_csv(report: &PaperLockReport, path: &std::path::Path) -> std::io::Result<usize> {
use std::io::Write;
let mut f = std::fs::File::create(path)?;
writeln!(f, "index,residual_norm_sq,drift,grammar")?;
let mut rows = 0;
if let Some(trace) = &report.trace {
for ep in trace {
if ep.grammar == "Admissible" {
continue;
}
writeln!(
f,
"{},{:.17},{:.17},{}",
ep.index, ep.residual_norm_sq, ep.drift, ep.grammar
)?;
rows += 1;
}
}
Ok(rows)
}
fn aggregate_from_episodes(episodes: &[Episode]) -> Aggregate {
debug_assert!(episodes.len() <= usize::MAX / 2, "episode slice unreasonably large");
let n = episodes.len();
let mut admissible = 0_usize;
let mut boundary = 0_usize;
let mut violation = 0_usize;
let mut max_sq = 0.0_f64;
for e in episodes {
match e.grammar {
"Admissible" => admissible += 1,
"Boundary" => boundary += 1,
"Violation" => violation += 1,
other => {
debug_assert!(
matches!(other, "Admissible" | "Boundary" | "Violation"),
"unexpected grammar tag from engine: {other}"
);
}
}
if e.residual_norm_sq > max_sq {
max_sq = e.residual_norm_sq;
}
}
debug_assert_eq!(admissible + boundary + violation, n, "grammar-state census must sum to episode count");
debug_assert!(max_sq >= 0.0, "peak squared norm must be non-negative");
let reviewed = boundary + violation;
let compression_ratio = if n == 0 { 0.0 } else { reviewed as f64 / n as f64 };
debug_assert!((0.0..=1.0).contains(&compression_ratio), "compression out of [0,1]");
Aggregate {
total_samples: n,
admissible,
boundary,
violation,
compression_ratio,
max_residual_norm_sq: max_sq,
}
}
fn calibrated_envelope(residuals: &[f64]) -> AdmissibilityEnvelope {
debug_assert!(residuals.len() <= usize::MAX / 2);
if residuals.is_empty() {
return AdmissibilityEnvelope::new(f64::INFINITY);
}
let cal_len = (residuals.len() / 5).max(1).min(residuals.len());
debug_assert!(cal_len >= 1 && cal_len <= residuals.len());
let mut cal_buf = Vec::with_capacity(cal_len);
for &r in &residuals[..cal_len] {
if r.is_finite() {
cal_buf.push(crate::math::abs_f64(r));
}
}
AdmissibilityEnvelope::calibrate_from_window(&cal_buf)
.unwrap_or_else(|| AdmissibilityEnvelope::new(f64::INFINITY))
}
fn family_label(f: DatasetFamily) -> String {
f.label().to_string()
}
#[must_use]
pub fn run_fixture(id: DatasetId) -> PaperLockReport {
let residuals = fixture_residuals_for(id);
build_report(id, Mode::FixtureSmokeTest, &residuals, false)
}
#[must_use]
pub fn run_fixture_with_trace(id: DatasetId) -> PaperLockReport {
let residuals = fixture_residuals_for(id);
build_report(id, Mode::FixtureSmokeTest, &residuals, true)
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct RealDataUnavailable {
pub dataset: DatasetId,
pub expected_path: String,
pub instructions: String,
}
pub fn run_real_data(id: DatasetId) -> Result<PaperLockReport, RealDataUnavailable> {
run_real_data_with_trace(id, false)
}
pub fn run_real_data_with_trace(
id: DatasetId,
include_trace: bool,
) -> Result<PaperLockReport, RealDataUnavailable> {
let slug = id.slug();
debug_assert!(!slug.is_empty(), "DatasetId::slug must be non-empty");
debug_assert!(slug.len() < 64, "unexpectedly long slug suggests a bug");
let pub_path = std::path::PathBuf::from(format!("data/processed/{slug}_published.csv"));
let pub_alt =
std::path::PathBuf::from(format!("crates/dsfb-robotics/data/processed/{slug}_published.csv"));
let csv_path = std::path::PathBuf::from(format!("data/processed/{slug}.csv"));
let alt_path =
std::path::PathBuf::from(format!("crates/dsfb-robotics/data/processed/{slug}.csv"));
let (path, residual_definition): (std::path::PathBuf, &'static str) = if pub_path.is_file() {
(pub_path, "published-theta")
} else if pub_alt.is_file() {
(pub_alt, "published-theta")
} else if csv_path.is_file() {
(csv_path, "early-window-nominal")
} else if alt_path.is_file() {
(alt_path, "early-window-nominal")
} else {
return Err(RealDataUnavailable {
dataset: id,
expected_path: format!("data/processed/{slug}.csv"),
instructions: format!(
"Run `python3 scripts/preprocess_datasets.py --only {slug}` to \
generate the preprocessed residual-norm CSV from the raw \
dataset under docs/{slug}_oracle_protocol.md. paper-lock does \
not silently substitute fixture data for real data."
),
});
};
std::eprintln!(
"paper-lock: {slug} residual definition = {residual_definition} \
({})",
path.display()
);
let residuals = load_residual_csv(&path).map_err(|e| RealDataUnavailable {
dataset: id,
expected_path: path.to_string_lossy().into_owned(),
instructions: format!("failed to parse CSV: {e}"),
})?;
Ok(build_report(id, Mode::RealData, &residuals, include_trace))
}
pub fn run_real_data_with_csv_path(
id: DatasetId,
include_trace: bool,
path: &std::path::Path,
) -> Result<PaperLockReport, RealDataUnavailable> {
debug_assert!(!id.slug().is_empty(), "DatasetId::slug must be non-empty");
debug_assert!(path.as_os_str().len() < 4096, "path unreasonably long");
let residuals = load_residual_csv(path).map_err(|e| RealDataUnavailable {
dataset: id,
expected_path: path.to_string_lossy().into_owned(),
instructions: format!("failed to parse CSV: {e}"),
})?;
Ok(build_report(id, Mode::RealData, &residuals, include_trace))
}
fn load_residual_csv(path: &std::path::Path) -> Result<Vec<f64>, String> {
use std::io::Read;
debug_assert!(path.as_os_str().len() < 4096, "path unreasonably long");
debug_assert!(path.extension().is_some(), "residual CSV must have an extension");
let mut s = String::new();
std::fs::File::open(path)
.and_then(|mut f| f.read_to_string(&mut s))
.map_err(|e| format!("open {path:?}: {e}"))?;
debug_assert!(!s.is_empty(), "CSV read returned empty contents");
let mut out = Vec::new();
let mut lines = s.lines();
if let Some(first) = lines.next() {
if first.parse::<f64>().is_ok()
|| first.trim().eq_ignore_ascii_case("nan")
|| first.trim().eq_ignore_ascii_case("inf")
|| first.trim().eq_ignore_ascii_case("-inf")
{
out.push(parse_residual_token(first.trim())?);
}
}
for line in lines {
let token = line.trim();
if token.is_empty() {
continue;
}
out.push(parse_residual_token(token)?);
}
if out.is_empty() {
return Err("empty residual stream".to_string());
}
Ok(out)
}
fn parse_residual_token(token: &str) -> Result<f64, String> {
debug_assert!(!token.is_empty(), "caller must trim and guard empty tokens");
debug_assert!(token.len() < 64, "token unreasonably long");
match token.to_ascii_lowercase().as_str() {
"nan" => Ok(f64::NAN),
"inf" | "+inf" | "infinity" => Ok(f64::INFINITY),
"-inf" | "-infinity" => Ok(f64::NEG_INFINITY),
numeric => {
debug_assert!(!numeric.is_empty(), "numeric branch precondition");
token.parse::<f64>().map_err(|e| format!("parse {token:?}: {e}"))
}
}
}
fn fixture_residuals_for(id: DatasetId) -> Vec<f64> {
debug_assert!(!id.slug().is_empty(), "DatasetId must have a non-empty slug");
let mut buf = [0.0_f64; 16];
debug_assert_eq!(buf.len(), 16, "fixture buffer must size to 16 — see fixture-cap comment");
let n = match id {
DatasetId::Cwru => cwru::fixture_residuals(&mut buf),
DatasetId::Ims => ims::fixture_residuals(&mut buf),
DatasetId::KukaLwr => kuka_lwr::fixture_residuals(&mut buf),
DatasetId::FemtoSt => femto_st::fixture_residuals(&mut buf),
DatasetId::PandaGaz => panda_gaz::fixture_residuals(&mut buf),
DatasetId::DlrJustin => dlr_justin::fixture_residuals(&mut buf),
DatasetId::Ur10Kufieta => ur10_kufieta::fixture_residuals(&mut buf),
DatasetId::Cheetah3 => cheetah3::fixture_residuals(&mut buf),
DatasetId::IcubPushRecovery => icub_pushrecovery::fixture_residuals(&mut buf),
DatasetId::Droid => droid::fixture_residuals(&mut buf),
DatasetId::Openx => openx::fixture_residuals(&mut buf),
DatasetId::AnymalParkour => anymal_parkour::fixture_residuals(&mut buf),
DatasetId::UnitreeG1 => unitree_g1::fixture_residuals(&mut buf),
DatasetId::AlohaStatic => aloha_static::fixture_residuals(&mut buf),
DatasetId::Icub3Sorrentino => icub3_sorrentino::fixture_residuals(&mut buf),
DatasetId::MobileAloha => mobile_aloha::fixture_residuals(&mut buf),
DatasetId::So100 => so100::fixture_residuals(&mut buf),
DatasetId::AlohaStaticTape => aloha_static_tape::fixture_residuals(&mut buf),
DatasetId::AlohaStaticScrewDriver => aloha_static_screw_driver::fixture_residuals(&mut buf),
DatasetId::AlohaStaticPingpongTest => aloha_static_pingpong_test::fixture_residuals(&mut buf),
};
debug_assert!(n > 0, "every adapter must emit at least one fixture sample");
debug_assert!(n <= buf.len(), "fixture sample count must respect fixed buffer cap");
buf[..n].to_vec()
}
pub fn serialize_report(report: &PaperLockReport) -> Result<String, serde_json::Error> {
let mut s = serde_json::to_string_pretty(report)?;
s.push('\n');
Ok(s)
}
#[cfg(test)]
mod tests {
use super::*;
fn all_datasets() -> [DatasetId; 20] {
[
DatasetId::Cwru,
DatasetId::Ims,
DatasetId::KukaLwr,
DatasetId::FemtoSt,
DatasetId::PandaGaz,
DatasetId::DlrJustin,
DatasetId::Ur10Kufieta,
DatasetId::Cheetah3,
DatasetId::IcubPushRecovery,
DatasetId::Droid,
DatasetId::Openx,
DatasetId::AnymalParkour,
DatasetId::UnitreeG1,
DatasetId::AlohaStatic,
DatasetId::Icub3Sorrentino,
DatasetId::MobileAloha,
DatasetId::So100,
DatasetId::AlohaStaticTape,
DatasetId::AlohaStaticScrewDriver,
DatasetId::AlohaStaticPingpongTest,
]
}
#[test]
fn every_dataset_produces_a_non_empty_report() {
for id in all_datasets() {
let r = run_fixture(id);
assert_eq!(r.dataset, id.slug());
assert!(r.aggregate.total_samples > 0, "{} produced 0 samples", id.slug());
}
}
#[test]
fn run_configuration_is_canonical_paper_lock() {
for id in all_datasets() {
let r = run_fixture(id);
assert_eq!(r.run_configuration, RunConfiguration::paper_lock());
assert_eq!(r.run_configuration.w, PAPER_LOCK_W);
assert_eq!(r.run_configuration.k, PAPER_LOCK_K);
}
}
#[test]
fn fixture_runs_are_deterministic_across_invocations() {
for id in all_datasets() {
let r1 = run_fixture(id);
let r2 = run_fixture(id);
let r3 = run_fixture(id);
assert_eq!(r1, r2, "determinism drift for {}", id.slug());
assert_eq!(r2, r3, "determinism drift for {}", id.slug());
}
}
#[test]
fn aggregate_counts_add_to_total_samples() {
for id in all_datasets() {
let r = run_fixture(id);
let sum = r.aggregate.admissible + r.aggregate.boundary + r.aggregate.violation;
assert_eq!(sum, r.aggregate.total_samples, "counts drift for {}", id.slug());
}
}
#[test]
fn compression_ratio_in_unit_interval() {
for id in all_datasets() {
let r = run_fixture(id);
let c = r.aggregate.compression_ratio;
assert!((0.0..=1.0).contains(&c), "compression_ratio out of bounds for {}: {}", id.slug(), c);
}
}
#[test]
fn family_label_matches_datasetid_family() {
for id in all_datasets() {
let r = run_fixture(id);
assert_eq!(r.family, id.family().label());
}
}
#[test]
#[cfg_attr(miri, ignore = "Miri cannot model filesystem syscalls")]
fn real_data_path_produces_report_or_actionable_error() {
for id in all_datasets() {
match run_real_data(id) {
Ok(report) => {
assert_eq!(report.dataset, id.slug());
assert_eq!(report.mode, "real-data");
assert!(report.aggregate.total_samples > 0, "{} real-data report has 0 samples", id.slug());
}
Err(err) => {
assert_eq!(err.dataset, id);
assert!(err.expected_path.contains(id.slug()));
assert!(err.instructions.contains("preprocess"));
}
}
}
}
#[test]
fn run_fixture_omits_trace_by_default() {
for id in all_datasets() {
assert!(run_fixture(id).trace.is_none(), "{}: default run_fixture must not carry trace", id.slug());
}
}
#[test]
fn run_fixture_with_trace_matches_aggregate_counts() {
for id in all_datasets() {
let r = run_fixture_with_trace(id);
let trace = r.trace.clone().expect("trace requested");
assert_eq!(trace.len(), r.aggregate.total_samples, "{}: trace length disagrees with aggregate", id.slug());
let adm = trace.iter().filter(|e| e.grammar == "Admissible").count();
let bnd = trace.iter().filter(|e| e.grammar == "Boundary").count();
let vio = trace.iter().filter(|e| e.grammar == "Violation").count();
assert_eq!(adm, r.aggregate.admissible);
assert_eq!(bnd, r.aggregate.boundary);
assert_eq!(vio, r.aggregate.violation);
}
}
#[test]
fn trace_variant_is_deterministic_across_invocations() {
for id in all_datasets() {
let a = run_fixture_with_trace(id);
let b = run_fixture_with_trace(id);
assert_eq!(a, b, "{}: trace variant drifted across invocations", id.slug());
}
}
#[test]
fn serialized_report_is_byte_identical_across_runs() {
for id in all_datasets() {
let a = serialize_report(&run_fixture(id)).expect("valid JSON");
let b = serialize_report(&run_fixture(id)).expect("valid JSON");
assert_eq!(a, b, "JSON drift for {}", id.slug());
assert!(a.ends_with('\n'), "report must end with newline");
assert!(a.contains("\"paper_lock_version\": \"0.1.0\""), "version field missing");
}
}
}