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; } }