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

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,
  };
}