✨ feat(*): 完善 FANUC J519 闭环、MoveJoint 与现场抓包验证
* 划分 J519 发送循环与稠密轨迹循环职责边界， FanucJ519Client 负责 UDP 周期发送， FanucControllerRuntime 按轨迹时间更新下一帧命令 * 执行时将规划输出 rad 转为 J519 deg 目标， 并按 speed_ratio 调整 8ms 发送时间尺度 * 补齐 accept_cmd/received_cmd/sysrdy/rbt_inmotion 状态位解析与启动前闭环检查 * MoveJoint 改为关节空间直线 + smoothstep 进度 的临时 PTP 稠密轨迹，按 status=15 运动窗口复现 * 新增 UTTC 2026-04-28 三份抓包 golden tests， 覆盖 0.5/0.7/1.0 speed_ratio 下的 J519 命令、 IO 脉冲与响应滞后 * 状态通道补充超时重连策略与退避逻辑 * TCP 10012 命令响应统一检查 result_code * 状态页扩展 J519 状态位与快照诊断信息 * 新增 docs/fanuc-field-runtime-workflow.md 现场工作流 * 补充 LR Mate 200iD 模型、RobotConfig.json 与 workpiece
This commit is contained in:
@@ -133,8 +133,9 @@
|
||||
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
|
||||
- HTTP 控制器已经按公开文档补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
|
||||
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试。
|
||||
- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态；真实 `10010 / 10012 / 60015` Socket 通讯与现场联调尚未落地。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在启动前检查已有 J519 响应中的 `accept_cmd/sysrdy` 状态。真实 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 运行包。
|
||||
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
||||
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||
|
||||
@@ -515,6 +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`。
|
||||
- 飞拍轨迹相关额外字符串线索：
|
||||
- `StartUploadFlyShotTraj`
|
||||
- `EndUploadFlyShotTraj`
|
||||
|
||||
169
docs/fanuc-field-runtime-workflow.md
Normal file
169
docs/fanuc-field-runtime-workflow.md
Normal file
@@ -0,0 +1,169 @@
|
||||
# FANUC Field Runtime Workflow
|
||||
|
||||
本文档记录当前现场主链路的 HTTP 调用顺序，以及每一步在 FANUC 三条真机通道上的动作。它替代旧 `ControllerClient` 工作流说明；旧 `50001/TCP+JSON` 入口不再作为运行目标。
|
||||
|
||||
## 1. 初始化
|
||||
|
||||
推荐使用聚合端点完成当前现场的一次性初始化：
|
||||
|
||||
```bash
|
||||
POST /init_mpc_robt
|
||||
{
|
||||
"server_ip": "127.0.0.1",
|
||||
"port": 50001,
|
||||
"robot_name": "FANUC_LR_Mate_200iD",
|
||||
"robot_ip": "192.168.10.11",
|
||||
"sim": false
|
||||
}
|
||||
```
|
||||
|
||||
该端点内部顺序：
|
||||
|
||||
1. `ConnectServer(server_ip, port)`：兼容旧参数形状，仅记录服务连接语义。
|
||||
2. `SetUpRobot(robot_name)`：加载机器人配置、关节限制和伺服周期。
|
||||
3. `SetActiveController(sim)`：选择仿真或 FANUC 真机运行时。
|
||||
4. `Connect(robot_ip)`：真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
|
||||
5. `EnableRobot(2)`：真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`，随后启动 J519 8ms 周期发送器。
|
||||
|
||||
也可以使用拆分端点按同样顺序调用：
|
||||
|
||||
```text
|
||||
POST /connect_server/?server_ip=127.0.0.1&port=50001
|
||||
POST /setup_robot/?robot_name=FANUC_LR_Mate_200iD
|
||||
POST /set_active_controller/?sim=false
|
||||
POST /connect_robot/?ip=192.168.10.11
|
||||
GET /enable_robot/?buffer_size=2
|
||||
```
|
||||
|
||||
## 2. 参数设置
|
||||
|
||||
速度倍率：
|
||||
|
||||
```bash
|
||||
POST /set_speedRatio/
|
||||
{ "speed": 0.7 }
|
||||
```
|
||||
|
||||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。J519 执行时仍必须按该倍率重采样轨迹时间轴：
|
||||
|
||||
```text
|
||||
t_traj = k * 0.008 * speed_ratio
|
||||
send_count = floor(duration / (0.008 * speed_ratio)) + 1
|
||||
```
|
||||
|
||||
TCP 和普通 IO：
|
||||
|
||||
```text
|
||||
POST /set_tcp/ body: { "x": 0, "y": 0, "z": 0 }
|
||||
GET /get_tcp/
|
||||
POST /set_io/?port=7&value=true&io_type=DO
|
||||
GET /get_io/?port=7&io_type=DO
|
||||
```
|
||||
|
||||
飞拍触发 IO 不走独立 `TCP 10012 SetIO`，而是嵌入 `UDP 60015` J519 命令包的 `write_io_type/index/mask/value` 字段。
|
||||
|
||||
## 3. 点到点 MoveJoint
|
||||
|
||||
```bash
|
||||
POST /move_joint/
|
||||
{ "joints": [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145] }
|
||||
```
|
||||
|
||||
`MoveJoint` 不再直接把最终关节写成单个 J519 目标，而是按现场抓包确认的 PTP 临时轨迹执行：
|
||||
|
||||
1. 从当前运行时状态读取当前关节坐标，单位为 `rad`。
|
||||
2. 以当前关节和目标关节构造关节空间直线。
|
||||
3. 用五次 smoothstep `10u^3 - 15u^4 + 6u^5` 生成起停平滑的进度。
|
||||
4. 真机执行时仍由 J519 层把 `rad` 转成 `deg`，并按当前 `speed_ratio` 重采样。
|
||||
|
||||
已确认抓包按响应 `status=15` 运动窗口统计：
|
||||
|
||||
| 抓包 | speed_ratio | 运动窗口点数 | 运动窗口时长 |
|
||||
|------|-------------|----------------------|----------|
|
||||
| `2026042802-mvpoint.pcap` | 1.0 | 40 | 约 0.312s |
|
||||
| `2026042802-mvpoint0.7.pcap` | 0.7 | 55 | 约 0.432s |
|
||||
| `2026042802-mvpoint0.5.pcap` | 0.5 | 77 | 约 0.608s |
|
||||
|
||||
抓包命令流在运动窗口前后还会持续发送保持不变的起点/终点目标；功能复刻以 `status=15` 运动窗口为点数口径，并把最后一个采样点压到目标关节。实际目标几乎严格位于“起点 -> 终点”的同一条关节空间直线上，`speed_ratio` 体现为 J519 发送时间轴上的减速重采样，而不是改变路径形状。
|
||||
|
||||
## 4. 飞拍轨迹
|
||||
|
||||
上传：
|
||||
|
||||
```bash
|
||||
POST /upload_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"waypoints": [[...]],
|
||||
"shot_flags": [false, true],
|
||||
"offset_values": [0, 0],
|
||||
"addrs": [[1, 3]]
|
||||
}
|
||||
```
|
||||
|
||||
校验：
|
||||
|
||||
```bash
|
||||
POST /is_flyShotTrajValid/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false
|
||||
}
|
||||
```
|
||||
|
||||
执行：
|
||||
|
||||
```bash
|
||||
POST /execute_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"move_to_start": true,
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false,
|
||||
"use_cache": true
|
||||
}
|
||||
```
|
||||
|
||||
执行链路：
|
||||
|
||||
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
||||
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹。
|
||||
3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。
|
||||
4. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 周期重采样并转成 `deg`。
|
||||
5. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪，则拒绝执行。
|
||||
6. 周期命令中嵌入 IO 脉冲；当前 UTTC 抓包确认 mask 集合为 `10/12/14`，共 17 个 set 脉冲和 17 个 clear 帧。
|
||||
|
||||
`method="doubles"` 当前明确返回未实现；现场主链路使用 `icsp` / `self-adapt-icsp`。
|
||||
|
||||
## 5. 停止与断开
|
||||
|
||||
```text
|
||||
GET /stop_move/
|
||||
GET /disable_robot/
|
||||
POST /disconnect_robot/
|
||||
```
|
||||
|
||||
真机模式下：
|
||||
|
||||
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 发送循环。
|
||||
- `DisableRobot()` 发送 J519 end 控制包，然后 `StopProg("RVBUSTSM")`。
|
||||
- `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。
|
||||
|
||||
## 6. 现场抓包覆盖
|
||||
|
||||
`tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs` 直接解析以下抓包并与 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 对比：
|
||||
|
||||
| 抓包 | 速度 | 运行 J519 点数 | 发送时长 |
|
||||
|------|------|----------------|----------|
|
||||
| `2026042802-0.5.pcap` | 0.5 | 1851 | 14.800309s |
|
||||
| `2026042802-0.7.pcap` | 0.7 | 1322 | 10.568313s |
|
||||
| `2026042802-1.pcap` | 1.0 | 926 | 7.400125s |
|
||||
|
||||
测试同时检查：
|
||||
|
||||
- 主运行窗口命令序号连续，无重复 seq。
|
||||
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
|
||||
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。
|
||||
- `lastData=0`，结束运动依赖 J519 end 控制包。
|
||||
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
|
||||
@@ -2,14 +2,23 @@
|
||||
|
||||
## 上下文
|
||||
|
||||
当前 `flyshot-replacement` 项目已完成：
|
||||
状态更新：本计划中的 Socket 客户端和 `FanucControllerRuntime` 改造已经落地；当前事实以 `README.md` 和 `docs/fanuc-field-runtime-workflow.md` 为准。本文保留为实现过程记录。
|
||||
|
||||
计划制定时 `flyshot-replacement` 项目已完成：
|
||||
- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`）
|
||||
- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过）
|
||||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||||
- HTTP 兼容层控制器和状态监控页
|
||||
- 轨迹规划与飞拍触发编排层
|
||||
|
||||
**缺失的关键环节**：`FanucControllerRuntime` 仍是纯内存状态桩，没有实际 Socket 通信。`Connect()` 只记录 IP，`ExecuteTrajectory()` 只修改内存变量，`GetJointPositions()` 返回的是上一次写入值而非真实控制器反馈。
|
||||
2026-04-28 `Rvbust/uttc-20260428/20260428.pcap` 新增约束：
|
||||
|
||||
- `TCP 10010` 状态帧继续确认为固定 `90B`。
|
||||
- `UDP 60015` 命令 `target[0..5]` 为关节角 `deg`，而 `JointDetialTraj.txt` 为 `rad`。
|
||||
- `speed_ratio=0.7` 在本抓包中表现为 UDP 下发时间轴约 `1.427730x` 拉伸；机器人侧 `TCP 10012` 未抓到 `0x2207 SetSpeedRatio`。
|
||||
- `UTTC_MS11` 的 17 个飞拍触发点与 17 个 UDP IO 脉冲一一对齐，`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
**历史缺失项（已完成）**：计划制定时 `FanucControllerRuntime` 仍是纯内存状态桩。当前实现已经改为持有 `FanucCommandClient`、`FanucStateClient` 和 `FanucJ519Client`，真机模式会建立三条通道并从状态/J519 响应读取运行状态。
|
||||
|
||||
## 目标
|
||||
|
||||
@@ -66,7 +75,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- `GetProgramStatusAsync(string name)` → `PackProgramCommand(0x2003, name)`
|
||||
- `StartProgramAsync(string name)` → `PackProgramCommand(0x2102, name)`
|
||||
- `GetTcpAsync()` / `SetTcpAsync()` — 已按 `tcp_id + f32[7] pose` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现；注意 2026-04-28 真实运行抓包未出现机器人侧 `0x2207`，执行链路仍必须在 UDP 发送时间尺度上应用当前速度倍率
|
||||
- `GetIoAsync()` / `SetIoAsync()` — 已按 `io_type / io_index / f32 io_value` 字段布局实现
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucCommandClientTests.cs`
|
||||
@@ -91,6 +100,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析。
|
||||
- 用本地模拟控制器验证无状态帧超时、EOF 后退避重连和重连后的继续收帧。
|
||||
- `FanucStateProtocol` 已用 `j519 协议.pcap` 中多条 90B 样本锁定 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]`。
|
||||
- `Rvbust/uttc-20260428/20260428.pcap` 再次确认 `10010` 状态帧固定 90B，平均间隔约 25.6ms。
|
||||
- 尾部状态字当前只作为 `ControllerStateSnapshot.stateTailWords` 诊断字段保留，不从 `[2,0,0,1]` 推断使能或运动状态。
|
||||
|
||||
### Phase 3: UDP 60015 J519 运动客户端
|
||||
@@ -107,6 +117,14 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- 接收线程：持续 `ReceiveAsync()` 解析 132B 响应，更新反馈状态
|
||||
- `Disconnect()` — 清理
|
||||
|
||||
执行注意事项：
|
||||
|
||||
- 规划层输出关节角为 `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` 行。
|
||||
- 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
|
||||
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs`
|
||||
- 用本地 UDP socket 模拟控制器收发
|
||||
|
||||
@@ -121,7 +139,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
|
||||
- `DisableRobot()` — 停止 J519，发送 StopProg
|
||||
- `Disconnect()` — 断开三条通道
|
||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点通过 J519 逐帧发送
|
||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 J519 逐周期发送
|
||||
- `StopMove()` — 立即停止 J519 发送循环
|
||||
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
|
||||
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
||||
@@ -164,6 +182,6 @@ dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTest
|
||||
|
||||
- `FanucControllerRuntime` 的 `Connect()` 能成功建立三条 TCP/UDP 连接
|
||||
- `EnableRobot()` 能走完 `RVBUSTSM` 启动序列
|
||||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点
|
||||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点，并按当前 `speed_ratio` 推进原始轨迹时间
|
||||
- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存
|
||||
- 现有 10 个集成测试和 25 个核心测试仍然通过
|
||||
|
||||
Reference in New Issue
Block a user