Complete XYZAC switchkins transitions

web/worker/sim-session.mjs

@@ -476,6 +476,9 @@ export class SimSession {
     const program = this.xyzacProgram.snapshot();
     const io = this.xyzacIo?.canonical?.() ?? {};
     const wasmMotion = this.wasmMotion.read();
+    if (input.kind === "KINEMATICS_SELECT" && wasmMotion.status === "verified" && Number(wasmMotion.state.queue_depth ?? 0) > 0) {
+      return { action: input.payload?.type === 0 ? "switchkinsIdentity" : input.payload?.type === 1 ? "switchkinsXyzac" : "switchkinsUserk", enabled: false, reason: "xyzac_motion_active", taskState: task.state, interpState: "READING", wasm: { queue_depth: wasmMotion.state.queue_depth, active_id: wasmMotion.state.active_id } };
+    }
     const homeLimit = this.wasmHomeLimit.read();
     const estop = Boolean(io.estop);
     const ready = Boolean(io.ready) && !estop;
@@ -528,6 +531,22 @@ export class SimSession {
     return { action, enabled: result === 0, reason, taskState: task.state, interpState: task.state === "PAUSED" || program.status === 3 ? "PAUSED" : task.state === "RUNNING" || program.status === 2 ? "READING" : "IDLE", wasm: { io_ready: ready, io_estop: estop, program_status: program.program_status, queue_depth: wasmMotion.state.queue_depth, gate_result: result } };
   }
 
+  switchXyzacKinematics(mode) {
+    if (!this.xyzacKinematics) return { status: "error", code: "xyzac_kinematics_unavailable" };
+    const current = this.wasmMotion.read();
+    if (current.status !== "verified") return { status: "error", code: "wasm_motion_unavailable" };
+    if (Number(current.state.queue_depth ?? 0) > 0) return { status: "error", code: "xyzac_motion_active" };
+    const joints = Array.isArray(current.state.joint_commanded) && current.state.joint_commanded.length === 5
+      ? [...current.state.joint_commanded]
+      : [...this.xyzacKinematics.snapshot().joints];
+    const selected = this.xyzacKinematics.select(mode, joints);
+    if (selected.status !== "verified") return selected;
+    this.xyzacProgramKinematicsMode = mode;
+    if (typeof this.wasmMotion.setActiveKinematicsTag === "function") this.wasmMotion.setActiveKinematicsTag(mode);
+    if (selected.pose && typeof this.wasmMotion.setPosition === "function") this.wasmMotion.setPosition(selected.pose);
+    return { ...selected, kinematics: this.xyzacKinematics.snapshot(), wasm_motion: this.wasmMotion.read() };
+  }
+
   stateForCheckpoint() {
     const state = {
       context_id: this.contextId,
@@ -2095,9 +2114,9 @@ export class SimSession {
             const controlCode = Number(result.program?.control_code ?? 0);
             if ([428, 429, 430].includes(controlCode) && this.xyzacKinematics) {
               const mode = { 428: XYZAC_MODES.xyzac, 429: XYZAC_MODES.identity, 430: XYZAC_MODES.userk }[controlCode];
-              const selected = this.xyzacKinematics.select(mode, this.xyzacKinematics.snapshot().joints);
+              const selected = this.switchXyzacKinematics(mode);
               if (selected.status !== "verified") return finish(this.error(input, "kinematics", selected.code ?? "xyzac_switchkins_failed"), "failed");
-              result.kinematics = this.xyzacKinematics.snapshot();
+              result.kinematics = selected.kinematics;
               result.remap = { code: `M${controlCode}`, hal_pin: "motion.switchkins-type", hal_pin_vars: true, sync: "M66 E0 L0" };
             }
             this.task.setMode("MDI");
@@ -2123,9 +2142,9 @@ export class SimSession {
         const mode = Number(input.payload?.type);
         if (![XYZAC_MODES.identity, XYZAC_MODES.xyzac, XYZAC_MODES.userk].includes(mode)) return finish(this.error(input, "kinematics", "xyzac_kinematics_type_invalid"), "failed");
         this.task.setMode("MDI");
-        const result = this.xyzacKinematics.select(mode, this.xyzacKinematics.snapshot().joints);
+        const result = this.switchXyzacKinematics(mode);
         const code = mode === XYZAC_MODES.xyzac ? "M428" : mode === XYZAC_MODES.identity ? "M429" : "M430";
-        return finish(this.envelope(result.status === "verified" ? "COMMAND_RESULT" : "ERROR", input, { ...result, command: input.kind, remap: { code, hal_pin: "motion.switchkins-type", hal_pin_vars: true, sync: "M66 E0 L0" }, kinematics: this.xyzacKinematics.snapshot(), task: this.task.export(), operation_id: input.operation_id ?? null }), result.status === "verified" ? "verified" : "failed");
+        return finish(this.envelope(result.status === "verified" ? "COMMAND_RESULT" : "ERROR", input, { ...result, command: input.kind, remap: { code, hal_pin: "motion.switchkins-type", hal_pin_vars: true, sync: "M66 E0 L0" }, kinematics: result.kinematics ?? this.xyzacKinematics.snapshot(), task: this.task.export(), operation_id: input.operation_id ?? null }), result.status === "verified" ? "verified" : "failed");
       }
       case "VISUAL_CLEAR":
         return visualClearResponse();

web/worker/test_xyzac_trt_case.mjs

@@ -48,6 +48,23 @@ for (const [code, expected] of [["M429", 0], ["M428", 1], ["M430", 2]]) {
   switchRows.push({ code, tick: result.simulation_tick, active_type: expected, state_hash: result.payload.panel_snapshot.state_hash });
 }
 
+// Direct panel selection must preserve the live WASM TP pose at a non-zero
+// position for every pair of IDENTITY/TCP:XYZAC/USERK transitions.
+const move = send("MDI_EXECUTE", "seed-position", { block: "G0 X10 Y5 Z20 A30 C45" });
+assert.equal(move.kind, "COMMAND_RESULT");
+for (let cycle = 0; cycle < 1000 && session.wasmMotion.read().state.queue_depth > 0; cycle += 1) send("SIM_STEP", `seed-step-${cycle}`, { cycles: 1 });
+assert.equal(session.wasmMotion.read().state.queue_depth, 0);
+const directSwitches = [];
+for (const expected of [0, 1, 2, 0, 1, 2, 0]) {
+  const result = send("KINEMATICS_SELECT", `direct-${expected}-${seq}`, { type: expected });
+  assert.equal(result.kind, "COMMAND_RESULT");
+  assert.equal(result.payload.kinematics.active_type, expected);
+  const pose = result.payload.kinematics.pose;
+  const tpPose = result.payload.wasm_motion.state.position;
+  for (const axis of ["x", "y", "z", "a", "b", "c", "u", "v", "w"]) assert.ok(Math.abs(Number(pose[axis]) - Number(tpPose[axis])) <= 1e-9, `${axis}: ${pose[axis]} != ${tpPose[axis]}`);
+  directSwitches.push({ active_type: expected, state_hash: result.payload.kinematics.state_hash });
+}
+
 const pose = session.xyzacKinematics.forward([100, 50, 100, 30, 45]);
 assert.equal(pose.status, "verified");
 assert.equal(pose.joint_count, 5);
@@ -70,6 +87,7 @@ console.log(JSON.stringify({
   motion_authority: loaded.payload.motion_authority,
   control_count: controls.length,
   switchkins: switchRows,
+  direct_switchkins: directSwitches,
   pose: pose.pose,
   wasm_state_hash: pose.wasm_state_hash,
   state_hash: pose.state_hash,