169 lines
9.4 KiB
JavaScript
169 lines
9.4 KiB
JavaScript
import assert from "node:assert/strict";
|
|
import { readFile } from "node:fs/promises";
|
|
import createModule from "../../build-wasm/linuxcnc-sim-core.js";
|
|
import { loadSimulationCase, toWorkerConfiguration } from "../workspace/sim-case-loader.mjs";
|
|
import { WasmXyzacKinematics, XYZAC_MODES } from "./wasm-xyzac-kinematics.mjs";
|
|
import { XyzacSession, CONTEXT_ID } from "./xyzac-session.mjs";
|
|
|
|
const CASE = "linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini";
|
|
const fetchImpl = async (url) => {
|
|
try {
|
|
const body = await readFile(new URL(`../../${String(url).replace(/^\//, "")}`, import.meta.url));
|
|
return { ok: true, text: async () => body.toString("utf8") };
|
|
} catch {
|
|
return { ok: false, text: async () => "" };
|
|
}
|
|
};
|
|
const close = (actual, expected, tolerance = 1e-9) => assert.ok(Math.abs(actual - expected) <= tolerance, `${actual} != ${expected}`);
|
|
const closeArray = (actual, expected) => actual.forEach((value, index) => close(value, expected[index]));
|
|
|
|
const bundle = await loadSimulationCase({ path: CASE, fetchImpl });
|
|
assert.equal(bundle.status, "ready");
|
|
const core = await createModule({ wasmBinary: await readFile(new URL("../../build-wasm/linuxcnc-sim-core.wasm", import.meta.url)) });
|
|
const limits = toWorkerConfiguration(bundle).home_limits;
|
|
|
|
const kinematics = new WasmXyzacKinematics(core, { x_rot_point: 0, y_offset: 20, z_offset: 10, tool_offset: 0, limits });
|
|
const initial = kinematics.snapshot();
|
|
assert.equal(initial.active_type, XYZAC_MODES.identity);
|
|
assert.equal(initial.coordinates, "XYZAC");
|
|
assert.equal(initial.linear_units, "mm");
|
|
assert.equal(initial.angular_units, "deg");
|
|
assert.deepEqual(initial.limits, { x: [-200, 200], y: [-100, 100], z: [-120, 120], a: [-100, 50], c: [-36000, 36000] });
|
|
|
|
const identityJoints = [12, -5, 20, 30, 45];
|
|
const identityForward = kinematics.forward(identityJoints);
|
|
assert.equal(identityForward.status, "verified");
|
|
assert.deepEqual(identityForward.pose, { x: 12, y: -5, z: 20, a: 30, b: 0, c: 45, u: 0, v: 0, w: 0 });
|
|
const identityInverse = kinematics.inverse(identityForward.pose);
|
|
assert.equal(identityInverse.status, "verified");
|
|
closeArray(identityInverse.joints, identityJoints);
|
|
|
|
const rangeError = kinematics.forward([0, 0, 121, 0, 0]);
|
|
assert.equal(rangeError.code, "wasm_xyzac_forward_-702");
|
|
const nonfiniteJoints = kinematics.forward([0, 0, Number.NaN, 0, 0]);
|
|
assert.equal(nonfiniteJoints.code, "wasm_xyzac_forward_-701");
|
|
const nonfinitePose = kinematics.inverse({ ...identityForward.pose, x: Number.NaN });
|
|
assert.equal(nonfinitePose.code, "wasm_xyzac_inverse_-701");
|
|
|
|
const selectedTrt = kinematics.select(XYZAC_MODES.xyzac, identityJoints);
|
|
assert.equal(selectedTrt.status, "verified");
|
|
assert.equal(selectedTrt.active_type, XYZAC_MODES.xyzac);
|
|
close(selectedTrt.pose.x, identityForward.pose.x);
|
|
close(selectedTrt.pose.y, identityForward.pose.y);
|
|
close(selectedTrt.pose.z, identityForward.pose.z);
|
|
const trtJoints = [50, 10, 30, 30, 45];
|
|
const trtForward = kinematics.forward(trtJoints);
|
|
assert.equal(trtForward.status, "verified");
|
|
const trtInverse = kinematics.inverse(trtForward.pose);
|
|
assert.equal(trtInverse.status, "verified");
|
|
closeArray(trtInverse.joints, trtJoints);
|
|
|
|
const pivotBoundary = kinematics.forward([20, 5, 30, -90, 45]);
|
|
assert.equal(pivotBoundary.status, "verified");
|
|
const pivotRoundtrip = kinematics.inverse(pivotBoundary.pose);
|
|
assert.equal(pivotRoundtrip.status, "verified");
|
|
closeArray(pivotRoundtrip.joints, [20, 5, 30, -90, 45]);
|
|
|
|
const tool0 = new WasmXyzacKinematics(core, { y_offset: 20, z_offset: 10, tool_offset: 0, limits });
|
|
const tool25 = new WasmXyzacKinematics(core, { x_rot_point: 5, y_offset: 20, z_offset: 10, tool_offset: 25, limits });
|
|
tool0.select(XYZAC_MODES.xyzac);
|
|
tool25.select(XYZAC_MODES.xyzac);
|
|
const offset0 = tool0.forward(trtJoints);
|
|
const offset25 = tool25.forward(trtJoints);
|
|
assert.equal(offset0.status, "verified");
|
|
assert.equal(offset25.status, "verified");
|
|
assert.notDeepEqual(offset25.pose, offset0.pose);
|
|
assert.deepEqual(offset25.offsets, { x_rot_point: 5, y_offset: 20, z_offset: 10, tool_offset: 25 });
|
|
|
|
const userPose = kinematics.forward(trtJoints).pose;
|
|
const userk = kinematics.select(XYZAC_MODES.userk, trtJoints);
|
|
assert.equal(userk.status, "verified");
|
|
assert.equal(userk.native_type, "userk-identity-template");
|
|
close(userk.pose.x, userPose.x);
|
|
close(userk.pose.y, userPose.y);
|
|
close(userk.pose.z, userPose.z);
|
|
const path = new TextEncoder().encode("/tmp/dynamic-userk.so\0");
|
|
const pathPointer = core._malloc(path.length);
|
|
core.HEAPU8.set(path, pathPointer);
|
|
assert.equal(core._sim_kinematics_user_reject_dynamic(pathPointer), -712);
|
|
core._free(pathPointer);
|
|
|
|
const session = new XyzacSession(core, { sessionId: "xyzac-rtcp" });
|
|
const base = { schema_version: 1, context_id: CONTEXT_ID, session_id: session.sessionId };
|
|
let seq = 0;
|
|
const send = (kind, operationId, payload = {}) => session.accept({ ...base, kind, seq: ++seq, operation_id: operationId, payload });
|
|
send("HELLO", "hello");
|
|
send("SESSION_ATTACH", "attach");
|
|
assert.equal(send("LOAD_WORKSPACE", "workspace", { root_path: "/workspace", configuration: toWorkerConfiguration(bundle) }).payload.status, "verified");
|
|
|
|
const switchRows = [];
|
|
for (const [code, expected] of [["M429", 0], ["M428", 1], ["M430", 2], ["M428", 1]]) {
|
|
const switched = send("MDI_EXECUTE", `switch-${code}-${seq}`, { block: code });
|
|
assert.equal(switched.kind, "COMMAND_RESULT");
|
|
assert.equal(switched.payload.kinematics.active_type, expected);
|
|
assert.equal(switched.payload.remap.hal_pin, "motion.switchkins-type");
|
|
assert.equal(switched.payload.remap.sync, "M66 E0 L0");
|
|
switchRows.push({ code, active_type: expected, state_hash: switched.payload.kinematics.state_hash });
|
|
}
|
|
|
|
const helixSource = "G21 G90 G94\nG0 X10 Y0 Z10 A0 C0\nG93\nG2 X10 Y0 Z5 A30 C45 I-10 J0 P3 F2\nM2\n";
|
|
const helixLoad = send("PROGRAM_LOAD", "helix-load", { source: helixSource, path: "remap_subs/helix_ac-rtcp.ngc" });
|
|
assert.equal(helixLoad.kind, "COMMAND_RESULT");
|
|
const helix = helixLoad.payload.preview.segments.find((segment) => segment.motion_type === 2);
|
|
assert.ok(helix);
|
|
assert.equal(helix.turn, -3);
|
|
assert.equal(helix.end.a, 30);
|
|
assert.equal(helix.end.c, 45);
|
|
assert.equal(helix.end.z, 5);
|
|
assert.ok(Number.isFinite(helix.velocity) && helix.velocity > 0);
|
|
|
|
const recoveryPose = session.xyzacKinematics.forward([20, 5, 30, -90, 45]);
|
|
assert.equal(recoveryPose.status, "verified");
|
|
const run = send("PROGRAM_RUN", "helix-run");
|
|
assert.equal(run.payload.panel_snapshot.kinematics.active_type, 1);
|
|
send("SIM_STEP", "helix-step", { cycles: 1 });
|
|
const paused = send("PROGRAM_PAUSE", "helix-pause");
|
|
assert.equal(paused.payload.panel_snapshot.kinematics.active_type, 1);
|
|
const resumed = send("PROGRAM_RESUME", "helix-resume");
|
|
assert.equal(resumed.payload.panel_snapshot.kinematics.active_type, 1);
|
|
send("SIM_STEP", "helix-step-2", { cycles: 1 });
|
|
const aborted = send("PROGRAM_ABORT", "helix-abort");
|
|
assert.equal(aborted.payload.panel_snapshot.kinematics.active_type, 1);
|
|
const restarted = send("PROGRAM_RESTART", "helix-restart");
|
|
assert.equal(restarted.payload.panel_snapshot.kinematics.active_type, 1);
|
|
|
|
const beforeCheckpoint = session.xyzacKinematics.snapshot();
|
|
const checkpoint = send("CHECKPOINT", "rtcp-checkpoint");
|
|
assert.equal(checkpoint.kind, "CHECKPOINT_READY");
|
|
assert.equal(checkpoint.payload.checkpoint.state.kinematics.state_hash, beforeCheckpoint.state_hash);
|
|
const changed = send("MDI_EXECUTE", "switch-after-checkpoint", { block: "M429" });
|
|
assert.equal(changed.payload.kinematics.active_type, 0);
|
|
close(changed.payload.kinematics.pose.x, beforeCheckpoint.pose.x);
|
|
close(changed.payload.kinematics.pose.y, beforeCheckpoint.pose.y);
|
|
close(changed.payload.kinematics.pose.z, beforeCheckpoint.pose.z);
|
|
const recovered = send("RECOVER", "rtcp-recover", { checkpoint: checkpoint.payload.checkpoint });
|
|
assert.equal(recovered.payload.status, "recovered");
|
|
assert.equal(recovered.payload.state.kinematics.active_type, 1);
|
|
assert.deepEqual(recovered.payload.state.kinematics.pose, beforeCheckpoint.pose);
|
|
assert.deepEqual(recovered.payload.state.kinematics.joints, beforeCheckpoint.joints);
|
|
assert.equal(recovered.payload.state.kinematics.state_hash, beforeCheckpoint.state_hash);
|
|
|
|
console.log(JSON.stringify({
|
|
schema: "xyzac-rtcp-08.v1",
|
|
status: "verified",
|
|
case: CASE,
|
|
mapping: { coordinates: initial.coordinates, linear_units: initial.linear_units, angular_units: initial.angular_units, limits: initial.limits },
|
|
identity: { pose: identityForward.pose, state_hash: identityInverse.state_hash },
|
|
trt: { pose: trtForward.pose, joints: trtInverse.joints, pivot_boundary: pivotBoundary.pose, state_hash: trtInverse.state_hash },
|
|
offsets: { base: offset0.pose, changed: offset25.pose, configured: offset25.offsets },
|
|
userk: { native_type: userk.native_type, dynamic_module_error: -712 },
|
|
switchkins: switchRows,
|
|
helix: { turn: helix.turn, end: helix.end, velocity: helix.velocity, motion_type: helix.motion_type },
|
|
boundaries: { range: rangeError.code, nonfinite_joints: nonfiniteJoints.code, nonfinite_pose: nonfinitePose.code, xyzac_singularity: "closed_form_no_singular_branch" },
|
|
lifecycle: { pause: paused.payload.task.state, resume: resumed.payload.task.state, abort: aborted.payload.task.state, restart: restarted.payload.task.program_status, recovery: recovered.payload.status },
|
|
recovery: { active_type: recovered.payload.state.kinematics.active_type, state_hash: recovered.payload.state.kinematics.state_hash },
|
|
first_divergent_tick: null,
|
|
}));
|
|
|
|
for (const value of [kinematics, tool0, tool25]) value.destroy();
|