255 lines
17 KiB
JavaScript
255 lines
17 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
|
|
const POSE_BYTES = 72;
|
|
const STATE_BYTES = 112;
|
|
const SOURCE_TP_SCHEMA = "linuxcnc_source_tp_state.v1";
|
|
|
|
function call(core, name, returnType, argTypes, args) {
|
|
const fn = core?.[`_${name}`] ?? core?.[name];
|
|
if (typeof fn === "function") return fn(...args);
|
|
if (typeof core?.ccall === "function") return core.ccall(name, returnType, argTypes, args);
|
|
throw new Error(`missing core export: ${name}`);
|
|
}
|
|
|
|
function poseArray(value = {}) { return ["x", "y", "z", "a", "b", "c", "u", "v", "w"].map((axis) => Number(value[axis] ?? 0)); }
|
|
function toPose(values) { return Object.fromEntries(["x", "y", "z", "a", "b", "c", "u", "v", "w"].map((axis, index) => [axis, Number(values[index] ?? 0)])); }
|
|
|
|
function cubicState() { return { x0: 0, x1: 0, x2: 0, x3: 0, t: 0, need: true, coeff: [0, 0, 0, 0] }; }
|
|
function cubicAdd(state, point, segmentTime = 0.001) {
|
|
if (!state.need) return;
|
|
state.x0 = state.x1; state.x1 = state.x2; state.x2 = state.x3; state.x3 = Number(point);
|
|
const wp0 = (state.x0 + 4 * state.x1 + state.x2) / 6;
|
|
const wp1 = (state.x1 + 4 * state.x2 + state.x3) / 6;
|
|
const vel0 = (state.x2 - state.x0) / (2 * segmentTime);
|
|
const vel1 = (state.x3 - state.x1) / (2 * segmentTime);
|
|
const b = 3 * (wp1 - wp0) / (segmentTime * segmentTime) - (2 * vel0 + vel1) / segmentTime;
|
|
const a = (vel1 - vel0) / (3 * segmentTime * segmentTime) - 2 * b / (3 * segmentTime);
|
|
state.coeff = [a, b, vel0, wp0]; state.t = 0; state.need = false;
|
|
}
|
|
function cubicInterpolate(state, segmentTime = 0.001) {
|
|
if (state.need) cubicAdd(state, state.x3, segmentTime);
|
|
const [a, b, c, d] = state.coeff;
|
|
const t = state.t;
|
|
const value = ((a * t + b) * t + c) * t + d;
|
|
state.t += segmentTime;
|
|
if (Math.abs(segmentTime - state.t) < segmentTime * 0.5) state.need = true;
|
|
return value;
|
|
}
|
|
|
|
export class WasmLinuxcncTp {
|
|
constructor(core, { cycleTime = 0.001, velocityLimit = 35, accelerationLimit = 300 } = {}) {
|
|
this.core = core;
|
|
this.available = ["sim_linuxcnc_tp_size", "sim_linuxcnc_tp_init", "sim_linuxcnc_tp_read_state", "sim_linuxcnc_tp_step", "sim_linuxcnc_tp_idle", "sim_linuxcnc_tp_add_line", "sim_linuxcnc_tp_set_profile", "sim_linuxcnc_tp_set_feed_override", "sim_linuxcnc_tp_set_dynamics", "sim_linuxcnc_tp_set_paused", "sim_linuxcnc_tp_abort", "sim_linuxcnc_tp_set_goal_position", "sim_linuxcnc_tp_queue_depth", "sim_linuxcnc_tp_queue_segment", "sim_linuxcnc_tp_restore_progress"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function");
|
|
this.cycleTime = cycleTime;
|
|
this.velocityLimit = velocityLimit;
|
|
this.accelerationLimit = accelerationLimit;
|
|
this.queueCapacity = 2000;
|
|
this.planner = 0;
|
|
this.posePointer = 0;
|
|
this.centerPointer = 0;
|
|
this.normalPointer = 0;
|
|
this.statePointer = 0;
|
|
this.history = [];
|
|
this.activeTag = 0;
|
|
this.segmentTags = new Map();
|
|
this.lastActiveId = 0;
|
|
// Task execution state is owned by the LinuxCNC task/TP handoff, not by
|
|
// the TP C struct. Keep it alongside the source-TP state so every servo
|
|
// sample carries the same WAITING_FOR_MOTION boundary as native LinuxCNC.
|
|
this.executionState = 3;
|
|
this.lastState = null;
|
|
this.queueDepthOverride = null;
|
|
this.kinematics = null;
|
|
this.cubic = Array.from({ length: 5 }, () => cubicState());
|
|
this.previousJointCommanded = [0, 0, 0, 0, 0];
|
|
this.controls = { profile: 0, lookahead: 50, acceleration_limit: accelerationLimit, jerk_limit: 1e9, feed_override: 1, paused: false };
|
|
if (!this.available || typeof core?._malloc !== "function") return;
|
|
this.planner = core._malloc(call(core, "sim_linuxcnc_tp_size", "number", [], []));
|
|
this.posePointer = core._malloc(POSE_BYTES);
|
|
this.centerPointer = core._malloc(24);
|
|
this.normalPointer = core._malloc(24);
|
|
this.statePointer = core._malloc(STATE_BYTES);
|
|
if (![this.planner, this.posePointer, this.centerPointer, this.normalPointer, this.statePointer].every(Boolean)) throw new Error("linuxcnc_tp_allocation_failed");
|
|
const result = call(core, "sim_linuxcnc_tp_init", "number", ["number", "number", "number", "number"], [this.planner, cycleTime, velocityLimit, accelerationLimit]);
|
|
if (result !== 0) throw new Error(`linuxcnc_tp_init_${result}`);
|
|
this.setPosition({ x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, u: 0, v: 0, w: 0 });
|
|
this.setProfile(0);
|
|
}
|
|
|
|
setPosition(value) {
|
|
if (!this.available) return this.read();
|
|
this.core.HEAPF64.set(poseArray(value), this.posePointer / 8);
|
|
const result = call(this.core, "sim_linuxcnc_tp_set_position", "number", ["number", "number"], [this.planner, this.posePointer]);
|
|
if (result !== 0) throw new Error(`linuxcnc_tp_set_position_${result}`);
|
|
return this.read();
|
|
}
|
|
|
|
setGoalPosition(value) {
|
|
if (!this.available) return this.read();
|
|
this.core.HEAPF64.set(poseArray(value), this.posePointer / 8);
|
|
const result = call(this.core, "sim_linuxcnc_tp_set_goal_position", "number", ["number", "number"], [this.planner, this.posePointer]);
|
|
if (result !== 0) throw new Error(`linuxcnc_tp_set_goal_position_${result}`);
|
|
return this.read();
|
|
}
|
|
|
|
setProfile(profile, tolerance = 0) {
|
|
const condition = profile === 0 ? 2 : profile === 1 ? 0 : profile === 2 ? 1 : -1;
|
|
if (![0, 1, 2].includes(profile) || !Number.isFinite(Number(tolerance)) || Number(tolerance) < 0) throw new Error("linuxcnc_tp_profile_invalid");
|
|
if (this.available) {
|
|
const result = call(this.core, "sim_linuxcnc_tp_set_profile", "number", ["number", "number", "number"], [this.planner, condition, Number(tolerance)]);
|
|
if (result !== 0) throw new Error(`linuxcnc_tp_profile_${result}`);
|
|
}
|
|
this.controls.profile = profile;
|
|
return this.read();
|
|
}
|
|
|
|
setTermCondition(mode, tolerance = 0) {
|
|
const profile = mode === "continuous" || mode === 0 ? 0 : mode === "exact_path" || mode === 1 ? 2 : 1;
|
|
return this.setProfile(profile, tolerance);
|
|
}
|
|
|
|
setLookahead(value) { if (!Number.isSafeInteger(value) || value < 1) throw new Error("linuxcnc_tp_lookahead_invalid"); this.controls.lookahead = value; return this.read(); }
|
|
setDynamics(accelerationLimit, jerkLimit) { this.accelerationLimit = Number(accelerationLimit); this.controls.acceleration_limit = this.accelerationLimit; this.controls.jerk_limit = Number(jerkLimit); if (this.available) call(this.core, "sim_linuxcnc_tp_set_dynamics", "number", ["number", "number"], [this.planner, this.accelerationLimit]); return this.read(); }
|
|
setFeedOverride(value) { this.controls.feed_override = Number(value); if (this.available) call(this.core, "sim_linuxcnc_tp_set_feed_override", "number", ["number", "number"], [this.planner, this.controls.feed_override]); return this.read(); }
|
|
setPaused(value) { this.controls.paused = Boolean(value); if (this.available) call(this.core, "sim_linuxcnc_tp_set_paused", "number", ["number", "number"], [this.planner, this.controls.paused ? 1 : 0]); return this.read(); }
|
|
|
|
enqueue(segment) {
|
|
if (!this.available || !segment) return { status: "skipped", code: "linuxcnc_tp_unavailable" };
|
|
this.core.HEAPF64.set(poseArray(segment.end), this.posePointer / 8);
|
|
const canonMotionType = Number(segment.canon_motion_type ?? (segment.motion_type === 2 ? 3 : segment.kind === "rapid" ? 1 : 2));
|
|
this.activeTag = Number(segment.kinematics_mode ?? segment.tag ?? 0);
|
|
const segmentTag = this.activeTag;
|
|
const segmentId = Number(segment.source_line ?? 0);
|
|
if (segmentId > 0) this.segmentTags.set(segmentId, segmentTag);
|
|
const acceleration = Math.max(Number(segment.acceleration ?? 0), this.accelerationLimit);
|
|
const result = segment.motion_type === 2
|
|
? (this.core.HEAPF64.set([Number(segment.center?.x ?? 0), Number(segment.center?.y ?? 0), Number(segment.center?.z ?? 0)], this.centerPointer / 8), this.core.HEAPF64.set([Number(segment.normal?.x ?? 0), Number(segment.normal?.y ?? 0), Number(segment.normal?.z ?? 0)], this.normalPointer / 8), call(this.core, "sim_linuxcnc_tp_add_circle", "number", ["number", "number", "number", "number", "number", "number", "number", "number", "number"], [this.planner, this.posePointer, this.centerPointer, this.normalPointer, Number(segment.turn ?? 1), Number(segment.velocity ?? 0), acceleration, Number(segment.source_line ?? 0), canonMotionType]))
|
|
: call(this.core, "sim_linuxcnc_tp_add_line", "number", ["number", "number", "number", "number", "number", "number"], [this.planner, this.posePointer, Number(segment.velocity ?? 0), acceleration, Number(segment.source_line ?? 0), canonMotionType]);
|
|
if (result !== 0) return { status: "error", code: `linuxcnc_tp_enqueue_${result}` };
|
|
if (this.lastState?.active_id && Number(this.lastState.active_id) !== segmentId) this.activeTag = Number(this.lastState.active_tag ?? this.activeTag);
|
|
return { status: "verified", source_line: segment.source_line ?? null };
|
|
}
|
|
|
|
setKinematics(kinematics) { this.kinematics = kinematics ?? null; return this.read(); }
|
|
|
|
setActiveKinematicsTag(mode) {
|
|
this.activeTag = Number(mode) === 0 ? 0 : Number(mode);
|
|
return this.read();
|
|
}
|
|
|
|
setExecutionState(value) {
|
|
const state = Number(value);
|
|
if (![2, 3, 4].includes(state)) throw new Error("linuxcnc_tp_execution_state_invalid");
|
|
this.executionState = state;
|
|
// The next source TP step publishes the state. Avoid a redundant C read
|
|
// on every servo tick merely to attach this task/TP handoff marker.
|
|
return { status: "verified", state: this.lastState };
|
|
}
|
|
|
|
queueDepth() { return Number(this.lastState?.queue_depth ?? 0); }
|
|
|
|
setQueueDepthOverride(value) {
|
|
this.queueDepthOverride = value === null ? null : Math.max(0, Number(value));
|
|
if (this.lastState && this.queueDepthOverride !== null) this.lastState.queue_depth = this.queueDepthOverride;
|
|
return this.read();
|
|
}
|
|
|
|
commandedJoints(position, activeTag) {
|
|
if (Number(activeTag) === 0) return [position.x, position.y, position.z, position.a ?? 0, position.c ?? 0];
|
|
const inverse = this.kinematics?.inverseMode?.(Number(activeTag), position) ?? this.kinematics?.inverse?.(position);
|
|
if (inverse?.status === "verified" && Array.isArray(inverse.joints)) return inverse.joints.slice(0, 5);
|
|
return this.previousJointCommanded.slice();
|
|
}
|
|
|
|
step() {
|
|
if (!this.available) return { status: "blocked", code: "linuxcnc_tp_unavailable" };
|
|
if (this.controls.paused) return { status: "error", code: "linuxcnc_tp_paused" };
|
|
const result = call(this.core, "sim_linuxcnc_tp_step", "number", ["number"], [this.planner]);
|
|
if (result !== 0) return { status: "error", code: `linuxcnc_tp_step_${result}` };
|
|
const state = this.read();
|
|
if (state.status === "verified") {
|
|
const targetJoints = this.commandedJoints(state.state.position, state.state.active_tag);
|
|
targetJoints.forEach((point, index) => cubicAdd(this.cubic[index], point, this.cycleTime));
|
|
const commanded = this.cubic.map((axis) => cubicInterpolate(axis, this.cycleTime));
|
|
state.state.joint_commanded = commanded;
|
|
state.state.joint_actual = this.previousJointCommanded.slice();
|
|
this.previousJointCommanded = commanded.slice();
|
|
}
|
|
if (state.status === "verified") this.history.push(structuredClone(state.state));
|
|
return state;
|
|
}
|
|
|
|
idle() {
|
|
if (!this.available) return this.read();
|
|
const result = call(this.core, "sim_linuxcnc_tp_idle", "number", ["number"], [this.planner]);
|
|
if (result !== 0) return { status: "error", code: `linuxcnc_tp_idle_${result}` };
|
|
const state = this.read();
|
|
if (state.status === "verified") {
|
|
const targetJoints = this.commandedJoints(state.state.position, state.state.active_tag);
|
|
targetJoints.forEach((point, index) => cubicAdd(this.cubic[index], point, this.cycleTime));
|
|
const commanded = this.cubic.map((axis) => cubicInterpolate(axis, this.cycleTime));
|
|
state.state.joint_commanded = commanded;
|
|
state.state.joint_actual = this.previousJointCommanded.slice();
|
|
this.previousJointCommanded = commanded.slice();
|
|
}
|
|
if (state.status === "verified") this.history.push(structuredClone(state.state));
|
|
return state;
|
|
}
|
|
|
|
drainTrace() { const trace = this.history; this.history = []; return trace; }
|
|
|
|
read() {
|
|
if (!this.available) return { status: "blocked", state: { position: toPose([]), velocity: 0, progress: 0, tick: 0, queue_depth: 0, active_id: 0, active_tag: 0, motion_type: 0, status: -1214, exec_state: this.executionState, source: "linuxcnc-source-tp" } };
|
|
const result = call(this.core, "sim_linuxcnc_tp_read_state", "number", ["number", "number"], [this.planner, this.statePointer]);
|
|
if (result !== 0) return { status: "error", code: `linuxcnc_tp_read_${result}` };
|
|
const doubles = this.core.HEAPF64.subarray(this.statePointer / 8, this.statePointer / 8 + 11);
|
|
const ints = this.core.HEAPU32.subarray(this.statePointer / 4 + 22, this.statePointer / 4 + 25);
|
|
const activeId = ints[2];
|
|
if (activeId && activeId !== this.lastActiveId && this.segmentTags.has(activeId)) this.activeTag = this.segmentTags.get(activeId);
|
|
this.lastActiveId = activeId;
|
|
const state = { position: toPose(doubles), velocity: doubles[9], acceleration: this.accelerationLimit, progress: doubles[10], tick: ints[0], queue_depth: this.queueDepthOverride === null ? ints[1] : this.queueDepthOverride, active_queue: activeId ? 1 : 0, active_id: activeId, active_tag: this.activeTag, joint_commanded: this.previousJointCommanded.slice(), joint_actual: this.previousJointCommanded.slice(), motion_type: this.core.HEAP32[this.statePointer / 4 + 25], status: this.core.HEAP32[this.statePointer / 4 + 26], exec_state: this.executionState, source: "linuxcnc-source-tp" };
|
|
this.lastState = state;
|
|
return { status: "verified", state };
|
|
}
|
|
|
|
reset() { if (this.available) call(this.core, "sim_linuxcnc_tp_reset", "number", ["number"], [this.planner]); this.controls.paused = false; this.history = []; this.activeTag = 0; this.segmentTags.clear(); this.lastActiveId = 0; this.queueDepthOverride = null; this.executionState = 3; this.cubic = Array.from({ length: 5 }, () => cubicState()); this.previousJointCommanded = [0, 0, 0, 0, 0]; return this.read(); }
|
|
abort() { if (this.available) call(this.core, "sim_linuxcnc_tp_abort", "number", ["number"], [this.planner]); this.history = []; this.activeTag = 0; this.segmentTags.clear(); this.lastActiveId = 0; this.queueDepthOverride = null; this.executionState = 3; return this.read(); }
|
|
exportState() {
|
|
const current = this.read();
|
|
const segments = [];
|
|
if (this.available) {
|
|
const depth = Number(call(this.core, "sim_linuxcnc_tp_queue_depth", "number", ["number"], [this.planner]));
|
|
const size = 240;
|
|
const pointer = this.core._malloc(size);
|
|
try {
|
|
for (let index = 0; index < depth; index += 1) {
|
|
if (call(this.core, "sim_linuxcnc_tp_queue_segment", "number", ["number", "number", "number"], [this.planner, index, pointer]) !== 0) continue;
|
|
const values = this.core.HEAPF64.subarray(pointer / 8, pointer / 8 + 29);
|
|
const ints = this.core.HEAP32.subarray(pointer / 4 + 52, pointer / 4 + 54);
|
|
const segmentView = new DataView(this.core.HEAPU8.buffer, pointer, size);
|
|
const id = this.core.HEAPU32[pointer / 4 + 54];
|
|
segments.push({ start: toPose(values), end: toPose(values.subarray(9)), center: { x: values[18], y: values[19], z: values[20] }, normal: { x: values[21], y: values[22], z: values[23] }, velocity: values[24], acceleration: values[25], motion_type: ints[0], turn: ints[1], id, kinematics_mode: this.segmentTags.get(id) ?? this.activeTag, term_cond: this.core.HEAP32[pointer / 4 + 55], progress: segmentView.getFloat64(224, true), target: segmentView.getFloat64(232, true) });
|
|
}
|
|
} finally { this.core._free(pointer); }
|
|
}
|
|
const unsigned = { schema: SOURCE_TP_SCHEMA, cycle_time: this.cycleTime, velocity_limit: this.velocityLimit, controls: { ...this.controls }, execution_state: this.executionState, active_tag: this.activeTag, state: current.state, segments };
|
|
return { ...unsigned, state_hash: fnv1a64(unsigned) };
|
|
}
|
|
restoreState(value) {
|
|
if (!value || value.schema !== SOURCE_TP_SCHEMA || value.cycle_time !== this.cycleTime || value.velocity_limit !== this.velocityLimit || value.state_hash !== fnv1a64(Object.fromEntries(Object.entries(value).filter(([key]) => key !== "state_hash")))) throw new Error("linuxcnc_tp_restore_schema");
|
|
this.reset();
|
|
this.executionState = Number(value.execution_state ?? 3);
|
|
this.setProfile(value.controls?.profile ?? 0, value.controls?.tolerance ?? 0);
|
|
this.setFeedOverride(value.controls?.feed_override ?? 1);
|
|
this.setPosition(value.state?.position ?? {});
|
|
for (const segment of value.segments ?? []) this.enqueue({ ...segment, kinematics_mode: segment.kinematics_mode ?? value.active_tag ?? 0 });
|
|
if (this.available && value.state) {
|
|
this.core.HEAPF64.set(poseArray(value.state.position), this.posePointer / 8);
|
|
const restored = call(this.core, "sim_linuxcnc_tp_restore_progress", "number", ["number", "number", "number", "number"], [this.planner, this.posePointer, Number(value.state.velocity ?? 0), Number(value.state.tick ?? 0)]);
|
|
if (restored !== 0) throw new Error(`linuxcnc_tp_restore_progress_${restored}`);
|
|
}
|
|
return this.read();
|
|
}
|
|
destroy() { for (const pointer of [this.planner, this.posePointer, this.centerPointer, this.normalPointer, this.statePointer]) if (pointer && typeof this.core?._free === "function") this.core._free(pointer); this.planner = this.posePointer = this.centerPointer = this.normalPointer = this.statePointer = 0; }
|
|
}
|