提交 xyzbc-trt 界面与验证更新

web-rtcp-5axis-xyzbc-trt-sim-plan/app/dist/src/runtime/rtcp-frame.js

@@ -0,0 +1,214 @@
+import { xyzacTrtProfile } from "../profiles/xyzac-trt.js";
+
+const DEG_TO_RAD = Math.PI / 180;
+
+export function buildRtcpFrame({
+  axisPose,
+  activeLine,
+  kinsType = "identity",
+  rtcpEnabled = false,
+  sourceMode = "fixture-ui-only",
+  profile = xyzacTrtProfile,
+  linuxCncKinematicsResult = null,
+}) {
+  if (linuxCncKinematicsResult) {
+    return buildLinuxCncKinematicsFrame({
+      axisPose,
+      activeLine,
+      kinsType,
+      rtcpEnabled,
+      sourceMode,
+      profile,
+      linuxCncKinematicsResult,
+    });
+  }
+
+  const pose = normalizeAxisPose(axisPose);
+  const toolLength = 84.019;
+  const toolAxisVector = computeToolAxisVector(pose, profile);
+  const compensation = rtcpEnabled
+    ? {
+        x: -toolAxisVector.x * toolLength,
+        y: -toolAxisVector.y * toolLength,
+        z: -toolAxisVector.z * toolLength,
+      }
+    : { x: 0, y: 0, z: 0 };
+  const tcpPose = {
+    x: pose.x + compensation.x,
+    y: pose.y + compensation.y,
+    z: pose.z + compensation.z,
+    a: pose.a,
+    b: pose.b,
+    c: pose.c,
+  };
+
+  return {
+    apiName: "web-rtcp-5axis-motion-frame",
+    profileId: profile.id,
+    sourceMode,
+    semanticBoundary:
+      sourceMode === "fixture-ui-only"
+        ? "fixture_frame_ui_plumbing_not_linuxcnc_kinematics_proof"
+        : "linuxcnc_or_source_derived_runtime_required",
+    activeLine,
+    kinsType,
+    rtcpEnabled,
+    rtcpState: rtcpEnabled ? "on" : "off",
+    axisPose: pose,
+    jointPose: buildJointPose(pose, profile),
+    tcpPose,
+    toolAxisVector,
+    compensation,
+    toolLength,
+    readiness: {
+      frameReady: true,
+      uiReady: true,
+      profileReady: true,
+      linuxCncKinematicsReady: false,
+      promotionAllowed: false,
+    },
+  };
+}
+
+function buildLinuxCncKinematicsFrame({
+  axisPose,
+  activeLine,
+  kinsType,
+  rtcpEnabled,
+  profile,
+  linuxCncKinematicsResult,
+}) {
+  const forward = linuxCncKinematicsResult.forward || {};
+  const inverse = linuxCncKinematicsResult.inverse || {};
+  const wasmPose = normalizeLinuxCncPose(forward.pose);
+  const pose = normalizeAxisPose({
+    ...axisPose,
+    ...wasmPose,
+  });
+  const jointValues = Array.isArray(inverse.joints) ? inverse.joints : [];
+  const toolAxisVector = computeToolAxisVector(pose, profile);
+
+  return {
+    apiName: "web-rtcp-5axis-motion-frame",
+    profileId: profile.id,
+    sourceMode: "source-derived-kinematics-wasm",
+    semanticBoundary: "linuxcnc_kinematics_wasm_c_abi",
+    activeLine,
+    kinsType,
+    rtcpEnabled,
+    rtcpState: rtcpEnabled ? "on" : "off",
+    axisPose: pose,
+    jointPose: buildJointPoseFromLinuxCncJoints(jointValues, pose, profile),
+    tcpPose: {
+      x: pose.x,
+      y: pose.y,
+      z: pose.z,
+      a: pose.a,
+      b: pose.b,
+      c: pose.c,
+    },
+    toolAxisVector,
+    compensation: { x: 0, y: 0, z: 0 },
+    toolLength: 84.019,
+    kinematicsModuleId: linuxCncKinematicsResult.moduleId,
+    kinematicsSwitchkinsType: linuxCncKinematicsResult.switchkinsType,
+    kinematicsForwardRc: forward.rc,
+    kinematicsInverseRc: inverse.rc,
+    kinematicsFlags: {
+      fflags: forward.fflags,
+      iflags: forward.iflags,
+      inverseFflags: inverse.fflags,
+      inverseIflags: inverse.iflags,
+    },
+    readiness: {
+      frameReady: true,
+      uiReady: true,
+      profileReady: true,
+      linuxCncKinematicsReady: forward.rc === 0 && inverse.rc === 0,
+      promotionAllowed: forward.rc === 0 && inverse.rc === 0,
+      fullLinuxCncProgramExecutionReady: false,
+    },
+  };
+}
+
+function normalizeAxisPose(axisPose) {
+  return {
+    x: Number(axisPose?.x ?? 0),
+    y: Number(axisPose?.y ?? 0),
+    z: Number(axisPose?.z ?? 0),
+    a: Number(axisPose?.a ?? 0),
+    b: Number(axisPose?.b ?? 0),
+    c: Number(axisPose?.c ?? 0),
+  };
+}
+
+function buildJointPose(pose, profile = xyzacTrtProfile) {
+  return axesForProfile(profile).map((axis, joint) => ({
+    joint,
+    axis,
+    value: pose[axis.toLowerCase()] ?? 0,
+  }));
+}
+
+function buildJointPoseFromLinuxCncJoints(joints, fallbackPose, profile = xyzacTrtProfile) {
+  const axes = axesForProfile(profile);
+  const fallbackValues = axes.map((axis) => fallbackPose[axis.toLowerCase()] ?? 0);
+  return axes.map((axis, joint) => ({
+    joint,
+    axis,
+    value: Number(joints[joint] ?? fallbackValues[joint] ?? 0),
+  }));
+}
+
+function axesForProfile(profile = xyzacTrtProfile) {
+  const coordinates = profile?.coordinates?.length
+    ? profile.coordinates
+    : String(profile?.traj?.coordinates || "XYZAC").split("");
+  return coordinates.map((axis) => String(axis).toUpperCase()).filter(Boolean);
+}
+
+function normalizeLinuxCncPose(pose = {}) {
+  return {
+    x: Number(pose.x ?? pose.tran?.x ?? 0),
+    y: Number(pose.y ?? pose.tran?.y ?? 0),
+    z: Number(pose.z ?? pose.tran?.z ?? 0),
+    a: Number(pose.a ?? 0),
+    b: Number(pose.b ?? 0),
+    c: Number(pose.c ?? 0),
+  };
+}
+
+function computeToolAxisVector(pose, profile = xyzacTrtProfile) {
+  const coordinates = profile?.traj?.coordinates || "XYZAC";
+  const tiltDegrees = coordinates.includes("B") ? pose.b : pose.a;
+  const cDegrees = pose.c;
+  const tilt = tiltDegrees * DEG_TO_RAD;
+  const c = cDegrees * DEG_TO_RAD;
+  const sinTilt = Math.sin(tilt);
+  const cosTilt = Math.cos(tilt);
+  const sinC = Math.sin(c);
+  const cosC = Math.cos(c);
+
+  if (coordinates.includes("B")) {
+    return normalizeVector({
+      x: sinTilt * cosC,
+      y: sinTilt * sinC,
+      z: cosTilt,
+    });
+  }
+
+  return normalizeVector({
+    x: sinTilt * sinC,
+    y: -sinTilt * cosC,
+    z: cosTilt,
+  });
+}
+
+function normalizeVector(vector) {
+  const length = Math.hypot(vector.x, vector.y, vector.z) || 1;
+  return {
+    x: vector.x / length,
+    y: vector.y / length,
+    z: vector.z / length,
+  };
+}