316 lines
23 KiB
JavaScript
316 lines
23 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
import { exportNativeCheckpoint, restoreNativeCheckpoint } from "./native-checkpoint.mjs";
|
|
import { WEB_SETTINGS } from "../workspace/web-settings.mjs";
|
|
const DEFAULTS = WEB_SETTINGS.runtime.planner;
|
|
|
|
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)])); }
|
|
|
|
export class WasmLinuxcncTp {
|
|
constructor(core, { cycleTime = DEFAULTS.cycle_time, velocityLimit = DEFAULTS.velocity_limit, accelerationLimit = DEFAULTS.acceleration_limit, limits = {}, axisVelocities = {}, axisAccelerations = {}, maxFeedOverride = DEFAULTS.max_feed_override, blend = DEFAULTS.blend } = {}) {
|
|
this.core = core;
|
|
this.available = ["sim_linuxcnc_tp_size", "sim_linuxcnc_tp_queue_capacity", "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_velocity_scale", "sim_linuxcnc_tp_set_jog_scale", "sim_linuxcnc_tp_set_axis_limit", "sim_linuxcnc_tp_jog", "sim_linuxcnc_tp_velocity_scale", "sim_linuxcnc_tp_velocity_limit", "sim_linuxcnc_tp_velocity_mm_min", "sim_linuxcnc_tp_jog_scale", "sim_linuxcnc_tp_jog_velocity", "sim_linuxcnc_tp_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 = 0;
|
|
this.planner = 0;
|
|
this.posePointer = 0;
|
|
this.centerPointer = 0;
|
|
this.normalPointer = 0;
|
|
this.statePointer = 0;
|
|
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.controls = { profile: 0, lookahead: blend.optimization_depth, acceleration_limit: accelerationLimit, jerk_limit: DEFAULTS.compatibility_jerk_limit, feed_override: 1, velocity_scale: 1, jog_scale: 1, paused: false };
|
|
this.maxFeedOverride = maxFeedOverride;
|
|
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.queueCapacity = Number(call(core, "sim_linuxcnc_tp_queue_capacity", "number", ["number"], [this.planner]));
|
|
if (!Number.isSafeInteger(this.queueCapacity) || this.queueCapacity < 1) throw new Error("linuxcnc_tp_queue_capacity_invalid");
|
|
this.setPosition({ x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, u: 0, v: 0, w: 0 });
|
|
this.setProfile(0);
|
|
for (const [axis, limit] of Object.entries(limits)) {
|
|
const index = { x: 0, y: 1, z: 2, a: 3, b: 4, c: 5, u: 6, v: 7, w: 8 }[axis];
|
|
if (index !== undefined && Array.isArray(limit) && limit.length === 2) this.setAxisLimit(index, Number(limit[0]), Number(limit[1]));
|
|
}
|
|
for (const [axis, velocity] of Object.entries(axisVelocities)) {
|
|
const index = "xyzabcuvw".indexOf(axis);
|
|
const configured = call(core, "sim_linuxcnc_tp_set_axis_dynamics", "number", ["number", "number", "number", "number"], [this.planner, index, velocity, axisAccelerations[axis]]);
|
|
if (configured !== 0) throw new Error(`linuxcnc_tp_axis_dynamics_${axis}_${configured}`);
|
|
}
|
|
const feedLimit = call(core, "sim_linuxcnc_tp_set_feed_limit", "number", ["number", "number"], [this.planner, maxFeedOverride]);
|
|
if (feedLimit !== 0) throw new Error(`linuxcnc_tp_feed_limit_${feedLimit}`);
|
|
const scale = call(core, "sim_linuxcnc_tp_set_scale_minimum", "number", ["number", "number"], [this.planner, WEB_SETTINGS.ui.motion_scale_min]);
|
|
if (scale !== 0) throw new Error(`linuxcnc_tp_scale_minimum_${scale}`);
|
|
const blending = call(core, "sim_linuxcnc_tp_set_blend", "number", Array(7).fill("number"), [this.planner, blend.enable, blend.fallback_enable, blend.optimization_depth, blend.gap_cycles, blend.ramp_frequency, blend.tangent_kink_ratio]);
|
|
if (blending !== 0) throw new Error(`linuxcnc_tp_blend_${blending}`);
|
|
}
|
|
|
|
setPosition(value, kinematicsMode = this.activeTag) {
|
|
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}`);
|
|
// Publish the pose and its coordinate interpretation together. Reading
|
|
// between these updates would inverse-solve an old pose in a new mode.
|
|
this.activeTag = Number(kinematicsMode);
|
|
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(); }
|
|
setVelocityScale(value) { if (!this.available) return this.read(); const result = call(this.core, "sim_linuxcnc_tp_set_velocity_scale", "number", ["number", "number"], [this.planner, Number(value)]); if (result !== 0) throw new Error(`linuxcnc_tp_velocity_scale_${result}`); return this.read(); }
|
|
setJogScale(value) { if (!this.available) return this.read(); const result = call(this.core, "sim_linuxcnc_tp_set_jog_scale", "number", ["number", "number"], [this.planner, Number(value)]); if (result !== 0) throw new Error(`linuxcnc_tp_jog_scale_${result}`); return this.read(); }
|
|
setAxisLimit(axis, minimum, maximum) { if (!this.available) return this.read(); const result = call(this.core, "sim_linuxcnc_tp_set_axis_limit", "number", ["number", "number", "number", "number"], [this.planner, Number(axis), Number(minimum), Number(maximum)]); if (result !== 0) throw new Error(`linuxcnc_tp_axis_limit_${result}`); return this.read(); }
|
|
jog(axis, direction, increment, tag = 0) {
|
|
if (!this.available) return { status: "blocked", code: "linuxcnc_tp_unavailable" };
|
|
const coordinate = "xyzabcuvw"[axis];
|
|
// Identity and rotary coordinates map directly to physical joints. Apply
|
|
// both INI constraints to the native jog target without changing normal
|
|
// coordinate limits (RTCP XYZ has a different physical interpretation).
|
|
const jointLimit = this.kinematics?.options?.limits?.[coordinate];
|
|
const directJoint = this.activeTag !== 1 || axis >= 3;
|
|
let savedLimits = null;
|
|
if (directJoint && jointLimit) {
|
|
savedLimits = [0, 1].map((maximum) => call(this.core, "sim_linuxcnc_tp_axis_limit", "number", ["number", "number", "number"], [this.planner, axis, maximum]));
|
|
this.setAxisLimit(axis, Math.max(savedLimits[0], jointLimit[0]), Math.min(savedLimits[1], jointLimit[1]));
|
|
}
|
|
let result;
|
|
try { result = call(this.core, "sim_linuxcnc_tp_jog", "number", ["number", "number", "number", "number", "number"], [this.planner, Number(axis), Number(direction), Number(increment), Number(tag)]); }
|
|
finally { if (savedLimits) this.setAxisLimit(axis, ...savedLimits); }
|
|
if (result !== 0) return { status: "error", code: `linuxcnc_tp_jog_${result}` };
|
|
// The native jog tag is a segment ID, not a kinematics mode. Replace any
|
|
// earlier program-line mapping with this jog's current coordinate frame.
|
|
if (Number(tag) > 0) this.segmentTags.set(Number(tag), this.activeTag);
|
|
return { status: "verified", command: "JOG_START" };
|
|
}
|
|
jogVelocity(axis) { if (!this.available) return 0; const index = { x: 0, y: 1, z: 2, a: 3, b: 4, c: 5, u: 6, v: 7, w: 8 }[String(axis).toLowerCase()]; return index === undefined ? 0 : Number(call(this.core, "sim_linuxcnc_tp_jog_velocity", "number", ["number", "number"], [this.planner, index])); }
|
|
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(); }
|
|
stepLine() {
|
|
const result = call(this.core, "sim_linuxcnc_tp_step_line", "number", ["number"], [this.planner]);
|
|
if (result !== 0) throw new Error(`linuxcnc_tp_step_line_${result}`);
|
|
this.controls.paused = false;
|
|
return this.read();
|
|
}
|
|
isStepping() { return Boolean(call(this.core, "sim_linuxcnc_tp_is_stepping", "number", ["number"], [this.planner])); }
|
|
axisLimits() {
|
|
const limits = [0, 1, 2].map((axis) => [0, 1].map((maximum) => Number(call(this.core, "sim_linuxcnc_tp_axis_limit", "number", ["number", "number", "number"], [this.planner, axis, maximum]))));
|
|
return limits.flat().every(Number.isFinite) ? limits : null;
|
|
}
|
|
|
|
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) {
|
|
const tilt = this.kinematics?.coordinates === "XYZBC" ? position.b : position.a;
|
|
return [position.x, position.y, position.z, tilt ?? 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);
|
|
throw new Error(`linuxcnc_tp_joint_inverse_${inverse?.code ?? "unavailable"}`);
|
|
}
|
|
|
|
step() {
|
|
if (!this.available) return { status: "blocked", code: "linuxcnc_tp_unavailable" };
|
|
if (this.controls.paused) return { status: "error", code: "linuxcnc_tp_paused" };
|
|
const before = this.read().state;
|
|
const result = call(this.core, "sim_linuxcnc_tp_step", "number", ["number"], [this.planner]);
|
|
if (result !== 0) return { status: "error", code: `linuxcnc_tp_step_${result}` };
|
|
let state;
|
|
try {
|
|
state = this.read();
|
|
if (this.kinematics && state.status === "verified") {
|
|
const checked = this.kinematics.forward(state.state.joint_commanded);
|
|
if (checked.status !== "verified") throw new Error(checked.code);
|
|
}
|
|
} catch (error) {
|
|
// Native IK/FK rejects a physical joint limit. Keep the last valid
|
|
// servo position, discard the unsafe remainder, and permit retreat.
|
|
if (!String(error?.message).endsWith("_-702")) throw error;
|
|
call(this.core, "sim_linuxcnc_tp_abort", "number", ["number"], [this.planner]);
|
|
this.segmentTags.clear(); this.lastActiveId = 0; this.queueDepthOverride = null;
|
|
this.executionState = 3;
|
|
const stopped = this.setPosition(before.position, before.active_tag);
|
|
this.kinematics.forward(stopped.state.joint_commanded);
|
|
throw new Error("motion_joint_limit");
|
|
}
|
|
if (state.status === "verified") {
|
|
const targetJoints = this.commandedJoints(state.state.position, state.state.active_tag);
|
|
state.state.joint_commanded = targetJoints;
|
|
state.state.joint_actual = targetJoints.slice();
|
|
}
|
|
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);
|
|
state.state.joint_commanded = targetJoints;
|
|
state.state.joint_actual = targetJoints.slice();
|
|
}
|
|
return state;
|
|
}
|
|
|
|
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 velocityScale = Number(call(this.core, "sim_linuxcnc_tp_velocity_scale", "number", ["number"], [this.planner]));
|
|
const velocityLimit = Number(call(this.core, "sim_linuxcnc_tp_velocity_limit", "number", ["number"], [this.planner]));
|
|
const jogScale = Number(call(this.core, "sim_linuxcnc_tp_jog_scale", "number", ["number"], [this.planner]));
|
|
const feedOverride = Number(call(this.core, "sim_linuxcnc_tp_feed_override", "number", ["number"], [this.planner]));
|
|
this.controls.velocity_scale = velocityScale;
|
|
this.controls.jog_scale = jogScale;
|
|
this.controls.feed_override = feedOverride;
|
|
const position = toPose(doubles);
|
|
const joints = this.commandedJoints(position, this.activeTag);
|
|
const velocityMmMin = Number(call(this.core, "sim_linuxcnc_tp_velocity_mm_min", "number", ["number"], [this.planner]));
|
|
const rotaryAxis = this.kinematics?.coordinates === "XYZBC" ? "b" : "a";
|
|
const state = { position, velocity: doubles[9], velocity_mm_min: velocityMmMin, 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: joints, joint_actual: joints.slice(), motion_type: this.core.HEAP32[this.statePointer / 4 + 25], status: this.core.HEAP32[this.statePointer / 4 + 26], exec_state: this.executionState, velocity_limit: velocityLimit, velocity_scale: velocityScale, jog_velocity_linear: this.jogVelocity("x"), jog_velocity_angular: this.jogVelocity(rotaryAxis), feed_override: feedOverride, jog_scale: jogScale, 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.activeTag = 0; this.segmentTags.clear(); this.lastActiveId = 0; this.queueDepthOverride = null; this.executionState = 3; return this.read(); }
|
|
abort() {
|
|
if (this.available) call(this.core, "sim_linuxcnc_tp_abort", "number", ["number"], [this.planner]);
|
|
// Stopping motion is not M429: keep the coordinate frame of the stopped
|
|
// pose, so its WASM inverse and the next jog retain RTCP compensation.
|
|
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, segment_tags: [...this.segmentTags], last_active_id: this.lastActiveId, queue_depth_override: this.queueDepthOverride, native_state: exportNativeCheckpoint(this.core, "sim_linuxcnc_tp", this.planner) };
|
|
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");
|
|
if (!value.native_state || !Array.isArray(value.segment_tags)) throw new Error("linuxcnc_tp_native_checkpoint_required");
|
|
restoreNativeCheckpoint(this.core, "sim_linuxcnc_tp", value.native_state, this.planner);
|
|
this.executionState = value.execution_state;
|
|
this.activeTag = value.active_tag;
|
|
this.controls = { ...value.controls };
|
|
this.segmentTags = new Map(value.segment_tags);
|
|
this.lastActiveId = value.last_active_id;
|
|
this.queueDepthOverride = value.queue_depth_override;
|
|
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; }
|
|
}
|