From af65ca03a01e99a36ea55dab7464034771f2d8d4 Mon Sep 17 00:00:00 2001 From: "yunxiao.zhu" Date: Sun, 3 May 2026 19:29:31 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat(compat):=20=E8=A1=A5=E9=BD=90?= =?UTF-8?q?=E9=A3=9E=E6=8B=8D=E6=89=A7=E8=A1=8C=E7=AD=89=E5=BE=85=E4=B8=8E?= =?UTF-8?q?=20FANUC=20=E7=8A=B6=E6=80=81=E9=A9=B1=E5=8A=A8=E9=93=BE?= =?UTF-8?q?=E8=B7=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 为 ExecuteFlyShotTraj 补齐 wait 语义，并让 move_to_start 先完成临时 PTP 运动后再启动正式飞拍轨迹 - 将 J519 命令发送改为由机器人 UDP status sequence 驱动， 避免在未收到状态包时主动发周期命令 - 将 10010 状态通道关节字段统一按 JointRadians 命名， 同步更新运行时读取逻辑与协议测试 - 新增 FANUC 10010 状态帧、流运动手册和 Python client 逆向文档，并更新 README 与兼容需求说明 - 补充兼容层编排测试与 HTTP 集成测试，覆盖 wait 和 move_to_start 串行化行为 --- README.md | 34 +- ...r-client-api-compatibility-requirements.md | 6 +- docs/fanuc-10010-state-frame.md | 165 +++++ docs/fanuc-field-runtime-workflow.md | 34 +- docs/fanuc-stream-motion-manual-notes.md | 232 +++++++ ...lanning-duration-mismatch-investigation.md | 591 ++++++++++++++++++ ...on-client-interface-reverse-engineering.md | 202 ++++++ .../ControllerClientCompatService.cs | 106 ++-- .../FlyshotExecutionOptions.cs | 10 +- .../FanucControllerRuntime.cs | 8 +- .../Protocol/FanucJ519Client.cs | 161 ++--- .../Protocol/FanucStateProtocol.cs | 8 +- .../Controllers/LegacyHttpApiController.cs | 12 +- .../FanucJ519ClientTests.cs | 85 +-- .../Flyshot.Core.Tests/FanucProtocolTests.cs | 4 +- .../RuntimeOrchestrationTests.cs | 247 ++++++++ .../LegacyHttpApiCompatibilityTests.cs | 3 +- 17 files changed, 1694 insertions(+), 214 deletions(-) create mode 100644 docs/fanuc-10010-state-frame.md create mode 100644 docs/fanuc-stream-motion-manual-notes.md create mode 100644 docs/planning-duration-mismatch-investigation.md create mode 100644 docs/python-client-interface-reverse-engineering.md diff --git a/README.md b/README.md index 1019a1d..286274c 100644 --- a/README.md +++ b/README.md @@ -19,10 +19,34 @@ - `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。 - `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。 - 2026-04-28 UTTC 抓包确认：UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`，`JointDetialTraj.txt` 为弧度制 `rad`，`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长，当前实际生成约 `7.4s` 轨迹。 -- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送能力；稠密轨迹下发已按 `speed_ratio` 做执行时间缩放，并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。 +- 2026-04-30 本机 `50001/TCP+JSON` 抓包确认：`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数，不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路，新系统按 replacement-only 内部参数限制规划加速度。 +- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送能力；稠密轨迹下发已按 `speed_ratio` 做执行时间缩放，并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。 - `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹：当前关节到目标关节的关节空间直线，五次 smoothstep 起停，按 `status=15` 运动窗口复现 `40/55/77` 点，并由 J519 层完成 `rad -> deg` 下发。 - 单程序只对应一台机器人，上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`，不再创建独立轨迹存储文件。 +单位约定总览： + +- 规划层、`JointDetialTraj.txt` 和运行时内部关节轨迹，默认按弧度制 `rad` 理解。 +- `UDP 60015` J519 命令 `target[0..5]` 和响应关节反馈按角度制 `deg` 理解；运行时下发前必须显式执行 `rad -> deg` 转换。 +- `TCP 10010` 状态通道是混合单位：`pose[0..2]` 更像 `mm`，`pose[3..5]` 更像 `deg`，`joint_or_ext[0..5]` 当前现场抓包更支持按 `rad` 理解。 +- 不要把“关节角”默认当成统一单位；在规划、状态监控和 J519 执行三条链路之间必须明确标注 `rad/deg`。 + +当前现场主链路的单位流转可简化为： + +| 位置 | 内容 | 当前更可信单位 | +| --- | --- | --- | +| 规划输入 / 轨迹算法 | 关节角 | `rad` | +| `JointDetialTraj.txt` / `JointTraj.txt` | 关节角 | `rad` | +| 运行时下发前内部轨迹 | 关节角 | `rad` | +| `UDP 60015` 命令 `target[0..5]` | 关节目标 | `deg` | +| `UDP 60015` 响应 `Joint` | 关节反馈 | `deg` | +| `TCP 10010` `pose[0..2]` | `X/Y/Z` | `mm` | +| `TCP 10010` `pose[3..5]` | 姿态角 | `deg` | +| `TCP 10010` `joint_or_ext[0..5]` | 关节状态 | 更像 `rad` | +| `TCP 10010` `joint_or_ext[6..8]` | 扩展轴槽位 | 当前样本为 `0` | + +`TCP 10010` 的正式字段表、样例帧和已确认/待确认说明见 `docs/fanuc-10010-state-frame.md`。 + 开发约定： - 建议从 `flyshot-replacement/` 根目录启动 IDE、终端和 Codex 会话。 @@ -58,6 +82,7 @@ 2. 轨迹规划 - [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。 - [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。 + - [ ] 新增 replacement-only 的 `planning_acceleration_scale` 规划加速度校准参数，用于复现旧服务端公开链路中抓不到的保守 effective limits；该参数只影响规划结果，不影响运行时 `speed_ratio`。 - [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。 - [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。 - [x] 将 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本：输入为 `rad`，下发为 `deg`，并按 `speed_ratio` 拉伸时间轴；覆盖 `2026042802-0.5/0.7/1.pcap`。 @@ -72,16 +97,19 @@ 4. FANUC TCP 10010 状态通道 - [x] 用 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。 - [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。 + - [x] 补充 `TCP 10010` 正式字段表与已确认/待确认说明：见 `docs/fanuc-10010-state-frame.md`。 - [x] 补充断线清理和异常帧拒绝测试。 - [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。 5. FANUC UDP 60015 J519 运动链路 - - [x] 重新确认 J519 发送循环与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 只负责 UDP 周期发送，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令。 + - [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] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位解析与启动前闭环检查；若已有 J519 响应且 `accept_cmd/sysrdy` 未就绪，则拒绝稠密轨迹执行。 - - [x] 校验序号递增、响应滞后、丢包、停止包和最后一帧语义：UTTC golden tests 覆盖连续 seq、无重复 seq、响应滞后 2 到 8 帧、`lastData=0`；停止包由 J519 客户端测试覆盖。 + - [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`。 - [x] 将 `MoveJoint` 从单点最终目标改为临时 PTP 稠密轨迹：按 `status=15` 运动窗口统计，speed=1 抓包 40 点，speed=0.7 抓包 55 点，speed=0.5 抓包 77 点，路径为关节空间直线 + smoothstep 进度。 + - [x] `ExecuteFlyShotTraj(move_to_start=true)` 复用临时 PTP 稠密轨迹移动到规划起点，并等待运行时完成后再启动飞拍轨迹，避免第一帧 J519 目标突变导致控制柜报警。 + - [x] `ExecuteFlyShotTraj(wait=true)` 等待正式飞拍轨迹执行完成后再返回；HTTP `/execute_flyshot/` 已接入旧抓包中的 `wait` 字段，默认值为 `true`。 6. 真机联调与运行安全 - [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。 diff --git a/docs/controller-client-api-compatibility-requirements.md b/docs/controller-client-api-compatibility-requirements.md index 792ca5b..74a6ad8 100644 --- a/docs/controller-client-api-compatibility-requirements.md +++ b/docs/controller-client-api-compatibility-requirements.md @@ -123,6 +123,7 @@ - 先做兼容测试矩阵，再补最小命令桩 - 区分“旧系统事实”和“replacement 当前策略” - 真机未接通前，允许实现层返回稳定错误或模拟状态，但不能反过来污染逆向文档 +- `50001/TCP+JSON` 抓包已经覆盖 `SetSpeedRatio` 与 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)`，请求中没有显式 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk` 字段；因此规划限制的补齐必须作为 replacement-only 策略记录，不能写成旧公开 API 合同。 ## 8. 当前 replacement 实现状态 @@ -131,10 +132,11 @@ - `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。 - `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。 - `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。 -- HTTP 控制器已经按公开文档补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。 +- HTTP 控制器已经按公开文档和抓包补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache, wait)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。 +- 规划阶段会继续消费旧配置中的 `acc_limit / jerk_limit`。如果现场需要复现旧服务端不可见的保守约束，replacement 设计上使用内部 `planning_acceleration_scale` 限制规划加速度；该字段不属于旧 `ControllerClient` 公开 API，也不会通过 `50001` JSON 下发。 - `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` 推进轨迹时间，并在启动前检查已有 J519 响应中的 `accept_cmd/sysrdy` 状态。真实 R30iB 全流程现场联调仍需执行。 +- `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 运行包。 - 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。 - `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。 diff --git a/docs/fanuc-10010-state-frame.md b/docs/fanuc-10010-state-frame.md new file mode 100644 index 0000000..476bb02 --- /dev/null +++ b/docs/fanuc-10010-state-frame.md @@ -0,0 +1,165 @@ +# FANUC TCP 10010 状态帧字段说明 + +本文档整理当前现场真实抓包中 `TCP 10010` 状态通道的已确认字段布局，并明确哪些结论已经由抓包和代码验证，哪些仍然只是工作假设。 + +读取时间：2026-05-03 + +## 1. 结论范围 + +本文基于以下证据整理： + +- `../analysis/UTTC_20260428_packet_validation.md` +- `../analysis/J519_stream_motion_analysis.md` +- `Rvbust/uttc-20260428/20260428.pcap` +- `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs` +- `tests/Flyshot.Core.Tests/FanucProtocolTests.cs` + +当前结论仅覆盖现场已确认的 `R30iB + RVBUSTSM` 这一路状态通道行为，不提前推广为所有 FANUC 机型或所有旧版本协议的通用结论。 + +## 2. 通道性质 + +真实抓包显示，`TCP 10010` 是控制柜到上位机的单向状态流： + +- 上位机先主动建立 TCP 连接。 +- 建连后，带应用层 payload 的业务包全部来自 `192.168.10.11:10010 -> 192.168.10.10:41726`。 +- 上位机在该通道上只回 TCP `ACK`，没有观察到应用层请求体。 + +因此当前实现应把 `10010` 当作“持续推送的固定长度状态帧”处理，而不是像 `TCP 10012` 那样按请求/响应语义建模。 + +## 3. 整体布局 + +当前现场抓包确认，状态帧固定为 `90B`： + +```text +doz 3 bytes +length u32 = 90 +msg_id u32 +pose[6] f32 +joint_or_ext[9] f32 +tail[4] u32 +zod 3 bytes +``` + +其中： + +- 帧头 magic 固定为 `doz` +- 帧尾 magic 固定为 `zod` +- 长度字段固定为 `0x5a`，即 `90` +- 当前全量抓包中 `msg_id` 恒为 `0` +- `tail[4]` 当前全量抓包中恒为 `(2, 0, 0, 1)` + +## 4. 样例帧 + +以下样例帧来自 `20260428.pcap` 中首条 `tcp.port == 10010 && tcp.len > 0` 的 payload： + +```text +646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64 +``` + +对应解析值： + +- `pose[6]` + - `273.26715` + - `483.85544` + - `467.73764` + - `-57.253044` + - `-89.618675` + - `-62.21883` +- `joint_or_ext[9]` + - `1.0567309` + - `0.011662138` + - `-0.01789339` + - `-0.015160045` + - `0.02149596` + - `0.009560025` + - `0` + - `0` + - `0` +- `tail[4]` + - `2` + - `0` + - `0` + - `1` + +## 5. 正式字段表 + +| 偏移 | 长度 | 类型 | 样例值(hex) | 样例值(解析后) | 当前推断含义 | +| --- | --- | --- | --- | --- | --- | +| `0x00` | `3` | `char[3]` | `64 6f 7a` | `"doz"` | 固定帧头 magic | +| `0x03` | `4` | `u32 be` | `00 00 00 5a` | `90` | 帧总长度 | +| `0x07` | `4` | `u32 be` | `00 00 00 00` | `0` | `msg_id`，当前抓包全为 `0` | +| `0x0B` | `4` | `f32 be` | `43 88 a2 32` | `273.26715` | `pose[0]`，推断为 TCP `X(mm)` | +| `0x0F` | `4` | `f32 be` | `43 f1 ed 7f` | `483.85544` | `pose[1]`，推断为 TCP `Y(mm)` | +| `0x13` | `4` | `f32 be` | `43 e9 de 6b` | `467.73764` | `pose[2]`，推断为 TCP `Z(mm)` | +| `0x17` | `4` | `f32 be` | `c2 65 03 1e` | `-57.253044` | `pose[3]`，推断为姿态角 `W(deg)` | +| `0x1B` | `4` | `f32 be` | `c2 b3 3c c3` | `-89.618675` | `pose[4]`，推断为姿态角 `P(deg)` | +| `0x1F` | `4` | `f32 be` | `c2 78 e0 15` | `-62.21883` | `pose[5]`，推断为姿态角 `R(deg)` | +| `0x23` | `4` | `f32 be` | `3f 87 42 f5` | `1.0567309` | `joint_or_ext[0]`，推断为 `J1(rad)` | +| `0x27` | `4` | `f32 be` | `3c 3f 12 8d` | `0.011662138` | `joint_or_ext[1]`，推断为 `J2(rad)` | +| `0x2B` | `4` | `f32 be` | `bc 92 95 29` | `-0.01789339` | `joint_or_ext[2]`，推断为 `J3(rad)` | +| `0x2F` | `4` | `f32 be` | `bc 78 61 d6` | `-0.015160045` | `joint_or_ext[3]`，推断为 `J4(rad)` | +| `0x33` | `4` | `f32 be` | `3c b0 18 4c` | `0.02149596` | `joint_or_ext[4]`，推断为 `J5(rad)` | +| `0x37` | `4` | `f32 be` | `3c 1c a1 a7` | `0.009560025` | `joint_or_ext[5]`，推断为 `J6(rad)` | +| `0x3B` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[6]`，扩展轴槽位，当前样本恒 `0` | +| `0x3F` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[7]`，扩展轴槽位，当前样本恒 `0` | +| `0x43` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[8]`，扩展轴槽位，当前样本恒 `0` | +| `0x47` | `4` | `u32 be` | `00 00 00 02` | `2` | `tail[0]`，诊断状态字，物理语义未坐实 | +| `0x4B` | `4` | `u32 be` | `00 00 00 00` | `0` | `tail[1]`，诊断状态字，物理语义未坐实 | +| `0x4F` | `4` | `u32 be` | `00 00 00 00` | `0` | `tail[2]`，诊断状态字，物理语义未坐实 | +| `0x53` | `4` | `u32 be` | `00 00 00 01` | `1` | `tail[3]`，诊断状态字，物理语义未坐实 | +| `0x57` | `3` | `char[3]` | `7a 6f 64` | `"zod"` | 固定帧尾 magic | + +## 6. 已确认结论 + +### 6.1 已由真实抓包确认 + +1. `TCP 10010` 是独立状态流，不是 `TCP 10012` 的请求/响应复用。 +2. 当前现场状态帧固定为 `90B`，不是早期静态分析里出现过的 `134B`。 +3. `msg_id` 在 `20260428.pcap` 当前全量样本中恒为 `0`。 +4. `tail[4]` 在 `20260428.pcap` 当前全量样本中恒为 `(2, 0, 0, 1)`。 +5. `pose[6]` 的量纲表现符合 `X/Y/Z(mm) + W/P/R(deg)`。 +6. `joint_or_ext[6..8]` 在当前现场样本中恒为 `0`。 + +### 6.2 已由数值范围和交叉对照强支持 + +1. `joint_or_ext[0..5]` 更像关节角 `rad`，而不是 `deg`。 +2. 该判断与 `../analysis/UTTC_20260428_packet_validation.md` 的结论一致。 +3. 该判断也与 `UDP 60015` 响应包中的关节 `deg` 形成互补关系：二者不能简单视作同单位直接复用。 + +## 7. 待确认项 + +以下内容当前不要写死为最终协议真义： + +1. `tail[4]` 四个 `u32` 分别代表什么控制器语义。 +2. `msg_id` 是否在其他控制柜版本、程序状态或异常态下会出现非零值。 +3. `pose[3..5]` 是否可以严格命名为 FANUC 标准 `W/P/R`，还是只是与其数值表现一致。 +4. `joint_or_ext[6..8]` 在带外部轴的现场是否仍复用同一布局。 + +## 8. 与当前代码实现的对齐情况 + +当前仓库里 `Flyshot.Runtime.Fanuc` 已按 `90B` 固定帧解析： + +- `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs` +- `tests/Flyshot.Core.Tests/FanucProtocolTests.cs` + +当前实现已经与抓包对齐的部分： + +1. 固定长度 `90B` +2. `doz ... zod` 帧头帧尾校验 +3. `pose[6] + joint_or_ext[9] + tail[4]` 的字节布局 +4. `tail[4]` 原样保留到 `ControllerStateSnapshot.StateTailWords` + +当前仍建议后续关注的点： + +1. `FanucStateFrame` 已把该字段从 `JointDegrees` 更正为 `JointRadians`，后续新增代码应继续沿用弧度制命名。 +2. 如果后续状态页或运行时逻辑需要直接展示该通道关节值，仍需明确标注这是 `10010` 的弧度值，避免和 `UDP 60015` 的 degree 语义混淆。 + +## 9. 建议用法 + +在当前 replacement 实现里，`TCP 10010` 更适合作为以下用途： + +1. 提供机器人当前笛卡尔位姿和关节反馈快照。 +2. 提供状态通道是否健康、是否陈旧的连接诊断依据。 +3. 保留 `tail[4]` 原始状态字，供现场排错或后续继续逆向。 + +当前不建议直接用 `tail[4]` 去驱动明确业务判断，除非后续拿到新的现场对照证据。 diff --git a/docs/fanuc-field-runtime-workflow.md b/docs/fanuc-field-runtime-workflow.md index 5ca780d..908b48b 100644 --- a/docs/fanuc-field-runtime-workflow.md +++ b/docs/fanuc-field-runtime-workflow.md @@ -23,7 +23,7 @@ POST /init_mpc_robt 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 周期发送器。 +5. `EnableRobot(2)`：真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`，随后允许 J519 在收到机器人 UDP status 包后回发下一帧命令。 也可以使用拆分端点按同样顺序调用： @@ -37,6 +37,15 @@ 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 @@ -44,7 +53,7 @@ POST /set_speedRatio/ { "speed": 0.7 } ``` -真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。J519 执行时仍必须按该倍率重采样轨迹时间轴： +真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍必须按该倍率重采样轨迹时间轴： ```text t_traj = k * 0.008 * speed_ratio @@ -121,18 +130,21 @@ POST /execute_flyshot/ "move_to_start": true, "method": "self-adapt-icsp", "save_traj": false, - "use_cache": true + "use_cache": true, + "wait": true } ``` 执行链路： 1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。 -2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹。 +2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹；规划阶段先应用 `acc_limit / jerk_limit`，再应用 replacement-only 的规划加速度校准参数。 3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。 -4. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 周期重采样并转成 `deg`。 -5. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪，则拒绝执行。 -6. 周期命令中嵌入 IO 脉冲；当前 UTTC 抓包确认 mask 集合为 `10/12/14`，共 17 个 set 脉冲和 17 个 clear 帧。 +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`。 @@ -146,8 +158,8 @@ POST /disconnect_robot/ 真机模式下： -- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 发送循环。 -- `DisableRobot()` 发送 J519 end 控制包，然后 `StopProg("RVBUSTSM")`。 +- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 状态包驱动发送。 +- `DisableRobot()` 发送 J519 packet type 2 状态输出停止包，然后 `StopProg("RVBUSTSM")`。 - `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。 ## 6. 现场抓包覆盖 @@ -162,8 +174,8 @@ POST /disconnect_robot/ 测试同时检查： -- 主运行窗口命令序号连续，无重复 seq。 +- 主运行窗口命令序号连续，无重复 seq；J519 客户端单元测试覆盖按最新 status sequence 回发命令。 - 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。 - 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。 -- `lastData=0`，结束运动依赖 J519 end 控制包。 +- `lastData=0`，结束运动当前依赖 J519 packet type 2 状态输出停止包；`../j519 协议.pcap` 中另有 1 个 `LastData=1` 后紧跟 type 2 的样本，停止语义后续单独验证。 - IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。 diff --git a/docs/fanuc-stream-motion-manual-notes.md b/docs/fanuc-stream-motion-manual-notes.md new file mode 100644 index 0000000..6fa01fa --- /dev/null +++ b/docs/fanuc-stream-motion-manual-notes.md @@ -0,0 +1,232 @@ +# FANUC Stream Motion 文档要点与实现差异 + +本文记录 `../FANUC_stream_motion.pdf` 中与本仓库 `Flyshot.Runtime.Fanuc` 直接相关的重点，并对照当前实现状态。 + +读取时间：2026-05-03 + +## 1. 文档定位 + +`FANUC_stream_motion.pdf` 对应 FANUC `Stream motion` 功能，选项号为 `A05B-2600-J519`。它描述的是外部设备通过以太网实时发送期望位置，让机器人按外部生成路径运动的控制方式。 + +文档明确要求外部设备自行生成满足机器人约束的路径，包括速度、加速度、jerk、可达性、姿态连续性等。FANUC 不提供完整运动学、逆解和碰撞检测公式。 + +## 2. 使用前提与示教程序 + +1. 机器人侧需要安装 J519 stream motion 选项。 +2. 物理网口通过 `$STMO.$PHYS_PORT` 选择，`1` 表示 `CD38A`，`2` 表示 `CD38B`。 +3. 机器人程序必须包含成对的 `IBGN start[*]` 和 `IBGN end[*]` 指令，二者编号必须一致，`start` 必须在 `end` 前一行。 +4. `IBGN start[*]` 执行期间，机器人根据外部设备发来的期望位置运动；`IBGN end[*]` 之后程序继续执行。 +5. 执行时要求 `AUTO` 模式和 `100% OVERRIDE`。 + +当前实现中的 `FanucControllerRuntime.EnableRobot()` 会按现场抓包流程启动 `RVBUSTSM` 程序，并随后允许 J519 在收到机器人 UDP status 包后回发命令。是否满足 `AUTO / 100% OVERRIDE / IBGN start` 已到位，当前只通过 J519 状态位和现场程序行为间接判断，没有在代码里读取或设置这些控制器状态。 + +## 3. UDP 60015 协议结构 + +协议使用 UDP，大端字节序，机器人侧端口为 `60015`。通信周期通常为 `8ms`，部分机型支持 `4ms`。状态包输出可以在任意时间通过 start/stop 控制包启停，不要求已经进入 `IBGN start[*]`。 + +### 3.1 状态输出 start 包 + +外部设备发给机器人： + +| 字段 | 长度 | 值 | +| --- | --- | --- | +| Packet type | 4B | `0` | +| Version | 4B | `1` | + +当前实现：`FanucJ519Protocol.PackInitPacket()` 已按 8B 大端控制包实现，`FanucJ519Client.ConnectAsync()` 连接后立即发送。 + +### 3.2 状态包 + +机器人发给外部设备，长度为 `132B`： + +| 偏移 | 字段 | 含义 | +| --- | --- | --- | +| `0x00` | Packet type | `0` | +| `0x04` | Version | `1` | +| `0x08` | Sequence No. | 状态包序号，发送 start 包后从 `1` 重新开始 | +| `0x0c` | Status | bit0 接受命令、bit1 已收到命令、bit2 SYSRDY、bit3 运动中 | +| `0x0d..0x12` | Read I/O 回显和值 | 回显命令包中的读取 IO 类型、索引、掩码，并返回 16 点 IO 值 | +| `0x14` | Timestamp | ms 单位，2ms 分辨率 | +| `0x18..0x38` | Cartesian / external axis | `X/Y/Z/W/P/R` 加 3 个扩展轴 | +| `0x3c..0x5c` | Joint | `J1..J9`，单位 degree | +| `0x60..0x80` | Motor current | `J1..J9` 电流，单位 A | + +当前实现：`FanucJ519Protocol.ParseResponse()` 已解析 `132B` 状态包，并暴露 `AcceptsCommand`、`ReceivedCommand`、`SystemReady`、`RobotInMotion` 四个状态位。`FanucControllerRuntime.GetSnapshot()` 也会把最新 J519 状态写进快照。 + +### 3.3 命令包 + +外部设备发给机器人，长度为 `64B`： + +| 偏移 | 字段 | 含义 | +| --- | --- | --- | +| `0x00` | Packet type | `1` | +| `0x04` | Version | `1` | +| `0x08` | Sequence No. | 第一包应等于刚收到的状态包序号，后续逐包递增 | +| `0x0c` | Last data | 正常为 `0`；结束外部控制时最后一包设为 `1` | +| `0x0d..0x11` | Reading I/O | 可读取最多 16 个连续 IO 点 | +| `0x12` | Data format | `0` 笛卡尔，`1` 关节 | +| `0x13..0x19` | Writing I/O | 可写入最多 16 个连续 IO 点 | +| `0x1a` | unused | 2B | +| `0x1c..0x3c` | target[9] | 9 个 f32 目标值；关节格式时单位 degree | + +当前实现：`FanucJ519Protocol.PackCommandPacket()` 已按上述布局打包，默认 `dataStyle=1`，也就是关节格式。运行时会把规划输出的弧度制关节轨迹转换为 degree 后下发。 + +### 3.4 状态输出 stop 包 + +外部设备发给机器人： + +| 字段 | 长度 | 值 | +| --- | --- | --- | +| Packet type | 4B | `2` | +| Version | 4B | `1` | + +文档把它定义为“停止状态包输出”的控制包，不是命令流正常终止的首选动作。命令流结束应通过 command packet 的 `Last data=1` 表达。 + +当前实现：`FanucJ519Client.StopMotionAsync()` 当前会停止状态包驱动发送并发送 packet type `2`，而稠密轨迹执行期间保持 `LastData=0`。这是与 FANUC 文档最明显的语义差异之一；已有多数 UTTC 抓包显示主运行窗口 `LastData=0`，但 `../j519 协议.pcap` 中存在 1 个 `LastData=1` 后紧跟 packet type `2` 的样本，后续应单独校准停止语义。 + +## 4. 通信时序重点 + +文档推荐的时序是： + +1. 外部设备发送状态输出 start 包。 +2. 机器人每个通信周期输出状态包。 +3. 机器人程序执行到 `IBGN start[*]` 后，状态包 bit0 变为 `1`，表示等待命令包。 +4. 外部设备收到 bit0 为 `1` 的状态包后，立即发送第一帧命令包，第一帧命令序号应等于刚收到的状态包序号。 +5. 后续每收到一个状态包，外部设备应立即发送下一帧命令包。 +6. 结束命令通信时，发送 `Last data=1` 的最后一帧命令包。 + +当前实现对照： + +1. `FanucJ519Client` 已改为收到机器人 132B status 包后立即回发当前命令，不再由上位机本地固定 8ms 发送循环主动发包。 +2. 命令包 sequence 已按刚收到的 status packet sequence 写入，避免第一帧从本地 `0` 起步。 +3. `FanucControllerRuntime.ExecuteTrajectory()` 启动前会检查已有 J519 响应中的 `AcceptsCommand` 和 `SystemReady`；但如果还没收到状态包，则会放行，后续命令仍要等第一帧 status 到达才会发出。 +4. 当前稠密轨迹结束不发送 `LastData=1`，而是依赖停止 J519 状态包驱动发送和 packet type `2` stop 控制包。 + +序号和节拍已经按手册方向校准；停止语义仍需在真实 R30iB 联调中继续确认。 + +## 5. 命令缓冲 + +文档说明机器人可以缓冲提前到达的 command packet。默认启用，缓冲上限为 `$STMO.$PKT_STACK - 1`，`$PKT_STACK` 默认 `10`，可配置范围 `2..10`。`$STMO.$START_MOVE` 决定积累多少未处理命令包后开始运动，默认 `1`。 + +注意事项： + +1. 只有 command packet 会进入缓冲。 +2. status output stop packet 会立即处理。 +3. 如果 command buffer 中还有未处理包，不应发送 status output stop packet。 +4. 使用 `Last data=1` 时，机器人会先处理完缓冲里的命令包，再结束外部控制。 + +当前实现没有显式预填 `$PKT_STACK` 缓冲，也没有读取 `$START_MOVE`。`FanucJ519Client` 只保存一个“当前命令”，由后台循环持续发送；`FanucControllerRuntime.SendDenseTrajectory()` 另一个 8ms 循环负责按轨迹时间更新这条当前命令。这与文档的“按状态包响应并可提前发多包缓冲”模型不同。 + +## 6. 可执行运动条件 + +文档列出的主要运动约束： + +1. 目标点必须可达。 +2. 笛卡尔格式下目标点对应的关节解必须唯一，且 configuration 要与 `IBGN start` 开始时一致。 +3. 各轴必须满足上下限。 +4. 不能发生自碰撞。 +5. 必须考虑 FANUC J3 轴定义：J3 不是相对 J2 臂的夹角，而是机器人视角下相对水平面的 J3 臂角度。 +6. 外部设备必须控制每轴速度、加速度、jerk 不超过 `$STMO_GRP` 下的限制。 +7. 状态包中的当前位置是 servo feedback position，不是 command position。轨迹起点应平滑连接到机器人 command position，而不能简单用当前 servo position 直接起步。 +8. reducer load 超限也会导致停机，负载相关计算不公开。 + +当前实现对这些条件的覆盖： + +| 条件 | 当前状态 | +| --- | --- | +| 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 | +| `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 | +| `speed_ratio` 下发时间轴缩放 | 已实现，规则为 `t_traj = k * 0.008 * speed_ratio` | +| IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` | +| 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 | +| J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 | +| command position 起步 | `MoveJoint` 会用当前运行时记录的关节作为起点生成 PTP 稠密轨迹；但没有通过 FANUC HMI 通信读取 command position | +| reducer load | 未建模，依赖保守规划和现场报警反馈 | +| 笛卡尔格式限制 | 运行时不走笛卡尔 J519 目标格式，暂不覆盖 configuration 变化报警 | + +## 7. 系统变量 + +与本仓库后续最相关的变量： + +| 变量 | 默认/含义 | +| --- | --- | +| `$STMO.$PHYS_PORT` | 物理口，`1=CD38A`，`2=CD38B` | +| `$STMO.$COM_INT` | 通信周期，单位 ms，通常 `8`，只读 | +| `$STMO.$PKT_STACK` | command buffer 最大保留量，默认 `10` | +| `$STMO.$START_MOVE` | 缓冲中积累多少未处理命令后开始运动，默认 `1` | +| `$STMO_GRP.$JNT_VEL_LIM[*]` | 各轴速度上限，degree/s，只读 | +| `$STMO_GRP.$JNT_ACC_LIM[*]` | 各轴加速度上限，degree/s^2，只读 | +| `$STMO_GRP.$JNT_JRK_LIM[*]` | 各轴 jerk 上限，degree/s^3，只读 | +| `$STMO_GRP.$LMT_MODE` | 加速度/jerk 限制计算模式 | +| `$STMO_GRP.$WARN_LIM` | 接近限制时报警阈值，默认 `80%` | +| `$STMO_GRP.$FLTR_LN` | 命令目标移动平均滤波窗口 | +| `$STMO_GRP.$MAX_SPD` | 用于限制计算的 flange center 最大速度 | + +当前实现没有读取或设置上述系统变量。`RobotProfile.ServoPeriod` 当前决定运行时发送周期；对当前现场而言应继续确认它与 `$COM_INT` 一致。 + +## 8. 附录 B：加速度和 jerk 限制 + +文档说明，在 `$STMO_GRP[].$LMT_MODE=0` 时，加速度和 jerk 的允许上限会根据 flange center speed 与 payload 计算： + +1. 以 `$MAX_SPD` 分成 20 档速度区间。 +2. 每个轴、每种限制类型都有无负载和最大负载两张 20 档表。 +3. 实际 payload 通过线性插值得到限制表。 +4. 实际 flange center speed 在相邻速度档之间线性插值。 +5. 限制值不是每个通信周期都更新，而是在超过 `Vmax/20` 到再次跌回阈值的整段时间内，以该段观测到的 `Vpeak` 决定。 +6. 如果长时间不跌回阈值，会按中间检查时间做临时判断。 + +文档还提供了 packet type `3` 的限制表查询协议： + +| 包 | 方向 | 重点字段 | +| --- | --- | --- | +| 请求 | 外部设备 -> 机器人 | packet type `3`、version `1`、axis `1..9`、limit type `0=velocity/1=acceleration/2=jerk` | +| 响应 | 机器人 -> 外部设备 | packet type `3`、version `1`、axis、limit type、`Vmax`、中间检查时间、无负载 20 档、最大负载 20 档 | + +当前实现没有 packet type `3` 查询，也没有实现手册描述的动态限制表算法。现阶段规划时长和保守程度主要依赖 replacement 自身参数与现场抓包对齐。 + +## 9. 报警与诊断 + +文档中与实现最相关的报警： + +| 报警 | 含义 | +| --- | --- | +| `MOTN-600` | 命令序号与机器人期望不一致 | +| `MOTN-602` | data format 非法 | +| `MOTN-603` | 后续命令包未在通信周期内到达 | +| `MOTN-604` | 命令包过多，超出缓冲 | +| `MOTN-605` | 目标位置包含 NaN 或 infinity | +| `MOTN-606` | 非 AUTO 或 override 不是 100% | +| `MOTN-607` | 协议版本不匹配 | +| `MOTN-609/610/611` | 速度、加速度、jerk 超限 | +| `MOTN-612/613/614` | 接近速度、加速度、jerk 限制 | +| `MOTN-617` | 目标点与当前位置不连续 | +| `MOTN-619` | 当前机型不支持笛卡尔目标格式 | +| `PRIO-023` | 读写的 IO 类型或索引未分配 | + +当前 `ControllerStateSnapshot.ActiveAlarms` 仍为空，Web 状态页也尚未接入 FANUC 报警列表。后续现场联调如果出现报警，应优先按上述表格关联 J519 包序号、目标数据、IO 字段、发送间隔和状态包 bit。 + +## 10. 与当前代码的结论 + +已基本对齐： + +1. UDP 60015、大端、start/stop 控制包、64B command packet、132B status packet 的基础二进制布局。 +2. `Data format=1` 的关节目标下发。 +3. 状态位 bit0..bit3 的解析和快照暴露。 +4. 规划输出 `rad` 转 J519 `deg`。 +5. 根据 `speed_ratio` 做运行期时间轴缩放，而不是改变规划文件时间。 +6. 飞拍 IO 触发通过 command packet 的写 IO 字段下发。 +7. 命令发送按机器人 UDP status 包驱动，并使用最新 status sequence 回发。 + +主要差异/风险： + +1. 当前未实现命令缓冲预填，也未读取 `$PKT_STACK / $START_MOVE`。 +2. 当前停止运动依赖 packet type `2` stop 控制包，没有稳定发送 `LastData=1` 的最后 command packet；这与手册标准结束语义不同。 +3. 当前未实现 packet type `3` 的速度/加速度/jerk 限制表查询，也未实现 payload/speed 20 档动态限制算法。 +4. 当前没有自动校验 `AUTO / 100% OVERRIDE / brake control / resume offset / payload` 等控制器前置状态。 +5. 当前没有报警码读取和 `MOTN-* / PRIO-*` 映射。 + +建议后续联调优先级： + +1. 验证运动结束是否必须补 `LastData=1`；如果当前 stop 控制包能稳定工作，也应在文档中标为现场兼容路径，而不是手册标准路径。 +2. 抓一次报警现场包，确认 `MOTN-600/603/617` 等是否能从包序号与状态位直接定位。 +3. 如果后续追求更稳的真实机运行，补 packet type `3` 限制表查询，并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。 diff --git a/docs/planning-duration-mismatch-investigation.md b/docs/planning-duration-mismatch-investigation.md new file mode 100644 index 0000000..53a1ec3 --- /dev/null +++ b/docs/planning-duration-mismatch-investigation.md @@ -0,0 +1,591 @@ +# 轨迹规划时长差异调查记录 + +## 背景 + +当前新 C# 规划链路在不额外缩放规划约束时，部分真实现场轨迹会比旧 RVBUST/FlyingShot 导出的 `JointTraj.txt` 更短。 + +最典型现象： + +- 真实 `Rvbust/uttc-20260428/Data/JointTraj.txt`：`UTTC_MS11` 总时长约 `7.403046s`。 +- 新 C# 当前默认规划输出：`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/UTTC_MS11/JointTraj.txt` 总时长约 `5.495112s`。 +- 实体机复核确认：修改运行时 `speed_ratio` 不影响 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 生成的 `JointTraj.txt` 规划时长。 + +因此，本问题不应继续归因到运行时 `speed_ratio`，而应归到规划阶段的有效关节约束来源。 + +## 已确认事实 + +1. `speed_ratio` 是运行执行倍率。 + + UTTC 抓包和实体机测试都显示，`speed_ratio=0.7` 会拉伸 J519 实际下发时间和包数，但不会改变已生成的 `JointTraj.txt` 规划时间轴。 + +2. `JointTraj.txt` 是规划结果点位。 + + `saveTrajectory` / `SaveTrajInfo` / `IsFlyshotTrajectoryValid(saveTrajectory=true)` 生成的 `JointTraj.txt` 表示规划后的 sparse waypoint 时间轴，不是上传的原始飞拍路径，也不是 J519 逐周期下发点。 + +3. UTTC_MS11 的差异是整条时间轴等比例缩放。 + + 对 `UTTC_MS11`，真实时间和当前 C# 默认规划时间之间的比例在所有 waypoint 上都一致： + + ```text + C#默认规划时间 / 真实规划时间 = 5.495112 / 7.403046 = 0.742277 + ``` + + 这说明路点顺序、相对分段时间和 ICSP 主要逻辑基本一致，差异更像是规划时传入的有效 `vel/acc/jerk` joint limits 存在整体倍率差异。 + +4. 现场配置中没有找到显式倍率字段。 + + 已检查现场现有配置，未发现类似 `planning_speed_scale` 或等价字段保存了 `0.742277`、`0.7`、`0.9` 等规划倍率值。 + +## 样本对比 + +| 样本 | 真实 `JointTraj.txt` 时长 | 当前/已有新规划时长 | 等效倍率 `新规划/真实` | 说明 | +| --- | ---: | ---: | ---: | --- | +| `UTTC_MS11` | `7.403046s` | `5.495112s` | `0.742277` | 所有 waypoint 时间均为同一比例 | +| `UTTC_MS11_TEST01` | `7.805885s` | `5.814370s` | `0.744870` | `20260428 多点` 新增 1 个路径点后仍几乎是整条时间轴等比例缩放 | +| `EOL10_EAU_0` | `14.849788s` | `10.489800s` | `0.706394` | 同样表现为新规划偏快 | +| `EOL9_EAU_0` / `EOL9_EAU_90` | `6.400851s` | `5.651140s` | `0.882873` | `EOL9 EAU 0` 与 `EOL9 EAU 90` 的真实 `JointTraj.txt` 文件一致 | +| `EOL9_EAU_90` | `6.400851s` | `6.471610s` | `1.011055` | 使用 `speedRatio=0.9 + self-adapt-icsp` 的旧离线结果已接近真实 | + +这些样本说明差异不是 `UTTC_MS11` 的个案，也不是一个可以全局写死的常数。不同真实样本对应的等效规划倍率不同。 + +## `20260428 多点` 新样本对比 + +2026-04-30 追加现场新样本： + +```text +../Rvbust/20260428 多点/RobotConfig.json +../Rvbust/20260428 多点/JointTraj.txt +../Rvbust/20260428 多点/JointDetialTraj.txt +``` + +该样本中的飞拍程序名为： + +```text +UTTC_MS11_TEST01 +``` + +配置摘要： + +```text +waypoints=21 +shot_flags=21 +acc_limit=1 +jerk_limit=1 +``` + +实机导出的 `JointTraj.txt`： + +```text +rows=21 +duration=7.805885s +``` + +用当前 C# `ICspPlanner`、同一个 `LR_Mate_200iD_7L.robot`、同一份 `RobotConfig.json` 规划： + +```text +rows=21 +duration=5.814370s +C# / 实机 = 0.744870 +``` + +逐点/逐段统计： + +```text +point_ratio_std = 2.18e-7 +segment_ratio_std = 9.0e-7 +max_joint_diff = 5.0e-7 rad +``` + +这说明： + +1. 新样本的路点关节值与 C# 输入基本完全一致，不是解析错点或单位错位。 +2. 新增 1 个路径点后，C# 与旧系统仍然保持几乎严格的整条时间轴等比例差异。 +3. `UTTC_MS11_TEST01` 的倍率 `0.744870` 与原 `UTTC_MS11` 的 `0.742277` 非常接近，进一步支持“同一类 UTTC 现场导出使用了一组更保守的 effective JointLimits”这一判断。 + +和原 `UTTC_MS11` 对比： + +```text +原 UTTC_MS11 实机 rows=20 duration=7.403046s +新 UTTC_MS11_TEST01 实机 rows=21 duration=7.805885s +新增路径点后实机时长增加 0.402839s +``` + +当前观察不到新增点导致规划形状或局部段比例失真；它更像是在同一套旧系统规划约束下正常增加了一段路径时间。 + +## `20260430.pcap` 初始化抓包复核 + +2026-04-30 继续复核现场提供的完整初始化抓包： + +```text +../Rvbust/20260428 多点/20260430.pcap +``` + +抓包总览： + +```text +packet_count=4821 +tcp_payload_bytes=35302 +udp_payload_bytes=451946 +``` + +主要有效负载会话为： + +```text +UDP 192.168.10.11:60015 -> 192.168.10.10:56118 260700B +UDP 192.168.10.11:60015 -> 192.168.10.10:48455 127116B +UDP 192.168.10.10:48455 -> 192.168.10.11:60015 62088B +TCP 192.168.10.11:10010 -> 192.168.10.10:42106 35102B +TCP 192.168.10.11:10012 -> 192.168.10.10:33528 106B +TCP 192.168.10.10:33528 -> 192.168.10.11:10012 94B +``` + +全包搜索以下明文关键字没有命中： + +```text +Joint / joint / Limit / limit / vel / acc / jerk / Speed / speed / +Robot / JSON / Traj / ratio / Ratio / GetJoint / SetJoint +``` + +`TCP 10012` 命令通道按 `doz + length + message_id + body + zod` 解码后，只看到以下初始化/程序命令： + +| 方向 | 消息号 | 含义 | 请求体/结果 | +| --- | ---: | --- | --- | +| C->R | `0x0001` | 未知握手 | 空请求 | +| R->C | `0x0001` | 未知握手响应 | `result=0` | +| C->R | `0x2000` | 未知版本/状态查询 | 空请求 | +| R->C | `0x2000` | 未知版本/状态响应 | 包含 `0.6.0` 字符串 | +| C->R | `0x2100` | `ResetRobot` | 空请求 | +| R->C | `0x2100` | `ResetRobot` 响应 | `result=0` | +| C->R | `0x2003` | `GetProgramStatus("RVBUSTSM")` | 程序名 `RVBUSTSM` | +| R->C | `0x2003` | 程序状态响应 | `result=0, status=1` | +| C->R | `0x2102` | `StartProgram("RVBUSTSM")` | 程序名 `RVBUSTSM` | +| R->C | `0x2102` | 启动响应 | `result=0` | + +没有看到： + +- `0x2207 SetSpeedRatio` +- `0x2206 GetSpeedRatio` +- `0x2200/0x2201 GetTcp/SetTcp` +- `0x2208/0x2209 GetIo/SetIo` +- 任何疑似 `JointLimits / velocity / acceleration / jerk` 的参数帧 + +`TCP 10010` 状态通道只有机器人侧到上位机的状态帧： + +```text +390 个 90B 状态帧 +1 个 2B 连接前导 +``` + +这些帧与已逆向的 `pose[6] + joint[6] + external_axes[3] + raw_tail_words[4]` 状态布局一致，不携带规划约束。 + +`UDP 60015` J519 通道只出现既有三类长度： + +```text +C->R 8B 初始化包 1 个 +C->R 64B 目标关节命令包 970 个 +R->C 132B 反馈包 2938 个 +``` + +没有出现其他长度的 UDP 参数帧。64B 命令包是 J519 逐周期目标关节/IO 命令，132B 是机器人反馈；这条链路承载的是执行期 streaming motion，而不是旧 RVBUST 规划器的 joint limit 配置。 + +阶段结论： + +```text +20260430.pcap 只覆盖机器人侧 10010 / 10012 / 60015 通信。 +它没有 50001/TCP+JSON，也没有 ControllerServer/Python 客户端到旧服务端的配置调用。 +因此，这份抓包看不到旧规划阶段的 effective JointLimits。 +``` + +这并不否定“旧系统规划瞬间存在更保守的 effective JointLimits”这一方向；它只说明这份初始化抓包不是抓取该信息的位置。若要抓到这类限制，需要抓旧服务端内部 `_GetJointLimits/_SetJointLimits`，或者抓上层 Python/GUI 与 ControllerServer 之间的配置/规划调用，而不是只抓机器人控制柜侧的执行链路。 + +## `all-50001.pcap` 本机 50001 抓包复核 + +2026-04-30 追加复核本机所有网卡抓包： + +```text +../Rvbust/20260428 多点/all-50001.pcap +SHA256=C3543F314AE446CABA8E2097EFAFB36F39DD73FFE166F051A1F9387CFD15990F +``` + +该文件由 `tcpdump -i any` 生成，pcap linktype 为 `113`，即 Linux cooked capture。按 SLL 头解析后，确认抓到了本机到本机的 `50001` TCP JSON 通信： + +```text +192.168.1.100:35814 -> 192.168.1.100:50001 217B payload +192.168.1.100:50001 -> 192.168.1.100:35814 91B payload +``` + +客户端到服务端的完整 JSON 命令序列为： + +```json +{"reply_from_client":true} +{"cmd":"SetUpRobot","robot_name":"FANUC_LR_Mate_200iD_7L"} +{"cmd":"IsSetUp"} +{"cmd":"SetActiveController","sim":false} +{"cmd":"Connect","ip":"192.168.10.11"} +{"buffer_size":8,"cmd":"EnableRobot"} +``` + +服务端返回为： + +```json +{"test_from_server": true} +{"res": true} +{"res": true} +{"res": true} +{"res": true} +{"res": true} +``` + +这说明本次抓包确实覆盖到了旧 `50001` 控制链路，但当前只包含机器人初始化、连接和使能流程。里面没有看到： + +- `ExecuteFlyShotTraj` +- `SaveTrajInfo` +- `IsFlyShotTrajValid` +- `GetJointLimits / SetJointLimits` +- `SetVelocityLimit / SetAccelerationLimit / SetJerkLimit` +- 任何包含 `acc_limit / jerk_limit / JointLimits / velocity / acceleration / jerk` 的配置 JSON + +阶段结论： + +```text +all-50001.pcap 已经证明抓包接口选对了； +但这次只抓到了初始化链路，没有抓到规划/保存轨迹那一刻的 50001 请求。 +``` + +该待确认点已由下一节 `all-50001-plan.pcap` 覆盖：后续抓包确实抓到了 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)`，仍未出现规划限制字段。 + +## `all-50001-plan.pcap` 规划执行抓包复核 + +2026-04-30 追加复核规划/执行动作期间的本机 50001 抓包： + +```text +../Rvbust/20260428 多点/all-50001-plan.pcap +SHA256=311DC45B4789ED11EBEAB7A396E2EE7A16EC8534E20F10127FB43BBAD823C21D +``` + +该抓包同样是 `tcpdump -i any` 生成的 Linux cooked capture，已按 SLL 头解析。有效 TCP JSON 流为： + +```text +192.168.1.100:35814 -> 192.168.1.100:50001 2612B payload +192.168.1.100:50001 -> 192.168.1.100:35814 516B payload +``` + +客户端到服务端的关键命令序列为： + +```json +{"cmd":"ListFlyShotTraj"} +{"cmd":"GetNextListFlyShotTraj","count":0} +{"cmd":"SetSpeedRatio","ratio":0.5} +{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true} +{"cmd":"SetSpeedRatio","ratio":1.0} +{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true} +{"cmd":"StartUploadFlyShotTraj","name":"UTTC_MS11"} +{"cmd":"UploadFlyShotTraj", "...":"4 批共 20 个 waypoint，每批包含 waypoints / shot_flags / offset_values / addrs"} +{"cmd":"EndUploadFlyShotTraj","name":"UTTC_MS11"} +{"cmd":"ListFlyShotTraj"} +{"cmd":"GetNextListFlyShotTraj","count":0} +``` + +两次执行请求均为： + +```json +{ + "cmd": "ExecuteFlyShotTraj", + "method": "icsp", + "move_to_start": true, + "name": "UTTC_MS11_TEST01", + "save_traj": true, + "use_cache": false, + "wait": true +} +``` + +它们前面的速度倍率分别为： + +```text +第一次：SetSpeedRatio ratio=0.5 +第二次：SetSpeedRatio ratio=1.0 +``` + +服务端对所有命令均返回： + +```json +{"res": true} +``` + +这份抓包确认了两点： + +1. 公开 50001 JSON 链路确实会把 `SetSpeedRatio` 和 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 发给旧服务端。 +2. 即便覆盖到了实际执行/保存轨迹动作，请求中仍没有出现 `GetJointLimits / SetJointLimits`、`SetVelocityLimit / SetAccelerationLimit / SetJerkLimit`，也没有 `acc_limit / jerk_limit / velocity / acceleration / jerk / JointLimits` 等规划限制字段。 + +因此，当前能从 50001 抓包确认的是： + +```text +规划方法、是否保存轨迹、是否使用缓存、是否等待执行，都会显式发到旧服务端； +速度倍率通过 SetSpeedRatio 单独发到旧服务端； +但 effective JointLimits 没有通过这次公开 50001 JSON 请求显式传入。 +``` + +这进一步收敛了差异来源：如果旧系统规划时确实使用了更保守的 joint limits，它更可能来自旧服务端在 `SetUpRobot("FANUC_LR_Mate_200iD_7L")` 后加载/初始化的内部状态，或来自 GUI/服务端内部私有路径，而不是这次 50001 公开 JSON 在 `ExecuteFlyShotTraj` 请求中传入的字段。 + +## Joint3/Joint2 couple A/B 测试 + +2026-04-30 追加测试：为了验证 `.robot` 中 `Joint3` 对 `Joint2` 的 couple 是否是规划时长差异主因，使用 Python ICSP demo 做了多组只读 A/B。 + +测试模型来自： + +```text +flyshot-replacement/Config/Models/LR_Mate_200iD_7L.robot +``` + +其中 `Joint3` 的 couple 信息为： + +```text +q3_kin = q3_raw + q2_kin * 1.0 + 0.0 +``` + +测试变体： + +- `raw`：原始 6 轴路点直接规划。 +- `replace_q3=q3+q2`：规划输入中把第 3 轴替换为耦合后的运动学角。 +- `replace_q3=q3-q2`：反向符号试探，排除符号理解错误。 +- `raw+constraint(q3+q2)`：保留原始 6 轴，同时追加虚拟约束轴 `q3+q2`，用 Joint3 的 `vel/acc/jerk` 限值检查。 +- `raw+constraint(q3-q2)`：反向符号的虚拟约束轴试探。 + +结果： + +| 样本 | 真实时长 | 最接近变体 | 变体时长 | 变体/真实 | 与真实差值 | 结论 | +| --- | ---: | --- | ---: | ---: | ---: | --- | +| `UTTC_MS11` | `7.403046s` | `raw` | `5.495112s` | `0.742277` | `1.907934s` | couple 变体全部更短，且破坏原本严格等比例关系 | +| `EOL10_EAU_0` | `14.849788s` | `replace_q3=q3+q2` | `10.600711s` | `0.713863` | `4.249077s` | couple 只改善约 `0.11s`，距离真实仍差 `4.25s` | +| `EOL9_EAU_90` | `6.400851s` | `raw+constraint(q3+q2)` | `5.748560s` | `0.898093` | `0.652291s` | couple 约束有小幅影响，但仍不足以解释真实时长 | + +关键观察： + +1. `UTTC_MS11` 的 `raw` 规划时间和真实时间保持严格等比例，`point_ratio_std=0`、`segment_ratio_std≈0`；加入 couple 后反而出现分段比例波动。 +2. `EOL10_EAU_0` 与 `EOL9_EAU_90` 的 couple 变体只带来小幅时长变化，不能解释 10% 到 30% 级别的差异。 +3. 因此，当前证据不支持“只要把 Joint3/Joint2 couple 带入 ICSP，就能对齐旧 RVBUST 规划时长”。 + +阶段结论： + +`Joint3` couple 确实是 C# 与 Python demo 当前都没有进入规划约束的缺口，但它不像本轮时长 mismatch 的主因。它更可能影响 FK/运动学边界或少数局部段约束；当前主要时长差异仍更像有效 joint limits、旧系统运行期规划倍率、或 RPS 内部 ICSP 参数来源不同。 + +## 同模型复核与更可能的差异层 + +2026-04-30 继续复核： + +1. 当前仓库固化的模型与旧 `FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot` 字节哈希一致。 +2. `ControllerClientCompatRobotCatalog` 当前会把 `FANUC_LR_Mate_200iD` 和 `FANUC_LR_Mate_200iD_7L` 都映射到 `LR_Mate_200iD_7L.robot`。 +3. `LR_Mate_200iD.robot` 短臂模型的前三轴 `vel/acc/jerk` 比 `7L` 更高。用短臂模型试算会让轨迹更短，不会解释“旧系统真实导出更慢”。 + +模型 A/B： + +| 样本 | 真实时长 | `LR_Mate_200iD_7L.robot` | `LR_Mate_200iD.robot` | 结论 | +| --- | ---: | ---: | ---: | --- | +| `UTTC_MS11` | `7.403046s` | `5.495112s` | `5.345600s` | 短臂模型方向更错 | +| `EOL10_EAU_0` | `14.849788s` | `10.489800s` | `10.342456s` | 短臂模型方向更错 | + +因此，如果现场确认机器人模型确实一致，差异层就不应继续放在 `.robot` 静态文件本身，而应放在旧服务端规划时的运行态： + +- 服务端内部存在 `_GetJointLimits / _SetJointLimits`，说明规划消费的是一份可能被运行期覆写的 `current JointLimits`。 +- `ControllerClient.h` 的 `ExecuteFlyShotTraj(..., use_cache=false)` 明确说明旧服务端可以把计算好的轨迹保存在内存中并复用。 +- `SaveTrajInfo(name, method)` 没有 `use_cache` 参数，不能仅凭公开头文件判断它一定每次从当前配置重新规划。 + +当前更合理的解释是： + +```text +同一个 .robot + -> SetUpRobot 初始化基础 JointLimits + -> 旧服务端运行期间可能被 _SetJointLimits / 速度倍率联动 / 缓存轨迹 覆盖 + -> SaveTrajInfo 或 IsFlyShotTrajValid(save_traj=true) 导出的是真正规划时那份状态 + -> 当前 C# 每次用静态 .robot + RobotConfig 重新规划，所以时长更短 +``` + +尤其需要注意：`EOL10_EAU_0` 的 `新规划/真实` 比例为 `0.706394`，接近 `0.7`；`EOL9_EAU_90` 的比例为 `0.882873`，接近 `0.9`。这不像模型误差，更像历史导出时混入了某个运行态速度/限制倍率。`UTTC_MS11` 的 `0.742277` 不等于抓包确认的执行层 `0.7`，所以不能简单把所有样本都归因到 `SetSpeedRatio`，但“运行态规划约束不是静态模型值”仍是目前最强方向。 + +## 旧服务端与 GUI 二进制复核 + +2026-04-30 继续从旧系统二进制字符串中复核，重点看公开 Python/HTTP 层没有暴露出来的运行态对象。 + +### 服务端确实持有 runtime JointLimits + +`../FlyingShot/FlyingShot/Python/ControllerServer/ControllerServer.cpython-37m-x86_64-linux-gnu.so` 中能稳定看到以下方法和关键字： + +```text +ControllerServer.ControllerServer._GetJointLimits +ControllerServer.ControllerServer._SetJointLimits +ControllerServer.ControllerServer._IsWaypointInJointLimits +ControllerServer.ControllerServer._IsTrajInJointLimits +ControllerServer.ControllerServer._IsTrajInJerkLimits +ControllerServer.ControllerServer._ExecuteFlyShotTraj +ControllerServer.ControllerServer._SaveTrajInfo +ControllerServer.ControllerServer._IsFlyShotTrajValid +SetVelocityLimit +SetAccelerationLimit +SetJerkLimit +GetMaxVelocity +GetMaxAcceleration +GetMaxJerk +m_acc_limit +m_jerk_limit +save_traj_only +use_cache +``` + +这比公开 `ControllerClient.h` 暴露的信息更多。它说明旧服务端内部不是只把 `.robot` 静态值直接传给 `TrajectoryRnICSP`，而是存在一份可以查询、设置、校验、再用于规划的运行期 `JointLimits`。 + +### GUI 也直接接触规划约束与保存逻辑 + +旧 GUI 二进制里也能看到同一条链： + +- `../FlyingShot/FlyingShot/Python/GUI/Robot/RobotManager.cpython-37m-x86_64-linux-gnu.so` + - `GetJointLimits` + - `TrajectoryRnICSP` + - `IsTrajInJointLimits` + - `IsTrajInJerkLimits` + - `acc_limit` + - `jerk_limit` +- `../FlyingShot/FlyingShot/Python/GUI/Robot/RobotConfig.cpython-37m-x86_64-linux-gnu.so` + - `SaveTraj` + - `m_acc_limit` + - `m_jerk_limit` +- `../FlyingShot/FlyingShot/Python/GUI/Panels/FlyshotDockPanel.cpython-37m-x86_64-linux-gnu.so` + - `__SaveTraj` + - `IsTrajInJointLimits` + - `IsTrajInJerkLimits` + - `m_acc_limit` + - `m_jerk_limit` + +这说明旧 GUI 的“保存轨迹/检查轨迹”路径很可能不是简单调用公开 `ControllerClient.SaveTrajInfo` 后结束，而是直接拿当前 `JointLimits + acc_limit + jerk_limit` 做规划、合法性检查或保存。 + +### UAES 接口没有显式对齐 JointLimits + +`../flyshot-uaes-interface/main.py` 中 `/execute_flyshot/` 的执行路径是： + +```text +c.ExecuteFlyShotTraj(name=name, move_to_start=True, method="icsp", save_traj=True) +``` + +`/set_speedRatio/` 是单独接口： + +```text +c.SetSpeedRatio(speed) +``` + +同时，`../flyshot-uaes-interface/lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so` 和 `../flyshot-uaes-interface/lib/libControllerClient.so` 中只看到公开客户端侧的： + +```text +GetSpeedRatio +SetSpeedRatio +ExecuteFlyShotTraj +SaveTrajInfo +IsFlyShotTrajValid +JointLimits +``` + +没有看到客户端侧 `GetJointLimits / SetJointLimits` 符号。也就是说，UAES Python 服务本身大概率没有主动把旧服务端的 runtime JointLimits 设置成某个值；如果现场旧导出时的 limits 被改过，更可能来自： + +- 旧 GUI 初始化/保存路径； +- 旧服务端内部默认初始化； +- 服务端隐藏 TCP JSON 方法； +- 历史上某次执行/保存后留下的缓存结果。 + +### 样本文件与配置文件可能不是同一次运行态 + +新增一个需要警惕的现象： + +- `../Rvbust/EOL9 EAU 0/eol9_eau_0.json` 中 `acc_limit=1`、`jerk_limit=1`。 +- `../Rvbust/EOL9 EAU 90/eol9_eau_90.json` 中 `acc_limit=0.8`、`jerk_limit=0.8`。 +- 但两个目录下保存的真实 `JointTraj.txt` 内容和时长一致。 + +哈希复核： + +```text +EOL9 EAU 0 JointTraj.txt SHA256=DFD8E1130742CFB4ED72F70D0E8CA4E3A16F421E0D0D9D921B9F5177717536EC +EOL9 EAU 90 JointTraj.txt SHA256=DFD8E1130742CFB4ED72F70D0E8CA4E3A16F421E0D0D9D921B9F5177717536EC +eol9_eau_0.json SHA256=354D0D3F71499951976504802C4B2860132D1E4FF753738715A500529CD0BB68 +eol9_eau_90.json SHA256=7F854AA227D842CAE734AFA378FEEFA742D797F99FBE536E1B98DF981CD32B27 +``` + +这说明不能默认认为“某个 JSON 文件当前内容”就一定是旁边 `Data/JointTraj.txt` 的生成状态。旧系统的保存文件可能来自缓存、拷贝、历史运行态，或 GUI/服务端中未落盘到该 JSON 的当前 `JointLimits`。 + +本轮新增证据把方向进一步收敛为： + +```text +同一个 .robot 文件本身不是问题核心； +真正影响时长的是旧系统规划瞬间的 effective JointLimits； +但这份状态没有出现在现有配置、机器人侧抓包或 50001 公开 JSON 中。 +``` + +如果未来能直接进入旧服务端进程，仍可在 `SaveTrajInfo` / `IsFlyShotTrajValid(save_traj=true)` 前后抓取 `_GetJointLimits` 返回值，并把它与 `.robot` 原始 `vel/acc/jerk` 和当前 JSON 的 `acc_limit/jerk_limit` 做数值对比。但这不再阻塞 replacement 的现场对齐：当前设计默认用显式内部规划加速度参数补齐这份不可见状态。 + +## 当前判断 + +当前最可信的解释是： + +1. 旧 RVBUST/FlyingShot 生成真实 `JointTraj.txt` 时，规划阶段使用的有效 joint limits 并不总是 `.robot` 文件中的原始 `velocity / acceleration / jerk`。 +2. 这些有效 joint limits 可能来自服务运行期状态，例如旧服务端内部的 `_SetJointLimits`、上层 GUI/脚本初始化流程、机器人环境配置，或其他未落入当前 JSON 文件的运行时参数。 +3. 现有现场 JSON 中只明确保存了： + - `acc_limit` + - `jerk_limit` + - `adapt_icsp_try_num` + - IO 相关配置 +4. 已重新抓取机器人侧 `10010/10012/60015` 和本机 `50001/TCP+JSON`，仍没有看到 `JointLimits / velocity / acceleration / jerk / acc_limit / jerk_limit` 通过公开链路在规划时下发。 +5. 目前没有证据表明现场配置文件或公开 TCP JSON 显式保存了一个“规划速度倍率”或“规划加速度限制”。 + +因此，`0.742277` 不应被理解为固定业务常量。它只是 `UTTC_MS11` 在当前 C# 默认约束和真实导出结果之间反推出来的等效规划倍率。 + +## 兼容设计决策 + +由于重新抓包后仍抓不到旧系统的 effective limits，新系统后续不再继续假设公开链路会传入这份数据，而是采用 replacement-only 的显式规划约束参数补齐不可见状态。 + +参数分层如下： + +1. `acc_limit / jerk_limit` + - 来源：旧 `RobotConfig.json` 中已经存在的字段。 + - 语义：继续作为旧配置的基础倍率，参与 `.robot` 模型加载。 + - 限制：现场样本中 `acc_limit=1`、`jerk_limit=1` 时，不能解释旧导出轨迹更慢的问题。 + +2. `planning_acceleration_scale` + - 来源：新系统内部兼容参数，不声称来自旧 RVBUST 配置或抓包。 + - 语义：只用于规划阶段，额外缩放 `JointLimit.AccelerationLimit`，用于复现旧服务端不可见的保守加速度约束。 + - 默认值：`1.0`，表示不额外限制。 + - 现场校准：若按纯加速度限制解释 `UTTC_MS11_TEST01`，可先用 `(5.814370 / 7.805885)^2 ≈ 0.5548` 作为候选起点，再用真实 `JointTraj.txt` 对拍确认。 + +3. `planning_speed_scale` + - 来源：当前 C# 已支持的显式兼容字段。 + - 语义：把整条规划时间轴按速度倍率解释，联动缩放速度、加速度、jerk。 + - 定位：保留为临时整体验证开关；当后续落地 `planning_acceleration_scale` 后，现场默认优先使用加速度限制参数，而不是把 `planning_speed_scale` 当成旧系统事实。 + +当前 C# 已支持的 `planning_speed_scale` 形式为： + +```json +{ + "robot": { + "planning_speed_scale": 0.742277 + } +} +``` + +该字段只用于规划阶段： + +- `vel *= planning_speed_scale` +- `acc *= planning_speed_scale^2` +- `jerk *= planning_speed_scale^3` + +它不等同于运行时 `/set_speedRatio/`，也不改变 J519 的 8ms 发送周期。运行阶段仍按： + +```text +t_traj = k * 0.008 * speed_ratio +``` + +从已生成轨迹中重采样。 + +由于现场真实配置和本轮抓包中都没有找到这类倍率，所有 `planning_*` 字段都必须标注为 replacement-only 兼容校准参数，不能声称它们来自旧配置文件或公开 TCP JSON。 + +## 后续设计方向 + +1. 默认不再把运行时 `speed_ratio` 混入 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 的规划时间计算。 +2. 后续实现优先新增 `planning_acceleration_scale`，只限制规划加速度，并将其写入 `RobotConfig.json` 的 `robot` 节点或当前现场默认配置。 +3. 若只需快速对齐整条时间轴，可临时使用现有 `planning_speed_scale`；但文档、日志和配置说明必须标注它是新系统校准值。 +4. 如果未来能直接调用旧服务端 `_GetJointLimits`，再用返回值替换当前反推参数；在此之前，显式内部参数是当前可控且可审计的兼容策略。 diff --git a/docs/python-client-interface-reverse-engineering.md b/docs/python-client-interface-reverse-engineering.md new file mode 100644 index 0000000..45723ee --- /dev/null +++ b/docs/python-client-interface-reverse-engineering.md @@ -0,0 +1,202 @@ +# Python ControllerClient 接口逆向记录 + +## 背景 + +本记录用于确认旧 `PyControllerClient` 对 Python 暴露了哪些接口，尤其确认是否能通过 Python client 直接查询或设置旧服务端运行态 `JointLimits`。 + +复核对象： + +```text +../flyshot-uaes-interface/lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so +../FlyingShot/FlyingShot/Lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so +../flyshot-uaes-interface/lib/libControllerClient.so +../FlyingShot/FlyingShot/Lib/libControllerClient.so +../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h +../FlyingShot/FlyingShot/Include/ControllerClient/Types.h +../flyshot-uaes-interface/UseControllerClient.py +../flyshot-uaes-interface/main.py +``` + +两份 Python 扩展与两份底层 client 库哈希一致： + +```text +PyControllerClient.cpython-37m-x86_64-linux-gnu.so +SHA256=648CC23CBC6DF83822B58AC4A10211EE1DF8029AD8933D31032187748DF7F4BC + +libControllerClient.so +SHA256=6D6FD3F20F0791F1CF11EEE5B1D479E2DCB6A1A2C8AB00A1165575BAB4B62813 +``` + +因此 `flyshot-uaes-interface/lib` 与 `FlyingShot/FlyingShot/Lib` 中的 Python client 可视为同一份接口。 + +## 暴露的 Python 类型 + +`PyControllerClient` 暴露以下类型： + +| 类型 | 来源 | 说明 | +| --- | --- | --- | +| `ControllerClient` | `ControllerClient.h` | TCP JSON client，高层控制入口 | +| `JointPositions` | `Types.h` | 关节位置容器，可用 6 维列表构造，也支持下标读写 | +| `Pose` | `Types.h` | TCP/末端位姿容器，C++ 侧为 7 元数组 | +| `JointLimits` | `Types.h` | 关节上下限、速度、加速度、jerk 容器 | +| `IOType` | `Types.h` | IO 枚举 | + +`IOType` 的枚举值： + +```text +IOType.kIOTypeDI = 1 +IOType.kIOTypeDO = 2 +IOType.kIOTypeRI = 8 +IOType.kIOTypeRO = 9 +``` + +## ControllerClient 暴露方法 + +二进制字符串和 C++ 公开头文件交叉确认，Python client 暴露的方法为： + +| Python 方法 | 典型调用 | 返回形态 | 说明 | +| --- | --- | --- | --- | +| `ConnectServer` | `c.ConnectServer(server_ip="127.0.0.1", port=50001)` | `bool` | 连接旧 `50001/TCP+JSON` 服务端 | +| `GetServerVersion` | `c.GetServerVersion()` | `str` | Python 包装层把 C++ out 参数折叠成返回值 | +| `GetClientVersion` | `c.GetClientVersion()` | `str` | 获取 client 版本 | +| `SetUpRobot` | `c.SetUpRobot("FANUC_LR_Mate_200iD")` | `bool` | 按机器人名称初始化服务端机器人模型 | +| `SetUpRobotFromEnv` | `c.SetUpRobotFromEnv(env_file)` | `bool` | 从环境文件初始化 | +| `IsSetUp` | `c.IsSetUp()` | `bool` | 判断服务端是否已经初始化机器人 | +| `SetShowTCP` | `c.SetShowTCP(is_show=True, axis_length=0.1, axis_size=2)` | `bool` | 仿真显示 TCP 坐标系 | +| `GetName` | `c.GetName()` | `str` | 获取机器人名称 | +| `GetDoF` | `c.GetDoF()` | `int` | 获取自由度 | +| `SetActiveController` | `c.SetActiveController(sim=True)` | `bool` | 切换仿真/真实控制器 | +| `Connect` | `c.Connect("192.168.10.101")` | `bool` | 连接机器人控制器 | +| `Disconnect` | `c.Disconnect()` | `bool` | 断开机器人控制器 | +| `EnableRobot` | `c.EnableRobot()` / `c.EnableRobot(8)` | `bool` | 使能机器人，参数为 buffer size | +| `DisableRobot` | `c.DisableRobot()` | `bool` | 下使能 | +| `GetSpeedRatio` | `c.GetSpeedRatio()` | `float` | 获取执行速度倍率 | +| `SetSpeedRatio` | `c.SetSpeedRatio(0.8)` | `bool` | 设置执行速度倍率 | +| `GetTCP` | `res, tcp = c.GetTCP()` | `(bool, Pose)` | 获取 TCP | +| `SetTCP` | `c.SetTCP(tcp)` | `bool` | 设置 TCP | +| `GetIO` | `res, value = c.GetIO(port=1, io_type=IOType.kIOTypeDI)` | `(bool, bool)` | 读取 IO | +| `SetIO` | `c.SetIO(port=1, value=True, io_type=IOType.kIOTypeDO)` | `bool` | 写 IO | +| `StopMove` | `c.StopMove()` | `bool` | 停止运动 | +| `GetJointPosition` | `res, joints = c.GetJointPosition()` | `(bool, JointPositions)` | 获取当前关节角 | +| `GetPose` | `res, pose = c.GetPose()` | `(bool, Pose)` | 获取当前末端位姿 | +| `GetNearestIK` | `res, ik = c.GetNearestIK(pose, joint_seed=joints)` | `(bool, JointPositions)` | 按 seed 求最近 IK | +| `MoveJoint` | `c.MoveJoint(joint_positions)` | `bool` | 关节运动 | +| `ExecuteTrajectory` | `c.ExecuteTrajectory(waypoints=[...], method="icsp", save_traj=True)` | `bool` | 执行普通轨迹 | +| `UploadFlyShotTraj` | `c.UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)` | `bool` | 上传飞拍轨迹 | +| `DeleteFlyShotTraj` | `c.DeleteFlyShotTraj(name)` | `bool` | 删除飞拍轨迹 | +| `ListFlyShotTraj` | `c.ListFlyShotTraj()` | `list[str]` | 列出已上传飞拍轨迹 | +| `ExecuteFlyShotTraj` | `c.ExecuteFlyShotTraj(name, move_to_start=True, method="icsp", save_traj=True)` | `bool` | 执行飞拍轨迹 | +| `SaveTrajInfo` | `c.SaveTrajInfo(name, method="icsp")` | `bool` | 保存规划结果到 `~/Rvbust/Data` | +| `IsFlyShotTrajValid` | `valid, time = c.IsFlyShotTrajValid(name, method="icsp", save_traj=True)` | `(bool, float)` | 检查飞拍轨迹是否合法并返回规划时长 | + +## 没有暴露的关键接口 + +本轮重点确认：Python client 暴露方法中没有看到： + +```text +GetJointLimits +SetJointLimits +_GetJointLimits +_SetJointLimits +``` + +虽然 `PyControllerClient` 绑定了 `JointLimits` 类型，并且 `libControllerClient.so` 中存在 `JointLimits` 的输出运算符符号，但公开 `ControllerClient` 方法表中没有任何接收或返回 `JointLimits` 的 client 入口。 + +这和旧服务端二进制不同。旧服务端 `ControllerServer.cpython-37m-x86_64-linux-gnu.so` 中能看到： + +```text +ControllerServer.ControllerServer._GetJointLimits +ControllerServer.ControllerServer._SetJointLimits +ControllerServer.ControllerServer._IsWaypointInJointLimits +ControllerServer.ControllerServer._IsTrajInJointLimits +ControllerServer.ControllerServer._IsTrajInJerkLimits +``` + +因此当前判断是： + +```text +Python client 公开 API 不能直接抓 runtime JointLimits； +runtime JointLimits 查询能力存在于旧服务端内部，而不是 PyControllerClient 公开接口中。 +``` + +## UAES Python 服务实际使用的接口 + +`../flyshot-uaes-interface/main.py` 只使用了公开 client 方法： + +- `ConnectServer` +- `SetUpRobot` +- `IsSetUp` +- `EnableRobot` +- `DisableRobot` +- `SetActiveController` +- `Connect` +- `GetName` +- `GetServerVersion` +- `GetDoF` +- `GetSpeedRatio` +- `SetTCP` +- `GetTCP` +- `SetIO` +- `GetJointPosition` +- `MoveJoint` +- `ListFlyShotTraj` +- `UploadFlyShotTraj` +- `ExecuteFlyShotTraj` +- `SetSpeedRatio` +- `DeleteFlyShotTraj` +- `GetPose` + +其中 `/execute_flyshot/` 调用： + +```text +c.ExecuteFlyShotTraj(name=name, move_to_start=True, method="icsp", save_traj=True) +``` + +`/set_speedRatio/` 调用： + +```text +c.SetSpeedRatio(speed) +``` + +没有看到 UAES 服务通过 Python client 设置或查询 `JointLimits`。 + +2026-04-30 追加 `50001/TCP+JSON` 抓包复核后，这个判断进一步收敛。`all-50001-plan.pcap` 中已经抓到两次真实规划/执行请求： + +```json +{"cmd":"SetSpeedRatio","ratio":0.5} +{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true} +{"cmd":"SetSpeedRatio","ratio":1.0} +{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true} +``` + +请求中仍没有 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此公开 Python client 与公开 50001 JSON 都没有把规划限制作为参数传给 `ExecuteFlyShotTraj`。 + +另外，`main.py` 的 `/execute_trajectory/` 中出现： + +```text +c.yrxm(waypoints=joint_positions, method='icsp', save_traj=True) +``` + +`yrxm` 不在 `PyControllerClient` 暴露方法表中，按上下文应是 `ExecuteTrajectory` 的笔误；这条不影响飞拍主路径 `/execute_flyshot/`。 + +## 对当前时长差异调查的含义 + +如果要抓旧系统规划时使用的 effective `vel/acc/jerk`，优先级应调整为： + +1. 在旧服务端进程内直接调用或插桩 `_GetJointLimits`。 +2. 或者逆向 `50001/TCP+JSON` 的 hidden command envelope，再尝试发送 `GetJointLimits` / `_GetJointLimits`。 +3. 不应指望现有 `PyControllerClient.ControllerClient` 直接提供 `GetJointLimits`。 + +如果短期内无法进入旧服务端内部，新系统不再继续等待这份不可见状态；设计上使用 replacement-only 的内部规划约束参数补齐，优先限制规划加速度，例如 `planning_acceleration_scale`。该参数必须标注为新系统校准值，不能写成旧 Python client 或旧 50001 JSON 的公开字段。 + +最小现场验证脚本可以先确认 Python client 暴露面： + +```python +from PyControllerClient import ControllerClient + +c = ControllerClient() +names = [x for x in dir(c) if "Limit" in x or "limit" in x] +print(names) +``` + +按当前二进制逆向，预期不会出现 `GetJointLimits` / `SetJointLimits`。 diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index 8f3b20f..f65e97e 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -373,18 +373,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi { var robot = RequireActiveRobot(); EnsureRuntimeEnabled(); - var currentJointPositions = _runtime.GetJointPositions(); - EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions)); - EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions)); - - var speedRatio = _runtime.GetSnapshot().SpeedRatio; - var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio, _logger); - _logger?.LogInformation( - "MoveJoint 规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}", - speedRatio, - moveResult.Duration.TotalSeconds, - moveResult.DenseJointTrajectory?.Count ?? 0); - _runtime.ExecuteTrajectory(moveResult, jointPositions); + ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint"); } _logger?.LogInformation("MoveJoint 完成"); @@ -480,8 +469,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } _logger?.LogInformation( - "ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}", - name, options.Method, options.MoveToStart, options.UseCache); + "ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}", + name, options.Method, options.MoveToStart, options.UseCache, options.Wait); lock (_stateLock) { @@ -516,16 +505,83 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi if (options.MoveToStart) { _logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点"); - _runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions); + ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start"); } var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions; _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions); + if (options.Wait) + { + WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration); + } } _logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name); } + /// + /// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。 + /// + /// 当前机器人模型。 + /// 目标关节位置，单位为弧度。 + /// 用于日志和超时异常的操作名。 + private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList targetJointPositions, string operationName) + { + var currentJointPositions = _runtime.GetJointPositions(); + EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions)); + EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions)); + + var speedRatio = _runtime.GetSnapshot().SpeedRatio; + var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger); + _logger?.LogInformation( + "{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}", + operationName, + speedRatio, + moveResult.Duration.TotalSeconds, + moveResult.DenseJointTrajectory?.Count ?? 0); + + _runtime.ExecuteTrajectory(moveResult, targetJointPositions); + WaitForRuntimeMotionComplete(operationName, moveResult.Duration); + } + + /// + /// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。 + /// + /// 用于日志和超时异常的操作名。 + /// 规划运动时长。 + private void WaitForRuntimeMotionComplete(string operationName, TimeSpan plannedDuration) + { + var timeout = ResolveMotionCompletionTimeout(plannedDuration); + var deadline = DateTimeOffset.UtcNow.Add(timeout); + + while (true) + { + if (!_runtime.GetSnapshot().IsInMotion) + { + _logger?.LogInformation("{OperationName} 运动完成", operationName); + return; + } + + if (DateTimeOffset.UtcNow >= deadline) + { + throw new TimeoutException($"{operationName} 等待运动完成超时，planned={plannedDuration.TotalSeconds:F3}s, timeout={timeout.TotalSeconds:F3}s。"); + } + + Thread.Sleep(TimeSpan.FromMilliseconds(10)); + } + } + + /// + /// 根据规划时长推导等待超时，给真机通信和状态更新留出余量。 + /// + /// 规划运动时长。 + /// 等待运行时完成的最大时长。 + private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration) + { + var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0); + return TimeSpan.FromSeconds(timeoutSeconds); + } + /// public void SaveTrajectoryInfo(string name, string method = "icsp") { @@ -703,26 +759,6 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } } - /// - /// 构造无需稠密轨迹的最小合法结果，供仍需单点状态更新的兼容路径使用。 - /// - /// 可立即执行的轨迹结果。 - private static TrajectoryResult CreateImmediateMoveResult() - { - return new TrajectoryResult( - programName: "move-joint", - method: PlanningMethod.Icsp, - isValid: true, - duration: TimeSpan.Zero, - shotEvents: Array.Empty(), - triggerTimeline: Array.Empty(), - artifacts: Array.Empty(), - failureReason: null, - usedCache: false, - originalWaypointCount: 1, - plannedWaypointCount: 1); - } - /// /// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。 /// diff --git a/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs b/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs index 14b4f80..67aa6b7 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs @@ -12,16 +12,19 @@ public sealed class FlyshotExecutionOptions /// 轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。 /// 是否保存轨迹信息。 /// 是否优先复用已规划轨迹缓存。 + /// 是否等待机器人执行完整条飞拍轨迹后再返回。 public FlyshotExecutionOptions( bool moveToStart = true, string method = "icsp", bool saveTrajectory = true, - bool useCache = true) + bool useCache = true, + bool wait = true) { MoveToStart = moveToStart; Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method; SaveTrajectory = saveTrajectory; UseCache = useCache; + Wait = wait; } /// @@ -43,4 +46,9 @@ public sealed class FlyshotExecutionOptions /// 获取是否优先复用已规划轨迹缓存。 /// public bool UseCache { get; } + + /// + /// 获取是否等待机器人执行完整条飞拍轨迹后再返回。 + /// + public bool Wait { get; } } diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index cf26d17..00e0ad7 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -353,9 +353,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable if (!IsSimulationMode) { var frame = GetFreshStateFrame(); - if (frame?.JointDegrees.Count >= _jointPositions.Length) + if (frame?.JointRadians.Count >= _jointPositions.Length) { - return frame.JointDegrees.Take(_jointPositions.Length).Select(v => (double)v).ToArray(); + return frame.JointRadians.Take(_jointPositions.Length).Select(v => (double)v).ToArray(); } } @@ -403,9 +403,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable var frame = GetFreshStateFrame(); if (frame is not null) { - if (frame.JointDegrees.Count >= jointPositions.Length) + if (frame.JointRadians.Count >= jointPositions.Length) { - jointPositions = frame.JointDegrees.Take(jointPositions.Length).Select(v => (double)v).ToArray(); + jointPositions = frame.JointRadians.Take(jointPositions.Length).Select(v => (double)v).ToArray(); } if (frame.CartesianPose.Count >= 6) diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs index cbd1602..140a892 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs @@ -1,11 +1,10 @@ -using System.Diagnostics; using System.Net.Sockets; using Microsoft.Extensions.Logging; namespace Flyshot.Runtime.Fanuc.Protocol; /// -/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供周期命令发送与响应接收能力。 +/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供状态包驱动的命令发送与响应接收能力。 /// public sealed class FanucJ519Client : IDisposable { @@ -14,13 +13,11 @@ public sealed class FanucJ519Client : IDisposable private readonly ILogger? _logger; private UdpClient? _udpClient; private CancellationTokenSource? _cts; - private CancellationTokenSource? _sendCts; - private Task? _sendTask; private Task? _receiveTask; private FanucJ519Command? _currentCommand; private List? _commandHistoryForTests; private FanucJ519Response? _latestResponse; - private uint _nextSequence; + private bool _motionStarted; private bool _disposed; /// @@ -71,7 +68,7 @@ public sealed class FanucJ519Client : IDisposable } /// - /// 启动约 8ms 周期的 J519 命令发送循环。 + /// 启动 J519 命令发送许可；实际发包由机器人状态包节拍驱动。 /// public void StartMotion() { @@ -82,19 +79,22 @@ public sealed class FanucJ519Client : IDisposable throw new InvalidOperationException("J519 通道未连接。"); } - if (_sendTask is not null) + lock (_commandLock) { - _logger?.LogDebug("J519 StartMotion: 发送循环已在运行"); - return; // 已在运行。 + if (_motionStarted) + { + _logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用"); + return; + } + + _motionStarted = true; } - _logger?.LogInformation("J519 StartMotion: 启动发送循环"); - _sendCts = CancellationTokenSource.CreateLinkedTokenSource(_cts!.Token); - _sendTask = Task.Run(() => SendLoopAsync(_sendCts.Token), _sendCts.Token); + _logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送"); } /// - /// 发送结束包并停止 J519 命令发送循环。 + /// 发送状态输出停止包并停止 J519 命令发送。 /// public async Task StopMotionAsync(CancellationToken cancellationToken = default) { @@ -105,34 +105,15 @@ public sealed class FanucJ519Client : IDisposable return; } - _logger?.LogInformation("J519 StopMotionAsync: 停止发送循环"); - - if (_sendTask is not null) + _logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送"); + lock (_commandLock) { - _sendCts?.Cancel(); - - try - { - await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false); - } - catch (TimeoutException) - { - _logger?.LogWarning("J519 StopMotionAsync: 发送循环未能在 1 秒内结束"); - } - catch (OperationCanceledException) - { - // 正常取消。 - } - - _sendTask = null; + _motionStarted = false; } - _sendCts?.Dispose(); - _sendCts = null; - - // 发送结束包通知控制器停止伺服流。 + // FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。 await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false); - _logger?.LogInformation("J519 StopMotionAsync: 结束包已发送"); + _logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送"); } /// @@ -219,23 +200,8 @@ public sealed class FanucJ519Client : IDisposable { ObjectDisposedException.ThrowIf(_disposed, this); - _sendCts?.Cancel(); _cts?.Cancel(); - try - { - _sendTask?.Wait(TimeSpan.FromSeconds(1)); - } - catch (AggregateException) - { - // 忽略取消异常。 - } - - _sendTask?.Dispose(); - _sendTask = null; - _sendCts?.Dispose(); - _sendCts = null; - try { _receiveTask?.Wait(TimeSpan.FromSeconds(1)); @@ -258,7 +224,7 @@ public sealed class FanucJ519Client : IDisposable { _currentCommand = null; _commandHistoryForTests = null; - _nextSequence = 0; + _motionStarted = false; } lock (_responseLock) @@ -278,18 +244,8 @@ public sealed class FanucJ519Client : IDisposable } _disposed = true; - _sendCts?.Cancel(); _cts?.Cancel(); - try - { - _sendTask?.Wait(TimeSpan.FromSeconds(1)); - } - catch (AggregateException) - { - // 忽略取消异常。 - } - try { _receiveTask?.Wait(TimeSpan.FromSeconds(1)); @@ -299,59 +255,11 @@ public sealed class FanucJ519Client : IDisposable // 忽略取消异常。 } - _sendTask?.Dispose(); _receiveTask?.Dispose(); - _sendCts?.Dispose(); _cts?.Dispose(); _udpClient?.Dispose(); } - /// - /// 后台发送循环：以 Stopwatch + SpinWait 实现高精度 8ms 周期发送当前命令。 - /// - private async Task SendLoopAsync(CancellationToken cancellationToken) - { - if (_udpClient is null) - { - return; - } - - // 8ms 伺服周期，对应 125Hz。 - var periodTicks = (long)(0.008 * Stopwatch.Frequency); - var stopwatch = Stopwatch.StartNew(); - long nextTick = stopwatch.ElapsedTicks; - - try - { - while (!cancellationToken.IsCancellationRequested) - { - nextTick += periodTicks; - - FanucJ519Command? command; - lock (_commandLock) - { - command = _currentCommand is null ? null : WithSequence(_currentCommand, _nextSequence++); - } - - if (command is not null) - { - var packet = FanucJ519Protocol.PackCommandPacket(command); - await _udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false); - } - - // 高精度忙等待直到下一周期，避免 PeriodicTimer 的 ±15ms 抖动。 - while (stopwatch.ElapsedTicks < nextTick) - { - Thread.SpinWait(1); - } - } - } - catch (OperationCanceledException) - { - // 正常取消，退出循环。 - } - } - private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence) { return new FanucJ519Command( @@ -396,6 +304,7 @@ public sealed class FanucJ519Client : IDisposable } receiveCount++; + await SendCommandForStatusAsync(response, cancellationToken).ConfigureAwait(false); // 仅在状态变化时记录 Info，避免高频日志。 if (lastLoggedResponse is null @@ -430,4 +339,34 @@ public sealed class FanucJ519Client : IDisposable _logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount); } } + + /// + /// 按机器人状态包的 sequence 立即回发当前 J519 命令。 + /// + /// 刚收到的状态包。 + /// 取消令牌。 + private async Task SendCommandForStatusAsync(FanucJ519Response response, CancellationToken cancellationToken) + { + var udpClient = _udpClient; + if (udpClient is null) + { + return; + } + + FanucJ519Command? command; + lock (_commandLock) + { + command = !_motionStarted || _currentCommand is null + ? null + : WithSequence(_currentCommand, response.Sequence); + } + + if (command is null) + { + return; + } + + var packet = FanucJ519Protocol.PackCommandPacket(command); + await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false); + } } diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs index 52df1c2..60883bf 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs @@ -9,7 +9,7 @@ public sealed class FanucStateFrame { private readonly double[] _pose; private readonly double[] _jointOrExtensionValues; - private readonly double[] _jointDegrees; + private readonly double[] _jointRadians; private readonly double[] _externalAxes; private readonly uint[] _tailWords; @@ -46,7 +46,7 @@ public sealed class FanucStateFrame throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords)); } - _jointDegrees = _jointOrExtensionValues.Take(6).ToArray(); + _jointRadians = _jointOrExtensionValues.Take(6).ToArray(); _externalAxes = _jointOrExtensionValues.Skip(6).ToArray(); } @@ -71,9 +71,9 @@ public sealed class FanucStateFrame public IReadOnlyList JointOrExtensionValues => _jointOrExtensionValues; /// - /// 获取前 6 个机器人关节角度，单位为度。 + /// 获取前 6 个机器人关节角度，当前现场抓包更支持按弧度制理解。 /// - public IReadOnlyList JointDegrees => _jointDegrees; + public IReadOnlyList JointRadians => _jointRadians; /// /// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。 diff --git a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs index 7cdfa47..5a6e700 100644 --- a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs +++ b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs @@ -564,8 +564,8 @@ public sealed class LegacyHttpApiController : ControllerBase public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data) { _logger.LogInformation( - "ExecuteFlyshot 调用: name={Name}, method={Method}, move_to_start={MoveToStart}, use_cache={UseCache}", - data.name, data.method, data.move_to_start, data.use_cache); + "ExecuteFlyshot 调用: name={Name}, method={Method}, move_to_start={MoveToStart}, use_cache={UseCache}, wait={Wait}", + data.name, data.method, data.move_to_start, data.use_cache, data.wait); try { _compatService.ExecuteTrajectoryByName( @@ -574,7 +574,8 @@ public sealed class LegacyHttpApiController : ControllerBase moveToStart: data.move_to_start, method: data.method, saveTrajectory: data.save_traj, - useCache: data.use_cache)); + useCache: data.use_cache, + wait: data.wait)); _logger.LogInformation("ExecuteFlyshot 成功: name={Name}", data.name); return Ok(new { status = "FlyShot executed", success = true }); } @@ -944,6 +945,11 @@ public sealed class LegacyExecuteFlyshotRequest /// 获取或设置是否复用轨迹缓存。 /// public bool use_cache { get; init; } = true; + + /// + /// 获取或设置是否等待机器人执行完整条飞拍轨迹后再返回。 + /// + public bool wait { get; init; } = true; } /// diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs index 45094a9..3c9c65b 100644 --- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs +++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs @@ -6,7 +6,7 @@ using Flyshot.Runtime.Fanuc.Protocol; namespace Flyshot.Core.Tests; /// -/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、周期发送与响应解析。 +/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、状态包驱动发送与响应解析。 /// public sealed class FanucJ519ClientTests : IDisposable { @@ -54,25 +54,24 @@ public sealed class FanucJ519ClientTests : IDisposable } /// - /// 验证启动运动后能按周期发送命令包。 + /// 验证启动运动后必须等到状态包到达，不能由上位机本地 8ms 循环主动发命令。 /// [Fact] - public async Task StartMotion_SendsPeriodicCommands() + public async Task StartMotion_WaitsForStatusPacketBeforeSendingCommand() { using var client = new FanucJ519Client(); await client.ConnectAsync("127.0.0.1", Port, _cts.Token); // 接收并丢弃初始化包。 - var initResult = await _server.ReceiveAsync(_cts.Token); + await _server.ReceiveAsync(_cts.Token); var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); client.UpdateCommand(command); client.StartMotion(); - // 接收至少一个命令包。 - var commandResult = await _server.ReceiveAsync(_cts.Token); - Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length); - Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4))); + // 机器人尚未回状态包时，上位机不应自行发 64B command packet。 + await Assert.ThrowsAsync( + () => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120))); await client.StopMotionAsync(_cts.Token); } @@ -140,51 +139,47 @@ public sealed class FanucJ519ClientTests : IDisposable } /// - /// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。 + /// 验证收到状态包后，下一帧命令使用该状态包的序号。 /// [Fact] - public async Task UpdateCommand_ReplacesCurrentCommand() + public async Task StartMotion_UsesLatestStatusSequenceForFirstCommand() { using var client = new FanucJ519Client(); await client.ConnectAsync("127.0.0.1", Port, _cts.Token); - await _server.ReceiveAsync(_cts.Token); // init + var initResult = await _server.ReceiveAsync(_cts.Token); - var command1 = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]); - var command2 = new FanucJ519Command(sequence: 2, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]); - - client.UpdateCommand(command1); + var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + client.UpdateCommand(command); client.StartMotion(); - var result1 = await _server.ReceiveAsync(_cts.Token); - Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4))); - Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6); + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100); - client.UpdateCommand(command2); - - var result2 = await _server.ReceiveAsync(_cts.Token); - Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4))); - Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6); + var result = await _server.ReceiveAsync(_cts.Token); + Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length); + Assert.Equal(100u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4))); + Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result.Buffer.AsSpan(0x1c, 4)), precision: 6); await client.StopMotionAsync(_cts.Token); } /// - /// 验证重复保持同一命令时实际 J519 包序号仍按客户端全局递增。 + /// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。 /// [Fact] - public async Task StartMotion_IncrementsSequenceForRepeatedHoldPackets() + public async Task StartMotion_SendsOneCommandForEachStatusPacketWithMatchingSequence() { using var client = new FanucJ519Client(); await client.ConnectAsync("127.0.0.1", Port, _cts.Token); - await _server.ReceiveAsync(_cts.Token); // init + var initResult = await _server.ReceiveAsync(_cts.Token); var command = new FanucJ519Command(sequence: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]); client.UpdateCommand(command); client.StartMotion(); var packets = new List(); - for (var index = 0; index < 4; index++) + for (uint sequence = 100; sequence < 104; sequence++) { + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence); var result = await _server.ReceiveAsync(_cts.Token); packets.Add(result.Buffer); } @@ -194,26 +189,27 @@ public sealed class FanucJ519ClientTests : IDisposable var sequences = packets .Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4))) .ToArray(); - Assert.Equal([0u, 1u, 2u, 3u], sequences); + Assert.Equal([100u, 101u, 102u, 103u], sequences); Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6)); } /// - /// 验证停止运动后可在同一连接内重启发送，且包序号不重置。 + /// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。 /// [Fact] - public async Task StartMotion_CanRestartAfterStopMotionWithoutResettingSequence() + public async Task StartMotion_CanRestartAfterStopMotionAndUseNewStatusSequence() { using var client = new FanucJ519Client(); await client.ConnectAsync("127.0.0.1", Port, _cts.Token); - await _server.ReceiveAsync(_cts.Token); // init + var initResult = await _server.ReceiveAsync(_cts.Token); client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0])); client.StartMotion(); + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 200); var first = await _server.ReceiveAsync(_cts.Token); var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4)); - Assert.Equal(0u, firstSequence); + Assert.Equal(200u, firstSequence); await client.StopMotionAsync(_cts.Token); @@ -227,10 +223,11 @@ public sealed class FanucJ519ClientTests : IDisposable client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0])); client.StartMotion(); + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 300); var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token); Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length); - Assert.True(BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)) > firstSequence); + Assert.Equal(300u, BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4))); Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6); await client.StopMotionAsync(_cts.Token); @@ -247,14 +244,14 @@ public sealed class FanucJ519ClientTests : IDisposable } /// - /// 验证发送循环能持续按协议周期输出命令包。 + /// 验证状态包驱动发送能持续输出命令包。 /// [Fact] - public async Task StartMotion_MaintainsPeriodicCommandStream() + public async Task StartMotion_MaintainsStatusDrivenCommandStream() { using var client = new FanucJ519Client(); await client.ConnectAsync("127.0.0.1", Port, _cts.Token); - await _server.ReceiveAsync(_cts.Token); // init + var initResult = await _server.ReceiveAsync(_cts.Token); var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); client.UpdateCommand(command); @@ -265,6 +262,7 @@ public sealed class FanucJ519ClientTests : IDisposable var sequences = new List(); for (var i = 0; i < 5; i++) { + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: (uint)(500 + i)); var result = await _server.ReceiveAsync(_cts.Token); Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length); sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4))); @@ -273,7 +271,7 @@ public sealed class FanucJ519ClientTests : IDisposable await client.StopMotionAsync(_cts.Token); - Assert.Equal([0u, 1u, 2u, 3u, 4u], sequences); + Assert.Equal([500u, 501u, 502u, 503u, 504u], sequences); // 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。 var intervals = new List(); @@ -288,4 +286,17 @@ public sealed class FanucJ519ClientTests : IDisposable Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。"); }); } + + /// + /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。 + /// + private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence) + { + 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] = 15; + await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token); + } } diff --git a/tests/Flyshot.Core.Tests/FanucProtocolTests.cs b/tests/Flyshot.Core.Tests/FanucProtocolTests.cs index 8c34409..9f2eec3 100644 --- a/tests/Flyshot.Core.Tests/FanucProtocolTests.cs +++ b/tests/Flyshot.Core.Tests/FanucProtocolTests.cs @@ -111,7 +111,7 @@ public sealed class FanucProtocolTests Assert.Equal(9, frame.JointOrExtensionValues.Count); Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords); Assert.Equal(frame.Pose, frame.CartesianPose); - Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointDegrees); + Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointRadians); Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes); Assert.Equal(frame.TailWords, frame.RawTailWords); Assert.Equal(2u, frame.StatusWord0); @@ -135,7 +135,7 @@ public sealed class FanucProtocolTests Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length); Assert.Equal(6, frame.CartesianPose.Count); - Assert.Equal(6, frame.JointDegrees.Count); + Assert.Equal(6, frame.JointRadians.Count); Assert.Equal(3, frame.ExternalAxes.Count); Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords); } diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index 55c8816..4b3583c 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -293,6 +293,92 @@ public sealed class RuntimeOrchestrationTests Assert.Throws(Act); } + /// + /// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。 + /// + [Fact] + public void ControllerClientCompatService_ExecuteTrajectoryByName_MoveToStartWaitsBeforeFlyshot() + { + var configRoot = CreateTempConfigRoot(); + try + { + var options = new ControllerClientCompatOptions + { + ConfigRoot = configRoot + }; + var runtime = new DelayedCompletionControllerRuntime( + initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0], + firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80)); + var service = new ControllerClientCompatService( + options, + new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), + runtime, + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + service.SetActiveController(sim: false); + service.Connect("192.168.10.101"); + service.EnableRobot(2); + service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot()); + + service.ExecuteTrajectoryByName( + "demo-flyshot", + new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false)); + + Assert.True(runtime.ExecuteCalls.Count >= 2); + Assert.NotNull(runtime.ExecuteCalls[0].Result.DenseJointTrajectory); + Assert.True(runtime.ExecuteCalls[0].Result.DenseJointTrajectory!.Count > 1); + Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted); + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + + /// + /// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。 + /// + [Fact] + public void ControllerClientCompatService_ExecuteTrajectoryByName_WaitTrueWaitsForFlyshotCompletion() + { + var configRoot = CreateTempConfigRoot(); + try + { + var options = new ControllerClientCompatOptions + { + ConfigRoot = configRoot + }; + var runtime = new DelayedCompletionControllerRuntime( + initialJointPositions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80)); + var service = new ControllerClientCompatService( + options, + new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), + runtime, + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + service.SetActiveController(sim: false); + service.Connect("192.168.10.101"); + service.EnableRobot(2); + service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot()); + + service.ExecuteTrajectoryByName( + "demo-flyshot", + new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)); + + Assert.Single(runtime.ExecuteCalls); + Assert.False(runtime.GetSnapshot().IsInMotion); + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + /// /// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。 /// @@ -650,3 +736,164 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime { } } + +/// +/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。 +/// +internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime +{ + private readonly object _lock = new(); + private readonly TimeSpan _firstMotionCompletionDelay; + private double[] _jointPositions; + private bool _isEnabled; + private bool _isInMotion; + private bool _firstMotionCompleted; + + /// + /// 初始化可延迟完成第一段运动的测试运行时。 + /// + /// 运行时报告的初始关节位置。 + /// 第一段运动完成前保持忙碌的时间。 + public DelayedCompletionControllerRuntime( + IReadOnlyList initialJointPositions, + TimeSpan firstMotionCompletionDelay) + { + _jointPositions = initialJointPositions.ToArray(); + _firstMotionCompletionDelay = firstMotionCompletionDelay; + } + + /// + /// 获取所有 ExecuteTrajectory 调用记录。 + /// + public List<(TrajectoryResult Result, IReadOnlyList FinalJointPositions)> ExecuteCalls { get; } = []; + + /// + /// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。 + /// + public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; } + + /// + public void ResetRobot(RobotProfile robot, string robotName) + { + } + + /// + public void SetActiveController(bool sim) + { + } + + /// + public void Connect(string robotIp) + { + } + + /// + public void Disconnect() + { + } + + /// + public void EnableRobot(int bufferSize) + { + _isEnabled = true; + } + + /// + public void DisableRobot() + { + _isEnabled = false; + } + + /// + public void StopMove() + { + lock (_lock) + { + _isInMotion = false; + } + } + + /// + public double GetSpeedRatio() => 1.0; + + /// + public void SetSpeedRatio(double ratio) + { + } + + /// + public IReadOnlyList GetTcp() => [0.0, 0.0, 0.0]; + + /// + public void SetTcp(double x, double y, double z) + { + } + + /// + public bool GetIo(int port, string ioType) => false; + + /// + public void SetIo(int port, bool value, string ioType) + { + } + + /// + public IReadOnlyList GetJointPositions() + { + lock (_lock) + { + return _jointPositions.ToArray(); + } + } + + /// + public IReadOnlyList GetPose() => Array.Empty(); + + /// + public ControllerStateSnapshot GetSnapshot() + { + lock (_lock) + { + return new ControllerStateSnapshot( + capturedAt: DateTimeOffset.UtcNow, + connectionState: "Connected", + isEnabled: _isEnabled, + isInMotion: _isInMotion, + speedRatio: 1.0, + jointPositions: _jointPositions.ToArray(), + cartesianPose: Array.Empty(), + activeAlarms: Array.Empty()); + } + } + + /// + public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions) + { + lock (_lock) + { + ExecuteCalls.Add((result, finalJointPositions.ToArray())); + if (ExecuteCalls.Count == 1) + { + _isInMotion = true; + _ = Task.Run(async () => + { + await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false); + lock (_lock) + { + _jointPositions = finalJointPositions.ToArray(); + _isInMotion = false; + _firstMotionCompleted = true; + } + }); + return; + } + + if (!_firstMotionCompleted) + { + SecondTrajectoryStartedBeforeFirstMotionCompleted = true; + } + + _jointPositions = finalJointPositions.ToArray(); + } + } +} diff --git a/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs b/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs index 1baacdb..89ff82e 100644 --- a/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs +++ b/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs @@ -229,7 +229,8 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory move_to_start = true, method = "icsp", save_traj = true, - use_cache = true + use_cache = true, + wait = true })) { Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);