完善五轴 RTCP 仿真与验证资料

web-rtcp-5axis-sim-plan/tests/node/verify_linuxcnc_kinematics_runtime.mjs

@@ -1,52 +1,126 @@
 import assert from "node:assert/strict";
 
+import { getFiveAxisProfile } from "../../app/src/profiles/index.js";
 import { createLinuxCncKinematicsRuntime } from "../../app/src/runtime/linuxcnc-kinematics-runtime.js";
 import { buildRtcpFrame } from "../../app/src/runtime/rtcp-frame.js";
 
-const runtime = await createLinuxCncKinematicsRuntime({ moduleId: "xyzac-trt" });
-const readiness = runtime.readiness();
+const DEG_TO_RAD = Math.PI / 180;
 
-assert.equal(runtime.apiName, "web-rtcp-5axis-linuxcnc-kinematics-runtime");
-assert.equal(runtime.moduleId, "xyzac-trt");
-assert.equal(runtime.loaded, true);
-assert.equal(runtime.sourceMode, "source-derived-kinematics-wasm");
-assert.equal(runtime.semanticBoundary, "linuxcnc_kinematics_wasm_c_abi");
-assert.equal(runtime.executionContext, "direct");
-assert.equal(runtime.wasmFile, "linuxcnc_xyzac_trt_kinematics.wasm");
-assert.equal(readiness.loaded, true);
-assert.equal(readiness.executionContext, "direct");
-assert.equal(readiness.supportedModules.includes("xyzac-trt"), true);
-assert.equal(runtime.switchRc, 0);
+const cases = [
+  {
+    moduleId: "xyzac-trt",
+    wasmFile: "linuxcnc_xyzac_trt_kinematics.wasm",
+    joints: [10, 20, 30, 25, 40],
+    fourthAxis: "A",
+    fourthKey: "a",
+    axisPose: { x: 10, y: 20, z: 30, a: 25, b: 0, c: 40 },
+    kinsType: "tcp-xyzac",
+    expectedToolAxis: xyzacToolAxis(25, 40),
+  },
+  {
+    moduleId: "xyzbc-trt",
+    wasmFile: "linuxcnc_xyzbc_trt_kinematics.wasm",
+    joints: [10, 20, 30, 35, 40],
+    fourthAxis: "B",
+    fourthKey: "b",
+    axisPose: { x: 10, y: 20, z: 30, a: 0, b: 35, c: 40 },
+    kinsType: "tcp-xyzbc",
+    expectedToolAxis: xyzbcToolAxis(35, 40),
+  },
+];
 
-const linuxCncKinematicsResult = runtime.frameForJoints([10, 20, 30, 25, 40]);
-assert.equal(linuxCncKinematicsResult.moduleId, "xyzac-trt");
-assert.equal(linuxCncKinematicsResult.forward.rc, 0);
-assert.equal(linuxCncKinematicsResult.inverse.rc, 0);
-assert.deepEqual(
-  linuxCncKinematicsResult.inverse.joints.map((value) => Math.round(value * 1e6) / 1e6),
-  [10, 20, 30, 25, 40],
-);
+for (const testCase of cases) {
+  const runtime = await createLinuxCncKinematicsRuntime({
+    moduleId: testCase.moduleId,
+    switchkinsType: 1,
+  });
+  const readiness = runtime.readiness();
 
-const frame = buildRtcpFrame({
-  axisPose: { x: 10, y: 20, z: 30, a: 25, b: 0, c: 40 },
-  activeLine: 777,
-  kinsType: "tcp-xyzac",
-  rtcpEnabled: true,
-  linuxCncKinematicsResult,
-});
+  assert.equal(runtime.apiName, "web-rtcp-5axis-linuxcnc-kinematics-runtime");
+  assert.equal(runtime.moduleId, testCase.moduleId);
+  assert.equal(runtime.loaded, true);
+  assert.equal(runtime.sourceMode, "source-derived-kinematics-wasm");
+  assert.equal(runtime.semanticBoundary, "linuxcnc_kinematics_wasm_c_abi");
+  assert.equal(runtime.executionContext, "direct");
+  assert.equal(runtime.wasmFile, testCase.wasmFile);
+  assert.equal(runtime.switchkinsType, 1);
+  assert.equal(readiness.loaded, true);
+  assert.equal(readiness.executionContext, "direct");
+  assert.equal(readiness.supportedModules.includes("xyzac-trt"), true);
+  assert.equal(readiness.supportedModules.includes("xyzbc-trt"), true);
+  assert.equal(runtime.switchRc, 0);
 
-assert.equal(frame.sourceMode, "source-derived-kinematics-wasm");
-assert.equal(frame.semanticBoundary, "linuxcnc_kinematics_wasm_c_abi");
-assert.equal(frame.readiness.linuxCncKinematicsReady, true);
-assert.equal(frame.readiness.promotionAllowed, true);
-assert.equal(frame.readiness.fullLinuxCncProgramExecutionReady, false);
-assert.equal(frame.kinematicsModuleId, "xyzac-trt");
-assert.equal(frame.kinematicsForwardRc, 0);
-assert.equal(frame.kinematicsInverseRc, 0);
-assert.equal(frame.jointPose[3].value, 25);
-assert.equal(frame.jointPose[4].value, 40);
-assert.equal(frame.tcpPose.x, linuxCncKinematicsResult.forward.pose.x);
-assert.equal(frame.tcpPose.y, linuxCncKinematicsResult.forward.pose.y);
-assert.equal(frame.tcpPose.z, linuxCncKinematicsResult.forward.pose.z);
+  const linuxCncKinematicsResult = runtime.frameForJoints(testCase.joints);
+  assert.equal(linuxCncKinematicsResult.moduleId, testCase.moduleId);
+  assert.equal(linuxCncKinematicsResult.switchkinsType, 1);
+  assert.equal(linuxCncKinematicsResult.forward.rc, 0);
+  assert.equal(linuxCncKinematicsResult.inverse.rc, 0);
+  assert.deepEqual(roundJoints(linuxCncKinematicsResult.inverse.joints), testCase.joints);
+
+  const frame = buildRtcpFrame({
+    axisPose: testCase.axisPose,
+    activeLine: 777,
+    kinsType: testCase.kinsType,
+    rtcpEnabled: true,
+    profile: getFiveAxisProfile(testCase.moduleId),
+    linuxCncKinematicsResult,
+  });
+
+  assert.equal(frame.sourceMode, "source-derived-kinematics-wasm");
+  assert.equal(frame.semanticBoundary, "linuxcnc_kinematics_wasm_c_abi");
+  assert.equal(frame.readiness.linuxCncKinematicsReady, true);
+  assert.equal(frame.readiness.promotionAllowed, true);
+  assert.equal(frame.readiness.fullLinuxCncProgramExecutionReady, false);
+  assert.equal(frame.profileId, testCase.moduleId);
+  assert.equal(frame.kinematicsModuleId, testCase.moduleId);
+  assert.equal(frame.kinematicsSwitchkinsType, 1);
+  assert.equal(frame.kinematicsForwardRc, 0);
+  assert.equal(frame.kinematicsInverseRc, 0);
+  assert.equal(frame.jointPose[3].axis, testCase.fourthAxis);
+  assert.equal(frame.jointPose[3].value, testCase.joints[3]);
+  assert.equal(frame.jointPose[4].axis, "C");
+  assert.equal(frame.jointPose[4].value, testCase.joints[4]);
+  assert.equal(frame.tcpPose.x, linuxCncKinematicsResult.forward.pose.x);
+  assert.equal(frame.tcpPose.y, linuxCncKinematicsResult.forward.pose.y);
+  assert.equal(frame.tcpPose.z, linuxCncKinematicsResult.forward.pose.z);
+  assert.equal(frame.tcpPose.a, linuxCncKinematicsResult.forward.pose.a);
+  assert.equal(frame.tcpPose.b, linuxCncKinematicsResult.forward.pose.b);
+  assert.equal(frame.tcpPose.c, linuxCncKinematicsResult.forward.pose.c);
+  assert.equal(frame.tcpPose[testCase.fourthKey], testCase.axisPose[testCase.fourthKey]);
+  assertVectorNear(frame.toolAxisVector, testCase.expectedToolAxis);
+
+  console.log(`${testCase.moduleId.replace(/-/g, "_")}_kinematics_runtime=ok`);
+}
 
 console.log("linuxcnc_kinematics_runtime_smoke=ok");
+console.log("xyzbc_b_axis_tcp_pose_preserved=1");
+
+function roundJoints(joints) {
+  return joints.map((value) => Math.round(value * 1e6) / 1e6);
+}
+
+function xyzacToolAxis(aDegrees, cDegrees) {
+  const tilt = aDegrees * DEG_TO_RAD;
+  const c = cDegrees * DEG_TO_RAD;
+  return {
+    x: Math.sin(tilt) * Math.sin(c),
+    y: -Math.sin(tilt) * Math.cos(c),
+    z: Math.cos(tilt),
+  };
+}
+
+function xyzbcToolAxis(bDegrees, cDegrees) {
+  const tilt = bDegrees * DEG_TO_RAD;
+  const c = cDegrees * DEG_TO_RAD;
+  return {
+    x: Math.sin(tilt) * Math.cos(c),
+    y: Math.sin(tilt) * Math.sin(c),
+    z: Math.cos(tilt),
+  };
+}
+
+function assertVectorNear(actual, expected, tolerance = 1e-12) {
+  assert.ok(Math.abs(actual.x - expected.x) < tolerance);
+  assert.ok(Math.abs(actual.y - expected.y) < tolerance);
+  assert.ok(Math.abs(actual.z - expected.z) < tolerance);
+}