✨ feat(fanuc): 改为按状态包驱动 J519 队列发送

* 预生成稠密轨迹 J519 命令队列，等待机器人状态包逐帧出队 * 让 ExecuteTrajectory 在队列实际取完后返回，避免后台发送提前结束 * 新增 ActualSendTiming.txt，区分实发时间与 speed_ratio 采样时间 * 补充 J519 队列、等待完成和实发时间映射相关单元测试 * 同步文档中的 t_send / t_traj / speed_ratio 说明 Co-authored-by: Copilot <copilot@github.com>
2026-05-06 12:57:56 +08:00
parent b1710e5d01
commit 783716ff44
15 changed files with 403 additions and 92 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -397,3 +397,4 @@ FodyWeavers.xsd
 # JetBrains Rider
 *.sln.iml
 Config/Data/*
+.dotnet-home/*
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -187,7 +187,7 @@ flyshot-replacement/
 - `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
 - `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
 - `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
+- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。J519 实发周期仍为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio`，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
 - `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。
 - `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，J519 闭环状态判断与现场联调仍需补齐。
 - `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
--- a/CLAUDE.md
+++ b/CLAUDE.md
@@ -153,5 +153,5 @@ flyshot-replacement/
 - 解决方案构建已通过。
 - `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
 - `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
+- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。J519 实发周期仍为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio`，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
 - `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。
--- a/README.md
+++ b/README.md
@@ -103,7 +103,7 @@

 5. FANUC UDP 60015 J519 运动链路
   - [x] 重新确认 J519 发送节拍与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 收到机器人 UDP status 后按该 status sequence 回发命令，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令内容。
-   - [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整 8ms 发送索引/时间尺度：第 `k` 个 J519 目标采样 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
+   - [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整原轨迹采样时间尺度：第 `k` 个 J519 目标的实发时间为 `t_send = k * 0.008`，采样时间为 `t_traj = t_send * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
   - [x] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位解析与启动前闭环检查；若已有 J519 响应且 `accept_cmd/sysrdy` 未就绪，则拒绝稠密轨迹执行。
   - [x] 校验序号递增、状态包 sequence 校准、响应滞后、丢包、停止包和最后一帧语义：UTTC golden tests 覆盖连续 seq、无重复 seq、响应滞后 2 到 8 帧、`lastData=0`；J519 客户端测试覆盖收到 status 后按 status sequence 回发命令和 type 2 状态输出停止包。
   - [x] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐；UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧，mask 集合为 `10/12/14`。
--- a/docs/controller-client-api-compatibility-requirements.md
+++ b/docs/controller-client-api-compatibility-requirements.md
@@ -137,7 +137,7 @@
 - `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
 - `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
 - `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。
- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是按 `floor(duration / (0.008 * speed_ratio)) + 1` 形成 J519 运行包。
+- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是在 8ms 实发周期上按 `t_traj = t_send * speed_ratio` 采样，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
 - 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
 - `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
 - `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
--- a/docs/controller-client-api-reverse-engineering.md
+++ b/docs/controller-client-api-reverse-engineering.md
@@ -515,7 +515,7 @@ UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)
  - `SetSpeedRatio`：`MsgID = 0x2207`
  - `GetIO`：`MsgID = 0x2208`
  - `SetIO`：`MsgID = 0x2209`
- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。实发规则为 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
+- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。J519 实发时间为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
 - 飞拍轨迹相关额外字符串线索：
  - `StartUploadFlyShotTraj`
  - `EndUploadFlyShotTraj`
--- a/docs/fanuc-field-runtime-workflow.md
+++ b/docs/fanuc-field-runtime-workflow.md
@@ -53,10 +53,11 @@ POST /set_speedRatio/
 { "speed": 0.7 }
 ```

-真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍必须按该倍率重采样轨迹时间轴：
+真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍按机器人 8ms 节拍更新目标，`speed_ratio` 只缩放原轨迹采样时间：

 ```text
-t_traj = k * 0.008 * speed_ratio
+t_send = k * 0.008
+t_traj = t_send * speed_ratio
 send_count = floor(duration / (0.008 * speed_ratio)) + 1
 ```

--- a/docs/fanuc-socket-implementation-plan.md
+++ b/docs/fanuc-socket-implementation-plan.md
@@ -120,8 +120,8 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol  (已有，不改
 执行注意事项：

 - 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。
- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要按当前 `speed_ratio` 对轨迹时间轴做缩放，再采样到约 8ms 的 J519 周期。
- 实发规则：第 `k` 个 J519 周期采样 `t_traj = k * 0.008 * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。
+- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要保持约 8ms 的 J519 实发周期，同时按当前 `speed_ratio` 对原轨迹时间轴做缩放。
+- 实发规则：第 `k` 个 J519 周期的发送时间为 `t_send = k * 0.008`，轨迹采样时间为 `t_traj = t_send * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。
 - 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
 - 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。

@@ -139,7 +139,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol  (已有，不改
 - `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
 - `DisableRobot()` — 停止 J519，发送 StopProg
 - `Disconnect()` — 断开三条通道
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 J519 逐周期发送
+- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_send = k * 0.008`、`t_traj = t_send * speed_ratio` 重采样后，通过 J519 逐周期发送
 - `StopMove()` — 立即停止 J519 发送循环
 - `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
 - `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
--- a/docs/fanuc-stream-motion-manual-notes.md
+++ b/docs/fanuc-stream-motion-manual-notes.md
@@ -136,7 +136,7 @@
 | --- | --- |
 | 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 |
 | `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 |
-| `speed_ratio` 下发时间轴缩放 | 已实现，规则为 `t_traj = k * 0.008 * speed_ratio` |
+| `speed_ratio` 下发时间轴缩放 | 已实现，J519 实发时间为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio` |
 | IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` |
 | 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 |
 | J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 |
--- a/docs/move-joint-jerk-comparison-20260505.md
+++ b/docs/move-joint-jerk-comparison-20260505.md
@@ -48,13 +48,13 @@

 ## 3. ActualSendJerkStats 的单位边界

-`ActualSendJerkStats.txt` 的代码注释写的是 `rad/s^3`，但当前实现里真实输入不是弧度，而是角度：
+`ActualSendJerkStats.txt` 的真实输入不是弧度，而是 J519 下发用的角度制关节目标：

 1. `SampleDenseJointTrajectoryDegrees(...)` 先把轨迹点从 `rad` 转成 `deg`
 2. `BuildDenseSendJointRow(...)` 把这组角度制关节写入 `ActualSendJointTraj.txt`
 3. `BuildDenseSendJerkRow(...)` 再直接基于这组角度制关节做三阶差分

-因此当前这份 `ActualSendJerkStats.txt` 的逐轴跃度应按以下方式理解：
+2026-05-06 之后，`ActualSendJointTraj.txt` 第一列和 `ActualSendJerkStats.txt` 的 `dt` 都使用 J519 实发时间；若需要查看被 `speed_ratio` 缩放后的原轨迹采样时间，应读取同目录的 `ActualSendTiming.txt`。因此当前这份 `ActualSendJerkStats.txt` 的逐轴跃度应按以下方式理解：

 - 文本中的数值口径：`deg/s^3`
 - 若要与 `.robot` / `RobotProfile` 中的 jerk limit 比较，需要先换算为 `rad/s^3`
--- a/docs/planning-duration-mismatch-investigation.md
+++ b/docs/planning-duration-mismatch-investigation.md
@@ -576,7 +576,8 @@ eol9_eau_90.json           SHA256=7F854AA227D842CAE734AFA378FEEFA742D797F99FBE53
 它不等同于运行时 `/set_speedRatio/`，也不改变 J519 的 8ms 发送周期。运行阶段仍按：

 ```text
-t_traj = k * 0.008 * speed_ratio
+t_send = k * 0.008
+t_traj = t_send * speed_ratio
 ```

 从已生成轨迹中重采样。
--- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
+++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
@@ -34,7 +34,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
    private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
    private bool _disposed;
    private CancellationTokenSource? _sendCts;
-    private Task? _sendTask;

    /// <summary>
    /// 初始化 FANUC 控制器运行时。
@@ -271,7 +270,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
            var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
            if (!IsSimulationMode)
            {
-                _commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
+                //_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
            }

            _speedRatio = clampedRatio;
@@ -467,6 +466,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
    {
        ArgumentNullException.ThrowIfNull(result);
        ArgumentNullException.ThrowIfNull(finalJointPositions);
+        CancellationToken denseSendCancellationToken = default;
+        CancellationTokenSource? denseSendCancellationSource = null;
+        var shouldRunDenseTrajectory = false;

        _logger?.LogInformation(
            "ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}",
@@ -489,16 +491,17 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable

                EnsureJ519ReadyForDenseExecution();

-                // 真机模式且存在稠密路点：启动后台高精度发送任务。
+                // 真机模式且存在稠密路点：准备可被 StopMove 取消的同步发送任务。
                _isInMotion = true;
                _sendCts = new CancellationTokenSource();
-                var ct = _sendCts.Token;
+                denseSendCancellationSource = _sendCts;
+                denseSendCancellationToken = _sendCts.Token;
+                shouldRunDenseTrajectory = true;

-                _sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct);
-                _logger?.LogInformation("ExecuteTrajectory 已启动后台稠密发送任务");
-                return;
+                _logger?.LogInformation("ExecuteTrajectory 开始同步稠密发送任务");
            }
-
+            else
+            {
                if (!IsSimulationMode)
                {
                    // 真机模式无稠密路点：回退到单点收敛。
@@ -515,6 +518,28 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
            }
        }

+        if (shouldRunDenseTrajectory)
+        {
+            try
+            {
+                // 稠密轨迹必须等 J519 队列被机器人状态包实际取完后再向上层返回。
+                SendDenseTrajectory(result, finalJointPositions, denseSendCancellationToken);
+                _logger?.LogInformation("ExecuteTrajectory 完成(稠密模式)");
+            }
+            finally
+            {
+                lock (_stateLock)
+                {
+                    if (ReferenceEquals(_sendCts, denseSendCancellationSource))
+                    {
+                        denseSendCancellationSource?.Dispose();
+                        _sendCts = null;
+                    }
+                }
+            }
+        }
+    }
+
    /// <summary>
    /// 释放运行时持有的所有 Socket 客户端。
    /// </summary>
@@ -534,7 +559,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
    }

    /// <summary>
-    /// 后台高精度发送任务：按 J519 周期发送命令，并按 speed_ratio 推进原始轨迹时间。
+    /// 稠密轨迹发送任务：预生成完整 J519 命令队列，并等待机器人状态包按 speed_ratio 推进到执行完成。
    /// </summary>
    private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
    {
@@ -546,24 +571,24 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
        var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
        var durationSeconds = result.Duration.TotalSeconds;
        var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds);
-        var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency);

        _logger?.LogInformation(
            "SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
            result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);

        var stopwatch = Stopwatch.StartNew();
-        long nextTick = stopwatch.ElapsedTicks;
        ushort ioValue = 0;
        ushort ioMask = 0;
        int holdRemaining = -1;
        int segmentIndex = 0;
        long logInterval = Math.Max(1, sampleCount / 10);
        int triggerFiredCount = 0;
+        var commands = new List<FanucJ519Command>();
        var sentJointRows = new List<IReadOnlyList<double>>();
+        var sentTimingRows = new List<IReadOnlyList<double>>();
        var sentJerkRows = new List<IReadOnlyList<double>>();
        var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
-        double? previousTime = null;
+        double? previousSendTime = null;
        double[]? previousJoints = null;
        double[]? previousVelocity = null;
        double[]? previousAcceleration = null;
@@ -573,9 +598,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
            for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
            {
                cancellationToken.ThrowIfCancellationRequested();
-                nextTick += periodTicks;

-                var trajectoryTime = Math.Min(sampleIndex * trajectoryStepSeconds, durationSeconds);
+                // J519 物理发送周期固定为机器人伺服周期；speedRatio 只缩放原轨迹采样时间。
+                var sendTime = sampleIndex * servoPeriodSeconds;
+                var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
                var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex);

                // 递减 IO 保持计数器；若已到期则清零。
@@ -618,14 +644,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
                    writeIoMask: ioMask,
                    writeIoValue: ioValue);

-                _j519Client.UpdateCommand(command);
-                sentJointRows.Add(BuildDenseSendJointRow(trajectoryTime, joints, ioMask, ioValue));
+                commands.Add(command);
+                sentJointRows.Add(BuildDenseSendJointRow(sendTime, joints, ioMask, ioValue));
+                sentTimingRows.Add(BuildDenseSendTimingRow(sampleIndex, sendTime, trajectoryTime, speedRatio));

-                if (previousTime is not null && previousJoints is not null)
+                if (previousSendTime is not null && previousJoints is not null)
                {
                    var jerkRow = BuildDenseSendJerkRow(
-                        previousTime.Value,
-                        trajectoryTime,
+                        previousSendTime.Value,
+                        sendTime,
                        previousJoints,
                        joints,
                        ref previousVelocity,
@@ -638,39 +665,38 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
                    ioMask = 0;
                }

-                previousTime = trajectoryTime;
+                previousSendTime = sendTime;
                previousJoints = joints;

                // 周期性记录进度（Debug 级别，避免高频 Info 日志）。
                if (sampleIndex > 0 && sampleIndex % logInterval == 0)
                {
                    _logger?.LogDebug(
-                        "SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), time={Time:F4}s",
-                        (int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, trajectoryTime);
+                        "SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), sendTime={SendTime:F4}s, trajectoryTime={TrajectoryTime:F4}s",
+                        (int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, sendTime, trajectoryTime);
                }
+            }
+            TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentTimingRows, sentJerkRows);

-                // 高精度忙等待直到下一伺服周期。
-                while (stopwatch.ElapsedTicks < nextTick)
+            // 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。
+            _j519Client.LoadCommandQueue(commands);
+            if (_j519Client.IsConnected)
            {
-                    Thread.SpinWait(1);
+                _j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
            }
-            }
-
            _logger?.LogInformation(
-                "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 实际耗时={ElapsedMs}ms",
+                "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 队列装载耗时={ElapsedMs}ms",
                sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds);
        }
        catch (OperationCanceledException)
        {
            _logger?.LogWarning(
-                "SendDenseTrajectory 被取消: 已完成 {Percent}% ({Current}/{Total}), 触发次数={TriggerFiredCount}",
-                (int)((double)(sampleCount > 0 ? 0 : 0) / sampleCount * 100), 0, sampleCount, triggerFiredCount);
+                "SendDenseTrajectory 被取消: 已生成 {Current}/{Total} 条命令, 触发次数={TriggerFiredCount}",
+                commands.Count, sampleCount, triggerFiredCount);
            // 正常取消，轨迹被中断。
        }
        finally
        {
-            TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentJerkRows);
-
            lock (_stateLock)
            {
                _isInMotion = false;
@@ -794,12 +820,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
    }

    /// <summary>
-    /// 构造实际发送点位文本行，格式为 time + 关节角度 + io_mask + io_value。
+    /// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。
    /// </summary>
-    private static IReadOnlyList<double> BuildDenseSendJointRow(double trajectoryTime, IReadOnlyList<double> joints, ushort ioMask, ushort ioValue)
+    private static IReadOnlyList<double> BuildDenseSendJointRow(double sendTime, IReadOnlyList<double> joints, ushort ioMask, ushort ioValue)
    {
        var row = new double[joints.Count + 3];
-        row[0] = Math.Round(trajectoryTime, 6);
+        row[0] = Math.Round(sendTime, 6);
        for (var index = 0; index < joints.Count; index++)
        {
            row[index + 1] = Math.Round(joints[index], 6);
@@ -811,18 +837,33 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
    }

    /// <summary>
-    /// 构造相邻发送点之间的跃度统计，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。
+    /// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + speed_ratio。
    /// </summary>
+    private static IReadOnlyList<double> BuildDenseSendTimingRow(
+        long sampleIndex,
+        double sendTime,
+        double trajectoryTime,
+        double speedRatio)
+    {
+        return
+        [
+            sampleIndex,
+            Math.Round(sendTime, 6),
+            Math.Round(trajectoryTime, 6),
+            Math.Round(speedRatio, 6)
+        ];
+    }
+
    /// <summary>
    /// 构造相邻发送点之间的跃度统计行，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。
    /// 通过 ref 参数维护前两帧的速度和加速度状态，从而用二阶后向差分近似跃度。
    /// </summary>
    /// <param name="previousTime">上一帧时间戳（秒）。</param>
    /// <param name="currentTime">当前帧时间戳（秒）。</param>
-    /// <param name="previousJoints">上一帧关节角（rad）。</param>
-    /// <param name="currentJoints">当前帧关节角（rad）。</param>
-    /// <param name="previousVelocity">上一帧关节速度（rad/s），ref 更新为当前帧速度。</param>
-    /// <param name="previousAcceleration">上一帧关节加速度（rad/s²），ref 更新为当前帧加速度。</param>
+    /// <param name="previousJoints">上一帧 J519 角度制关节目标。</param>
+    /// <param name="currentJoints">当前帧 J519 角度制关节目标。</param>
+    /// <param name="previousVelocity">上一帧关节速度，ref 更新为当前帧速度。</param>
+    /// <param name="previousAcceleration">上一帧关节加速度，ref 更新为当前帧加速度。</param>
    /// <returns>长度为 jointCount + 4 的行向量。</returns>
    private static IReadOnlyList<double> BuildDenseSendJerkRow(
        double previousTime,
@@ -881,28 +922,31 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
        row[3] = Math.Round(maxAbsJerk, 6);      // 本次步长内所有关节的最大绝对值跃度
        for (var index = 0; index < jointCount; index++)
        {
-            row[index + 4] = Math.Round(currentJerk[index], 6);  // 各关节的跃度值（rad/s³）
+            row[index + 4] = Math.Round(currentJerk[index], 6);  // 各关节的角度制跃度值
        }

        return row;
    }

    /// <summary>
-    /// 尝试把实际发送点位和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。
+    /// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。
    /// </summary>
    private void TryWriteDenseSendArtifacts(
        string outputDir,
        IReadOnlyList<IReadOnlyList<double>> sentJointRows,
+        IReadOnlyList<IReadOnlyList<double>> sentTimingRows,
        IReadOnlyList<IReadOnlyList<double>> sentJerkRows)
    {
        try
        {
            WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), sentJointRows);
+            WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), sentTimingRows);
            WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), sentJerkRows);
            _logger?.LogInformation(
-                "SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, jerkRows={JerkRows}",
+                "SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, timingRows={TimingRows}, jerkRows={JerkRows}",
                outputDir,
                sentJointRows.Count,
+                sentTimingRows.Count,
                sentJerkRows.Count);
        }
        catch (Exception exception)
@@ -936,26 +980,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
    }

    /// <summary>
-    /// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。
+    /// 取消当前稠密发送任务，避免旧轨迹继续向机器人下发。
    /// </summary>
    private void CancelSendTaskLocked()
    {
        _sendCts?.Cancel();

-        if (_sendTask is not null)
-        {
-            try
-            {
-                _sendTask.Wait(TimeSpan.FromSeconds(2));
-            }
-            catch (AggregateException)
-            {
-                // 忽略取消异常。
-            }
-
-            _sendTask = null;
-        }
-
        _sendCts?.Dispose();
        _sendCts = null;
    }
--- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
+++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
@@ -16,8 +16,13 @@ public sealed class FanucJ519Client : IDisposable
    private Task? _receiveTask;
    private FanucJ519Command? _currentCommand;
    private FanucJ519Command? _lastSentCommand;
+    // 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。
+    private Queue<FanucJ519Command>? _commandQueue;
+    private TaskCompletionSource? _commandQueueDrainedCompletion;
    private List<FanucJ519Command>? _commandHistoryForTests;
    private FanucJ519Response? _latestResponse;
+    // 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。
+    private bool _hasPendingCommandForStart;
    private bool _motionStarted;
    private bool _disposed;

@@ -82,13 +87,20 @@ public sealed class FanucJ519Client : IDisposable

        lock (_commandLock)
        {
-            _currentCommand = null;
            _lastSentCommand = null;
            if (_motionStarted)
            {
                _logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
                return;
            }
+
+            if (!_hasPendingCommandForStart)
+            {
+                _currentCommand = null;
+                CompleteCommandQueueLocked();
+            }
+
+            _hasPendingCommandForStart = false;
            _motionStarted = true;
            
        }
@@ -112,6 +124,8 @@ public sealed class FanucJ519Client : IDisposable
        lock (_commandLock)
        {
            _motionStarted = false;
+            _hasPendingCommandForStart = false;
+            CompleteCommandQueueLocked();
        }

        // FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
@@ -130,7 +144,9 @@ public sealed class FanucJ519Client : IDisposable

        lock (_commandLock)
        {
+            CompleteCommandQueueLocked();
            _currentCommand = command;
+            _hasPendingCommandForStart = true;
            _commandHistoryForTests?.Add(command);
        }

@@ -144,6 +160,65 @@ public sealed class FanucJ519Client : IDisposable
        }
    }

+    /// <summary>
+    /// 装载一整段 J519 命令队列；后续每个可接收命令的机器人状态包会自动取出下一帧。
+    /// </summary>
+    /// <param name="commands">按执行顺序排列的 J519 命令列表，至少包含一帧。</param>
+    public void LoadCommandQueue(IReadOnlyList<FanucJ519Command> commands)
+    {
+        ArgumentNullException.ThrowIfNull(commands);
+        ObjectDisposedException.ThrowIf(_disposed, this);
+        if (commands.Count == 0)
+        {
+            throw new ArgumentException("J519 命令队列至少需要包含一帧。", nameof(commands));
+        }
+
+        lock (_commandLock)
+        {
+            CompleteCommandQueueLocked();
+            _commandQueue = new Queue<FanucJ519Command>(commands);
+            // 队列耗尽后继续保持最后一帧目标，避免运动结束后回落到旧目标或空目标。
+            _currentCommand = commands[^1];
+            _hasPendingCommandForStart = true;
+            _commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
+            _commandHistoryForTests?.AddRange(commands);
+        }
+
+        _logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count);
+    }
+
+    /// <summary>
+    /// 等待当前预装命令队列被状态包全部取出；无队列时立即完成。
+    /// </summary>
+    /// <param name="cancellationToken">取消令牌。</param>
+    /// <returns>表示等待过程的任务。</returns>
+    internal Task WaitForCommandQueueDrainedAsync(CancellationToken cancellationToken = default)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, this);
+
+        Task waitTask;
+        lock (_commandLock)
+        {
+            waitTask = _commandQueueDrainedCompletion?.Task ?? Task.CompletedTask;
+        }
+
+        return waitTask.WaitAsync(cancellationToken);
+    }
+
+    /// <summary>
+    /// 判断当前是否没有等待出队的命令；仅供单元测试断言。
+    /// </summary>
+    /// <returns>如果队列为空或尚未装载队列，则返回 true。</returns>
+    internal bool IsCommandQueueDrainedForTests()
+    {
+        ObjectDisposedException.ThrowIf(_disposed, this);
+
+        lock (_commandLock)
+        {
+            return _commandQueue is null || _commandQueue.Count == 0;
+        }
+    }
+
    /// <summary>
    /// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。
    /// </summary>
@@ -230,7 +305,9 @@ public sealed class FanucJ519Client : IDisposable
        {
            _currentCommand = null;
            _lastSentCommand = null;
+            CompleteCommandQueueLocked();
            _commandHistoryForTests = null;
+            _hasPendingCommandForStart = false;
            _motionStarted = false;
        }

@@ -265,6 +342,12 @@ public sealed class FanucJ519Client : IDisposable
        _receiveTask?.Dispose();
        _cts?.Dispose();
        _udpClient?.Dispose();
+
+        lock (_commandLock)
+        {
+            CompleteCommandQueueLocked();
+            _hasPendingCommandForStart = false;
+        }
    }

    private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
@@ -369,12 +452,28 @@ public sealed class FanucJ519Client : IDisposable
        }

        FanucJ519Command? command;
+        var willDrainQueue = false;
        lock (_commandLock)
        {
-            command = !_motionStarted || _currentCommand is null
+            if (!_motionStarted)
+            {
+                command = null;
+            }
+            else if (_commandQueue is { Count: > 0 } queue)
+            {
+                // 状态包是唯一节拍源：每收到一帧可接收状态，才取出下一条目标。
+                var queuedCommand = queue.Dequeue();
+                _currentCommand = queuedCommand;
+                willDrainQueue = queue.Count == 0;
+                command = WithSequence(queuedCommand, response.Sequence);
+            }
+            else
+            {
+                command = _currentCommand is null
                    ? null
                    : WithSequence(_currentCommand, response.Sequence);
            }
+        }

        if (command is null)
        {
@@ -383,12 +482,32 @@ public sealed class FanucJ519Client : IDisposable

        var packet = FanucJ519Protocol.PackCommandPacket(command);
        await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
-
+        _logger?.LogDebug("J519 已回发命令包，seq={Seq}", command.Sequence);
+        _logger?.LogTrace(
+            "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
+            command.TargetJoints,
+            command.WriteIoMask,
+            command.WriteIoValue);
        lock (_commandLock)
        {
            _lastSentCommand = command;
+            if (willDrainQueue && _commandQueue is { Count: 0 })
+            {
+                CompleteCommandQueueLocked();
            }
        }
+    }
+
+    /// <summary>
+    /// 清空当前命令队列，并唤醒等待队列结束的运行时任务。
+    /// </summary>
+    private void CompleteCommandQueueLocked()
+    {
+        _commandQueue?.Clear();
+        _commandQueue = null;
+        _commandQueueDrainedCompletion?.TrySetResult();
+        _commandQueueDrainedCompletion = null;
+    }

    /// <summary>
    /// 读取最近一次已成功发送命令的目标关节轴文本，便于状态日志直接对照控制目标。
--- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
+++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
@@ -3,6 +3,9 @@ using Flyshot.ControllerClientCompat;
 using Flyshot.Core.Config;
 using Flyshot.Runtime.Fanuc;
 using Flyshot.Runtime.Fanuc.Protocol;
+using System.Buffers.Binary;
+using System.Net;
+using System.Net.Sockets;
 using System.Reflection;

 namespace Flyshot.Core.Tests;
@@ -17,7 +20,7 @@ public sealed class FanucControllerRuntimeDenseTests
    private const double SmoothPtpJerkShapeCoefficient = 52.5;

    /// <summary>
-    /// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
+    /// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列，并输出角度制目标。
    /// </summary>
    [Fact]
    public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
@@ -59,6 +62,65 @@ public sealed class FanucControllerRuntimeDenseTests
        Assert.Equal(5, commands.Count);
        Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
        Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
+        Assert.False(j519Client.IsCommandQueueDrainedForTests());
+    }
+
+    /// <summary>
+    /// 验证真机稠密轨迹接口会等待 J519 队列被状态包实际取完后再返回。
+    /// </summary>
+    [Fact]
+    public async Task ExecuteTrajectory_WithDenseWaypoints_RealMode_WaitsForJ519QueueToDrainBeforeReturning()
+    {
+        using var server = new UdpClient(0);
+        using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(5));
+        using var commandClient = new FanucCommandClient();
+        using var stateClient = new FanucStateClient();
+        using var j519Client = new FanucJ519Client();
+        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
+        var robot = TestRobotFactory.CreateRobotProfile();
+        var port = ((IPEndPoint)server.Client.LocalEndPoint!).Port;
+        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
+        j519Client.EnableCommandHistoryForTests();
+        await j519Client.ConnectAsync("127.0.0.1", port, cts.Token);
+        var initResult = await server.ReceiveAsync(cts.Token);
+        j519Client.StartMotion();
+        ForceRealModeEnabled(runtime, speedRatio: 1.0);
+
+        var denseTrajectory = new[]
+        {
+            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
+            new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
+            new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
+        };
+        var result = new TrajectoryResult(
+            programName: "wait-drain",
+            method: PlanningMethod.Icsp,
+            isValid: true,
+            duration: TimeSpan.FromSeconds(0.016),
+            shotEvents: Array.Empty<ShotEvent>(),
+            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
+            artifacts: Array.Empty<TrajectoryArtifact>(),
+            failureReason: null,
+            usedCache: false,
+            originalWaypointCount: 4,
+            plannedWaypointCount: 4,
+            denseJointTrajectory: denseTrajectory);
+
+        var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
+        await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
+
+        // 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
+        Assert.False(executeTask.IsCompleted);
+
+        for (uint sequence = 100; sequence < 103; sequence++)
+        {
+            await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token);
+            _ = await server.ReceiveAsync(cts.Token);
+        }
+
+        await executeTask.WaitAsync(TimeSpan.FromSeconds(2), cts.Token);
+        Assert.True(j519Client.IsCommandQueueDrainedForTests());
+        Assert.False(runtime.GetSnapshot().IsInMotion);
    }

    /// <summary>
@@ -108,27 +170,47 @@ public sealed class FanucControllerRuntimeDenseTests
            var commands = j519Client.GetCommandHistoryForTests();
            var runDir = GetSingleDenseSendRunDirectory(outputRoot);
            var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
+            var timingPath = Path.Combine(runDir, "ActualSendTiming.txt");
            var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");

            Assert.True(File.Exists(pointsPath));
+            Assert.True(File.Exists(timingPath));
            Assert.True(File.Exists(jerkPath));

            var pointLines = File.ReadAllLines(pointsPath);
+            var timingLines = File.ReadAllLines(timingPath);
            var jerkLines = File.ReadAllLines(jerkPath);
            Assert.Equal(commands.Count, pointLines.Length);
+            Assert.Equal(commands.Count, timingLines.Length);
            Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);

            var firstColumns = ParseColumns(pointLines[0]);
+            var secondColumns = ParseColumns(pointLines[1]);
            var lastColumns = ParseColumns(pointLines[^1]);
            Assert.Equal(9, firstColumns.Length);
            Assert.Equal(9, lastColumns.Length);
            Assert.Equal(0.0, firstColumns[0], precision: 6);
+            Assert.Equal(0.008, secondColumns[0], precision: 6);
            Assert.Equal(180.0, lastColumns[1], precision: 6);

+            var firstTimingColumns = ParseColumns(timingLines[0]);
+            var secondTimingColumns = ParseColumns(timingLines[1]);
+            var lastTimingColumns = ParseColumns(timingLines[^1]);
+            Assert.Equal(4, firstTimingColumns.Length);
+            Assert.Equal(0.0, firstTimingColumns[0], precision: 6);
+            Assert.Equal(0.0, firstTimingColumns[1], precision: 6);
+            Assert.Equal(0.0, firstTimingColumns[2], precision: 6);
+            Assert.Equal(0.5, firstTimingColumns[3], precision: 6);
+            Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
+            Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
+            Assert.Equal(0.004, secondTimingColumns[2], precision: 6);
+            Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
+            Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
+
            var firstJerkColumns = ParseColumns(jerkLines[0]);
            Assert.Equal(10, firstJerkColumns.Length);
            Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
-            Assert.Equal(0.004, firstJerkColumns[2], precision: 6);
+            Assert.Equal(0.008, firstJerkColumns[2], precision: 6);
        }
        finally
        {
@@ -797,6 +879,43 @@ public sealed class FanucControllerRuntimeDenseTests
        throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
    }

+    /// <summary>
+    /// 等待测试条件成立，用于观察后台执行路径是否已经到达指定状态。
+    /// </summary>
+    private static async Task WaitUntilAsync(Func<bool> condition, CancellationToken cancellationToken)
+    {
+        var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
+        while (DateTimeOffset.UtcNow < deadline)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            if (condition())
+            {
+                return;
+            }
+
+            await Task.Delay(1, cancellationToken);
+        }
+
+        throw new TimeoutException("Timed out waiting for test condition.");
+    }
+
+    /// <summary>
+    /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
+    /// </summary>
+    private static async Task SendStatusPacketAsync(
+        UdpClient server,
+        IPEndPoint clientEndpoint,
+        uint sequence,
+        CancellationToken cancellationToken)
+    {
+        var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
+        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
+        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
+        BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
+        responsePacket[0x0c] = 0b0111;
+        await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
+    }
+
    /// <summary>
    /// 获取一次测试执行生成的唯一稠密发送记录目录。
    /// </summary>
--- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
+++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
@@ -194,6 +194,46 @@ public sealed class FanucJ519ClientTests : IDisposable
        Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
    }

+    /// <summary>
+    /// 验证预装命令队列会被机器人状态包逐帧出队，队列耗尽后继续保持最后目标。
+    /// </summary>
+    [Fact]
+    public async Task StartMotion_DequeuesPreparedCommandsForStatusPacketsAndHoldsLastCommand()
+    {
+        using var client = new FanucJ519Client();
+        await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
+        var initResult = await _server.ReceiveAsync(_cts.Token);
+
+        client.StartMotion();
+        client.LoadCommandQueue(
+        [
+            new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+            new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+            new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        ]);
+
+        var packets = new List<byte[]>();
+        for (uint sequence = 700; sequence < 704; sequence++)
+        {
+            await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
+            var result = await _server.ReceiveAsync(_cts.Token);
+            packets.Add(result.Buffer);
+        }
+
+        await client.StopMotionAsync(_cts.Token);
+
+        var sequences = packets
+            .Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
+            .ToArray();
+        var firstJointTargets = packets
+            .Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)))
+            .ToArray();
+
+        Assert.Equal([700u, 701u, 702u, 703u], sequences);
+        Assert.Equal([1.0f, 2.0f, 3.0f, 3.0f], firstJointTargets);
+        Assert.True(client.IsCommandQueueDrainedForTests());
+    }
+
    /// <summary>
    /// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
    /// </summary>
@@ -310,7 +350,7 @@ public sealed class FanucJ519ClientTests : IDisposable
            logger.Entries,
            entry => entry.Level == LogLevel.Information
                && entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal)
-                && entry.Message.Contains("1.000, 2.000, 3.000, 4.000, 5.000, 6.000", StringComparison.Ordinal));
+                && entry.Message.Contains("1.00000, 2.00000, 3.00000, 4.00000, 5.00000, 6.00000", StringComparison.Ordinal));
    }

    /// <summary>