✨ feat(compat): 补齐飞拍执行等待与 FANUC 状态驱动链路
- 为 ExecuteFlyShotTraj 补齐 wait 语义，并让 move_to_start 先完成临时 PTP 运动后再启动正式飞拍轨迹 - 将 J519 命令发送改为由机器人 UDP status sequence 驱动， 避免在未收到状态包时主动发周期命令 - 将 10010 状态通道关节字段统一按 JointRadians 命名， 同步更新运行时读取逻辑与协议测试 - 新增 FANUC 10010 状态帧、流运动手册和 Python client 逆向文档，并更新 README 与兼容需求说明 - 补充兼容层编排测试与 HTTP 集成测试，覆盖 wait 和 move_to_start 串行化行为
This commit is contained in:
34
README.md
34
README.md
@@ -19,10 +19,34 @@
|
|||||||
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。
|
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。
|
||||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。
|
- `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` 轨迹。
|
- 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` 下发。
|
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹：当前关节到目标关节的关节空间直线，五次 smoothstep 起停，按 `status=15` 运动窗口复现 `40/55/77` 点，并由 J519 层完成 `rad -> deg` 下发。
|
||||||
- 单程序只对应一台机器人，上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`，不再创建独立轨迹存储文件。
|
- 单程序只对应一台机器人，上传/删除/恢复飞拍轨迹统一读写运行目录 `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 会话。
|
- 建议从 `flyshot-replacement/` 根目录启动 IDE、终端和 Codex 会话。
|
||||||
@@ -58,6 +82,7 @@
|
|||||||
2. 轨迹规划
|
2. 轨迹规划
|
||||||
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
||||||
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
||||||
|
- [ ] 新增 replacement-only 的 `planning_acceleration_scale` 规划加速度校准参数，用于复现旧服务端公开链路中抓不到的保守 effective limits；该参数只影响规划结果，不影响运行时 `speed_ratio`。
|
||||||
- [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。
|
- [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。
|
||||||
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。
|
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。
|
||||||
- [x] 将 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本：输入为 `rad`，下发为 `deg`，并按 `speed_ratio` 拉伸时间轴；覆盖 `2026042802-0.5/0.7/1.pcap`。
|
- [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 状态通道
|
4. FANUC TCP 10010 状态通道
|
||||||
- [x] 用 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
- [x] 用 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||||
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
||||||
|
- [x] 补充 `TCP 10010` 正式字段表与已确认/待确认说明：见 `docs/fanuc-10010-state-frame.md`。
|
||||||
- [x] 补充断线清理和异常帧拒绝测试。
|
- [x] 补充断线清理和异常帧拒绝测试。
|
||||||
- [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。
|
- [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。
|
||||||
|
|
||||||
5. FANUC UDP 60015 J519 运动链路
|
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] 执行时将规划输出 `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] 补齐 `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] 将飞拍 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] 将 `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. 真机联调与运行安全
|
6. 真机联调与运行安全
|
||||||
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。
|
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。
|
||||||
|
|||||||
@@ -123,6 +123,7 @@
|
|||||||
- 先做兼容测试矩阵，再补最小命令桩
|
- 先做兼容测试矩阵，再补最小命令桩
|
||||||
- 区分“旧系统事实”和“replacement 当前策略”
|
- 区分“旧系统事实”和“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 实现状态
|
## 8. 当前 replacement 实现状态
|
||||||
|
|
||||||
@@ -131,10 +132,11 @@
|
|||||||
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。
|
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。
|
||||||
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
|
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
|
||||||
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
|
- `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。
|
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
||||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
|
- `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 运行包。
|
- 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`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
||||||
|
|||||||
165
docs/fanuc-10010-state-frame.md
Normal file
165
docs/fanuc-10010-state-frame.md
Normal file
@@ -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]` 去驱动明确业务判断，除非后续拿到新的现场对照证据。
|
||||||
@@ -23,7 +23,7 @@ POST /init_mpc_robt
|
|||||||
2. `SetUpRobot(robot_name)`：加载机器人配置、关节限制和伺服周期。
|
2. `SetUpRobot(robot_name)`：加载机器人配置、关节限制和伺服周期。
|
||||||
3. `SetActiveController(sim)`：选择仿真或 FANUC 真机运行时。
|
3. `SetActiveController(sim)`：选择仿真或 FANUC 真机运行时。
|
||||||
4. `Connect(robot_ip)`：真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
|
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. 参数设置
|
## 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
|
```bash
|
||||||
@@ -44,7 +53,7 @@ POST /set_speedRatio/
|
|||||||
{ "speed": 0.7 }
|
{ "speed": 0.7 }
|
||||||
```
|
```
|
||||||
|
|
||||||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。J519 执行时仍必须按该倍率重采样轨迹时间轴：
|
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍必须按该倍率重采样轨迹时间轴：
|
||||||
|
|
||||||
```text
|
```text
|
||||||
t_traj = k * 0.008 * speed_ratio
|
t_traj = k * 0.008 * speed_ratio
|
||||||
@@ -121,18 +130,21 @@ POST /execute_flyshot/
|
|||||||
"move_to_start": true,
|
"move_to_start": true,
|
||||||
"method": "self-adapt-icsp",
|
"method": "self-adapt-icsp",
|
||||||
"save_traj": false,
|
"save_traj": false,
|
||||||
"use_cache": true
|
"use_cache": true,
|
||||||
|
"wait": true
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|
||||||
执行链路：
|
执行链路：
|
||||||
|
|
||||||
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
||||||
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹。
|
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹；规划阶段先应用 `acc_limit / jerk_limit`，再应用 replacement-only 的规划加速度校准参数。
|
||||||
3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。
|
3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。
|
||||||
4. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 周期重采样并转成 `deg`。
|
4. 若 `move_to_start=true`，先从运行时当前关节位置生成临时 PTP 稠密轨迹移动到规划轨迹起点，并等待运行时 `IsInMotion=false` 后再启动飞拍轨迹，避免第一帧 J519 目标从当前位置跳到起点。
|
||||||
5. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪，则拒绝执行。
|
5. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 轨迹时间步长重采样并转成 `deg`，命令实际发包由机器人 UDP status 包驱动。
|
||||||
6. 周期命令中嵌入 IO 脉冲；当前 UTTC 抓包确认 mask 集合为 `10/12/14`，共 17 个 set 脉冲和 17 个 clear 帧。
|
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`。
|
`method="doubles"` 当前明确返回未实现；现场主链路使用 `icsp` / `self-adapt-icsp`。
|
||||||
|
|
||||||
@@ -146,8 +158,8 @@ POST /disconnect_robot/
|
|||||||
|
|
||||||
真机模式下：
|
真机模式下：
|
||||||
|
|
||||||
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 发送循环。
|
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 状态包驱动发送。
|
||||||
- `DisableRobot()` 发送 J519 end 控制包，然后 `StopProg("RVBUSTSM")`。
|
- `DisableRobot()` 发送 J519 packet type 2 状态输出停止包，然后 `StopProg("RVBUSTSM")`。
|
||||||
- `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。
|
- `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。
|
||||||
|
|
||||||
## 6. 现场抓包覆盖
|
## 6. 现场抓包覆盖
|
||||||
@@ -162,8 +174,8 @@ POST /disconnect_robot/
|
|||||||
|
|
||||||
测试同时检查：
|
测试同时检查：
|
||||||
|
|
||||||
- 主运行窗口命令序号连续，无重复 seq。
|
- 主运行窗口命令序号连续，无重复 seq；J519 客户端单元测试覆盖按最新 status sequence 回发命令。
|
||||||
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
|
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
|
||||||
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。
|
- 实发点位相对重采样期望的全局 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` 与抓包一致。
|
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
|
||||||
|
|||||||
232
docs/fanuc-stream-motion-manual-notes.md
Normal file
232
docs/fanuc-stream-motion-manual-notes.md
Normal file
@@ -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 手册算法靠近。
|
||||||
591
docs/planning-duration-mismatch-investigation.md
Normal file
591
docs/planning-duration-mismatch-investigation.md
Normal file
@@ -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`，再用返回值替换当前反推参数；在此之前，显式内部参数是当前可控且可审计的兼容策略。
|
||||||
202
docs/python-client-interface-reverse-engineering.md
Normal file
202
docs/python-client-interface-reverse-engineering.md
Normal file
@@ -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`。
|
||||||
@@ -373,18 +373,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
{
|
{
|
||||||
var robot = RequireActiveRobot();
|
var robot = RequireActiveRobot();
|
||||||
EnsureRuntimeEnabled();
|
EnsureRuntimeEnabled();
|
||||||
var currentJointPositions = _runtime.GetJointPositions();
|
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("MoveJoint 完成");
|
_logger?.LogInformation("MoveJoint 完成");
|
||||||
@@ -480,8 +469,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}",
|
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
|
||||||
name, options.Method, options.MoveToStart, options.UseCache);
|
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
|
||||||
|
|
||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
@@ -516,16 +505,83 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
if (options.MoveToStart)
|
if (options.MoveToStart)
|
||||||
{
|
{
|
||||||
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
|
_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;
|
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||||
|
if (options.Wait)
|
||||||
|
{
|
||||||
|
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
|
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="robot">当前机器人模型。</param>
|
||||||
|
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
|
||||||
|
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||||||
|
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||||||
|
/// <param name="plannedDuration">规划运动时长。</param>
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 根据规划时长推导等待超时，给真机通信和状态更新留出余量。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="plannedDuration">规划运动时长。</param>
|
||||||
|
/// <returns>等待运行时完成的最大时长。</returns>
|
||||||
|
private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration)
|
||||||
|
{
|
||||||
|
var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0);
|
||||||
|
return TimeSpan.FromSeconds(timeoutSeconds);
|
||||||
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
||||||
{
|
{
|
||||||
@@ -703,26 +759,6 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// 构造无需稠密轨迹的最小合法结果，供仍需单点状态更新的兼容路径使用。
|
|
||||||
/// </summary>
|
|
||||||
/// <returns>可立即执行的轨迹结果。</returns>
|
|
||||||
private static TrajectoryResult CreateImmediateMoveResult()
|
|
||||||
{
|
|
||||||
return new TrajectoryResult(
|
|
||||||
programName: "move-joint",
|
|
||||||
method: PlanningMethod.Icsp,
|
|
||||||
isValid: true,
|
|
||||||
duration: TimeSpan.Zero,
|
|
||||||
shotEvents: Array.Empty<ShotEvent>(),
|
|
||||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
|
||||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
|
||||||
failureReason: null,
|
|
||||||
usedCache: false,
|
|
||||||
originalWaypointCount: 1,
|
|
||||||
plannedWaypointCount: 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -12,16 +12,19 @@ public sealed class FlyshotExecutionOptions
|
|||||||
/// <param name="method">轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
|
/// <param name="method">轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
|
||||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||||
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
|
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
|
||||||
|
/// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param>
|
||||||
public FlyshotExecutionOptions(
|
public FlyshotExecutionOptions(
|
||||||
bool moveToStart = true,
|
bool moveToStart = true,
|
||||||
string method = "icsp",
|
string method = "icsp",
|
||||||
bool saveTrajectory = true,
|
bool saveTrajectory = true,
|
||||||
bool useCache = true)
|
bool useCache = true,
|
||||||
|
bool wait = true)
|
||||||
{
|
{
|
||||||
MoveToStart = moveToStart;
|
MoveToStart = moveToStart;
|
||||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||||
SaveTrajectory = saveTrajectory;
|
SaveTrajectory = saveTrajectory;
|
||||||
UseCache = useCache;
|
UseCache = useCache;
|
||||||
|
Wait = wait;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -43,4 +46,9 @@ public sealed class FlyshotExecutionOptions
|
|||||||
/// 获取是否优先复用已规划轨迹缓存。
|
/// 获取是否优先复用已规划轨迹缓存。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public bool UseCache { get; }
|
public bool UseCache { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取是否等待机器人执行完整条飞拍轨迹后再返回。
|
||||||
|
/// </summary>
|
||||||
|
public bool Wait { get; }
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -353,9 +353,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
if (!IsSimulationMode)
|
if (!IsSimulationMode)
|
||||||
{
|
{
|
||||||
var frame = GetFreshStateFrame();
|
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();
|
var frame = GetFreshStateFrame();
|
||||||
if (frame is not null)
|
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)
|
if (frame.CartesianPose.Count >= 6)
|
||||||
|
|||||||
@@ -1,11 +1,10 @@
|
|||||||
using System.Diagnostics;
|
|
||||||
using System.Net.Sockets;
|
using System.Net.Sockets;
|
||||||
using Microsoft.Extensions.Logging;
|
using Microsoft.Extensions.Logging;
|
||||||
|
|
||||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供周期命令发送与响应接收能力。
|
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供状态包驱动的命令发送与响应接收能力。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class FanucJ519Client : IDisposable
|
public sealed class FanucJ519Client : IDisposable
|
||||||
{
|
{
|
||||||
@@ -14,13 +13,11 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
private readonly ILogger<FanucJ519Client>? _logger;
|
private readonly ILogger<FanucJ519Client>? _logger;
|
||||||
private UdpClient? _udpClient;
|
private UdpClient? _udpClient;
|
||||||
private CancellationTokenSource? _cts;
|
private CancellationTokenSource? _cts;
|
||||||
private CancellationTokenSource? _sendCts;
|
|
||||||
private Task? _sendTask;
|
|
||||||
private Task? _receiveTask;
|
private Task? _receiveTask;
|
||||||
private FanucJ519Command? _currentCommand;
|
private FanucJ519Command? _currentCommand;
|
||||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||||
private FanucJ519Response? _latestResponse;
|
private FanucJ519Response? _latestResponse;
|
||||||
private uint _nextSequence;
|
private bool _motionStarted;
|
||||||
private bool _disposed;
|
private bool _disposed;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -71,7 +68,7 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 启动约 8ms 周期的 J519 命令发送循环。
|
/// 启动 J519 命令发送许可；实际发包由机器人状态包节拍驱动。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public void StartMotion()
|
public void StartMotion()
|
||||||
{
|
{
|
||||||
@@ -82,19 +79,22 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
throw new InvalidOperationException("J519 通道未连接。");
|
throw new InvalidOperationException("J519 通道未连接。");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_sendTask is not null)
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
_logger?.LogDebug("J519 StartMotion: 发送循环已在运行");
|
if (_motionStarted)
|
||||||
return; // 已在运行。
|
{
|
||||||
|
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("J519 StartMotion: 启动发送循环");
|
_motionStarted = true;
|
||||||
_sendCts = CancellationTokenSource.CreateLinkedTokenSource(_cts!.Token);
|
}
|
||||||
_sendTask = Task.Run(() => SendLoopAsync(_sendCts.Token), _sendCts.Token);
|
|
||||||
|
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 发送结束包并停止 J519 命令发送循环。
|
/// 发送状态输出停止包并停止 J519 命令发送。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
||||||
{
|
{
|
||||||
@@ -105,34 +105,15 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("J519 StopMotionAsync: 停止发送循环");
|
_logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送");
|
||||||
|
lock (_commandLock)
|
||||||
if (_sendTask is not null)
|
|
||||||
{
|
{
|
||||||
_sendCts?.Cancel();
|
_motionStarted = false;
|
||||||
|
|
||||||
try
|
|
||||||
{
|
|
||||||
await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false);
|
|
||||||
}
|
|
||||||
catch (TimeoutException)
|
|
||||||
{
|
|
||||||
_logger?.LogWarning("J519 StopMotionAsync: 发送循环未能在 1 秒内结束");
|
|
||||||
}
|
|
||||||
catch (OperationCanceledException)
|
|
||||||
{
|
|
||||||
// 正常取消。
|
|
||||||
}
|
}
|
||||||
|
|
||||||
_sendTask = null;
|
// FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
|
||||||
}
|
|
||||||
|
|
||||||
_sendCts?.Dispose();
|
|
||||||
_sendCts = null;
|
|
||||||
|
|
||||||
// 发送结束包通知控制器停止伺服流。
|
|
||||||
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||||
_logger?.LogInformation("J519 StopMotionAsync: 结束包已发送");
|
_logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送");
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -219,23 +200,8 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
{
|
{
|
||||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
_sendCts?.Cancel();
|
|
||||||
_cts?.Cancel();
|
_cts?.Cancel();
|
||||||
|
|
||||||
try
|
|
||||||
{
|
|
||||||
_sendTask?.Wait(TimeSpan.FromSeconds(1));
|
|
||||||
}
|
|
||||||
catch (AggregateException)
|
|
||||||
{
|
|
||||||
// 忽略取消异常。
|
|
||||||
}
|
|
||||||
|
|
||||||
_sendTask?.Dispose();
|
|
||||||
_sendTask = null;
|
|
||||||
_sendCts?.Dispose();
|
|
||||||
_sendCts = null;
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||||
@@ -258,7 +224,7 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
{
|
{
|
||||||
_currentCommand = null;
|
_currentCommand = null;
|
||||||
_commandHistoryForTests = null;
|
_commandHistoryForTests = null;
|
||||||
_nextSequence = 0;
|
_motionStarted = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
lock (_responseLock)
|
lock (_responseLock)
|
||||||
@@ -278,18 +244,8 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
_disposed = true;
|
_disposed = true;
|
||||||
_sendCts?.Cancel();
|
|
||||||
_cts?.Cancel();
|
_cts?.Cancel();
|
||||||
|
|
||||||
try
|
|
||||||
{
|
|
||||||
_sendTask?.Wait(TimeSpan.FromSeconds(1));
|
|
||||||
}
|
|
||||||
catch (AggregateException)
|
|
||||||
{
|
|
||||||
// 忽略取消异常。
|
|
||||||
}
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||||
@@ -299,59 +255,11 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
// 忽略取消异常。
|
// 忽略取消异常。
|
||||||
}
|
}
|
||||||
|
|
||||||
_sendTask?.Dispose();
|
|
||||||
_receiveTask?.Dispose();
|
_receiveTask?.Dispose();
|
||||||
_sendCts?.Dispose();
|
|
||||||
_cts?.Dispose();
|
_cts?.Dispose();
|
||||||
_udpClient?.Dispose();
|
_udpClient?.Dispose();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// 后台发送循环：以 Stopwatch + SpinWait 实现高精度 8ms 周期发送当前命令。
|
|
||||||
/// </summary>
|
|
||||||
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)
|
private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
|
||||||
{
|
{
|
||||||
return new FanucJ519Command(
|
return new FanucJ519Command(
|
||||||
@@ -396,6 +304,7 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
receiveCount++;
|
receiveCount++;
|
||||||
|
await SendCommandForStatusAsync(response, cancellationToken).ConfigureAwait(false);
|
||||||
|
|
||||||
// 仅在状态变化时记录 Info，避免高频日志。
|
// 仅在状态变化时记录 Info，避免高频日志。
|
||||||
if (lastLoggedResponse is null
|
if (lastLoggedResponse is null
|
||||||
@@ -430,4 +339,34 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
|
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="response">刚收到的状态包。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ public sealed class FanucStateFrame
|
|||||||
{
|
{
|
||||||
private readonly double[] _pose;
|
private readonly double[] _pose;
|
||||||
private readonly double[] _jointOrExtensionValues;
|
private readonly double[] _jointOrExtensionValues;
|
||||||
private readonly double[] _jointDegrees;
|
private readonly double[] _jointRadians;
|
||||||
private readonly double[] _externalAxes;
|
private readonly double[] _externalAxes;
|
||||||
private readonly uint[] _tailWords;
|
private readonly uint[] _tailWords;
|
||||||
|
|
||||||
@@ -46,7 +46,7 @@ public sealed class FanucStateFrame
|
|||||||
throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
|
throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
|
||||||
}
|
}
|
||||||
|
|
||||||
_jointDegrees = _jointOrExtensionValues.Take(6).ToArray();
|
_jointRadians = _jointOrExtensionValues.Take(6).ToArray();
|
||||||
_externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
|
_externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -71,9 +71,9 @@ public sealed class FanucStateFrame
|
|||||||
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取前 6 个机器人关节角度，单位为度。
|
/// 获取前 6 个机器人关节角度，当前现场抓包更支持按弧度制理解。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
public IReadOnlyList<double> JointRadians => _jointRadians;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
|
/// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
|
||||||
|
|||||||
@@ -564,8 +564,8 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data)
|
public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data)
|
||||||
{
|
{
|
||||||
_logger.LogInformation(
|
_logger.LogInformation(
|
||||||
"ExecuteFlyshot 调用: name={Name}, method={Method}, move_to_start={MoveToStart}, use_cache={UseCache}",
|
"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.name, data.method, data.move_to_start, data.use_cache, data.wait);
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_compatService.ExecuteTrajectoryByName(
|
_compatService.ExecuteTrajectoryByName(
|
||||||
@@ -574,7 +574,8 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
moveToStart: data.move_to_start,
|
moveToStart: data.move_to_start,
|
||||||
method: data.method,
|
method: data.method,
|
||||||
saveTrajectory: data.save_traj,
|
saveTrajectory: data.save_traj,
|
||||||
useCache: data.use_cache));
|
useCache: data.use_cache,
|
||||||
|
wait: data.wait));
|
||||||
_logger.LogInformation("ExecuteFlyshot 成功: name={Name}", data.name);
|
_logger.LogInformation("ExecuteFlyshot 成功: name={Name}", data.name);
|
||||||
return Ok(new { status = "FlyShot executed", success = true });
|
return Ok(new { status = "FlyShot executed", success = true });
|
||||||
}
|
}
|
||||||
@@ -944,6 +945,11 @@ public sealed class LegacyExecuteFlyshotRequest
|
|||||||
/// 获取或设置是否复用轨迹缓存。
|
/// 获取或设置是否复用轨迹缓存。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public bool use_cache { get; init; } = true;
|
public bool use_cache { get; init; } = true;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置是否等待机器人执行完整条飞拍轨迹后再返回。
|
||||||
|
/// </summary>
|
||||||
|
public bool wait { get; init; } = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|||||||
@@ -6,7 +6,7 @@ using Flyshot.Runtime.Fanuc.Protocol;
|
|||||||
namespace Flyshot.Core.Tests;
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、周期发送与响应解析。
|
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、状态包驱动发送与响应解析。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class FanucJ519ClientTests : IDisposable
|
public sealed class FanucJ519ClientTests : IDisposable
|
||||||
{
|
{
|
||||||
@@ -54,25 +54,24 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证启动运动后能按周期发送命令包。
|
/// 验证启动运动后必须等到状态包到达，不能由上位机本地 8ms 循环主动发命令。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public async Task StartMotion_SendsPeriodicCommands()
|
public async Task StartMotion_WaitsForStatusPacketBeforeSendingCommand()
|
||||||
{
|
{
|
||||||
using var client = new FanucJ519Client();
|
using var client = new FanucJ519Client();
|
||||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
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]);
|
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||||
client.UpdateCommand(command);
|
client.UpdateCommand(command);
|
||||||
client.StartMotion();
|
client.StartMotion();
|
||||||
|
|
||||||
// 接收至少一个命令包。
|
// 机器人尚未回状态包时，上位机不应自行发 64B command packet。
|
||||||
var commandResult = await _server.ReceiveAsync(_cts.Token);
|
await Assert.ThrowsAsync<TimeoutException>(
|
||||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
|
() => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
|
||||||
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
|
||||||
|
|
||||||
await client.StopMotionAsync(_cts.Token);
|
await client.StopMotionAsync(_cts.Token);
|
||||||
}
|
}
|
||||||
@@ -140,51 +139,47 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。
|
/// 验证收到状态包后，下一帧命令使用该状态包的序号。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public async Task UpdateCommand_ReplacesCurrentCommand()
|
public async Task StartMotion_UsesLatestStatusSequenceForFirstCommand()
|
||||||
{
|
{
|
||||||
using var client = new FanucJ519Client();
|
using var client = new FanucJ519Client();
|
||||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
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 command = 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(command);
|
||||||
|
|
||||||
client.UpdateCommand(command1);
|
|
||||||
client.StartMotion();
|
client.StartMotion();
|
||||||
|
|
||||||
var result1 = await _server.ReceiveAsync(_cts.Token);
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
|
||||||
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
|
|
||||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
|
||||||
|
|
||||||
client.UpdateCommand(command2);
|
var result = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||||
var result2 = await _server.ReceiveAsync(_cts.Token);
|
Assert.Equal(100u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
|
||||||
|
|
||||||
await client.StopMotionAsync(_cts.Token);
|
await client.StopMotionAsync(_cts.Token);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证重复保持同一命令时实际 J519 包序号仍按客户端全局递增。
|
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public async Task StartMotion_IncrementsSequenceForRepeatedHoldPackets()
|
public async Task StartMotion_SendsOneCommandForEachStatusPacketWithMatchingSequence()
|
||||||
{
|
{
|
||||||
using var client = new FanucJ519Client();
|
using var client = new FanucJ519Client();
|
||||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
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]);
|
var command = new FanucJ519Command(sequence: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||||
client.UpdateCommand(command);
|
client.UpdateCommand(command);
|
||||||
client.StartMotion();
|
client.StartMotion();
|
||||||
|
|
||||||
var packets = new List<byte[]>();
|
var packets = new List<byte[]>();
|
||||||
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);
|
var result = await _server.ReceiveAsync(_cts.Token);
|
||||||
packets.Add(result.Buffer);
|
packets.Add(result.Buffer);
|
||||||
}
|
}
|
||||||
@@ -194,26 +189,27 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
var sequences = packets
|
var sequences = packets
|
||||||
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
|
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
|
||||||
.ToArray();
|
.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));
|
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证停止运动后可在同一连接内重启发送，且包序号不重置。
|
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public async Task StartMotion_CanRestartAfterStopMotionWithoutResettingSequence()
|
public async Task StartMotion_CanRestartAfterStopMotionAndUseNewStatusSequence()
|
||||||
{
|
{
|
||||||
using var client = new FanucJ519Client();
|
using var client = new FanucJ519Client();
|
||||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
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.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||||
client.StartMotion();
|
client.StartMotion();
|
||||||
|
|
||||||
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 200);
|
||||||
var first = await _server.ReceiveAsync(_cts.Token);
|
var first = await _server.ReceiveAsync(_cts.Token);
|
||||||
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
|
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
|
||||||
Assert.Equal(0u, firstSequence);
|
Assert.Equal(200u, firstSequence);
|
||||||
|
|
||||||
await client.StopMotionAsync(_cts.Token);
|
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.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||||
client.StartMotion();
|
client.StartMotion();
|
||||||
|
|
||||||
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 300);
|
||||||
var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
||||||
|
|
||||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
|
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);
|
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||||
|
|
||||||
await client.StopMotionAsync(_cts.Token);
|
await client.StopMotionAsync(_cts.Token);
|
||||||
@@ -247,14 +244,14 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证发送循环能持续按协议周期输出命令包。
|
/// 验证状态包驱动发送能持续输出命令包。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public async Task StartMotion_MaintainsPeriodicCommandStream()
|
public async Task StartMotion_MaintainsStatusDrivenCommandStream()
|
||||||
{
|
{
|
||||||
using var client = new FanucJ519Client();
|
using var client = new FanucJ519Client();
|
||||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
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]);
|
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||||
client.UpdateCommand(command);
|
client.UpdateCommand(command);
|
||||||
@@ -265,6 +262,7 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
var sequences = new List<uint>();
|
var sequences = new List<uint>();
|
||||||
for (var i = 0; i < 5; i++)
|
for (var i = 0; i < 5; i++)
|
||||||
{
|
{
|
||||||
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: (uint)(500 + i));
|
||||||
var result = await _server.ReceiveAsync(_cts.Token);
|
var result = await _server.ReceiveAsync(_cts.Token);
|
||||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||||
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||||
@@ -273,7 +271,7 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
|
|
||||||
await client.StopMotionAsync(_cts.Token);
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
|
||||||
Assert.Equal([0u, 1u, 2u, 3u, 4u], sequences);
|
Assert.Equal([500u, 501u, 502u, 503u, 504u], sequences);
|
||||||
|
|
||||||
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
|
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
|
||||||
var intervals = new List<TimeSpan>();
|
var intervals = new List<TimeSpan>();
|
||||||
@@ -288,4 +286,17 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
||||||
|
/// </summary>
|
||||||
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -111,7 +111,7 @@ public sealed class FanucProtocolTests
|
|||||||
Assert.Equal(9, frame.JointOrExtensionValues.Count);
|
Assert.Equal(9, frame.JointOrExtensionValues.Count);
|
||||||
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
|
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
|
||||||
Assert.Equal(frame.Pose, frame.CartesianPose);
|
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.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
|
||||||
Assert.Equal(frame.TailWords, frame.RawTailWords);
|
Assert.Equal(frame.TailWords, frame.RawTailWords);
|
||||||
Assert.Equal(2u, frame.StatusWord0);
|
Assert.Equal(2u, frame.StatusWord0);
|
||||||
@@ -135,7 +135,7 @@ public sealed class FanucProtocolTests
|
|||||||
|
|
||||||
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
|
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
|
||||||
Assert.Equal(6, frame.CartesianPose.Count);
|
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(3, frame.ExternalAxes.Count);
|
||||||
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
|
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -293,6 +293,92 @@ public sealed class RuntimeOrchestrationTests
|
|||||||
Assert.Throws<ArgumentException>(Act);
|
Assert.Throws<ArgumentException>(Act);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
|
||||||
|
/// </summary>
|
||||||
|
[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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
|
||||||
|
/// </summary>
|
||||||
|
[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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -650,3 +736,164 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。
|
||||||
|
/// </summary>
|
||||||
|
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;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化可延迟完成第一段运动的测试运行时。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="initialJointPositions">运行时报告的初始关节位置。</param>
|
||||||
|
/// <param name="firstMotionCompletionDelay">第一段运动完成前保持忙碌的时间。</param>
|
||||||
|
public DelayedCompletionControllerRuntime(
|
||||||
|
IReadOnlyList<double> initialJointPositions,
|
||||||
|
TimeSpan firstMotionCompletionDelay)
|
||||||
|
{
|
||||||
|
_jointPositions = initialJointPositions.ToArray();
|
||||||
|
_firstMotionCompletionDelay = firstMotionCompletionDelay;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取所有 ExecuteTrajectory 调用记录。
|
||||||
|
/// </summary>
|
||||||
|
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。
|
||||||
|
/// </summary>
|
||||||
|
public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; }
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void ResetRobot(RobotProfile robot, string robotName)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SetActiveController(bool sim)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void Connect(string robotIp)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void Disconnect()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void EnableRobot(int bufferSize)
|
||||||
|
{
|
||||||
|
_isEnabled = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void DisableRobot()
|
||||||
|
{
|
||||||
|
_isEnabled = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void StopMove()
|
||||||
|
{
|
||||||
|
lock (_lock)
|
||||||
|
{
|
||||||
|
_isInMotion = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public double GetSpeedRatio() => 1.0;
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SetSpeedRatio(double ratio)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SetTcp(double x, double y, double z)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public bool GetIo(int port, string ioType) => false;
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SetIo(int port, bool value, string ioType)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public IReadOnlyList<double> GetJointPositions()
|
||||||
|
{
|
||||||
|
lock (_lock)
|
||||||
|
{
|
||||||
|
return _jointPositions.ToArray();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
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<double>(),
|
||||||
|
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> 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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
@@ -229,7 +229,8 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
|||||||
move_to_start = true,
|
move_to_start = true,
|
||||||
method = "icsp",
|
method = "icsp",
|
||||||
save_traj = true,
|
save_traj = true,
|
||||||
use_cache = true
|
use_cache = true,
|
||||||
|
wait = true
|
||||||
}))
|
}))
|
||||||
{
|
{
|
||||||
Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);
|
Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);
|
||||||
|
|||||||
Reference in New Issue
Block a user