Implement AXIS parity UI and execution baseline
This commit is contained in:
206
web/worker/authoritative-runtime.mjs
Normal file
206
web/worker/authoritative-runtime.mjs
Normal file
@@ -0,0 +1,206 @@
|
||||
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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user