yunxiao.zhu
af65ca03a0
- 为 ExecuteFlyShotTraj 补齐 wait 语义，并让 move_to_start 先完成临时 PTP 运动后再启动正式飞拍轨迹 - 将 J519 命令发送改为由机器人 UDP status sequence 驱动， 避免在未收到状态包时主动发周期命令 - 将 10010 状态通道关节字段统一按 JointRadians 命名， 同步更新运行时读取逻辑与协议测试 - 新增 FANUC 10010 状态帧、流运动手册和 Python client 逆向文档，并更新 README 与兼容需求说明 - 补充兼容层编排测试与 HTTP 集成测试，覆盖 wait 和 move_to_start 串行化行为
182 lines
7.5 KiB
Markdown
182 lines
7.5 KiB
Markdown
# 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 在收到机器人 UDP status 包后回发下一帧命令。
|
||
|
||
也可以使用拆分端点按同样顺序调用：
|
||
|
||
```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. 参数设置
|
||
|
||
规划约束参数：
|
||
|
||
当前现场抓包已经确认，`50001/TCP+JSON` 的 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求不会显式携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此新系统把规划约束分成两层处理：
|
||
|
||
1. 旧 `RobotConfig.json` 中已有的 `acc_limit / jerk_limit` 继续作为模型加载时的基础倍率。
|
||
2. 若旧系统导出的 `JointTraj.txt` 明显比当前 C# 规划更慢，使用 replacement-only 的内部校准参数限制规划阶段加速度，设计字段为 `planning_acceleration_scale`。
|
||
|
||
`planning_acceleration_scale` 只影响 `JointTraj.txt` 这类规划结果时间轴，不下发到 FANUC 控制柜，也不改变 J519 发送周期。若需要临时整体验证，也可以使用当前已有的 `planning_speed_scale`，但它是新系统兼容开关，不是旧抓包中出现的字段。
|
||
|
||
速度倍率：
|
||
|
||
```bash
|
||
POST /set_speedRatio/
|
||
{ "speed": 0.7 }
|
||
```
|
||
|
||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。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,
|
||
"wait": true
|
||
}
|
||
```
|
||
|
||
执行链路：
|
||
|
||
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
||
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹；规划阶段先应用 `acc_limit / jerk_limit`，再应用 replacement-only 的规划加速度校准参数。
|
||
3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。
|
||
4. 若 `move_to_start=true`，先从运行时当前关节位置生成临时 PTP 稠密轨迹移动到规划轨迹起点，并等待运行时 `IsInMotion=false` 后再启动飞拍轨迹，避免第一帧 J519 目标从当前位置跳到起点。
|
||
5. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 轨迹时间步长重采样并转成 `deg`，命令实际发包由机器人 UDP status 包驱动。
|
||
6. 若 `wait=true`，正式飞拍轨迹启动后继续等待运行时 `IsInMotion=false`，机器人执行完整条飞拍轨迹后 HTTP 才返回；`wait=false` 时启动后立即返回。
|
||
7. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪，则拒绝执行。
|
||
8. 周期命令中嵌入 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 packet type 2 状态输出停止包，然后 `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；J519 客户端单元测试覆盖按最新 status sequence 回发命令。
|
||
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
|
||
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。
|
||
- `lastData=0`，结束运动当前依赖 J519 packet type 2 状态输出停止包；`../j519 协议.pcap` 中另有 1 个 `LastData=1` 后紧跟 type 2 的样本，停止语义后续单独验证。
|
||
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
|