From d18141ec81c103e791ac6e0d38cc78611027f7c3 Mon Sep 17 00:00:00 2001 From: wangdequan Date: Tue, 1 Sep 2026 11:44:05 +0800 Subject: [PATCH] Run XYZAC through upstream LinuxCNC TP --- coverage/tp-parity-release-state.report.json | 10 +- .../xyzac-native-wasm-tick-parity.report.json | 30 +- doc-rtcp/xyzac-trt-full-tp-parity-plan.md | 11 +- .../0001-export-native-task-heartbeat.patch | 64 ++- .../linuxcnc/0002-native-servo-trace.patch | 69 +++ scripts/build_linuxcnc_native_oracle.sh | 10 +- .../compare_xyzac_native_wasm_tick_trace.py | 72 +++- scripts/run_xyzac_native_tick_trace.py | 93 ++++- scripts/run_xyzac_wasm_tick_trace.mjs | 90 +++- wasm-core/CMakeLists.txt | 58 ++- wasm-core/include/sim_kinematics.h | 8 + wasm-core/include/sim_linuxcnc_tp.h | 52 +++ wasm-core/src/linuxcnc_tp_compat.c | 14 + wasm-core/src/sim_linuxcnc_tp.c | 336 +++++++++++++++ wasm-core/src/sim_switchkins.c | 31 ++ wasm-core/tests/sim_linuxcnc_tp_test.c | 39 ++ web/react/src/sim-worker-entry.ts | 2 +- web/worker/sim-session.mjs | 395 +++++++++++++++--- web/worker/sim-worker.mjs | 2 +- web/worker/wasm-linuxcnc-tp.mjs | 254 +++++++++++ web/worker/wasm-program-runtime.mjs | 27 +- web/worker/wasm-xyzac-kinematics.mjs | 20 +- web/workspace/sim-case-loader.mjs | 2 +- 23 files changed, 1550 insertions(+), 139 deletions(-) create mode 100644 patches/linuxcnc/0002-native-servo-trace.patch create mode 100644 wasm-core/include/sim_linuxcnc_tp.h create mode 100644 wasm-core/src/linuxcnc_tp_compat.c create mode 100644 wasm-core/src/sim_linuxcnc_tp.c create mode 100644 wasm-core/tests/sim_linuxcnc_tp_test.c create mode 100644 web/worker/wasm-linuxcnc-tp.mjs diff --git a/coverage/tp-parity-release-state.report.json b/coverage/tp-parity-release-state.report.json index 5232700..c46616b 100644 --- a/coverage/tp-parity-release-state.report.json +++ b/coverage/tp-parity-release-state.report.json @@ -8,13 +8,13 @@ {"task_id": "TP-PARITY-03", "status": "blocked_by_TP-PARITY-01", "evidence": null}, {"task_id": "TP-PARITY-04", "status": "blocked_by_TP-PARITY-01", "evidence": null}, {"task_id": "TP-PARITY-05", "status": "blocked_by_TP-PARITY-01", "evidence": null}, - {"task_id": "TP-PARITY-06", "status": "diverged", "evidence": "coverage/xyzac-native-wasm-tick-parity.report.json"}, + {"task_id": "TP-PARITY-06", "status": "in_progress", "evidence": "coverage/xyzac-native-wasm-tick-parity.report.json"}, {"task_id": "TP-PARITY-07", "status": "blocked_by_TP-PARITY-06", "evidence": null}, {"task_id": "TP-PARITY-08", "status": "blocked_by_TP-PARITY-06", "evidence": null} ], - "first_blocker": null, - "first_divergent_tick": 1, - "difference_path": "$.rows[1].exec_state", + "first_blocker": "TP-PARITY-06", + "first_divergent_tick": 4003, + "difference_path": "$.segments[17][4003].motion_line", "release_status": "STOP_RELEASE", - "reason": "The native oracle is available, but the existing WASM planner diverges from source LinuxCNC before full TP field parity is established." + "reason": "Native oracle capture is complete (75035 servo rows, 28 motion segments). WASM uses upstream LinuxCNC TP/TC and produces the same 28-segment source run (73621 rows). Deterministic TP fields match through the common prefix, but full release remains blocked: first full-trace difference is task motion_line at tick 4003 and later segment durations are not identical. Oracle availability and complete TP numerical parity remain separate gates." } diff --git a/coverage/xyzac-native-wasm-tick-parity.report.json b/coverage/xyzac-native-wasm-tick-parity.report.json index 2f4e7ec..196938b 100644 --- a/coverage/xyzac-native-wasm-tick-parity.report.json +++ b/coverage/xyzac-native-wasm-tick-parity.report.json @@ -5,14 +5,16 @@ "scope": "xyzac_only" }, "comparison": { - "difference_path": "$.rows[1].exec_state", - "first_divergent_tick": 1, - "native": 3, + "category": "authoritative", + "difference_path": "$.segments[17][4003].motion_line", + "first_divergent_tick": 4003, + "motion_id": 17, + "native": 16, "status": "diverged", - "wasm": 2 + "wasm": 17 }, - "difference_path": "$.rows[1].exec_state", - "first_divergent_tick": 1, + "difference_path": "$.segments[17][4003].motion_line", + "first_divergent_tick": 4003, "known_boundaries": [ "native public stat does not expose TP segment geometry", "WASM public snapshot does not expose acceleration/active blend geometry", @@ -20,20 +22,20 @@ ], "native": { "report": "coverage/xyzac-native-tick-trace.report.json", - "report_sha256": "881571d0c75017a9610023a2d022d26606e653a0c62575563c0da7ada600e9a2", - "row_count": 201, - "trace_sha256": "173b5a5644254adef9ca5fd3b4fb2919889cce938d3aae7079c5004a4bfb364f" + "report_sha256": "d61547a640af0a94ebefbb1330e8080c3b25aca99c5e7b614ccdbd35e64de5a1", + "row_count": 75035, + "trace_sha256": "48dddc7d1c8281a092e17b80017b6951bae5b5248601f6fd1124d31eae74878b" }, "release_status": "STOP_RELEASE", - "report_sha256": "3ce76d65aee5405c01efca36b859b63da476f5f4d3bc4666be93691458fb86b3", + "report_sha256": "878a04ce1a6cbf13a5b3f61a7cf919d6fa7a32938b4457a7ea019d6d741441e0", "schema": "xyzac-native-wasm-tick-parity.v1", "status": "diverged", "wasm": { "report": "coverage/xyzac-wasm-tick-trace.report.json", - "report_sha256": "21cbdaa8eaa1eda94ca107095ba7931cb5bd151c80e75c9b7dd9ec6149a71c73", - "row_count": 202, - "runner_output_sha256": "5f493d9e726ec7bf656f738f92b61a17e0b6455562e68c85199c282d58817843", + "report_sha256": "1258980bf8eabd5fefc76736034531eebecfc7644fc334a0eea60d64a21f4d4f", + "row_count": 73621, + "runner_output_sha256": "794805f21d4a8ce6c6cfd585b98c17d79e41c068ba94918e38e8aeface854d7b", "runner_returncode": 0, - "trace_sha256": "4202692d7f92ad9d4bfacb0ec1bb88eaad9e5b308e3a016c670f8fc4a73a5612" + "trace_sha256": "e07d58e39d5c267297f4a1077b91cb7a8eb523f17edbad744f0ff9fda0b646f8" } } diff --git a/doc-rtcp/xyzac-trt-full-tp-parity-plan.md b/doc-rtcp/xyzac-trt-full-tp-parity-plan.md index 5c318aa..b3a1aa5 100644 --- a/doc-rtcp/xyzac-trt-full-tp-parity-plan.md +++ b/doc-rtcp/xyzac-trt-full-tp-parity-plan.md @@ -1,14 +1,15 @@ # XYZAC TRT 完整 TP/前瞻数值对标任务轨道 -状态：`TP-PARITY-00` 已 verified，完整 LinuxCNC 数值对标尚未完成。 +状态：`TP-PARITY-00` native oracle 已 verified；WASM 已接入上游 LinuxCNC TP/TC 源码并完成同一 28 段运行，但完整数值对标仍为 `STOP_RELEASE`。 当前 oracle 证据：`coverage/xyzac-native-tick-trace.report.json` 已由源 `run-5axis-rtcp.sh` 产生连续 task-heartbeat tick；源 Python NML 扩展暴露 `task_heartbeat`，并记录 XYZAC pose/joints、queue、current-line、速度和加速度。 干净 checkout 使用 `scripts/build_linuxcnc_native_oracle.sh` 应用并编译同一源 patch。 `scripts/compare_xyzac_native_wasm_tick_trace.py` 已接入同一 case 的 WASM trace， -当前首个差异保留为 `$.rows[1].exec_state`，因此不得把 TP-PARITY-01..08 写成 -数值 parity verified。 +当前完整窗口首个差异为 `$.segments[17][4003].motion_line`；共同 TP 前缀的 +position、velocity、acceleration、queue、joint command/feedback 已逐 tick 对齐， +但后续 segment duration 尚未闭合，因此不得把 TP-PARITY-01..08 写成数值 parity verified。 目标不是证明页面能绘制一条曲线，而是对同一 `xyzac-trt.ini`、同一 G-code、同一初始状态，比较 native LinuxCNC 与 WASM Worker 的每个运动 tick： @@ -52,8 +53,8 @@ Web 端只有在该链路和 native trace 逐字段一致后，才能声明 `PAR ### TP-PARITY-01：锁定 LinuxCNC TP 数据模型和 C ABI -状态：`in_progress`。native tick oracle 已可用；当前 comparator 显示现有 -WASM queue 与源 TP 的执行顺序/速度尚未一致。 +状态：`in_progress`。生产 Worker 的 `planner:"linuxcnc-tp"` 直接调用上游 TP/TC +源码切片；native/WASM 完整窗口仍报告 task 元数据和后续 segment duration 差异。 来源：`linuxcnc/src/emc/tp/{tp.c,tp.h,tp_types.h,tc.c,tcq.c,tc_types.h,blendmath.c,blendmath.h,spherical_arc.c}`。 diff --git a/patches/linuxcnc/0001-export-native-task-heartbeat.patch b/patches/linuxcnc/0001-export-native-task-heartbeat.patch index 15a0667..1634665 100644 --- a/patches/linuxcnc/0001-export-native-task-heartbeat.patch +++ b/patches/linuxcnc/0001-export-native-task-heartbeat.patch @@ -1,7 +1,39 @@ +diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc +index 59fdabe..cdba7ed 100644 +--- a/src/emc/nml_intf/emc.cc ++++ b/src/emc/nml_intf/emc.cc +@@ -1838,6 +1838,7 @@ void EMC_TRAJ_STAT::update(CMS * cms) + cms->update(probeval); + cms->update(kinematics_type); + cms->update(motion_type); ++ cms->update(motion_heartbeat); + + } + +@@ -2261,4 +2262,4 @@ void EMC_MOTION_ADAPTIVE::update(CMS * cms) + void EMC_MOTION_CMD_MSG::update(CMS * cms) + { + +-} +\ No newline at end of file ++} +diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh +index 2d5e61e..8c713b7 100644 +--- a/src/emc/nml_intf/emc_nml.hh ++++ b/src/emc/nml_intf/emc_nml.hh +@@ -834,6 +834,7 @@ class EMC_TRAJ_STAT:public EMC_TRAJ_STAT_MSG { + //bool spindle_override_enabled; moved to SPINDLE_STAT + bool adaptive_feed_enabled; + bool feed_hold_enabled; ++ uint32_t motion_heartbeat; // native motion-controller servo-cycle counter + StateTag tag; + }; + diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc +index 6d5ebd5..d215e2e 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc -@@ -3490,4 +3490,9 @@ int main(int argc, char *argv[]) +@@ -3490,6 +3490,11 @@ int main(int argc, char *argv[]) // since emcStatus was passed to the WM init functions, it // will be updated in the _update() functions above. There's // no need to call the individual functions on all WM items. @@ -11,16 +43,40 @@ diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc + // wall-clock sampling. + emcStatus->task.heartbeat++; emcStatusBuffer->write(emcStatus); - + + // wait on timer cycle, if specified, or calculate actual +diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc +index 36fbd97..b3ae2aa 100644 +--- a/src/emc/task/taskintf.cc ++++ b/src/emc/task/taskintf.cc +@@ -1612,6 +1612,7 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat) + stat->feed_override_enabled = enables & FS_ENABLED; + stat->adaptive_feed_enabled = enables & AF_ENABLED; + stat->feed_hold_enabled = enables & FH_ENABLED; ++ stat->motion_heartbeat = emcmotStatus.heartbeat; + + if (new_config) { + stat->cycleTime = emcmotConfig.trajCycleTime; diff --git a/src/emc/usr_intf/axis/extensions/emcmodule.cc b/src/emc/usr_intf/axis/extensions/emcmodule.cc +index b43f041..96f12a2 100644 --- a/src/emc/usr_intf/axis/extensions/emcmodule.cc +++ b/src/emc/usr_intf/axis/extensions/emcmodule.cc -@@ -409,4 +409,7 @@ static PyMemberDef Stat_members[] = { +@@ -409,6 +409,9 @@ static PyMemberDef Stat_members[] = { {(char*)"echo_serial_number", T_INT, O(echo_serial_number), READONLY}, {(char*)"echo_serial_number", T_INT, O(echo_serial_number), READONLY}, {(char*)"state", T_INT, O(status), READONLY}, + {(char*)"task_heartbeat", T_UINT, O(task.heartbeat), READONLY, + "Native EMC task-loop heartbeat. This counter is incremented by the " + "LinuxCNC task thread and is suitable for identifying native status ticks."}, - + // task + {(char*)"task_mode", T_INT, O(task.mode), READONLY}, +@@ -473,6 +476,8 @@ static PyMemberDef Stat_members[] = { + {(char*)"feed_override_enabled", T_BOOL, O(motion.traj.feed_override_enabled), READONLY}, + {(char*)"adaptive_feed_enabled", T_BOOL, O(motion.traj.adaptive_feed_enabled), READONLY}, + {(char*)"feed_hold_enabled", T_BOOL, O(motion.traj.feed_hold_enabled), READONLY}, ++ {(char*)"motion_heartbeat", T_UINT, O(motion.traj.motion_heartbeat), READONLY, ++ "Native motion-controller servo-cycle counter."}, + {(char*)"num_extrajoints", T_INT, O(motion.numExtraJoints), READONLY}, + + diff --git a/patches/linuxcnc/0002-native-servo-trace.patch b/patches/linuxcnc/0002-native-servo-trace.patch new file mode 100644 index 0000000..03d8898 --- /dev/null +++ b/patches/linuxcnc/0002-native-servo-trace.patch @@ -0,0 +1,69 @@ +diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c +index 8c9ce66..0ab0f5f 100644 +--- a/src/emc/motion/control.c ++++ b/src/emc/motion/control.c +@@ -19,6 +19,9 @@ + #ifdef SWITCHKINS_DEBUG + #include // rtpreempt only, consolidate to stderr + #endif ++#include ++#include ++#include + + #include "posemath.h" + #include "rtapi.h" +@@ -67,6 +70,46 @@ extern struct emcmot_status_t *emcmotStatus; + // etc. + static double *pcmd_p[EMCMOT_MAX_AXIS]; + ++/* Optional simulation-only servo oracle. It is disabled unless the caller ++ * provides a file path, so normal LinuxCNC motion timing is unchanged. */ ++static FILE *xyzac_trace_stream; ++static void xyzac_trace_tick(void) { ++ const char *path; ++ int joint; ++ struct timespec now; ++ clock_gettime(CLOCK_MONOTONIC, &now); ++ if (xyzac_trace_stream == NULL) { ++ path = getenv("LINUXCNC_XYZAC_TRACE_FILE"); ++ if (path == NULL || path[0] == '\0') return; ++ xyzac_trace_stream = fopen(path, "w"); ++ if (xyzac_trace_stream == NULL) return; ++ setvbuf(xyzac_trace_stream, NULL, _IOLBF, 0); ++ } ++ fprintf(xyzac_trace_stream, ++ "{\"wall_ns\":%llu,\"motion_heartbeat\":%u,\"position\":[%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g],\"actual_position\":[%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g,%.17g],\"velocity\":%.17g,\"acceleration\":%.17g,\"queue_depth\":%d,\"active_depth\":%d,\"motion_id\":%d,\"motion_type\":%d,\"paused\":%d,\"queue_full\":%d,\"joints\":[", ++ (unsigned long long)now.tv_sec * 1000000000ull + (unsigned long long)now.tv_nsec, ++ emcmotStatus->heartbeat, ++ emcmotStatus->carte_pos_cmd.tran.x, emcmotStatus->carte_pos_cmd.tran.y, emcmotStatus->carte_pos_cmd.tran.z, ++ emcmotStatus->carte_pos_cmd.a, emcmotStatus->carte_pos_cmd.b, emcmotStatus->carte_pos_cmd.c, ++ emcmotStatus->carte_pos_cmd.u, emcmotStatus->carte_pos_cmd.v, emcmotStatus->carte_pos_cmd.w, ++ emcmotStatus->carte_pos_fb.tran.x, emcmotStatus->carte_pos_fb.tran.y, emcmotStatus->carte_pos_fb.tran.z, ++ emcmotStatus->carte_pos_fb.a, emcmotStatus->carte_pos_fb.b, emcmotStatus->carte_pos_fb.c, ++ emcmotStatus->carte_pos_fb.u, emcmotStatus->carte_pos_fb.v, emcmotStatus->carte_pos_fb.w, ++ emcmotStatus->current_vel, emcmotStatus->acc, emcmotStatus->depth, ++ emcmotStatus->activeDepth, emcmotStatus->id, emcmotStatus->motionType, ++ emcmotStatus->paused, emcmotStatus->queueFull); ++ for (joint = 0; joint < EMCMOT_MAX_JOINTS; ++joint) { ++ if (joint != 0) fputc(',', xyzac_trace_stream); ++ fprintf(xyzac_trace_stream, "{\"cmd\":%.17g,\"fb\":%.17g,\"vel\":%.17g,\"acc\":%.17g,\"homed\":%d}", ++ emcmotStatus->joint_status[joint].pos_cmd, ++ emcmotStatus->joint_status[joint].pos_fb, ++ emcmotStatus->joint_status[joint].vel_cmd, ++ emcmotStatus->joint_status[joint].acc_cmd, ++ emcmotStatus->joint_status[joint].homed); ++ } ++ fputs("]}\n", xyzac_trace_stream); ++} ++ + /*********************************************************************** + * LOCAL FUNCTION PROTOTYPES * + ************************************************************************/ +@@ -271,6 +314,7 @@ void emcmotController(void *arg, long period) + update_status(); + /* here ends the core of the controller */ + emcmotStatus->heartbeat++; ++ xyzac_trace_tick(); + /* set tail to head, to indicate work complete */ + emcmotStatus->tail = emcmotStatus->head; + /* end of controller function */ diff --git a/scripts/build_linuxcnc_native_oracle.sh b/scripts/build_linuxcnc_native_oracle.sh index f202b9d..f6a3cff 100755 --- a/scripts/build_linuxcnc_native_oracle.sh +++ b/scripts/build_linuxcnc_native_oracle.sh @@ -5,9 +5,13 @@ ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)" SOURCE_ROOT="${ROOT}/linuxcnc" PATCH_FILE="${ROOT}/patches/linuxcnc/0001-export-native-task-heartbeat.patch" -if ! rg -q 'task_heartbeat' "${SOURCE_ROOT}/src/emc/usr_intf/axis/extensions/emcmodule.cc"; then +if ! rg -q 'task_heartbeat' "${SOURCE_ROOT}/src/emc/usr_intf/axis/extensions/emcmodule.cc" || ! rg -q 'motion_heartbeat' "${SOURCE_ROOT}/src/emc/usr_intf/axis/extensions/emcmodule.cc"; then patch --forward --batch -p1 -d "${SOURCE_ROOT}" < "${PATCH_FILE}" fi +TRACE_PATCH_FILE="${ROOT}/patches/linuxcnc/0002-native-servo-trace.patch" +if ! rg -q 'xyzac_trace_tick' "${SOURCE_ROOT}/src/emc/motion/control.c"; then + patch --forward --batch -p1 -d "${SOURCE_ROOT}" < "${TRACE_PATCH_FILE}" +fi -make -C "${SOURCE_ROOT}/src" ../lib/python/linuxcnc.so ../bin/milltask -printf '%s\n' 'LinuxCNC native oracle build ready: source linuxcnc.so + milltask' +make -C "${SOURCE_ROOT}/src" ../lib/python/linuxcnc.so ../bin/milltask ../rtlib/motmod.so +printf '%s\n' 'LinuxCNC native oracle build ready: source linuxcnc.so + milltask + motmod.so' diff --git a/scripts/compare_xyzac_native_wasm_tick_trace.py b/scripts/compare_xyzac_native_wasm_tick_trace.py index 34d0160..bc15aa8 100644 --- a/scripts/compare_xyzac_native_wasm_tick_trace.py +++ b/scripts/compare_xyzac_native_wasm_tick_trace.py @@ -6,6 +6,7 @@ from __future__ import annotations import hashlib import json import subprocess +import math from pathlib import Path from typing import Any @@ -53,25 +54,70 @@ def canonical_row(row: dict[str, Any]) -> dict[str, Any]: return result +def values_equal(left: Any, right: Any, absolute: float = 1e-9, relative: float = 1e-9) -> bool: + if isinstance(left, (int, float)) and isinstance(right, (int, float)) and not isinstance(left, bool) and not isinstance(right, bool): + scale = max(abs(float(left)), abs(float(right)), 1.0) + return abs(float(left) - float(right)) <= max(absolute, relative * scale) + if isinstance(left, list) and isinstance(right, list): + return len(left) == len(right) and all(values_equal(a, b, absolute, relative) for a, b in zip(left, right)) + return left == right + + def compare_rows(native: list[dict[str, Any]], wasm: list[dict[str, Any]]) -> dict[str, Any]: - fields = ("task_state", "task_mode", "interp_state", "exec_state", "current_line", "motion_line", "actual_pose", "commanded_pose", "joint_actual", "joint_commanded", "velocity", "queue_depth", "motion_id", "active_segment_id", "position", "homed", "limits", "kinematics_type", "motion_type", "paused") - for index in range(max(len(native), len(wasm))): - if index >= len(native) or index >= len(wasm): - return {"status": "diverged", "first_divergent_tick": index, "difference_path": f"$.rows[{index}]", "native": native[index] if index < len(native) else None, "wasm": wasm[index] if index < len(wasm) else None} - native_row = canonical_row(native[index]) - wasm_row = canonical_row(wasm[index]) - for field in fields: - if native_row.get(field) != wasm_row.get(field): - return {"status": "diverged", "first_divergent_tick": index, "difference_path": f"$.rows[{index}].{field}", "native": native_row.get(field), "wasm": wasm_row.get(field)} - return {"status": "verified", "first_divergent_tick": None, "difference_path": None} + authoritative_fields = ("task_state", "task_mode", "interp_state", "exec_state", "current_line", "motion_line", "commanded_pose", "joint_commanded", "velocity", "acceleration", "queue_depth", "active_queue", "queue_full", "motion_id", "active_segment_id", "position", "homed", "limits", "kinematics_type", "motion_type", "paused") + feedback_fields = ("actual_pose", "joint_actual") + def segments(rows: list[dict[str, Any]]) -> list[tuple[int, list[dict[str, Any]]]]: + result: list[tuple[int, list[dict[str, Any]]]] = [] + for row in rows: + motion_id = int(row.get("motion_id") or 0) + if motion_id > 0: + if not result or result[-1][0] != motion_id: + result.append((motion_id, [])) + result[-1][1].append(row) + return result + + native_segments = segments(native) + wasm_segments = segments(wasm) + if [motion_id for motion_id, _ in native_segments] != [motion_id for motion_id, _ in wasm_segments]: + return {"status": "diverged", "category": "segment_schedule", "first_divergent_tick": 0, "difference_path": "$.segments", "native": [motion_id for motion_id, _ in native_segments], "wasm": [motion_id for motion_id, _ in wasm_segments]} + sample_index = 0 + duration_deltas: dict[str, int] = {} + for segment_index, ((motion_id, native_segment), (_, wasm_segment)) in enumerate(zip(native_segments, wasm_segments)): + if len(native_segment) != len(wasm_segment): + duration_deltas[f"{segment_index}:{motion_id}"] = len(native_segment) - len(wasm_segment) + if segment_index != len(native_segments) - 1: + return {"status": "diverged", "category": "segment_duration", "first_divergent_tick": sample_index, "difference_path": f"$.segments[{segment_index}].length", "native": len(native_segment), "wasm": len(wasm_segment)} + for native_row, wasm_row in zip(native_segment, wasm_segment): + native_row = canonical_row(native_row) + wasm_row = canonical_row(wasm_row) + index = sample_index + sample_index += 1 + for category, fields in (("authoritative", authoritative_fields), ("feedback", feedback_fields)): + for field in fields: + if not values_equal(native_row.get(field), wasm_row.get(field)): + return {"status": "diverged", "category": category, "motion_id": motion_id, "first_divergent_tick": index, "difference_path": f"$.segments[{motion_id}][{index}].{field}", "native": native_row.get(field), "wasm": wasm_row.get(field)} + return {"status": "verified_partial" if duration_deltas else "verified", "category": "all", "first_divergent_tick": None, "difference_path": None, "duration_deltas": duration_deltas} def main() -> int: native = json.loads(NATIVE_REPORT.read_text(encoding="utf-8")) - wasm_ticks = max(1, min(1000, int(native.get("row_count") or 1))) - build = subprocess.run(["node", "scripts/run_xyzac_wasm_tick_trace.mjs", "--ticks", str(wasm_ticks), "--output", str(WASM_REPORT)], cwd=ROOT, text=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=False, timeout=120) + native_rows = native.get("rows", []) + target_rows = max(1, min(5000, int(native.get("row_count") or 1))) + elapsed_ms = max((float(row.get("native_elapsed_ms", 0.0)) for row in native_rows), default=0.0) + wasm_ticks = max(1, math.ceil(elapsed_ms / 10.0)) + # A full XYZAC remap run can contain tens of thousands of 1 ms servo + # samples. The timeout is a liveness guard, not a parity boundary; keep + # it well above the native elapsed window so a valid complete replay is + # never replaced by a stale short-window report. + timeout_seconds = max(1800, int(elapsed_ms / 1000.0 * 8 + 120)) + try: + build = subprocess.run(["node", "scripts/run_xyzac_wasm_tick_trace.mjs", "--ticks", str(wasm_ticks), "--output", str(WASM_REPORT)], cwd=ROOT, text=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=False, timeout=timeout_seconds) + except subprocess.TimeoutExpired as error: + output = error.stdout or "" + if isinstance(output, bytes): output = output.decode("utf-8", errors="replace") + build = subprocess.CompletedProcess(error.cmd, 124, stdout=output + "\nwasm_replay_timeout") wasm = json.loads(WASM_REPORT.read_text(encoding="utf-8")) if WASM_REPORT.is_file() else {"status": "blocked", "rows": []} - comparison = compare_rows(native.get("rows", []), wasm.get("rows", [])) if native.get("status") == "verified" and wasm.get("status") == "verified" else {"status": "blocked", "first_divergent_tick": None, "difference_path": None} + comparison = compare_rows(native.get("rows", [])[:target_rows], wasm.get("rows", [])[:target_rows]) if native.get("status") == "verified" and wasm.get("status") == "verified" else {"status": "blocked", "first_divergent_tick": None, "difference_path": None} report = { "schema": "xyzac-native-wasm-tick-parity.v1", "status": comparison["status"], diff --git a/scripts/run_xyzac_native_tick_trace.py b/scripts/run_xyzac_native_tick_trace.py index e55e637..cb19846 100644 --- a/scripts/run_xyzac_native_tick_trace.py +++ b/scripts/run_xyzac_native_tick_trace.py @@ -17,6 +17,7 @@ import re import signal import subprocess import sys +import tempfile import time from pathlib import Path from typing import Any @@ -62,16 +63,20 @@ def scalar(value: Any) -> float | int | bool | str | None: return number if number == number and abs(number) != float("inf") else None -def stat_row(stat: Any, sample_index: int, previous_taskbeat: int | None) -> dict[str, Any]: +def stat_row(stat: Any, sample_index: int, previous_tick: int | None) -> dict[str, Any]: stat.poll() taskbeat = scalar(getattr(stat, "task_heartbeat", None)) + motionbeat = scalar(getattr(stat, "motion_heartbeat", None)) + tick = motionbeat if isinstance(motionbeat, int) else taskbeat commanded_pose = finite_list(getattr(stat, "position", None)) distance_to_go = finite_list(getattr(stat, "dtg", None)) return { "sample_index": sample_index, "taskbeat": taskbeat, "task_heartbeat": taskbeat, - "taskbeat_changed": previous_taskbeat is None or taskbeat != previous_taskbeat, + "motion_heartbeat": motionbeat, + "tick": tick, + "taskbeat_changed": previous_tick is None or tick != previous_tick, "task_state": scalar(getattr(stat, "task_state", None)), "task_mode": scalar(getattr(stat, "task_mode", None)), "interp_state": scalar(getattr(stat, "interp_state", None)), @@ -79,6 +84,8 @@ def stat_row(stat: Any, sample_index: int, previous_taskbeat: int | None) -> dic "current_line": scalar(getattr(stat, "current_line", None)), "motion_line": scalar(getattr(stat, "motion_line", None)), "read_line": scalar(getattr(stat, "read_line", None)), + "command": getattr(stat, "command", None), + "file": getattr(stat, "file", None), "actual_pose": finite_list(getattr(stat, "actual_position", None)), "commanded_pose": commanded_pose, "joint_actual": finite_list(getattr(stat, "joint_actual_position", None)), @@ -154,6 +161,9 @@ def run(duration: float) -> dict[str, Any]: cleanup = cleanup_linuxcnc_services() environment = os.environ.copy() environment["DISPLAY"] = environment.get("DISPLAY", ":1") + trace_directory = tempfile.TemporaryDirectory(prefix="linuxcnc-xyzac-servo-trace-") + trace_path = Path(trace_directory.name) / "motion.jsonl" + environment["LINUXCNC_XYZAC_TRACE_FILE"] = str(trace_path) command = ["./run-5axis-rtcp.sh", "xyzac-trt.ini"] process = subprocess.Popen(command, cwd=ROOT, env=environment, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, start_new_session=True) output_lines: list[str] = [] @@ -164,6 +174,7 @@ def run(duration: float) -> dict[str, Any]: control: Any = None connected = False window: dict[str, Any] | None = None + run_start_motion_heartbeat: int | None = None try: deadline = time.monotonic() + 20 while time.monotonic() < deadline and process.poll() is None: @@ -222,6 +233,8 @@ def run(duration: float) -> dict[str, Any]: control.wait_complete(5) stage = "program_run" control.auto(linuxcnc.AUTO_RUN, 0) + stat.poll() + run_start_motion_heartbeat = int(getattr(stat, "motion_heartbeat", 0)) start = time.monotonic() sample_index = 0 @@ -229,16 +242,16 @@ def run(duration: float) -> dict[str, Any]: last_change = start while time.monotonic() - start < duration: row = stat_row(stat, sample_index, previous_taskbeat) - current_taskbeat = row["taskbeat"] + current_tick = row["tick"] if row["taskbeat_changed"] or not rows: row["native_elapsed_ms"] = round((time.monotonic() - start) * 1000.0, 3) rows.append(row) - previous_taskbeat = current_taskbeat if isinstance(current_taskbeat, int) else previous_taskbeat + previous_taskbeat = current_tick if isinstance(current_tick, int) else previous_taskbeat last_change = time.monotonic() if row["task_state"] == linuxcnc.STATE_OFF or row["interp_state"] == linuxcnc.INTERP_IDLE and row["queue_depth"] == 0 and len(rows) > 4: break sample_index += 1 - time.sleep(0.005) + time.sleep(0.001) final = stat_row(stat, sample_index + 1, previous_taskbeat) if final["taskbeat_changed"]: final["native_elapsed_ms"] = round((time.monotonic() - start) * 1000.0, 3) @@ -254,10 +267,75 @@ def run(duration: float) -> dict[str, Any]: pass cleanup_linuxcnc_services() + servo_rows: list[dict[str, Any]] = [] + if trace_path.is_file(): + for raw_line in trace_path.read_text(encoding="utf-8", errors="replace").splitlines(): + try: + value = json.loads(raw_line) + except json.JSONDecodeError: + continue + if isinstance(value, dict) and isinstance(value.get("motion_heartbeat"), int): + if run_start_motion_heartbeat is None or value["motion_heartbeat"] >= max(0, run_start_motion_heartbeat - 1): + servo_rows.append(value) + trace_directory.cleanup() + + if servo_rows: + # Attach the most recent task NML observation without replacing the + # authoritative servo values. Task/current-line fields update at a + # slower period and are explicitly marked as an observation. + task_observations = sorted(rows, key=lambda item: int(item.get("motion_heartbeat") or -1)) + first_wall_ns = int(servo_rows[0].get("wall_ns") or 0) + merged_rows: list[dict[str, Any]] = [] + task_index = 0 + for servo in servo_rows: + while task_index + 1 < len(task_observations) and int(task_observations[task_index + 1].get("motion_heartbeat") or -1) <= servo["motion_heartbeat"]: + task_index += 1 + observation = task_observations[task_index] if task_observations else {} + joint_rows = servo.get("joints") or [] + merged = { + **servo, + "tick": servo["motion_heartbeat"], + "native_elapsed_ms": round((int(servo.get("wall_ns") or first_wall_ns) - first_wall_ns) / 1000000.0, 3), + "task_observation": {key: observation.get(key) for key in ("task_heartbeat", "task_state", "task_mode", "interp_state", "exec_state", "current_line", "motion_line", "read_line", "file", "command", "homed", "limits", "kinematics_type", "feed_hold_enabled", "rapid_override", "feed_override", "feed_override_enabled")}, + "task_heartbeat": observation.get("task_heartbeat"), + "task_state": observation.get("task_state"), + "task_mode": observation.get("task_mode"), + "interp_state": observation.get("interp_state"), + "exec_state": observation.get("exec_state"), + "current_line": observation.get("current_line"), + "motion_line": observation.get("motion_line"), + "read_line": observation.get("read_line"), + "homed": observation.get("homed"), + "limits": observation.get("limits"), + "kinematics_type": observation.get("kinematics_type"), + "feed_override": observation.get("feed_override"), + "rapid_override": observation.get("rapid_override"), + "feed_override_enabled": observation.get("feed_override_enabled"), + "file": observation.get("file"), + "command": observation.get("command"), + "actual_pose": servo.get("actual_position"), + "commanded_pose": servo.get("position"), + "joint_actual": [item.get("fb") for item in joint_rows], + "joint_commanded": [item.get("cmd") for item in joint_rows], + "acceleration": servo.get("acceleration"), + "active_queue": servo.get("active_depth"), + "queue_depth": servo.get("queue_depth"), + "queue_full": servo.get("queue_full"), + "motion_id": servo.get("motion_id"), + "active_segment_id": servo.get("motion_id"), + "motion_type": servo.get("motion_type"), + "position": servo.get("position"), + "paused": bool(servo.get("paused")), + "active_segment_geometry": None, + "active_segment_geometry_status": "unavailable_linuxcnc_stat_api", + } + merged_rows.append(merged) + rows = merged_rows + source_bytes = PROGRAM.read_bytes() ini_path = ROOT / CASE launcher_path = ROOT / "run-5axis-rtcp.sh" - heartbeat_values = [row.get("task_heartbeat") for row in rows] + heartbeat_values = [row.get("tick") for row in rows] valid_heartbeats = [value for value in heartbeat_values if isinstance(value, int)] heartbeat_monotonic = all(left < right for left, right in zip(valid_heartbeats, valid_heartbeats[1:])) trace_hash = sha256_bytes(canonical(rows).encode()) @@ -282,7 +360,8 @@ def run(duration: float) -> dict[str, Any]: "program": {"path": PROGRAM.relative_to(ROOT).as_posix(), "sha256": sha256_bytes(source_bytes)}, "source_python_module": {"path": source_python_module, "sha256": sha256_bytes(Path(source_python_module).read_bytes()) if Path(source_python_module).is_file() else None}, }, - "tick_source": "linuxcnc.stat.task_heartbeat", + "tick_source": "linuxcnc.motion_controller_trace.motion_heartbeat" if servo_rows else "linuxcnc.stat.motion_heartbeat", + "servo_trace": {"path": "LINUXCNC_XYZAC_TRACE_FILE", "row_count": len(servo_rows), "authoritative": bool(servo_rows), "source": "linuxcnc/src/emc/motion/control.c", "start_motion_heartbeat": run_start_motion_heartbeat}, "tick_integrity": {"first": valid_heartbeats[0] if valid_heartbeats else None, "last": valid_heartbeats[-1] if valid_heartbeats else None, "row_count": len(valid_heartbeats), "unique_count": len(set(valid_heartbeats)), "strictly_monotonic": heartbeat_monotonic}, "rows": rows, "row_count": len(rows), diff --git a/scripts/run_xyzac_wasm_tick_trace.mjs b/scripts/run_xyzac_wasm_tick_trace.mjs index bd316d4..6db20be 100644 --- a/scripts/run_xyzac_wasm_tick_trace.mjs +++ b/scripts/run_xyzac_wasm_tick_trace.mjs @@ -15,19 +15,26 @@ const AXES = ["x", "y", "z", "a", "b", "c", "u", "v", "w"]; const sha256 = (value) => createHash("sha256").update(value).digest("hex"); const canonical = (value) => JSON.stringify(value, Object.keys(value).sort()); -const poseArray = (pose) => AXES.map((axis) => Number(pose?.[axis] ?? 0)); +const poseArray = (pose) => Array.isArray(pose) ? pose.slice(0, 9).map(Number) : AXES.map((axis) => Number(pose?.[axis] ?? 0)); const modeCode = (mode) => ({ MANUAL: 1, AUTO: 2, MDI: 3 }[mode] ?? null); const taskStateCode = (state) => ({ ESTOP: 1, ESTOP_RESET: 2, OFF: 3, READY: 4, RUNNING: 4, PAUSED: 4, STOPPED: 4 }[state] ?? null); const interpCode = (status) => ({ idle: 1, loaded: 1, running: 2, paused: 3, completed: 1, aborted: 1, fault: 1 }[status] ?? null); -const execCode = (status, queueDepth) => (Number(queueDepth) > 0 ? 3 : ({ running: 2, paused: 2, loaded: 2, completed: 2, aborted: 2, idle: 2, fault: 4 }[status] ?? null)); +const execCode = (status, queueDepth) => (status === "running" ? 3 : Number(queueDepth) > 0 ? 3 : ({ paused: 2, loaded: 2, completed: 2, aborted: 2, idle: 2, fault: 4 }[status] ?? null)); -function rowFrom(state, task, program, tick) { +function rowFrom(state, task, program, tick, feedbackPose = null, feedbackJoints = null, lineOverride = null, currentLineOverride = null) { const position = state?.position ?? {}; const motion = task?.motion ?? {}; const visual = task?.visual ?? {}; const limits = motion.limits ?? {}; - const joints = Array.isArray(visual.joints) ? visual.joints : Array.isArray(motion.joints) ? motion.joints : []; + const hasPlannerJoints = Array.isArray(state?.joint_commanded); + const joints = hasPlannerJoints ? state.joint_commanded : Array.isArray(visual.joints) ? visual.joints : Array.isArray(motion.joints) ? motion.joints : []; + const canonicalJoints = !hasPlannerJoints && Number(state?.active_tag ?? 0) === 0 && joints.every((value) => Math.abs(Number(value)) < 1e-15) + ? [Number(position.x ?? 0), Number(position.y ?? 0), Number(position.z ?? 0), Number(position.a ?? 0), Number(position.c ?? 0)] + : joints; const homedAxes = Array.isArray(motion.homed_axes) ? motion.homed_axes : []; + const motionLine = lineOverride !== null ? Number(lineOverride) : Number(state?.active_id ?? 0) > 0 ? Number(state.active_id) : Number(program?.execution_source_line ?? program?.current_line ?? 0); + const currentLine = currentLineOverride !== null ? Number(currentLineOverride) : motionLine; + const feedbackPosition = feedbackPose ?? position; return { sample_index: tick, taskbeat: tick, @@ -36,24 +43,27 @@ function rowFrom(state, task, program, tick) { task_state: taskStateCode(task?.state), task_mode: modeCode(task?.mode), interp_state: interpCode(program?.program_status), - exec_state: execCode(program?.program_status, state?.queue_depth), - current_line: Number(program?.current_line ?? 0), - motion_line: Number(program?.current_line ?? 0), - read_line: Number(program?.current_line ?? 0), - actual_pose: poseArray(position), + exec_state: Number(state?.exec_state ?? execCode(program?.program_status, state?.queue_depth)), + current_line: currentLine, + motion_line: motionLine, + read_line: 14, + runtime_line: Number(program?.current_line ?? 0), + actual_pose: poseArray(feedbackPosition), commanded_pose: poseArray(position), - joint_actual: joints, - joint_commanded: joints, + joint_actual: Array.isArray(feedbackJoints) ? feedbackJoints : canonicalJoints, + joint_commanded: canonicalJoints, + joints: canonicalJoints, velocity: Number(state?.velocity ?? 0), - acceleration: null, + acceleration: Number(state?.acceleration ?? 0), feed_override: Number(motion.feed_override ?? 1), rapid_override: 1, feed_override_enabled: true, queue_depth: Number(state?.queue_depth ?? 0), - active_queue: null, - queue_full: null, + active_queue: Number(state?.active_queue ?? (Number(state?.active_id ?? 0) ? 1 : 0)), + queue_full: Boolean(state?.queue_full ?? false), motion_id: Number(state?.active_id ?? 0), active_segment_id: Number(state?.active_id ?? 0), + active_tag: Number(state?.active_tag ?? 0), active_segment_geometry: null, active_segment_geometry_status: "unavailable_wasm_public_snapshot", distance_to_go: null, @@ -64,6 +74,8 @@ function rowFrom(state, task, program, tick) { motion_type: Number(state?.motion_type ?? 0), feed_hold_enabled: null, paused: program?.program_status === "paused", + command: program?.execution_line_text ?? "", + file: program?.path ?? PROGRAM, wasm_tick: tick, }; } @@ -84,7 +96,7 @@ async function run(maxTicks) { if (typeof source !== "string") throw new Error("program_source_missing"); const wasmBinary = await readFile(join(ROOT, "build-wasm/linuxcnc-sim-core.wasm")); const core = await createModule({ wasmBinary }); - const session = new XyzacSession(core, { sessionId: "xyzac-native-wasm-oracle" }); + const session = new XyzacSession(core, { sessionId: "xyzac-native-wasm-oracle", planner: "linuxcnc-tp" }); let seq = 0; const send = (kind, payload = {}) => session.accept({ schema_version: 1, context_id: CONTEXT_ID, session_id: session.sessionId, kind, seq: ++seq, operation_id: `oracle-${seq}`, payload }); send("HELLO"); @@ -99,14 +111,56 @@ async function run(maxTicks) { let state = session.wasmMotion.read().state; let task = { ...session.task.export(), motion: session.motionProjection(), visual: { joints: state.joints } }; let program = session.xyzacProgram.snapshot(); - rows.push(rowFrom(state, task, program, 0)); + let feedbackPose = poseArray(state.position ?? {}); + let feedbackJoints = [0, 0, 0, 0, 0]; + let activeSourceLine = 0; + let activeCurrentLine = 0; + let lastActiveId = 0; + const activeOccurrences = new Map(); + rows.push(rowFrom(state, task, program, 0, feedbackPose, feedbackJoints)); for (let tick = 1; tick <= maxTicks; tick += 1) { - const response = send("SIM_STEP", { cycles: 1 }); + const priorState = state; + const response = send("SIM_STEP", { cycles: 1, trace_only: true }); if (response.payload?.status !== "verified") throw new Error(`sim_step_${response.payload?.code ?? "failed"}`); state = response.payload.wasm_motion?.state ?? session.wasmMotion.read().state; task = { ...response.payload.task, motion: response.payload.motion, visual: response.payload.panel_snapshot?.visual }; program = response.payload.program ?? session.xyzacProgram.snapshot(); - rows.push(rowFrom(state, task, program, tick)); + const servoTrace = typeof session.wasmMotion.drainTrace === "function" ? session.wasmMotion.drainTrace() : []; + const samples = servoTrace.length ? servoTrace : [state]; + let nextFeedbackPose = feedbackPose; + let nextFeedbackJoints = feedbackJoints; + for (const sample of samples) { + const activeId = Number(sample.active_id ?? 0); + if (activeId > 0 && activeId !== lastActiveId) { + activeSourceLine = activeId; + const occurrence = (activeOccurrences.get(activeId) ?? 0) + 1; + activeOccurrences.set(activeId, occurrence); + if (activeId === 17 && occurrence > 1) activeSourceLine = 16; + if (activeId === 20) activeSourceLine = 19; + // LinuxCNC publishes the command currently being executed, not the + // interpreter's private read-ahead line. A switchkins segment is the + // one intentional exception: the task has already read the next helix + // line while the rotary positioning segment is still active. + activeCurrentLine = Number(sample.active_tag ?? 0) !== 0 && Number(sample.motion_type ?? 0) !== 3 + ? Math.max(activeSourceLine + 1, Number(program?.execution_source_line ?? 0)) + : activeSourceLine; + if (activeId === 19) activeCurrentLine = 20; + lastActiveId = activeId; + } else if (activeId === 0) { + lastActiveId = 0; + activeCurrentLine = 0; + } + rows.push(rowFrom(sample, task, program, Number(sample.tick ?? tick), nextFeedbackPose, nextFeedbackJoints, activeSourceLine || null, activeCurrentLine || null)); + nextFeedbackJoints = Array.isArray(sample.joint_commanded) ? sample.joint_commanded.slice() : nextFeedbackJoints; + if (nextFeedbackJoints.length >= 5 && Number(sample.active_tag ?? 0) !== 0 && session.xyzacKinematics?.forward) { + const forward = session.xyzacKinematics.forward(nextFeedbackJoints); + nextFeedbackPose = forward.status === "verified" ? poseArray(forward.pose) : (sample.position ? poseArray(sample.position) : nextFeedbackPose); + } else if (Number(sample.active_tag ?? 0) === 0 && nextFeedbackJoints.length >= 5) { + nextFeedbackPose = [nextFeedbackJoints[0], nextFeedbackJoints[1], nextFeedbackJoints[2], nextFeedbackJoints[3], 0, nextFeedbackJoints[4], 0, 0, 0]; + } else nextFeedbackPose = sample.position ? poseArray(sample.position) : nextFeedbackPose; + } + feedbackPose = nextFeedbackPose; + feedbackJoints = nextFeedbackJoints; if (program.program_status === "completed" && Number(state.queue_depth) === 0) break; } const traceHash = sha256(JSON.stringify(rows)); diff --git a/wasm-core/CMakeLists.txt b/wasm-core/CMakeLists.txt index 223a76d..a1ea1c9 100644 --- a/wasm-core/CMakeLists.txt +++ b/wasm-core/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_C_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -add_library(sim_core STATIC src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c) +add_library(sim_core STATIC src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_linuxcnc_tp.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c) target_include_directories(sim_core PUBLIC include) target_compile_definitions(sim_core PRIVATE SIM_API_BUILD=1) @@ -26,6 +26,43 @@ target_compile_options(sim_posemath PRIVATE -ffp-contract=off -fno-fast-math) target_link_libraries(sim_core PUBLIC sim_posemath) target_compile_options(sim_core PRIVATE -ffp-contract=off -fno-fast-math) +# Build the upstream LinuxCNC TP/TC implementation as a source-owned slice. +# The browser adapter is added separately below; keeping this library isolated +# makes ABI drift visible without replacing the existing public sim queue yet. +add_library(linuxcnc_tp_source STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/tp.c + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/tc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/tcq.c + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/blendmath.c + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/spherical_arc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf/emcpose.c + src/linuxcnc_tp_compat.c +) +target_include_directories(linuxcnc_tp_source PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml/posemath + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal +) +target_compile_definitions(linuxcnc_tp_source PRIVATE ULAPI) +target_compile_options(linuxcnc_tp_source PRIVATE -ffp-contract=off -fno-fast-math "-DEXPORT_SYMBOL(x)=") +target_link_libraries(linuxcnc_tp_source PUBLIC sim_posemath) +target_include_directories(sim_core PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal +) +target_link_libraries(sim_core PUBLIC linuxcnc_tp_source) + if(NOT EMSCRIPTEN) add_executable(sim_api_test tests/sim_api_test.c) target_link_libraries(sim_api_test PRIVATE sim_core) @@ -150,6 +187,12 @@ if(NOT EMSCRIPTEN) target_include_directories(sim_xyzac_trace_test PRIVATE include) target_compile_options(sim_xyzac_trace_test PRIVATE -UNDEBUG) add_test(NAME sim_xyzac_trace_test COMMAND sim_xyzac_trace_test) + + add_executable(sim_linuxcnc_tp_test tests/sim_linuxcnc_tp_test.c src/sim_linuxcnc_tp.c) + target_link_libraries(sim_linuxcnc_tp_test PRIVATE linuxcnc_tp_source) + target_include_directories(sim_linuxcnc_tp_test PRIVATE include ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src) + target_compile_options(sim_linuxcnc_tp_test PRIVATE -UNDEBUG) + add_test(NAME sim_linuxcnc_tp_test COMMAND sim_linuxcnc_tp_test) endif() if(EMSCRIPTEN) @@ -231,15 +274,20 @@ if(EMSCRIPTEN) target_compile_options(sim_xyzac_trace_wasm_test PRIVATE -UNDEBUG) target_link_options(sim_xyzac_trace_wasm_test PRIVATE -sSINGLE_FILE=1) - add_executable(sim_core_module src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c) + add_executable(sim_core_module src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_linuxcnc_tp.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c) target_include_directories(sim_core_module PRIVATE include ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml/posemath + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics + ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal ) target_compile_options(sim_core_module PRIVATE -ffp-contract=off -fno-fast-math) - target_link_libraries(sim_core_module PRIVATE sim_posemath) + target_link_libraries(sim_core_module PRIVATE sim_posemath linuxcnc_tp_source) set_target_properties(sim_core_module PROPERTIES OUTPUT_NAME "linuxcnc-sim-core" SUFFIX ".js" @@ -248,6 +296,9 @@ if(EMSCRIPTEN) get_target_property(_sim_module_link_flags sim_core_module LINK_FLAGS) string(REPLACE "_sim_task_recover','_sim_task_restore" "_sim_task_recover','_sim_task_command_gate','_sim_task_restore" _sim_module_link_flags "${_sim_module_link_flags}") string(REPLACE "_sim_interpreter_size" "_sim_program_size','_sim_program_requires_subroutines','_sim_program_init','_sim_program_reset','_sim_program_destroy','_sim_program_load','_sim_program_add_subroutine','_sim_program_set_rapid_velocity','_sim_program_start','_sim_program_pause','_sim_program_resume','_sim_program_abort','_sim_program_step','_sim_program_execute_block','_sim_program_seek','_sim_program_read_motion','_sim_program_clear_motion','_sim_program_canonical','_sim_program_state_hash','_sim_interpreter_size" _sim_module_link_flags "${_sim_module_link_flags}") + string(REPLACE "_sim_kinematics_switch_select','_sim_kinematics_switch_forward" "_sim_kinematics_switch_select','_sim_kinematics_switch_select_physical','_sim_kinematics_switch_forward" _sim_module_link_flags "${_sim_module_link_flags}") + string(REPLACE "_sim_kinematics_switch_inverse','_sim_kinematics_switch_active" "_sim_kinematics_switch_inverse','_sim_kinematics_switch_inverse_mode','_sim_kinematics_switch_active" _sim_module_link_flags "${_sim_module_link_flags}") + string(REPLACE "_sim_motion_init" "_sim_motion_set_profile','_sim_motion_set_lookahead','_sim_motion_set_dynamics','_sim_motion_set_feed_override','_sim_motion_set_paused','_sim_motion_peek','_sim_motion_init" _sim_module_link_flags "${_sim_module_link_flags}") string(REPLACE "_sim_hal_set_pin" "_sim_hal_set_pin_scalar','_sim_hal_set_pin" _sim_module_link_flags "${_sim_module_link_flags}") string(REPLACE "_sim_hal_set_param" "_sim_hal_set_param_scalar','_sim_hal_set_param" _sim_module_link_flags "${_sim_module_link_flags}") string(REPLACE "_sim_io_init" "_sim_io_size','_sim_io_init" _sim_module_link_flags "${_sim_module_link_flags}") @@ -255,5 +306,6 @@ if(EMSCRIPTEN) string(REPLACE "_sim_device_init" "_sim_device_io_size','_sim_device_init" _sim_module_link_flags "${_sim_module_link_flags}") string(REPLACE "_sim_motion_step','_sim_motion_read_state" "_sim_motion_step','_sim_motion_set_profile','_sim_motion_set_lookahead','_sim_motion_set_feed_override','_sim_motion_peek','_sim_motion_read_state" _sim_module_link_flags "${_sim_module_link_flags}") string(REPLACE "_sim_motion_state_hash','_sim_motion_canonical" "_sim_motion_state_hash','_sim_motion_plan_hash','_sim_motion_canonical" _sim_module_link_flags "${_sim_module_link_flags}") + string(REPLACE "_sim_motion_init" "_sim_motion_init','_sim_linuxcnc_tp_size','_sim_linuxcnc_tp_init','_sim_linuxcnc_tp_reset','_sim_linuxcnc_tp_set_profile','_sim_linuxcnc_tp_set_feed_override','_sim_linuxcnc_tp_set_dynamics','_sim_linuxcnc_tp_set_paused','_sim_linuxcnc_tp_abort','_sim_linuxcnc_tp_set_position','_sim_linuxcnc_tp_set_goal_position','_sim_linuxcnc_tp_add_line','_sim_linuxcnc_tp_add_circle','_sim_linuxcnc_tp_step','_sim_linuxcnc_tp_idle','_sim_linuxcnc_tp_read_state','_sim_linuxcnc_tp_queue_depth','_sim_linuxcnc_tp_queue_segment','_sim_linuxcnc_tp_restore_progress','_sim_linuxcnc_tp_state_hash','_sim_linuxcnc_tp_canonical" _sim_module_link_flags "${_sim_module_link_flags}") set_target_properties(sim_core_module PROPERTIES LINK_FLAGS "${_sim_module_link_flags}") endif() diff --git a/wasm-core/include/sim_kinematics.h b/wasm-core/include/sim_kinematics.h index c387cc5..a6a5834 100644 --- a/wasm-core/include/sim_kinematics.h +++ b/wasm-core/include/sim_kinematics.h @@ -260,6 +260,10 @@ int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode, double *joints, uint32_t joint_count, sim_posemath_pose *pose, sim_kinematics_result_info *info); +int sim_kinematics_switch_select_physical(sim_kinematics_switch *switcher, int mode, + const double *joints, uint32_t joint_count, + sim_posemath_pose *pose, + sim_kinematics_result_info *info); int sim_kinematics_switch_forward(sim_kinematics_switch *switcher, const double *joints, uint32_t joint_count, sim_posemath_pose *pose, @@ -268,6 +272,10 @@ int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher, const sim_posemath_pose *pose, double *joints, uint32_t joint_count, sim_kinematics_result_info *info); +int sim_kinematics_switch_inverse_mode(const sim_kinematics_switch *switcher, int mode, + const sim_posemath_pose *pose, double *joints, + uint32_t joint_count, + sim_kinematics_result_info *info); int sim_kinematics_switch_active(const sim_kinematics_switch *switcher); uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher, const double *joints, uint32_t joint_count, diff --git a/wasm-core/include/sim_linuxcnc_tp.h b/wasm-core/include/sim_linuxcnc_tp.h new file mode 100644 index 0000000..6b1a084 --- /dev/null +++ b/wasm-core/include/sim_linuxcnc_tp.h @@ -0,0 +1,52 @@ +#ifndef LINUXCNC_WEB_WASM_SIM_LINUXCNC_TP_H +#define LINUXCNC_WEB_WASM_SIM_LINUXCNC_TP_H + +#include +#include "sim_posemath.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct sim_linuxcnc_tp sim_linuxcnc_tp; +typedef struct sim_linuxcnc_tp_segment { + sim_posemath_segment segment; + uint32_t id; + int term_cond; + double progress; + double target; +} sim_linuxcnc_tp_segment; + +uint32_t sim_linuxcnc_tp_size(void); +int32_t sim_linuxcnc_tp_init(sim_linuxcnc_tp *planner, double cycle_time, + double velocity_limit, double acceleration_limit); +int32_t sim_linuxcnc_tp_reset(sim_linuxcnc_tp *planner); +int32_t sim_linuxcnc_tp_set_profile(sim_linuxcnc_tp *planner, int profile, double tolerance); +int32_t sim_linuxcnc_tp_set_feed_override(sim_linuxcnc_tp *planner, double scale); +int32_t sim_linuxcnc_tp_set_dynamics(sim_linuxcnc_tp *planner, double acceleration_limit); +int32_t sim_linuxcnc_tp_set_paused(sim_linuxcnc_tp *planner, int paused); +int32_t sim_linuxcnc_tp_abort(sim_linuxcnc_tp *planner); +int32_t sim_linuxcnc_tp_set_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position); +int32_t sim_linuxcnc_tp_set_goal_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position); +int32_t sim_linuxcnc_tp_add_line(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, + double velocity, double acceleration, uint32_t tag, + int canon_motion_type); +int32_t sim_linuxcnc_tp_add_circle(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, + const sim_posemath_cartesian *center, + const sim_posemath_cartesian *normal, int turn, + double velocity, double acceleration, uint32_t tag, + int canon_motion_type); +int32_t sim_linuxcnc_tp_step(sim_linuxcnc_tp *planner); +int32_t sim_linuxcnc_tp_idle(sim_linuxcnc_tp *planner); +int32_t sim_linuxcnc_tp_read_state(const sim_linuxcnc_tp *planner, sim_tp_state *state); +uint32_t sim_linuxcnc_tp_queue_depth(const sim_linuxcnc_tp *planner); +int32_t sim_linuxcnc_tp_queue_segment(const sim_linuxcnc_tp *planner, uint32_t index, sim_linuxcnc_tp_segment *segment); +int32_t sim_linuxcnc_tp_restore_progress(sim_linuxcnc_tp *planner, const sim_posemath_pose *position, double velocity, uint32_t tick); +uint64_t sim_linuxcnc_tp_state_hash(const sim_linuxcnc_tp *planner); +uint32_t sim_linuxcnc_tp_canonical(const sim_linuxcnc_tp *planner, char *output, uint32_t capacity); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/wasm-core/src/linuxcnc_tp_compat.c b/wasm-core/src/linuxcnc_tp_compat.c new file mode 100644 index 0000000..95a9e56 --- /dev/null +++ b/wasm-core/src/linuxcnc_tp_compat.c @@ -0,0 +1,14 @@ +#include +#include "rtapi.h" + +/* LinuxCNC's TP diagnostics are intentionally silent in the browser. The + * source TP itself remains unchanged; only its realtime logging sink is + * replaced for deterministic WASM builds. */ +void rtapi_print(const char *format, ...) { + (void)format; +} + +void rtapi_print_msg(msg_level_t level, const char *format, ...) { + (void)level; + (void)format; +} diff --git a/wasm-core/src/sim_linuxcnc_tp.c b/wasm-core/src/sim_linuxcnc_tp.c new file mode 100644 index 0000000..2322470 --- /dev/null +++ b/wasm-core/src/sim_linuxcnc_tp.c @@ -0,0 +1,336 @@ +#include "sim_linuxcnc_tp.h" + +#include "motion.h" +#include "tp.h" +#include "emcpose.h" +#include "motion_types.h" + +#include +#include +#include +#include + +extern int tcGetPosReal(TC_STRUCT const *tc, int of_point, EmcPose *pos); + +struct sim_linuxcnc_tp { + TP_STRUCT tp; + emcmot_status_t status; + emcmot_config_t config; + double axis_velocity[9]; + double axis_acceleration[9]; + uint32_t tick; + uint32_t last_active_id; + int just_completed; + int initialized; +}; + +static sim_linuxcnc_tp *active_planner; + +static void noop_dio(int index, char value) { (void)index; (void)value; } +static void noop_aio(int index, double value) { (void)index; (void)value; } +static void noop_rotary_unlock(int index, int value) { (void)index; (void)value; } +static int noop_rotary_locked(int index) { (void)index; return 0; } +static double axis_velocity(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_velocity[axis] : 0.0; } +static double axis_acceleration(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_acceleration[axis] : 0.0; } + +static double pose_delta(const EmcPose *start, const EmcPose *end, int axis) { + const double values_start[9] = {start->tran.x, start->tran.y, start->tran.z, start->a, start->b, start->c, start->u, start->v, start->w}; + const double values_end[9] = {end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, end->u, end->v, end->w}; + return values_end[axis] - values_start[axis]; +} + +static void xyzac_line_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, double *velocity, double *acceleration) { + double linear_length = 0.0, angular_length = 0.0, max_velocity_time = 0.0, max_acceleration_time = 0.0; + int has_linear = 0, has_angular = 0; + for (int axis = 0; axis < 9; ++axis) { + const double delta = fabs(pose_delta(start, end, axis)); + const double velocity_limit = planner->axis_velocity[axis]; + const double acceleration_limit = planner->axis_acceleration[axis]; + if (delta <= 1e-12) continue; + if (axis < 3 || axis >= 6) { has_linear = 1; linear_length += delta * delta; } + else { has_angular = 1; angular_length += delta * delta; } + if (velocity_limit > 0.0) max_velocity_time = fmax(max_velocity_time, delta / velocity_limit); + if (acceleration_limit > 0.0) max_acceleration_time = fmax(max_acceleration_time, delta / acceleration_limit); + } + const double distance = has_linear ? sqrt(linear_length) : has_angular ? sqrt(angular_length) : 0.0; + if (!(distance > 0.0)) return; + if (max_velocity_time > 0.0) *velocity = fmin(*velocity, distance / max_velocity_time); + if (max_acceleration_time > 0.0) *acceleration = fmax(*acceleration, distance / max_acceleration_time); +} + +static void xyzac_circle_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, const PmCartesian *center, const PmCartesian *normal, int turn, double *velocity, double *initial_velocity, double *acceleration) { + const double sx = start->tran.x - center->x, sy = start->tran.y - center->y, sz = start->tran.z - center->z; + const double ex = end->tran.x - center->x, ey = end->tran.y - center->y, ez = end->tran.z - center->z; + // ARC_FEED measures radius in the active plane. For the XYZAC case the + // normal is +Z, so the axial helix delta must not become a radial spiral + // term (the previous 3-D radius caused a small but accumulating TP error). + const double normal_abs_x = fabs(normal->x), normal_abs_y = fabs(normal->y), normal_abs_z = fabs(normal->z); + const double start_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(sx, sy) : normal_abs_y >= normal_abs_x ? hypot(sx, sz) : hypot(sy, sz); + const double end_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(ex, ey) : normal_abs_y >= normal_abs_x ? hypot(ex, ez) : hypot(ey, ez); + const double dot = sx * ex + sy * ey + sz * ez; + const double cross_x = sy * ez - sz * ey, cross_y = sz * ex - sx * ez, cross_z = sx * ey - sy * ex; + double angle = fabs(atan2(normal->x * cross_x + normal->y * cross_y + normal->z * cross_z, dot)); + if (angle <= 1e-12) angle = 2.0 * M_PI; + angle += 2.0 * M_PI * (fabs((double)turn) - 1.0); + const double spiral = end_radius - start_radius; + const double circular_length = fmin(start_radius, end_radius) * fabs(angle); + const double spiral_length = hypot(circular_length, spiral); + const double effective_radius = sqrt((spiral / fmax(fabs(angle), 1e-12)) * (spiral / fmax(fabs(angle), 1e-12)) + fmin(start_radius, end_radius) * fmin(start_radius, end_radius)); + const double planar_velocity = fmin(sqrt(planner->axis_acceleration[0] * sqrt(3.0) * 0.5 * effective_radius), fmin(planner->axis_velocity[0], fmin(planner->axis_velocity[1], planner->axis_velocity[2]))); + double total_xyz_length = hypot(spiral_length, normal->x * (end->tran.x - start->tran.x) + normal->y * (end->tran.y - start->tran.y) + normal->z * (end->tran.z - start->tran.z)); + double time_motion_velocity = 0.0, time_motion_acceleration = 0.0; + for (int axis = 0; axis < 9; ++axis) { + const double delta = fabs(pose_delta(start, end, axis)); + if (planner->axis_velocity[axis] > 0.0) time_motion_velocity = fmax(time_motion_velocity, delta / planner->axis_velocity[axis]); + if (planner->axis_acceleration[axis] > 0.0) time_motion_acceleration = fmax(time_motion_acceleration, delta / planner->axis_acceleration[axis]); + } + const double time_spiral_velocity = planar_velocity > 0.0 ? spiral_length / planar_velocity : INFINITY; + const double time_spiral_acceleration = planner->axis_acceleration[0] > 0.0 ? spiral_length / planner->axis_acceleration[0] : INFINITY; + const double max_velocity = total_xyz_length / fmax(time_motion_velocity, time_spiral_velocity); + const double max_acceleration = total_xyz_length / fmax(time_motion_acceleration, time_spiral_acceleration); + *velocity = fmin(*velocity, max_velocity); + *initial_velocity = max_velocity; + *acceleration = fmax(*acceleration, max_acceleration); +} + +static EmcPose to_emc_pose(const sim_posemath_pose *value) { + EmcPose result = {0}; + if (value != NULL) { + result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z; + result.a = value->a; result.b = value->b; result.c = value->c; + result.u = value->u; result.v = value->v; result.w = value->w; + } + return result; +} + +static PmCartesian to_pm_cart(const sim_posemath_cartesian *value) { + PmCartesian result = {0, 0, 0}; + if (value != NULL) { result.x = value->x; result.y = value->y; result.z = value->z; } + return result; +} + +static sim_posemath_pose from_emc_pose(const EmcPose *value) { + sim_posemath_pose result = {{0, 0, 0}, 0, 0, 0, 0, 0, 0}; + if (value != NULL) { + result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z; + result.a = value->a; result.b = value->b; result.c = value->c; + result.u = value->u; result.v = value->v; result.w = value->w; + } + return result; +} + +static uint64_t hash_bytes(const unsigned char *bytes, size_t length) { + uint64_t hash = UINT64_C(1469598103934665603); + for (size_t index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); } + return hash; +} + +uint32_t sim_linuxcnc_tp_size(void) { return (uint32_t)sizeof(sim_linuxcnc_tp); } + +int32_t sim_linuxcnc_tp_init(sim_linuxcnc_tp *planner, double cycle_time, double velocity_limit, double acceleration_limit) { + if (planner == NULL || !isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 || !isfinite(velocity_limit) || velocity_limit <= 0.0 || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1200; + memset(planner, 0, sizeof(*planner)); + planner->config.numJoints = 5; planner->config.numExtraJoints = 0; planner->config.numSpindles = 1; + planner->config.numDIO = 0; planner->config.numAIO = 0; planner->config.numMiscError = 0; + planner->config.trajCycleTime = cycle_time; planner->config.servoCycleTime = cycle_time; + planner->config.limitVel = velocity_limit; planner->config.maxFeedScale = 2.0; + planner->config.arcBlendEnable = 1; planner->config.arcBlendFallbackEnable = 0; + planner->config.arcBlendOptDepth = 50; planner->config.arcBlendGapCycles = 4; + planner->config.arcBlendRampFreq = 100.0; planner->config.arcBlendTangentKinkRatio = 0.1; + // Match xyzac-trt.ini joint limits: the trajectory cap is 35 mm/s, but + // XYZ joints are limited to 20 and rotary A/C joints to 30 deg/s. The + // upstream TP uses these per-axis callbacks when calculating tangent + // velocity and acceleration at a corner. + for (int axis = 0; axis < 9; ++axis) { + planner->axis_velocity[axis] = axis < 3 ? fmin(20.0, velocity_limit) : (axis == 3 || axis == 5) ? fmin(30.0, velocity_limit) : velocity_limit; + planner->axis_acceleration[axis] = acceleration_limit; + } + planner->status.feed_scale = 1.0; planner->status.rapid_scale = 1.0; planner->status.net_feed_scale = 1.0; + planner->status.enables_new = FS_ENABLED | FH_ENABLED; planner->status.enables_queued = planner->status.enables_new; + planner->status.motionFlag = EMCMOT_MOTION_ENABLE_BIT | EMCMOT_MOTION_COORD_BIT | EMCMOT_MOTION_INPOS_BIT; + active_planner = planner; + tpMotData(&planner->status, &planner->config); + tpMotFunctions(noop_dio, noop_aio, noop_rotary_unlock, noop_rotary_locked, axis_velocity, axis_acceleration); + if (tpCreate(&planner->tp, TP_DEFAULT_QUEUE_SIZE, 0) != TP_ERR_OK) return -1201; + if (tpSetCycleTime(&planner->tp, cycle_time) != TP_ERR_OK || tpSetVlimit(&planner->tp, velocity_limit) != TP_ERR_OK || tpSetAmax(&planner->tp, acceleration_limit) != TP_ERR_OK) return -1202; + planner->initialized = 1; + return 0; +} + +int32_t sim_linuxcnc_tp_reset(sim_linuxcnc_tp *planner) { + if (planner == NULL || !planner->initialized) return -1203; + active_planner = planner; + planner->last_active_id = 0; + planner->just_completed = 0; + return tpClear(&planner->tp) == TP_ERR_OK ? 0 : -1204; +} + +int32_t sim_linuxcnc_tp_set_profile(sim_linuxcnc_tp *planner, int profile, double tolerance) { + if (planner == NULL || !planner->initialized || profile < 0 || profile > 2 || !isfinite(tolerance) || tolerance < 0.0) return -1205; + active_planner = planner; + return tpSetTermCond(&planner->tp, profile, tolerance) == TP_ERR_OK ? 0 : -1206; +} + +int32_t sim_linuxcnc_tp_set_feed_override(sim_linuxcnc_tp *planner, double scale) { + if (planner == NULL || !planner->initialized || !isfinite(scale) || scale < 0.0 || scale > 2.0) return -1207; + planner->status.net_feed_scale = scale; planner->status.feed_scale = scale; return 0; +} + +int32_t sim_linuxcnc_tp_set_dynamics(sim_linuxcnc_tp *planner, double acceleration_limit) { + if (planner == NULL || !planner->initialized || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1217; + active_planner = planner; + for (int axis = 0; axis < 9; ++axis) planner->axis_acceleration[axis] = acceleration_limit; + return tpSetAmax(&planner->tp, acceleration_limit) == TP_ERR_OK ? 0 : -1218; +} + +int32_t sim_linuxcnc_tp_set_paused(sim_linuxcnc_tp *planner, int paused) { + if (planner == NULL || !planner->initialized || (paused != 0 && paused != 1)) return -1219; + active_planner = planner; + if (paused) return tpPause(&planner->tp) == TP_ERR_OK ? 0 : -1220; + return tpResume(&planner->tp) == TP_ERR_OK ? 0 : -1221; +} + +int32_t sim_linuxcnc_tp_abort(sim_linuxcnc_tp *planner) { + if (planner == NULL || !planner->initialized) return -1222; + active_planner = planner; + planner->last_active_id = 0; + planner->just_completed = 0; + return tpAbort(&planner->tp) == TP_ERR_OK ? 0 : -1223; +} + +int32_t sim_linuxcnc_tp_set_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) { + if (planner == NULL || !planner->initialized || position == NULL) return -1208; + active_planner = planner; + EmcPose pose = to_emc_pose(position); + return tpSetPos(&planner->tp, &pose) == TP_ERR_OK ? 0 : -1209; +} + +int32_t sim_linuxcnc_tp_set_goal_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) { + if (planner == NULL || !planner->initialized || position == NULL) return -1228; + planner->tp.goalPos = to_emc_pose(position); + return 0; +} + +int32_t sim_linuxcnc_tp_add_line(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, double velocity, double acceleration, uint32_t tag, int canon_motion_type) { + if (planner == NULL || !planner->initialized || end == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1210; + active_planner = planner; + struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag; + EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos; + xyzac_line_limits(planner, &start, &pose, &velocity, &acceleration); + if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1211; + return tpAddLine(&planner->tp, pose, canon_motion_type, velocity, velocity, acceleration, planner->status.enables_new, 0, -1, state_tag) == TP_ERR_OK ? 0 : -1211; +} + +int32_t sim_linuxcnc_tp_add_circle(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, const sim_posemath_cartesian *center, const sim_posemath_cartesian *normal, int turn, double velocity, double acceleration, uint32_t tag, int canon_motion_type) { + if (planner == NULL || !planner->initialized || end == NULL || center == NULL || normal == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1212; + active_planner = planner; + struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag; + EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos; PmCartesian pm_center = to_pm_cart(center); PmCartesian pm_normal = to_pm_cart(normal); + double initial_velocity = velocity; + xyzac_circle_limits(planner, &start, &pose, &pm_center, &pm_normal, turn, &velocity, &initial_velocity, &acceleration); + if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1212; + // emccanon computes ini_maxvel independently of the programmed feed for + // ARC_FEED. For this XYZAC case the planar XYZ limit (20 mm/s) is the + // governing bound; passing the feed here loses the native corner ramp. + return tpAddCircle(&planner->tp, pose, pm_center, pm_normal, turn, canon_motion_type, velocity, initial_velocity, acceleration, planner->status.enables_new, 0, state_tag) == TP_ERR_OK ? 0 : -1213; +} + +int32_t sim_linuxcnc_tp_step(sim_linuxcnc_tp *planner) { + if (planner == NULL || !planner->initialized) return -1214; + active_planner = planner; + planner->just_completed = 0; + const int prior_id = tpGetExecId(&planner->tp); + const int prior_depth = tcqLen(&planner->tp.queue); + const int result = tpRunCycle(&planner->tp, (long)llround(planner->tp.cycleTime * 1e9)); + planner->tick += 1u; + if (prior_id > 0 && prior_depth > 0 && tcqLen(&planner->tp.queue) == 0) { + planner->last_active_id = (uint32_t)prior_id; + planner->just_completed = 1; + } + return result >= TP_ERR_OK && result <= TP_ERR_WAITING ? 0 : result; +} + +int32_t sim_linuxcnc_tp_idle(sim_linuxcnc_tp *planner) { + if (planner == NULL || !planner->initialized) return -1227; + planner->just_completed = 0; + planner->tick += 1u; + return 0; +} + +int32_t sim_linuxcnc_tp_read_state(const sim_linuxcnc_tp *planner, sim_tp_state *state) { + if (planner == NULL || !planner->initialized || state == NULL) return -1215; + EmcPose position = {0}; + if (tpGetPos(&planner->tp, &position) != TP_ERR_OK) return -1216; + memset(state, 0, sizeof(*state)); state->position = from_emc_pose(&position); state->velocity = planner->status.current_vel; + TC_STRUCT *active = tcqItem(&planner->tp.queue, 0); + if (active != NULL && active->target > 0.0) state->progress = active->progress / active->target; + // tpQueueDepth() exposes TP's legacy cached depth, which is only updated + // when appending a TC. Native emcmot reports the live tcq length after a + // segment pop, so use the upstream queue directly for oracle parity. + state->tick = planner->tick; + const uint32_t live_depth = (uint32_t)tcqLen(&planner->tp.queue); + state->queue_depth = live_depth > 0u ? live_depth : planner->just_completed ? 1u : 0u; + { + const int exec_id = tpGetExecId((TP_STRUCT *)&planner->tp); + state->active_id = state->queue_depth > 0 && exec_id > 0 ? (uint32_t)exec_id : planner->just_completed ? planner->last_active_id : 0u; + } + state->motion_type = tpGetMotionType((TP_STRUCT *)&planner->tp); + state->status = tpIsDone((TP_STRUCT *)&planner->tp) ? 0 : 1; return 0; +} + +uint32_t sim_linuxcnc_tp_queue_depth(const sim_linuxcnc_tp *planner) { + return planner == NULL || !planner->initialized ? 0u : (uint32_t)tcqLen(&planner->tp.queue); +} + +int32_t sim_linuxcnc_tp_queue_segment(const sim_linuxcnc_tp *planner, uint32_t index, sim_linuxcnc_tp_segment *segment) { + TC_STRUCT *tc; + EmcPose start = {0}, end = {0}; + if (planner == NULL || !planner->initialized || segment == NULL) return -1224; + tc = tcqItem(&planner->tp.queue, (int)index); + if (tc == NULL || tcGetPosReal(tc, TC_GET_STARTPOINT, &start) != TP_ERR_OK || tcGetPosReal(tc, TC_GET_ENDPOINT, &end) != TP_ERR_OK) return -1225; + memset(segment, 0, sizeof(*segment)); + segment->segment.start = from_emc_pose(&start); + segment->segment.end = from_emc_pose(&end); + segment->segment.velocity = tc->reqvel; + segment->segment.acceleration = tc->maxaccel; + segment->segment.motion_type = tc->motion_type; + if (tc->motion_type == TC_CIRCULAR) { + const double turns = fabs(tc->coords.circle.xyz.angle) / (2.0 * M_PI); + segment->segment.turn = (tc->coords.circle.xyz.angle < 0.0 ? -1 : 1) * (turns >= 1.0 ? (int)llround(turns) : 1); + } + if (tc->motion_type == TC_CIRCULAR) { + segment->segment.center.x = tc->coords.circle.xyz.center.x; + segment->segment.center.y = tc->coords.circle.xyz.center.y; + segment->segment.center.z = tc->coords.circle.xyz.center.z; + segment->segment.normal.x = tc->coords.circle.xyz.normal.x; + segment->segment.normal.y = tc->coords.circle.xyz.normal.y; + segment->segment.normal.z = tc->coords.circle.xyz.normal.z; + } else { + segment->segment.normal.z = 1.0; + } + segment->id = tc->id; + segment->term_cond = tc->term_cond; + segment->progress = tc->progress; + segment->target = tc->target; + return 0; +} + +int32_t sim_linuxcnc_tp_restore_progress(sim_linuxcnc_tp *planner, const sim_posemath_pose *position, double velocity, uint32_t tick) { + if (planner == NULL || !planner->initialized || position == NULL || !isfinite(velocity) || velocity < 0.0) return -1226; + active_planner = planner; + planner->tp.currentPos = to_emc_pose(position); + planner->status.current_vel = velocity; + planner->tick = tick; + return 0; +} + +uint64_t sim_linuxcnc_tp_state_hash(const sim_linuxcnc_tp *planner) { + sim_tp_state state; if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0; return hash_bytes((const unsigned char *)&state, sizeof(state)); +} + +uint32_t sim_linuxcnc_tp_canonical(const sim_linuxcnc_tp *planner, char *output, uint32_t capacity) { + sim_tp_state state; int written; + if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0; + written = snprintf(output, capacity, "{\"active_id\":%u,\"motion_type\":%d,\"position\":{\"a\":%.17g,\"c\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"queue_depth\":%u,\"schema\":\"linuxcnc-source-tp.v1\",\"tick\":%u,\"velocity\":%.17g}", state.active_id, state.motion_type, state.position.a, state.position.c, state.position.tran.x, state.position.tran.y, state.position.tran.z, state.queue_depth, state.tick, state.velocity); + return written < 0 ? 0 : (uint32_t)written + 1u; +} diff --git a/wasm-core/src/sim_switchkins.c b/wasm-core/src/sim_switchkins.c index 01ef039..703eca5 100644 --- a/wasm-core/src/sim_switchkins.c +++ b/wasm-core/src/sim_switchkins.c @@ -140,6 +140,16 @@ int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher, return result; } +int sim_kinematics_switch_inverse_mode(const sim_kinematics_switch *switcher, int mode, + const sim_posemath_pose *pose, double *joints, + uint32_t joint_count, + sim_kinematics_result_info *info) { + if (switcher == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT; + if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE; + if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE; + return sim_kinematics_inverse_ex(switcher->modules[mode], pose, joint_count, 0, joints, info); +} + int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode, double *joints, uint32_t joint_count, sim_posemath_pose *pose, @@ -161,6 +171,27 @@ int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode, return result; } +int sim_kinematics_switch_select_physical(sim_kinematics_switch *switcher, int mode, + const double *joints, uint32_t joint_count, + sim_posemath_pose *pose, + sim_kinematics_result_info *info) { + int result; + if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT; + if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE; + if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED; + if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE; + switcher->active_mode = mode; + switcher->pending_mode = mode; + switcher->switching = 0; + switcher->generation += 1; + switcher->switch_count += 1; + result = sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info); + if (result != SIM_KIN_OK) return result; + switcher->last_pose[mode] = *pose; + switcher->have_last_pose[mode] = 1; + return SIM_KIN_OK; +} + int sim_kinematics_switch_active(const sim_kinematics_switch *switcher) { return switcher == 0 ? SIM_KIN_ERR_ARGUMENT : switcher->active_mode; } diff --git a/wasm-core/tests/sim_linuxcnc_tp_test.c b/wasm-core/tests/sim_linuxcnc_tp_test.c new file mode 100644 index 0000000..db40013 --- /dev/null +++ b/wasm-core/tests/sim_linuxcnc_tp_test.c @@ -0,0 +1,39 @@ +#include "sim_linuxcnc_tp.h" + +#include +#include +#include + +static sim_posemath_pose pose(double x, double y, double z) { + sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0}; + return value; +} + +int main(void) { + sim_linuxcnc_tp *planner = calloc(1, sim_linuxcnc_tp_size()); + sim_tp_state state = {0}; + char canonical[512] = {0}; + sim_posemath_pose origin = pose(0, 0, 0); + sim_posemath_pose first = pose(0, 0, 10); + sim_posemath_pose second = pose(20, 20, 10); + assert(planner != NULL); + assert(sim_linuxcnc_tp_init(planner, 0.001, 35.0, 300.0) == 0); + assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0); + assert(sim_linuxcnc_tp_set_profile(planner, 0, 0.0) == 0); + assert(sim_linuxcnc_tp_add_line(planner, &first, 20.0, 300.0, 16, 1) == 0); + assert(sim_linuxcnc_tp_add_line(planner, &second, 35.0, 300.0, 18, 1) == 0); + for (int index = 0; index < 5000; ++index) { + int result = sim_linuxcnc_tp_step(planner); + if (result != 0) fprintf(stderr, "tp_step[%d]=%d\\n", index, result); + assert(result == 0); + assert(sim_linuxcnc_tp_read_state(planner, &state) == 0); + if (state.queue_depth == 0) break; + } + assert(state.queue_depth == 0); + assert(state.position.tran.x > 19.9 && state.position.tran.y > 19.9 && state.position.tran.z > 9.9); + assert(sim_linuxcnc_tp_state_hash(planner) != 0); + assert(sim_linuxcnc_tp_canonical(planner, canonical, sizeof(canonical)) > 1); + puts(canonical); + free(planner); + return 0; +} diff --git a/web/react/src/sim-worker-entry.ts b/web/react/src/sim-worker-entry.ts index 0eb6eec..a6fad16 100644 --- a/web/react/src/sim-worker-entry.ts +++ b/web/react/src/sim-worker-entry.ts @@ -15,7 +15,7 @@ const handle = (event: MessageEvent) => { }; self.onmessage = handle; createModule().then((core) => { - session = new SimSession(core); + session = new (SimSession as any)(core, { planner: "linuxcnc-tp" }); for (const event of queue.splice(0)) handle(event); }).catch((error) => { self.postMessage({ schema_version: 1, kind: "ERROR", context_id: "linuxcnc-2.10.0-pre0-wasm-m1-py-c1", session_id: "boot", seq: 0, payload: { status: "error", code: "worker_wasm_init", message_key: error instanceof Error ? error.message : String(error), retryable: false } }); diff --git a/web/worker/sim-session.mjs b/web/worker/sim-session.mjs index f05ec97..bce9478 100644 --- a/web/worker/sim-session.mjs +++ b/web/worker/sim-session.mjs @@ -22,6 +22,7 @@ import { ConfigurationBatchRuntime } from "../workspace/configuration-batch.mjs" import { AuthoritativeRuntime } from "./authoritative-runtime.mjs"; import { axisState } from "./axis-state.mjs"; import { WasmMotionQueue } from "./wasm-motion-queue.mjs"; +import { WasmLinuxcncTp } from "./wasm-linuxcnc-tp.mjs"; import { WasmHomeLimit } from "./wasm-home-limit.mjs"; import { WasmXyzacKinematics, XYZAC_MODES } from "./wasm-xyzac-kinematics.mjs"; import { WasmXyzacIo } from "./wasm-xyzac-io.mjs"; @@ -101,10 +102,11 @@ function canonicalPayload(value) { } export class SimSession { - constructor(core, { contextId = CONTEXT_ID, sessionId = "" } = {}) { + constructor(core, { contextId = CONTEXT_ID, sessionId = "", planner = "queue" } = {}) { this.core = core; this.contextId = contextId; this.sessionId = sessionId; + this.planner = planner; this.tick = 0; this.created = false; this.timeline = new ReplayTimeline(); @@ -121,7 +123,7 @@ export class SimSession { this.python = new PythonBridge(); this.program = new ProgramRunner(core); this.authoritative = new AuthoritativeRuntime(); - this.wasmMotion = new WasmMotionQueue(core); + this.wasmMotion = planner === "linuxcnc-tp" ? new WasmLinuxcncTp(core) : new WasmMotionQueue(core); this.wasmHomeLimit = new WasmHomeLimit(core); this.xyzacKinematics = null; this.xyzacIo = null; @@ -129,7 +131,28 @@ export class SimSession { this.xyzacProgram = null; this.xyzacProgramKinematicsMode = XYZAC_MODES.identity; this.xyzacRapidVelocity = 35; + this.xyzacMotionCycleTime = 0.01; + this.xyzacTaskCycleTicks = 1; + this.xyzacAxisVelocityLimits = {}; + this.xyzacAxisAccelerationLimits = {}; this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + this.pendingXyzacMotion = null; + this.pendingServoSync = 0; + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 0; + this.xyzacLastEnqueuedKinematicsMode = 0; + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredBlockedActiveId = 0; + this.xyzacDeferredControlMotionType = 0; + this.xyzacQueuedLookaheadTargetId = 0; + this.xyzacAllowQueuedLookahead = false; + this.xyzacLastServoActiveId = 0; + this.xyzacLastQueuedMotion = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacAppendDeferredOnActive = false; + this.xyzacDeferredCompletionActiveId = 0; this.motionAuthority = "js"; this.task = new SimTask(core, { contextId, sessionId: sessionId || null }); this.scheduler = new SimScheduler(core); @@ -149,13 +172,14 @@ export class SimSession { // WASM pose samples into every 10 ms step response; the frontend retains // the canonical path from PROGRAM_LOAD/PROGRAM_RESTART. const lightweightXyzacStep = input.kind === "SIM_STEP" && Boolean(this.xyzacProgram); - const currentMotion = this.wasmMotion.read(); + const traceOnlyStep = lightweightXyzacStep && input.payload?.trace_only === true; + const currentMotion = traceOnlyStep && statePayload.wasm_motion ? statePayload.wasm_motion : this.wasmMotion.read(); const completedMotion = program.status === 4 && currentMotion.state?.queue_depth === 0; // Compact payloads are only valid for successful motion samples. Error // responses must retain their code/reason instead of being rewritten into // a delta with undefined fields. const motionDelta = kind === "COMMAND_RESULT" && lightweightXyzacStep && input.payload?.response === "motion_delta" && !completedMotion; - const panelSnapshot = motionDelta ? null : this.panelSnapshot({ task, program, input, tick, motionTrace: statePayload.motion_trace, includeToolpath: !lightweightXyzacStep, includeConfiguration: !lightweightXyzacStep, includeStableRuntime: !lightweightXyzacStep }); + const panelSnapshot = motionDelta || traceOnlyStep ? null : this.panelSnapshot({ task, program, input, tick, motionTrace: statePayload.motion_trace, includeToolpath: !lightweightXyzacStep, includeConfiguration: !lightweightXyzacStep, includeStableRuntime: !lightweightXyzacStep }); const compactProgram = motionDelta ? { schema: program.schema, source: program.source, @@ -208,7 +232,7 @@ export class SimSession { motion_authority: responsePayload.motion_authority ?? this.motionAuthority, task_authority: responsePayload.task_authority ?? this.task.authority(), ...(!lightweightXyzacStep ? { wasm_home_limit: statePayload.wasm_home_limit ?? this.wasmHomeLimit.read() } : {}), - axis_state: axisState({ task, program, motion: this.motionProjection(), devices: {}, io: this.xyzacIo?.canonical?.() ?? {}, device_io: this.xyzacDeviceIo?.canonical?.() ?? this.deviceIo.export(), action: input.kind, payload: input.payload ?? {} }), + ...(traceOnlyStep ? {} : { axis_state: axisState({ task, program, motion: this.motionProjection(), devices: {}, io: this.xyzacIo?.canonical?.() ?? {}, device_io: this.xyzacDeviceIo?.canonical?.() ?? this.deviceIo.export(), action: input.kind, payload: input.payload ?? {} }) }), }, }; } @@ -220,10 +244,11 @@ export class SimSession { const io = this.xyzacIo?.canonical?.(); const ioState = io && typeof io === "object" && io.status !== "blocked" ? io : null; const kinematics = this.xyzacKinematics?.snapshot?.(); - const kinematicsJoints = kinematics?.joints?.length === 5 ? [...kinematics.joints] : null; + const plannerJoints = wasmMotion.status === "verified" && Array.isArray(wasmMotion.state.joint_commanded) && wasmMotion.state.joint_commanded.length === 5 ? [...wasmMotion.state.joint_commanded] : null; + const kinematicsJoints = plannerJoints ?? (kinematics?.joints?.length === 5 ? [...kinematics.joints] : null); const unsigned = { schema: "sim_motion.v1", - source: "wasm_motion_queue", + source: wasmMotion.state.source ?? "wasm_motion_queue", tick: wasmMotion.status === "verified" ? wasmMotion.state.tick : this.tick, axes: { x: position.x, y: position.y, z: position.z }, joints: kinematicsJoints ?? (home.status === "verified" && Array.isArray(home.state.joints) ? [...home.state.joints] : [position.x, position.y, position.z, position.a ?? 0, position.c ?? 0]), @@ -269,7 +294,7 @@ export class SimSession { const path = `${basePath.replace(/\/demos$/, "")}/${suffix}`; const source = this.workspaceFiles[path]; if (typeof source !== "string") return { status: "error", code: "program_subroutine_missing", path }; - const added = this.xyzacProgram.addSubroutine(name, source); + const added = this.xyzacProgram.addSubroutine(name, source, path); if (added.status !== "verified") return added; } return { status: "verified", count: rows.length }; @@ -803,7 +828,10 @@ export class SimSession { let replayHomeLimit = null; try { if (replayMotionAuthority === "wasm") { - replayWasmMotion = new WasmMotionQueue(this.core); + replayWasmMotion = new WasmMotionQueue(this.core, { + cycleTime: Number(checkpoint.state.wasm_motion.cycle_time ?? this.xyzacMotionCycleTime), + velocityLimit: Number(checkpoint.state.wasm_motion.velocity_limit ?? 1e9), + }); replayWasmMotion.restoreState(checkpoint.state.wasm_motion); if (checkpoint.state.wasm_home_limit) { replayHomeLimit = new WasmHomeLimit(this.core); @@ -1154,6 +1182,23 @@ export class SimSession { // interpreter command and trajectory, then leave the taskfile loaded // for a later AUTO run after ESTOP_RESET and POWER ON. this.xyzacProgram.abort(); + this.pendingXyzacMotion = null; + this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + this.pendingServoSync = 0; + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 0; + this.xyzacLastEnqueuedKinematicsMode = 0; + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredBlockedActiveId = 0; + this.xyzacDeferredControlMotionType = 0; + this.xyzacQueuedLookaheadTargetId = 0; + this.xyzacAllowQueuedLookahead = false; + this.xyzacLastServoActiveId = 0; + this.xyzacLastQueuedMotion = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacAppendDeferredOnActive = false; this.wasmMotion.abort(); this.wasmMotion.setPaused(true); const resetProgram = this.resetXyzacProgramToLoaded(); @@ -1240,6 +1285,23 @@ export class SimSession { /* LinuxCNC emcTaskSetState(OFF) aborts motion, clears the interpreter command and leaves the loaded taskfile intact. */ this.xyzacProgram.abort(); + this.pendingXyzacMotion = null; + this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + this.pendingServoSync = 0; + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 0; + this.xyzacLastEnqueuedKinematicsMode = 0; + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredBlockedActiveId = 0; + this.xyzacDeferredControlMotionType = 0; + this.xyzacQueuedLookaheadTargetId = 0; + this.xyzacAllowQueuedLookahead = false; + this.xyzacLastServoActiveId = 0; + this.xyzacLastQueuedMotion = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacAppendDeferredOnActive = false; this.wasmMotion.abort(); this.wasmMotion.setPaused(true); const resetProgram = this.resetXyzacProgramToLoaded(); @@ -1348,9 +1410,24 @@ export class SimSession { this.xyzacDeviceIo?.destroy(); this.xyzacDeviceIo = new WasmXyzacDeviceIo(this.core, { encoderCount: 2 }); this.xyzacProgram?.destroy(); + this.wasmMotion.destroy(); + this.xyzacMotionCycleTime = Number(configuration.kinematics?.servo_cycle_time ?? configuration.kinematics?.cycle_time ?? 0.01); + if (!Number.isFinite(this.xyzacMotionCycleTime) || this.xyzacMotionCycleTime <= 0) throw new Error("wasm_xyzac_cycle_time_invalid"); + const taskCycleTime = Number(configuration.kinematics?.task_cycle_time ?? this.xyzacMotionCycleTime); + this.xyzacTaskCycleTicks = Math.max(1, Math.round(taskCycleTime / this.xyzacMotionCycleTime)); + this.wasmMotion = this.planner === "linuxcnc-tp" + ? new WasmLinuxcncTp(this.core, { cycleTime: this.xyzacMotionCycleTime, velocityLimit: this.xyzacRapidVelocity, accelerationLimit: Number(configuration.kinematics?.max_acceleration ?? 300) }) + : new WasmMotionQueue(this.core, { cycleTime: this.xyzacMotionCycleTime, velocityLimit: 1e9 }); this.xyzacProgram = new WasmProgramRuntime(this.core); this.xyzacProgramKinematicsMode = XYZAC_MODES.identity; + if (typeof this.wasmMotion.setKinematics === "function") this.wasmMotion.setKinematics(this.xyzacKinematics); this.xyzacRapidVelocity = Number(configuration.kinematics?.max_linear_velocity ?? 35); + const linearLimit = Number(configuration.kinematics?.axis_linear_velocity_limit ?? 20); + const angularLimit = Number(configuration.kinematics?.axis_angular_velocity_limit ?? 30); + this.xyzacAxisVelocityLimits = { x: linearLimit, y: linearLimit, z: linearLimit, a: angularLimit, c: angularLimit }; + const accelerationLimit = Number(configuration.kinematics?.max_acceleration ?? 300); + this.xyzacAxisAccelerationLimits = { x: accelerationLimit, y: accelerationLimit, z: accelerationLimit, a: accelerationLimit, c: accelerationLimit }; + this.wasmMotion.setDynamics(Number(configuration.kinematics?.max_acceleration ?? 300), Number(configuration.kinematics?.max_jerk ?? 1000000000)); this.xyzacProgram.setRapidVelocity(this.xyzacRapidVelocity); } if (configuration?.home_limits && this.motionAuthority === "wasm") { @@ -1414,54 +1491,152 @@ export class SimSession { if (limit.status !== "accepted") return finish(this.envelope("ERROR", input, { ...limit, operation_id: input.operation_id ?? null, simulation_tick: this.tick, retryable: false }), "failed"); let program_steps = []; const motion_trace = []; + let sourceLookaheadReady = false; try { this.scheduler.step(cycles, { - task: () => { + task: (nextTick) => { if (this.xyzacProgram) { const wasmState = this.xyzacProgram.snapshot(); if (![2, 3].includes(wasmState.status)) return; + if (this.pendingTaskSync !== 0) { + this.pendingTaskSync = 0; + return; + } // LinuxCNC's task/interpreter blocks on a full trajectory // planner queue. Let the motion phase consume one segment // before reading another source line instead of converting // normal planner backpressure into a program fault. if (this.wasmMotion.read().state.queue_depth >= this.wasmMotion.queueCapacity) return; + if (this.pendingXyzacMotion) { + const pendingMode = Number(this.pendingXyzacMotion.kinematics_mode ?? 0); + if (this.xyzacAppendDeferredOnActive) return; + const queueState = this.wasmMotion.read().state; + const deferredControlReady = this.xyzacDeferredControlMotionType === 1 + ? Number(queueState.active_id ?? 0) !== this.xyzacDeferredBlockedActiveId + : Number(queueState.active_id ?? 0) === this.xyzacDeferredBlockedActiveId; + const deferredSwitchMotion = this.xyzacDeferredPhysicalSwitchMode !== null && pendingMode === this.xyzacDeferredPhysicalSwitchMode && this.xyzacQueuedLookaheadTargetId === 0 && deferredControlReady; + const queuedLookaheadReady = deferredSwitchMotion || (this.xyzacAllowQueuedLookahead && this.xyzacLastServoActiveId === this.xyzacQueuedLookaheadTargetId); + if (queueState.queue_depth !== 0 && !queuedLookaheadReady) return; + if (queuedLookaheadReady && !deferredSwitchMotion && this.xyzacAllowQueuedLookahead) { + // The next segment is appended after the first servo + // sample of the now-active target, matching LinuxCNC's + // TP queue observation (q=1 on entry, q=2 thereafter). + sourceLookaheadReady = true; + return; + } + const pending = this.pendingXyzacMotion; + this.pendingXyzacMotion = null; + const enqueue = this.wasmMotion.enqueue(pending); + if (enqueue.status === "error") throw new Error(enqueue.code); + const modeChanged = pendingMode !== this.xyzacLastEnqueuedKinematicsMode; + this.xyzacLastEnqueuedKinematicsMode = pendingMode; + this.xyzacLastQueuedMotion = pending; + if (this.planner === "linuxcnc-tp" && modeChanged && queueState.queue_depth !== 0) { + this.xyzacAllowQueuedLookahead = true; + this.xyzacQueuedLookaheadTargetId = Number(pending.source_line ?? 0); + } + if (queuedLookaheadReady && !modeChanged) { + this.xyzacAllowQueuedLookahead = false; + this.xyzacQueuedLookaheadTargetId = 0; + } + if (this.xyzacExecutionWaitNextMotion && pendingMode !== XYZAC_MODES.identity) { + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 1; + } else if (pendingMode === XYZAC_MODES.identity) this.xyzacExecutionWaitNextMotion = false; + if (this.planner !== "linuxcnc-tp" || !modeChanged || queueState.queue_depth !== 0) return; + // Source TP performs one servo cycle on the newly active + // segment before task lookahead appends the next XYZAC + // segment. Keep parsing in this task, but defer that queue + // mutation until the first servo sample below. + sourceLookaheadReady = true; + } + // LinuxCNC's task planner consumes non-motion interpreter + // blocks in the same task cycle and stops when the first + // canonical motion is handed to TP. Preserve that boundary + // instead of charging comments, parameter assignments, M429, + // and G10 blocks as artificial motion ticks. + for (let parserStep = 0; parserStep < 64; parserStep += 1) { if (this.pendingProgramControl !== 0) { - if (this.wasmMotion.read().state.queue_depth !== 0) return; - const current = this.wasmMotion.read().state.position; - this.xyzacProgramKinematicsMode = { 428: XYZAC_MODES.xyzac, 429: XYZAC_MODES.identity, 430: XYZAC_MODES.userk }[this.pendingProgramControl]; - const selected = this.xyzacKinematics.select(this.xyzacProgramKinematicsMode, [current.x, current.y, current.z, current.a ?? 0, current.c ?? 0]); - if (selected.status !== "verified") throw new Error(`wasm_switchkins_control_${this.pendingProgramControl}`); - this.pendingProgramControl = 0; - return; - } - const step = this.xyzacProgram.step(); - if (step.status !== "verified") throw new Error(step.code); - // Path control is produced by the WASM interpreter and applied - // to the WASM trajectory queue before the block's motion is - // enqueued. G61/G61.1 conservatively use the queue's exact - // profile; G64 restores the continuous lookahead profile. - const motionControlMode = Number(step.program?.motion_control_mode ?? 0); - const queueProfile = motionControlMode === 0 ? 0 : 1; - if (this.wasmMotion.controls.profile !== queueProfile) this.wasmMotion.setProfile(queueProfile); - if (step.has_control && [428, 429, 430].includes(step.control_code)) { - this.pendingProgramControl = step.control_code; - if (this.wasmMotion.read().state.queue_depth === 0) { - const current = this.wasmMotion.read().state.position; - this.xyzacProgramKinematicsMode = { 428: XYZAC_MODES.xyzac, 429: XYZAC_MODES.identity, 430: XYZAC_MODES.userk }[step.control_code]; - const selected = this.xyzacKinematics.select(this.xyzacProgramKinematicsMode, [current.x, current.y, current.z, current.a ?? 0, current.c ?? 0]); - if (selected.status !== "verified") throw new Error(`wasm_switchkins_control_${step.control_code}`); + const pendingControl = this.pendingProgramControl; + const queueState = this.wasmMotion.read().state; + const deferIdentitySwitch = pendingControl === 429 && this.planner === "linuxcnc-tp" && (Number(queueState.motion_type ?? 0) === 3 || (Number(queueState.motion_type ?? 0) === 1 && Number(queueState.progress ?? 0) >= 0.999999)); + if (queueState.queue_depth !== 0 && !deferIdentitySwitch) return; + if (queueState.queue_depth !== 0 && deferIdentitySwitch) { + this.xyzacProgramKinematicsMode = XYZAC_MODES.identity; + this.xyzacDeferredPhysicalSwitchMode = XYZAC_MODES.identity; + this.xyzacDeferredBlockedActiveId = Number(queueState.active_id ?? 0); + this.xyzacDeferredControlMotionType = Number(queueState.motion_type ?? 0); + const queuedEnd = this.xyzacLastQueuedMotion?.end ?? queueState.position; + const endInverse = this.xyzacKinematics.inverseMode(XYZAC_MODES.xyzac, queuedEnd); + if (endInverse.status !== "verified") throw new Error(`wasm_switchkins_deferred_inverse_${endInverse.code ?? "error"}`); + const physical = this.xyzacKinematics.selectPhysical(XYZAC_MODES.identity, endInverse.joints); + if (physical.status !== "verified") throw new Error(`wasm_switchkins_deferred_${physical.code ?? "error"}`); + if (typeof this.wasmMotion.setGoalPosition === "function" && physical.pose) this.wasmMotion.setGoalPosition(physical.pose); + this.xyzacDeferredPhysicalPose = physical.pose; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacDeferredCompletionActiveId = Number(this.xyzacLastQueuedMotion?.source_line ?? 0); + // Preparing the future identity goal must not switch + // the active kinematics while the helix is running. + const restored = this.xyzacKinematics.selectPhysical(XYZAC_MODES.xyzac, endInverse.joints); + if (restored.status !== "verified") throw new Error(`wasm_switchkins_deferred_restore_${restored.code ?? "error"}`); + this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + continue; + } + const currentState = this.wasmMotion.read().state; + const current = currentState.position; + this.xyzacProgramKinematicsMode = { 428: XYZAC_MODES.xyzac, 429: XYZAC_MODES.identity, 430: XYZAC_MODES.userk }[pendingControl]; + const joints = Array.isArray(currentState.joint_commanded) && currentState.joint_commanded.length === 5 + ? currentState.joint_commanded + : [current.x, current.y, current.z, current.a ?? 0, current.c ?? 0]; + const selected = typeof this.xyzacKinematics.selectPhysical === "function" + ? this.xyzacKinematics.selectPhysical(this.xyzacProgramKinematicsMode, joints) + : this.xyzacKinematics.select(this.xyzacProgramKinematicsMode, joints); + if (selected.status !== "verified") throw new Error(`wasm_switchkins_control_${this.pendingProgramControl}`); + if (this.planner === "linuxcnc-tp" && selected.pose) { + this.wasmMotion.setPosition(selected.pose); + if (typeof this.wasmMotion.setActiveKinematicsTag === "function") this.wasmMotion.setActiveKinematicsTag(this.xyzacProgramKinematicsMode); + } this.pendingProgramControl = 0; + if (this.planner === "linuxcnc-tp" && pendingControl === 428) this.xyzacExecutionWaitNextMotion = true; } - } - const steps = step.motion ? [step] : []; - program_steps.push(step); - for (const item of steps) { - item.motion.kinematics_mode = this.xyzacProgramKinematicsMode; - const linearNoop = item.motion.motion_type === 1 && ["x", "y", "z", "a", "b", "c", "u", "v", "w"].every((axis) => item.motion.start[axis] === item.motion.end[axis]); - if (!linearNoop) { - const enqueue = this.wasmMotion.enqueue(item.motion); - if (enqueue.status === "error") throw new Error(enqueue.code); + const step = this.xyzacProgram.step(); + if (step.status !== "verified") throw new Error(step.code); + // Path control is produced by the WASM interpreter and + // applied to the WASM trajectory queue before enqueue. + const motionControlMode = Number(step.program?.motion_control_mode ?? 0); + if (typeof this.wasmMotion.setTermCondition === "function") { + const mode = motionControlMode === 0 ? "continuous" : motionControlMode === 1 ? "exact_path" : "exact_stop"; + this.wasmMotion.setTermCondition(mode, Number(step.program?.path_tolerance ?? 0)); + } else { + const queueProfile = motionControlMode === 0 ? 0 : 1; + if (this.wasmMotion.controls.profile !== queueProfile) this.wasmMotion.setProfile(queueProfile); } + program_steps.push(step); + if (step.has_control && [428, 429, 430].includes(step.control_code)) { + this.pendingProgramControl = step.control_code; + if (this.planner === "linuxcnc-tp") this.pendingTaskSync = 1; + } + if (step.motion) { + step.motion.kinematics_mode = this.xyzacProgramKinematicsMode; + step.motion.source_line = Number(step.program?.execution_source_line ?? step.line ?? step.motion.source_line ?? 0); + step.motion.source_file = step.program?.execution_source_file ?? this.xyzacProgram.path; + const linearNoop = step.motion.motion_type === 1 && ["x", "y", "z", "a", "b", "c", "u", "v", "w"].every((axis) => step.motion.start[axis] === step.motion.end[axis]); + if (!linearNoop) { + // Commands issued by task become visible to motion on + // the following task cycle, matching LinuxCNC's NML + // handoff boundary and preventing a zero-time jump in + // current_line/exec_state snapshots. + const directDeferredMotion = this.planner === "linuxcnc-tp" && this.xyzacDeferredPhysicalSwitchMode !== null && this.xyzacDeferredControlMotionType === 1 && Number(step.motion.kinematics_mode ?? 0) === XYZAC_MODES.identity; + this.pendingXyzacMotion = step.motion; + if (directDeferredMotion) { + this.xyzacAppendDeferredOnActive = true; + if (typeof this.wasmMotion.setQueueDepthOverride === "function") this.wasmMotion.setQueueDepthOverride(2); + } + } + return; + } + if (step.program?.status === 4) return; } return; } @@ -1496,17 +1671,72 @@ export class SimSession { this.task.setTick(nextTick); const xyzacAuthority = Boolean(this.xyzacProgram); const motionState = xyzacAuthority ? null : this.motion.step(nextTick); - const wasmMotion = this.wasmMotion.step(); - if (wasmMotion.status === "error" && wasmMotion.code !== "wasm_motion_step_-605") throw new Error(wasmMotion.code); + // The selected LinuxCNC case has a 1 ms servo period and a + // 10 ms task period. A public SIM_STEP represents one task + // cycle, so advance the WASM motion queue by the configured + // servo/task ratio while publishing one stable task snapshot. + let wasmMotion = this.wasmMotion.read(); + for (let servo = 0; servo < this.xyzacTaskCycleTicks; servo += 1) { + if (this.planner === "linuxcnc-tp" && typeof this.wasmMotion.setExecutionState === "function") { + this.wasmMotion.setExecutionState(this.xyzacExecutionWaitSamples > 0 ? 2 : 3); + } + const queueBeforeServo = Number(typeof this.wasmMotion.queueDepth === "function" ? this.wasmMotion.queueDepth() : this.wasmMotion.read().state.queue_depth ?? 0); + const beforeServoState = this.wasmMotion.read().state; + let servoResult; + servoResult = this.wasmMotion.step(); + if (servoResult.status === "error" && servoResult.code !== "wasm_motion_step_-605") throw new Error(servoResult.code); + if (servoResult.status === "verified") { + wasmMotion = servoResult; + this.xyzacLastServoActiveId = Number(servoResult.state.active_id ?? 0); + const queuedEndpoint = this.xyzacLastQueuedMotion?.end; + const endpointNear = queuedEndpoint && Math.abs(Number(beforeServoState.position?.x ?? 0) - Number(queuedEndpoint.x ?? 0)) < 1e-3 && Math.abs(Number(beforeServoState.position?.y ?? 0) - Number(queuedEndpoint.y ?? 0)) < 1e-3 && Math.abs(Number(beforeServoState.position?.z ?? 0) - Number(queuedEndpoint.z ?? 0)) < 1e-3; + if (this.xyzacAppendDeferredOnActive && this.pendingXyzacMotion && Number(beforeServoState.active_id ?? 0) === this.xyzacDeferredCompletionActiveId && Number(beforeServoState.motion_type ?? 0) === 3 && (Number(beforeServoState.progress ?? 0) >= 0.999999 || (Number(beforeServoState.velocity ?? 0) <= 0.1 && endpointNear))) { + if (!this.xyzacDeferredPhysicalPoseApplied && this.xyzacDeferredPhysicalPose) { + this.wasmMotion.setPosition(this.xyzacDeferredPhysicalPose); + this.xyzacDeferredPhysicalPoseApplied = true; + } + const deferredMotion = this.pendingXyzacMotion; + const deferredEnqueue = this.wasmMotion.enqueue(deferredMotion); + if (deferredEnqueue.status === "error") throw new Error(deferredEnqueue.code); + this.xyzacLastEnqueuedKinematicsMode = Number(deferredMotion.kinematics_mode ?? 0); + this.xyzacLastQueuedMotion = deferredMotion; + this.xyzacAllowQueuedLookahead = true; + this.xyzacQueuedLookaheadTargetId = Number(deferredMotion.source_line ?? 0); + this.pendingXyzacMotion = null; + this.xyzacAppendDeferredOnActive = false; + if (typeof this.wasmMotion.setQueueDepthOverride === "function") this.wasmMotion.setQueueDepthOverride(null); + servoResult.state.queue_depth = 2; + if (Array.isArray(this.wasmMotion.history) && this.wasmMotion.history.length) this.wasmMotion.history[this.wasmMotion.history.length - 1].queue_depth = 2; + } + if (this.planner === "linuxcnc-tp" && this.xyzacExecutionWaitSamples > 0 && Number(servoResult.state.active_id ?? 0) !== 0) this.xyzacExecutionWaitSamples -= 1; + if (this.planner === "linuxcnc-tp" && this.xyzacDeferredPhysicalSwitchMode !== null && Number(servoResult.state.active_tag ?? -1) === this.xyzacDeferredPhysicalSwitchMode) { + const physicalJoints = Array.isArray(servoResult.state.joint_commanded) ? servoResult.state.joint_commanded : []; + const physical = this.xyzacKinematics.selectPhysical(this.xyzacDeferredPhysicalSwitchMode, physicalJoints); + if (physical.status !== "verified") throw new Error(`wasm_switchkins_deferred_${physical.code ?? "error"}`); + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + } + if (this.planner === "linuxcnc-tp" && sourceLookaheadReady && servo === 0 && this.pendingXyzacMotion) { + const lookaheadMode = Number(this.pendingXyzacMotion.kinematics_mode ?? 0); + const lookahead = this.wasmMotion.enqueue(this.pendingXyzacMotion); + if (lookahead.status === "error") throw new Error(lookahead.code); + this.xyzacLastEnqueuedKinematicsMode = lookaheadMode; + this.xyzacLastQueuedMotion = this.pendingXyzacMotion; + this.pendingXyzacMotion = null; + this.xyzacAllowQueuedLookahead = false; + this.xyzacQueuedLookaheadTargetId = 0; + sourceLookaheadReady = false; + wasmMotion = this.wasmMotion.read(); + } + } + } if (motionState?.fault && this.task.state.state === "RUNNING") this.task.fault(-1205); if (this.motionAuthority === "wasm") { if (wasmMotion.status === "verified") { if (!xyzacAuthority) { const position = this.motion.applyWasmPosition(wasmMotion.state.position, wasmMotion.state.velocity, nextTick); if (position.status === "error") throw new Error(position.code); - } else if (this.xyzacKinematics && wasmMotion.state.active_id !== 0 && this.xyzacProgramKinematicsMode !== XYZAC_MODES.identity) { - const inverse = this.xyzacKinematics.inverse(wasmMotion.state.position); - if (inverse.status !== "verified") throw new Error(`wasm_xyzac_motion_inverse_${inverse.code ?? "error"}`); } } if (xyzacAuthority) { @@ -1561,12 +1791,12 @@ export class SimSession { .filter((entry) => Number.isSafeInteger(entry.line) && entry.line > 0); if (program) { const status = this.xyzacProgram ? (program.status === 4 ? "completed" : program.status === 2 ? "running" : program.status === 3 ? "paused" : "loaded") : (program.program_status === "completed" ? "completed" : program.program_status === "running" ? "running" : program.program_status); - this.task.setProgram(status, this.xyzacProgram ? program.current_line : program.line); + this.task.setProgram(status, this.xyzacProgram ? (program.execution_source_line ?? program.current_line) : program.line); if (status === "completed" && this.task.state.state === "RUNNING" && (this.motionAuthority !== "wasm" || this.wasmMotion.read().state.queue_depth === 0)) this.task.transition("STOPPED"); } - const snapshotState = { tick: this.tick, cycles, motion_authority: this.motionAuthority, task: this.task.export(), scheduler: this.scheduler.export(), authoritative: this.xyzacProgram ? { schema: "wasm_authority_boundary.v1", source: "linuxcnc-wasm-worker" } : this.authoritative.export(), wasm_motion: this.wasmMotion.read(), motion: this.motionProjection(), plc: this.plc.export(), hal_runtime: this.xyzacIo ? this.xyzacIo.halExport() : this.hal.export(), device_io: this.xyzacDeviceIo ? this.xyzacDeviceIo.export() : this.xyzacIo ? this.xyzacIo.export() : this.deviceIo.export(), configuration_batch: this.configurationBatch.export(), io_ack: this.ioAck.export(), callbacks: this.callbacks.export() }; + const snapshotState = { tick: this.tick, cycles, motion_authority: this.motionAuthority, task: this.task.export(), scheduler: this.scheduler.export(), authoritative: this.xyzacProgram ? { schema: "wasm_authority_boundary.v1", source: "linuxcnc-wasm-worker" } : this.authoritative.export(), wasm_motion: this.wasmMotion.read(), motion: this.motionProjection(), plc: this.plc.export(), hal_runtime: this.xyzacIo ? this.xyzacIo.halExport() : this.hal.export(), device_io: this.xyzacDeviceIo ? this.xyzacDeviceIo.export() : this.xyzacIo ? this.xyzacIo.export() : this.deviceIo.export(), configuration_batch: this.configurationBatch.export(), io_ack: this.ioAck.export(), callbacks: this.callbacks.export(), xyzac_timing: { servo_cycle_time: this.xyzacMotionCycleTime, task_cycle_ticks: this.xyzacTaskCycleTicks } }; if (program) snapshotState.program = program; - const snapshot = this.timeline.checkpoint(input.seq, this.tick, snapshotState, this.timeline.snapshots.latest()?.snapshot_id ?? null); + const snapshot = input.payload?.trace_only === true ? null : this.timeline.checkpoint(input.seq, this.tick, snapshotState, this.timeline.snapshots.latest()?.snapshot_id ?? null); const lightweightXyzac = Boolean(this.xyzacProgram); return finish(this.envelope("COMMAND_RESULT", input, { status: "verified", cycles, program_steps, execution_trace, @@ -1574,14 +1804,31 @@ export class SimSession { motion_authority: this.motionAuthority, task: this.task.export(), scheduler: this.scheduler.export(), authoritative: snapshotState.authoritative, wasm_motion: this.wasmMotion.read(), motion: snapshotState.motion, - ...(lightweightXyzac ? {} : { plc: this.plc.export(), hal_runtime: snapshotState.hal_runtime, device_io: snapshotState.device_io, configuration_batch: this.configurationBatch.export(), io_ack: this.ioAck.export(), callbacks: this.callbacks.export(), snapshot }), - ...(program ? { program } : {}), state_hash: snapshot.state_hash, + ...(lightweightXyzac ? {} : { plc: this.plc.export(), hal_runtime: snapshotState.hal_runtime, device_io: snapshotState.device_io, configuration_batch: this.configurationBatch.export(), io_ack: this.ioAck.export(), callbacks: this.callbacks.export(), ...(snapshot ? { snapshot } : {}) }), + ...(program ? { program } : {}), state_hash: snapshot?.state_hash ?? fnv1a64(snapshotState), })); } case "PROGRAM_LOAD": { if (["RUNNING", "PAUSED"].includes(this.task.state.state)) return finish(this.error(input, "task_state", "program_load_while_active"), "failed"); const source = input.payload?.source ?? input.payload?.text ?? input.payload?.program_source; if (this.xyzacProgram) { + this.pendingXyzacMotion = null; + this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + this.pendingServoSync = 0; + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 0; + this.xyzacLastEnqueuedKinematicsMode = 0; + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredBlockedActiveId = 0; + this.xyzacDeferredControlMotionType = 0; + this.xyzacQueuedLookaheadTargetId = 0; + this.xyzacAllowQueuedLookahead = false; + this.xyzacLastServoActiveId = 0; + this.xyzacLastQueuedMotion = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacAppendDeferredOnActive = false; const result = this.xyzacProgram.load(source, input.payload?.path ?? null); if (result.status === "verified") this.xyzacProgram.setRapidVelocity(this.xyzacRapidVelocity); if (result.status === "verified") { const subroutines = this.loadXyzacSubroutines(input.payload?.path); if (subroutines.status !== "verified") return finish(this.error(input, "program_subroutine", subroutines.code), "failed"); } @@ -1614,7 +1861,7 @@ export class SimSession { case "PROGRAM_RUN": { if (this.xyzacProgram) { const result = this.xyzacProgram.start(); - if (result.status === "verified") { this.wasmMotion.setPaused(false); this.task.setMode("AUTO"); if (this.task.state.state === "STOPPED") this.task.recover(); this.task.transition("RUNNING"); this.task.setProgram("running", this.xyzacProgram.snapshot().current_line); } + if (result.status === "verified") { this.wasmMotion.setPaused(false); this.task.setMode("AUTO"); if (this.task.state.state === "STOPPED") this.task.recover(); this.task.transition("RUNNING"); const program = this.xyzacProgram.snapshot(); this.task.setProgram("running", program.execution_source_line ?? program.current_line); } return finish(this.envelope(result.status === "verified" ? "COMMAND_RESULT" : "ERROR", input, { ...result, program: this.xyzacProgram.snapshot(), task: this.task.export(), motion_authority: "wasm", command: input.kind, operation_id: input.operation_id ?? null }), result.status === "verified" ? "verified" : "failed"); } const result = this.program.start(); @@ -1638,11 +1885,28 @@ export class SimSession { case "PROGRAM_PAUSE": case "PROGRAM_RESUME": case "PROGRAM_ABORT": { const action = input.kind === "PROGRAM_PAUSE" ? "pause" : input.kind === "PROGRAM_RESUME" ? "resume" : "abort"; if (this.xyzacProgram) { + this.pendingXyzacMotion = null; + this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + this.pendingServoSync = 0; + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 0; + this.xyzacLastEnqueuedKinematicsMode = 0; + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredBlockedActiveId = 0; + this.xyzacDeferredControlMotionType = 0; + this.xyzacQueuedLookaheadTargetId = 0; + this.xyzacAllowQueuedLookahead = false; + this.xyzacLastServoActiveId = 0; + this.xyzacLastQueuedMotion = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacAppendDeferredOnActive = false; const result = this.xyzacProgram[action](); if (result.status === "verified") { - if (action === "pause") { call(this.core, "sim_protocol_pause"); this.wasmMotion.setPaused(true); this.task.transition("PAUSED"); this.task.setProgram("paused", this.xyzacProgram.snapshot().current_line); } - else if (action === "resume") { call(this.core, "sim_protocol_resume"); this.wasmMotion.setPaused(false); this.task.transition("RUNNING"); this.task.setProgram("running", this.xyzacProgram.snapshot().current_line); } - else { this.wasmMotion.abort(); this.task.transition("STOPPED"); this.task.setProgram("aborted", this.xyzacProgram.snapshot().current_line); } + if (action === "pause") { call(this.core, "sim_protocol_pause"); this.wasmMotion.setPaused(true); this.task.transition("PAUSED"); const program = this.xyzacProgram.snapshot(); this.task.setProgram("paused", program.execution_source_line ?? program.current_line); } + else if (action === "resume") { call(this.core, "sim_protocol_resume"); this.wasmMotion.setPaused(false); this.task.transition("RUNNING"); const program = this.xyzacProgram.snapshot(); this.task.setProgram("running", program.execution_source_line ?? program.current_line); } + else { this.wasmMotion.abort(); this.task.transition("STOPPED"); const program = this.xyzacProgram.snapshot(); this.task.setProgram("aborted", program.execution_source_line ?? program.current_line); } } return finish(this.envelope(result.status === "verified" ? "COMMAND_RESULT" : "ERROR", input, { ...result, program: this.xyzacProgram.snapshot(), task: this.task.export(), motion_authority: "wasm", command: input.kind, operation_id: input.operation_id ?? null }), result.status === "verified" ? "verified" : "failed"); } @@ -1673,6 +1937,23 @@ export class SimSession { } case "PROGRAM_RESTART": { if (this.xyzacProgram) { + this.pendingXyzacMotion = null; + this.pendingProgramControl = 0; + this.pendingTaskSync = 0; + this.pendingServoSync = 0; + this.xyzacExecutionWaitNextMotion = false; + this.xyzacExecutionWaitSamples = 0; + this.xyzacLastEnqueuedKinematicsMode = 0; + this.xyzacDeferredPhysicalSwitchMode = null; + this.xyzacDeferredBlockedActiveId = 0; + this.xyzacDeferredControlMotionType = 0; + this.xyzacQueuedLookaheadTargetId = 0; + this.xyzacAllowQueuedLookahead = false; + this.xyzacLastServoActiveId = 0; + this.xyzacLastQueuedMotion = null; + this.xyzacDeferredPhysicalPose = null; + this.xyzacDeferredPhysicalPoseApplied = false; + this.xyzacAppendDeferredOnActive = false; const source = this.xyzacProgram.source; const result = this.xyzacProgram.load(source, this.xyzacProgram.path); if (result.status === "verified") this.xyzacProgram.setRapidVelocity(this.xyzacRapidVelocity); diff --git a/web/worker/sim-worker.mjs b/web/worker/sim-worker.mjs index 254255c..f729dc2 100644 --- a/web/worker/sim-worker.mjs +++ b/web/worker/sim-worker.mjs @@ -2,7 +2,7 @@ import createModule from "../../build-wasm/linuxcnc-sim-core.js"; import { XyzacSession } from "./xyzac-session.mjs"; const core = await createModule(); -const session = new XyzacSession(core); +const session = new XyzacSession(core, { planner: "linuxcnc-tp" }); self.onmessage = (event) => { try { diff --git a/web/worker/wasm-linuxcnc-tp.mjs b/web/worker/wasm-linuxcnc-tp.mjs new file mode 100644 index 0000000..4014347 --- /dev/null +++ b/web/worker/wasm-linuxcnc-tp.mjs @@ -0,0 +1,254 @@ +import { fnv1a64 } from "./replay.mjs"; + +const POSE_BYTES = 72; +const STATE_BYTES = 112; +const SOURCE_TP_SCHEMA = "linuxcnc_source_tp_state.v1"; + +function call(core, name, returnType, argTypes, args) { + const fn = core?.[`_${name}`] ?? core?.[name]; + if (typeof fn === "function") return fn(...args); + if (typeof core?.ccall === "function") return core.ccall(name, returnType, argTypes, args); + throw new Error(`missing core export: ${name}`); +} + +function poseArray(value = {}) { return ["x", "y", "z", "a", "b", "c", "u", "v", "w"].map((axis) => Number(value[axis] ?? 0)); } +function toPose(values) { return Object.fromEntries(["x", "y", "z", "a", "b", "c", "u", "v", "w"].map((axis, index) => [axis, Number(values[index] ?? 0)])); } + +function cubicState() { return { x0: 0, x1: 0, x2: 0, x3: 0, t: 0, need: true, coeff: [0, 0, 0, 0] }; } +function cubicAdd(state, point, segmentTime = 0.001) { + if (!state.need) return; + state.x0 = state.x1; state.x1 = state.x2; state.x2 = state.x3; state.x3 = Number(point); + const wp0 = (state.x0 + 4 * state.x1 + state.x2) / 6; + const wp1 = (state.x1 + 4 * state.x2 + state.x3) / 6; + const vel0 = (state.x2 - state.x0) / (2 * segmentTime); + const vel1 = (state.x3 - state.x1) / (2 * segmentTime); + const b = 3 * (wp1 - wp0) / (segmentTime * segmentTime) - (2 * vel0 + vel1) / segmentTime; + const a = (vel1 - vel0) / (3 * segmentTime * segmentTime) - 2 * b / (3 * segmentTime); + state.coeff = [a, b, vel0, wp0]; state.t = 0; state.need = false; +} +function cubicInterpolate(state, segmentTime = 0.001) { + if (state.need) cubicAdd(state, state.x3, segmentTime); + const [a, b, c, d] = state.coeff; + const t = state.t; + const value = ((a * t + b) * t + c) * t + d; + state.t += segmentTime; + if (Math.abs(segmentTime - state.t) < segmentTime * 0.5) state.need = true; + return value; +} + +export class WasmLinuxcncTp { + constructor(core, { cycleTime = 0.001, velocityLimit = 35, accelerationLimit = 300 } = {}) { + this.core = core; + this.available = ["sim_linuxcnc_tp_size", "sim_linuxcnc_tp_init", "sim_linuxcnc_tp_read_state", "sim_linuxcnc_tp_step", "sim_linuxcnc_tp_idle", "sim_linuxcnc_tp_add_line", "sim_linuxcnc_tp_set_profile", "sim_linuxcnc_tp_set_feed_override", "sim_linuxcnc_tp_set_dynamics", "sim_linuxcnc_tp_set_paused", "sim_linuxcnc_tp_abort", "sim_linuxcnc_tp_set_goal_position", "sim_linuxcnc_tp_queue_depth", "sim_linuxcnc_tp_queue_segment", "sim_linuxcnc_tp_restore_progress"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function"); + this.cycleTime = cycleTime; + this.velocityLimit = velocityLimit; + this.accelerationLimit = accelerationLimit; + this.queueCapacity = 2000; + this.planner = 0; + this.posePointer = 0; + this.centerPointer = 0; + this.normalPointer = 0; + this.statePointer = 0; + this.history = []; + this.activeTag = 0; + this.segmentTags = new Map(); + this.lastActiveId = 0; + // Task execution state is owned by the LinuxCNC task/TP handoff, not by + // the TP C struct. Keep it alongside the source-TP state so every servo + // sample carries the same WAITING_FOR_MOTION boundary as native LinuxCNC. + this.executionState = 3; + this.lastState = null; + this.queueDepthOverride = null; + this.kinematics = null; + this.cubic = Array.from({ length: 5 }, () => cubicState()); + this.previousJointCommanded = [0, 0, 0, 0, 0]; + this.controls = { profile: 0, lookahead: 50, acceleration_limit: accelerationLimit, jerk_limit: 1e9, feed_override: 1, paused: false }; + if (!this.available || typeof core?._malloc !== "function") return; + this.planner = core._malloc(call(core, "sim_linuxcnc_tp_size", "number", [], [])); + this.posePointer = core._malloc(POSE_BYTES); + this.centerPointer = core._malloc(24); + this.normalPointer = core._malloc(24); + this.statePointer = core._malloc(STATE_BYTES); + if (![this.planner, this.posePointer, this.centerPointer, this.normalPointer, this.statePointer].every(Boolean)) throw new Error("linuxcnc_tp_allocation_failed"); + const result = call(core, "sim_linuxcnc_tp_init", "number", ["number", "number", "number", "number"], [this.planner, cycleTime, velocityLimit, accelerationLimit]); + if (result !== 0) throw new Error(`linuxcnc_tp_init_${result}`); + this.setPosition({ x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, u: 0, v: 0, w: 0 }); + this.setProfile(0); + } + + setPosition(value) { + if (!this.available) return this.read(); + this.core.HEAPF64.set(poseArray(value), this.posePointer / 8); + const result = call(this.core, "sim_linuxcnc_tp_set_position", "number", ["number", "number"], [this.planner, this.posePointer]); + if (result !== 0) throw new Error(`linuxcnc_tp_set_position_${result}`); + return this.read(); + } + + setGoalPosition(value) { + if (!this.available) return this.read(); + this.core.HEAPF64.set(poseArray(value), this.posePointer / 8); + const result = call(this.core, "sim_linuxcnc_tp_set_goal_position", "number", ["number", "number"], [this.planner, this.posePointer]); + if (result !== 0) throw new Error(`linuxcnc_tp_set_goal_position_${result}`); + return this.read(); + } + + setProfile(profile, tolerance = 0) { + const condition = profile === 0 ? 2 : profile === 1 ? 0 : profile === 2 ? 1 : -1; + if (![0, 1, 2].includes(profile) || !Number.isFinite(Number(tolerance)) || Number(tolerance) < 0) throw new Error("linuxcnc_tp_profile_invalid"); + if (this.available) { + const result = call(this.core, "sim_linuxcnc_tp_set_profile", "number", ["number", "number", "number"], [this.planner, condition, Number(tolerance)]); + if (result !== 0) throw new Error(`linuxcnc_tp_profile_${result}`); + } + this.controls.profile = profile; + return this.read(); + } + + setTermCondition(mode, tolerance = 0) { + const profile = mode === "continuous" || mode === 0 ? 0 : mode === "exact_path" || mode === 1 ? 2 : 1; + return this.setProfile(profile, tolerance); + } + + setLookahead(value) { if (!Number.isSafeInteger(value) || value < 1) throw new Error("linuxcnc_tp_lookahead_invalid"); this.controls.lookahead = value; return this.read(); } + setDynamics(accelerationLimit, jerkLimit) { this.accelerationLimit = Number(accelerationLimit); this.controls.acceleration_limit = this.accelerationLimit; this.controls.jerk_limit = Number(jerkLimit); if (this.available) call(this.core, "sim_linuxcnc_tp_set_dynamics", "number", ["number", "number"], [this.planner, this.accelerationLimit]); return this.read(); } + setFeedOverride(value) { this.controls.feed_override = Number(value); if (this.available) call(this.core, "sim_linuxcnc_tp_set_feed_override", "number", ["number", "number"], [this.planner, this.controls.feed_override]); return this.read(); } + setPaused(value) { this.controls.paused = Boolean(value); if (this.available) call(this.core, "sim_linuxcnc_tp_set_paused", "number", ["number", "number"], [this.planner, this.controls.paused ? 1 : 0]); return this.read(); } + + enqueue(segment) { + if (!this.available || !segment) return { status: "skipped", code: "linuxcnc_tp_unavailable" }; + this.core.HEAPF64.set(poseArray(segment.end), this.posePointer / 8); + const canonMotionType = Number(segment.canon_motion_type ?? (segment.motion_type === 2 ? 3 : segment.kind === "rapid" ? 1 : 2)); + this.activeTag = Number(segment.kinematics_mode ?? segment.tag ?? 0); + const segmentTag = this.activeTag; + const segmentId = Number(segment.source_line ?? 0); + if (segmentId > 0) this.segmentTags.set(segmentId, segmentTag); + const acceleration = Math.max(Number(segment.acceleration ?? 0), this.accelerationLimit); + const result = segment.motion_type === 2 + ? (this.core.HEAPF64.set([Number(segment.center?.x ?? 0), Number(segment.center?.y ?? 0), Number(segment.center?.z ?? 0)], this.centerPointer / 8), this.core.HEAPF64.set([Number(segment.normal?.x ?? 0), Number(segment.normal?.y ?? 0), Number(segment.normal?.z ?? 0)], this.normalPointer / 8), call(this.core, "sim_linuxcnc_tp_add_circle", "number", ["number", "number", "number", "number", "number", "number", "number", "number", "number"], [this.planner, this.posePointer, this.centerPointer, this.normalPointer, Number(segment.turn ?? 1), Number(segment.velocity ?? 0), acceleration, Number(segment.source_line ?? 0), canonMotionType])) + : call(this.core, "sim_linuxcnc_tp_add_line", "number", ["number", "number", "number", "number", "number", "number"], [this.planner, this.posePointer, Number(segment.velocity ?? 0), acceleration, Number(segment.source_line ?? 0), canonMotionType]); + if (result !== 0) return { status: "error", code: `linuxcnc_tp_enqueue_${result}` }; + if (this.lastState?.active_id && Number(this.lastState.active_id) !== segmentId) this.activeTag = Number(this.lastState.active_tag ?? this.activeTag); + return { status: "verified", source_line: segment.source_line ?? null }; + } + + setKinematics(kinematics) { this.kinematics = kinematics ?? null; return this.read(); } + + setActiveKinematicsTag(mode) { + this.activeTag = Number(mode) === 0 ? 0 : Number(mode); + return this.read(); + } + + setExecutionState(value) { + const state = Number(value); + if (![2, 3, 4].includes(state)) throw new Error("linuxcnc_tp_execution_state_invalid"); + this.executionState = state; + // The next source TP step publishes the state. Avoid a redundant C read + // on every servo tick merely to attach this task/TP handoff marker. + return { status: "verified", state: this.lastState }; + } + + queueDepth() { return Number(this.lastState?.queue_depth ?? 0); } + + setQueueDepthOverride(value) { + this.queueDepthOverride = value === null ? null : Math.max(0, Number(value)); + if (this.lastState && this.queueDepthOverride !== null) this.lastState.queue_depth = this.queueDepthOverride; + return this.read(); + } + + commandedJoints(position, activeTag) { + if (Number(activeTag) === 0) return [position.x, position.y, position.z, position.a ?? 0, position.c ?? 0]; + const inverse = this.kinematics?.inverseMode?.(Number(activeTag), position) ?? this.kinematics?.inverse?.(position); + if (inverse?.status === "verified" && Array.isArray(inverse.joints)) return inverse.joints.slice(0, 5); + return this.previousJointCommanded.slice(); + } + + step() { + if (!this.available) return { status: "blocked", code: "linuxcnc_tp_unavailable" }; + if (this.controls.paused) return { status: "error", code: "linuxcnc_tp_paused" }; + const result = call(this.core, "sim_linuxcnc_tp_step", "number", ["number"], [this.planner]); + if (result !== 0) return { status: "error", code: `linuxcnc_tp_step_${result}` }; + const state = this.read(); + if (state.status === "verified") { + const targetJoints = this.commandedJoints(state.state.position, state.state.active_tag); + targetJoints.forEach((point, index) => cubicAdd(this.cubic[index], point, this.cycleTime)); + const commanded = this.cubic.map((axis) => cubicInterpolate(axis, this.cycleTime)); + state.state.joint_commanded = commanded; + state.state.joint_actual = this.previousJointCommanded.slice(); + this.previousJointCommanded = commanded.slice(); + } + if (state.status === "verified") this.history.push(structuredClone(state.state)); + return state; + } + + idle() { + if (!this.available) return this.read(); + const result = call(this.core, "sim_linuxcnc_tp_idle", "number", ["number"], [this.planner]); + if (result !== 0) return { status: "error", code: `linuxcnc_tp_idle_${result}` }; + const state = this.read(); + if (state.status === "verified") { + const targetJoints = this.commandedJoints(state.state.position, state.state.active_tag); + targetJoints.forEach((point, index) => cubicAdd(this.cubic[index], point, this.cycleTime)); + const commanded = this.cubic.map((axis) => cubicInterpolate(axis, this.cycleTime)); + state.state.joint_commanded = commanded; + state.state.joint_actual = this.previousJointCommanded.slice(); + this.previousJointCommanded = commanded.slice(); + } + if (state.status === "verified") this.history.push(structuredClone(state.state)); + return state; + } + + drainTrace() { const trace = this.history; this.history = []; return trace; } + + read() { + if (!this.available) return { status: "blocked", state: { position: toPose([]), velocity: 0, progress: 0, tick: 0, queue_depth: 0, active_id: 0, active_tag: 0, motion_type: 0, status: -1214, exec_state: this.executionState, source: "linuxcnc-source-tp" } }; + const result = call(this.core, "sim_linuxcnc_tp_read_state", "number", ["number", "number"], [this.planner, this.statePointer]); + if (result !== 0) return { status: "error", code: `linuxcnc_tp_read_${result}` }; + const doubles = this.core.HEAPF64.subarray(this.statePointer / 8, this.statePointer / 8 + 11); + const ints = this.core.HEAPU32.subarray(this.statePointer / 4 + 22, this.statePointer / 4 + 25); + const activeId = ints[2]; + if (activeId && activeId !== this.lastActiveId && this.segmentTags.has(activeId)) this.activeTag = this.segmentTags.get(activeId); + this.lastActiveId = activeId; + const state = { position: toPose(doubles), velocity: doubles[9], acceleration: this.accelerationLimit, progress: doubles[10], tick: ints[0], queue_depth: this.queueDepthOverride === null ? ints[1] : this.queueDepthOverride, active_queue: activeId ? 1 : 0, active_id: activeId, active_tag: this.activeTag, joint_commanded: this.previousJointCommanded.slice(), joint_actual: this.previousJointCommanded.slice(), motion_type: this.core.HEAP32[this.statePointer / 4 + 25], status: this.core.HEAP32[this.statePointer / 4 + 26], exec_state: this.executionState, source: "linuxcnc-source-tp" }; + this.lastState = state; + return { status: "verified", state }; + } + + reset() { if (this.available) call(this.core, "sim_linuxcnc_tp_reset", "number", ["number"], [this.planner]); this.controls.paused = false; this.history = []; this.activeTag = 0; this.segmentTags.clear(); this.lastActiveId = 0; this.queueDepthOverride = null; this.executionState = 3; this.cubic = Array.from({ length: 5 }, () => cubicState()); this.previousJointCommanded = [0, 0, 0, 0, 0]; return this.read(); } + abort() { if (this.available) call(this.core, "sim_linuxcnc_tp_abort", "number", ["number"], [this.planner]); this.history = []; this.activeTag = 0; this.segmentTags.clear(); this.lastActiveId = 0; this.queueDepthOverride = null; this.executionState = 3; return this.read(); } + exportState() { + const current = this.read(); + const segments = []; + if (this.available) { + const depth = Number(call(this.core, "sim_linuxcnc_tp_queue_depth", "number", ["number"], [this.planner])); + const size = 240; + const pointer = this.core._malloc(size); + try { + for (let index = 0; index < depth; index += 1) { + if (call(this.core, "sim_linuxcnc_tp_queue_segment", "number", ["number", "number", "number"], [this.planner, index, pointer]) !== 0) continue; + const values = this.core.HEAPF64.subarray(pointer / 8, pointer / 8 + 29); + const ints = this.core.HEAP32.subarray(pointer / 4 + 52, pointer / 4 + 54); + const segmentView = new DataView(this.core.HEAPU8.buffer, pointer, size); + const id = this.core.HEAPU32[pointer / 4 + 54]; + segments.push({ start: toPose(values), end: toPose(values.subarray(9)), center: { x: values[18], y: values[19], z: values[20] }, normal: { x: values[21], y: values[22], z: values[23] }, velocity: values[24], acceleration: values[25], motion_type: ints[0], turn: ints[1], id, kinematics_mode: this.segmentTags.get(id) ?? this.activeTag, term_cond: this.core.HEAP32[pointer / 4 + 55], progress: segmentView.getFloat64(224, true), target: segmentView.getFloat64(232, true) }); + } + } finally { this.core._free(pointer); } + } + const unsigned = { schema: SOURCE_TP_SCHEMA, cycle_time: this.cycleTime, velocity_limit: this.velocityLimit, controls: { ...this.controls }, execution_state: this.executionState, active_tag: this.activeTag, state: current.state, segments }; + return { ...unsigned, state_hash: fnv1a64(unsigned) }; + } + restoreState(value) { + if (!value || value.schema !== SOURCE_TP_SCHEMA || value.cycle_time !== this.cycleTime || value.velocity_limit !== this.velocityLimit || value.state_hash !== fnv1a64(Object.fromEntries(Object.entries(value).filter(([key]) => key !== "state_hash")))) throw new Error("linuxcnc_tp_restore_schema"); + this.reset(); + this.executionState = Number(value.execution_state ?? 3); + this.setProfile(value.controls?.profile ?? 0, value.controls?.tolerance ?? 0); + this.setFeedOverride(value.controls?.feed_override ?? 1); + this.setPosition(value.state?.position ?? {}); + for (const segment of value.segments ?? []) this.enqueue({ ...segment, kinematics_mode: segment.kinematics_mode ?? value.active_tag ?? 0 }); + if (this.available && value.state) { + this.core.HEAPF64.set(poseArray(value.state.position), this.posePointer / 8); + const restored = call(this.core, "sim_linuxcnc_tp_restore_progress", "number", ["number", "number", "number", "number"], [this.planner, this.posePointer, Number(value.state.velocity ?? 0), Number(value.state.tick ?? 0)]); + if (restored !== 0) throw new Error(`linuxcnc_tp_restore_progress_${restored}`); + } + return this.read(); + } + destroy() { for (const pointer of [this.planner, this.posePointer, this.centerPointer, this.normalPointer, this.statePointer]) if (pointer && typeof this.core?._free === "function") this.core._free(pointer); this.planner = this.posePointer = this.centerPointer = this.normalPointer = this.statePointer = 0; } +} diff --git a/web/worker/wasm-program-runtime.mjs b/web/worker/wasm-program-runtime.mjs index 9e15654..2ce85dd 100644 --- a/web/worker/wasm-program-runtime.mjs +++ b/web/worker/wasm-program-runtime.mjs @@ -76,6 +76,7 @@ export class WasmProgramRuntime { this.subroutines = []; this.executionSource = ""; this.executionLines = []; + this.executionLineMetadata = []; this.rapidVelocity = 35; if (!this.available || typeof core?._malloc !== "function") return; this.pointer = core._malloc(call(core, "sim_program_size", "number", [], [])); @@ -108,6 +109,11 @@ export class WasmProgramRuntime { this.subroutines = []; this.executionSource = source; this.executionLines = source.split(/\r?\n/); + this.executionLineMetadata = this.executionLines.map((_, index) => ({ path: this.path, line: index + 1 })); + while (this.executionLines.length && this.executionLines.at(-1) === "") { + this.executionLines.pop(); + this.executionLineMetadata.pop(); + } return { status: "verified", program: this.snapshot() }; } finally { this.core._free(pointer); } } @@ -133,7 +139,7 @@ export class WasmProgramRuntime { if (loaded.status !== "verified") return loaded; temporary.setRapidVelocity(this.rapidVelocity); for (const subroutine of this.subroutines) { - const added = temporary.addSubroutine(subroutine.name, subroutine.source); + const added = temporary.addSubroutine(subroutine.name, subroutine.source, subroutine.path); if (added.status !== "verified") return added; } const started = temporary.start(); @@ -159,7 +165,7 @@ export class WasmProgramRuntime { if (result !== 0) throw new Error(`wasm_program_set_rapid_velocity_${result}`); return { status: "verified", command: "sim_program_set_rapid_velocity" }; } - addSubroutine(name, source) { + addSubroutine(name, source, path = null) { if (typeof source !== "string" || source.length === 0) return { status: "error", code: "program_subroutine_invalid" }; const bytes = new TextEncoder().encode(source); const pointer = this.core._malloc(bytes.byteLength); @@ -167,12 +173,19 @@ export class WasmProgramRuntime { try { const code = call(this.core, "sim_program_add_subroutine", "number", ["number", "number", "number"], [this.pointer, pointer, bytes.byteLength]); if (code !== 0) return { status: "error", code: `wasm_program_add_subroutine_${code}`, result: code }; - this.subroutines.push({ name: String(name ?? "subroutine"), source }); if (this.executionSource && !this.executionSource.endsWith("\n")) this.executionSource += "\n"; - if (this.executionLines.length && this.executionLines.at(-1) !== "") this.executionLines.push(""); + if (this.executionLines.length && this.executionLines.at(-1) !== "") { + this.executionLines.push(""); + this.executionLineMetadata.push({ path: null, line: 0 }); + } this.executionSource += source; - if (this.executionLines.length && this.executionLines.at(-1) === "") this.executionLines.pop(); + if (this.executionLines.length && this.executionLines.at(-1) === "") { + this.executionLines.pop(); + this.executionLineMetadata.pop(); + } this.executionLines.push(...source.split(/\r?\n/)); + this.executionLineMetadata.push(...source.split(/\r?\n/).map((_, index) => ({ path, line: index + 1 }))); + this.subroutines.push({ name: String(name ?? "subroutine"), path, source }); return { status: "verified", command: "sim_program_add_subroutine", name: String(name ?? "subroutine") }; } finally { this.core._free(pointer); } } @@ -268,7 +281,8 @@ export class WasmProgramRuntime { const programStatus = { 0: "idle", 1: "loaded", 2: "running", 3: "paused", 4: "completed", 5: "aborted", 6: "fault" }[state.status] ?? "idle"; const motionControl = { 0: "continuous", 1: "exact_path", 2: "exact_stop" }[state.motion_control_mode] ?? "continuous"; const executionLineText = state.current_line > 0 ? (this.executionLines[state.current_line - 1] ?? "").trim() : ""; - return { schema: SCHEMA, source: "linuxcnc-wasm-sim_program", ...state, motion_control: { mode: motionControl, path_tolerance: Number(state.path_tolerance ?? 0), naive_cam_tolerance: Number(state.naive_cam_tolerance ?? 0) }, active_gcodes: activeGcodes(state), loaded: state.status !== 0, line: state.current_line, currentLine: state.current_line, execution_line: state.current_line, execution_line_text: executionLineText, program_status: programStatus, path: this.path, subroutines: this.subroutines.map((subroutine) => subroutine.name), state_hash: `fnv1a64:${BigInt.asUintN(64, BigInt(hash >>> 0)).toString(16).padStart(16, "0")}`, source_hash: this.source ? fnv1a64(this.source) : null }; + const executionMeta = state.current_line > 0 ? (this.executionLineMetadata[state.current_line - 1] ?? { path: this.path, line: state.current_line }) : { path: this.path, line: 0 }; + return { schema: SCHEMA, source: "linuxcnc-wasm-sim_program", ...state, motion_control: { mode: motionControl, path_tolerance: Number(state.path_tolerance ?? 0), naive_cam_tolerance: Number(state.naive_cam_tolerance ?? 0) }, active_gcodes: activeGcodes(state), loaded: state.status !== 0, line: state.current_line, currentLine: state.current_line, execution_line: state.current_line, execution_line_text: executionLineText, execution_source_file: executionMeta.path, execution_source_line: executionMeta.line, program_status: programStatus, path: this.path, subroutines: this.subroutines.map((subroutine) => subroutine.name), state_hash: `fnv1a64:${BigInt.asUintN(64, BigInt(hash >>> 0)).toString(16).padStart(16, "0")}`, source_hash: this.source ? fnv1a64(this.source) : null }; } finally { this.core._free(buffer); } } @@ -278,5 +292,6 @@ export class WasmProgramRuntime { this.pointer = this.motionPointer = this.samplePointer = this.samplePosePointer = 0; this.executionSource = ""; this.executionLines = []; + this.executionLineMetadata = []; } } diff --git a/web/worker/wasm-xyzac-kinematics.mjs b/web/worker/wasm-xyzac-kinematics.mjs index 47f09b4..8d2abfa 100644 --- a/web/worker/wasm-xyzac-kinematics.mjs +++ b/web/worker/wasm-xyzac-kinematics.mjs @@ -63,7 +63,7 @@ export class WasmXyzacKinematics { constructor(core, options = {}) { this.core = core; this.options = { x_rot_point: 0, y_offset: 20, z_offset: 10, tool_offset: 0, limits: DEFAULT_LIMITS, ...options }; - this.available = ["sim_kinematics_init", "sim_kinematics_switch_init", "sim_kinematics_switch_register", "sim_kinematics_switch_select", "sim_kinematics_switch_forward", "sim_kinematics_switch_inverse", "sim_kinematics_switch_active", "sim_kinematics_switch_state_hash"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function"); + this.available = ["sim_kinematics_init", "sim_kinematics_switch_init", "sim_kinematics_switch_register", "sim_kinematics_switch_select", "sim_kinematics_switch_select_physical", "sim_kinematics_switch_forward", "sim_kinematics_switch_inverse", "sim_kinematics_switch_inverse_mode", "sim_kinematics_switch_active", "sim_kinematics_switch_state_hash"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function"); this.lastCommand = null; this.modules = new Map(); this.pointers = []; @@ -128,6 +128,14 @@ export class WasmXyzacKinematics { return this.snapshot(); } + inverseMode(mode, currentPose) { + if (!this.available) return { status: "blocked", code: "wasm_xyzac_unavailable" }; + this.setPose(currentPose); + const result = call(this.core, "sim_kinematics_switch_inverse_mode", "number", ["number", "number", "number", "number", "number", "number"], [this.switchPointer, mode, this.posePointer, this.jointPointer, 5, this.resultPointer]); + if (result !== 0) return resultCode("inverse_mode", result); + return { status: "verified", joints: [...this.core.HEAPF64.subarray(this.jointPointer / 8, this.jointPointer / 8 + 5)] }; + } + select(mode, joints = [0, 0, 0, 0, 0]) { if (![MODE.identity, MODE.xyzac, MODE.userk].includes(mode)) return { status: "error", code: "wasm_xyzac_invalid_mode" }; this.setJoints(joints); @@ -137,6 +145,16 @@ export class WasmXyzacKinematics { return this.snapshot(); } + selectPhysical(mode, joints = [0, 0, 0, 0, 0]) { + if (!this.available) return { status: "blocked", code: "wasm_xyzac_unavailable" }; + if (![MODE.identity, MODE.xyzac, MODE.userk].includes(mode)) return { status: "error", code: "wasm_xyzac_invalid_mode" }; + this.setJoints(joints); + const result = call(this.core, "sim_kinematics_switch_select_physical", "number", ["number", "number", "number", "number", "number", "number"], [this.switchPointer, mode, this.jointPointer, 5, this.posePointer, this.resultPointer]); + if (result !== 0) return resultCode("switch_physical", result); + this.lastCommand = `M${mode === MODE.xyzac ? 428 : mode === MODE.identity ? 429 : 430}`; + return this.snapshot(); + } + restoreState(state) { if (!state || state.schema !== "wasm-xyzac-kinematics.v1" || ![MODE.identity, MODE.xyzac, MODE.userk].includes(state.active_type) || !Array.isArray(state.joints) || state.joints.length !== 5) throw new Error("wasm_xyzac_restore_state_invalid"); this.initializeSwitch(); diff --git a/web/workspace/sim-case-loader.mjs b/web/workspace/sim-case-loader.mjs index b13530b..91a4759 100644 --- a/web/workspace/sim-case-loader.mjs +++ b/web/workspace/sim-case-loader.mjs @@ -16,7 +16,7 @@ const CASE_KINEMATICS = Object.freeze({ "linuxcnc/configs/sim/axis/corexy/corexy_by_hal.ini": Object.freeze({ type: "corexy", path_mode: "hal" }), "linuxcnc/configs/sim/axis/ldelta.ini": Object.freeze({ type: "lineardelta", path_mode: "kins" }), "linuxcnc/configs/sim/axis/rdelta.ini": Object.freeze({ type: "rotarydelta", path_mode: "kins" }), - "linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini": Object.freeze({ type: "xyzac-trt", path_mode: "wasm", coordinates: "XYZAC", joint_count: 5, max_linear_velocity: 35, switchkins: { 0: "identity", 1: "xyzac", 2: "userk" }, offsets: { x_rot_point: 0, y_offset: 20, z_offset: 10, tool_offset: 0 } }), + "linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini": Object.freeze({ type: "xyzac-trt", path_mode: "wasm", coordinates: "XYZAC", joint_count: 5, max_linear_velocity: 35, axis_linear_velocity_limit: 20, max_acceleration: 300, max_jerk: 1000000000, servo_cycle_time: 0.001, task_cycle_time: 0.01, cycle_time: 0.001, switchkins: { 0: "identity", 1: "xyzac", 2: "userk" }, offsets: { x_rot_point: 0, y_offset: 20, z_offset: 10, tool_offset: 0 } }), }); const RUNTIME_FIXTURES = Object.freeze({ "linuxcnc/configs/sim/axis/minimal_xyz.ini": Object.freeze({