200 lines
13 KiB
JavaScript
200 lines
13 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
|
|
const CONFIG_SIZE = 1608;
|
|
const KINEMATICS_SIZE = 1760;
|
|
const SWITCH_SIZE = 512;
|
|
const RESULT_INFO_SIZE = 32;
|
|
const POSE_SIZE = 72;
|
|
const JOINT_BYTES = 9 * 8;
|
|
const CONFIG = Object.freeze({
|
|
type: 0, mode: 4, joint_count: 8, kinematics_joint_count: 12, extra_joint_count: 16,
|
|
coordinates: 20, x_rot_point: 40, y_rot_point: 48, z_rot_point: 56,
|
|
x_offset: 64, y_offset: 72, z_offset: 80, tool_offset: 88,
|
|
limit_enabled: 1448, joint_min: 1464, joint_max: 1536,
|
|
});
|
|
const SWITCH = Object.freeze({ switching: 16, active_mode: 20, pending_mode: 24, generation: 28, switch_count: 32, abort_count: 36 });
|
|
const TYPE = Object.freeze({ identity: 1, xyzac: 2, userk: 1 });
|
|
const MODE = Object.freeze({ identity: 0, xyzac: 1, userk: 2 });
|
|
const AXES = Object.freeze(["x", "y", "z", "a", "b", "c", "u", "v", "w"]);
|
|
const COORDINATES = Object.freeze(["x", "y", "z", "a", "c"]);
|
|
const DEFAULT_LIMITS = Object.freeze({ x: [-200, 200], y: [-100, 100], z: [-120, 120], a: [-100, 50], c: [-36000, 36000] });
|
|
|
|
function call(core, name, returnType, argTypes, args) {
|
|
const direct = core?.[`_${name}`] ?? core?.[name];
|
|
if (typeof direct === "function") return direct(...args);
|
|
if (typeof core?.ccall === "function") return core.ccall(name, returnType, argTypes, args);
|
|
throw new Error(`missing core export: ${name}`);
|
|
}
|
|
|
|
function pose(core, pointer) {
|
|
const doubles = core.HEAPF64.subarray(pointer / 8, pointer / 8 + 9);
|
|
return Object.fromEntries(AXES.map((axis, index) => [axis, doubles[index]]));
|
|
}
|
|
|
|
function setCoordinates(core, pointer) {
|
|
for (const [index, value] of [..."XYZAC"].entries()) core.HEAPU8[pointer + CONFIG.coordinates + index] = value.charCodeAt(0);
|
|
}
|
|
|
|
function configFor(core, pointer, type, options) {
|
|
core.HEAPU8.fill(0, pointer, pointer + CONFIG_SIZE);
|
|
const view = new DataView(core.HEAPU8.buffer, pointer, CONFIG_SIZE);
|
|
view.setInt32(CONFIG.type, type, true);
|
|
view.setInt32(CONFIG.mode, 0, true);
|
|
view.setUint32(CONFIG.joint_count, 5, true);
|
|
view.setUint32(CONFIG.kinematics_joint_count, 5, true);
|
|
view.setUint32(CONFIG.extra_joint_count, 0, true);
|
|
setCoordinates(core, pointer);
|
|
for (const [index, axis] of COORDINATES.entries()) {
|
|
const limit = options.limits?.[axis] ?? DEFAULT_LIMITS[axis];
|
|
core.HEAPU8[pointer + CONFIG.limit_enabled + index] = 1;
|
|
view.setFloat64(CONFIG.joint_min + index * 8, Number(limit[0]), true);
|
|
view.setFloat64(CONFIG.joint_max + index * 8, Number(limit[1]), true);
|
|
}
|
|
for (const [key, offset] of Object.entries({ x_rot_point: CONFIG.x_rot_point, y_offset: CONFIG.y_offset, z_offset: CONFIG.z_offset, tool_offset: CONFIG.tool_offset })) {
|
|
view.setFloat64(offset, Number(options[key] ?? 0), true);
|
|
}
|
|
}
|
|
|
|
function resultCode(operation, result) {
|
|
return { status: "error", code: `wasm_xyzac_${operation}_${result}`, result };
|
|
}
|
|
|
|
export class WasmXyzacKinematics {
|
|
constructor(core, options = {}) {
|
|
this.core = core;
|
|
this.options = { x_rot_point: 0, y_offset: 20, z_offset: 10, tool_offset: 0, limits: DEFAULT_LIMITS, ...options };
|
|
this.available = ["sim_kinematics_init", "sim_kinematics_switch_init", "sim_kinematics_switch_register", "sim_kinematics_switch_select", "sim_kinematics_switch_select_physical", "sim_kinematics_switch_forward", "sim_kinematics_switch_inverse", "sim_kinematics_switch_inverse_mode", "sim_kinematics_switch_active", "sim_kinematics_switch_state_hash"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function");
|
|
this.lastCommand = null;
|
|
this.modules = new Map();
|
|
this.pointers = [];
|
|
if (!this.available || typeof core?._malloc !== "function") return;
|
|
for (const [mode, type] of [[MODE.identity, TYPE.identity], [MODE.xyzac, TYPE.xyzac], [MODE.userk, TYPE.userk]]) {
|
|
const config = core._malloc(CONFIG_SIZE);
|
|
const kinematics = core._malloc(KINEMATICS_SIZE);
|
|
if (!config || !kinematics) throw new Error("wasm_xyzac_allocation_failed");
|
|
configFor(core, config, type, this.options);
|
|
const result = call(core, "sim_kinematics_init", "number", ["number", "number"], [kinematics, config]);
|
|
if (result !== 0) throw new Error(`wasm_xyzac_init_${result}`);
|
|
this.modules.set(mode, { config, kinematics });
|
|
this.pointers.push(config, kinematics);
|
|
}
|
|
this.switchPointer = core._malloc(SWITCH_SIZE);
|
|
this.resultPointer = core._malloc(RESULT_INFO_SIZE);
|
|
this.posePointer = core._malloc(POSE_SIZE);
|
|
this.jointPointer = core._malloc(JOINT_BYTES);
|
|
if (![this.switchPointer, this.resultPointer, this.posePointer, this.jointPointer].every(Boolean)) throw new Error("wasm_xyzac_allocation_failed");
|
|
this.pointers.push(this.switchPointer, this.resultPointer, this.posePointer, this.jointPointer);
|
|
for (const [pointer, size] of [[this.switchPointer, SWITCH_SIZE], [this.resultPointer, RESULT_INFO_SIZE], [this.posePointer, POSE_SIZE], [this.jointPointer, JOINT_BYTES]]) core.HEAPU8.fill(0, pointer, pointer + size);
|
|
this.initializeSwitch();
|
|
}
|
|
|
|
initializeSwitch() {
|
|
const initialized = call(this.core, "sim_kinematics_switch_init", "number", ["number", "number", "number"], [this.switchPointer, MODE.identity, 0]);
|
|
if (initialized !== 0) throw new Error(`wasm_xyzac_switch_init_${initialized}`);
|
|
for (const [mode, module] of this.modules) {
|
|
const registered = call(this.core, "sim_kinematics_switch_register", "number", ["number", "number", "number", "number"], [this.switchPointer, mode, module.kinematics, 0]);
|
|
if (registered !== 0) throw new Error(`wasm_xyzac_switch_register_${mode}_${registered}`);
|
|
}
|
|
}
|
|
|
|
get activeMode() {
|
|
if (!this.available) return MODE.identity;
|
|
return call(this.core, "sim_kinematics_switch_active", "number", ["number"], [this.switchPointer]);
|
|
}
|
|
|
|
setJoints(joints) {
|
|
if (!Array.isArray(joints) || joints.length < 5) throw new Error("wasm_xyzac_joints_invalid");
|
|
this.core.HEAPF64.set(joints.slice(0, 9).map(Number), this.jointPointer / 8);
|
|
}
|
|
|
|
setPose(value) {
|
|
if (!value || typeof value !== "object") throw new Error("wasm_xyzac_pose_invalid");
|
|
this.core.HEAPF64.set(AXES.map((axis) => Number(value[axis] ?? 0)), this.posePointer / 8);
|
|
}
|
|
|
|
forward(joints = [0, 0, 0, 0, 0]) {
|
|
if (!this.available) return { status: "blocked", code: "wasm_xyzac_unavailable" };
|
|
this.setJoints(joints);
|
|
const result = call(this.core, "sim_kinematics_switch_forward", "number", ["number", "number", "number", "number", "number"], [this.switchPointer, this.jointPointer, 5, this.posePointer, this.resultPointer]);
|
|
if (result !== 0) return resultCode("forward", result);
|
|
return this.snapshot();
|
|
}
|
|
|
|
inverse(currentPose) {
|
|
if (!this.available) return { status: "blocked", code: "wasm_xyzac_unavailable" };
|
|
this.setPose(currentPose);
|
|
const result = call(this.core, "sim_kinematics_switch_inverse", "number", ["number", "number", "number", "number", "number"], [this.switchPointer, this.posePointer, this.jointPointer, 5, this.resultPointer]);
|
|
if (result !== 0) return resultCode("inverse", result);
|
|
return this.snapshot();
|
|
}
|
|
|
|
inverseMode(mode, currentPose) {
|
|
if (!this.available) return { status: "blocked", code: "wasm_xyzac_unavailable" };
|
|
this.setPose(currentPose);
|
|
const result = call(this.core, "sim_kinematics_switch_inverse_mode", "number", ["number", "number", "number", "number", "number", "number"], [this.switchPointer, mode, this.posePointer, this.jointPointer, 5, this.resultPointer]);
|
|
if (result !== 0) return resultCode("inverse_mode", result);
|
|
return { status: "verified", joints: [...this.core.HEAPF64.subarray(this.jointPointer / 8, this.jointPointer / 8 + 5)] };
|
|
}
|
|
|
|
select(mode, joints = [0, 0, 0, 0, 0]) {
|
|
if (![MODE.identity, MODE.xyzac, MODE.userk].includes(mode)) return { status: "error", code: "wasm_xyzac_invalid_mode" };
|
|
this.setJoints(joints);
|
|
const result = call(this.core, "sim_kinematics_switch_select", "number", ["number", "number", "number", "number", "number", "number"], [this.switchPointer, mode, this.jointPointer, 5, this.posePointer, this.resultPointer]);
|
|
if (result !== 0) return resultCode("switch", result);
|
|
this.lastCommand = `M${mode === MODE.xyzac ? 428 : mode === MODE.identity ? 429 : 430}`;
|
|
return this.snapshot();
|
|
}
|
|
|
|
selectPhysical(mode, joints = [0, 0, 0, 0, 0]) {
|
|
if (!this.available) return { status: "blocked", code: "wasm_xyzac_unavailable" };
|
|
if (![MODE.identity, MODE.xyzac, MODE.userk].includes(mode)) return { status: "error", code: "wasm_xyzac_invalid_mode" };
|
|
this.setJoints(joints);
|
|
const result = call(this.core, "sim_kinematics_switch_select_physical", "number", ["number", "number", "number", "number", "number", "number"], [this.switchPointer, mode, this.jointPointer, 5, this.posePointer, this.resultPointer]);
|
|
if (result !== 0) return resultCode("switch_physical", result);
|
|
this.lastCommand = `M${mode === MODE.xyzac ? 428 : mode === MODE.identity ? 429 : 430}`;
|
|
return this.snapshot();
|
|
}
|
|
|
|
restoreState(state) {
|
|
if (!state || state.schema !== "wasm-xyzac-kinematics.v1" || ![MODE.identity, MODE.xyzac, MODE.userk].includes(state.active_type) || !Array.isArray(state.joints) || state.joints.length !== 5) throw new Error("wasm_xyzac_restore_state_invalid");
|
|
this.initializeSwitch();
|
|
this.setPose(state.pose);
|
|
this.setJoints(state.joints);
|
|
if (state.active_type !== MODE.identity) {
|
|
let result = call(this.core, "sim_kinematics_switch_begin", "number", ["number", "number"], [this.switchPointer, state.active_type]);
|
|
if (result === 0) result = call(this.core, "sim_kinematics_switch_commit", "number", ["number", "number", "number", "number", "number"], [this.switchPointer, this.jointPointer, 5, this.posePointer, this.resultPointer]);
|
|
if (result !== 0) throw new Error(`wasm_xyzac_restore_switch_${result}`);
|
|
} else {
|
|
const result = call(this.core, "sim_kinematics_switch_inverse", "number", ["number", "number", "number", "number", "number"], [this.switchPointer, this.posePointer, this.jointPointer, 5, this.resultPointer]);
|
|
if (result !== 0) throw new Error(`wasm_xyzac_restore_inverse_${result}`);
|
|
}
|
|
this.setJoints(state.joints);
|
|
this.setPose(state.pose);
|
|
const view = new DataView(this.core.HEAPU8.buffer, this.switchPointer, SWITCH_SIZE);
|
|
view.setUint8(SWITCH.switching, 0);
|
|
view.setInt32(SWITCH.active_mode, state.active_type, true);
|
|
view.setInt32(SWITCH.pending_mode, state.active_type, true);
|
|
view.setUint32(SWITCH.generation, Number(state.generation ?? 0), true);
|
|
view.setUint32(SWITCH.switch_count, Number(state.switch_count ?? state.generation ?? 0), true);
|
|
view.setUint32(SWITCH.abort_count, Number(state.abort_count ?? 0), true);
|
|
this.lastCommand = state.last_command ?? null;
|
|
const restored = this.snapshot();
|
|
if (restored.active_type !== state.active_type) throw new Error("wasm_xyzac_restore_projection_mismatch");
|
|
return restored;
|
|
}
|
|
|
|
snapshot(currentPose = pose(this.core, this.posePointer), joints = [...this.core.HEAPF64.subarray(this.jointPointer / 8, this.jointPointer / 8 + 5)]) {
|
|
const activeMode = this.activeMode;
|
|
const nativeType = activeMode === MODE.xyzac ? "xyzac-trt-kins" : activeMode === MODE.userk ? "userk-identity-template" : "identity";
|
|
const view = new DataView(this.core.HEAPU8.buffer, this.switchPointer, SWITCH_SIZE);
|
|
const limits = Object.fromEntries(COORDINATES.map((axis) => [axis, [...(this.options.limits?.[axis] ?? DEFAULT_LIMITS[axis])]]));
|
|
const unsigned = { schema: "wasm-xyzac-kinematics.v1", active_type: activeMode, native_type: nativeType, coordinates: "XYZAC", joint_count: 5, linear_units: "mm", angular_units: "deg", pose: currentPose, joints: joints.slice(0, 5), offsets: { x_rot_point: this.options.x_rot_point, y_offset: this.options.y_offset, z_offset: this.options.z_offset, tool_offset: this.options.tool_offset }, limits, generation: view.getUint32(SWITCH.generation, true), switch_count: view.getUint32(SWITCH.switch_count, true), abort_count: view.getUint32(SWITCH.abort_count, true), last_command: this.lastCommand, capability: { userk: "linked_identity_template", dynamic_modules: "denied" }, source: "linuxcnc-wasm-sim_kinematics_switch" };
|
|
const hash = call(this.core, "sim_kinematics_switch_state_hash", "number", ["number", "number", "number", "number"], [this.switchPointer, this.jointPointer, 5, this.posePointer]);
|
|
return { status: "verified", ...unsigned, wasm_state_hash: `fnv1a64:${BigInt.asUintN(64, BigInt(hash >>> 0)).toString(16).padStart(16, "0")}`, state_hash: fnv1a64(unsigned) };
|
|
}
|
|
|
|
destroy() { for (const pointer of this.pointers) if (pointer && typeof this.core?._free === "function") this.core._free(pointer); this.pointers = []; }
|
|
}
|
|
|
|
export { MODE as XYZAC_MODES };
|