207 lines
9.1 KiB
JavaScript
207 lines
9.1 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
|
|
export const AUTHORITATIVE_RUNTIME_SCHEMA = "sim_authoritative_runtime.v1";
|
|
const AXES = Object.freeze(["x", "y", "z"]);
|
|
const STEP_FRACTION = 0.25;
|
|
const KINEMATICS = new Set(["trivkins", "corexy", "tripod", "lineardelta", "rotarydelta"]);
|
|
const KINEMATICS_ALIASES = new Map([["corexykins", "corexy"], ["tripodkins", "tripod"], ["lineardeltakins", "lineardelta"], ["rotarydeltakins", "rotarydelta"]]);
|
|
const TRIPOD_BASE = Object.freeze([[0, 0], [10, 0], [5, 7.071]]);
|
|
const LINEAR_DELTA_RADIUS = 130.25;
|
|
const LINEAR_DELTA_ROD = 269;
|
|
const ROTARY_DELTA_PLATFORM = 10;
|
|
const ROTARY_DELTA_THIGH = 10;
|
|
const ROTARY_DELTA_SHIN = 14;
|
|
const ROTARY_DELTA_FOOT = 6;
|
|
const DEG_TO_RAD = Math.PI / 180;
|
|
const RAD_TO_DEG = 180 / Math.PI;
|
|
|
|
function clone(value) { return structuredClone(value); }
|
|
function finite(value) { return typeof value === "number" && Number.isFinite(value); }
|
|
function poseFromProgram(program) {
|
|
const state = program?.state;
|
|
if (!state || !AXES.every((axis) => finite(state[axis]))) return null;
|
|
return { x: state.x, y: state.y, z: state.z };
|
|
}
|
|
function samePose(left, right) { return left && right && AXES.every((axis) => left[axis] === right[axis]); }
|
|
function tripodJoints(pose) {
|
|
return TRIPOD_BASE.map(([x, y]) => Math.sqrt((pose.x - x) ** 2 + (pose.y - y) ** 2 + pose.z ** 2));
|
|
}
|
|
function linearDeltaJoints(pose) {
|
|
const radius = LINEAR_DELTA_RADIUS;
|
|
const rod = LINEAR_DELTA_ROD;
|
|
const bases = [[0, radius], [-Math.sqrt(3) * 0.5 * radius, -0.5 * radius], [Math.sqrt(3) * 0.5 * radius, -0.5 * radius]];
|
|
const joints = bases.map(([x, y]) => {
|
|
const radicand = rod * rod - (x - pose.x) ** 2 - (y - pose.y) ** 2;
|
|
return radicand < -1e-12 ? null : pose.z + Math.sqrt(Math.max(0, radicand));
|
|
});
|
|
return joints.some((value) => value === null || !finite(value)) ? null : joints;
|
|
}
|
|
function rotaryDeltaJoints(pose) {
|
|
if (Math.abs(pose.z) < 1e-12) return null;
|
|
const solve = (x, y) => {
|
|
const pfr = ROTARY_DELTA_PLATFORM; const tl = ROTARY_DELTA_THIGH; const sl = ROTARY_DELTA_SHIN; const fr = ROTARY_DELTA_FOOT;
|
|
const a = 0.5 * (x * x + (y - fr) ** 2 + pose.z ** 2 + tl ** 2 - sl ** 2 - pfr ** 2) / pose.z;
|
|
const b = (fr - pfr - y) / pose.z;
|
|
const d = tl ** 2 * (b ** 2 + 1) - (a - b * pfr) ** 2;
|
|
if (d < -1e-12) return null;
|
|
const kneeY = (pfr + a * b + Math.sqrt(Math.max(0, d))) / (b ** 2 + 1);
|
|
const kneeZ = b * kneeY - a;
|
|
return Math.atan2(kneeZ, kneeY - pfr) * RAD_TO_DEG;
|
|
};
|
|
const c = Math.cos(-2 * Math.PI / 3); const s = Math.sin(-2 * Math.PI / 3);
|
|
const c2 = Math.cos(2 * Math.PI / 3); const s2 = Math.sin(2 * Math.PI / 3);
|
|
const leg0 = solve(pose.x, pose.y);
|
|
const leg1 = solve(pose.x * c - pose.y * s, pose.x * s + pose.y * c);
|
|
const leg2 = solve(pose.x * c2 - pose.y * s2, pose.x * s2 + pose.y * c2);
|
|
return [leg0, leg1, leg2].some((value) => value === null || !finite(value)) ? null : [leg0, leg1, leg2];
|
|
}
|
|
function jointsFor(type, pose) {
|
|
if (type === "corexy") return [pose.x + pose.y, pose.x - pose.y, pose.z];
|
|
if (type === "tripod") return tripodJoints(pose);
|
|
if (type === "lineardelta") return linearDeltaJoints(pose);
|
|
if (type === "rotarydelta") return rotaryDeltaJoints(pose);
|
|
return [pose.x, pose.y, pose.z];
|
|
}
|
|
function mappingError(type, pose) { return type === "rotarydelta" && Math.abs(pose.z) < 1e-12 ? "authoritative_kinematics_singular" : "authoritative_kinematics_range"; }
|
|
function canonicalKinematicsType(type) { return KINEMATICS_ALIASES.get(type) ?? type; }
|
|
|
|
export class AuthoritativeRuntime {
|
|
constructor() { this.reset(); }
|
|
|
|
reset() {
|
|
const kinematicsType = this.state?.kinematics?.type ?? "trivkins";
|
|
this.state = {
|
|
schema: AUTHORITATIVE_RUNTIME_SCHEMA,
|
|
tick: 0,
|
|
status: "idle",
|
|
program_status: "idle",
|
|
kinematics: { type: kinematicsType, joint_count: 3, pose: { x: 0, y: 0, z: 0 }, joints: [0, 0, 0], status: 0, blocked: null },
|
|
planner: { next_id: 1, queue: [], active: null, active_id: null, progress: 0, queue_depth: 0, last_error: 0 },
|
|
};
|
|
return this.export();
|
|
}
|
|
|
|
export() {
|
|
const unsigned = clone(this.state);
|
|
return { ...unsigned, state_hash: fnv1a64(unsigned) };
|
|
}
|
|
|
|
hash() { return this.export().state_hash; }
|
|
|
|
setKinematics(type) {
|
|
const selected = canonicalKinematicsType(type);
|
|
if (!KINEMATICS.has(selected)) {
|
|
this.state.kinematics.blocked = "authoritative_kinematics_unsupported";
|
|
return { status: "blocked", code: "authoritative_kinematics_unsupported", type };
|
|
}
|
|
this.state.kinematics.type = selected;
|
|
this.state.kinematics.blocked = null;
|
|
const mapped = jointsFor(selected, this.state.kinematics.pose);
|
|
if (!mapped && selected === "rotarydelta" && Math.abs(this.state.kinematics.pose.z) < 1e-12) {
|
|
this.state.kinematics.type = selected;
|
|
this.state.kinematics.blocked = null;
|
|
this.state.kinematics.joints = [0, 0, 0];
|
|
return { status: "verified", type: selected };
|
|
}
|
|
if (!mapped) {
|
|
const code = mappingError(selected, this.state.kinematics.pose);
|
|
this.state.kinematics.blocked = code;
|
|
return { status: "blocked", code, type: selected };
|
|
}
|
|
this.state.kinematics.joints = mapped;
|
|
return { status: "verified", type: selected };
|
|
}
|
|
|
|
syncProgram(program) {
|
|
const target = poseFromProgram(program);
|
|
if (!target) return { status: "blocked", code: "authoritative_program_pose_missing" };
|
|
this.state.program_status = program.program_status ?? "idle";
|
|
if (this.state.program_status === "aborted") {
|
|
this.state.status = "aborted";
|
|
this.state.planner.queue = [];
|
|
this.state.planner.active_id = null;
|
|
this.state.planner.progress = 0;
|
|
this.state.planner.queue_depth = 0;
|
|
return { status: "verified", command: "AUTHORITATIVE_ABORT" };
|
|
}
|
|
if (this.state.program_status === "completed") {
|
|
this.state.status = "completed";
|
|
return { status: "verified", command: "AUTHORITATIVE_COMPLETE", queue_depth: this.state.planner.queue_depth };
|
|
}
|
|
const planner = this.state.planner;
|
|
const last = planner.queue.at(-1)?.end ?? (planner.active_id === null ? this.state.kinematics.pose : null);
|
|
if (!samePose(target, last)) {
|
|
const id = planner.next_id++;
|
|
planner.queue.push({ id, source_line: program.line ?? null, start: clone(this.state.kinematics.pose), end: target });
|
|
}
|
|
planner.queue_depth = planner.queue.length + (planner.active_id === null ? 0 : 1);
|
|
this.state.status = this.state.program_status === "completed" ? "completed" : "running";
|
|
return { status: "verified", command: "AUTHORITATIVE_SYNC", queue_depth: planner.queue_depth };
|
|
}
|
|
|
|
abort() {
|
|
this.state.status = "aborted";
|
|
this.state.program_status = "aborted";
|
|
this.state.planner.queue = [];
|
|
this.state.planner.active = null;
|
|
this.state.planner.active_id = null;
|
|
this.state.planner.progress = 0;
|
|
this.state.planner.queue_depth = 0;
|
|
return this.export();
|
|
}
|
|
|
|
step(tick) {
|
|
if (!Number.isSafeInteger(tick) || tick < this.state.tick) return { status: "error", code: "authoritative_tick_regression" };
|
|
const planner = this.state.planner;
|
|
for (let current = this.state.tick + 1; current <= tick; current += 1) {
|
|
if (planner.active_id === null && planner.queue.length > 0) {
|
|
const next = planner.queue.shift();
|
|
planner.active_id = next.id;
|
|
planner.active = next;
|
|
planner.progress = 0;
|
|
}
|
|
if (planner.active_id !== null) {
|
|
planner.progress = Math.min(1, planner.progress + STEP_FRACTION);
|
|
const active = planner.active;
|
|
const pose = {};
|
|
for (const axis of AXES) pose[axis] = active.start[axis] + (active.end[axis] - active.start[axis]) * planner.progress;
|
|
this.state.kinematics.pose = pose;
|
|
const mapped = jointsFor(this.state.kinematics.type, pose);
|
|
if (!mapped) {
|
|
const code = mappingError(this.state.kinematics.type, pose);
|
|
this.state.kinematics.blocked = code;
|
|
return { status: "error", code };
|
|
}
|
|
this.state.kinematics.joints = mapped;
|
|
if (planner.progress >= 1) {
|
|
planner.active_id = null;
|
|
planner.active = null;
|
|
planner.progress = 0;
|
|
}
|
|
}
|
|
planner.queue_depth = planner.queue.length + (planner.active_id === null ? 0 : 1);
|
|
this.state.tick = current;
|
|
}
|
|
return { status: "verified", state: this.export() };
|
|
}
|
|
|
|
applyMotion(motion) {
|
|
if (!motion?.state) return;
|
|
const pose = this.state.kinematics.pose;
|
|
motion.state.axes = { ...pose };
|
|
motion.state.joints = [...this.state.kinematics.joints];
|
|
motion.state.tick = this.state.tick;
|
|
motion.state.velocity = this.state.planner.active_id === null ? 0 : STEP_FRACTION;
|
|
}
|
|
|
|
restore(value) {
|
|
if (!value || value.schema !== AUTHORITATIVE_RUNTIME_SCHEMA) throw new Error("authoritative_restore_schema");
|
|
const unsigned = clone(value);
|
|
delete unsigned.state_hash;
|
|
if (value.state_hash !== fnv1a64(unsigned)) throw new Error("authoritative_restore_hash");
|
|
if (!KINEMATICS.has(unsigned.kinematics?.type) || unsigned.kinematics?.joint_count !== 3) throw new Error("authoritative_restore_kinematics");
|
|
this.state = unsigned;
|
|
return this.export();
|
|
}
|
|
}
|