linuxcncWebWASM/web/worker/wasm-generic-kinematics.mjs

import { fnv1a64 } from "./replay.mjs";

const CONFIG_SIZE = 1608;
const KINEMATICS_SIZE = 1760;
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, limit_enabled: 1448, joint_min: 1464, joint_max: 1536 });
const POSE_AXES = Object.freeze(["x", "y", "z", "a", "b", "c", "u", "v", "w"]);
const TYPES = Object.freeze({ trivkins: 1, "xyzab-tdr": 1, xyzac: 2, xyzbc: 3, xyzbcw: 4, corexy: 5, rose: 6, maxkins: 7, rotatekins: 8, "5axiskins": 9, scara: 10, pumakins: 11, "scorbot-kins": 12, genserkins: 13, lineardeltakins: 14, rotarydeltakins: 15, tripodkins: 16, genhexkins: 17, pentakins: 18 });

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

export class WasmGenericKinematics {
  constructor(core, { type, coordinates, jointCount, kinematicsJointCount = jointCount, extraJointCount = jointCount - kinematicsJointCount, limits = {}, linearUnits = "mm", angularUnits = "deg", parameters = {} }) {
    this.core = core;
    this.type = String(type ?? "").toLowerCase();
    this.coordinates = String(coordinates ?? "").replace(/\s+/g, "").toUpperCase();
    this.jointCount = Number(jointCount);
    this.kinematicsJointCount = Number(kinematicsJointCount);
    this.extraJointCount = Number(extraJointCount);
    this.limits = structuredClone(limits);
    this.linearUnits = linearUnits;
    this.angularUnits = angularUnits;
    this.parameters = structuredClone(parameters ?? {});
    const typeId = TYPES[this.type];
    if (!typeId || !this.coordinates || !Number.isInteger(this.jointCount) || this.jointCount < 1 || this.jointCount > 9 || !Number.isInteger(this.kinematicsJointCount) || this.kinematicsJointCount < 1 || this.kinematicsJointCount > this.jointCount || this.extraJointCount !== this.jointCount - this.kinematicsJointCount) throw new Error("wasm_generic_kinematics_profile_invalid");
    if (["sim_kinematics_init", "sim_kinematics_inverse", "sim_kinematics_forward", "sim_kinematics_home"].some((name) => typeof core?.[`_${name}`] !== "function" && typeof core?.[name] !== "function") || typeof core?._malloc !== "function") throw new Error("wasm_generic_kinematics_unavailable");
    this.config = core._malloc(CONFIG_SIZE); this.kinematics = core._malloc(KINEMATICS_SIZE); this.pose = core._malloc(POSE_SIZE); this.joints = core._malloc(JOINT_BYTES); this.info = core._malloc(32);
    this.pointers = [this.config, this.kinematics, this.pose, this.joints, this.info];
    if (this.pointers.some((pointer) => !pointer)) throw new Error("wasm_generic_kinematics_allocation_failed");
    core.HEAPU8.fill(0, this.config, this.config + CONFIG_SIZE); core.HEAPU8.fill(0, this.kinematics, this.kinematics + KINEMATICS_SIZE); core.HEAPU8.fill(0, this.pose, this.pose + POSE_SIZE); core.HEAPU8.fill(0, this.joints, this.joints + JOINT_BYTES); core.HEAPU8.fill(0, this.info, this.info + 32);
    const view = new DataView(core.HEAPU8.buffer, this.config, CONFIG_SIZE);
    view.setInt32(CONFIG.type, typeId, true); view.setInt32(CONFIG.mode, 0, true); view.setUint32(CONFIG.joint_count, this.jointCount, true); view.setUint32(CONFIG.kinematics_joint_count, this.kinematicsJointCount, true); view.setUint32(CONFIG.extra_joint_count, this.extraJointCount, true);
    for (const [index, axis] of [...this.coordinates].entries()) {
      core.HEAPU8[this.config + CONFIG.coordinates + index] = axis.charCodeAt(0);
      const range = limits[axis.toLowerCase()];
      if (!range) continue;
      if (range) {
        if (!Array.isArray(range) || range.length !== 2 || !range.every(Number.isFinite) || range[0] >= range[1]) throw new Error(`wasm_generic_kinematics_limit_invalid:${axis}`);
        core.HEAPU8[this.config + CONFIG.limit_enabled + index] = 1;
        view.setFloat64(CONFIG.joint_min + index * 8, range[0], true); view.setFloat64(CONFIG.joint_max + index * 8, range[1], true);
      }
    }
    const scalarOffsets = { x_rot_point: 40, y_rot_point: 48, z_rot_point: 56, x_offset: 64, y_offset: 72, z_offset: 80, tool_offset: 88, pivot_length: 96, puma_a2: 104, puma_a3: 112, puma_d3: 120, puma_d4: 128, puma_d6: 136 };
    for (const [key, offset] of Object.entries(scalarOffsets)) if (Number.isFinite(Number(this.parameters[key]))) view.setFloat64(offset, Number(this.parameters[key]), true);
    if (Array.isArray(this.parameters.dh_a) && Array.isArray(this.parameters.dh_alpha) && Array.isArray(this.parameters.dh_d)) {
      if (![this.parameters.dh_a, this.parameters.dh_alpha, this.parameters.dh_d].every((array) => array.length === 6 && array.every(Number.isFinite))) throw new Error("wasm_generic_kinematics_dh_invalid");
      view.setUint32(192, 6, true);
      for (let index = 0; index < 6; index += 1) {
        view.setFloat64(200 + index * 8, this.parameters.dh_a[index], true);
        view.setFloat64(272 + index * 8, this.parameters.dh_alpha[index], true);
        view.setFloat64(344 + index * 8, this.parameters.dh_d[index], true);
      }
    }
    const initialized = call(core, "sim_kinematics_init", "number", ["number", "number"], [this.kinematics, this.config]);
    if (initialized !== 0) throw new Error(`wasm_generic_kinematics_init_${initialized}`);
    this.lastJoints = new Array(this.jointCount).fill(0); this.lastPose = Object.fromEntries(POSE_AXES.map((axis) => [axis, 0]));
    const home = this.home();
    if (home.status !== "verified") throw new Error(`wasm_generic_kinematics_home_${home.code}`);
  }

  inverse(pose) {
    if (!pose || POSE_AXES.some((axis) => !Number.isFinite(Number(pose[axis] ?? 0)))) return { status: "error", code: "wasm_generic_kinematics_pose_nonfinite" };
    this.core.HEAPF64.set(POSE_AXES.map((axis) => Number(pose[axis] ?? 0)), this.pose / 8);
    const result = call(this.core, "sim_kinematics_inverse", "number", ["number", "number", "number", "number"], [this.kinematics, this.pose, this.joints, this.kinematicsJointCount]);
    if (result !== 0) return { status: "error", code: `wasm_generic_kinematics_inverse_${result}`, result };
    this.lastJoints = [...this.core.HEAPF64.subarray(this.joints / 8, this.joints / 8 + this.jointCount)]; this.lastPose = Object.fromEntries(POSE_AXES.map((axis) => [axis, Number(pose[axis] ?? 0)]));
    return { status: "verified", joints: [...this.lastJoints] };
  }

  forward(joints = this.lastJoints) {
    if (!Array.isArray(joints) || joints.length < this.jointCount || joints.slice(0, this.jointCount).some((value) => !Number.isFinite(value))) return { status: "error", code: "wasm_generic_kinematics_joints_invalid" };
    this.core.HEAPF64.set(joints.slice(0, this.jointCount), this.joints / 8);
    const result = call(this.core, "sim_kinematics_forward", "number", ["number", "number", "number", "number"], [this.kinematics, this.joints, this.kinematicsJointCount, this.pose]);
    if (result !== 0) return { status: "error", code: `wasm_generic_kinematics_forward_${result}`, result };
    this.lastJoints = joints.slice(0, this.jointCount); this.lastPose = Object.fromEntries(POSE_AXES.map((axis, index) => [axis, this.core.HEAPF64[this.pose / 8 + index]]));
    return { status: "verified", pose: { ...this.lastPose }, joints: [...this.lastJoints] };
  }

  home() {
    const result = call(this.core, "sim_kinematics_home", "number", ["number", "number", "number", "number"], [this.kinematics, this.joints, this.kinematicsJointCount, this.pose]);
    if (result !== 0) return { status: "error", code: `wasm_generic_kinematics_home_${result}`, result };
    this.lastJoints = [...this.core.HEAPF64.subarray(this.joints / 8, this.joints / 8 + this.jointCount)]; this.lastPose = Object.fromEntries(POSE_AXES.map((axis, index) => [axis, this.core.HEAPF64[this.pose / 8 + index]]));
    return { status: "verified", pose: { ...this.lastPose }, joints: [...this.lastJoints] };
  }

  snapshot(pose = this.lastPose, joints = this.lastJoints) {
    const unsigned = { schema: "wasm-generic-kinematics.v1", type: this.type, coordinates: this.coordinates, joint_count: this.jointCount, linear_units: this.linearUnits, angular_units: this.angularUnits, pose: { ...pose }, joints: [...joints], limits: structuredClone(this.limits), source: "linuxcnc-wasm-sim_kinematics" };
    return { status: "verified", ...unsigned, 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 { TYPES as WASM_KINEMATICS_TYPES };