FlyShotHost/docs/fanuc-field-runtime-workflow.md

FANUC Field Runtime Workflow

本文档记录当前现场主链路的 HTTP 调用顺序，以及每一步在 FANUC 三条真机通道上的动作。它替代旧 ControllerClient 工作流说明；旧 50001/TCP+JSON 入口不再作为运行目标。

1. 初始化

推荐使用聚合端点完成当前现场的一次性初始化：

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 周期发送器。

也可以使用拆分端点按同样顺序调用：

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. 参数设置

速度倍率：

POST /set_speedRatio/
{ "speed": 0.7 }

真机模式下会通过 TCP 10012 下发 0x2207 SetSpeedRatio，同时运行时保存当前倍率。J519 执行时仍必须按该倍率重采样轨迹时间轴：

t_traj = k * 0.008 * speed_ratio
send_count = floor(duration / (0.008 * speed_ratio)) + 1

TCP 和普通 IO：

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

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. 飞拍轨迹

上传：

POST /upload_flyshot/
{
  "name": "UTTC_MS11",
  "waypoints": [[...]],
  "shot_flags": [false, true],
  "offset_values": [0, 0],
  "addrs": [[1, 3]]
}

校验：

POST /is_flyShotTrajValid/
{
  "name": "UTTC_MS11",
  "method": "self-adapt-icsp",
  "save_traj": false
}

执行：

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. 停止与断开

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 与抓包一致。