✨ feat(fanuc): 改为按状态包驱动 J519 队列发送
* 预生成稠密轨迹 J519 命令队列，等待机器人状态包逐帧出队 * 让 ExecuteTrajectory 在队列实际取完后返回，避免后台发送提前结束 * 新增 ActualSendTiming.txt，区分实发时间与 speed_ratio 采样时间 * 补充 J519 队列、等待完成和实发时间映射相关单元测试 * 同步文档中的 t_send / t_traj / speed_ratio 说明 Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
@@ -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` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||
|
||||
@@ -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`
|
||||
|
||||
@@ -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
|
||||
```
|
||||
|
||||
|
||||
@@ -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()` — 优先从真实通道读取
|
||||
|
||||
@@ -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 定义 |
|
||||
|
||||
@@ -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`
|
||||
|
||||
@@ -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
|
||||
```
|
||||
|
||||
从已生成轨迹中重采样。
|
||||
|
||||
Reference in New Issue
Block a user