✨ feat(*): 完善 FANUC J519 闭环、MoveJoint 与现场抓包验证
* 划分 J519 发送循环与稠密轨迹循环职责边界， FanucJ519Client 负责 UDP 周期发送， FanucControllerRuntime 按轨迹时间更新下一帧命令 * 执行时将规划输出 rad 转为 J519 deg 目标， 并按 speed_ratio 调整 8ms 发送时间尺度 * 补齐 accept_cmd/received_cmd/sysrdy/rbt_inmotion 状态位解析与启动前闭环检查 * MoveJoint 改为关节空间直线 + smoothstep 进度 的临时 PTP 稠密轨迹，按 status=15 运动窗口复现 * 新增 UTTC 2026-04-28 三份抓包 golden tests， 覆盖 0.5/0.7/1.0 speed_ratio 下的 J519 命令、 IO 脉冲与响应滞后 * 状态通道补充超时重连策略与退避逻辑 * TCP 10012 命令响应统一检查 result_code * 状态页扩展 J519 状态位与快照诊断信息 * 新增 docs/fanuc-field-runtime-workflow.md 现场工作流 * 补充 LR Mate 200iD 模型、RobotConfig.json 与 workpiece
This commit is contained in:
1
.gitignore
vendored
1
.gitignore
vendored
@@ -396,3 +396,4 @@ FodyWeavers.xsd
|
||||
|
||||
# JetBrains Rider
|
||||
*.sln.iml
|
||||
Config/Data/*
|
||||
|
||||
@@ -141,6 +141,7 @@ flyshot-replacement/
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/CommonMsg_protocol_analysis.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/FANUC_realtime_comm_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
|
||||
@@ -174,7 +175,10 @@ flyshot-replacement/
|
||||
- 新 HTTP API / HTTP-only `ControllerClientCompat` 已覆盖旧 HTTP 控制器后端的主要兼容语义。
|
||||
- `Flyshot.Core.Planning` 已落地 `icsp` 与 `self-adapt-icsp`，并已完成旧系统导出轨迹对齐。
|
||||
- `Flyshot.Core.Triggering` 已能从 `shot_flags / offset_values / addr` 生成触发时间轴。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 真机抓包确认为 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路，但 J519 闭环与现场联调仍需补齐。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，J519 闭环状态判断与现场联调仍需补齐。
|
||||
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
|
||||
|
||||
@@ -134,6 +134,7 @@ flyshot-replacement/
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/CommonMsg_protocol_analysis.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/FANUC_realtime_comm_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
|
||||
@@ -150,3 +151,7 @@ flyshot-replacement/
|
||||
- `Flyshot.Server.Host` 已提供最小 `/healthz`。
|
||||
- 最小集成测试已通过。
|
||||
- 解决方案构建已通过。
|
||||
- `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
BIN
Config/Models/LR_Mate_200iD.robot
Normal file
BIN
Config/Models/LR_Mate_200iD.robot
Normal file
Binary file not shown.
BIN
Config/Models/LR_Mate_200iD_7L -.glb
Normal file
BIN
Config/Models/LR_Mate_200iD_7L -.glb
Normal file
Binary file not shown.
BIN
Config/Models/LR_Mate_200iD_7L.robot
Normal file
BIN
Config/Models/LR_Mate_200iD_7L.robot
Normal file
Binary file not shown.
BIN
Config/Models/workpiece.stl
Normal file
BIN
Config/Models/workpiece.stl
Normal file
Binary file not shown.
1
Config/RobotConfig.json
Normal file
1
Config/RobotConfig.json
Normal file
@@ -0,0 +1 @@
|
||||
{"robot": {"use_do": true, "io_addr": [7, 8], "io_keep_cycles": 2, "acc_limit": 1, "jerk_limit": 1, "adapt_icsp_try_num": 5}, "flying_shots": {"20251015": {"traj_waypoints": [[1.047438621520996, -0.0002488955215085298, -0.0014060207176953554, 0.009022523649036884, 0.010111905634403229, 0.009573347866535187], [0.7661270499229431, -0.04437164217233658, -0.13630111515522003, -0.41718506813049316, 0.010093353688716888, 0.009594489820301533], [0.7661266326904297, 0.2170650213956833, -0.13630135357379913, -0.4171852171421051, 0.010093353688716888, 0.009594779461622238], [1.0311520099639893, -0.062108494341373444, -0.1363297700881958, 0.30276036262512207, 0.15847623348236084, 0.00956842489540577], [1.4012629985809326, -0.05120057240128517, -0.13633012771606445, 0.3027600347995758, 0.15847666561603546, 0.00956842489540577], [1.0567246675491333, 0.01165649201720953, -0.01786380261182785, -0.015170873142778873, 0.02149667963385582, 0.009576244279742241]], "shot_flags": [false, true, true, true, true, true], "offset_values": [0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4], [2, 4], [3, 4], [2, 4]]}, "TEST20251214": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8067416, 0.011661344, -0.11788314, -0.01516874, 0.021492079, 0.009567846], [0.60675246, -0.03833516, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.14043556], [1.1667324, 0.05164983, -0.11789217, 0.23482007, 0.021492079, -0.14043556], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0], "addr": [[], [], [3, 4, 2], [3, 4, 2], [3, 4, 2], [3, 4, 2], []]}, "UTTC_MS11": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145], [0.96600056, 0.20607172, -0.12233179, -1.2394339, 0.10493033, 1.2958988], [0.9618476, 0.15288207, -0.14867093, -0.7176314, 0.1764264, 0.73228663], [0.76189893, -0.028442925, -0.30919823, 0.10463613, 0.5615024, -0.39399016], [1.1271763, 0.074403025, -0.27347943, -0.5227772, 0.52098846, 0.79633313], [1.0555661, 0.4026262, -0.08746306, 0.6301835, 0.09644133, -0.5463328], [1.2300354, 0.28612664, -0.23486805, -0.4868128, 0.25369516, 0.55347764], [1.2144431, -0.29855102, -0.15202847, -1.0205934, 0.13317892, 1.1246506], [1.2840607, -0.11222197, -0.16805042, -2.248135, 0.2560587, 2.4434967], [1.3189346, -0.25620222, -0.12730704, -2.285038, 0.30872014, 2.4765089], [1.502615, -0.25304365, -0.23878741, -1.2194318, 0.46674785, 1.5533328], [1.07723, -0.07387611, -0.1707704, -1.8916591, 0.38677844, 2.061968], [1.3920237, 0.08098731, -0.2672306, -0.9780007, 0.4561093, 0.9102286], [1.9016331, 0.023924276, -0.58633333, -0.8441697, 0.76730615, 1.4842151], [1.9300697, -0.06738541, -0.56542397, -0.892083, 0.77194446, 1.5293273], [2.0611632, -0.30327517, -0.54225636, -1.0395275, 0.8505439, 1.6429617], [1.0921186, -0.40034482, -0.1803499, 1.3524796, 0.6210477, -1.2159473], [1.0521278, -0.40034503, -0.1803492, 1.3524843, 0.6210471, -1.2159531], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, false, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4, 2], [3, 4, 2], [4, 2], [4, 2], [3, 4], [3, 4], [4, 2], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], []]}, "5U": {"traj_waypoints": [[-0.95982397, 0.6331447, -1.0055008, 0.79858834, 1.1564041, -0.4260437], [-0.98353565, 0.66203266, -0.9758351, 0.8320198, 1.1455917, -0.45941326]], "shot_flags": [false, false], "offset_values": [0, 0], "addr": [[], []]}}}
|
||||
43
NLog.config
Normal file
43
NLog.config
Normal file
@@ -0,0 +1,43 @@
|
||||
<?xml version="1.0" encoding="utf-8" ?>
|
||||
<nlog xmlns="http://www.nlog-project.org/schemas/NLog.xsd"
|
||||
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
|
||||
xsi:schemaLocation="http://www.nlog-project.org/schemas/NLog.xsd NLog.xsd"
|
||||
autoReload="true"
|
||||
throwExceptions="false"
|
||||
internalLogLevel="Off" >
|
||||
|
||||
<!-- optional, add some variables
|
||||
https://github.com/nlog/NLog/wiki/Configuration-file#variables
|
||||
-->
|
||||
<variable name="myvar" value="myvalue"/>
|
||||
<!-- 环境变量配置：如果 ASPNETCORE_ENVIRONMENT 为空，则默认为 Development -->
|
||||
<variable name="env" value="${environment:ASPNETCORE_ENVIRONMENT:whenEmpty=Production}"/>
|
||||
|
||||
<!--
|
||||
See https://github.com/nlog/nlog/wiki/Configuration-file
|
||||
for information on customizing logging rules and outputs.
|
||||
-->
|
||||
|
||||
<target name="logfile" xsi:type="File"
|
||||
fileName="${basedir}/logs/${shortdate}.log"
|
||||
layout="${longdate}|${level:uppercase=true}|${message}"
|
||||
archiveFileName="${basedir}/logs/${shortdate}.{#}.log"
|
||||
archiveAboveSize="4048576"
|
||||
archiveNumbering="Sequence"
|
||||
maxArchiveFiles="50"
|
||||
concurrentWrites="true"
|
||||
keepFileOpen="false"
|
||||
encoding="utf-8" />
|
||||
<!--<target name="logfile" xsi:type="File" fileName="${basedir}/logs/${shortdate}.log" layout="${longdate}|${level:uppercase=true}|${logger}|${message}" />-->
|
||||
<target name="logconsole" xsi:type="Console" layout="${longdate}|${level:uppercase=true}|${message}" />
|
||||
|
||||
</targets>
|
||||
|
||||
<rules>
|
||||
<!-- 开发环境：显示控制台 + 详细文件，最低 Debug -->
|
||||
<logger name="*" minLevel="Debug" writeTo="logconsole,logfile" condition="equals('${var:env}','Development')" />
|
||||
|
||||
<!-- 生产环境：仅文件+UI，最低 Info -->
|
||||
<logger name="*" minLevel="Info" writeTo="logfile" condition="not_equals('${var:env}','Development')" />
|
||||
</rules>
|
||||
</nlog>
|
||||
23
README.md
23
README.md
@@ -17,8 +17,10 @@
|
||||
- 宿主只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。
|
||||
- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。
|
||||
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 真机抓包确认为 90B 固定帧。
|
||||
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送能力，但 J519 闭环和现场联调仍需补齐。
|
||||
- `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` 拉伸。
|
||||
- 真机 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` 下发。
|
||||
|
||||
开发约定：
|
||||
|
||||
@@ -48,13 +50,14 @@
|
||||
1. 配置与测试基线
|
||||
- [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移：`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`，应改用稳定样本或更新断言。
|
||||
- [x] 将 `RobotConfig.json` 中的 `use_do`、`io_keep_cycles`、`acc_limit`、`jerk_limit`、`adapt_icsp_try_num` 全部贯通到规划和执行链路。
|
||||
- [ ] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流（`/debug` 页已提供交互式覆盖，仍需补静态文档说明现场调用顺序）。
|
||||
- [x] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流：见 `docs/fanuc-field-runtime-workflow.md`。
|
||||
|
||||
2. 轨迹规划
|
||||
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
||||
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
||||
- [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。
|
||||
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。
|
||||
- [x] 将 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本：输入为 `rad`，下发为 `deg`，并按 `speed_ratio` 拉伸时间轴；覆盖 `2026042802-0.5/0.7/1.pcap`。
|
||||
- [ ] 补齐 `save_traj` / `SaveTrajInfo` 的规划结果导出，将稠密关节轨迹、笛卡尔轨迹和 ShotEvents 写入可诊断 artifacts。
|
||||
|
||||
3. FANUC TCP 10012 命令通道
|
||||
@@ -64,16 +67,18 @@
|
||||
- [x] 所有命令响应必须检查 `result_code`，失败时返回可诊断错误，而不是只更新本地缓存。
|
||||
|
||||
4. FANUC TCP 10010 状态通道
|
||||
- [x] 用 `j519 协议.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [x] 用 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
||||
- [x] 补充断线清理和异常帧拒绝测试。
|
||||
- [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。
|
||||
|
||||
5. FANUC UDP 60015 J519 运动链路
|
||||
- [ ] 重新确认 J519 发送循环与 `FanucControllerRuntime` 稠密轨迹循环的职责边界，避免双重节拍或命令覆盖。
|
||||
- [ ] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位闭环检查。
|
||||
- [ ] 校验序号递增、响应滞后、丢包、停止包和最后一帧语义。
|
||||
- [ ] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐。
|
||||
- [x] 重新确认 J519 发送循环与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 只负责 UDP 周期发送，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令。
|
||||
- [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整 8ms 发送索引/时间尺度：第 `k` 个 J519 目标采样 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- [x] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位解析与启动前闭环检查；若已有 J519 响应且 `accept_cmd/sysrdy` 未就绪，则拒绝稠密轨迹执行。
|
||||
- [x] 校验序号递增、响应滞后、丢包、停止包和最后一帧语义：UTTC golden tests 覆盖连续 seq、无重复 seq、响应滞后 2 到 8 帧、`lastData=0`；停止包由 J519 客户端测试覆盖。
|
||||
- [x] 将飞拍 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 进度。
|
||||
|
||||
6. 真机联调与运行安全
|
||||
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。
|
||||
@@ -84,4 +89,4 @@
|
||||
7. 发布与部署
|
||||
- [ ] 固化 Windows / Linux 启动脚本和 systemd 服务配置。
|
||||
- [ ] 补充生产配置模板、端口说明和现场部署检查表。
|
||||
- [ ] 给 Web 状态页增加真机连接、程序状态、J519 状态位和最近报警显示。
|
||||
- [ ] 给 Web 状态页增加程序状态和最近报警显示；J519 状态位已通过快照和状态页显示。
|
||||
|
||||
@@ -133,8 +133,9 @@
|
||||
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
|
||||
- HTTP 控制器已经按公开文档补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
|
||||
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试。
|
||||
- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态；真实 `10010 / 10012 / 60015` Socket 通讯与现场联调尚未落地。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在启动前检查已有 J519 响应中的 `accept_cmd/sysrdy` 状态。真实 R30iB 全流程现场联调仍需执行。
|
||||
- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是按 `floor(duration / (0.008 * speed_ratio)) + 1` 形成 J519 运行包。
|
||||
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
||||
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||
|
||||
@@ -515,6 +515,7 @@ UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)
|
||||
- `SetSpeedRatio`：`MsgID = 0x2207`
|
||||
- `GetIO`：`MsgID = 0x2208`
|
||||
- `SetIO`：`MsgID = 0x2209`
|
||||
- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。实发规则为 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- 飞拍轨迹相关额外字符串线索：
|
||||
- `StartUploadFlyShotTraj`
|
||||
- `EndUploadFlyShotTraj`
|
||||
|
||||
169
docs/fanuc-field-runtime-workflow.md
Normal file
169
docs/fanuc-field-runtime-workflow.md
Normal file
@@ -0,0 +1,169 @@
|
||||
# FANUC Field Runtime Workflow
|
||||
|
||||
本文档记录当前现场主链路的 HTTP 调用顺序，以及每一步在 FANUC 三条真机通道上的动作。它替代旧 `ControllerClient` 工作流说明；旧 `50001/TCP+JSON` 入口不再作为运行目标。
|
||||
|
||||
## 1. 初始化
|
||||
|
||||
推荐使用聚合端点完成当前现场的一次性初始化：
|
||||
|
||||
```bash
|
||||
POST /init_mpc_robt
|
||||
{
|
||||
"server_ip": "127.0.0.1",
|
||||
"port": 50001,
|
||||
"robot_name": "FANUC_LR_Mate_200iD",
|
||||
"robot_ip": "192.168.10.11",
|
||||
"sim": false
|
||||
}
|
||||
```
|
||||
|
||||
该端点内部顺序：
|
||||
|
||||
1. `ConnectServer(server_ip, port)`：兼容旧参数形状，仅记录服务连接语义。
|
||||
2. `SetUpRobot(robot_name)`：加载机器人配置、关节限制和伺服周期。
|
||||
3. `SetActiveController(sim)`：选择仿真或 FANUC 真机运行时。
|
||||
4. `Connect(robot_ip)`：真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
|
||||
5. `EnableRobot(2)`：真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`，随后启动 J519 8ms 周期发送器。
|
||||
|
||||
也可以使用拆分端点按同样顺序调用：
|
||||
|
||||
```text
|
||||
POST /connect_server/?server_ip=127.0.0.1&port=50001
|
||||
POST /setup_robot/?robot_name=FANUC_LR_Mate_200iD
|
||||
POST /set_active_controller/?sim=false
|
||||
POST /connect_robot/?ip=192.168.10.11
|
||||
GET /enable_robot/?buffer_size=2
|
||||
```
|
||||
|
||||
## 2. 参数设置
|
||||
|
||||
速度倍率：
|
||||
|
||||
```bash
|
||||
POST /set_speedRatio/
|
||||
{ "speed": 0.7 }
|
||||
```
|
||||
|
||||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。J519 执行时仍必须按该倍率重采样轨迹时间轴：
|
||||
|
||||
```text
|
||||
t_traj = k * 0.008 * speed_ratio
|
||||
send_count = floor(duration / (0.008 * speed_ratio)) + 1
|
||||
```
|
||||
|
||||
TCP 和普通 IO：
|
||||
|
||||
```text
|
||||
POST /set_tcp/ body: { "x": 0, "y": 0, "z": 0 }
|
||||
GET /get_tcp/
|
||||
POST /set_io/?port=7&value=true&io_type=DO
|
||||
GET /get_io/?port=7&io_type=DO
|
||||
```
|
||||
|
||||
飞拍触发 IO 不走独立 `TCP 10012 SetIO`，而是嵌入 `UDP 60015` J519 命令包的 `write_io_type/index/mask/value` 字段。
|
||||
|
||||
## 3. 点到点 MoveJoint
|
||||
|
||||
```bash
|
||||
POST /move_joint/
|
||||
{ "joints": [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145] }
|
||||
```
|
||||
|
||||
`MoveJoint` 不再直接把最终关节写成单个 J519 目标，而是按现场抓包确认的 PTP 临时轨迹执行：
|
||||
|
||||
1. 从当前运行时状态读取当前关节坐标，单位为 `rad`。
|
||||
2. 以当前关节和目标关节构造关节空间直线。
|
||||
3. 用五次 smoothstep `10u^3 - 15u^4 + 6u^5` 生成起停平滑的进度。
|
||||
4. 真机执行时仍由 J519 层把 `rad` 转成 `deg`，并按当前 `speed_ratio` 重采样。
|
||||
|
||||
已确认抓包按响应 `status=15` 运动窗口统计：
|
||||
|
||||
| 抓包 | speed_ratio | 运动窗口点数 | 运动窗口时长 |
|
||||
|------|-------------|----------------------|----------|
|
||||
| `2026042802-mvpoint.pcap` | 1.0 | 40 | 约 0.312s |
|
||||
| `2026042802-mvpoint0.7.pcap` | 0.7 | 55 | 约 0.432s |
|
||||
| `2026042802-mvpoint0.5.pcap` | 0.5 | 77 | 约 0.608s |
|
||||
|
||||
抓包命令流在运动窗口前后还会持续发送保持不变的起点/终点目标；功能复刻以 `status=15` 运动窗口为点数口径，并把最后一个采样点压到目标关节。实际目标几乎严格位于“起点 -> 终点”的同一条关节空间直线上，`speed_ratio` 体现为 J519 发送时间轴上的减速重采样，而不是改变路径形状。
|
||||
|
||||
## 4. 飞拍轨迹
|
||||
|
||||
上传：
|
||||
|
||||
```bash
|
||||
POST /upload_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"waypoints": [[...]],
|
||||
"shot_flags": [false, true],
|
||||
"offset_values": [0, 0],
|
||||
"addrs": [[1, 3]]
|
||||
}
|
||||
```
|
||||
|
||||
校验：
|
||||
|
||||
```bash
|
||||
POST /is_flyShotTrajValid/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false
|
||||
}
|
||||
```
|
||||
|
||||
执行：
|
||||
|
||||
```bash
|
||||
POST /execute_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"move_to_start": true,
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false,
|
||||
"use_cache": true
|
||||
}
|
||||
```
|
||||
|
||||
执行链路：
|
||||
|
||||
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
||||
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹。
|
||||
3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。
|
||||
4. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 周期重采样并转成 `deg`。
|
||||
5. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪，则拒绝执行。
|
||||
6. 周期命令中嵌入 IO 脉冲；当前 UTTC 抓包确认 mask 集合为 `10/12/14`，共 17 个 set 脉冲和 17 个 clear 帧。
|
||||
|
||||
`method="doubles"` 当前明确返回未实现；现场主链路使用 `icsp` / `self-adapt-icsp`。
|
||||
|
||||
## 5. 停止与断开
|
||||
|
||||
```text
|
||||
GET /stop_move/
|
||||
GET /disable_robot/
|
||||
POST /disconnect_robot/
|
||||
```
|
||||
|
||||
真机模式下：
|
||||
|
||||
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 发送循环。
|
||||
- `DisableRobot()` 发送 J519 end 控制包，然后 `StopProg("RVBUSTSM")`。
|
||||
- `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。
|
||||
|
||||
## 6. 现场抓包覆盖
|
||||
|
||||
`tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs` 直接解析以下抓包并与 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 对比：
|
||||
|
||||
| 抓包 | 速度 | 运行 J519 点数 | 发送时长 |
|
||||
|------|------|----------------|----------|
|
||||
| `2026042802-0.5.pcap` | 0.5 | 1851 | 14.800309s |
|
||||
| `2026042802-0.7.pcap` | 0.7 | 1322 | 10.568313s |
|
||||
| `2026042802-1.pcap` | 1.0 | 926 | 7.400125s |
|
||||
|
||||
测试同时检查：
|
||||
|
||||
- 主运行窗口命令序号连续，无重复 seq。
|
||||
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
|
||||
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。
|
||||
- `lastData=0`，结束运动依赖 J519 end 控制包。
|
||||
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
|
||||
@@ -2,14 +2,23 @@
|
||||
|
||||
## 上下文
|
||||
|
||||
当前 `flyshot-replacement` 项目已完成：
|
||||
状态更新：本计划中的 Socket 客户端和 `FanucControllerRuntime` 改造已经落地；当前事实以 `README.md` 和 `docs/fanuc-field-runtime-workflow.md` 为准。本文保留为实现过程记录。
|
||||
|
||||
计划制定时 `flyshot-replacement` 项目已完成：
|
||||
- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`）
|
||||
- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过）
|
||||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||||
- HTTP 兼容层控制器和状态监控页
|
||||
- 轨迹规划与飞拍触发编排层
|
||||
|
||||
**缺失的关键环节**：`FanucControllerRuntime` 仍是纯内存状态桩，没有实际 Socket 通信。`Connect()` 只记录 IP，`ExecuteTrajectory()` 只修改内存变量，`GetJointPositions()` 返回的是上一次写入值而非真实控制器反馈。
|
||||
2026-04-28 `Rvbust/uttc-20260428/20260428.pcap` 新增约束：
|
||||
|
||||
- `TCP 10010` 状态帧继续确认为固定 `90B`。
|
||||
- `UDP 60015` 命令 `target[0..5]` 为关节角 `deg`，而 `JointDetialTraj.txt` 为 `rad`。
|
||||
- `speed_ratio=0.7` 在本抓包中表现为 UDP 下发时间轴约 `1.427730x` 拉伸；机器人侧 `TCP 10012` 未抓到 `0x2207 SetSpeedRatio`。
|
||||
- `UTTC_MS11` 的 17 个飞拍触发点与 17 个 UDP IO 脉冲一一对齐，`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
**历史缺失项（已完成）**：计划制定时 `FanucControllerRuntime` 仍是纯内存状态桩。当前实现已经改为持有 `FanucCommandClient`、`FanucStateClient` 和 `FanucJ519Client`，真机模式会建立三条通道并从状态/J519 响应读取运行状态。
|
||||
|
||||
## 目标
|
||||
|
||||
@@ -66,7 +75,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- `GetProgramStatusAsync(string name)` → `PackProgramCommand(0x2003, name)`
|
||||
- `StartProgramAsync(string name)` → `PackProgramCommand(0x2102, name)`
|
||||
- `GetTcpAsync()` / `SetTcpAsync()` — 已按 `tcp_id + f32[7] pose` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现；注意 2026-04-28 真实运行抓包未出现机器人侧 `0x2207`，执行链路仍必须在 UDP 发送时间尺度上应用当前速度倍率
|
||||
- `GetIoAsync()` / `SetIoAsync()` — 已按 `io_type / io_index / f32 io_value` 字段布局实现
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucCommandClientTests.cs`
|
||||
@@ -91,6 +100,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析。
|
||||
- 用本地模拟控制器验证无状态帧超时、EOF 后退避重连和重连后的继续收帧。
|
||||
- `FanucStateProtocol` 已用 `j519 协议.pcap` 中多条 90B 样本锁定 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]`。
|
||||
- `Rvbust/uttc-20260428/20260428.pcap` 再次确认 `10010` 状态帧固定 90B，平均间隔约 25.6ms。
|
||||
- 尾部状态字当前只作为 `ControllerStateSnapshot.stateTailWords` 诊断字段保留，不从 `[2,0,0,1]` 推断使能或运动状态。
|
||||
|
||||
### Phase 3: UDP 60015 J519 运动客户端
|
||||
@@ -107,6 +117,14 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- 接收线程：持续 `ReceiveAsync()` 解析 132B 响应，更新反馈状态
|
||||
- `Disconnect()` — 清理
|
||||
|
||||
执行注意事项：
|
||||
|
||||
- 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。
|
||||
- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要按当前 `speed_ratio` 对轨迹时间轴做缩放，再采样到约 8ms 的 J519 周期。
|
||||
- 实发规则：第 `k` 个 J519 周期采样 `t_traj = k * 0.008 * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。
|
||||
- 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
|
||||
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs`
|
||||
- 用本地 UDP socket 模拟控制器收发
|
||||
|
||||
@@ -121,7 +139,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
|
||||
- `DisableRobot()` — 停止 J519，发送 StopProg
|
||||
- `Disconnect()` — 断开三条通道
|
||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点通过 J519 逐帧发送
|
||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 J519 逐周期发送
|
||||
- `StopMove()` — 立即停止 J519 发送循环
|
||||
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
|
||||
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
||||
@@ -164,6 +182,6 @@ dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTest
|
||||
|
||||
- `FanucControllerRuntime` 的 `Connect()` 能成功建立三条 TCP/UDP 连接
|
||||
- `EnableRobot()` 能走完 `RVBUSTSM` 启动序列
|
||||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点
|
||||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点，并按当前 `speed_ratio` 推进原始轨迹时间
|
||||
- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存
|
||||
- 现有 10 个集成测试和 25 个核心测试仍然通过
|
||||
|
||||
@@ -337,8 +337,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.ExecuteTrajectory(CreateImmediateMoveResult(), jointPositions);
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
var currentJointPositions = _runtime.GetJointPositions();
|
||||
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
|
||||
EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions));
|
||||
|
||||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio);
|
||||
_runtime.ExecuteTrajectory(moveResult, jointPositions);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -568,7 +575,30 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 MoveJoint 直达运行时所需的最小合法轨迹结果。
|
||||
/// 校验关节向量与当前机器人自由度一致，且所有值都是有限数值。
|
||||
/// </summary>
|
||||
/// <param name="joints">待校验关节向量，单位为弧度。</param>
|
||||
/// <param name="expectedCount">期望自由度。</param>
|
||||
/// <param name="paramName">调用方参数名。</param>
|
||||
private static void EnsureJointVector(IReadOnlyList<double> joints, int expectedCount, string paramName)
|
||||
{
|
||||
if (joints.Count != expectedCount)
|
||||
{
|
||||
throw new ArgumentException($"关节数量必须为 {expectedCount}。", paramName);
|
||||
}
|
||||
|
||||
for (var index = 0; index < joints.Count; index++)
|
||||
{
|
||||
var value = joints[index];
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个关节值必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造无需稠密轨迹的最小合法结果，供仍需单点状态更新的兼容路径使用。
|
||||
/// </summary>
|
||||
/// <returns>可立即执行的轨迹结果。</returns>
|
||||
private static TrajectoryResult CreateImmediateMoveResult()
|
||||
|
||||
@@ -13,4 +13,10 @@
|
||||
<FrameworkReference Include="Microsoft.AspNetCore.App" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -0,0 +1,194 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
internal static class MoveJointTrajectoryGenerator
|
||||
{
|
||||
private const double BaseMoveJointDurationSeconds = 0.320;
|
||||
private const double VelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double AccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double JerkShapeCoefficient = 36.12609273600853;
|
||||
private const int MaxMoveJointSampleCount = 1_000_000;
|
||||
|
||||
private static readonly double[] CapturedMvpointAlpha =
|
||||
[
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
];
|
||||
|
||||
public static TrajectoryResult CreateResult(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double speedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(startJoints);
|
||||
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
if (startJoints.Count != robot.DegreesOfFreedom || targetJoints.Count != robot.DegreesOfFreedom)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
|
||||
}
|
||||
|
||||
var requestedDurationSeconds = ResolveDurationSeconds(robot, startJoints, targetJoints);
|
||||
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds * speedRatio;
|
||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: "move-joint",
|
||||
method: PlanningMethod.Doubles,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 2,
|
||||
plannedWaypointCount: denseJointTrajectory.Count,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
internal static double ResolveDurationSeconds(RobotProfile robot, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var duration = BaseMoveJointDurationSeconds;
|
||||
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
|
||||
if (distance <= 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var limit = robot.JointLimits[index];
|
||||
var velocityDuration = distance * VelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * AccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * JerkShapeCoefficient / limit.JerkLimit);
|
||||
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
|
||||
return duration;
|
||||
}
|
||||
|
||||
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
||||
|
||||
return Math.Max(1, intervals) * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint sample count is not representable.");
|
||||
}
|
||||
|
||||
var intervals = (long)Math.Ceiling(rawIntervals - 1e-9);
|
||||
if (intervals < 1 || intervals + 1 > MaxMoveJointSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
}
|
||||
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double durationSeconds,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
|
||||
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
|
||||
|
||||
for (var index = 0L; index < sampleCount; index++)
|
||||
{
|
||||
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
|
||||
rows.Add(CreateRow(time, durationSeconds, startJoints, targetJoints));
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CreateRow(double timeSeconds, double durationSeconds, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||
var alpha = InterpolateCapturedAlpha(u);
|
||||
var row = new double[startJoints.Count + 1];
|
||||
row[0] = Math.Round(timeSeconds, 9);
|
||||
|
||||
for (var index = 0; index < startJoints.Count; index++)
|
||||
{
|
||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * alpha);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
internal static double InterpolateCapturedAlpha(double normalizedTime)
|
||||
{
|
||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var scaledIndex = clamped * (CapturedMvpointAlpha.Length - 1);
|
||||
var lower = (int)Math.Floor(scaledIndex);
|
||||
var upper = Math.Min(lower + 1, CapturedMvpointAlpha.Length - 1);
|
||||
var fraction = scaledIndex - lower;
|
||||
|
||||
return CapturedMvpointAlpha[lower]
|
||||
+ ((CapturedMvpointAlpha[upper] - CapturedMvpointAlpha[lower]) * fraction);
|
||||
}
|
||||
}
|
||||
@@ -19,7 +19,13 @@ public sealed class ControllerStateSnapshot
|
||||
IEnumerable<double>? jointPositions = null,
|
||||
IEnumerable<double>? cartesianPose = null,
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null,
|
||||
IEnumerable<uint>? stateTailWords = null)
|
||||
IEnumerable<uint>? stateTailWords = null,
|
||||
byte? j519Status = null,
|
||||
uint? j519Sequence = null,
|
||||
bool? j519AcceptsCommand = null,
|
||||
bool? j519ReceivedCommand = null,
|
||||
bool? j519SystemReady = null,
|
||||
bool? j519RobotInMotion = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(connectionState))
|
||||
{
|
||||
@@ -46,6 +52,12 @@ public sealed class ControllerStateSnapshot
|
||||
CartesianPose = copiedCartesianPose;
|
||||
ActiveAlarms = copiedActiveAlarms;
|
||||
StateTailWords = copiedStateTailWords;
|
||||
J519Status = j519Status;
|
||||
J519Sequence = j519Sequence;
|
||||
J519AcceptsCommand = j519AcceptsCommand;
|
||||
J519ReceivedCommand = j519ReceivedCommand;
|
||||
J519SystemReady = j519SystemReady;
|
||||
J519RobotInMotion = j519RobotInMotion;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -101,4 +113,40 @@ public sealed class ControllerStateSnapshot
|
||||
/// </summary>
|
||||
[JsonPropertyName("stateTailWords")]
|
||||
public IReadOnlyList<uint> StateTailWords { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应的原始状态字节；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Status")]
|
||||
public byte? J519Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应序号；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Sequence")]
|
||||
public uint? J519Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 accept_cmd 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519AcceptsCommand")]
|
||||
public bool? J519AcceptsCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 received_cmd 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519ReceivedCommand")]
|
||||
public bool? J519ReceivedCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 sysrdy 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519SystemReady")]
|
||||
public bool? J519SystemReady { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 rbt_inmotion 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519RobotInMotion")]
|
||||
public bool? J519RobotInMotion { get; }
|
||||
}
|
||||
|
||||
@@ -363,6 +363,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
var cartesianPose = _pose;
|
||||
var isInMotion = _isInMotion;
|
||||
IReadOnlyList<uint> stateTailWords = Array.Empty<uint>();
|
||||
byte? j519Status = null;
|
||||
uint? j519Sequence = null;
|
||||
bool? j519AcceptsCommand = null;
|
||||
bool? j519ReceivedCommand = null;
|
||||
bool? j519SystemReady = null;
|
||||
bool? j519RobotInMotion = null;
|
||||
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
@@ -386,6 +392,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
if (j519Response is not null)
|
||||
{
|
||||
isInMotion = j519Response.RobotInMotion;
|
||||
j519Status = j519Response.Status;
|
||||
j519Sequence = j519Response.Sequence;
|
||||
j519AcceptsCommand = j519Response.AcceptsCommand;
|
||||
j519ReceivedCommand = j519Response.ReceivedCommand;
|
||||
j519SystemReady = j519Response.SystemReady;
|
||||
j519RobotInMotion = j519Response.RobotInMotion;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -398,7 +410,13 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
jointPositions: jointPositions,
|
||||
cartesianPose: cartesianPose,
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>(),
|
||||
stateTailWords: stateTailWords);
|
||||
stateTailWords: stateTailWords,
|
||||
j519Status: j519Status,
|
||||
j519Sequence: j519Sequence,
|
||||
j519AcceptsCommand: j519AcceptsCommand,
|
||||
j519ReceivedCommand: j519ReceivedCommand,
|
||||
j519SystemReady: j519SystemReady,
|
||||
j519RobotInMotion: j519RobotInMotion);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -417,6 +435,13 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
|
||||
if (!IsSimulationMode && result.DenseJointTrajectory is not null)
|
||||
{
|
||||
if (_speedRatio <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
|
||||
}
|
||||
|
||||
EnsureJ519ReadyForDenseExecution();
|
||||
|
||||
// 真机模式且存在稠密路点：启动后台高精度发送任务。
|
||||
_isInMotion = true;
|
||||
_sendCts = new CancellationTokenSource();
|
||||
@@ -459,33 +484,39 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台高精度发送任务：按伺服周期遍历稠密路点并注入 IO 触发。
|
||||
/// 后台高精度发送任务：按 J519 周期发送命令，并按 speed_ratio 推进原始轨迹时间。
|
||||
/// </summary>
|
||||
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
|
||||
{
|
||||
var denseTrajectory = result.DenseJointTrajectory!;
|
||||
var triggers = result.TriggerTimeline;
|
||||
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
|
||||
var halfServoPeriod = servoPeriodSeconds / 2.0;
|
||||
var speedRatio = _speedRatio;
|
||||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||||
var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
|
||||
var durationSeconds = result.Duration.TotalSeconds;
|
||||
var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds);
|
||||
var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency);
|
||||
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
long nextTick = stopwatch.ElapsedTicks;
|
||||
uint sequence = 0;
|
||||
ushort ioValue = 0;
|
||||
ushort ioMask = 0;
|
||||
int holdRemaining = -1;
|
||||
int segmentIndex = 0;
|
||||
|
||||
try
|
||||
{
|
||||
foreach (var row in denseTrajectory)
|
||||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
nextTick += periodTicks;
|
||||
|
||||
double t = row[0];
|
||||
var joints = row.Skip(1).Select(static v => (double)v).ToArray();
|
||||
var trajectoryTime = Math.Min(sampleIndex * trajectoryStepSeconds, durationSeconds);
|
||||
var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex);
|
||||
|
||||
// 递减 IO 保持计数器；若已到期则清零。
|
||||
var clearMaskAfterSend = false;
|
||||
if (holdRemaining > 0)
|
||||
{
|
||||
holdRemaining--;
|
||||
@@ -494,32 +525,39 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
{
|
||||
ioValue = 0;
|
||||
holdRemaining = -1;
|
||||
clearMaskAfterSend = true;
|
||||
}
|
||||
|
||||
// 检查当前周期是否有新的触发事件。
|
||||
if (holdRemaining < 0)
|
||||
if (holdRemaining < 0 && !clearMaskAfterSend)
|
||||
{
|
||||
foreach (var trigger in triggers)
|
||||
{
|
||||
if (Math.Abs(t - trigger.TriggerTime) < halfServoPeriod)
|
||||
if (Math.Abs(trajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
|
||||
{
|
||||
ioValue = ComputeIoValue(trigger.AddressGroup);
|
||||
holdRemaining = trigger.HoldCycles;
|
||||
ioMask = ComputeIoValue(trigger.AddressGroup);
|
||||
ioValue = ioMask;
|
||||
holdRemaining = Math.Max(trigger.HoldCycles - 1, 0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var command = new FanucJ519Command(
|
||||
sequence: sequence++,
|
||||
sequence: 0,
|
||||
targetJoints: joints,
|
||||
writeIoType: 2,
|
||||
writeIoIndex: 1,
|
||||
writeIoMask: 255,
|
||||
writeIoMask: ioMask,
|
||||
writeIoValue: ioValue);
|
||||
|
||||
_j519Client.UpdateCommand(command);
|
||||
|
||||
if (clearMaskAfterSend)
|
||||
{
|
||||
ioMask = 0;
|
||||
}
|
||||
|
||||
// 高精度忙等待直到下一伺服周期。
|
||||
while (stopwatch.ElapsedTicks < nextTick)
|
||||
{
|
||||
@@ -541,6 +579,102 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发包数。
|
||||
/// </summary>
|
||||
private static long CalculateDenseSendSampleCount(double durationSeconds, double trajectoryStepSeconds)
|
||||
{
|
||||
if (durationSeconds < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "Trajectory duration must be non-negative.");
|
||||
}
|
||||
|
||||
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
|
||||
}
|
||||
|
||||
return (long)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 要求的角度制关节目标。
|
||||
/// </summary>
|
||||
private static double[] SampleDenseJointTrajectoryDegrees(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseTrajectory,
|
||||
double trajectoryTime,
|
||||
ref int segmentIndex)
|
||||
{
|
||||
if (denseTrajectory.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException("Dense joint trajectory is empty.");
|
||||
}
|
||||
|
||||
if (denseTrajectory.Count == 1 || trajectoryTime <= denseTrajectory[0][0])
|
||||
{
|
||||
return denseTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = denseTrajectory.Count - 1;
|
||||
if (trajectoryTime >= denseTrajectory[lastIndex][0])
|
||||
{
|
||||
return denseTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && denseTrajectory[segmentIndex + 1][0] < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = denseTrajectory[segmentIndex];
|
||||
var end = denseTrajectory[segmentIndex + 1];
|
||||
var startTime = start[0];
|
||||
var endTime = end[0];
|
||||
var segmentDuration = endTime - startTime;
|
||||
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
|
||||
|
||||
var jointCount = start.Count - 1;
|
||||
var joints = new double[jointCount];
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
var radians = start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha);
|
||||
joints[index] = RadiansToDegrees(radians);
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
|
||||
/// </summary>
|
||||
private void EnsureJ519ReadyForDenseExecution()
|
||||
{
|
||||
var response = _j519Client.GetLatestResponse();
|
||||
if (response is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (response.AcceptsCommand && response.SystemReady)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
"J519 status is not ready for dense execution: "
|
||||
+ $"accept_cmd={response.AcceptsCommand}, "
|
||||
+ $"received_cmd={response.ReceivedCommand}, "
|
||||
+ $"sysrdy={response.SystemReady}, "
|
||||
+ $"rbt_inmotion={response.RobotInMotion}, "
|
||||
+ $"seq={response.Sequence}, "
|
||||
+ $"status=0x{response.Status:X2}.");
|
||||
}
|
||||
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。
|
||||
/// </summary>
|
||||
@@ -592,7 +726,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
{
|
||||
if (_activeControllerIsSimulation is null)
|
||||
{
|
||||
throw new InvalidOperationException("Active controller has not been selected.");
|
||||
return false;
|
||||
}
|
||||
|
||||
return _activeControllerIsSimulation.Value;
|
||||
|
||||
@@ -12,10 +12,13 @@ public sealed class FanucJ519Client : IDisposable
|
||||
private readonly object _responseLock = new();
|
||||
private UdpClient? _udpClient;
|
||||
private CancellationTokenSource? _cts;
|
||||
private CancellationTokenSource? _sendCts;
|
||||
private Task? _sendTask;
|
||||
private Task? _receiveTask;
|
||||
private FanucJ519Command? _currentCommand;
|
||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||
private FanucJ519Response? _latestResponse;
|
||||
private uint _nextSequence;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
@@ -70,7 +73,8 @@ public sealed class FanucJ519Client : IDisposable
|
||||
return; // 已在运行。
|
||||
}
|
||||
|
||||
_sendTask = Task.Run(() => SendLoopAsync(_cts!.Token), _cts!.Token);
|
||||
_sendCts = CancellationTokenSource.CreateLinkedTokenSource(_cts!.Token);
|
||||
_sendTask = Task.Run(() => SendLoopAsync(_sendCts.Token), _sendCts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -85,10 +89,10 @@ public sealed class FanucJ519Client : IDisposable
|
||||
return;
|
||||
}
|
||||
|
||||
_cts?.Cancel();
|
||||
|
||||
if (_sendTask is not null)
|
||||
{
|
||||
_sendCts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false);
|
||||
@@ -105,6 +109,9 @@ public sealed class FanucJ519Client : IDisposable
|
||||
_sendTask = null;
|
||||
}
|
||||
|
||||
_sendCts?.Dispose();
|
||||
_sendCts = null;
|
||||
|
||||
// 发送结束包通知控制器停止伺服流。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
@@ -121,6 +128,34 @@ public sealed class FanucJ519Client : IDisposable
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = command;
|
||||
_commandHistoryForTests?.Add(command);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。
|
||||
/// </summary>
|
||||
internal void EnableCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_commandHistoryForTests = [];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取测试记录的命令历史。
|
||||
/// </summary>
|
||||
/// <returns>命令历史快照。</returns>
|
||||
internal IReadOnlyList<FanucJ519Command> GetCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _commandHistoryForTests?.ToArray() ?? Array.Empty<FanucJ519Command>();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -159,6 +194,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_sendCts?.Cancel();
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
@@ -172,6 +208,8 @@ public sealed class FanucJ519Client : IDisposable
|
||||
|
||||
_sendTask?.Dispose();
|
||||
_sendTask = null;
|
||||
_sendCts?.Dispose();
|
||||
_sendCts = null;
|
||||
|
||||
try
|
||||
{
|
||||
@@ -194,6 +232,8 @@ public sealed class FanucJ519Client : IDisposable
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
_commandHistoryForTests = null;
|
||||
_nextSequence = 0;
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
@@ -213,6 +253,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_sendCts?.Cancel();
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
@@ -235,6 +276,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
|
||||
_sendTask?.Dispose();
|
||||
_receiveTask?.Dispose();
|
||||
_sendCts?.Dispose();
|
||||
_cts?.Dispose();
|
||||
_udpClient?.Dispose();
|
||||
}
|
||||
@@ -263,7 +305,7 @@ public sealed class FanucJ519Client : IDisposable
|
||||
FanucJ519Command? command;
|
||||
lock (_commandLock)
|
||||
{
|
||||
command = _currentCommand;
|
||||
command = _currentCommand is null ? null : WithSequence(_currentCommand, _nextSequence++);
|
||||
}
|
||||
|
||||
if (command is not null)
|
||||
@@ -285,6 +327,22 @@ public sealed class FanucJ519Client : IDisposable
|
||||
}
|
||||
}
|
||||
|
||||
private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
|
||||
{
|
||||
return new FanucJ519Command(
|
||||
sequence,
|
||||
source.TargetJoints,
|
||||
source.LastData,
|
||||
source.ReadIoType,
|
||||
source.ReadIoIndex,
|
||||
source.ReadIoMask,
|
||||
source.DataStyle,
|
||||
source.WriteIoType,
|
||||
source.WriteIoIndex,
|
||||
source.WriteIoMask,
|
||||
source.WriteIoValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台接收循环：持续接收 132B 响应并解析。
|
||||
/// </summary>
|
||||
|
||||
@@ -274,6 +274,8 @@ public sealed class StatusController : ControllerBase
|
||||
<dt>客户端版本</dt><dd id="client-version">--</dd>
|
||||
<dt>已初始化</dt><dd id="setup-state">--</dd>
|
||||
<dt>已使能</dt><dd id="enabled-state">--</dd>
|
||||
<dt>J519 状态</dt><dd id="j519-status">--</dd>
|
||||
<dt>J519 序号</dt><dd id="j519-sequence">--</dd>
|
||||
<dt>采样时间</dt><dd id="captured-at">--</dd>
|
||||
</dl>
|
||||
</section>
|
||||
@@ -299,6 +301,8 @@ public sealed class StatusController : ControllerBase
|
||||
clientVersion: document.getElementById("client-version"),
|
||||
setupState: document.getElementById("setup-state"),
|
||||
enabledState: document.getElementById("enabled-state"),
|
||||
j519Status: document.getElementById("j519-status"),
|
||||
j519Sequence: document.getElementById("j519-sequence"),
|
||||
capturedAt: document.getElementById("captured-at"),
|
||||
dof: document.getElementById("dof"),
|
||||
joints: document.getElementById("joints"),
|
||||
@@ -315,6 +319,27 @@ public sealed class StatusController : ControllerBase
|
||||
return values.map(value => Number(value).toFixed(4)).join(", ");
|
||||
}
|
||||
|
||||
function formatNullableBool(value) {
|
||||
if (value === true) {
|
||||
return "是";
|
||||
}
|
||||
|
||||
if (value === false) {
|
||||
return "否";
|
||||
}
|
||||
|
||||
return "--";
|
||||
}
|
||||
|
||||
function formatJ519Status(snapshot) {
|
||||
if (snapshot.j519Status === null || snapshot.j519Status === undefined) {
|
||||
return "--";
|
||||
}
|
||||
|
||||
const status = Number(snapshot.j519Status).toString(16).padStart(2, "0").toUpperCase();
|
||||
return `0x${status} accept=${formatNullableBool(snapshot.j519AcceptsCommand)} received=${formatNullableBool(snapshot.j519ReceivedCommand)} sysrdy=${formatNullableBool(snapshot.j519SystemReady)} motion=${formatNullableBool(snapshot.j519RobotInMotion)}`;
|
||||
}
|
||||
|
||||
function setDot(connectionState) {
|
||||
fields.stateDot.className = "dot";
|
||||
if (connectionState === "Connected") {
|
||||
@@ -339,6 +364,8 @@ public sealed class StatusController : ControllerBase
|
||||
fields.clientVersion.textContent = payload.clientVersion;
|
||||
fields.setupState.textContent = payload.isSetup ? "是" : "否";
|
||||
fields.enabledState.textContent = snapshot.isEnabled ? "是" : "否";
|
||||
fields.j519Status.textContent = formatJ519Status(snapshot);
|
||||
fields.j519Sequence.textContent = snapshot.j519Sequence ?? "--";
|
||||
fields.capturedAt.textContent = new Date(snapshot.capturedAt).toLocaleString();
|
||||
fields.dof.textContent = payload.degreesOfFreedom;
|
||||
fields.joints.textContent = formatArray(snapshot.jointPositions);
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using System.Reflection;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
@@ -8,6 +12,302 @@ namespace Flyshot.Core.Tests;
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntimeDenseTests
|
||||
{
|
||||
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 0.5);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(5, commands.Count);
|
||||
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
|
||||
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(1.0)]
|
||||
[InlineData(0.7)]
|
||||
[InlineData(0.5)]
|
||||
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var service = CreateCompatService(runtime);
|
||||
var startJoints = new[]
|
||||
{
|
||||
1.056731,
|
||||
0.011664811,
|
||||
-0.017892333,
|
||||
-0.01516874,
|
||||
0.021492079,
|
||||
0.009567846
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
0.8532358,
|
||||
0.03837953,
|
||||
-0.19235304,
|
||||
0.0071595116,
|
||||
0.109054826,
|
||||
0.040055145
|
||||
};
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio);
|
||||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||||
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
WorkspaceRoot = TestRobotFactory.GetWorkspaceRoot()
|
||||
};
|
||||
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
|
||||
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
|
||||
var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio);
|
||||
|
||||
service.MoveJoint(targetJoints);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
|
||||
AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
|
||||
AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);
|
||||
|
||||
var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
|
||||
Assert.InRange(middleAlpha, 0.45, 0.55);
|
||||
|
||||
var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
|
||||
Assert.InRange(earlyAlpha, 0.0, 0.02);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
|
||||
var targetJoints = new[] { 0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145 };
|
||||
|
||||
var fullSpeed = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var speed07 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var speed05 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.5);
|
||||
|
||||
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
||||
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
||||
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
|
||||
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
|
||||
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var service = CreateCompatService(runtime);
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||||
|
||||
service.MoveJoint(targetJoints);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var currentCommand = j519Client.GetCurrentCommand();
|
||||
Assert.NotNull(currentCommand);
|
||||
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 0.0);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机模式下若 J519 响应明确显示伺服侧未就绪，则拒绝启动稠密轨迹发送。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsNotReadyJ519Status()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetLatestJ519Response(j519Client, status: 0b0011);
|
||||
|
||||
var result = CreateDenseResult(
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
],
|
||||
durationSeconds: 0.008);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => runtime.ExecuteTrajectory(result, [Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
|
||||
Assert.Contains("J519 status is not ready", exception.Message);
|
||||
Assert.Contains("sysrdy=False", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证控制器快照暴露最近一次 J519 响应中的四个状态位，便于状态页和诊断接口显示。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void GetSnapshot_RealMode_IncludesLatestJ519StatusBits()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetLatestJ519Response(j519Client, status: 0b0111);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
|
||||
Assert.Equal((byte)0b0111, snapshot.J519Status);
|
||||
Assert.Equal(10u, snapshot.J519Sequence);
|
||||
Assert.True(snapshot.J519AcceptsCommand);
|
||||
Assert.True(snapshot.J519ReceivedCommand);
|
||||
Assert.True(snapshot.J519SystemReady);
|
||||
Assert.False(snapshot.J519RobotInMotion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍 IO 脉冲按轨迹时间轴嵌入 J519 命令，并在保持周期后用同一 mask 清零。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_EmbedsIoPulseOnTrajectoryTimeline()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.024),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline:
|
||||
[
|
||||
new TrajectoryDoEvent(
|
||||
waypointIndex: 1,
|
||||
triggerTime: 0.008,
|
||||
offsetCycles: 0,
|
||||
holdCycles: 2,
|
||||
addressGroup: new IoAddressGroup([1, 3]))
|
||||
],
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(4, commands.Count);
|
||||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)10], commands.Select(static command => command.WriteIoMask));
|
||||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)0], commands.Select(static command => command.WriteIoValue));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。
|
||||
/// </summary>
|
||||
@@ -93,4 +393,287 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
var actual = FanucControllerRuntime.ComputeIoValue(group);
|
||||
Assert.Equal((ushort)(1 | 128), actual);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[]
|
||||
{
|
||||
DegreesToRadians(60.546227),
|
||||
DegreesToRadians(0.668344),
|
||||
DegreesToRadians(-1.025155),
|
||||
DegreesToRadians(-0.869105),
|
||||
DegreesToRadians(1.231405),
|
||||
DegreesToRadians(0.548197)
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
DegreesToRadians(48.886810),
|
||||
DegreesToRadians(2.198985),
|
||||
DegreesToRadians(-11.021017),
|
||||
DegreesToRadians(0.410210),
|
||||
DegreesToRadians(6.248381),
|
||||
DegreesToRadians(2.294991)
|
||||
};
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.Equal(41, rows.Count);
|
||||
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
|
||||
|
||||
var expectedAlpha = new[]
|
||||
{
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
};
|
||||
|
||||
for (var index = 0; index < rows.Count; index++)
|
||||
{
|
||||
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
|
||||
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
|
||||
}
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_RejectsUnrepresentableSampleCountForTinySpeedRatio()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1e-12));
|
||||
|
||||
Assert.Contains("sample count", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_StretchesLongMoveFromJointLimitsInsteadOfKeepingFortyCycles()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||||
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||||
var expectedMinimumDuration = new[]
|
||||
{
|
||||
0.320,
|
||||
expectedVelocityDuration,
|
||||
expectedAccelerationDuration,
|
||||
expectedJerkDuration
|
||||
}.Max();
|
||||
var expectedCountFromDuration = (int)Math.Floor(result.Duration.TotalSeconds / robot.ServoPeriod.TotalSeconds + 1e-9) + 1;
|
||||
|
||||
Assert.True(rows.Count > 41, $"Expected long MoveJoint to produce more than 41 points, got {rows.Count}.");
|
||||
Assert.True(
|
||||
result.Duration.TotalSeconds >= expectedMinimumDuration,
|
||||
$"Expected duration >= {expectedMinimumDuration:F6}s from joint limits, got {result.Duration.TotalSeconds:F6}s.");
|
||||
Assert.Equal(expectedCountFromDuration, rows.Count);
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
|
||||
private static void ForceRealModeEnabled(FanucControllerRuntime runtime, double speedRatio)
|
||||
{
|
||||
SetPrivateField(runtime, "_activeControllerIsSimulation", false);
|
||||
SetPrivateField(runtime, "_connectedRobotIp", "127.0.0.1");
|
||||
SetPrivateField(runtime, "_isEnabled", true);
|
||||
SetPrivateField(runtime, "_bufferSize", 2);
|
||||
SetPrivateField(runtime, "_speedRatio", speedRatio);
|
||||
}
|
||||
|
||||
private static ControllerClientCompatService CreateCompatService(FanucControllerRuntime runtime)
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
WorkspaceRoot = TestRobotFactory.GetWorkspaceRoot()
|
||||
};
|
||||
|
||||
return new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader(),
|
||||
new InMemoryFlyshotTrajectoryStore());
|
||||
}
|
||||
|
||||
private static double ComputeLineAlpha(
|
||||
IReadOnlyList<double> actualDegrees,
|
||||
IReadOnlyList<double> startRadians,
|
||||
IReadOnlyList<double> targetRadians)
|
||||
{
|
||||
var numerator = 0.0;
|
||||
var denominator = 0.0;
|
||||
for (var index = 0; index < startRadians.Count; index++)
|
||||
{
|
||||
var startDegrees = RadiansToDegrees(startRadians[index]);
|
||||
var deltaDegrees = RadiansToDegrees(targetRadians[index]) - startDegrees;
|
||||
numerator += (actualDegrees[index] - startDegrees) * deltaDegrees;
|
||||
denominator += deltaDegrees * deltaDegrees;
|
||||
}
|
||||
|
||||
return denominator <= 0.0 ? 0.0 : numerator / denominator;
|
||||
}
|
||||
|
||||
private static void AssertJointDegreesEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualDegrees)
|
||||
{
|
||||
Assert.Equal(expectedRadians.Count, actualDegrees.Count);
|
||||
for (var index = 0; index < expectedRadians.Count; index++)
|
||||
{
|
||||
Assert.Equal(RadiansToDegrees(expectedRadians[index]), actualDegrees[index], precision: 4);
|
||||
}
|
||||
}
|
||||
|
||||
private static RobotProfile CreateMoveJointReferenceRobotProfile()
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: "FANUC_LR_Mate_200iD",
|
||||
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||||
degreesOfFreedom: 6,
|
||||
jointLimits:
|
||||
[
|
||||
new JointLimit("J1", 7.85, 32.72, 272.7),
|
||||
new JointLimit("J2", 6.63, 27.63, 230.28),
|
||||
new JointLimit("J3", 9.07, 37.81, 315.12),
|
||||
new JointLimit("J4", 9.59, 39.99, 333.3),
|
||||
new JointLimit("J5", 9.51, 39.63, 330.27),
|
||||
new JointLimit("J6", 17.45, 72.72, 606.01)
|
||||
],
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
private static double DegreesToRadians(double degrees)
|
||||
{
|
||||
return degrees * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
private static void WaitUntilIdle(FanucControllerRuntime runtime)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
if (!runtime.GetSnapshot().IsInMotion)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Thread.Sleep(1);
|
||||
}
|
||||
|
||||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||||
}
|
||||
|
||||
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
||||
{
|
||||
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
Assert.NotNull(field);
|
||||
field.SetValue(runtime, value);
|
||||
}
|
||||
|
||||
private static TrajectoryResult CreateDenseResult(IReadOnlyList<IReadOnlyList<double>> denseTrajectory, double durationSeconds)
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
}
|
||||
|
||||
private static void SetLatestJ519Response(FanucJ519Client client, byte status)
|
||||
{
|
||||
var response = new FanucJ519Response(
|
||||
messageType: 0,
|
||||
version: 1,
|
||||
sequence: 10,
|
||||
status: status,
|
||||
readIoType: 2,
|
||||
readIoIndex: 1,
|
||||
readIoMask: 255,
|
||||
readIoValue: 0,
|
||||
timestamp: 1234,
|
||||
pose: new double[6],
|
||||
externalAxes: new double[3],
|
||||
jointDegrees: new double[9],
|
||||
motorCurrents: new double[9]);
|
||||
var field = typeof(FanucJ519Client).GetField("_latestResponse", BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
Assert.NotNull(field);
|
||||
field.SetValue(client, response);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -72,7 +72,7 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
// 接收至少一个命令包。
|
||||
var commandResult = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
@@ -156,18 +156,86 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
client.StartMotion();
|
||||
|
||||
var result1 = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
|
||||
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 result2 = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证重复保持同一命令时实际 J519 包序号仍按客户端全局递增。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_IncrementsSequenceForRepeatedHoldPackets()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await _server.ReceiveAsync(_cts.Token); // init
|
||||
|
||||
var command = new FanucJ519Command(sequence: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
var packets = new List<byte[]>();
|
||||
for (var index = 0; index < 4; index++)
|
||||
{
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
packets.Add(result.Buffer);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
var sequences = packets
|
||||
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
|
||||
.ToArray();
|
||||
Assert.Equal([0u, 1u, 2u, 3u], sequences);
|
||||
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证停止运动后可在同一连接内重启发送，且包序号不重置。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_CanRestartAfterStopMotionWithoutResettingSequence()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await _server.ReceiveAsync(_cts.Token); // init
|
||||
|
||||
client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
client.StartMotion();
|
||||
|
||||
var first = await _server.ReceiveAsync(_cts.Token);
|
||||
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
|
||||
Assert.Equal(0u, firstSequence);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
byte[] packet;
|
||||
do
|
||||
{
|
||||
packet = (await _server.ReceiveAsync(_cts.Token)).Buffer;
|
||||
}
|
||||
while (packet.Length != FanucJ519Protocol.ControlPacketLength);
|
||||
|
||||
client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
client.StartMotion();
|
||||
|
||||
var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
||||
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
|
||||
Assert.True(BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)) > firstSequence);
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
|
||||
/// </summary>
|
||||
@@ -179,10 +247,10 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。
|
||||
/// 验证发送循环能持续按协议周期输出命令包。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_MaintainsSubMillisecondPeriod()
|
||||
public async Task StartMotion_MaintainsPeriodicCommandStream()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
@@ -192,28 +260,32 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
// 收集 5 个命令包到达时间戳。
|
||||
// 收集 5 个命令包到达时间戳和序号。
|
||||
var timestamps = new List<DateTimeOffset>();
|
||||
var sequences = new List<uint>();
|
||||
for (var i = 0; i < 5; i++)
|
||||
{
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||
timestamps.Add(DateTimeOffset.UtcNow);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
// 计算相邻包间隔并断言最大抖动。
|
||||
Assert.Equal([0u, 1u, 2u, 3u, 4u], sequences);
|
||||
|
||||
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
|
||||
var intervals = new List<TimeSpan>();
|
||||
for (var i = 1; i < timestamps.Count; i++)
|
||||
{
|
||||
intervals.Add(timestamps[i] - timestamps[i - 1]);
|
||||
}
|
||||
|
||||
// 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。
|
||||
Assert.All(intervals, interval =>
|
||||
{
|
||||
Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。");
|
||||
Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||
Assert.True(interval > TimeSpan.Zero, $"间隔 {interval.TotalMilliseconds:F2}ms 必须为正。");
|
||||
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs
Normal file
526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs
Normal file
@@ -0,0 +1,526 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Globalization;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 使用 2026-04-28 UTTC 真实抓包验证 J519 主运行点位与 JointDetialTraj 重采样规则一致。
|
||||
/// </summary>
|
||||
public sealed class UttcJ519GoldenTests
|
||||
{
|
||||
private const int JointCount = 6;
|
||||
private const int RobotJ519Port = 60015;
|
||||
private const double ServoPeriodSeconds = 0.008;
|
||||
|
||||
public static IEnumerable<object[]> SpeedSweepCases()
|
||||
{
|
||||
yield return ["2026042802-0.5.pcap", 0.5, 1851, 14.800309];
|
||||
yield return ["2026042802-0.7.pcap", 0.7, 1322, 10.568313];
|
||||
yield return ["2026042802-1.pcap", 1.0, 926, 7.400125];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed=0.5/0.7/1.0 三份真实抓包都符合当前运行时采用的发送点生成规则。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[MemberData(nameof(SpeedSweepCases))]
|
||||
public void CapturedJ519Run_ReplaysJointDetailTrajectoryWithSpeedRatio(
|
||||
string pcapFileName,
|
||||
double speedRatio,
|
||||
int expectedPointCount,
|
||||
double expectedSendDurationSeconds)
|
||||
{
|
||||
var repositoryRoot = FindRepositoryRoot();
|
||||
var pcapPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", pcapFileName);
|
||||
var jointDetailPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
|
||||
|
||||
var packets = ParsePcapUdp(pcapPath);
|
||||
var hostPort = DetectHostJ519Port(packets);
|
||||
var commands = ParseJ519Commands(packets, hostPort);
|
||||
var responses = ParseJ519Responses(packets, hostPort);
|
||||
var responseSegment = LongestStatusSegment(responses, status: 15);
|
||||
var jointRows = ReadJointDetail(jointDetailPath);
|
||||
var expected = GenerateExpectedPoints(jointRows, speedRatio);
|
||||
|
||||
var commandBySequence = new Dictionary<uint, CapturedJ519Command>();
|
||||
var duplicateSequenceCount = 0;
|
||||
foreach (var command in commands)
|
||||
{
|
||||
if (!commandBySequence.TryAdd(command.Sequence, command))
|
||||
{
|
||||
duplicateSequenceCount++;
|
||||
}
|
||||
}
|
||||
|
||||
var (startSequence, windowRms) = FindBestCommandWindow(commandBySequence, expected, responseSegment, searchRadius: 32);
|
||||
var actualRun = Enumerable
|
||||
.Range(0, expected.Length)
|
||||
.Select(index => commandBySequence[startSequence + (uint)index])
|
||||
.ToArray();
|
||||
var comparison = Compare(actualRun, expected);
|
||||
|
||||
Assert.Equal(464, jointRows.Length);
|
||||
Assert.Equal(expectedPointCount, expected.Length);
|
||||
Assert.Equal(expectedPointCount, actualRun.Length);
|
||||
Assert.Equal(0, duplicateSequenceCount);
|
||||
Assert.Equal(17, comparison.IoSetPulses);
|
||||
Assert.Equal(17, comparison.IoClearFrames);
|
||||
Assert.Equal(
|
||||
new ushort[] { 10, 12, 14 },
|
||||
actualRun
|
||||
.Where(static command => command.WriteIoMask != 0)
|
||||
.Select(static command => command.WriteIoMask)
|
||||
.Distinct()
|
||||
.Order()
|
||||
.ToArray());
|
||||
Assert.True(responseSegment.Length >= expectedPointCount - 1, $"status=15 segment too short: {responseSegment.Length}");
|
||||
Assert.InRange((long)responseSegment[0].Sequence - startSequence, 2, 8);
|
||||
Assert.All(actualRun, static command => Assert.Equal(0, command.LastData));
|
||||
for (var index = 0; index < actualRun.Length; index++)
|
||||
{
|
||||
Assert.Equal(startSequence + (uint)index, actualRun[index].Sequence);
|
||||
}
|
||||
|
||||
Assert.True(windowRms < 0.012, $"J519 global RMS {windowRms:F9} deg exceeds tolerance.");
|
||||
Assert.True(comparison.GlobalMaxAbsDeg < 0.07, $"J519 max abs diff {comparison.GlobalMaxAbsDeg:F9} deg exceeds tolerance.");
|
||||
|
||||
var sendDuration = actualRun[^1].TimeRelativeSeconds - actualRun[0].TimeRelativeSeconds;
|
||||
Assert.InRange(sendDuration, expectedSendDurationSeconds - 0.04, expectedSendDurationSeconds + 0.04);
|
||||
}
|
||||
|
||||
private static string FindRepositoryRoot()
|
||||
{
|
||||
var directory = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (directory is not null)
|
||||
{
|
||||
var jointDetailPath = Path.Combine(directory.FullName, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
|
||||
if (File.Exists(jointDetailPath))
|
||||
{
|
||||
return directory.FullName;
|
||||
}
|
||||
|
||||
directory = directory.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Cannot locate repository root containing Rvbust/uttc-20260428/Data/JointDetialTraj.txt.");
|
||||
}
|
||||
|
||||
private static IReadOnlyList<UdpPacket> ParsePcapUdp(string path)
|
||||
{
|
||||
using var stream = File.OpenRead(path);
|
||||
var header = new byte[24];
|
||||
stream.ReadExactly(header);
|
||||
|
||||
var magic = header.AsSpan(0, 4);
|
||||
var bigEndian = false;
|
||||
var timestampScale = 1_000_000.0;
|
||||
if (magic.SequenceEqual(new byte[] { 0xd4, 0xc3, 0xb2, 0xa1 }))
|
||||
{
|
||||
bigEndian = false;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0xc3, 0xd4 }))
|
||||
{
|
||||
bigEndian = true;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0x4d, 0x3c, 0xb2, 0xa1 }))
|
||||
{
|
||||
bigEndian = false;
|
||||
timestampScale = 1_000_000_000.0;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0x3c, 0x4d }))
|
||||
{
|
||||
bigEndian = true;
|
||||
timestampScale = 1_000_000_000.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidDataException($"Unsupported pcap magic: {Convert.ToHexString(header.AsSpan(0, 4))}");
|
||||
}
|
||||
|
||||
var linkType = ReadUInt32(header.AsSpan(20, 4), bigEndian);
|
||||
if (linkType != 1)
|
||||
{
|
||||
throw new InvalidDataException($"Only Ethernet pcap is supported, got linktype {linkType}.");
|
||||
}
|
||||
|
||||
var packets = new List<UdpPacket>();
|
||||
var recordHeader = new byte[16];
|
||||
double? firstTimestamp = null;
|
||||
var frameNumber = 0;
|
||||
while (ReadFullOrEnd(stream, recordHeader))
|
||||
{
|
||||
frameNumber++;
|
||||
var tsSec = ReadUInt32(recordHeader.AsSpan(0, 4), bigEndian);
|
||||
var tsFraction = ReadUInt32(recordHeader.AsSpan(4, 4), bigEndian);
|
||||
var includedLength = ReadUInt32(recordHeader.AsSpan(8, 4), bigEndian);
|
||||
var packet = new byte[includedLength];
|
||||
stream.ReadExactly(packet);
|
||||
|
||||
var timestamp = tsSec + (tsFraction / timestampScale);
|
||||
firstTimestamp ??= timestamp;
|
||||
var udp = ParseEthernetIpv4Udp(packet, frameNumber, timestamp - firstTimestamp.Value);
|
||||
if (udp is not null)
|
||||
{
|
||||
packets.Add(udp);
|
||||
}
|
||||
}
|
||||
|
||||
return packets;
|
||||
}
|
||||
|
||||
private static bool ReadFullOrEnd(Stream stream, byte[] buffer)
|
||||
{
|
||||
var offset = 0;
|
||||
while (offset < buffer.Length)
|
||||
{
|
||||
var read = stream.Read(buffer, offset, buffer.Length - offset);
|
||||
if (read == 0)
|
||||
{
|
||||
if (offset == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
throw new EndOfStreamException("Truncated pcap record header.");
|
||||
}
|
||||
|
||||
offset += read;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static UdpPacket? ParseEthernetIpv4Udp(byte[] packet, int frameNumber, double timeRelativeSeconds)
|
||||
{
|
||||
if (packet.Length < 14)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var offset = 14;
|
||||
var etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(12, 2));
|
||||
if (etherType == 0x8100)
|
||||
{
|
||||
if (packet.Length < 18)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(16, 2));
|
||||
offset = 18;
|
||||
}
|
||||
|
||||
if (etherType != 0x0800 || packet.Length < offset + 20)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var versionIhl = packet[offset];
|
||||
var version = versionIhl >> 4;
|
||||
var ihl = (versionIhl & 0x0f) * 4;
|
||||
if (version != 4 || ihl < 20 || packet.Length < offset + ihl)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var protocol = packet[offset + 9];
|
||||
if (protocol != 17)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var totalLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(offset + 2, 2));
|
||||
var udpOffset = offset + ihl;
|
||||
if (packet.Length < udpOffset + 8)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var sourcePort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset, 2));
|
||||
var destinationPort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 2, 2));
|
||||
var udpLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 4, 2));
|
||||
var payloadOffset = udpOffset + 8;
|
||||
var payloadLength = Math.Max(0, Math.Min(udpLength - 8, totalLength - ihl - 8));
|
||||
if (packet.Length < payloadOffset + payloadLength)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return new UdpPacket(
|
||||
frameNumber,
|
||||
timeRelativeSeconds,
|
||||
sourcePort,
|
||||
destinationPort,
|
||||
packet.AsSpan(payloadOffset, payloadLength).ToArray());
|
||||
}
|
||||
|
||||
private static ushort DetectHostJ519Port(IEnumerable<UdpPacket> packets)
|
||||
{
|
||||
return packets
|
||||
.Where(static packet => packet.DestinationPort == RobotJ519Port && packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
|
||||
.GroupBy(static packet => packet.SourcePort)
|
||||
.OrderByDescending(static group => group.Count())
|
||||
.Select(static group => group.Key)
|
||||
.First();
|
||||
}
|
||||
|
||||
private static CapturedJ519Command[] ParseJ519Commands(IEnumerable<UdpPacket> packets, ushort hostPort)
|
||||
{
|
||||
return packets
|
||||
.Where(packet =>
|
||||
packet.SourcePort == hostPort
|
||||
&& packet.DestinationPort == RobotJ519Port
|
||||
&& packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
|
||||
.Select(ParseCommand)
|
||||
.Where(static command => command is not null)
|
||||
.Cast<CapturedJ519Command>()
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static CapturedJ519Command? ParseCommand(UdpPacket packet)
|
||||
{
|
||||
var payload = packet.Payload;
|
||||
var messageType = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x00, 4));
|
||||
var version = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x04, 4));
|
||||
if (messageType != 1 || version != 1)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var targets = new double[9];
|
||||
for (var index = 0; index < targets.Length; index++)
|
||||
{
|
||||
targets[index] = BinaryPrimitives.ReadSingleBigEndian(payload.AsSpan(0x1c + (index * 4), 4));
|
||||
}
|
||||
|
||||
return new CapturedJ519Command(
|
||||
packet.FrameNumber,
|
||||
packet.TimeRelativeSeconds,
|
||||
BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x08, 4)),
|
||||
payload[0x0c],
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x16, 2)),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x18, 2)),
|
||||
targets);
|
||||
}
|
||||
|
||||
private static FanucJ519Response[] ParseJ519Responses(IEnumerable<UdpPacket> packets, ushort hostPort)
|
||||
{
|
||||
return packets
|
||||
.Where(packet =>
|
||||
packet.SourcePort == RobotJ519Port
|
||||
&& packet.DestinationPort == hostPort
|
||||
&& packet.Payload.Length == FanucJ519Protocol.ResponsePacketLength)
|
||||
.Select(packet => FanucJ519Protocol.ParseResponse(packet.Payload))
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static FanucJ519Response[] LongestStatusSegment(IEnumerable<FanucJ519Response> responses, byte status)
|
||||
{
|
||||
var best = new List<FanucJ519Response>();
|
||||
var current = new List<FanucJ519Response>();
|
||||
foreach (var response in responses)
|
||||
{
|
||||
if (response.Status == status)
|
||||
{
|
||||
current.Add(response);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (current.Count > best.Count)
|
||||
{
|
||||
best = current;
|
||||
}
|
||||
|
||||
current = [];
|
||||
}
|
||||
|
||||
return (current.Count > best.Count ? current : best).ToArray();
|
||||
}
|
||||
|
||||
private static JointRow[] ReadJointDetail(string path)
|
||||
{
|
||||
return File.ReadLines(path)
|
||||
.Where(static line => !string.IsNullOrWhiteSpace(line) && !line.TrimStart().StartsWith('#'))
|
||||
.Select(static line =>
|
||||
{
|
||||
var values = line
|
||||
.Split((char[]?)null, StringSplitOptions.RemoveEmptyEntries)
|
||||
.Select(value => double.Parse(value, CultureInfo.InvariantCulture))
|
||||
.ToArray();
|
||||
return new JointRow(values[0], values.Skip(1).Take(JointCount).ToArray());
|
||||
})
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static ExpectedPoint[] GenerateExpectedPoints(IReadOnlyList<JointRow> rows, double speedRatio)
|
||||
{
|
||||
var durationSeconds = rows[^1].TimeSeconds;
|
||||
var trajectoryStepSeconds = ServoPeriodSeconds * speedRatio;
|
||||
var sampleCount = (int)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
|
||||
var points = new ExpectedPoint[sampleCount];
|
||||
var segmentIndex = 0;
|
||||
|
||||
for (var index = 0; index < sampleCount; index++)
|
||||
{
|
||||
var trajectoryTime = Math.Min(index * trajectoryStepSeconds, durationSeconds);
|
||||
var jointsRad = Interpolate(rows, trajectoryTime, ref segmentIndex);
|
||||
points[index] = new ExpectedPoint(
|
||||
index,
|
||||
trajectoryTime,
|
||||
jointsRad.Select(static value => value * 180.0 / Math.PI).ToArray());
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
private static double[] Interpolate(IReadOnlyList<JointRow> rows, double trajectoryTime, ref int segmentIndex)
|
||||
{
|
||||
if (rows.Count == 1 || trajectoryTime <= rows[0].TimeSeconds)
|
||||
{
|
||||
return rows[0].JointsRad.ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = rows.Count - 1;
|
||||
if (trajectoryTime >= rows[lastIndex].TimeSeconds)
|
||||
{
|
||||
return rows[lastIndex].JointsRad.ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && rows[segmentIndex + 1].TimeSeconds < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = rows[segmentIndex];
|
||||
var end = rows[segmentIndex + 1];
|
||||
var duration = end.TimeSeconds - start.TimeSeconds;
|
||||
var alpha = duration <= 0.0 ? 0.0 : (trajectoryTime - start.TimeSeconds) / duration;
|
||||
var joints = new double[JointCount];
|
||||
for (var index = 0; index < joints.Length; index++)
|
||||
{
|
||||
joints[index] = start.JointsRad[index] + ((end.JointsRad[index] - start.JointsRad[index]) * alpha);
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
private static (uint StartSequence, double RmsDeg) FindBestCommandWindow(
|
||||
IReadOnlyDictionary<uint, CapturedJ519Command> commandBySequence,
|
||||
IReadOnlyList<ExpectedPoint> expected,
|
||||
IReadOnlyList<FanucJ519Response> responseSegment,
|
||||
int searchRadius)
|
||||
{
|
||||
if (responseSegment.Count == 0)
|
||||
{
|
||||
throw new InvalidDataException("No status=15 response segment found.");
|
||||
}
|
||||
|
||||
var responseStartSequence = (long)responseSegment[0].Sequence;
|
||||
uint? bestStartSequence = null;
|
||||
var bestRms = double.PositiveInfinity;
|
||||
|
||||
for (var startSequence = responseStartSequence - searchRadius; startSequence <= responseStartSequence + searchRadius; startSequence++)
|
||||
{
|
||||
if (startSequence < 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var differences = new List<double>(expected.Count * JointCount);
|
||||
var completeWindow = true;
|
||||
for (var index = 0; index < expected.Count; index++)
|
||||
{
|
||||
var sequence = (uint)(startSequence + index);
|
||||
if (!commandBySequence.TryGetValue(sequence, out var command))
|
||||
{
|
||||
completeWindow = false;
|
||||
break;
|
||||
}
|
||||
|
||||
for (var joint = 0; joint < JointCount; joint++)
|
||||
{
|
||||
differences.Add(command.TargetDegrees[joint] - expected[index].JointsDeg[joint]);
|
||||
}
|
||||
}
|
||||
|
||||
if (!completeWindow)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var rms = Rms(differences);
|
||||
if (rms < bestRms)
|
||||
{
|
||||
bestRms = rms;
|
||||
bestStartSequence = (uint)startSequence;
|
||||
}
|
||||
}
|
||||
|
||||
if (bestStartSequence is null)
|
||||
{
|
||||
throw new InvalidDataException("No complete command window found near the status=15 response segment.");
|
||||
}
|
||||
|
||||
return (bestStartSequence.Value, bestRms);
|
||||
}
|
||||
|
||||
private static ComparisonSummary Compare(IReadOnlyList<CapturedJ519Command> actual, IReadOnlyList<ExpectedPoint> expected)
|
||||
{
|
||||
var differences = new List<double>(actual.Count * JointCount);
|
||||
var maxAbs = 0.0;
|
||||
for (var index = 0; index < actual.Count; index++)
|
||||
{
|
||||
for (var joint = 0; joint < JointCount; joint++)
|
||||
{
|
||||
var difference = actual[index].TargetDegrees[joint] - expected[index].JointsDeg[joint];
|
||||
differences.Add(difference);
|
||||
maxAbs = Math.Max(maxAbs, Math.Abs(difference));
|
||||
}
|
||||
}
|
||||
|
||||
var ioSetPulses = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue != 0);
|
||||
var ioClearFrames = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue == 0);
|
||||
return new ComparisonSummary(Rms(differences), maxAbs, ioSetPulses, ioClearFrames);
|
||||
}
|
||||
|
||||
private static double Rms(IEnumerable<double> values)
|
||||
{
|
||||
var sum = 0.0;
|
||||
var count = 0;
|
||||
foreach (var value in values)
|
||||
{
|
||||
sum += value * value;
|
||||
count++;
|
||||
}
|
||||
|
||||
return count == 0 ? 0.0 : Math.Sqrt(sum / count);
|
||||
}
|
||||
|
||||
private static uint ReadUInt32(ReadOnlySpan<byte> value, bool bigEndian)
|
||||
{
|
||||
return bigEndian ? BinaryPrimitives.ReadUInt32BigEndian(value) : BinaryPrimitives.ReadUInt32LittleEndian(value);
|
||||
}
|
||||
|
||||
private sealed record UdpPacket(
|
||||
int FrameNumber,
|
||||
double TimeRelativeSeconds,
|
||||
ushort SourcePort,
|
||||
ushort DestinationPort,
|
||||
byte[] Payload);
|
||||
|
||||
private sealed record CapturedJ519Command(
|
||||
int FrameNumber,
|
||||
double TimeRelativeSeconds,
|
||||
uint Sequence,
|
||||
byte LastData,
|
||||
ushort WriteIoMask,
|
||||
ushort WriteIoValue,
|
||||
IReadOnlyList<double> TargetDegrees);
|
||||
|
||||
private sealed record JointRow(double TimeSeconds, IReadOnlyList<double> JointsRad);
|
||||
|
||||
private sealed record ExpectedPoint(int Index, double TrajectoryTimeSeconds, IReadOnlyList<double> JointsDeg);
|
||||
|
||||
private sealed record ComparisonSummary(double GlobalRmsDeg, double GlobalMaxAbsDeg, int IoSetPulses, int IoClearFrames);
|
||||
}
|
||||
Reference in New Issue
Block a user