18 Commits

Author SHA1 Message Date
bcbf8017ad 📝 docs(planning): 统一 Rust 规划库命名
将 Rust 原生规划库设计口径统一为 flyshot_planning。

同步更新 Windows 和 Linux 产物名以及复制命令示例。
2026-06-27 15:10:06 +08:00
537c6b5320 💥 feat(planning)!: 拆分 C# ICSP 并默认 Rust
* 新增可选 Flyshot.CSharp.Planning 项目

* 将 icsp 映射到 Rust 后端并拒绝旧 icsp_rust

* 补充后端注册、文档、调试页和回归测试

BREAKING CHANGE: method=icsp 现在默认调用 Rust 后端;C# ICSP 改为 method=icsp-csharp,旧 method=icsp_rust 不再作为正式入口。
2026-06-27 15:03:05 +08:00
32cc740df5 ✨ feat(native-planning): 更新 Rust ICSP 原生库及构建脚本
* 将 Rust ICSP 原生库名称更改为 `flyshot_planning`,更新相关引用。
* 修改构建脚本以支持新的库名称,并确保库文件复制到正确的目录。
* 添加对 Rust ICSP 原生库的依赖检查,确保发布时包含必要的库文件。
* 更新测试项目以加载新的原生库,确保兼容性和功能完整性。
2026-06-26 17:47:04 +08:00
9c56ee825e ✨ feat(native-planning): 添加 Rust ICSP 原生规划核心及 C# 互操作集成
* 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口)
* 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装)
* 更新 ControllerClientCompat 适配新的规划接口
* 添加构建脚本 (Copy-NativePlanningArtifacts.ps1)
* 新增互操作测试及一致性对比测试
* 更新解决方案文件和设计文档
2026-06-26 15:41:01 +08:00
bb21e0923d docs(planning): 添加 Rust ICSP 原生核心设计 2026-06-26 15:41:01 +08:00
9a01ce851d Merge branch 'feat/offline-flyshot-planning' into dev 2026-06-24 11:43:44 +08:00
5193f537b0 ✨ feat(planning): 添加离线飞拍规划能力
* 固化离线飞拍轨迹加载、保存和规划结果。

* 真机初始化失败时保留模型加载后的离线规划能力。

* 更新状态接口、调试页和回归测试覆盖。
2026-06-24 11:40:07 +08:00
070d2ba5c0 ✨ feat(server): 增加混淆发布链路与调试页静态说明
- 新增 Obfuscar 混淆发布脚本、发布测试与产物隐私检查
- 调整 VS Code 发布任务和宿主发布配置,统一走混淆流程
- 为 debug 页面内嵌静态接口说明,并补充对应集成测试
- 更新 README 与设计计划文档,记录跨平台发布约束
2026-06-22 15:55:34 +08:00
ac41d04a54 ✨ feat(server): 添加服务启动自动初始化机器人功能
* 在 ControllerClientCompatOptions 中新增 AutoInitOptions 配置模型
* 创建 AutoInitRobotHostedService 实现启动时自动初始化链路
* 在 appsettings.json 的 ControllerClientCompat.AutoInit 节配置
  RobotName 和 RobotIp
* 启动时依次执行 SetUpRobot → SetActiveController → Connect →
  EnableRobot;配置缺失或连接失败不阻止服务启动
* 更新 README.md 和现场工作流文档
2026-06-22 13:29:04 +08:00
5c5eaf850d ✨ feat(docs): 添加飞拍控制系统宣传文稿与飞拍轨迹规划逻辑图 2026-06-10 19:52:22 +08:00
4aae6219cc Merge branch 'legacy-gateway-compat' 2026-05-28 11:02:01 +08:00
35bdb08d11 Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352' 2026-05-28 10:44:03 +08:00
0952958af7 ✨ feat(status): 添加点动摇杆请求日志
* 将状态页直角坐标点动改为 XYZ/WPR 双摇杆布局
* 添加 MovePose 请求日志与清空入口,便于现场观察下发内容
* 扩展状态页集成测试,覆盖摇杆控件和日志资源
2026-05-19 11:17:53 +08:00
2cd42f04e5 ✨ feat(fanuc): 添加直角坐标点动功能与相关接口
* 新增 `MovePose` 方法,支持以直角坐标执行点到点移动。
* 引入 `LegacyCartesianPoseRequest` 类,处理直角位姿请求体的解析与验证。
* 更新 `LegacyHttpApiController`,实现 `/move_pose/` 路由以支持新功能。
* 增强状态快照元数据,提供机器人初始化状态与已上传轨迹信息。
* 更新前端状态页面,增加直角坐标点动控制面板与步长设置选项。
* 相关文档与测试用例同步更新,确保新功能的完整性与稳定性。
2026-05-14 17:46:42 +08:00
d120ef2a39 ✨ feat(fanuc): 统一 speedRatio 执行倍率语义
* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列
* 区分旧格式规划导出与 ActualSend 实发诊断工件
* 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
2026-05-11 17:21:18 +08:00
d82128da4a ✨ feat(config): 更新 RobotConfig.json 以支持运行时速度倍率配置
* 在 RobotConfig.json 中新增 speed_ratio 配置,允许在运行时设置默认速度倍率。
* 调整 ControllerClientCompatService 以使用 speed_ratio 初始化机器人设置。
* 更新 TrajectoryLimitValidator 和 FlyshotExecutionSendSequenceBuilder,支持在飞拍链路中临时关闭 jerk 校验,仅保留速度和加速度约束。
* 新增文档记录对 UTTC_MS11 的 jerk 阻断策略调整,确保飞拍链路的执行效率。
* 增加单元测试以验证 speed_ratio 的加载和 jerk 校验的行为。
2026-05-11 14:44:58 +08:00
74761bb5da ✨ feat(flyshot): 引入飞拍执行侧最终发送队列构建与校验机制
* 新增 FlyshotExecutionSendSequenceBuilder,负责在运行时前构建最终 8ms 发送队列,并进行离散限幅校验。
* 引入 FlyshotPreparedExecution 类,封装最终发送结果及相关诊断信息。
* 调整 ControllerClientCompatService 和 FanucControllerRuntime,确保运行时直接使用预生成的发送队列,避免临场重采样。
* 更新 TrajectoryResult 和 PlannedExecutionBundle,支持准备好的执行队列。
* 增加单元测试,验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
2026-05-09 19:06:49 +08:00
f7e2bb0e7b ✨ feat(*): 添加触发样本偏移与实发轨迹分析导出
* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置
  * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发
  * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出
  * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档
  * 补齐配置兼容、规划绑定和运行时触发相关测试
2026-05-09 11:12:31 +08:00
126 changed files with 18429 additions and 1287 deletions

8
.gitignore vendored
View File

@@ -400,3 +400,11 @@ Config/Data/*
.dotnet-home/*
codex-dotnet-home/*
.dotnet-sdk8/*
# Flyshot 混淆发布产物、映射文件和临时工作目录
outputs/obfuscated/
outputs/obfuscation-maps/
.tmp/obfuscation/
# Rust 原生规划库构建产物
**/target/

15
.vscode/tasks.json vendored
View File

@@ -142,19 +142,16 @@
}
},
{
// Release 配置发布到 publish/,用于现场部署包打包
// 调用环境中的 Obfuscar,生成 Windows/Linux 两套自包含混淆发布包
"label": "publish",
"command": "dotnet",
"command": "pwsh",
"type": "process",
"args": [
"publish",
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
"-c",
"Release",
"-o",
"${workspaceFolder}/publish"
"-NoProfile",
"-File",
"${workspaceFolder}/scripts/Publish-Obfuscated.ps1"
],
"problemMatcher": "$msCompile"
"problemMatcher": []
}
]
}

View File

@@ -6,11 +6,13 @@
8
],
"io_keep_cycles": 2,
"trigger_sample_index_offset_cycles": 6,
"acc_limit": 1,
"jerk_limit": 1,
"adapt_icsp_try_num": 5,
"speed_ratio": 1,
"planning_speed_scale": 0.74227,
"smooth_start_stop_timing": false
"smooth_start_stop_timing": true
},
"flying_shots": {
"UTTC_MS11": {
@@ -300,4 +302,4 @@
]
}
}
}
}

View File

@@ -1,4 +1,3 @@

Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17
VisualStudioVersion = 17.0.31903.59
@@ -27,6 +26,10 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.ControllerClientCom
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Fanuc", "src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj", "{B705FA6C-19CA-44A8-882C-6CE26A5379C9}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Native.Planning", "src\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj", "{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.CSharp.Planning", "src\Flyshot.CSharp.Planning\Flyshot.CSharp.Planning.csproj", "{4717558C-0A09-4D4D-BB1A-C0FD9241070E}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
@@ -157,6 +160,30 @@ Global
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.Build.0 = Release|Any CPU
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.ActiveCfg = Release|Any CPU
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.Build.0 = Release|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|Any CPU.Build.0 = Debug|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x64.ActiveCfg = Debug|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x64.Build.0 = Debug|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x86.ActiveCfg = Debug|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x86.Build.0 = Debug|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|Any CPU.ActiveCfg = Release|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|Any CPU.Build.0 = Release|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x64.ActiveCfg = Release|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x64.Build.0 = Release|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x86.ActiveCfg = Release|Any CPU
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x86.Build.0 = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|Any CPU.Build.0 = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x64.ActiveCfg = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x64.Build.0 = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x86.ActiveCfg = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x86.Build.0 = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|Any CPU.ActiveCfg = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|Any CPU.Build.0 = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x64.ActiveCfg = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x64.Build.0 = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x86.ActiveCfg = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x86.Build.0 = Release|Any CPU
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
@@ -172,5 +199,7 @@ Global
{E4DDC34C-9AB6-4050-A927-3DF69804708A} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{B705FA6C-19CA-44A8-882C-6CE26A5379C9} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{4717558C-0A09-4D4D-BB1A-C0FD9241070E} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
EndGlobalSection
EndGlobal

View File

@@ -16,13 +16,18 @@
- 当前仓库不再恢复旧 `50001/TCP+JSON` 监听入口;旧 `ControllerClient` 逆向资料只作为接口语义参考,不作为运行时目标。
- 宿主只保留 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.Core.Planning` 保留规划公共模型、样条、采样、导出和运动学能力;C# ICSP / self-adapt-icsp 已拆分到可选项目 `Flyshot.CSharp.Planning`。
- `Flyshot.Native.Planning` 已提供默认 Rust ICSP 规划入口,通过 C ABI 调用 `flyshot_planning`;普通轨迹和飞拍轨迹的默认 `method="icsp"` 使用 Rust 后端,`method="icsp-csharp"` 和 `method="self-adapt-icsp"` 只有显式注册 C# 后端时可用。
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。
- 2026-04-28 UTTC 抓包确认:UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`,`JointDetialTraj.txt` 为弧度制 `rad`,`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸;2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长,当前实际生成约 `7.4s` 轨迹。
- 2026-04-30 本机 `50001/TCP+JSON` 抓包确认:`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数,不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路,新系统按 replacement-only 内部参数限制规划加速度。
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送能力;稠密轨迹下发已按 `speed_ratio` 做执行时间缩放,并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。
- 2026-05-11 对 `UTTC_MS11` 的独立 `ConfigRoot` 重新生成确认:flyshot 链路若继续用离散 `jerk` 作为阻断条件,会把新结果拉长到约 `16.576s / 33.152s`;只保留速度、加速度校验并跳过 flyshot jerk 阻断后,新 `1.0x / 0.5x` 时长回落到约 `7.552s / 15.104s`,与旧抓包主段 `7.408s / 14.808s` 的差异已很小,当前按可忽略处理。详见 `docs/uttc-ms11-jerk-validation-decision-20260511.md`。
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹:当前关节到目标关节的关节空间直线,五次 smoothstep 起停,按 `status=15` 运动窗口复现 `40/55/77` 点,并由 J519 层完成 `rad -> deg` 下发。
- 单程序只对应一台机器人,上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`,不再创建独立轨迹存储文件。
- 服务启动时支持真机优先初始化:在 `ControllerClientCompat.AutoInit` 配置 `RobotName` 和 `RobotIp`,启动后执行 `SetUpRobot → Connect → EnableRobot(8)`;连接失败后进入 `OfflineReady`,模型仍可用于离线飞拍规划。
- 飞拍资产与机器人型号解耦:未加载模型时也可上传、列出和删除;模型自由度与关节限制延迟到规划阶段校验。
- `/save_traj_info/` 已成为离线规划入口,返回最终 8ms 发送点列、ShotEvents、触发绑定和全部导出文件绝对路径;`/execute_flyshot/` 仍只允许 `RealReady`。
单位约定总览:
@@ -60,16 +65,48 @@
- [x] 打通最小宿主与 `/healthz`
- [x] 建立领域模型与模块边界
- [x] 落地配置兼容与机器人模型解析
- [x] 落地 ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴
- [x] 落地 Rust ICSP、可选 C# ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴
- [x] 完成 ICSP 轨迹导出结果与旧系统对齐
- [x] 增加 Rust `cdylib` ICSP 核心、C# P/Invoke 适配和 C#/Rust 自动对拍
- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路
- [x] 落地 Web 状态页
- [x] 落地浏览器内 OpenAPI 自动驱动的接口调试页(`/debug`),与 `Swagger:Enabled` 同步可见
- [x] 调试页的人类可读说明文本已静态内嵌到 `debug.html`,即使发布产物删除 XML 文档也能继续展示接口提示
- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码,确认 `10010` 状态帧为 90B
- [x] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发
- [x] 补齐 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 真机命令体与响应解析
- [x] 保留新 HTTP 接口路线,明确不再实现旧 `50001/TCP+JSON` 网关
- [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.json`
- [x] 服务启动自动初始化机器人:通过 `ControllerClientCompat.AutoInit` 配置节指定 `RobotName` 和 `RobotIp`,启动时自动执行初始化链路
- [x] 完成真机优先与离线飞拍规划改造:`RealReady / OfflineReady / ModelLoadFailed` 状态、固定轨迹资产、完整离线规划响应和 Debug 页面
- [x] 删除生产仿真切换 HTTP 入口及旧 `/init_mpc_robt`,统一使用 `/init_mpc_robot`
### 规划后端与 Rust ICSP 原生库构建
普通轨迹或飞拍轨迹的默认参数 `method="icsp"` 会调用 Rust ICSP 后端。旧 `method="icsp_rust"` 已不再作为正式入口,调用时会提示改用 `method="icsp"`;Rust 原生库缺失时会明确报错,不会静默回退 C#。
C# `ICspPlanner` 与 `SelfAdaptIcspPlanner` 已拆分到 `Flyshot.CSharp.Planning.dll`。默认服务项目不引用该 DLL;只有在部署方显式引用并调用 `AddCSharpPlanningBackends()` 后,`method="icsp-csharp"` 与 `method="self-adapt-icsp"` 才可用。
Windows x64 需要安装:
- `rustup` 的 `stable-x86_64-pc-windows-msvc` 工具链。
- Visual Studio 或 Visual Studio Build Tools 的 C++ x64 编译工具和 Windows SDK。
在仓库根目录执行:
```powershell
.\build\Copy-NativePlanningArtifacts.ps1 -Configuration Release
```
脚本会执行 `cargo build --release`,构建
`native/flyshot-native-planning/target/release/flyshot_planning.dll`,
并默认复制到 `src/Libs/flyshot_planning.dll` 与
`tests/Flyshot.Core.Tests/bin/Release/net8.0/`。发布服务时可通过
`-DestinationDirectories` 把原生库复制到实际发布目录。
Linux x64 在已安装 `stable-x86_64-unknown-linux-gnu` 工具链的 Linux 环境执行同一脚本,
产物为 `libflyshot_planning.so`,默认写入 `src/Libs/libflyshot_planning.so`。
Windows 第一阶段不要求交叉编译 Linux 原生库。
剩余 Todo:
@@ -119,6 +156,48 @@
- [ ] 增加运行日志、协议摘要日志和状态快照导出,便于现场排查。
7. 发布与部署
- [x] 增加 Obfuscar 跨平台混淆发布脚本,全部 `Flyshot.*` 自研程序集会生成一份不区分运行平台的 .NET 8 框架依赖部署包。
- [ ] 固化 Windows / Linux 启动脚本和 systemd 服务配置。
- [ ] 补充生产配置模板、端口说明和现场部署检查表。
- [ ] 给 Web 状态页增加程序状态和最近报警显示;J519 状态位已通过快照和状态页显示。
## 混淆发布
发布脚本直接依赖构建环境,不在仓库中引用或还原 Obfuscar 包。首次使用前安装全局工具:
```powershell
dotnet tool install --global Obfuscar.GlobalTool --version 2.2.50
```
安装后确认 `obfuscar.console` 已加入当前终端的 `PATH`,然后在仓库根目录执行:
```powershell
pwsh -NoProfile -File .\scripts\Publish-Obfuscated.ps1
```
也可以在 VS Code 中运行 `publish` 任务。脚本不再接受 RID 或运行平台参数,默认生成:
- `outputs/obfuscated/framework-dependent/`:Windows 与 Linux 共用的 .NET 8 框架依赖混淆部署包。
- `outputs/obfuscation-maps/framework-dependent/Mapping.txt`:仅供内部还原异常堆栈使用,不得交付给用户。
Visual Studio 的 `FolderProfile` 已接入同一套混淆流程。在“发布”页面选择 `FolderProfile` 并点击“发布”后,Visual Studio 会生成 .NET 8 框架依赖多文件包,再自动混淆发布目录中的全部 `Flyshot.*.dll`。如果环境缺少 `obfuscar.console`、缺少对应的 .NET 8 共享框架或混淆失败,Visual Studio 发布会直接失败,不会静默留下未保护产物。
混淆范围包含发布目录中的全部 `Flyshot.*.dll` 自研程序集,并校验当前八个核心程序集不可缺失;后续新增的 `Flyshot.*` 模块也会自动纳入。第三方程序集不会被处理;公共 API、属性和事件名称会保留,以维持 ASP.NET Core 控制器发现、依赖注入、配置绑定、JSON 序列化和 Swagger 行为,内部类型、私有方法及字段会执行重命名。
若环境缺少 `obfuscar.console`、发布目录缺少任一自研程序集、混淆器返回失败或未生成完整输出,脚本会返回非零退出码并停止,不会把未完整保护的产物作为成功结果交付。
混淆完成后会删除发布目录中的 `Flyshot*.xml` XML 文档和 `Flyshot*.pdb` 调试符号,避免反编译器自动显示源码文档注释。生产环境 Swagger 因此不再展示来自 XML 文档的接口说明,但接口路由和调用不受影响。
启动部署包时必须先进入对应 RID 的发布目录,确保宿主能从当前目录加载 `appsettings.json`、`NLog.config` 和 `Config/`:
```powershell
Set-Location .\outputs\obfuscated\framework-dependent
dotnet .\Flyshot.Server.Host.dll
```
```bash
cd outputs/obfuscated/framework-dependent
dotnet Flyshot.Server.Host.dll
```
Windows 与 Linux 目标机器都必须预先安装 .NET 8 ASP.NET Core Runtime。

View File

@@ -0,0 +1,21 @@
{
"pcap_path": "D:\\Dev\\Codes\\rvbust-code\\FlyingShotPkg_3.15_VDA\\Rvbust\\uttc-20260428\\2026042802-1.pcap",
"all_command_count": 1788,
"trigger_count": 17,
"existing_runtime_actual_send_exists": true,
"existing_runtime_actual_send_has_io_columns": false,
"existing_shot_events_exists": true,
"pcap_specific_combined_export_preexisting": false,
"average_max_error_deg": 4.241583591714439,
"max_error_deg": 11.130744251720401,
"average_rms_error_deg": 2.5406136706026206,
"max_error_axis_counter": {
"J6": 9,
"J4": 5,
"J5": 1,
"J2": 1,
"J1": 1
},
"order_only_addr_mismatch_count": 14,
"real_addr_mismatch_count": 0
}

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,18 @@
frame_number,time_relative_s,sequence,last_data,write_io_type,write_io_index,write_io_mask,write_io_value,io_addrs,j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,ext1_deg,ext2_deg,ext3_deg
1955,5.888271,1381124,0,2,1,10,10,"[2, 4]",45.8632698059082,2.9724788665771484,-13.609341621398926,-2.1851141452789307,7.582361221313477,5.701517581939697,0.0,0.0,0.0
2151,6.480252,1381198,0,2,1,14,14,"[2, 3, 4]",56.34336471557617,11.66723918914795,-6.600923538208008,-68.1976089477539,6.058437347412109,71.5392837524414,0.0,0.0,0.0
2223,6.704225,1381226,0,2,1,14,14,"[2, 3, 4]",53.50831985473633,7.412222862243652,-9.629739761352539,-32.30571365356445,12.488306045532227,30.826189041137695,0.0,0.0,0.0
2336,7.048266,1381269,0,2,1,10,10,"[2, 4]",44.433006286621094,-2.1156294345855713,-18.39777374267578,4.8371992111206055,33.99372482299805,-20.26228904724121,0.0,0.0,0.0
2477,7.464289,1381321,0,2,1,10,10,"[2, 4]",65.84967041015625,5.695003032684326,-14.563636779785156,-28.69342613220215,27.678611755371094,46.437156677246094,0.0,0.0,0.0
2665,8.024277,1381391,0,2,1,12,12,"[3, 4]",60.847530364990234,24.47777557373047,-5.617043972015381,34.8044319152832,5.786255359649658,-31.2253475189209,0.0,0.0,0.0
2811,8.472317,1381447,0,2,1,12,12,"[3, 4]",70.8250503540039,10.95259952545166,-13.136046409606934,-34.68459701538086,14.022420883178711,38.90604019165039,0.0,0.0,0.0
2908,8.760361,1381483,0,2,1,10,10,"[2, 4]",69.40516662597656,-20.066781997680664,-8.152935981750488,-63.93235397338867,6.775912284851074,70.37761688232422,0.0,0.0,0.0
3112,9.384332,1381561,0,2,1,10,10,"[2, 4]",73.90592956542969,-8.016124725341797,-9.002426147460938,-131.3327178955078,15.526206970214844,142.41668701171875,0.0,0.0,0.0
3170,9.560358,1381583,0,2,1,10,10,"[2, 4]",76.72688293457031,-16.518726348876953,-7.15147066116333,-127.53003692626953,18.53788948059082,138.7897491455078,0.0,0.0,0.0
3350,10.104322,1381651,0,2,1,10,10,"[2, 4]",83.71662139892578,-13.186573028564453,-13.738808631896973,-72.70317840576172,26.66758918762207,91.98423767089844,0.0,0.0,0.0
3470,10.472333,1381697,0,2,1,10,10,"[2, 4]",59.78955841064453,-2.7094168663024902,-8.933828353881836,-108.24857330322266,21.177492141723633,114.14110565185547,0.0,0.0,0.0
3627,10.944371,1381756,0,2,1,12,12,"[3, 4]",86.41329193115234,4.266173362731934,-19.15987777709961,-52.24028015136719,29.853723526000977,55.86907196044922,0.0,0.0,0.0
3710,11.192339,1381787,0,2,1,10,10,"[2, 4]",110.7523193359375,0.8751887679100037,-34.756534576416016,-48.14332580566406,45.0865592956543,87.97444152832031,0.0,0.0,0.0
3796,11.456361,1381820,0,2,1,10,10,"[2, 4]",111.79177856445312,-6.225440502166748,-31.958988189697266,-53.68573760986328,44.76150894165039,89.33454895019531,0.0,0.0,0.0
3870,11.680362,1381848,0,2,1,10,10,"[2, 4]",117.87474060058594,-19.607177734375,-30.329103469848633,-56.600250244140625,49.08655548095703,90.32424926757812,0.0,0.0,0.0
4138,12.496365,1381950,0,2,1,12,12,"[3, 4]",59.93589401245117,-21.102140426635742,-9.684239387512207,70.64311981201172,32.941307067871094,-63.34855651855469,0.0,0.0,0.0
1 frame_number time_relative_s sequence last_data write_io_type write_io_index write_io_mask write_io_value io_addrs j1_deg j2_deg j3_deg j4_deg j5_deg j6_deg ext1_deg ext2_deg ext3_deg
2 1955 5.888271 1381124 0 2 1 10 10 [2, 4] 45.8632698059082 2.9724788665771484 -13.609341621398926 -2.1851141452789307 7.582361221313477 5.701517581939697 0.0 0.0 0.0
3 2151 6.480252 1381198 0 2 1 14 14 [2, 3, 4] 56.34336471557617 11.66723918914795 -6.600923538208008 -68.1976089477539 6.058437347412109 71.5392837524414 0.0 0.0 0.0
4 2223 6.704225 1381226 0 2 1 14 14 [2, 3, 4] 53.50831985473633 7.412222862243652 -9.629739761352539 -32.30571365356445 12.488306045532227 30.826189041137695 0.0 0.0 0.0
5 2336 7.048266 1381269 0 2 1 10 10 [2, 4] 44.433006286621094 -2.1156294345855713 -18.39777374267578 4.8371992111206055 33.99372482299805 -20.26228904724121 0.0 0.0 0.0
6 2477 7.464289 1381321 0 2 1 10 10 [2, 4] 65.84967041015625 5.695003032684326 -14.563636779785156 -28.69342613220215 27.678611755371094 46.437156677246094 0.0 0.0 0.0
7 2665 8.024277 1381391 0 2 1 12 12 [3, 4] 60.847530364990234 24.47777557373047 -5.617043972015381 34.8044319152832 5.786255359649658 -31.2253475189209 0.0 0.0 0.0
8 2811 8.472317 1381447 0 2 1 12 12 [3, 4] 70.8250503540039 10.95259952545166 -13.136046409606934 -34.68459701538086 14.022420883178711 38.90604019165039 0.0 0.0 0.0
9 2908 8.760361 1381483 0 2 1 10 10 [2, 4] 69.40516662597656 -20.066781997680664 -8.152935981750488 -63.93235397338867 6.775912284851074 70.37761688232422 0.0 0.0 0.0
10 3112 9.384332 1381561 0 2 1 10 10 [2, 4] 73.90592956542969 -8.016124725341797 -9.002426147460938 -131.3327178955078 15.526206970214844 142.41668701171875 0.0 0.0 0.0
11 3170 9.560358 1381583 0 2 1 10 10 [2, 4] 76.72688293457031 -16.518726348876953 -7.15147066116333 -127.53003692626953 18.53788948059082 138.7897491455078 0.0 0.0 0.0
12 3350 10.104322 1381651 0 2 1 10 10 [2, 4] 83.71662139892578 -13.186573028564453 -13.738808631896973 -72.70317840576172 26.66758918762207 91.98423767089844 0.0 0.0 0.0
13 3470 10.472333 1381697 0 2 1 10 10 [2, 4] 59.78955841064453 -2.7094168663024902 -8.933828353881836 -108.24857330322266 21.177492141723633 114.14110565185547 0.0 0.0 0.0
14 3627 10.944371 1381756 0 2 1 12 12 [3, 4] 86.41329193115234 4.266173362731934 -19.15987777709961 -52.24028015136719 29.853723526000977 55.86907196044922 0.0 0.0 0.0
15 3710 11.192339 1381787 0 2 1 10 10 [2, 4] 110.7523193359375 0.8751887679100037 -34.756534576416016 -48.14332580566406 45.0865592956543 87.97444152832031 0.0 0.0 0.0
16 3796 11.456361 1381820 0 2 1 10 10 [2, 4] 111.79177856445312 -6.225440502166748 -31.958988189697266 -53.68573760986328 44.76150894165039 89.33454895019531 0.0 0.0 0.0
17 3870 11.680362 1381848 0 2 1 10 10 [2, 4] 117.87474060058594 -19.607177734375 -30.329103469848633 -56.600250244140625 49.08655548095703 90.32424926757812 0.0 0.0 0.0
18 4138 12.496365 1381950 0 2 1 12 12 [3, 4] 59.93589401245117 -21.102140426635742 -9.684239387512207 70.64311981201172 32.941307067871094 -63.34855651855469 0.0 0.0 0.0

View File

@@ -0,0 +1,37 @@
{
"pcap_path": "D:\\Dev\\Codes\\rvbust-code\\FlyingShotPkg_3.15_VDA\\Rvbust\\uttc-20260428\\2026042802-1.pcap",
"command_count": 1788,
"trigger_count": 17,
"command_minus_paired_status_sequence_counter": {
"8": 17
},
"paired_status_average_max_error_deg": 6.469219290127501,
"paired_status_max_error_deg": 16.123934474475888,
"paired_status_max_error_axis_counter": {
"J1": 2,
"J6": 9,
"J2": 4,
"J4": 2
},
"best_status_average_max_error_deg": 0.14953970473465206,
"best_status_max_error_deg": 0.5696919293327056,
"best_status_max_error_axis_counter": {
"J6": 10,
"J1": 3,
"J5": 1,
"J2": 2,
"J4": 1
},
"best_status_delta_from_paired_cycles_counter": {
"10": 8,
"9": 9
},
"best_status_delta_from_trigger_sequence_counter": {
"2": 8,
"1": 9
},
"best_status_time_after_trigger_ms_min": 71.83799999999962,
"best_status_time_after_trigger_ms_max": 79.96399999999859,
"best_status_time_after_trigger_ms_avg": 75.6613529411763,
"search_window_cycles": 20
}

View File

@@ -0,0 +1,18 @@
trigger_no,waypoint_index,trigger_command_sequence,trigger_command_frame,trigger_command_time_relative_s,trigger_current_status_sequence,trigger_current_status_frame,trigger_current_status_time_relative_s,command_leads_status_cycles,trigger_current_status_max_error_axis,trigger_current_status_max_error_deg,trigger_current_status_rms_error_deg,best_status_sequence,best_status_frame,best_status_time_relative_s,best_status_delay_from_current_status_cycles,best_status_delay_from_trigger_command_cycles,best_status_delay_from_trigger_command_ms,best_status_max_error_axis,best_status_max_error_deg,best_status_rms_error_deg
1,1,1381124,1955,5.888271,1381116,1954,5.888166,8,J1,6.588241610414407,3.8225443630072706,1381126,1981,5.968124,10,2,79.85300000000083,J6,0.20758988641565335,0.12239685259209541
2,2,1381198,2151,6.480252,1381190,2150,6.480138,8,J1,2.33531536749085,1.3827543088729335,1381199,2172,6.552164,9,1,71.9120000000002,J1,0.08213390997131853,0.04616054814443023
3,3,1381226,2223,6.704225,1381218,2222,6.704166,8,J6,16.123934474475888,8.852376616648156,1381228,2248,6.784154,10,2,79.92899999999992,J6,0.5696919293327056,0.3239642953120798
4,4,1381269,2336,7.048266,1381261,2335,7.048148,8,J6,4.901882807288516,3.2440556235708993,1381271,2362,7.128175,10,2,79.90899999999979,J6,0.09470430997406254,0.06580474324148583
5,5,1381321,2477,7.464289,1381313,2476,7.464178,8,J6,9.21943820204428,4.621321121191458,1381323,2502,7.544122,10,2,79.83299999999983,J5,0.056622412878706285,0.03357478517689402
6,6,1381391,2665,8.024277,1381383,2664,8.024156,8,J6,6.1464605154096255,3.0478405356855083,1381392,2687,8.096181,9,1,71.90399999999997,J6,0.17697427883735983,0.08563125303054821
7,7,1381447,2811,8.472317,1381439,2810,8.472208,8,J6,13.15171783052326,7.78202379046887,1381448,2834,8.54419,9,1,71.87300000000008,J6,0.30658691011310424,0.18210669390591905
8,8,1381483,2908,8.760361,1381475,2907,8.760245,8,J6,7.436279407197546,5.127041386597523,1381484,2929,8.832199,9,1,71.83799999999962,J2,0.06074767130727565,0.02750705896232285
9,9,1381561,3112,9.384332,1381553,3111,9.384204,8,J6,5.585605293050492,3.277206017832995,1381562,3134,9.456205,9,1,71.87300000000008,J4,0.15225994654176134,0.089150370994325
10,10,1381583,3170,9.560358,1381575,3169,9.56025,8,J2,4.165077087081428,2.128983861449047,1381585,3196,9.640214,10,2,79.85599999999948,J6,0.0734395372110157,0.043285948566531826
11,11,1381651,3350,10.104322,1381643,3349,10.104215,8,J2,1.8309533977318093,1.169340011162825,1381652,3373,10.17622,9,1,71.8980000000009,J6,0.07766831894582538,0.04246508921571141
12,12,1381697,3470,10.472333,1381689,3469,10.472191,8,J4,4.822353276363401,2.8611252897135864,1381698,3493,10.544283,9,1,71.94999999999929,J1,0.021077280093038553,0.014935517911187784
13,13,1381756,3627,10.944371,1381748,3626,10.944251,8,J4,8.152507963730145,5.168659196117862,1381757,3651,11.016226,9,1,71.85499999999934,J1,0.14554304541260876,0.0699602863618738
14,14,1381787,3710,11.192339,1381779,3709,11.19225,8,J6,10.40441920064211,5.679810575214033,1381789,3735,11.272261,10,2,79.92199999999983,J6,0.11945317484617135,0.05835950688086092
15,15,1381820,3796,11.456361,1381812,3795,11.456253,8,J2,2.6161077284098373,1.5911754635072362,1381822,3821,11.536287,10,2,79.92600000000039,J2,0.14980253942005772,0.08716897465096986
16,16,1381848,3870,11.680362,1381840,3869,11.680263,8,J2,4.682473812334727,2.2067399623329567,1381850,3895,11.760326,10,2,79.96399999999859,J6,0.17295643126782068,0.10419421346699378
17,18,1381950,4138,12.496365,1381942,4137,12.496271,8,J6,1.8139599579791934,1.1536991630766555,1381951,4159,12.568313,9,1,71.94799999999901,J6,0.07492339792059965,0.03834497543010648
1 trigger_no waypoint_index trigger_command_sequence trigger_command_frame trigger_command_time_relative_s trigger_current_status_sequence trigger_current_status_frame trigger_current_status_time_relative_s command_leads_status_cycles trigger_current_status_max_error_axis trigger_current_status_max_error_deg trigger_current_status_rms_error_deg best_status_sequence best_status_frame best_status_time_relative_s best_status_delay_from_current_status_cycles best_status_delay_from_trigger_command_cycles best_status_delay_from_trigger_command_ms best_status_max_error_axis best_status_max_error_deg best_status_rms_error_deg
2 1 1 1381124 1955 5.888271 1381116 1954 5.888166 8 J1 6.588241610414407 3.8225443630072706 1381126 1981 5.968124 10 2 79.85300000000083 J6 0.20758988641565335 0.12239685259209541
3 2 2 1381198 2151 6.480252 1381190 2150 6.480138 8 J1 2.33531536749085 1.3827543088729335 1381199 2172 6.552164 9 1 71.9120000000002 J1 0.08213390997131853 0.04616054814443023
4 3 3 1381226 2223 6.704225 1381218 2222 6.704166 8 J6 16.123934474475888 8.852376616648156 1381228 2248 6.784154 10 2 79.92899999999992 J6 0.5696919293327056 0.3239642953120798
5 4 4 1381269 2336 7.048266 1381261 2335 7.048148 8 J6 4.901882807288516 3.2440556235708993 1381271 2362 7.128175 10 2 79.90899999999979 J6 0.09470430997406254 0.06580474324148583
6 5 5 1381321 2477 7.464289 1381313 2476 7.464178 8 J6 9.21943820204428 4.621321121191458 1381323 2502 7.544122 10 2 79.83299999999983 J5 0.056622412878706285 0.03357478517689402
7 6 6 1381391 2665 8.024277 1381383 2664 8.024156 8 J6 6.1464605154096255 3.0478405356855083 1381392 2687 8.096181 9 1 71.90399999999997 J6 0.17697427883735983 0.08563125303054821
8 7 7 1381447 2811 8.472317 1381439 2810 8.472208 8 J6 13.15171783052326 7.78202379046887 1381448 2834 8.54419 9 1 71.87300000000008 J6 0.30658691011310424 0.18210669390591905
9 8 8 1381483 2908 8.760361 1381475 2907 8.760245 8 J6 7.436279407197546 5.127041386597523 1381484 2929 8.832199 9 1 71.83799999999962 J2 0.06074767130727565 0.02750705896232285
10 9 9 1381561 3112 9.384332 1381553 3111 9.384204 8 J6 5.585605293050492 3.277206017832995 1381562 3134 9.456205 9 1 71.87300000000008 J4 0.15225994654176134 0.089150370994325
11 10 10 1381583 3170 9.560358 1381575 3169 9.56025 8 J2 4.165077087081428 2.128983861449047 1381585 3196 9.640214 10 2 79.85599999999948 J6 0.0734395372110157 0.043285948566531826
12 11 11 1381651 3350 10.104322 1381643 3349 10.104215 8 J2 1.8309533977318093 1.169340011162825 1381652 3373 10.17622 9 1 71.8980000000009 J6 0.07766831894582538 0.04246508921571141
13 12 12 1381697 3470 10.472333 1381689 3469 10.472191 8 J4 4.822353276363401 2.8611252897135864 1381698 3493 10.544283 9 1 71.94999999999929 J1 0.021077280093038553 0.014935517911187784
14 13 13 1381756 3627 10.944371 1381748 3626 10.944251 8 J4 8.152507963730145 5.168659196117862 1381757 3651 11.016226 9 1 71.85499999999934 J1 0.14554304541260876 0.0699602863618738
15 14 14 1381787 3710 11.192339 1381779 3709 11.19225 8 J6 10.40441920064211 5.679810575214033 1381789 3735 11.272261 10 2 79.92199999999983 J6 0.11945317484617135 0.05835950688086092
16 15 15 1381820 3796 11.456361 1381812 3795 11.456253 8 J2 2.6161077284098373 1.5911754635072362 1381822 3821 11.536287 10 2 79.92600000000039 J2 0.14980253942005772 0.08716897465096986
17 16 16 1381848 3870 11.680362 1381840 3869 11.680263 8 J2 4.682473812334727 2.2067399623329567 1381850 3895 11.760326 10 2 79.96399999999859 J6 0.17295643126782068 0.10419421346699378
18 17 18 1381950 4138 12.496365 1381942 4137 12.496271 8 J6 1.8139599579791934 1.1536991630766555 1381951 4159 12.568313 9 1 71.94799999999901 J6 0.07492339792059965 0.03834497543010648

View File

@@ -0,0 +1,18 @@
trigger_no,waypoint_index,best_sample_order,best_frame_number,best_sequence,best_time_relative_s,teach_j1_deg,teach_j2_deg,teach_j3_deg,teach_j4_deg,teach_j5_deg,teach_j6_deg,offset_6_frame_number,offset_6_sequence,offset_6_time_relative_s,offset_6_max_error_axis,offset_6_max_error_deg,offset_6_rms_error_deg,offset_6_delta_from_best_ms,offset_6_j1_actual_deg,offset_6_diff_j1_deg,offset_6_j2_actual_deg,offset_6_diff_j2_deg,offset_6_j3_actual_deg,offset_6_diff_j3_deg,offset_6_j4_actual_deg,offset_6_diff_j4_deg,offset_6_j5_actual_deg,offset_6_diff_j5_deg,offset_6_j6_actual_deg,offset_6_diff_j6_deg,offset_7_frame_number,offset_7_sequence,offset_7_time_relative_s,offset_7_max_error_axis,offset_7_max_error_deg,offset_7_rms_error_deg,offset_7_delta_from_best_ms,offset_7_j1_actual_deg,offset_7_diff_j1_deg,offset_7_j2_actual_deg,offset_7_diff_j2_deg,offset_7_j3_actual_deg,offset_7_diff_j3_deg,offset_7_j4_actual_deg,offset_7_diff_j4_deg,offset_7_j5_actual_deg,offset_7_diff_j5_deg,offset_7_j6_actual_deg,offset_7_diff_j6_deg,offset_8_frame_number,offset_8_sequence,offset_8_time_relative_s,offset_8_max_error_axis,offset_8_max_error_deg,offset_8_rms_error_deg,offset_8_delta_from_best_ms,offset_8_j1_actual_deg,offset_8_diff_j1_deg,offset_8_j2_actual_deg,offset_8_diff_j2_deg,offset_8_j3_actual_deg,offset_8_diff_j3_deg,offset_8_j4_actual_deg,offset_8_diff_j4_deg,offset_8_j5_actual_deg,offset_8_diff_j5_deg,offset_8_j6_actual_deg,offset_8_diff_j6_deg,best_of_6_7_8_offset
1,1,730,1941,1381118,5.840207,48.886810269468405,2.1989850886957285,-11.021017368511105,0.4102097980549552,6.248381265333557,2.294990756284542,1955,1381124,5.888271,J6,3.4065268256551553,2.468438977201345,48.06399999999922,45.8632698059082,-3.0235404635602023,2.9724788665771484,0.7734937778814199,-13.609341621398926,-2.588324252887821,-2.1851141452789307,-2.5953239433338857,7.582361221313477,1.3339799559799195,5.701517581939697,3.4065268256551553,1958,1381125,5.896184,J6,4.180931828249149,2.9355776062802286,55.9769999999995,45.449710845947266,-3.43709942352114,3.117816925048828,0.9188318363530996,-13.963269233703613,-2.9422518651925085,-2.845515012741089,-3.255724810796044,7.768581867218018,1.5202006018844605,6.475922584533691,4.180931828249149,1961,1381126,5.904272,J6,5.0045521476156045,3.418446646752538,64.06499999999937,45.06731414794922,-3.8194961215191867,3.2660434246063232,1.0670583359105947,-14.290565490722656,-3.2695481222115514,-3.562589168548584,-3.972798966603539,7.9421586990356445,1.6937774337020874,7.2995429039001465,5.0045521476156045,6
2,2,804,2135,1381192,6.432175,55.34775509527405,11.807039833001637,-7.0090952672806885,-71.01433145543973,6.012065051914967,74.24953191606797,2151,1381198,6.480252,J4,2.8167225076858244,1.6562461612538633,48.07700000000015,56.34336471557617,0.995609620302119,11.66723918914795,-0.13980064385368784,-6.600923538208008,0.40817172907268073,-68.1976089477539,2.8167225076858244,6.058437347412109,0.04637229549714217,71.5392837524414,-2.7102481636265594,2153,1381199,6.488246,J4,3.526759739131137,2.060948429836345,56.071000000000204,56.451515197753906,1.1037601024798533,11.618118286132812,-0.18892154686882456,-6.580313682556152,0.4287815847245362,-67.4875717163086,3.526759739131137,6.09804105758667,0.08597600567170272,70.84339141845703,-3.4061404976109344,2155,1381200,6.496271,J4,4.301478977412387,2.5033932349312007,64.09600000000015,56.54535675048828,1.1976016552142283,11.561552047729492,-0.24548778527214488,-6.571277618408203,0.4378176488724854,-66.71285247802734,4.301478977412387,6.147788047790527,0.13572299587556014,70.078857421875,-4.170674494192966,6
3,3,832,2207,1381220,6.656264,55.109808014787404,8.759497374223619,-8.518216825284897,-41.11725046606459,10.108488114686867,41.956933292858096,2223,1381226,6.704225,J6,11.130744251720401,5.955607695749779,47.96099999999992,53.50831985473633,-1.6014881600510762,7.412222862243652,-1.3472745119799665,-9.629739761352539,-1.111522936067642,-32.30571365356445,8.811536812500137,12.488306045532227,2.379817930845359,30.826189041137695,-11.130744251720401,2227,1381227,6.712268,J6,13.165517124645206,7.03533795629365,56.00399999999972,53.18336486816406,-1.9264431466233418,7.154932975769043,-1.6045643984545759,-9.842713356018066,-1.3244965307331693,-30.750762939453125,10.366487526611465,12.961709976196289,2.8532218615094216,28.79141616821289,-13.165517124645206,2229,1381228,6.720286,J6,15.212756428234073,8.119717107623766,64.02199999999958,52.84844207763672,-2.2613659371506856,6.892857074737549,-1.86664029948607,-10.059968948364258,-1.5417521230793607,-29.200178146362305,11.917072319702285,13.449018478393555,3.340530363706687,26.744176864624023,-15.212756428234073,6
4,4,875,2321,1381263,7.000291,43.65359310453334,-1.629659559507137,-17.715753611915318,5.995208633582219,32.17171770646655,-22.573973337684023,2336,1381269,7.048266,J6,2.3116842904428125,1.3731583485126422,47.9750000000001,44.433006286621094,0.779413182087751,-2.1156294345855713,-0.48596987507843425,-18.39777374267578,-0.6820201307604634,4.8371992111206055,-1.158009422461613,33.99372482299805,1.8220071165314948,-20.26228904724121,2.3116842904428125,2339,1381270,7.056293,J6,3.083185831580508,1.7437179916275436,56.00200000000033,44.65972137451172,1.006128269978376,-2.1617355346679688,-0.5320759751608317,-18.490602493286133,-0.774848881370815,4.406780242919922,-1.5884283906622967,34.24979019165039,2.0780724851838386,-19.490787506103516,3.083185831580508,2341,1381271,7.064278,J6,3.9520155587533594,2.15807761350517,63.987000000000016,44.91068649291992,1.2570933883865791,-2.196718215942383,-0.5670586564352458,-18.575654983520508,-0.85990137160519,3.9164717197418213,-2.0787369138403973,34.48752212524414,2.3158044187775886,-18.621957778930664,3.9520155587533594,6
5,5,927,2457,1381315,7.416267,64.58244475717193,4.262979315506351,-15.669217122643433,-29.952927185666542,29.850439933020308,45.62652743544272,2477,1381321,7.464289,J5,2.171828177649214,1.404704060246151,48.021999999999565,65.84967041015625,1.2672256529843224,5.695003032684326,1.4320237171779748,-14.563636779785156,1.1055803428582767,-28.69342613220215,1.259501053464394,27.678611755371094,-2.171828177649214,46.437156677246094,0.810629241803376,2479,1381322,7.472265,J5,2.585320380408003,1.6670103454750829,55.99799999999977,66.01153564453125,1.4290908873593224,5.957343578338623,1.6943642628322717,-14.353693008422852,1.3155241142205814,-28.232824325561523,1.720102860105019,27.265119552612305,-2.585320380408003,46.24872589111328,0.6221984556705635,2483,1381323,7.480284,J5,3.0049218208376907,1.9507458917658702,64.01699999999977,66.15470886230469,1.57226410513276,6.221103191375732,1.958123875869381,-14.140877723693848,1.5283393989495853,-27.70670509338379,2.2462220922827534,26.845518112182617,-3.0049218208376907,45.96609115600586,0.33956372056314166,6
6,6,997,2649,1381385,7.976283,60.47948252708421,23.068781981390185,-5.01126420129949,36.10685486878251,5.525681179628412,-31.3025636495649,2665,1381391,8.024277,J2,1.408993592340284,0.8424031169595625,47.99400000000009,60.847530364990234,0.36804783790602613,24.47777557373047,1.408993592340284,-5.617043972015381,-0.6057797707158912,34.8044319152832,-1.3024229534993097,5.786255359649658,0.260574180021246,-31.2253475189209,0.07721613064400046,2667,1381392,8.032258,J4,1.7701696881184503,1.0669192783996628,55.975000000000996,60.95240783691406,0.47292530982985426,24.69621467590332,1.6274326945131357,-5.75187873840332,-0.7406145371038306,34.33668518066406,-1.7701696881184503,5.885775089263916,0.3600939096355038,-30.920429229736328,0.38213441982857077,2669,1381393,8.040307,J4,2.303903056038372,1.3309750271252634,64.02400000000074,61.06847381591797,0.5889912888337605,24.908491134643555,1.83970915325337,-5.895160675048828,-0.8838964737493384,33.80295181274414,-2.303903056038372,5.999467372894287,0.4737861932658749,-30.538488388061523,0.7640752615033755,6
7,7,1053,2795,1381441,8.424315,70.47583707168602,16.39384887825908,-13.456948007467595,-27.892318852946243,14.53566195089614,31.71193282686115,2811,1381447,8.472317,J6,7.1941073647892395,4.618545262978315,48.00200000000032,70.8250503540039,0.3492132823178906,10.95259952545166,-5.4412493528074215,-13.136046409606934,0.3209015978606615,-34.68459701538086,-6.7922781624346165,14.022420883178711,-0.5132410677174288,38.90604019165039,7.1941073647892395,2814,1381448,8.480253,J6,8.24868042387127,5.331515658278243,55.93799999999938,70.84622192382812,0.37038485214210937,9.979106903076172,-6.41474197518291,-13.042067527770996,0.414880479696599,-35.67851257324219,-7.786193720295945,13.882524490356445,-0.6531374605396945,39.96061325073242,8.24868042387127,2817,1381449,8.488291,J6,9.275558780804865,6.035874346469871,63.976000000000255,70.85871887207031,0.38288180038429687,8.986706733703613,-7.407142144555468,-12.9381742477417,0.5187737597258959,-36.64576721191406,-8.75344835896782,13.72922134399414,-0.8064406069019991,40.987491607666016,9.275558780804865,6
8,8,1089,2892,1381477,8.71231,69.58246408878419,-17.10571341532583,-8.710589696831251,-58.47569441890704,7.63059003611043,64.43773280685575,2908,1381483,8.760361,J6,5.93988407546847,3.533132924843701,48.05099999999918,69.40516662597656,-0.1772974628076298,-20.066781997680664,-2.9610685823548337,-8.152935981750488,0.5576537150807628,-63.93235397338867,-5.456659554481632,6.775912284851074,-0.8546777512593557,70.37761688232422,5.93988407546847,2910,1381484,8.768294,J6,6.922168621366907,4.090127751125079,55.9839999999987,69.39288330078125,-0.1895807880029423,-20.37982177734375,-3.2741083620179197,-8.094615936279297,0.6159737605519542,-64.83366394042969,-6.357969521522648,6.682157039642334,-0.948432996468096,71.35990142822266,6.922168621366907,2912,1381485,8.77637,J6,7.916950115507532,4.649086823187336,64.05999999999956,69.38455200195312,-0.1979120868310673,-20.654869079589844,-3.5491556642640134,-8.043557167053223,0.6670325297780284,-65.74622344970703,-7.270529030799992,6.598722457885742,-1.0318675782246878,72.35468292236328,7.916950115507532,6
9,9,1167,3096,1381555,9.336375,73.57125874861414,-6.42984524964374,-9.628579811400881,-128.80864727564332,14.671082817606491,140.00204816414424,3112,1381561,9.384332,J4,2.5240706198644887,1.630593251762862,47.95700000000025,73.90592956542969,0.3346708168155459,-8.016124725341797,-1.5862794756980572,-9.002426147460938,0.6261536639399434,-131.3327178955078,-2.5240706198644887,15.526206970214844,0.8551241526083526,142.41668701171875,2.4146388475745084,3114,1381562,9.392355,J4,2.830116152091051,1.8576181251506447,55.97999999999992,73.96361541748047,0.39235666886632714,-8.363268852233887,-1.933423602590147,-8.884164810180664,0.7444150012202169,-131.63876342773438,-2.830116152091051,15.65822696685791,0.987144149251419,142.69912719726562,2.6970790331213834,3118,1381563,9.400331,J4,3.1013106100988637,2.0726473342415157,63.955999999999236,74.02384948730469,0.4525907386905459,-8.728874206542969,-2.299028956899229,-8.76374626159668,0.8648335498042012,-131.9099578857422,-3.1013106100988637,15.788768768310547,1.1176859507040557,142.9464569091797,2.944408745035446,6
10,10,1189,3154,1381577,9.512325,75.56938603377543,-14.67930590788221,-7.294156094303152,-130.92303342701462,17.688361072687904,141.89350789658602,3170,1381583,9.560358,J4,3.3929965007450846,2.1059862266772753,48.033000000000214,76.72688293457031,1.1574969007948823,-16.518726348876953,-1.8394204409947434,-7.15147066116333,0.14268543313982196,-127.53003692626953,3.3929965007450846,18.53788948059082,0.8495284079029162,138.7897491455078,-3.103758751078203,3172,1381584,9.568328,J4,4.172522257581022,2.562681778316925,56.00299999999869,76.96759033203125,1.3982042982558198,-16.79741096496582,-2.1181050570836106,-7.157340049743652,0.1368160445594997,-126.7505111694336,4.172522257581022,18.698274612426758,1.0099135397388537,138.08377075195312,-3.8097371446328907,3175,1381585,9.576354,J4,4.999006937756803,3.042227585289095,64.02899999999967,77.21700286865234,1.6476168348769136,-17.061315536499023,-2.3820096286168138,-7.1720428466796875,0.12211324762346454,-125.92402648925781,4.999006937756803,18.86079978942871,1.1724387167408068,137.33689880371094,-4.556609092875078,6
11,11,1257,3334,1381645,10.056378,86.0934977330502,-14.498333177585573,-13.681510793859989,-69.8682955440411,26.742681901805224,88.9994136192388,3350,1381651,10.104322,J6,2.9848240516596434,2.0134925384056306,47.94399999999932,83.71662139892578,-2.376876334124418,-13.186573028564453,1.3117601490211204,-13.738808631896973,-0.05729783803698396,-72.70317840576172,-2.834882861720615,26.66758918762207,-0.07509271418315322,91.98423767089844,2.9848240516596434,3352,1381652,10.112375,J6,3.668104996483862,2.4582215610180698,55.99699999999963,83.25711822509766,-2.836379507952543,-12.97336483001709,1.5249683475684837,-13.721805572509766,-0.04029477864977693,-73.39217376708984,-3.52387822304874,26.632198333740234,-0.11048356806498916,92.66751861572266,3.668104996483862,3355,1381653,10.120334,J6,4.397696366112768,2.932320329432031,63.955999999999236,82.77881622314453,-3.314681509905668,-12.759296417236328,1.7390367603492454,-13.69810676574707,-0.016595971887081618,-74.13638305664062,-4.2680875125995215,26.590784072875977,-0.15189782892924697,93.39710998535156,4.397696366112768,6
12,12,1303,3455,1381691,10.424364,61.720732564877665,-4.2327893098442155,-9.784423185760874,-108.38408270751574,22.16077221865394,118.14206389103131,3470,1381697,10.472333,J6,4.000958239175844,1.9902461373621054,47.96900000000015,59.78955841064453,-1.9311741542331333,-2.7094168663024902,1.5233724435417253,-8.933828353881836,0.8505948318790377,-108.24857330322266,0.13550940429308866,21.177492141723633,-0.9832800769303063,114.14110565185547,-4.000958239175844,3474,1381698,10.480323,J6,4.9290969720860005,2.3936680954994465,55.95899999999965,59.577754974365234,-2.14297759051243,-2.4748787879943848,1.7579105218498308,-8.810709953308105,0.9737132324527682,-107.96444702148438,0.4196356860313699,21.03158187866211,-1.1291903399918297,113.21296691894531,-4.9290969720860005,3476,1381699,10.488357,J6,5.929425036050844,2.82736270797146,63.99299999999997,59.389400482177734,-2.33133208269993,-2.241248369216919,1.9915409406272966,-8.690967559814453,1.0934556259464205,-107.61585998535156,0.7682227221641824,20.888513565063477,-1.2722586535904625,112.21263885498047,-5.929425036050844,6
13,13,1362,3611,1381750,10.896324,79.75708299218505,4.6402310571176475,-15.311185536748695,-56.03531247084017,26.13313788666632,52.152257172101606,3627,1381756,10.944371,J1,6.656208938967296,4.109282348845555,48.047000000000395,86.41329193115234,6.656208938967296,4.266173362731934,-0.3740576943857139,-19.15987777709961,-3.848692240350914,-52.24028015136719,3.7950323194729805,29.853723526000977,3.720585639334658,55.86907196044922,3.7168147883476124,3629,1381757,10.952347,J1,7.722973770510265,4.798701518313248,56.02299999999971,87.48005676269531,7.722973770510265,4.181386947631836,-0.45884410948581156,-19.803678512573242,-4.492492975824547,-51.80745315551758,4.22785931532259,30.479778289794922,4.346640403128603,56.773921966552734,4.621664794451128,3633,1381758,10.960362,J1,8.793827957521984,5.501947903898078,64.03800000000004,88.55091094970703,8.793827957521984,4.09064245223999,-0.5495886048776573,-20.455745697021484,-5.144560160272789,-51.41057586669922,4.624736604140949,31.11459732055664,4.981459433890322,57.749176025390625,5.596918853289019,6
14,14,1393,3694,1381781,11.144347,108.95555081237923,1.3707600427061273,-33.59442519685133,-48.36736100282485,43.963403989432074,85.03926111958742,3710,1381787,11.192339,J6,2.93518040873289,1.5679827735552678,47.9920000000007,110.7523193359375,1.7967685235582707,0.8751887679100037,-0.4955712747961236,-34.756534576416016,-1.1621093795646829,-48.14332580566406,0.2240351971607879,45.0865592956543,1.1231553062222233,87.97444152832031,2.93518040873289,3712,1381788,11.20037,J6,3.224395496623515,1.7217974200989363,56.02299999999971,110.92666625976562,1.9711154473863957,0.7935436964035034,-0.5772163463026239,-34.86042404174805,-1.265998844896714,-48.102813720703125,0.2645472821217254,45.1879768371582,1.2245728477261295,88.26365661621094,3.224395496623515,3714,1381789,11.208359,J6,3.4549634287524214,1.844232474600388,64.01199999999996,111.0645751953125,2.1090243829332707,0.7117024660110474,-0.6590575766950799,-34.93739700317383,-1.3429718063224954,-48.06473159790039,0.3026294049244598,45.2645149230957,1.3011109336636295,88.49422454833984,3.4549634287524214,6
15,15,1426,3780,1381814,11.408361,110.58484797608095,-3.860899593758653,-32.39640711653168,-51.112590875369015,44.2291595765054,87.62399978413751,3796,1381820,11.456361,J4,2.573146734494266,1.686689503314435,48.00000000000004,111.79177856445312,1.206930588372174,-6.225440502166748,-2.364540908408095,-31.958988189697266,0.4374189268344111,-53.68573760986328,-2.573146734494266,44.76150894165039,0.532349365144988,89.33454895019531,1.7105491660578025,3798,1381821,11.464349,J4,3.0742606260958283,2.037357874022524,55.98800000000104,112.08367919921875,1.498831223137799,-6.6951823234558105,-2.8342827296971573,-31.910240173339844,0.48616694319183296,-54.186851501464844,-3.0742606260958283,44.907752990722656,0.6785934142172536,89.73998260498047,2.1159828208429587,3801,1381822,11.472359,J4,3.579433355588016,2.3980591339233284,63.998000000001554,112.39252471923828,1.8076767431573302,-7.177155494689941,-3.316255900931288,-31.86791229248047,0.528494824051208,-54.69202423095703,-3.579433355588016,45.06621170043945,0.8370521239340505,90.16300964355469,2.5390098594171775,6
16,16,1454,3854,1381842,11.63236,118.09595224767921,-17.37638727211256,-31.06900084212659,-59.56053843778568,48.73257576059714,94.13477131163891,3870,1381848,11.680362,J6,3.8105220440607894,2.197782779718695,48.00200000000032,117.87474060058594,-0.2212116470932699,-19.607177734375,-2.2307904622624406,-30.329103469848633,0.7398973722779587,-56.600250244140625,2.960288193645056,49.08655548095703,0.353979720359888,90.32424926757812,-3.8105220440607894,3872,1381849,11.688367,J6,4.756826365349852,2.710409629634467,56.00699999999925,117.7159423828125,-0.3800098648667074,-19.938819885253906,-2.562432613141347,-30.167478561401367,0.9015222807252243,-55.84638595581055,3.714152481975134,49.0964241027832,0.3638483421860599,89.37794494628906,-4.756826365349852,3874,1381850,11.696354,J6,5.783033335564696,3.263710825457299,63.99399999999922,117.52371978759766,-0.5722324600815512,-20.255823135375977,-2.879435863263417,-29.996501922607422,1.0724989195191696,-55.02521514892578,4.5353232888598995,49.092247009277344,0.3596712486802005,88.35173797607422,-5.783033335564696,6
17,18,1556,4121,1381944,12.448375,60.28248244838438,-22.938080568038327,-10.333247998560786,77.49164224770549,35.58337770883919,-69.66898071584893,4138,1381950,12.496365,J4,6.848522435693766,4.03614029285736,47.99000000000042,59.93589401245117,-0.34658843593320654,-21.102140426635742,1.8359401414025847,-9.684239387512207,0.6490086110485791,70.64311981201172,-6.848522435693766,32.941307067871094,-2.642070640968093,-63.34855651855469,6.320424197294244,4140,1381951,12.504369,J4,8.296535741357829,4.890437505980655,55.9940000000001,59.902915954589844,-0.37956649379453467,-20.689186096191406,2.2488944718469206,-9.52907943725586,0.8041685613049268,69.19510650634766,-8.296535741357829,32.343406677246094,-3.239971031593093,-62.032222747802734,7.636757968046197,4142,1381952,12.512395,J4,9.82779337075236,5.794164211715548,64.0199999999993,59.8752326965332,-0.4072497518511753,-20.24807357788086,2.6900069901574675,-9.361802101135254,0.9714458974255322,67.66384887695312,-9.82779337075236,31.704130172729492,-3.8792475361096947,-60.64377975463867,9.02520096121026,6
1 trigger_no waypoint_index best_sample_order best_frame_number best_sequence best_time_relative_s teach_j1_deg teach_j2_deg teach_j3_deg teach_j4_deg teach_j5_deg teach_j6_deg offset_6_frame_number offset_6_sequence offset_6_time_relative_s offset_6_max_error_axis offset_6_max_error_deg offset_6_rms_error_deg offset_6_delta_from_best_ms offset_6_j1_actual_deg offset_6_diff_j1_deg offset_6_j2_actual_deg offset_6_diff_j2_deg offset_6_j3_actual_deg offset_6_diff_j3_deg offset_6_j4_actual_deg offset_6_diff_j4_deg offset_6_j5_actual_deg offset_6_diff_j5_deg offset_6_j6_actual_deg offset_6_diff_j6_deg offset_7_frame_number offset_7_sequence offset_7_time_relative_s offset_7_max_error_axis offset_7_max_error_deg offset_7_rms_error_deg offset_7_delta_from_best_ms offset_7_j1_actual_deg offset_7_diff_j1_deg offset_7_j2_actual_deg offset_7_diff_j2_deg offset_7_j3_actual_deg offset_7_diff_j3_deg offset_7_j4_actual_deg offset_7_diff_j4_deg offset_7_j5_actual_deg offset_7_diff_j5_deg offset_7_j6_actual_deg offset_7_diff_j6_deg offset_8_frame_number offset_8_sequence offset_8_time_relative_s offset_8_max_error_axis offset_8_max_error_deg offset_8_rms_error_deg offset_8_delta_from_best_ms offset_8_j1_actual_deg offset_8_diff_j1_deg offset_8_j2_actual_deg offset_8_diff_j2_deg offset_8_j3_actual_deg offset_8_diff_j3_deg offset_8_j4_actual_deg offset_8_diff_j4_deg offset_8_j5_actual_deg offset_8_diff_j5_deg offset_8_j6_actual_deg offset_8_diff_j6_deg best_of_6_7_8_offset
2 1 1 730 1941 1381118 5.840207 48.886810269468405 2.1989850886957285 -11.021017368511105 0.4102097980549552 6.248381265333557 2.294990756284542 1955 1381124 5.888271 J6 3.4065268256551553 2.468438977201345 48.06399999999922 45.8632698059082 -3.0235404635602023 2.9724788665771484 0.7734937778814199 -13.609341621398926 -2.588324252887821 -2.1851141452789307 -2.5953239433338857 7.582361221313477 1.3339799559799195 5.701517581939697 3.4065268256551553 1958 1381125 5.896184 J6 4.180931828249149 2.9355776062802286 55.9769999999995 45.449710845947266 -3.43709942352114 3.117816925048828 0.9188318363530996 -13.963269233703613 -2.9422518651925085 -2.845515012741089 -3.255724810796044 7.768581867218018 1.5202006018844605 6.475922584533691 4.180931828249149 1961 1381126 5.904272 J6 5.0045521476156045 3.418446646752538 64.06499999999937 45.06731414794922 -3.8194961215191867 3.2660434246063232 1.0670583359105947 -14.290565490722656 -3.2695481222115514 -3.562589168548584 -3.972798966603539 7.9421586990356445 1.6937774337020874 7.2995429039001465 5.0045521476156045 6
3 2 2 804 2135 1381192 6.432175 55.34775509527405 11.807039833001637 -7.0090952672806885 -71.01433145543973 6.012065051914967 74.24953191606797 2151 1381198 6.480252 J4 2.8167225076858244 1.6562461612538633 48.07700000000015 56.34336471557617 0.995609620302119 11.66723918914795 -0.13980064385368784 -6.600923538208008 0.40817172907268073 -68.1976089477539 2.8167225076858244 6.058437347412109 0.04637229549714217 71.5392837524414 -2.7102481636265594 2153 1381199 6.488246 J4 3.526759739131137 2.060948429836345 56.071000000000204 56.451515197753906 1.1037601024798533 11.618118286132812 -0.18892154686882456 -6.580313682556152 0.4287815847245362 -67.4875717163086 3.526759739131137 6.09804105758667 0.08597600567170272 70.84339141845703 -3.4061404976109344 2155 1381200 6.496271 J4 4.301478977412387 2.5033932349312007 64.09600000000015 56.54535675048828 1.1976016552142283 11.561552047729492 -0.24548778527214488 -6.571277618408203 0.4378176488724854 -66.71285247802734 4.301478977412387 6.147788047790527 0.13572299587556014 70.078857421875 -4.170674494192966 6
4 3 3 832 2207 1381220 6.656264 55.109808014787404 8.759497374223619 -8.518216825284897 -41.11725046606459 10.108488114686867 41.956933292858096 2223 1381226 6.704225 J6 11.130744251720401 5.955607695749779 47.96099999999992 53.50831985473633 -1.6014881600510762 7.412222862243652 -1.3472745119799665 -9.629739761352539 -1.111522936067642 -32.30571365356445 8.811536812500137 12.488306045532227 2.379817930845359 30.826189041137695 -11.130744251720401 2227 1381227 6.712268 J6 13.165517124645206 7.03533795629365 56.00399999999972 53.18336486816406 -1.9264431466233418 7.154932975769043 -1.6045643984545759 -9.842713356018066 -1.3244965307331693 -30.750762939453125 10.366487526611465 12.961709976196289 2.8532218615094216 28.79141616821289 -13.165517124645206 2229 1381228 6.720286 J6 15.212756428234073 8.119717107623766 64.02199999999958 52.84844207763672 -2.2613659371506856 6.892857074737549 -1.86664029948607 -10.059968948364258 -1.5417521230793607 -29.200178146362305 11.917072319702285 13.449018478393555 3.340530363706687 26.744176864624023 -15.212756428234073 6
5 4 4 875 2321 1381263 7.000291 43.65359310453334 -1.629659559507137 -17.715753611915318 5.995208633582219 32.17171770646655 -22.573973337684023 2336 1381269 7.048266 J6 2.3116842904428125 1.3731583485126422 47.9750000000001 44.433006286621094 0.779413182087751 -2.1156294345855713 -0.48596987507843425 -18.39777374267578 -0.6820201307604634 4.8371992111206055 -1.158009422461613 33.99372482299805 1.8220071165314948 -20.26228904724121 2.3116842904428125 2339 1381270 7.056293 J6 3.083185831580508 1.7437179916275436 56.00200000000033 44.65972137451172 1.006128269978376 -2.1617355346679688 -0.5320759751608317 -18.490602493286133 -0.774848881370815 4.406780242919922 -1.5884283906622967 34.24979019165039 2.0780724851838386 -19.490787506103516 3.083185831580508 2341 1381271 7.064278 J6 3.9520155587533594 2.15807761350517 63.987000000000016 44.91068649291992 1.2570933883865791 -2.196718215942383 -0.5670586564352458 -18.575654983520508 -0.85990137160519 3.9164717197418213 -2.0787369138403973 34.48752212524414 2.3158044187775886 -18.621957778930664 3.9520155587533594 6
6 5 5 927 2457 1381315 7.416267 64.58244475717193 4.262979315506351 -15.669217122643433 -29.952927185666542 29.850439933020308 45.62652743544272 2477 1381321 7.464289 J5 2.171828177649214 1.404704060246151 48.021999999999565 65.84967041015625 1.2672256529843224 5.695003032684326 1.4320237171779748 -14.563636779785156 1.1055803428582767 -28.69342613220215 1.259501053464394 27.678611755371094 -2.171828177649214 46.437156677246094 0.810629241803376 2479 1381322 7.472265 J5 2.585320380408003 1.6670103454750829 55.99799999999977 66.01153564453125 1.4290908873593224 5.957343578338623 1.6943642628322717 -14.353693008422852 1.3155241142205814 -28.232824325561523 1.720102860105019 27.265119552612305 -2.585320380408003 46.24872589111328 0.6221984556705635 2483 1381323 7.480284 J5 3.0049218208376907 1.9507458917658702 64.01699999999977 66.15470886230469 1.57226410513276 6.221103191375732 1.958123875869381 -14.140877723693848 1.5283393989495853 -27.70670509338379 2.2462220922827534 26.845518112182617 -3.0049218208376907 45.96609115600586 0.33956372056314166 6
7 6 6 997 2649 1381385 7.976283 60.47948252708421 23.068781981390185 -5.01126420129949 36.10685486878251 5.525681179628412 -31.3025636495649 2665 1381391 8.024277 J2 1.408993592340284 0.8424031169595625 47.99400000000009 60.847530364990234 0.36804783790602613 24.47777557373047 1.408993592340284 -5.617043972015381 -0.6057797707158912 34.8044319152832 -1.3024229534993097 5.786255359649658 0.260574180021246 -31.2253475189209 0.07721613064400046 2667 1381392 8.032258 J4 1.7701696881184503 1.0669192783996628 55.975000000000996 60.95240783691406 0.47292530982985426 24.69621467590332 1.6274326945131357 -5.75187873840332 -0.7406145371038306 34.33668518066406 -1.7701696881184503 5.885775089263916 0.3600939096355038 -30.920429229736328 0.38213441982857077 2669 1381393 8.040307 J4 2.303903056038372 1.3309750271252634 64.02400000000074 61.06847381591797 0.5889912888337605 24.908491134643555 1.83970915325337 -5.895160675048828 -0.8838964737493384 33.80295181274414 -2.303903056038372 5.999467372894287 0.4737861932658749 -30.538488388061523 0.7640752615033755 6
8 7 7 1053 2795 1381441 8.424315 70.47583707168602 16.39384887825908 -13.456948007467595 -27.892318852946243 14.53566195089614 31.71193282686115 2811 1381447 8.472317 J6 7.1941073647892395 4.618545262978315 48.00200000000032 70.8250503540039 0.3492132823178906 10.95259952545166 -5.4412493528074215 -13.136046409606934 0.3209015978606615 -34.68459701538086 -6.7922781624346165 14.022420883178711 -0.5132410677174288 38.90604019165039 7.1941073647892395 2814 1381448 8.480253 J6 8.24868042387127 5.331515658278243 55.93799999999938 70.84622192382812 0.37038485214210937 9.979106903076172 -6.41474197518291 -13.042067527770996 0.414880479696599 -35.67851257324219 -7.786193720295945 13.882524490356445 -0.6531374605396945 39.96061325073242 8.24868042387127 2817 1381449 8.488291 J6 9.275558780804865 6.035874346469871 63.976000000000255 70.85871887207031 0.38288180038429687 8.986706733703613 -7.407142144555468 -12.9381742477417 0.5187737597258959 -36.64576721191406 -8.75344835896782 13.72922134399414 -0.8064406069019991 40.987491607666016 9.275558780804865 6
9 8 8 1089 2892 1381477 8.71231 69.58246408878419 -17.10571341532583 -8.710589696831251 -58.47569441890704 7.63059003611043 64.43773280685575 2908 1381483 8.760361 J6 5.93988407546847 3.533132924843701 48.05099999999918 69.40516662597656 -0.1772974628076298 -20.066781997680664 -2.9610685823548337 -8.152935981750488 0.5576537150807628 -63.93235397338867 -5.456659554481632 6.775912284851074 -0.8546777512593557 70.37761688232422 5.93988407546847 2910 1381484 8.768294 J6 6.922168621366907 4.090127751125079 55.9839999999987 69.39288330078125 -0.1895807880029423 -20.37982177734375 -3.2741083620179197 -8.094615936279297 0.6159737605519542 -64.83366394042969 -6.357969521522648 6.682157039642334 -0.948432996468096 71.35990142822266 6.922168621366907 2912 1381485 8.77637 J6 7.916950115507532 4.649086823187336 64.05999999999956 69.38455200195312 -0.1979120868310673 -20.654869079589844 -3.5491556642640134 -8.043557167053223 0.6670325297780284 -65.74622344970703 -7.270529030799992 6.598722457885742 -1.0318675782246878 72.35468292236328 7.916950115507532 6
10 9 9 1167 3096 1381555 9.336375 73.57125874861414 -6.42984524964374 -9.628579811400881 -128.80864727564332 14.671082817606491 140.00204816414424 3112 1381561 9.384332 J4 2.5240706198644887 1.630593251762862 47.95700000000025 73.90592956542969 0.3346708168155459 -8.016124725341797 -1.5862794756980572 -9.002426147460938 0.6261536639399434 -131.3327178955078 -2.5240706198644887 15.526206970214844 0.8551241526083526 142.41668701171875 2.4146388475745084 3114 1381562 9.392355 J4 2.830116152091051 1.8576181251506447 55.97999999999992 73.96361541748047 0.39235666886632714 -8.363268852233887 -1.933423602590147 -8.884164810180664 0.7444150012202169 -131.63876342773438 -2.830116152091051 15.65822696685791 0.987144149251419 142.69912719726562 2.6970790331213834 3118 1381563 9.400331 J4 3.1013106100988637 2.0726473342415157 63.955999999999236 74.02384948730469 0.4525907386905459 -8.728874206542969 -2.299028956899229 -8.76374626159668 0.8648335498042012 -131.9099578857422 -3.1013106100988637 15.788768768310547 1.1176859507040557 142.9464569091797 2.944408745035446 6
11 10 10 1189 3154 1381577 9.512325 75.56938603377543 -14.67930590788221 -7.294156094303152 -130.92303342701462 17.688361072687904 141.89350789658602 3170 1381583 9.560358 J4 3.3929965007450846 2.1059862266772753 48.033000000000214 76.72688293457031 1.1574969007948823 -16.518726348876953 -1.8394204409947434 -7.15147066116333 0.14268543313982196 -127.53003692626953 3.3929965007450846 18.53788948059082 0.8495284079029162 138.7897491455078 -3.103758751078203 3172 1381584 9.568328 J4 4.172522257581022 2.562681778316925 56.00299999999869 76.96759033203125 1.3982042982558198 -16.79741096496582 -2.1181050570836106 -7.157340049743652 0.1368160445594997 -126.7505111694336 4.172522257581022 18.698274612426758 1.0099135397388537 138.08377075195312 -3.8097371446328907 3175 1381585 9.576354 J4 4.999006937756803 3.042227585289095 64.02899999999967 77.21700286865234 1.6476168348769136 -17.061315536499023 -2.3820096286168138 -7.1720428466796875 0.12211324762346454 -125.92402648925781 4.999006937756803 18.86079978942871 1.1724387167408068 137.33689880371094 -4.556609092875078 6
12 11 11 1257 3334 1381645 10.056378 86.0934977330502 -14.498333177585573 -13.681510793859989 -69.8682955440411 26.742681901805224 88.9994136192388 3350 1381651 10.104322 J6 2.9848240516596434 2.0134925384056306 47.94399999999932 83.71662139892578 -2.376876334124418 -13.186573028564453 1.3117601490211204 -13.738808631896973 -0.05729783803698396 -72.70317840576172 -2.834882861720615 26.66758918762207 -0.07509271418315322 91.98423767089844 2.9848240516596434 3352 1381652 10.112375 J6 3.668104996483862 2.4582215610180698 55.99699999999963 83.25711822509766 -2.836379507952543 -12.97336483001709 1.5249683475684837 -13.721805572509766 -0.04029477864977693 -73.39217376708984 -3.52387822304874 26.632198333740234 -0.11048356806498916 92.66751861572266 3.668104996483862 3355 1381653 10.120334 J6 4.397696366112768 2.932320329432031 63.955999999999236 82.77881622314453 -3.314681509905668 -12.759296417236328 1.7390367603492454 -13.69810676574707 -0.016595971887081618 -74.13638305664062 -4.2680875125995215 26.590784072875977 -0.15189782892924697 93.39710998535156 4.397696366112768 6
13 12 12 1303 3455 1381691 10.424364 61.720732564877665 -4.2327893098442155 -9.784423185760874 -108.38408270751574 22.16077221865394 118.14206389103131 3470 1381697 10.472333 J6 4.000958239175844 1.9902461373621054 47.96900000000015 59.78955841064453 -1.9311741542331333 -2.7094168663024902 1.5233724435417253 -8.933828353881836 0.8505948318790377 -108.24857330322266 0.13550940429308866 21.177492141723633 -0.9832800769303063 114.14110565185547 -4.000958239175844 3474 1381698 10.480323 J6 4.9290969720860005 2.3936680954994465 55.95899999999965 59.577754974365234 -2.14297759051243 -2.4748787879943848 1.7579105218498308 -8.810709953308105 0.9737132324527682 -107.96444702148438 0.4196356860313699 21.03158187866211 -1.1291903399918297 113.21296691894531 -4.9290969720860005 3476 1381699 10.488357 J6 5.929425036050844 2.82736270797146 63.99299999999997 59.389400482177734 -2.33133208269993 -2.241248369216919 1.9915409406272966 -8.690967559814453 1.0934556259464205 -107.61585998535156 0.7682227221641824 20.888513565063477 -1.2722586535904625 112.21263885498047 -5.929425036050844 6
14 13 13 1362 3611 1381750 10.896324 79.75708299218505 4.6402310571176475 -15.311185536748695 -56.03531247084017 26.13313788666632 52.152257172101606 3627 1381756 10.944371 J1 6.656208938967296 4.109282348845555 48.047000000000395 86.41329193115234 6.656208938967296 4.266173362731934 -0.3740576943857139 -19.15987777709961 -3.848692240350914 -52.24028015136719 3.7950323194729805 29.853723526000977 3.720585639334658 55.86907196044922 3.7168147883476124 3629 1381757 10.952347 J1 7.722973770510265 4.798701518313248 56.02299999999971 87.48005676269531 7.722973770510265 4.181386947631836 -0.45884410948581156 -19.803678512573242 -4.492492975824547 -51.80745315551758 4.22785931532259 30.479778289794922 4.346640403128603 56.773921966552734 4.621664794451128 3633 1381758 10.960362 J1 8.793827957521984 5.501947903898078 64.03800000000004 88.55091094970703 8.793827957521984 4.09064245223999 -0.5495886048776573 -20.455745697021484 -5.144560160272789 -51.41057586669922 4.624736604140949 31.11459732055664 4.981459433890322 57.749176025390625 5.596918853289019 6
15 14 14 1393 3694 1381781 11.144347 108.95555081237923 1.3707600427061273 -33.59442519685133 -48.36736100282485 43.963403989432074 85.03926111958742 3710 1381787 11.192339 J6 2.93518040873289 1.5679827735552678 47.9920000000007 110.7523193359375 1.7967685235582707 0.8751887679100037 -0.4955712747961236 -34.756534576416016 -1.1621093795646829 -48.14332580566406 0.2240351971607879 45.0865592956543 1.1231553062222233 87.97444152832031 2.93518040873289 3712 1381788 11.20037 J6 3.224395496623515 1.7217974200989363 56.02299999999971 110.92666625976562 1.9711154473863957 0.7935436964035034 -0.5772163463026239 -34.86042404174805 -1.265998844896714 -48.102813720703125 0.2645472821217254 45.1879768371582 1.2245728477261295 88.26365661621094 3.224395496623515 3714 1381789 11.208359 J6 3.4549634287524214 1.844232474600388 64.01199999999996 111.0645751953125 2.1090243829332707 0.7117024660110474 -0.6590575766950799 -34.93739700317383 -1.3429718063224954 -48.06473159790039 0.3026294049244598 45.2645149230957 1.3011109336636295 88.49422454833984 3.4549634287524214 6
16 15 15 1426 3780 1381814 11.408361 110.58484797608095 -3.860899593758653 -32.39640711653168 -51.112590875369015 44.2291595765054 87.62399978413751 3796 1381820 11.456361 J4 2.573146734494266 1.686689503314435 48.00000000000004 111.79177856445312 1.206930588372174 -6.225440502166748 -2.364540908408095 -31.958988189697266 0.4374189268344111 -53.68573760986328 -2.573146734494266 44.76150894165039 0.532349365144988 89.33454895019531 1.7105491660578025 3798 1381821 11.464349 J4 3.0742606260958283 2.037357874022524 55.98800000000104 112.08367919921875 1.498831223137799 -6.6951823234558105 -2.8342827296971573 -31.910240173339844 0.48616694319183296 -54.186851501464844 -3.0742606260958283 44.907752990722656 0.6785934142172536 89.73998260498047 2.1159828208429587 3801 1381822 11.472359 J4 3.579433355588016 2.3980591339233284 63.998000000001554 112.39252471923828 1.8076767431573302 -7.177155494689941 -3.316255900931288 -31.86791229248047 0.528494824051208 -54.69202423095703 -3.579433355588016 45.06621170043945 0.8370521239340505 90.16300964355469 2.5390098594171775 6
17 16 16 1454 3854 1381842 11.63236 118.09595224767921 -17.37638727211256 -31.06900084212659 -59.56053843778568 48.73257576059714 94.13477131163891 3870 1381848 11.680362 J6 3.8105220440607894 2.197782779718695 48.00200000000032 117.87474060058594 -0.2212116470932699 -19.607177734375 -2.2307904622624406 -30.329103469848633 0.7398973722779587 -56.600250244140625 2.960288193645056 49.08655548095703 0.353979720359888 90.32424926757812 -3.8105220440607894 3872 1381849 11.688367 J6 4.756826365349852 2.710409629634467 56.00699999999925 117.7159423828125 -0.3800098648667074 -19.938819885253906 -2.562432613141347 -30.167478561401367 0.9015222807252243 -55.84638595581055 3.714152481975134 49.0964241027832 0.3638483421860599 89.37794494628906 -4.756826365349852 3874 1381850 11.696354 J6 5.783033335564696 3.263710825457299 63.99399999999922 117.52371978759766 -0.5722324600815512 -20.255823135375977 -2.879435863263417 -29.996501922607422 1.0724989195191696 -55.02521514892578 4.5353232888598995 49.092247009277344 0.3596712486802005 88.35173797607422 -5.783033335564696 6
18 17 18 1556 4121 1381944 12.448375 60.28248244838438 -22.938080568038327 -10.333247998560786 77.49164224770549 35.58337770883919 -69.66898071584893 4138 1381950 12.496365 J4 6.848522435693766 4.03614029285736 47.99000000000042 59.93589401245117 -0.34658843593320654 -21.102140426635742 1.8359401414025847 -9.684239387512207 0.6490086110485791 70.64311981201172 -6.848522435693766 32.941307067871094 -2.642070640968093 -63.34855651855469 6.320424197294244 4140 1381951 12.504369 J4 8.296535741357829 4.890437505980655 55.9940000000001 59.902915954589844 -0.37956649379453467 -20.689186096191406 2.2488944718469206 -9.52907943725586 0.8041685613049268 69.19510650634766 -8.296535741357829 32.343406677246094 -3.239971031593093 -62.032222747802734 7.636757968046197 4142 1381952 12.512395 J4 9.82779337075236 5.794164211715548 64.0199999999993 59.8752326965332 -0.4072497518511753 -20.24807357788086 2.6900069901574675 -9.361802101135254 0.9714458974255322 67.66384887695312 -9.82779337075236 31.704130172729492 -3.8792475361096947 -60.64377975463867 9.02520096121026 6

View File

@@ -0,0 +1,18 @@
{
"rows": 17,
"best_offset_win_counts": {
"6": 17,
"7": 0,
"8": 0
},
"average_max_error_deg": {
"6": 4.241583591714439,
"7": 5.068092118561757,
"8": 5.9316839578472145
},
"average_rms_error_deg": {
"6": 2.5406136706026206,
"7": 3.021296972079456,
"8": 3.5201758351699857
}
}

View File

@@ -0,0 +1,18 @@
trigger_no,waypoint_index,trigger_frame_number,trigger_time_relative_s,trigger_sequence,paired_status_frame_number,paired_status_time_relative_s,paired_status_sequence,paired_status_timestamp,paired_status_to_trigger_sequence_delta,paired_status_to_trigger_time_ms,teach_j1_deg,teach_j2_deg,teach_j3_deg,teach_j4_deg,teach_j5_deg,teach_j6_deg,paired_status_j1_actual_deg,paired_status_diff_j1_deg,paired_status_j2_actual_deg,paired_status_diff_j2_deg,paired_status_j3_actual_deg,paired_status_diff_j3_deg,paired_status_j4_actual_deg,paired_status_diff_j4_deg,paired_status_j5_actual_deg,paired_status_diff_j5_deg,paired_status_j6_actual_deg,paired_status_diff_j6_deg,paired_status_max_error_axis,paired_status_max_error_deg,paired_status_rms_error_deg,best_status_frame_number,best_status_time_relative_s,best_status_sequence,best_status_timestamp,best_status_delta_from_paired_cycles,best_status_delta_from_trigger_sequence,best_status_time_after_trigger_ms,best_status_j1_actual_deg,best_status_diff_j1_deg,best_status_j2_actual_deg,best_status_diff_j2_deg,best_status_j3_actual_deg,best_status_diff_j3_deg,best_status_j4_actual_deg,best_status_diff_j4_deg,best_status_j5_actual_deg,best_status_diff_j5_deg,best_status_j6_actual_deg,best_status_diff_j6_deg,best_status_max_error_axis,best_status_max_error_deg,best_status_rms_error_deg
1,1,1955,5.888271,1381124,1954,5.888166,1381116,624368380,-8,-0.10499999999957765,48.886810269468405,2.1989850886957285,-11.021017368511105,0.4102097980549552,6.248381265333557,2.294990756284542,55.47505187988281,6.588241610414407,1.2506256103515625,-0.948359478344166,-5.400295734405518,5.620721634105587,0.5259769558906555,0.1157671578357003,3.3612561225891113,-2.8871251427444458,0.44421958923339844,-1.8507711670511435,J1,6.588241610414407,3.8225443630072706,1981,5.968124,1381126,624368460,10,2,79.85300000000083,48.76339340209961,-0.12341686736879609,2.256481647491455,0.05749655879572657,-11.157909393310547,-0.13689202479944207,0.37860527634620667,-0.031604521708748556,6.3406662940979,0.0922850287643433,2.5025806427001953,0.20758988641565335,J6,0.20758988641565335,0.12239685259209541
2,2,2151,6.480252,1381198,2150,6.480138,1381190,624368972,-8,-0.11399999999994748,55.34775509527405,11.807039833001637,-7.0090952672806885,-71.01433145543973,6.012065051914967,74.24953191606797,53.0124397277832,-2.33531536749085,11.506516456604004,-0.30052337639763316,-8.466276168823242,-1.4571809015425536,-69.63382720947266,1.3805042459670744,6.426647663116455,0.41458261120148787,72.93537902832031,-1.3141528877476532,J1,2.33531536749085,1.3827543088729335,2172,6.552164,1381199,624369044,9,1,71.9120000000002,55.265621185302734,-0.08213390997131853,11.803675651550293,-0.003364181451344095,-7.050373554229736,-0.04127828694904778,-71.06609344482422,-0.05176198938448806,6.026607990264893,0.014542938349925372,74.28738403320312,0.03785211713515935,J1,0.08213390997131853,0.04616054814443023
3,3,2223,6.704225,1381226,2222,6.704166,1381218,624369196,-8,-0.059000000000253294,55.109808014787404,8.759497374223619,-8.518216825284897,-41.11725046606459,10.108488114686867,41.956933292858096,56.5781364440918,1.4683284293043926,10.494372367858887,1.7348749936352679,-7.114619255065918,1.4035975702189791,-55.112239837646484,-13.994989371581894,7.423236846923828,-2.6852512677630394,58.080867767333984,16.123934474475888,J6,16.123934474475888,8.852376616648156,2248,6.784154,1381228,624369276,10,2,79.92899999999992,55.00252151489258,-0.10728649989482619,8.69005012512207,-0.06944724910154854,-8.577027320861816,-0.05881049557691931,-40.59286117553711,0.5243892905274805,10.210430145263672,0.1019420305768044,41.38724136352539,-0.5696919293327056,J6,0.5696919293327056,0.3239642953120798
4,4,2336,7.048266,1381269,2335,7.048148,1381261,624369540,-8,-0.11799999999961841,43.65359310453334,-1.629659559507137,-17.715753611915318,5.995208633582219,32.17171770646655,-22.573973337684023,44.479610443115234,0.8260173385818916,0.07192186266183853,1.7015814221689756,-15.996956825256348,1.7187967866589702,2.40028715133667,-3.5949214822455486,27.737812042236328,-4.433905664230224,-17.672090530395508,4.901882807288516,J6,4.901882807288516,3.2440556235708993,2362,7.128175,1381271,624369620,10,2,79.90899999999979,43.72715759277344,0.07356448824009476,-1.6386029720306396,-0.008943412523502614,-17.75617218017578,-0.04041856826046342,5.952699661254883,-0.04250897232733575,32.2616081237793,0.08989041731274483,-22.47926902770996,0.09470430997406254,J6,0.09470430997406254,0.06580474324148583
5,5,2477,7.464289,1381321,2476,7.464178,1381313,624369956,-8,-0.11099999999952814,64.58244475717193,4.262979315506351,-15.669217122643433,-29.952927185666542,29.850439933020308,45.62652743544272,60.830230712890625,-3.7522140442813026,1.8808640241622925,-2.382115291344059,-17.297021865844727,-1.6278047432012936,-26.684900283813477,3.268026901853066,33.02191925048828,3.1714793174679734,36.40708923339844,-9.21943820204428,J6,9.21943820204428,4.621321121191458,2502,7.544122,1381323,624370036,10,2,79.83299999999983,64.5761947631836,-0.006249993988333813,4.283629417419434,0.020650101913082253,-15.615406036376953,0.05381108626647979,-29.94143295288086,0.011494232785683067,29.7938175201416,-0.056622412878706285,45.61851119995117,-0.008016235491545842,J5,0.056622412878706285,0.03357478517689402
6,6,2665,8.024277,1381391,2664,8.024156,1381383,624370516,-8,-0.12100000000003774,60.47948252708421,23.068781981390185,-5.01126420129949,36.10685486878251,5.525681179628412,-31.3025636495649,60.89759063720703,0.418108110122823,20.716407775878906,-2.3523742055112784,-4.913214206695557,0.09804999460393304,32.72084045410156,-3.3860144146809503,6.405356407165527,0.8796752275371151,-25.156103134155273,6.1464605154096255,J6,6.1464605154096255,3.0478405356855083,2687,8.096181,1381392,624370588,9,1,71.90399999999997,60.54315185546875,0.06366932838454176,22.99761962890625,-0.07116235248393465,-5.037043571472168,-0.025779370172678284,36.056732177734375,-0.050122691048137824,5.545217037200928,0.019535857572515525,-31.12558937072754,0.17697427883735983,J6,0.17697427883735983,0.08563125303054821
7,7,2811,8.472317,1381447,2810,8.472208,1381439,624370964,-8,-0.10900000000013677,70.47583707168602,16.39384887825908,-13.456948007467595,-27.892318852946243,14.53566195089614,31.71193282686115,69.13842010498047,-1.3374169667055469,22.202428817749023,5.808579939489942,-12.907807350158691,0.5491406573089037,-15.469873428344727,12.422445424601516,14.040997505187988,-0.4946644457081515,18.56021499633789,-13.15171783052326,J6,13.15171783052326,7.78202379046887,2834,8.54419,1381448,624371036,9,1,71.87300000000008,70.3967514038086,-0.07908566787742188,16.456680297851562,0.06283141959248084,-13.330730438232422,0.12621756923517324,-27.611600875854492,0.2807179770917507,14.542427062988281,0.006765112092141479,31.405345916748047,-0.30658691011310424,J6,0.30658691011310424,0.18210669390591905
8,8,2908,8.760361,1381483,2907,8.760245,1381475,624371252,-8,-0.11600000000022703,69.58246408878419,-17.10571341532583,-8.710589696831251,-58.47569441890704,7.63059003611043,64.43773280685575,70.10379791259766,0.5213338238134639,-10.075156211853027,7.030557203472803,-9.98978042602539,-1.2791907291941396,-51.5866584777832,6.889035941123836,9.534638404846191,1.9040483687357614,57.0014533996582,-7.436279407197546,J6,7.436279407197546,5.127041386597523,2929,8.832199,1381484,624371324,9,1,71.83799999999962,69.59716033935547,0.014696250571276437,-17.044965744018555,0.06074767130727565,-8.703485488891602,0.007104207939649498,-58.473426818847656,0.0022676000593833123,7.6463942527771,0.015804216666669646,64.45584869384766,0.01811588699190736,J2,0.06074767130727565,0.02750705896232285
9,9,3112,9.384332,1381561,3111,9.384204,1381553,624371876,-8,-0.128000000000128,73.57125874861414,-6.42984524964374,-9.628579811400881,-128.80864727564332,14.671082817606491,140.00204816414424,72.96144104003906,-0.6098177085750791,-6.3001484870910645,0.12969676255267526,-10.035088539123535,-0.40650872772265423,-123.29313659667969,5.515510678963636,13.165532112121582,-1.505550705484909,134.41644287109375,-5.585605293050492,J6,5.585605293050492,3.277206017832995,3134,9.456205,1381562,624371948,9,1,71.87300000000008,73.55095672607422,-0.02030202253992286,-6.441007614135742,-0.011162364492002474,-9.614203453063965,0.014376358336916084,-128.65638732910156,0.15225994654176134,14.635379791259766,-0.03570302634672551,139.8520965576172,-0.14995160652705408,J4,0.15225994654176134,0.089150370994325
10,10,3170,9.560358,1381583,3169,9.56025,1381575,624372052,-8,-0.10800000000088517,75.56938603377543,-14.67930590788221,-7.294156094303152,-130.92303342701462,17.688361072687904,141.89350789658602,74.36400604248047,-1.2053799912949614,-10.514228820800781,4.165077087081428,-8.236283302307129,-0.9421272080039769,-132.6507110595703,-1.7276776325556966,16.38176155090332,-1.3065995217845838,143.5712890625,1.6777811659139843,J2,4.165077087081428,2.128983861449047,3196,9.640214,1381585,624372132,10,2,79.85599999999948,75.62290954589844,0.05352351212300732,-14.662798881530762,0.016507026351447962,-7.297325611114502,-0.003169516811349915,-130.88812255859375,0.03491086842086588,17.726865768432617,0.03850469574471305,141.820068359375,-0.0734395372110157,J6,0.0734395372110157,0.043285948566531826
11,11,3350,10.104322,1381651,3349,10.104215,1381643,624372596,-8,-0.10699999999985721,86.0934977330502,-14.498333177585573,-13.681510793859989,-69.8682955440411,26.742681901805224,88.9994136192388,87.81878662109375,1.7252888880435506,-16.329286575317383,-1.8309533977318093,-13.073507308959961,0.6080034849000278,-70.97254180908203,-1.1042462650409277,26.366661071777344,-0.3760208300278798,89.37981414794922,0.3804005287104246,J2,1.8309533977318093,1.169340011162825,3373,10.17622,1381652,624372668,9,1,71.8980000000009,86.11857604980469,0.025078316754488128,-14.53605842590332,-0.03772524831774682,-13.651655197143555,0.029855596716434007,-69.8272933959961,0.04100214804500979,26.72992706298828,-0.012754838816942282,88.92174530029297,-0.07766831894582538,J6,0.07766831894582538,0.04246508921571141
12,12,3470,10.472333,1381697,3469,10.472191,1381689,624372964,-8,-0.14200000000030855,61.720732564877665,-4.2327893098442155,-9.784423185760874,-108.38408270751574,22.16077221865394,118.14206389103131,65.91849517822266,4.197762613344992,-6.331206798553467,-2.0984174887092513,-11.008773803710938,-1.2243506179500638,-103.56172943115234,4.822353276363401,23.532543182373047,1.3717709637191078,117.46694946289062,-0.675114428140688,J4,4.822353276363401,2.8611252897135864,3493,10.544283,1381698,624373036,9,1,71.94999999999929,61.7418098449707,0.021077280093038553,-4.212831497192383,0.019957812651832718,-9.789913177490234,-0.00548999172936071,-108.36312103271484,0.020961674800901164,22.163673400878906,0.002901182224967158,118.13783264160156,-0.0042312494297505054,J1,0.021077280093038553,0.014935517911187784
13,13,3627,10.944371,1381756,3626,10.944251,1381748,624373436,-8,-0.12000000000078614,79.75708299218505,4.6402310571176475,-15.311185536748695,-56.03531247084017,26.13313788666632,52.152257172101606,72.04009246826172,-7.716990523923329,4.5515007972717285,-0.08873025984591898,-11.385177612304688,3.926007924444008,-64.18782043457031,-8.152507963730145,22.412456512451172,-3.7206813742151468,54.39056396484375,2.2383067927421436,J4,8.152507963730145,5.168659196117862,3651,11.016226,1381757,624373508,9,1,71.85499999999934,79.90262603759766,0.14554304541260876,4.62429666519165,-0.015934391925997105,-15.370849609375,-0.05966407262630469,-55.99199676513672,0.043315705703449225,26.182939529418945,0.04980164275262666,52.15596008300781,0.003702910906206114,J1,0.14554304541260876,0.0699602863618738
14,14,3710,11.192339,1381787,3709,11.19225,1381779,624373684,-8,-0.08900000000089392,108.95555081237923,1.3707600427061273,-33.59442519685133,-48.36736100282485,43.963403989432074,85.03926111958742,102.20675659179688,-6.748794220582354,2.5045764446258545,1.1338164019197272,-29.148040771484375,4.446384425366958,-48.76271438598633,-0.3953533831614777,39.65629959106445,-4.3071043983676205,74.63484191894531,-10.40441920064211,J6,10.40441920064211,5.679810575214033,3735,11.272261,1381789,624373764,10,2,79.92199999999983,108.94917297363281,-0.006377838746416842,1.3447811603546143,-0.025978882351513022,-33.561458587646484,0.032966609204848396,-48.376487731933594,-0.009126729108743348,44.02882766723633,0.06542367780425451,85.1587142944336,0.11945317484617135,J6,0.11945317484617135,0.05835950688086092
15,15,3796,11.456361,1381820,3795,11.456253,1381812,624373948,-8,-0.10799999999910881,110.58484797608095,-3.860899593758653,-32.39640711653168,-51.112590875369015,44.2291595765054,87.62399978413751,110.58193969726562,-0.002908278815326071,-1.244791865348816,2.6161077284098373,-33.74542999267578,-1.3490228761441045,-48.585723876953125,2.5268669984158905,44.58126449584961,0.35210491934420673,87.75857543945312,0.134575655315615,J2,2.6161077284098373,1.5911754635072362,3821,11.536287,1381822,624374028,10,2,79.92600000000039,110.67292022705078,0.08807225096983018,-4.010702133178711,-0.14980253942005772,-32.37457275390625,0.021834362625426706,-51.22603225708008,-0.11344138171106266,44.249629974365234,0.020470397859831735,87.66435241699219,0.040352632854677495,J2,0.14980253942005772,0.08716897465096986
16,16,3870,11.680362,1381848,3869,11.680263,1381840,624374172,-8,-0.09900000000051534,118.09595224767921,-17.37638727211256,-31.06900084212659,-59.56053843778568,48.73257576059714,94.13477131163891,115.98970031738281,-2.106251930296395,-12.693913459777832,4.682473812334727,-31.563575744628906,-0.49457490250231473,-59.300933837890625,0.2596045998950558,47.15116500854492,-1.5814107520522214,94.34331512451172,0.20854381287280432,J2,4.682473812334727,2.2067399623329567,3895,11.760326,1381850,624374252,10,2,79.96399999999859,118.06391906738281,-0.0320331802963949,-17.536325454711914,-0.15993818259935466,-31.00341796875,0.06558287337659152,-59.507286071777344,0.05325236600833705,48.77106475830078,0.038488997703638006,93.9618148803711,-0.17295643126782068,J6,0.17295643126782068,0.10419421346699378
17,18,4138,12.496365,1381950,4137,12.496271,1381942,624374988,-8,-0.09400000000070463,60.28248244838438,-22.938080568038327,-10.333247998560786,77.49164224770549,35.58337770883919,-69.66898071584893,61.48033142089844,1.197848972514059,-23.1823787689209,-0.2442982008825716,-10.346473693847656,-0.01322569528687012,79.24906921386719,1.7574269661617024,35.91868591308594,0.33530820424675056,-71.48294067382812,-1.8139599579791934,J6,1.8139599579791934,1.1536991630766555,4159,12.568313,1381951,624375060,9,1,71.94799999999901,60.27695846557617,-0.00552398280820654,-22.918088912963867,0.019991655074459658,-10.347146987915039,-0.013898989354252933,77.54158782958984,0.0499455818843586,35.57386016845703,-0.009517540382155687,-69.74390411376953,-0.07492339792059965,J6,0.07492339792059965,0.03834497543010648
1 trigger_no waypoint_index trigger_frame_number trigger_time_relative_s trigger_sequence paired_status_frame_number paired_status_time_relative_s paired_status_sequence paired_status_timestamp paired_status_to_trigger_sequence_delta paired_status_to_trigger_time_ms teach_j1_deg teach_j2_deg teach_j3_deg teach_j4_deg teach_j5_deg teach_j6_deg paired_status_j1_actual_deg paired_status_diff_j1_deg paired_status_j2_actual_deg paired_status_diff_j2_deg paired_status_j3_actual_deg paired_status_diff_j3_deg paired_status_j4_actual_deg paired_status_diff_j4_deg paired_status_j5_actual_deg paired_status_diff_j5_deg paired_status_j6_actual_deg paired_status_diff_j6_deg paired_status_max_error_axis paired_status_max_error_deg paired_status_rms_error_deg best_status_frame_number best_status_time_relative_s best_status_sequence best_status_timestamp best_status_delta_from_paired_cycles best_status_delta_from_trigger_sequence best_status_time_after_trigger_ms best_status_j1_actual_deg best_status_diff_j1_deg best_status_j2_actual_deg best_status_diff_j2_deg best_status_j3_actual_deg best_status_diff_j3_deg best_status_j4_actual_deg best_status_diff_j4_deg best_status_j5_actual_deg best_status_diff_j5_deg best_status_j6_actual_deg best_status_diff_j6_deg best_status_max_error_axis best_status_max_error_deg best_status_rms_error_deg
2 1 1 1955 5.888271 1381124 1954 5.888166 1381116 624368380 -8 -0.10499999999957765 48.886810269468405 2.1989850886957285 -11.021017368511105 0.4102097980549552 6.248381265333557 2.294990756284542 55.47505187988281 6.588241610414407 1.2506256103515625 -0.948359478344166 -5.400295734405518 5.620721634105587 0.5259769558906555 0.1157671578357003 3.3612561225891113 -2.8871251427444458 0.44421958923339844 -1.8507711670511435 J1 6.588241610414407 3.8225443630072706 1981 5.968124 1381126 624368460 10 2 79.85300000000083 48.76339340209961 -0.12341686736879609 2.256481647491455 0.05749655879572657 -11.157909393310547 -0.13689202479944207 0.37860527634620667 -0.031604521708748556 6.3406662940979 0.0922850287643433 2.5025806427001953 0.20758988641565335 J6 0.20758988641565335 0.12239685259209541
3 2 2 2151 6.480252 1381198 2150 6.480138 1381190 624368972 -8 -0.11399999999994748 55.34775509527405 11.807039833001637 -7.0090952672806885 -71.01433145543973 6.012065051914967 74.24953191606797 53.0124397277832 -2.33531536749085 11.506516456604004 -0.30052337639763316 -8.466276168823242 -1.4571809015425536 -69.63382720947266 1.3805042459670744 6.426647663116455 0.41458261120148787 72.93537902832031 -1.3141528877476532 J1 2.33531536749085 1.3827543088729335 2172 6.552164 1381199 624369044 9 1 71.9120000000002 55.265621185302734 -0.08213390997131853 11.803675651550293 -0.003364181451344095 -7.050373554229736 -0.04127828694904778 -71.06609344482422 -0.05176198938448806 6.026607990264893 0.014542938349925372 74.28738403320312 0.03785211713515935 J1 0.08213390997131853 0.04616054814443023
4 3 3 2223 6.704225 1381226 2222 6.704166 1381218 624369196 -8 -0.059000000000253294 55.109808014787404 8.759497374223619 -8.518216825284897 -41.11725046606459 10.108488114686867 41.956933292858096 56.5781364440918 1.4683284293043926 10.494372367858887 1.7348749936352679 -7.114619255065918 1.4035975702189791 -55.112239837646484 -13.994989371581894 7.423236846923828 -2.6852512677630394 58.080867767333984 16.123934474475888 J6 16.123934474475888 8.852376616648156 2248 6.784154 1381228 624369276 10 2 79.92899999999992 55.00252151489258 -0.10728649989482619 8.69005012512207 -0.06944724910154854 -8.577027320861816 -0.05881049557691931 -40.59286117553711 0.5243892905274805 10.210430145263672 0.1019420305768044 41.38724136352539 -0.5696919293327056 J6 0.5696919293327056 0.3239642953120798
5 4 4 2336 7.048266 1381269 2335 7.048148 1381261 624369540 -8 -0.11799999999961841 43.65359310453334 -1.629659559507137 -17.715753611915318 5.995208633582219 32.17171770646655 -22.573973337684023 44.479610443115234 0.8260173385818916 0.07192186266183853 1.7015814221689756 -15.996956825256348 1.7187967866589702 2.40028715133667 -3.5949214822455486 27.737812042236328 -4.433905664230224 -17.672090530395508 4.901882807288516 J6 4.901882807288516 3.2440556235708993 2362 7.128175 1381271 624369620 10 2 79.90899999999979 43.72715759277344 0.07356448824009476 -1.6386029720306396 -0.008943412523502614 -17.75617218017578 -0.04041856826046342 5.952699661254883 -0.04250897232733575 32.2616081237793 0.08989041731274483 -22.47926902770996 0.09470430997406254 J6 0.09470430997406254 0.06580474324148583
6 5 5 2477 7.464289 1381321 2476 7.464178 1381313 624369956 -8 -0.11099999999952814 64.58244475717193 4.262979315506351 -15.669217122643433 -29.952927185666542 29.850439933020308 45.62652743544272 60.830230712890625 -3.7522140442813026 1.8808640241622925 -2.382115291344059 -17.297021865844727 -1.6278047432012936 -26.684900283813477 3.268026901853066 33.02191925048828 3.1714793174679734 36.40708923339844 -9.21943820204428 J6 9.21943820204428 4.621321121191458 2502 7.544122 1381323 624370036 10 2 79.83299999999983 64.5761947631836 -0.006249993988333813 4.283629417419434 0.020650101913082253 -15.615406036376953 0.05381108626647979 -29.94143295288086 0.011494232785683067 29.7938175201416 -0.056622412878706285 45.61851119995117 -0.008016235491545842 J5 0.056622412878706285 0.03357478517689402
7 6 6 2665 8.024277 1381391 2664 8.024156 1381383 624370516 -8 -0.12100000000003774 60.47948252708421 23.068781981390185 -5.01126420129949 36.10685486878251 5.525681179628412 -31.3025636495649 60.89759063720703 0.418108110122823 20.716407775878906 -2.3523742055112784 -4.913214206695557 0.09804999460393304 32.72084045410156 -3.3860144146809503 6.405356407165527 0.8796752275371151 -25.156103134155273 6.1464605154096255 J6 6.1464605154096255 3.0478405356855083 2687 8.096181 1381392 624370588 9 1 71.90399999999997 60.54315185546875 0.06366932838454176 22.99761962890625 -0.07116235248393465 -5.037043571472168 -0.025779370172678284 36.056732177734375 -0.050122691048137824 5.545217037200928 0.019535857572515525 -31.12558937072754 0.17697427883735983 J6 0.17697427883735983 0.08563125303054821
8 7 7 2811 8.472317 1381447 2810 8.472208 1381439 624370964 -8 -0.10900000000013677 70.47583707168602 16.39384887825908 -13.456948007467595 -27.892318852946243 14.53566195089614 31.71193282686115 69.13842010498047 -1.3374169667055469 22.202428817749023 5.808579939489942 -12.907807350158691 0.5491406573089037 -15.469873428344727 12.422445424601516 14.040997505187988 -0.4946644457081515 18.56021499633789 -13.15171783052326 J6 13.15171783052326 7.78202379046887 2834 8.54419 1381448 624371036 9 1 71.87300000000008 70.3967514038086 -0.07908566787742188 16.456680297851562 0.06283141959248084 -13.330730438232422 0.12621756923517324 -27.611600875854492 0.2807179770917507 14.542427062988281 0.006765112092141479 31.405345916748047 -0.30658691011310424 J6 0.30658691011310424 0.18210669390591905
9 8 8 2908 8.760361 1381483 2907 8.760245 1381475 624371252 -8 -0.11600000000022703 69.58246408878419 -17.10571341532583 -8.710589696831251 -58.47569441890704 7.63059003611043 64.43773280685575 70.10379791259766 0.5213338238134639 -10.075156211853027 7.030557203472803 -9.98978042602539 -1.2791907291941396 -51.5866584777832 6.889035941123836 9.534638404846191 1.9040483687357614 57.0014533996582 -7.436279407197546 J6 7.436279407197546 5.127041386597523 2929 8.832199 1381484 624371324 9 1 71.83799999999962 69.59716033935547 0.014696250571276437 -17.044965744018555 0.06074767130727565 -8.703485488891602 0.007104207939649498 -58.473426818847656 0.0022676000593833123 7.6463942527771 0.015804216666669646 64.45584869384766 0.01811588699190736 J2 0.06074767130727565 0.02750705896232285
10 9 9 3112 9.384332 1381561 3111 9.384204 1381553 624371876 -8 -0.128000000000128 73.57125874861414 -6.42984524964374 -9.628579811400881 -128.80864727564332 14.671082817606491 140.00204816414424 72.96144104003906 -0.6098177085750791 -6.3001484870910645 0.12969676255267526 -10.035088539123535 -0.40650872772265423 -123.29313659667969 5.515510678963636 13.165532112121582 -1.505550705484909 134.41644287109375 -5.585605293050492 J6 5.585605293050492 3.277206017832995 3134 9.456205 1381562 624371948 9 1 71.87300000000008 73.55095672607422 -0.02030202253992286 -6.441007614135742 -0.011162364492002474 -9.614203453063965 0.014376358336916084 -128.65638732910156 0.15225994654176134 14.635379791259766 -0.03570302634672551 139.8520965576172 -0.14995160652705408 J4 0.15225994654176134 0.089150370994325
11 10 10 3170 9.560358 1381583 3169 9.56025 1381575 624372052 -8 -0.10800000000088517 75.56938603377543 -14.67930590788221 -7.294156094303152 -130.92303342701462 17.688361072687904 141.89350789658602 74.36400604248047 -1.2053799912949614 -10.514228820800781 4.165077087081428 -8.236283302307129 -0.9421272080039769 -132.6507110595703 -1.7276776325556966 16.38176155090332 -1.3065995217845838 143.5712890625 1.6777811659139843 J2 4.165077087081428 2.128983861449047 3196 9.640214 1381585 624372132 10 2 79.85599999999948 75.62290954589844 0.05352351212300732 -14.662798881530762 0.016507026351447962 -7.297325611114502 -0.003169516811349915 -130.88812255859375 0.03491086842086588 17.726865768432617 0.03850469574471305 141.820068359375 -0.0734395372110157 J6 0.0734395372110157 0.043285948566531826
12 11 11 3350 10.104322 1381651 3349 10.104215 1381643 624372596 -8 -0.10699999999985721 86.0934977330502 -14.498333177585573 -13.681510793859989 -69.8682955440411 26.742681901805224 88.9994136192388 87.81878662109375 1.7252888880435506 -16.329286575317383 -1.8309533977318093 -13.073507308959961 0.6080034849000278 -70.97254180908203 -1.1042462650409277 26.366661071777344 -0.3760208300278798 89.37981414794922 0.3804005287104246 J2 1.8309533977318093 1.169340011162825 3373 10.17622 1381652 624372668 9 1 71.8980000000009 86.11857604980469 0.025078316754488128 -14.53605842590332 -0.03772524831774682 -13.651655197143555 0.029855596716434007 -69.8272933959961 0.04100214804500979 26.72992706298828 -0.012754838816942282 88.92174530029297 -0.07766831894582538 J6 0.07766831894582538 0.04246508921571141
13 12 12 3470 10.472333 1381697 3469 10.472191 1381689 624372964 -8 -0.14200000000030855 61.720732564877665 -4.2327893098442155 -9.784423185760874 -108.38408270751574 22.16077221865394 118.14206389103131 65.91849517822266 4.197762613344992 -6.331206798553467 -2.0984174887092513 -11.008773803710938 -1.2243506179500638 -103.56172943115234 4.822353276363401 23.532543182373047 1.3717709637191078 117.46694946289062 -0.675114428140688 J4 4.822353276363401 2.8611252897135864 3493 10.544283 1381698 624373036 9 1 71.94999999999929 61.7418098449707 0.021077280093038553 -4.212831497192383 0.019957812651832718 -9.789913177490234 -0.00548999172936071 -108.36312103271484 0.020961674800901164 22.163673400878906 0.002901182224967158 118.13783264160156 -0.0042312494297505054 J1 0.021077280093038553 0.014935517911187784
14 13 13 3627 10.944371 1381756 3626 10.944251 1381748 624373436 -8 -0.12000000000078614 79.75708299218505 4.6402310571176475 -15.311185536748695 -56.03531247084017 26.13313788666632 52.152257172101606 72.04009246826172 -7.716990523923329 4.5515007972717285 -0.08873025984591898 -11.385177612304688 3.926007924444008 -64.18782043457031 -8.152507963730145 22.412456512451172 -3.7206813742151468 54.39056396484375 2.2383067927421436 J4 8.152507963730145 5.168659196117862 3651 11.016226 1381757 624373508 9 1 71.85499999999934 79.90262603759766 0.14554304541260876 4.62429666519165 -0.015934391925997105 -15.370849609375 -0.05966407262630469 -55.99199676513672 0.043315705703449225 26.182939529418945 0.04980164275262666 52.15596008300781 0.003702910906206114 J1 0.14554304541260876 0.0699602863618738
15 14 14 3710 11.192339 1381787 3709 11.19225 1381779 624373684 -8 -0.08900000000089392 108.95555081237923 1.3707600427061273 -33.59442519685133 -48.36736100282485 43.963403989432074 85.03926111958742 102.20675659179688 -6.748794220582354 2.5045764446258545 1.1338164019197272 -29.148040771484375 4.446384425366958 -48.76271438598633 -0.3953533831614777 39.65629959106445 -4.3071043983676205 74.63484191894531 -10.40441920064211 J6 10.40441920064211 5.679810575214033 3735 11.272261 1381789 624373764 10 2 79.92199999999983 108.94917297363281 -0.006377838746416842 1.3447811603546143 -0.025978882351513022 -33.561458587646484 0.032966609204848396 -48.376487731933594 -0.009126729108743348 44.02882766723633 0.06542367780425451 85.1587142944336 0.11945317484617135 J6 0.11945317484617135 0.05835950688086092
16 15 15 3796 11.456361 1381820 3795 11.456253 1381812 624373948 -8 -0.10799999999910881 110.58484797608095 -3.860899593758653 -32.39640711653168 -51.112590875369015 44.2291595765054 87.62399978413751 110.58193969726562 -0.002908278815326071 -1.244791865348816 2.6161077284098373 -33.74542999267578 -1.3490228761441045 -48.585723876953125 2.5268669984158905 44.58126449584961 0.35210491934420673 87.75857543945312 0.134575655315615 J2 2.6161077284098373 1.5911754635072362 3821 11.536287 1381822 624374028 10 2 79.92600000000039 110.67292022705078 0.08807225096983018 -4.010702133178711 -0.14980253942005772 -32.37457275390625 0.021834362625426706 -51.22603225708008 -0.11344138171106266 44.249629974365234 0.020470397859831735 87.66435241699219 0.040352632854677495 J2 0.14980253942005772 0.08716897465096986
17 16 16 3870 11.680362 1381848 3869 11.680263 1381840 624374172 -8 -0.09900000000051534 118.09595224767921 -17.37638727211256 -31.06900084212659 -59.56053843778568 48.73257576059714 94.13477131163891 115.98970031738281 -2.106251930296395 -12.693913459777832 4.682473812334727 -31.563575744628906 -0.49457490250231473 -59.300933837890625 0.2596045998950558 47.15116500854492 -1.5814107520522214 94.34331512451172 0.20854381287280432 J2 4.682473812334727 2.2067399623329567 3895 11.760326 1381850 624374252 10 2 79.96399999999859 118.06391906738281 -0.0320331802963949 -17.536325454711914 -0.15993818259935466 -31.00341796875 0.06558287337659152 -59.507286071777344 0.05325236600833705 48.77106475830078 0.038488997703638006 93.9618148803711 -0.17295643126782068 J6 0.17295643126782068 0.10419421346699378
18 17 18 4138 12.496365 1381950 4137 12.496271 1381942 624374988 -8 -0.09400000000070463 60.28248244838438 -22.938080568038327 -10.333247998560786 77.49164224770549 35.58337770883919 -69.66898071584893 61.48033142089844 1.197848972514059 -23.1823787689209 -0.2442982008825716 -10.346473693847656 -0.01322569528687012 79.24906921386719 1.7574269661617024 35.91868591308594 0.33530820424675056 -71.48294067382812 -1.8139599579791934 J6 1.8139599579791934 1.1536991630766555 4159 12.568313 1381951 624375060 9 1 71.94799999999901 60.27695846557617 -0.00552398280820654 -22.918088912963867 0.019991655074459658 -10.347146987915039 -0.013898989354252933 77.54158782958984 0.0499455818843586 35.57386016845703 -0.009517540382155687 -69.74390411376953 -0.07492339792059965 J6 0.07492339792059965 0.03834497543010648

View File

@@ -0,0 +1,18 @@
trigger_no,waypoint_index,frame_number,sequence,time_relative_s,write_io_value,io_addrs,config_addr,max_error_axis,max_error_deg,rms_error_deg,j1_actual_deg,j1_teach_deg,diff_j1_deg,j2_actual_deg,j2_teach_deg,diff_j2_deg,j3_actual_deg,j3_teach_deg,diff_j3_deg,j4_actual_deg,j4_teach_deg,diff_j4_deg,j5_actual_deg,j5_teach_deg,diff_j5_deg,j6_actual_deg,j6_teach_deg,diff_j6_deg
1,1,1955,1381124,5.888271,10,"[2, 4]","[2, 4]",J6,3.4065268256551553,2.468438977201345,45.8632698059082,48.886810269468405,-3.0235404635602023,2.9724788665771484,2.1989850886957285,0.7734937778814199,-13.609341621398926,-11.021017368511105,-2.588324252887821,-2.1851141452789307,0.4102097980549552,-2.5953239433338857,7.582361221313477,6.248381265333557,1.3339799559799195,5.701517581939697,2.294990756284542,3.4065268256551553
2,2,2151,1381198,6.480252,14,"[2, 3, 4]","[3, 4, 2]",J4,2.8167225076858244,1.6562461612538633,56.34336471557617,55.34775509527405,0.995609620302119,11.66723918914795,11.807039833001637,-0.13980064385368784,-6.600923538208008,-7.0090952672806885,0.40817172907268073,-68.1976089477539,-71.01433145543973,2.8167225076858244,6.058437347412109,6.012065051914967,0.04637229549714217,71.5392837524414,74.24953191606797,-2.7102481636265594
3,3,2223,1381226,6.704225,14,"[2, 3, 4]","[3, 4, 2]",J6,11.130744251720401,5.955607695749779,53.50831985473633,55.109808014787404,-1.6014881600510762,7.412222862243652,8.759497374223619,-1.3472745119799665,-9.629739761352539,-8.518216825284897,-1.111522936067642,-32.30571365356445,-41.11725046606459,8.811536812500137,12.488306045532227,10.108488114686867,2.379817930845359,30.826189041137695,41.956933292858096,-11.130744251720401
4,4,2336,1381269,7.048266,10,"[2, 4]","[4, 2]",J6,2.3116842904428125,1.3731583485126422,44.433006286621094,43.65359310453334,0.779413182087751,-2.1156294345855713,-1.629659559507137,-0.48596987507843425,-18.39777374267578,-17.715753611915318,-0.6820201307604634,4.8371992111206055,5.995208633582219,-1.158009422461613,33.99372482299805,32.17171770646655,1.8220071165314948,-20.26228904724121,-22.573973337684023,2.3116842904428125
5,5,2477,1381321,7.464289,10,"[2, 4]","[4, 2]",J5,2.171828177649214,1.404704060246151,65.84967041015625,64.58244475717193,1.2672256529843224,5.695003032684326,4.262979315506351,1.4320237171779748,-14.563636779785156,-15.669217122643433,1.1055803428582767,-28.69342613220215,-29.952927185666542,1.259501053464394,27.678611755371094,29.850439933020308,-2.171828177649214,46.437156677246094,45.62652743544272,0.810629241803376
6,6,2665,1381391,8.024277,12,"[3, 4]","[3, 4]",J2,1.408993592340284,0.8424031169595625,60.847530364990234,60.47948252708421,0.36804783790602613,24.47777557373047,23.068781981390185,1.408993592340284,-5.617043972015381,-5.01126420129949,-0.6057797707158912,34.8044319152832,36.10685486878251,-1.3024229534993097,5.786255359649658,5.525681179628412,0.260574180021246,-31.2253475189209,-31.3025636495649,0.07721613064400046
7,7,2811,1381447,8.472317,12,"[3, 4]","[3, 4]",J6,7.1941073647892395,4.618545262978315,70.8250503540039,70.47583707168602,0.3492132823178906,10.95259952545166,16.39384887825908,-5.4412493528074215,-13.136046409606934,-13.456948007467595,0.3209015978606615,-34.68459701538086,-27.892318852946243,-6.7922781624346165,14.022420883178711,14.53566195089614,-0.5132410677174288,38.90604019165039,31.71193282686115,7.1941073647892395
8,8,2908,1381483,8.760361,10,"[2, 4]","[4, 2]",J6,5.93988407546847,3.533132924843701,69.40516662597656,69.58246408878419,-0.1772974628076298,-20.066781997680664,-17.10571341532583,-2.9610685823548337,-8.152935981750488,-8.710589696831251,0.5576537150807628,-63.93235397338867,-58.47569441890704,-5.456659554481632,6.775912284851074,7.63059003611043,-0.8546777512593557,70.37761688232422,64.43773280685575,5.93988407546847
9,9,3112,1381561,9.384332,10,"[2, 4]","[4, 2]",J4,2.5240706198644887,1.630593251762862,73.90592956542969,73.57125874861414,0.3346708168155459,-8.016124725341797,-6.42984524964374,-1.5862794756980572,-9.002426147460938,-9.628579811400881,0.6261536639399434,-131.3327178955078,-128.80864727564332,-2.5240706198644887,15.526206970214844,14.671082817606491,0.8551241526083526,142.41668701171875,140.00204816414424,2.4146388475745084
10,10,3170,1381583,9.560358,10,"[2, 4]","[4, 2]",J4,3.3929965007450846,2.1059862266772753,76.72688293457031,75.56938603377543,1.1574969007948823,-16.518726348876953,-14.67930590788221,-1.8394204409947434,-7.15147066116333,-7.294156094303152,0.14268543313982196,-127.53003692626953,-130.92303342701462,3.3929965007450846,18.53788948059082,17.688361072687904,0.8495284079029162,138.7897491455078,141.89350789658602,-3.103758751078203
11,11,3350,1381651,10.104322,10,"[2, 4]","[4, 2]",J6,2.9848240516596434,2.0134925384056306,83.71662139892578,86.0934977330502,-2.376876334124418,-13.186573028564453,-14.498333177585573,1.3117601490211204,-13.738808631896973,-13.681510793859989,-0.05729783803698396,-72.70317840576172,-69.8682955440411,-2.834882861720615,26.66758918762207,26.742681901805224,-0.07509271418315322,91.98423767089844,88.9994136192388,2.9848240516596434
12,12,3470,1381697,10.472333,10,"[2, 4]","[4, 2]",J6,4.000958239175844,1.9902461373621054,59.78955841064453,61.720732564877665,-1.9311741542331333,-2.7094168663024902,-4.2327893098442155,1.5233724435417253,-8.933828353881836,-9.784423185760874,0.8505948318790377,-108.24857330322266,-108.38408270751574,0.13550940429308866,21.177492141723633,22.16077221865394,-0.9832800769303063,114.14110565185547,118.14206389103131,-4.000958239175844
13,13,3627,1381756,10.944371,12,"[3, 4]","[4, 3]",J1,6.656208938967296,4.109282348845555,86.41329193115234,79.75708299218505,6.656208938967296,4.266173362731934,4.6402310571176475,-0.3740576943857139,-19.15987777709961,-15.311185536748695,-3.848692240350914,-52.24028015136719,-56.03531247084017,3.7950323194729805,29.853723526000977,26.13313788666632,3.720585639334658,55.86907196044922,52.152257172101606,3.7168147883476124
14,14,3710,1381787,11.192339,10,"[2, 4]","[4, 2]",J6,2.93518040873289,1.5679827735552678,110.7523193359375,108.95555081237923,1.7967685235582707,0.8751887679100037,1.3707600427061273,-0.4955712747961236,-34.756534576416016,-33.59442519685133,-1.1621093795646829,-48.14332580566406,-48.36736100282485,0.2240351971607879,45.0865592956543,43.963403989432074,1.1231553062222233,87.97444152832031,85.03926111958742,2.93518040873289
15,15,3796,1381820,11.456361,10,"[2, 4]","[4, 2]",J4,2.573146734494266,1.686689503314435,111.79177856445312,110.58484797608095,1.206930588372174,-6.225440502166748,-3.860899593758653,-2.364540908408095,-31.958988189697266,-32.39640711653168,0.4374189268344111,-53.68573760986328,-51.112590875369015,-2.573146734494266,44.76150894165039,44.2291595765054,0.532349365144988,89.33454895019531,87.62399978413751,1.7105491660578025
16,16,3870,1381848,11.680362,10,"[2, 4]","[4, 2]",J6,3.8105220440607894,2.197782779718695,117.87474060058594,118.09595224767921,-0.2212116470932699,-19.607177734375,-17.37638727211256,-2.2307904622624406,-30.329103469848633,-31.06900084212659,0.7398973722779587,-56.600250244140625,-59.56053843778568,2.960288193645056,49.08655548095703,48.73257576059714,0.353979720359888,90.32424926757812,94.13477131163891,-3.8105220440607894
17,18,4138,1381950,12.496365,12,"[3, 4]","[4, 3]",J4,6.848522435693766,4.03614029285736,59.93589401245117,60.28248244838438,-0.34658843593320654,-21.102140426635742,-22.938080568038327,1.8359401414025847,-9.684239387512207,-10.333247998560786,0.6490086110485791,70.64311981201172,77.49164224770549,-6.848522435693766,32.941307067871094,35.58337770883919,-2.642070640968093,-63.34855651855469,-69.66898071584893,6.320424197294244
1 trigger_no waypoint_index frame_number sequence time_relative_s write_io_value io_addrs config_addr max_error_axis max_error_deg rms_error_deg j1_actual_deg j1_teach_deg diff_j1_deg j2_actual_deg j2_teach_deg diff_j2_deg j3_actual_deg j3_teach_deg diff_j3_deg j4_actual_deg j4_teach_deg diff_j4_deg j5_actual_deg j5_teach_deg diff_j5_deg j6_actual_deg j6_teach_deg diff_j6_deg
2 1 1 1955 1381124 5.888271 10 [2, 4] [2, 4] J6 3.4065268256551553 2.468438977201345 45.8632698059082 48.886810269468405 -3.0235404635602023 2.9724788665771484 2.1989850886957285 0.7734937778814199 -13.609341621398926 -11.021017368511105 -2.588324252887821 -2.1851141452789307 0.4102097980549552 -2.5953239433338857 7.582361221313477 6.248381265333557 1.3339799559799195 5.701517581939697 2.294990756284542 3.4065268256551553
3 2 2 2151 1381198 6.480252 14 [2, 3, 4] [3, 4, 2] J4 2.8167225076858244 1.6562461612538633 56.34336471557617 55.34775509527405 0.995609620302119 11.66723918914795 11.807039833001637 -0.13980064385368784 -6.600923538208008 -7.0090952672806885 0.40817172907268073 -68.1976089477539 -71.01433145543973 2.8167225076858244 6.058437347412109 6.012065051914967 0.04637229549714217 71.5392837524414 74.24953191606797 -2.7102481636265594
4 3 3 2223 1381226 6.704225 14 [2, 3, 4] [3, 4, 2] J6 11.130744251720401 5.955607695749779 53.50831985473633 55.109808014787404 -1.6014881600510762 7.412222862243652 8.759497374223619 -1.3472745119799665 -9.629739761352539 -8.518216825284897 -1.111522936067642 -32.30571365356445 -41.11725046606459 8.811536812500137 12.488306045532227 10.108488114686867 2.379817930845359 30.826189041137695 41.956933292858096 -11.130744251720401
5 4 4 2336 1381269 7.048266 10 [2, 4] [4, 2] J6 2.3116842904428125 1.3731583485126422 44.433006286621094 43.65359310453334 0.779413182087751 -2.1156294345855713 -1.629659559507137 -0.48596987507843425 -18.39777374267578 -17.715753611915318 -0.6820201307604634 4.8371992111206055 5.995208633582219 -1.158009422461613 33.99372482299805 32.17171770646655 1.8220071165314948 -20.26228904724121 -22.573973337684023 2.3116842904428125
6 5 5 2477 1381321 7.464289 10 [2, 4] [4, 2] J5 2.171828177649214 1.404704060246151 65.84967041015625 64.58244475717193 1.2672256529843224 5.695003032684326 4.262979315506351 1.4320237171779748 -14.563636779785156 -15.669217122643433 1.1055803428582767 -28.69342613220215 -29.952927185666542 1.259501053464394 27.678611755371094 29.850439933020308 -2.171828177649214 46.437156677246094 45.62652743544272 0.810629241803376
7 6 6 2665 1381391 8.024277 12 [3, 4] [3, 4] J2 1.408993592340284 0.8424031169595625 60.847530364990234 60.47948252708421 0.36804783790602613 24.47777557373047 23.068781981390185 1.408993592340284 -5.617043972015381 -5.01126420129949 -0.6057797707158912 34.8044319152832 36.10685486878251 -1.3024229534993097 5.786255359649658 5.525681179628412 0.260574180021246 -31.2253475189209 -31.3025636495649 0.07721613064400046
8 7 7 2811 1381447 8.472317 12 [3, 4] [3, 4] J6 7.1941073647892395 4.618545262978315 70.8250503540039 70.47583707168602 0.3492132823178906 10.95259952545166 16.39384887825908 -5.4412493528074215 -13.136046409606934 -13.456948007467595 0.3209015978606615 -34.68459701538086 -27.892318852946243 -6.7922781624346165 14.022420883178711 14.53566195089614 -0.5132410677174288 38.90604019165039 31.71193282686115 7.1941073647892395
9 8 8 2908 1381483 8.760361 10 [2, 4] [4, 2] J6 5.93988407546847 3.533132924843701 69.40516662597656 69.58246408878419 -0.1772974628076298 -20.066781997680664 -17.10571341532583 -2.9610685823548337 -8.152935981750488 -8.710589696831251 0.5576537150807628 -63.93235397338867 -58.47569441890704 -5.456659554481632 6.775912284851074 7.63059003611043 -0.8546777512593557 70.37761688232422 64.43773280685575 5.93988407546847
10 9 9 3112 1381561 9.384332 10 [2, 4] [4, 2] J4 2.5240706198644887 1.630593251762862 73.90592956542969 73.57125874861414 0.3346708168155459 -8.016124725341797 -6.42984524964374 -1.5862794756980572 -9.002426147460938 -9.628579811400881 0.6261536639399434 -131.3327178955078 -128.80864727564332 -2.5240706198644887 15.526206970214844 14.671082817606491 0.8551241526083526 142.41668701171875 140.00204816414424 2.4146388475745084
11 10 10 3170 1381583 9.560358 10 [2, 4] [4, 2] J4 3.3929965007450846 2.1059862266772753 76.72688293457031 75.56938603377543 1.1574969007948823 -16.518726348876953 -14.67930590788221 -1.8394204409947434 -7.15147066116333 -7.294156094303152 0.14268543313982196 -127.53003692626953 -130.92303342701462 3.3929965007450846 18.53788948059082 17.688361072687904 0.8495284079029162 138.7897491455078 141.89350789658602 -3.103758751078203
12 11 11 3350 1381651 10.104322 10 [2, 4] [4, 2] J6 2.9848240516596434 2.0134925384056306 83.71662139892578 86.0934977330502 -2.376876334124418 -13.186573028564453 -14.498333177585573 1.3117601490211204 -13.738808631896973 -13.681510793859989 -0.05729783803698396 -72.70317840576172 -69.8682955440411 -2.834882861720615 26.66758918762207 26.742681901805224 -0.07509271418315322 91.98423767089844 88.9994136192388 2.9848240516596434
13 12 12 3470 1381697 10.472333 10 [2, 4] [4, 2] J6 4.000958239175844 1.9902461373621054 59.78955841064453 61.720732564877665 -1.9311741542331333 -2.7094168663024902 -4.2327893098442155 1.5233724435417253 -8.933828353881836 -9.784423185760874 0.8505948318790377 -108.24857330322266 -108.38408270751574 0.13550940429308866 21.177492141723633 22.16077221865394 -0.9832800769303063 114.14110565185547 118.14206389103131 -4.000958239175844
14 13 13 3627 1381756 10.944371 12 [3, 4] [4, 3] J1 6.656208938967296 4.109282348845555 86.41329193115234 79.75708299218505 6.656208938967296 4.266173362731934 4.6402310571176475 -0.3740576943857139 -19.15987777709961 -15.311185536748695 -3.848692240350914 -52.24028015136719 -56.03531247084017 3.7950323194729805 29.853723526000977 26.13313788666632 3.720585639334658 55.86907196044922 52.152257172101606 3.7168147883476124
15 14 14 3710 1381787 11.192339 10 [2, 4] [4, 2] J6 2.93518040873289 1.5679827735552678 110.7523193359375 108.95555081237923 1.7967685235582707 0.8751887679100037 1.3707600427061273 -0.4955712747961236 -34.756534576416016 -33.59442519685133 -1.1621093795646829 -48.14332580566406 -48.36736100282485 0.2240351971607879 45.0865592956543 43.963403989432074 1.1231553062222233 87.97444152832031 85.03926111958742 2.93518040873289
16 15 15 3796 1381820 11.456361 10 [2, 4] [4, 2] J4 2.573146734494266 1.686689503314435 111.79177856445312 110.58484797608095 1.206930588372174 -6.225440502166748 -3.860899593758653 -2.364540908408095 -31.958988189697266 -32.39640711653168 0.4374189268344111 -53.68573760986328 -51.112590875369015 -2.573146734494266 44.76150894165039 44.2291595765054 0.532349365144988 89.33454895019531 87.62399978413751 1.7105491660578025
17 16 16 3870 1381848 11.680362 10 [2, 4] [4, 2] J6 3.8105220440607894 2.197782779718695 117.87474060058594 118.09595224767921 -0.2212116470932699 -19.607177734375 -17.37638727211256 -2.2307904622624406 -30.329103469848633 -31.06900084212659 0.7398973722779587 -56.600250244140625 -59.56053843778568 2.960288193645056 49.08655548095703 48.73257576059714 0.353979720359888 90.32424926757812 94.13477131163891 -3.8105220440607894
18 17 18 4138 1381950 12.496365 12 [3, 4] [4, 3] J4 6.848522435693766 4.03614029285736 59.93589401245117 60.28248244838438 -0.34658843593320654 -21.102140426635742 -22.938080568038327 1.8359401414025847 -9.684239387512207 -10.333247998560786 0.6490086110485791 70.64311981201172 77.49164224770549 -6.848522435693766 32.941307067871094 35.58337770883919 -2.642070640968093 -63.34855651855469 -69.66898071584893 6.320424197294244

View File

@@ -0,0 +1,363 @@
#!/usr/bin/env python3
"""提取 2026042802-1 抓包中的 60015 状态反馈,并和 UTTC_MS11 示教点对比。"""
from __future__ import annotations
import csv
import json
import math
import struct
import subprocess
from collections import Counter
from pathlib import Path
REPO_ROOT = Path(__file__).resolve().parents[1]
DEFAULT_PCAP = REPO_ROOT.parent / "Rvbust" / "uttc-20260428" / "2026042802-1.pcap"
DEFAULT_TSHARK = Path(r"D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe")
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
SEARCH_WINDOW_CYCLES = 20
def be_u32(data: bytes, offset: int) -> int:
"""按大端读取 4 字节无符号整数。"""
return struct.unpack(">I", data[offset : offset + 4])[0]
def be_u16(data: bytes, offset: int) -> int:
"""按大端读取 2 字节无符号整数。"""
return struct.unpack(">H", data[offset : offset + 2])[0]
def be_f32(data: bytes, offset: int) -> float:
"""按大端读取 4 字节浮点数。"""
return struct.unpack(">f", data[offset : offset + 4])[0]
def load_udp_rows(pcap: Path, tshark: Path) -> list[list[str]]:
"""提取 UDP 60015 原始字段,后续按方向和长度拆分命令与状态。"""
command = [
str(tshark),
"-r",
str(pcap),
"-Y",
"udp.port==60015",
"-T",
"fields",
"-e",
"frame.number",
"-e",
"frame.time_relative",
"-e",
"ip.src",
"-e",
"ip.dst",
"-e",
"udp.payload",
]
output = subprocess.check_output(command, text=True, encoding="utf-8", errors="ignore")
rows: list[list[str]] = []
for line in output.splitlines():
if not line.strip():
continue
parts = line.split("\t")
if len(parts) >= 5:
rows.append(parts[:5])
return rows
def decode_command_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
"""把 64B J519 命令帧解码成结构化记录。"""
records: list[dict] = []
for frame_no, time_rel, ip_src, ip_dst, payload_hex in rows:
if ip_src != client_ip or ip_dst != robot_ip:
continue
payload = bytes.fromhex(payload_hex)
if len(payload) != 64:
continue
records.append(
{
"frame_number": int(frame_no),
"time_relative_s": float(time_rel),
"sequence": be_u32(payload, 0x08),
"write_io_value": be_u16(payload, 0x18),
"j1_deg": be_f32(payload, 0x1C),
"j2_deg": be_f32(payload, 0x20),
"j3_deg": be_f32(payload, 0x24),
"j4_deg": be_f32(payload, 0x28),
"j5_deg": be_f32(payload, 0x2C),
"j6_deg": be_f32(payload, 0x30),
}
)
return records
def decode_status_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
"""把 132B J519 状态帧按运行时代码同口径解码。"""
records: list[dict] = []
for frame_no, time_rel, ip_src, ip_dst, payload_hex in rows:
if ip_src != robot_ip or ip_dst != client_ip:
continue
payload = bytes.fromhex(payload_hex)
if len(payload) != 132:
continue
status = payload[0x0C]
joints = [be_f32(payload, 0x3C + index * 4) for index in range(6)]
pose = [be_f32(payload, 0x18 + index * 4) for index in range(6)]
records.append(
{
"frame_number": int(frame_no),
"time_relative_s": float(time_rel),
"sequence": be_u32(payload, 0x08),
"status": status,
"accepts_command": bool(status & 0b0001),
"received_command": bool(status & 0b0010),
"system_ready": bool(status & 0b0100),
"robot_in_motion": bool(status & 0b1000),
"read_io_value": be_u16(payload, 0x12),
"timestamp": be_u32(payload, 0x14),
"pose_x_mm": pose[0],
"pose_y_mm": pose[1],
"pose_z_mm": pose[2],
"pose_w_deg": pose[3],
"pose_p_deg": pose[4],
"pose_r_deg": pose[5],
"j1_deg": joints[0],
"j2_deg": joints[1],
"j3_deg": joints[2],
"j4_deg": joints[3],
"j5_deg": joints[4],
"j6_deg": joints[5],
}
)
return records
def pick_trigger_first_high_frames(records: list[dict]) -> list[dict]:
"""由于 io_keep_cycles=2,只保留每组高电平脉冲的第一帧。"""
trigger_frames: list[dict] = []
previous_high = False
for record in records:
current_high = record["write_io_value"] > 0
if current_high and not previous_high:
trigger_frames.append(record)
previous_high = current_high
return trigger_frames
def load_uttc_ms11_config() -> dict:
"""读取 UTTC_MS11 的示教点和触发配置。"""
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
return config["flying_shots"]["UTTC_MS11"]
def build_diff_row(prefix: str, actual_deg: list[float], teach_deg: list[float], row: dict) -> tuple[float, float, str]:
"""向结果行写入逐轴误差,并返回聚合误差。"""
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
abs_diffs = [abs(value) for value in diffs]
max_error = max(abs_diffs)
max_error_axis = f"J{abs_diffs.index(max_error) + 1}"
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
for joint_index in range(6):
joint_no = joint_index + 1
row[f"{prefix}_j{joint_no}_actual_deg"] = actual_deg[joint_index]
row[f"{prefix}_diff_j{joint_no}_deg"] = diffs[joint_index]
row[f"{prefix}_max_error_axis"] = max_error_axis
row[f"{prefix}_max_error_deg"] = max_error
row[f"{prefix}_rms_error_deg"] = rms_error
return max_error, rms_error, max_error_axis
def build_trigger_status_rows(trigger_frames: list[dict], status_records: list[dict], shot_config: dict) -> list[dict]:
"""按触发顺序对齐命令帧、当前状态帧以及最接近示教点的反馈状态帧。"""
rows: list[dict] = []
trigger_waypoint_indices = [index for index, flag in enumerate(shot_config["shot_flags"]) if flag]
status_by_sequence = {record["sequence"]: record for record in status_records}
status_sequence_set = set(status_by_sequence)
for trigger_no, (trigger_frame, waypoint_index) in enumerate(zip(trigger_frames, trigger_waypoint_indices), start=1):
teach_deg = [math.degrees(value) for value in shot_config["traj_waypoints"][waypoint_index]]
current_status_sequence = trigger_frame["sequence"] - 8
current_status = status_by_sequence[current_status_sequence]
row = {
"trigger_no": trigger_no,
"waypoint_index": waypoint_index,
"trigger_frame_number": trigger_frame["frame_number"],
"trigger_time_relative_s": trigger_frame["time_relative_s"],
"trigger_sequence": trigger_frame["sequence"],
"paired_status_frame_number": current_status["frame_number"],
"paired_status_time_relative_s": current_status["time_relative_s"],
"paired_status_sequence": current_status["sequence"],
"paired_status_timestamp": current_status["timestamp"],
"paired_status_to_trigger_sequence_delta": current_status["sequence"] - trigger_frame["sequence"],
"paired_status_to_trigger_time_ms": (current_status["time_relative_s"] - trigger_frame["time_relative_s"]) * 1000.0,
}
for joint_index in range(6):
joint_no = joint_index + 1
row[f"teach_j{joint_no}_deg"] = teach_deg[joint_index]
build_diff_row(
"paired_status",
[current_status[f"j{joint_no}_deg"] for joint_no in range(1, 7)],
teach_deg,
row,
)
best_candidate = None
for delta_cycles in range(-SEARCH_WINDOW_CYCLES, SEARCH_WINDOW_CYCLES + 1):
candidate_sequence = current_status_sequence + delta_cycles
if candidate_sequence not in status_sequence_set:
continue
candidate = status_by_sequence[candidate_sequence]
diffs = [candidate[f"j{joint_no}_deg"] - teach_deg[joint_no - 1] for joint_no in range(1, 7)]
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
max_error = max(abs(value) for value in diffs)
score = (rms_error, max_error, abs(delta_cycles))
if best_candidate is None or score < best_candidate["score"]:
best_candidate = {
"score": score,
"delta_cycles": delta_cycles,
"record": candidate,
}
if best_candidate is None:
raise RuntimeError(f"Trigger {trigger_no} 未找到候选状态帧。")
best_status = best_candidate["record"]
row["best_status_frame_number"] = best_status["frame_number"]
row["best_status_time_relative_s"] = best_status["time_relative_s"]
row["best_status_sequence"] = best_status["sequence"]
row["best_status_timestamp"] = best_status["timestamp"]
row["best_status_delta_from_paired_cycles"] = best_candidate["delta_cycles"]
row["best_status_delta_from_trigger_sequence"] = best_status["sequence"] - trigger_frame["sequence"]
row["best_status_time_after_trigger_ms"] = (best_status["time_relative_s"] - trigger_frame["time_relative_s"]) * 1000.0
build_diff_row(
"best_status",
[best_status[f"j{joint_no}_deg"] for joint_no in range(1, 7)],
teach_deg,
row,
)
rows.append(row)
return rows
def write_csv(path: Path, rows: list[dict]) -> None:
"""把分析结果落成 UTF-8 CSV。"""
if not rows:
raise ValueError(f"No rows to write: {path}")
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=list(rows[0].keys()))
writer.writeheader()
writer.writerows(rows)
def build_summary(command_records: list[dict], trigger_status_rows: list[dict]) -> dict:
"""汇总命令序列偏移和状态反馈误差分布。"""
sequence_offsets = [row["trigger_sequence"] - row["paired_status_sequence"] for row in trigger_status_rows]
best_paired_cycle_offsets = [row["best_status_delta_from_paired_cycles"] for row in trigger_status_rows]
best_trigger_sequence_offsets = [row["best_status_delta_from_trigger_sequence"] for row in trigger_status_rows]
best_time_offsets = [row["best_status_time_after_trigger_ms"] for row in trigger_status_rows]
paired_max_errors = [row["paired_status_max_error_deg"] for row in trigger_status_rows]
best_max_errors = [row["best_status_max_error_deg"] for row in trigger_status_rows]
paired_axes = Counter(row["paired_status_max_error_axis"] for row in trigger_status_rows)
best_axes = Counter(row["best_status_max_error_axis"] for row in trigger_status_rows)
sequence_offset_counter = Counter()
trigger_frames = pick_trigger_first_high_frames(command_records)
status_pairs = {row["trigger_frame_number"]: row for row in trigger_status_rows}
for trigger_frame in trigger_frames:
sequence_offset_counter[trigger_frame["sequence"] - status_pairs[trigger_frame["frame_number"]]["paired_status_sequence"]] += 1
return {
"pcap_path": str(DEFAULT_PCAP),
"command_count": len(command_records),
"trigger_count": len(trigger_status_rows),
"command_minus_paired_status_sequence_counter": dict(sequence_offset_counter),
"paired_status_average_max_error_deg": sum(paired_max_errors) / len(paired_max_errors),
"paired_status_max_error_deg": max(paired_max_errors),
"paired_status_max_error_axis_counter": dict(paired_axes),
"best_status_average_max_error_deg": sum(best_max_errors) / len(best_max_errors),
"best_status_max_error_deg": max(best_max_errors),
"best_status_max_error_axis_counter": dict(best_axes),
"best_status_delta_from_paired_cycles_counter": dict(Counter(best_paired_cycle_offsets)),
"best_status_delta_from_trigger_sequence_counter": dict(Counter(best_trigger_sequence_offsets)),
"best_status_time_after_trigger_ms_min": min(best_time_offsets),
"best_status_time_after_trigger_ms_max": max(best_time_offsets),
"best_status_time_after_trigger_ms_avg": sum(best_time_offsets) / len(best_time_offsets),
"search_window_cycles": SEARCH_WINDOW_CYCLES,
}
def build_manual_compare_rows(trigger_status_rows: list[dict]) -> list[dict]:
"""整理成便于人工逐点核对的三时刻对照表。"""
rows: list[dict] = []
for row in trigger_status_rows:
rows.append(
{
"trigger_no": row["trigger_no"],
"waypoint_index": row["waypoint_index"],
"trigger_command_sequence": row["trigger_sequence"],
"trigger_command_frame": row["trigger_frame_number"],
"trigger_command_time_relative_s": row["trigger_time_relative_s"],
"trigger_current_status_sequence": row["paired_status_sequence"],
"trigger_current_status_frame": row["paired_status_frame_number"],
"trigger_current_status_time_relative_s": row["paired_status_time_relative_s"],
"command_leads_status_cycles": row["trigger_sequence"] - row["paired_status_sequence"],
"trigger_current_status_max_error_axis": row["paired_status_max_error_axis"],
"trigger_current_status_max_error_deg": row["paired_status_max_error_deg"],
"trigger_current_status_rms_error_deg": row["paired_status_rms_error_deg"],
"best_status_sequence": row["best_status_sequence"],
"best_status_frame": row["best_status_frame_number"],
"best_status_time_relative_s": row["best_status_time_relative_s"],
"best_status_delay_from_current_status_cycles": row["best_status_delta_from_paired_cycles"],
"best_status_delay_from_trigger_command_cycles": row["best_status_delta_from_trigger_sequence"],
"best_status_delay_from_trigger_command_ms": row["best_status_time_after_trigger_ms"],
"best_status_max_error_axis": row["best_status_max_error_axis"],
"best_status_max_error_deg": row["best_status_max_error_deg"],
"best_status_rms_error_deg": row["best_status_rms_error_deg"],
}
)
return rows
def main() -> None:
"""执行状态反馈提取、触发对齐和摘要落盘。"""
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
rows = load_udp_rows(DEFAULT_PCAP, DEFAULT_TSHARK)
command_records = decode_command_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
status_records = decode_status_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
trigger_frames = pick_trigger_first_high_frames(command_records)
shot_config = load_uttc_ms11_config()
trigger_status_rows = build_trigger_status_rows(trigger_frames, status_records, shot_config)
manual_compare_rows = build_manual_compare_rows(trigger_status_rows)
summary = build_summary(command_records, trigger_status_rows)
write_csv(OUTPUT_DIR / "2026042802-1_j519_status_feedback_all.csv", status_records)
write_csv(OUTPUT_DIR / "2026042802-1_trigger_status_feedback_vs_teach_points.csv", trigger_status_rows)
write_csv(OUTPUT_DIR / "2026042802-1_trigger_manual_compare.csv", manual_compare_rows)
(OUTPUT_DIR / "2026042802-1_status_feedback_summary.json").write_text(
json.dumps(summary, ensure_ascii=False, indent=2),
encoding="utf-8",
)
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()

View File

@@ -0,0 +1,250 @@
#!/usr/bin/env python3
"""提取 2026042802-1 抓包中的真实 J519 发包,并对比 UTTC_MS11 示教点。"""
from __future__ import annotations
import csv
import json
import math
import struct
import subprocess
from collections import Counter
from pathlib import Path
REPO_ROOT = Path(__file__).resolve().parents[1]
DEFAULT_PCAP = REPO_ROOT.parent / "Rvbust" / "uttc-20260428" / "2026042802-1.pcap"
DEFAULT_TSHARK = Path(r"D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe")
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
RUNTIME_DATA_DIR = REPO_ROOT / "Config" / "Data" / "UTTC_MS11"
def be_u32(data: bytes, offset: int) -> int:
"""按大端读取 4 字节无符号整数。"""
return struct.unpack(">I", data[offset : offset + 4])[0]
def be_u16(data: bytes, offset: int) -> int:
"""按大端读取 2 字节无符号整数。"""
return struct.unpack(">H", data[offset : offset + 2])[0]
def be_f32(data: bytes, offset: int) -> float:
"""按大端读取 4 字节浮点数。"""
return struct.unpack(">f", data[offset : offset + 4])[0]
def load_j519_command_rows(pcap: Path, tshark: Path) -> list[list[str]]:
"""只提取 UDP 60015 的原始字段,后续再按 IP 方向筛选真实下发命令。"""
command = [
str(tshark),
"-r",
str(pcap),
"-Y",
"udp.port==60015",
"-T",
"fields",
"-e",
"frame.number",
"-e",
"frame.time_relative",
"-e",
"ip.src",
"-e",
"udp.srcport",
"-e",
"ip.dst",
"-e",
"udp.dstport",
"-e",
"udp.payload",
]
output = subprocess.check_output(command, text=True, encoding="utf-8", errors="ignore")
rows: list[list[str]] = []
for line in output.splitlines():
if not line.strip():
continue
parts = line.split("\t")
if len(parts) >= 7:
rows.append(parts[:7])
return rows
def decode_command_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
"""把抓包中的 64B J519 命令帧解码成带 IO 信息的结构化记录。"""
records: list[dict] = []
for frame_no, time_rel, ip_src, _udp_src, ip_dst, _udp_dst, payload_hex in rows:
if ip_src != client_ip or ip_dst != robot_ip:
continue
payload = bytes.fromhex(payload_hex)
if len(payload) != 64:
continue
io_value = be_u16(payload, 0x18)
io_addrs = [bit + 1 for bit in range(16) if io_value & (1 << bit)]
targets = [be_f32(payload, 0x1C + index * 4) for index in range(9)]
records.append(
{
"frame_number": int(frame_no),
"time_relative_s": float(time_rel),
"sequence": be_u32(payload, 0x08),
"last_data": payload[0x0C],
"write_io_type": payload[0x13],
"write_io_index": be_u16(payload, 0x14),
"write_io_mask": be_u16(payload, 0x16),
"write_io_value": io_value,
"io_addrs": io_addrs,
"j1_deg": targets[0],
"j2_deg": targets[1],
"j3_deg": targets[2],
"j4_deg": targets[3],
"j5_deg": targets[4],
"j6_deg": targets[5],
"ext1_deg": targets[6],
"ext2_deg": targets[7],
"ext3_deg": targets[8],
}
)
return records
def pick_trigger_first_high_frames(records: list[dict]) -> list[dict]:
"""由于 io_keep_cycles=2,只记录每组高电平脉冲的第一帧。"""
trigger_frames: list[dict] = []
previous_high = False
for record in records:
current_high = record["write_io_value"] > 0
if current_high and not previous_high:
trigger_frames.append(record)
previous_high = current_high
return trigger_frames
def load_uttc_ms11_config() -> dict:
"""读取 UTTC_MS11 的示教点和触发配置。"""
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
return config["flying_shots"]["UTTC_MS11"]
def build_trigger_vs_teach_rows(trigger_frames: list[dict], shot_config: dict) -> list[dict]:
"""按 shot_flags 为 true 的 waypoint 顺序,对齐抓包触发帧和示教点。"""
rows: list[dict] = []
trigger_waypoint_indices = [index for index, flag in enumerate(shot_config["shot_flags"]) if flag]
for trigger_no, (frame, waypoint_index) in enumerate(zip(trigger_frames, trigger_waypoint_indices), start=1):
teach_rad = shot_config["traj_waypoints"][waypoint_index]
teach_deg = [math.degrees(value) for value in teach_rad]
actual_deg = [frame[f"j{joint_index}_deg"] for joint_index in range(1, 7)]
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
abs_diffs = [abs(value) for value in diffs]
max_error = max(abs_diffs)
max_error_axis = f"J{abs_diffs.index(max_error) + 1}"
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
row = {
"trigger_no": trigger_no,
"waypoint_index": waypoint_index,
"frame_number": frame["frame_number"],
"sequence": frame["sequence"],
"time_relative_s": frame["time_relative_s"],
"write_io_value": frame["write_io_value"],
"io_addrs": frame["io_addrs"],
"config_addr": shot_config["addr"][waypoint_index],
"max_error_axis": max_error_axis,
"max_error_deg": max_error,
"rms_error_deg": rms_error,
}
for joint_index in range(6):
joint_no = joint_index + 1
row[f"j{joint_no}_actual_deg"] = actual_deg[joint_index]
row[f"j{joint_no}_teach_deg"] = teach_deg[joint_index]
row[f"diff_j{joint_no}_deg"] = diffs[joint_index]
rows.append(row)
return rows
def write_csv(path: Path, rows: list[dict]) -> None:
"""把分析结果落成 UTF-8 CSV,便于后续继续筛选和画图。"""
if not rows:
raise ValueError(f"No rows to write: {path}")
serializable_rows: list[dict] = []
for row in rows:
serializable_row: dict = {}
for key, value in row.items():
if isinstance(value, list):
serializable_row[key] = json.dumps(value, ensure_ascii=False)
else:
serializable_row[key] = value
serializable_rows.append(serializable_row)
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=list(serializable_rows[0].keys()))
writer.writeheader()
writer.writerows(serializable_rows)
def build_summary(records: list[dict], trigger_rows: list[dict]) -> dict:
"""汇总本次分析关心的导出状态和误差统计。"""
max_errors = [float(row["max_error_deg"]) for row in trigger_rows]
rms_errors = [float(row["rms_error_deg"]) for row in trigger_rows]
axis_counter = Counter(str(row["max_error_axis"]) for row in trigger_rows)
order_only_addr_mismatch = 0
real_addr_mismatch = 0
for row in trigger_rows:
io_addrs = list(row["io_addrs"])
config_addr = list(row["config_addr"])
if io_addrs != config_addr:
if sorted(io_addrs) == sorted(config_addr):
order_only_addr_mismatch += 1
else:
real_addr_mismatch += 1
return {
"pcap_path": str(DEFAULT_PCAP),
"all_command_count": len(records),
"trigger_count": len(trigger_rows),
"existing_runtime_actual_send_exists": (RUNTIME_DATA_DIR / "ActualSendJointTraj.txt").exists(),
"existing_runtime_actual_send_has_io_columns": False,
"existing_shot_events_exists": (RUNTIME_DATA_DIR / "ShotEvents.json").exists(),
"pcap_specific_combined_export_preexisting": False,
"average_max_error_deg": sum(max_errors) / len(max_errors),
"max_error_deg": max(max_errors),
"average_rms_error_deg": sum(rms_errors) / len(rms_errors),
"max_error_axis_counter": dict(axis_counter),
"order_only_addr_mismatch_count": order_only_addr_mismatch,
"real_addr_mismatch_count": real_addr_mismatch,
}
def main() -> None:
"""执行抓包提取、示教点对齐、CSV 导出和摘要落盘。"""
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
rows = load_j519_command_rows(DEFAULT_PCAP, DEFAULT_TSHARK)
records = decode_command_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
trigger_frames = pick_trigger_first_high_frames(records)
shot_config = load_uttc_ms11_config()
trigger_rows = build_trigger_vs_teach_rows(trigger_frames, shot_config)
summary = build_summary(records, trigger_rows)
write_csv(OUTPUT_DIR / "2026042802-1_j519_actual_send_all_with_io.csv", records)
write_csv(OUTPUT_DIR / "2026042802-1_j519_trigger_frames.csv", trigger_frames)
write_csv(OUTPUT_DIR / "2026042802-1_trigger_vs_teach_points.csv", trigger_rows)
(OUTPUT_DIR / "2026042802-1_analysis_summary.json").write_text(
json.dumps(summary, ensure_ascii=False, indent=2),
encoding="utf-8",
)
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()

View File

@@ -0,0 +1,137 @@
#!/usr/bin/env python3
"""生成 2026042802-1 抓包中触发偏移 6/7/8 周期的对照表。"""
from __future__ import annotations
import csv
import json
import math
from pathlib import Path
REPO_ROOT = Path(__file__).resolve().parents[1]
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
ACTUAL_SEND_CSV = OUTPUT_DIR / "2026042802-1_j519_actual_send_all_with_io.csv"
TRIGGER_CSV = OUTPUT_DIR / "2026042802-1_j519_trigger_frames.csv"
OUTPUT_CSV = OUTPUT_DIR / "2026042802-1_trigger_offset_6_7_8_compare.csv"
OUTPUT_JSON = OUTPUT_DIR / "2026042802-1_trigger_offset_6_7_8_summary.json"
def load_rows(path: Path) -> list[dict]:
with path.open(encoding="utf-8") as handle:
return list(csv.DictReader(handle))
def to_float_list(record: dict, prefix: str = "j") -> list[float]:
return [float(record[f"{prefix}{index}_deg"]) for index in range(1, 7)]
def compute_diff_metrics(actual_deg: list[float], teach_deg: list[float]) -> tuple[list[float], float, float, str]:
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
abs_diffs = [abs(value) for value in diffs]
max_error = max(abs_diffs)
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
max_axis = f"J{abs_diffs.index(max_error) + 1}"
return diffs, max_error, rms_error, max_axis
def main() -> None:
actual_rows = load_rows(ACTUAL_SEND_CSV)
trigger_rows = load_rows(TRIGGER_CSV)
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))["flying_shots"]["UTTC_MS11"]
trigger_waypoint_indices = [index for index, flag in enumerate(config["shot_flags"]) if flag]
actual_index_by_frame = {int(row["frame_number"]): row for row in actual_rows}
actual_order_by_frame = {int(row["frame_number"]): idx for idx, row in enumerate(actual_rows)}
output_rows: list[dict] = []
offset_win_counts = {6: 0, 7: 0, 8: 0}
for trigger_no, (trigger_row, waypoint_index) in enumerate(zip(trigger_rows, trigger_waypoint_indices), start=1):
trigger_frame = int(trigger_row["frame_number"])
trigger_order = actual_order_by_frame[trigger_frame]
teach_deg = [math.degrees(value) for value in config["traj_waypoints"][waypoint_index]]
window_start = max(0, trigger_order - 20)
window_end = min(len(actual_rows) - 1, trigger_order + 20)
best_order = trigger_order
best_rms = float("inf")
best_max = float("inf")
for candidate_order in range(window_start, window_end + 1):
candidate = actual_rows[candidate_order]
_, max_error, rms_error, _ = compute_diff_metrics(to_float_list(candidate), teach_deg)
score = (rms_error, max_error, abs(candidate_order - trigger_order))
if (best_rms, best_max, abs(best_order - trigger_order)) > score:
best_order = candidate_order
best_rms = rms_error
best_max = max_error
row = {
"trigger_no": trigger_no,
"waypoint_index": waypoint_index,
"best_sample_order": best_order,
"best_frame_number": int(actual_rows[best_order]["frame_number"]),
"best_sequence": int(actual_rows[best_order]["sequence"]),
"best_time_relative_s": float(actual_rows[best_order]["time_relative_s"]),
}
for joint_index in range(6):
row[f"teach_j{joint_index + 1}_deg"] = teach_deg[joint_index]
for offset in (6, 7, 8):
target_order = min(len(actual_rows) - 1, best_order + offset)
target = actual_rows[target_order]
actual_deg = to_float_list(target)
diffs, max_error, rms_error, max_axis = compute_diff_metrics(actual_deg, teach_deg)
row[f"offset_{offset}_frame_number"] = int(target["frame_number"])
row[f"offset_{offset}_sequence"] = int(target["sequence"])
row[f"offset_{offset}_time_relative_s"] = float(target["time_relative_s"])
row[f"offset_{offset}_max_error_axis"] = max_axis
row[f"offset_{offset}_max_error_deg"] = max_error
row[f"offset_{offset}_rms_error_deg"] = rms_error
row[f"offset_{offset}_delta_from_best_ms"] = (
float(target["time_relative_s"]) - float(actual_rows[best_order]["time_relative_s"])
) * 1000.0
for joint_index in range(6):
joint_no = joint_index + 1
row[f"offset_{offset}_j{joint_no}_actual_deg"] = actual_deg[joint_index]
row[f"offset_{offset}_diff_j{joint_no}_deg"] = diffs[joint_index]
best_offset = min(
(6, 7, 8),
key=lambda offset: (
row[f"offset_{offset}_rms_error_deg"],
row[f"offset_{offset}_max_error_deg"],
),
)
row["best_of_6_7_8_offset"] = best_offset
offset_win_counts[best_offset] += 1
output_rows.append(row)
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
with OUTPUT_CSV.open("w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=list(output_rows[0].keys()))
writer.writeheader()
writer.writerows(output_rows)
summary = {
"rows": len(output_rows),
"best_offset_win_counts": offset_win_counts,
"average_max_error_deg": {
str(offset): sum(row[f"offset_{offset}_max_error_deg"] for row in output_rows) / len(output_rows)
for offset in (6, 7, 8)
},
"average_rms_error_deg": {
str(offset): sum(row[f"offset_{offset}_rms_error_deg"] for row in output_rows) / len(output_rows)
for offset in (6, 7, 8)
},
}
OUTPUT_JSON.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()

View File

@@ -0,0 +1,200 @@
#!/usr/bin/env python3
"""读取 FANUC J519 允许速度/加速度/jerk 上限表。
本脚本只做 UDP packet type=3 查询,不启动运行时服务,也不写入机器人参数。
响应中的 limit 值保留为控制器原始单位:速度 deg/s、加速度 deg/s^2、jerk deg/s^3。
"""
from __future__ import annotations
import argparse
import csv
import socket
import struct
import sys
from dataclasses import dataclass
from pathlib import Path
LIMIT_TYPES = {
0: ("velocity", "deg/s"),
1: ("acceleration", "deg/s^2"),
2: ("jerk", "deg/s^3"),
}
@dataclass(frozen=True)
class LimitTable:
"""保存单个轴、单种限制类型的 20 档 no-payload / max-payload 上限表。"""
axis: int
limit_type: int
vmax_mm_s: int
intermediate_check_s: int
no_payload: tuple[float, ...]
max_payload: tuple[float, ...]
def pack_request(axis: int, limit_type: int) -> bytes:
"""按手册 Table 5 封装允许上限表查询请求。"""
return struct.pack(">IIII", 3, 1, axis, limit_type)
def parse_response(packet: bytes, expected_axis: int, expected_type: int) -> LimitTable:
"""按手册 Table 6 解析允许上限表响应。"""
if len(packet) != 184:
raise ValueError(f"响应长度应为 184 字节,实际为 {len(packet)} 字节。")
packet_type, version, axis, limit_type, vmax, check_time = struct.unpack_from(">IIIIII", packet, 0)
if packet_type != 3 or version != 1:
raise ValueError(f"响应头不正确:packet_type={packet_type}, version={version}。")
if axis != expected_axis or limit_type != expected_type:
raise ValueError(
f"响应与请求不匹配:期望 axis={expected_axis}, type={expected_type},"
f"实际 axis={axis}, type={limit_type}。"
)
values = struct.unpack_from(">40f", packet, 24)
return LimitTable(
axis=axis,
limit_type=limit_type,
vmax_mm_s=vmax,
intermediate_check_s=check_time,
no_payload=tuple(values[:20]),
max_payload=tuple(values[20:]),
)
def query_table(sock: socket.socket, robot_ip: str, port: int, axis: int, limit_type: int) -> LimitTable:
"""发送单个查询请求并等待对应响应。"""
sock.sendto(pack_request(axis, limit_type), (robot_ip, port))
packet, address = sock.recvfrom(2048)
if address[0] != robot_ip:
raise ValueError(f"收到非目标机器人响应:{address[0]}:{address[1]}。")
return parse_response(packet, axis, limit_type)
def iter_requested_tables(
robot_ip: str,
port: int,
timeout_s: float,
axes: range,
limit_types: tuple[int, ...],
) -> list[LimitTable]:
"""按轴和限制类型顺序读取所有上限表。"""
tables: list[LimitTable] = []
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
sock.settimeout(timeout_s)
for axis in axes:
for limit_type in limit_types:
tables.append(query_table(sock, robot_ip, port, axis, limit_type))
return tables
def write_csv(path: Path, tables: list[LimitTable]) -> None:
"""把 20 档速度区间和两套 payload 表写成扁平 CSV。"""
with path.open("w", newline="", encoding="utf-8") as file:
writer = csv.writer(file)
writer.writerow(
[
"axis",
"limit_type",
"unit",
"vmax_mm_s",
"intermediate_check_s",
"speed_bin_index",
"speed_bin_upper_mm_s",
"no_payload_limit",
"max_payload_limit",
]
)
for table in tables:
limit_name, unit = LIMIT_TYPES[table.limit_type]
for index, (no_payload, max_payload) in enumerate(zip(table.no_payload, table.max_payload), start=1):
writer.writerow(
[
table.axis,
limit_name,
unit,
table.vmax_mm_s,
table.intermediate_check_s,
index,
table.vmax_mm_s * index / 20.0,
f"{no_payload:.9g}",
f"{max_payload:.9g}",
]
)
def print_summary(tables: list[LimitTable]) -> None:
"""打印每张表的首末档,便于现场快速确认返回值是否正常。"""
for table in tables:
limit_name, unit = LIMIT_TYPES[table.limit_type]
print(
f"axis={table.axis} type={limit_name:<12} unit={unit:<8} "
f"vmax={table.vmax_mm_s}mm/s check={table.intermediate_check_s}s "
f"no_payload[1]={table.no_payload[0]:.6g} no_payload[20]={table.no_payload[-1]:.6g} "
f"max_payload[1]={table.max_payload[0]:.6g} max_payload[20]={table.max_payload[-1]:.6g}"
)
def parse_args() -> argparse.Namespace:
"""解析命令行参数。"""
parser = argparse.ArgumentParser(
description="Read FANUC J519 packet type=3 allowable upper limit tables.",
)
parser.add_argument("robot_ip", help="机器人控制柜 IP,例如 192.168.10.11。")
parser.add_argument("--port", type=int, default=60015, help="J519 UDP 端口,默认 60015。")
parser.add_argument("--timeout", type=float, default=2.0, help="单次响应超时秒数,默认 2.0。")
parser.add_argument("--axis-start", type=int, default=1, help="起始轴号,默认 1。")
parser.add_argument("--axis-end", type=int, default=9, help="结束轴号,默认 9。")
parser.add_argument(
"--types",
default="0,1,2",
help="限制类型列表:0=velocity, 1=acceleration, 2=jerk;默认 0,1,2。",
)
parser.add_argument(
"--csv",
type=Path,
default=Path("analysis/fanuc_allowable_limit_tables.csv"),
help="CSV 输出路径。",
)
return parser.parse_args()
def main() -> int:
"""脚本入口。"""
args = parse_args()
limit_types = tuple(int(item.strip()) for item in args.types.split(",") if item.strip())
unknown_types = [item for item in limit_types if item not in LIMIT_TYPES]
if unknown_types:
print(f"未知限制类型:{unknown_types}", file=sys.stderr)
return 2
if not 1 <= args.axis_start <= args.axis_end <= 9:
print("轴范围必须满足 1 <= axis-start <= axis-end <= 9。", file=sys.stderr)
return 2
tables = iter_requested_tables(
args.robot_ip,
args.port,
args.timeout,
range(args.axis_start, args.axis_end + 1),
limit_types,
)
args.csv.parent.mkdir(parents=True, exist_ok=True)
write_csv(args.csv, tables)
print_summary(tables)
print(f"\nCSV 已写入:{args.csv}")
return 0
if __name__ == "__main__":
raise SystemExit(main())

View File

@@ -0,0 +1,76 @@
<#
.SYNOPSIS
构建 Rust ICSP 原生规划库并复制到 src/Libs 与可选目标目录。
.DESCRIPTION
在 Windows x64 上通过 cargo release 构建 flyshot_planning,默认写入
src/Libs/flyshot_planning.dll;Linux x64 写入 src/Libs/libflyshot_planning.so。
可通过 -DestinationDirectories 额外复制到测试或发布输出目录。
.PARAMETER Configuration
传递给 dotnet 相关路径的构建配置名,默认 Release。
.PARAMETER DestinationDirectories
除 src/Libs 外需要覆盖复制的额外目标目录列表。
#>
[CmdletBinding()]
param(
[ValidateSet('Debug', 'Release')]
[string]$Configuration = 'Release',
[string[]]$DestinationDirectories = @()
)
$ErrorActionPreference = 'Stop'
Set-StrictMode -Version Latest
$repositoryRoot = (Resolve-Path (Join-Path $PSScriptRoot '..')).Path
$cargoManifest = Join-Path $repositoryRoot 'native\flyshot-native-planning\Cargo.toml'
$libsDirectory = Join-Path $repositoryRoot 'src\Libs'
$cargoProfile = if ($Configuration -eq 'Debug') { 'debug' } else { 'release' }
$targetDirectory = Join-Path $repositoryRoot "native\flyshot-native-planning\target\$cargoProfile"
if ($IsLinux -or (-not $IsWindows -and -not $IsMacOS))
{
$artifactFileName = 'libflyshot_planning.so'
}
else
{
$artifactFileName = 'flyshot_planning.dll'
}
$builtArtifactPath = Join-Path $targetDirectory $artifactFileName
$libsArtifactPath = Join-Path $libsDirectory $artifactFileName
Write-Host "正在构建 Rust 原生规划库 ($Configuration) ..."
& cargo build --manifest-path $cargoManifest --$Configuration
if ($LASTEXITCODE -ne 0)
{
throw "cargo build 失败,退出码:$LASTEXITCODE"
}
if (-not (Test-Path -LiteralPath $builtArtifactPath -PathType Leaf))
{
throw "未找到构建产物:$builtArtifactPath"
}
New-Item -ItemType Directory -Path $libsDirectory -Force | Out-Null
Copy-Item -LiteralPath $builtArtifactPath -Destination $libsArtifactPath -Force
Write-Host "已复制到:$libsArtifactPath"
$defaultTestOutput = Join-Path $repositoryRoot "tests\Flyshot.Core.Tests\bin\$Configuration\net8.0"
$allDestinations = @($DestinationDirectories) + @($defaultTestOutput)
foreach ($destination in $allDestinations | Where-Object { -not [string]::IsNullOrWhiteSpace($_) } | Select-Object -Unique)
{
$absoluteDestination = [System.IO.Path]::GetFullPath($destination)
if (-not (Test-Path -LiteralPath $absoluteDestination -PathType Container))
{
Write-Host "跳过不存在的目标目录:$absoluteDestination"
continue
}
Copy-Item -LiteralPath $builtArtifactPath -Destination (Join-Path $absoluteDestination $artifactFileName) -Force
Write-Host "已复制到:$absoluteDestination\$artifactFileName"
}

BIN
docs/0.7x.pcap Normal file

Binary file not shown.

View File

@@ -0,0 +1,526 @@
# 2026042802-1 抓包触发点与示教点对比记录
## 1. 目的
- 使用 `../Rvbust/uttc-20260428/2026042802-1.pcap` 确认当前仓库是否已经导出过这份抓包对应的真实运动轨迹发包记录。
- 如果现有导出不包含 IO 触发信息,则补一份“真实 J519 发包 + IO 标记”导出。
- 将抓包里真实触发 IO 的关节坐标,与 `Config/RobotConfig.json` 中 `flying_shots.UTTC_MS11` 的示教点逐点对比。
- 把结论固定成仓库文档,后续继续和新程序抓包、`ShotEvents.json`、运行时导出结果做同口径比较。
## 2. 数据来源与口径
- 抓包文件:`../Rvbust/uttc-20260428/2026042802-1.pcap`
- 旧目录配置文件:`../Rvbust/uttc-20260428/RobotConfig.json`
- 新版程序运行时配置文件:`Config/RobotConfig.json`
- 对比对象:两份配置中的 `flying_shots.UTTC_MS11`
- 协议口径:只看 `192.168.10.10 -> 192.168.10.11` 的 `UDP 60015` 64B J519 命令帧
- 触发判定:`write_io_value > 0` 视为 IO 置位
- 去重规则:由于 `io_keep_cycles=2`,每次触发只记录首个高电平帧
- 关节单位:抓包 J519 目标关节为 `deg`;`RobotConfig.json` `traj_waypoints` 为 `rad`,分析时先转成 `deg`
补充说明:
- `../Rvbust/uttc-20260428/` 目录中的内容代表旧抓包及其同目录资料。
- `Config/Data/UTTC_MS11/` 目录中的内容代表现在新版程序的运行时导出。
- 本文不会把两者混成同一次运行的数据;这里只拿新版程序目录里的文件做“现状参照”,核心分析对象仍然是旧抓包 `2026042802-1.pcap`。
## 3. 现状核对
### 3.1 仓库里已经存在什么
- 旧抓包目录已有原始抓包:`../Rvbust/uttc-20260428/2026042802-1.pcap`
- 旧抓包目录已有按运动段拆分的导出:`../Rvbust/uttc-20260428/2026042802-1_joint_segments/`
- 旧抓包目录已有速度档位整理结果:`../Rvbust/uttc-20260428/1倍速度 角度坐标点/真实轨迹JointDetialTraj.txt`
- 新版程序目录已有运行时导出:`Config/Data/UTTC_MS11/ActualSendJointTraj.txt`
- 新版程序目录已有运行时导出:`Config/Data/UTTC_MS11/ActualSendTiming.txt`
- 新版程序目录已有理论触发时间轴:`Config/Data/UTTC_MS11/ShotEvents.json`
### 3.2 当前缺的是什么
- 新版程序目录中的 `ActualSendJointTraj.txt` 是现在新版程序落盘的真实发送关节轨迹,但不带 IO 列,也不属于 `2026042802-1.pcap` 这次旧抓包本身。
- 旧抓包目录中的 `2026042802-1_joint_segments/` 是按运动段拆开的 `JointDetialTraj.txt` 风格导出,也不包含整条抓包的 IO 标记。
- 旧抓包目录中的 `1倍速度 角度坐标点/真实轨迹JointDetialTraj.txt` 提供了真实轨迹文本,但同样不带 IO 触发列。
- 本次分析前,没有发现一份专门对应 `2026042802-1.pcap` 的“整条真实 J519 发包记录 + IO 标记 + 触发帧筛选”组合导出。
结论:
旧抓包目录和新版程序目录里都已经有各自的轨迹资料,但在本次分析前,还没有一份专门对应 `2026042802-1.pcap` 的、带 IO 触发信息的整条实际发包记录。
## 4. 本次新增导出
本次补充生成了以下文件:
- `analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv`
- `1788` 条客户端真实下发 J519 命令
- 包含 `frame_number / time_relative_s / sequence / write_io_value / io_addrs / j1..j6`
- `analysis/2026042802-1/2026042802-1_j519_trigger_frames.csv`
- 只保留每次 IO 高电平脉冲的第一帧
- 共 `17` 条,和 `ShotEvents.json` 的触发次数一致
- `analysis/2026042802-1/2026042802-1_trigger_vs_teach_points.csv`
- 每个真实触发帧与 `UTTC_MS11` 示教点的逐点偏差表
- `analysis/2026042802-1/2026042802-1_analysis_summary.json`
- 本文档引用的汇总统计
对应复现脚本:
- `analysis/analyze_2026042802_1_trigger_vs_teach_points.py`
## 5. 触发与地址核对
- 抓包提取到的真实触发次数:`17`
- `ShotEvents.json` 中的触发次数:`17`
- `write_io_value` 解码出的 `io_addrs` 与 `UTTC_MS11.addr` 没有真实内容不一致
- 其中有 `14` 处列表顺序不同,例如抓包里是 `[2, 3, 4]`,配置里写的是 `[3, 4, 2]`
- 这 `14` 处只是顺序差异,不是触发地址集合差异
- 真正的地址集合不匹配数:`0`
结论:
`2026042802-1.pcap` 中真实发出的 IO 地址集合,与 `UTTC_MS11` 配置的 `addr` 集合是一致的。
## 6. 触发点与示教点误差统计
- 平均单点最大单轴误差:`4.241584 deg`
- 最大单轴误差:`11.130744 deg`
- 平均 RMS 误差:`2.540614 deg`
- 最大误差轴分布:`J6=9`、`J4=5`、`J5=1`、`J2=1`、`J1=1`
这说明:
- 这份旧抓包里的真实触发点,并不是严格贴在配置示教点上触发
- 误差主要集中在 `J6` 和 `J4`
- 由于 `../Rvbust/uttc-20260428/RobotConfig.json` 和当前仓库 `Config/RobotConfig.json` 的 `UTTC_MS11` 四组关键字段完全一致,因此这里的“示教点”对旧抓包和当前配置是同一组数据
- 如果用户的“对焦准确示教点”就是这组 `traj_waypoints`,那么旧程序实际触发时已经存在几度量级的偏移,而不是亚角度级别的完全重合
## 7. 最大偏差点
按最大单轴误差从大到小列出前 5 个触发点:
| 触发序号 | waypoint_index | 实际 io_addrs | 最大误差轴 | 最大误差(deg) | RMS误差(deg) | 主要差值 |
| --- | --- | --- | --- | ---: | ---: | --- |
| 3 | 3 | `[2, 3, 4]` | `J6` | `11.130744` | `5.955608` | `J4 +8.811537`,`J6 -11.130744` |
| 7 | 7 | `[3, 4]` | `J6` | `7.194107` | `4.618545` | `J2 -5.441249`,`J4 -6.792278`,`J6 +7.194107` |
| 17 | 18 | `[3, 4]` | `J4` | `6.848522` | `4.036140` | `J2 +1.835940`,`J4 -6.848522`,`J6 +6.320424` |
| 13 | 13 | `[3, 4]` | `J1` | `6.656209` | `4.109282` | `J1 +6.656209`,`J3 -3.848692`,`J4 +3.795032` |
| 8 | 8 | `[2, 4]` | `J6` | `5.939884` | `3.533133` | `J2 -2.961069`,`J4 -5.456660`,`J6 +5.939884` |
## 8. 逐点结果
完整逐点数据已写入:
- `analysis/2026042802-1/2026042802-1_trigger_vs_teach_points.csv`
这里保留简表,方便直接看每次触发的最大偏差:
| 触发序号 | waypoint_index | frame | seq | time(s) | io_addrs | 最大误差轴 | 最大误差(deg) | RMS误差(deg) |
| --- | --- | ---: | ---: | ---: | --- | --- | ---: | ---: |
| 1 | 1 | 1955 | 1381124 | 5.888271 | `[2, 4]` | `J6` | `3.096909` | `2.341857` |
| 2 | 2 | 2151 | 1381198 | 6.480252 | `[2, 3, 4]` | `J4` | `2.737488` | `1.612807` |
| 3 | 3 | 2223 | 1381226 | 6.704225 | `[2, 3, 4]` | `J6` | `11.130744` | `5.955608` |
| 4 | 4 | 2336 | 1381269 | 7.048266 | `[2, 4]` | `J6` | `2.373383` | `1.439206` |
| 5 | 5 | 2477 | 1381321 | 7.464289 | `[2, 4]` | `J5` | `1.973330` | `1.273254` |
| 6 | 6 | 2665 | 1381391 | 8.024277 | `[3, 4]` | `J2` | `1.265078` | `0.786940` |
| 7 | 7 | 2811 | 1381447 | 8.472317 | `[3, 4]` | `J6` | `7.194107` | `4.618545` |
| 8 | 8 | 2908 | 1381483 | 8.760361 | `[2, 4]` | `J6` | `5.939884` | `3.533133` |
| 9 | 9 | 3112 | 1381561 | 9.384332 | `[2, 4]` | `J4` | `2.414749` | `1.565121` |
| 10 | 10 | 3170 | 1381583 | 9.560358 | `[2, 4]` | `J4` | `3.629631` | `2.268783` |
| 11 | 11 | 3350 | 1381651 | 10.104322 | `[2, 4]` | `J6` | `3.113037` | `2.115595` |
| 12 | 12 | 3470 | 1381697 | 10.472333 | `[2, 4]` | `J6` | `4.018547` | `2.003201` |
| 13 | 13 | 3627 | 1381756 | 10.944371 | `[3, 4]` | `J1` | `6.656209` | `4.109282` |
| 14 | 14 | 3710 | 1381787 | 11.192339 | `[2, 4]` | `J6` | `2.834801` | `1.514764` |
| 15 | 15 | 3796 | 1381820 | 11.456361 | `[2, 4]` | `J4` | `2.496544` | `1.640818` |
| 16 | 16 | 3870 | 1381848 | 11.680362 | `[2, 4]` | `J6` | `3.782349` | `2.176883` |
| 17 | 18 | 4138 | 1381950 | 12.496365 | `[3, 4]` | `J4` | `6.848522` | `4.036140` |
## 9. UTTC_MS11 示教点角度表
为了便于和导出的触发序列直接手工比对,下面把当前 `UTTC_MS11` 示教点从 `rad` 转成 `deg`。
说明:
- `waypoint_index` 使用配置里的原始下标。
- `shot_flag=true` 表示该点在配置里是拍照点候选。
- `addr` 保留原配置里的触发地址列表,便于和 `io_addrs` 对照。
| waypoint_index | shot_flag | addr | J1(deg) | J2(deg) | J3(deg) | J4(deg) | J5(deg) | J6(deg) |
| --- | --- | --- | ---: | ---: | ---: | ---: | ---: | ---: |
| 0 | `false` | `[]` | 60.546226 | 0.668344 | -1.025155 | -0.869105 | 1.231405 | 0.548197 |
| 1 | `true` | `[2, 4]` | 48.886810 | 2.198985 | -11.021017 | 0.410210 | 6.248381 | 2.294991 |
| 2 | `true` | `[3, 4, 2]` | 55.347755 | 11.807040 | -7.009095 | -71.014331 | 6.012065 | 74.249532 |
| 3 | `true` | `[3, 4, 2]` | 55.109808 | 8.759497 | -8.518217 | -41.117250 | 10.108488 | 41.956933 |
| 4 | `true` | `[4, 2]` | 43.653593 | -1.629660 | -17.715754 | 5.995209 | 32.171718 | -22.573973 |
| 5 | `true` | `[4, 2]` | 64.582445 | 4.262979 | -15.669217 | -29.952927 | 29.850440 | 45.626527 |
| 6 | `true` | `[3, 4]` | 60.479483 | 23.068782 | -5.011264 | 36.106855 | 5.525681 | -31.302564 |
| 7 | `true` | `[3, 4]` | 70.475837 | 16.393849 | -13.456948 | -27.892319 | 14.535662 | 31.711933 |
| 8 | `true` | `[4, 2]` | 69.582464 | -17.105713 | -8.710590 | -58.475694 | 7.630590 | 64.437733 |
| 9 | `true` | `[4, 2]` | 73.571259 | -6.429845 | -9.628580 | -128.808647 | 14.671083 | 140.002048 |
| 10 | `true` | `[4, 2]` | 75.569386 | -14.679306 | -7.294156 | -130.923033 | 17.688361 | 141.893508 |
| 11 | `true` | `[4, 2]` | 86.093498 | -14.498333 | -13.681511 | -69.868296 | 26.742682 | 88.999414 |
| 12 | `true` | `[4, 2]` | 61.720733 | -4.232789 | -9.784423 | -108.384083 | 22.160772 | 118.142064 |
| 13 | `true` | `[4, 3]` | 79.757083 | 4.640231 | -15.311186 | -56.035312 | 26.133138 | 52.152257 |
| 14 | `true` | `[4, 2]` | 108.955551 | 1.370760 | -33.594425 | -48.367361 | 43.963404 | 85.039261 |
| 15 | `true` | `[4, 2]` | 110.584848 | -3.860900 | -32.396407 | -51.112591 | 44.229160 | 87.624000 |
| 16 | `true` | `[4, 2]` | 118.095952 | -17.376387 | -31.069001 | -59.560538 | 48.732576 | 94.134771 |
| 17 | `false` | `[4, 2]` | 62.573787 | -22.938069 | -10.333288 | 77.491373 | 35.583412 | -69.668648 |
| 18 | `true` | `[4, 3]` | 60.282482 | -22.938081 | -10.333248 | 77.491642 | 35.583378 | -69.668981 |
| 19 | `false` | `[]` | 60.546226 | 0.668344 | -1.025155 | -0.869105 | 1.231405 | 0.548197 |
## 10. 结论
1. `../Rvbust/uttc-20260428/` 目录里已有旧抓包相关轨迹资料,`Config/Data/UTTC_MS11/` 目录里已有新版程序运行时导出,但在本次分析前,没有一份专门对应 `2026042802-1.pcap` 的“整条真实 J519 发包记录 + IO 标记”导出。
2. 本次已经补齐该导出,文件位于 `analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv`。
3. `2026042802-1.pcap` 的真实 IO 触发次数是 `17`,与 `ShotEvents.json` 一致。
4. 抓包中真实触发时使用的 IO 地址集合,与 `UTTC_MS11.addr` 一致;只有列表顺序差异,没有地址集合差异。
5. 抓包中真实触发点相对示教点的平均最大单轴误差约 `4.24 deg`,最大达到 `11.13 deg`,主要偏在 `J6` 和 `J4`。
6. 因此,如果后续要分析“为什么新程序拍照位置偏了”,不能把 `2026042802-1.pcap` 视为“与当前示教点完全重合的零误差基准”;它本身相对当前配置示教点就存在明显偏差。
## 11. 触发点前 6 个周期的实际关节坐标与示教点差值
在这份旧抓包里,17 个真实触发点对应的“最接近示教点”的实际采样点,统一出现在触发前 `6` 个周期,也就是前 `48ms`。
为了方便手工比对,这里把每个触发点前 `6` 个周期的实际关节坐标单独列出来,并和对应示教点逐轴做差。
说明:
- `pre6_frame` / `pre6_time` 是触发帧前 `6` 个 `8ms` 周期的实际 J519 发包点。
- `pre6_j*` 是该时刻的实际关节角度,单位 `deg`。
- `diff_j*` 定义为 `pre6_actual - teach_point`,单位 `deg`。
- 这张表更适合你直接和导出的触发序列逐点人工核对。
| 触发序号 | waypoint_index | pre6_frame | pre6_time(s) | pre6_J1 | pre6_J2 | pre6_J3 | pre6_J4 | pre6_J5 | pre6_J6 | diff_J1 | diff_J2 | diff_J3 | diff_J4 | diff_J5 | diff_J6 | max_abs_diff(deg) | rms_diff(deg) |
| --- | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
| 1 | 1 | 1941 | 5.840207 | 49.036247 | 2.169637 | -10.893022 | 0.469577 | 6.183248 | 2.196172 | +0.149437 | -0.029348 | +0.127996 | +0.059368 | -0.065133 | -0.098818 | 0.149437 | 0.097560 |
| 2 | 2 | 2135 | 6.432175 | 55.402473 | 11.806953 | -6.980721 | -70.955170 | 6.007696 | 74.193436 | +0.054718 | -0.000086 | +0.028375 | +0.059162 | -0.004369 | -0.056096 | 0.059162 | 0.041764 |
| 3 | 3 | 2207 | 6.656264 | 55.185375 | 8.828770 | -8.461165 | -41.609406 | 9.991771 | 42.555542 | +0.075567 | +0.069272 | +0.057051 | -0.492155 | -0.116717 | +0.598609 | 0.598609 | 0.323506 |
| 4 | 4 | 2321 | 7.000291 | 43.639214 | -1.593877 | -17.673382 | 5.988685 | 32.060623 | -22.587208 | -0.014380 | +0.035782 | +0.042372 | -0.006524 | -0.111095 | -0.013234 | 0.111095 | 0.051385 |
| 5 | 5 | 2457 | 7.416267 | 64.456329 | 4.153820 | -15.749991 | -29.951208 | 30.008862 | 45.422455 | -0.126115 | -0.109159 | -0.080774 | +0.001719 | +0.158422 | -0.204073 | 0.204073 | 0.129802 |
| 6 | 6 | 2649 | 7.976283 | 60.479656 | 23.070845 | -5.011842 | 36.106964 | 5.525600 | -31.304708 | +0.000174 | +0.002063 | -0.000578 | +0.000109 | -0.000081 | -0.002145 | 0.002145 | 0.001241 |
| 7 | 7 | 2795 | 8.424315 | 70.489815 | 16.270372 | -13.455009 | -28.079985 | 14.531018 | 31.910471 | +0.013978 | -0.123476 | +0.001939 | -0.187666 | -0.004644 | +0.198538 | 0.198538 | 0.122545 |
| 8 | 8 | 2892 | 8.712310 | 69.566750 | -17.333443 | -8.667440 | -58.787853 | 7.566183 | 64.777092 | -0.015715 | -0.227729 | +0.043149 | -0.312159 | -0.064407 | +0.339359 | 0.339359 | 0.212417 |
| 9 | 9 | 3096 | 9.336375 | 73.577042 | -6.447144 | -9.620103 | -128.858627 | 14.686206 | 140.051056 | +0.005783 | -0.017298 | +0.008477 | -0.049980 | +0.015123 | +0.049008 | 0.049980 | 0.030367 |
| 10 | 10 | 3154 | 9.512325 | 75.498016 | -14.523483 | -7.317341 | -131.103271 | 17.629053 | 142.061157 | -0.071370 | +0.155823 | -0.023185 | -0.180238 | -0.059308 | +0.167649 | 0.180238 | 0.125181 |
| 11 | 11 | 3334 | 10.056378 | 86.017281 | -14.446944 | -13.689492 | -69.926414 | 26.744770 | 89.069130 | -0.076217 | +0.051389 | -0.007981 | -0.058119 | +0.002088 | +0.069716 | 0.076217 | 0.052846 |
| 12 | 12 | 3455 | 10.424364 | 61.554295 | -4.126511 | -9.723098 | -108.476807 | 22.091070 | 117.986275 | -0.166438 | +0.106278 | +0.061325 | -0.092724 | -0.069702 | -0.155789 | 0.166438 | 0.115819 |
| 13 | 13 | 3611 | 10.896324 | 80.232681 | 4.624288 | -15.574081 | -55.685375 | 26.385595 | 52.279755 | +0.475598 | -0.015943 | -0.262896 | +0.349937 | +0.252457 | +0.127497 | 0.475598 | 0.288100 |
| 14 | 14 | 3694 | 11.144347 | 108.790268 | 1.405766 | -33.485344 | -48.378551 | 43.857502 | 84.771103 | -0.165283 | +0.035006 | +0.109081 | -0.011190 | -0.105902 | -0.268158 | 0.268158 | 0.143580 |
| 15 | 15 | 3780 | 11.408361 | 110.543327 | -3.739877 | -32.432842 | -50.983562 | 44.216587 | 87.566269 | -0.041521 | +0.121023 | -0.036435 | +0.129028 | -0.012573 | -0.057731 | 0.129028 | 0.079412 |
| 16 | 16 | 3854 | 11.632360 | 118.081940 | -17.294281 | -31.087101 | -59.614193 | 48.711964 | 94.208046 | -0.014013 | +0.082106 | -0.018100 | -0.053655 | -0.020612 | +0.073275 | 0.082106 | 0.051540 |
| 17 | 18 | 4121 | 12.448375 | 60.258179 | -22.880474 | -10.315624 | 77.249168 | 35.501560 | -69.439224 | -0.024304 | +0.057606 | +0.017624 | -0.242474 | -0.081817 | +0.229756 | 0.242474 | 0.142884 |
这张表说明:
- 如果把旧程序的真实 IO 触发点整体前移 `6` 个周期,再去看对应关节坐标,它们和示教点已经非常接近。
- 17 个点里,大多数前 `6` 周期采样点与示教点的最大单轴误差都在 `0.05 ~ 0.34 deg`。
- 误差较大的两个点是:
- `trigger 3`:`max_abs_diff = 0.598609 deg`
- `trigger 13`:`max_abs_diff = 0.475598 deg`
- 这进一步支持一个很强的现象:旧抓包里的真实 IO 触发时刻,相对“最接近示教点”的实际运动位置,整体滞后约 `48ms`。
## 12. 复现命令
在仓库根目录执行:
```powershell
python analysis/analyze_2026042802_1_trigger_vs_teach_points.py
python analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py
```
脚本默认读取:
- `../Rvbust/uttc-20260428/2026042802-1.pcap`
- `Config/RobotConfig.json`
- `D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe`
如果后续 Wireshark 路径变化,需要先同步修改脚本中的 `DEFAULT_TSHARK`。
## 13. 60015 状态反馈视角复核
上面第 11 节的“前 6 个周期最接近示教点”,只是在看:
- `192.168.10.10 -> 192.168.10.11` 的 64B J519 下发命令序列
它说明的是“命令目标流”和示教点之间的相对时序,不能直接等价成“机械臂实时反馈已经过了示教点才触发”。
针对你补充的 `buffer_size` 线索,这里进一步直接解析了:
- `192.168.10.11 -> 192.168.10.10` 的 132B `UDP 60015` 状态包
- 解码口径与仓库运行时代码 `src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs` 一致,全部按大端解析
- 关节反馈字段使用状态包里的 `0x3C ~ 0x57` 六轴 `float32 deg`
本次新增文件:
- `analysis/2026042802-1/2026042802-1_j519_status_feedback_all.csv`
- `analysis/2026042802-1/2026042802-1_trigger_status_feedback_vs_teach_points.csv`
- `analysis/2026042802-1/2026042802-1_trigger_manual_compare.csv`
- `analysis/2026042802-1/2026042802-1_status_feedback_summary.json`
- `analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py`
### 13.1 序列号关系先确认
整条抓包里共提取到:
- 状态包 `1789` 条
- 命令包 `1788` 条
对每一条命令帧,取它前一个状态帧做对齐,结果是:
- `命令 sequence - 前一条状态 sequence = 8`
- 统计结果:`1788 / 1788` 全部成立
这说明在这份旧抓包里,`buffer_size=8` 不是偶发现象,而是整条 J519 流都稳定成立。
换句话说:
- 触发命令包上的 `sequence=1381124`
- 它对应“当前实时反馈”的状态包 `sequence=1381116`
- 中间固定隔着 `8` 个状态周期,也就是约 `64ms`
### 13.2 触发当下的实时反馈,并不在示教点附近
如果直接取“每个触发命令帧对应的当前状态包”,也就是:
- `paired_status_sequence = trigger_sequence - 8`
那么 17 个点相对示教点的误差反而更大:
- 平均最大单轴误差:`6.469219 deg`
- 最大单轴误差:`16.123934 deg`
- 最大误差轴分布:`J6=9`、`J2=4`、`J1=2`、`J4=2`
这说明:
- 机械臂在触发命令真正下发的那个时刻,对应的实时反馈位置,大多数情况下还没有跑到示教点附近
- 所以“最接近示教点都出现在触发前 6 个周期”这个现象,不能理解成“机器人已经过了示教点才开始触发”
- 那只是命令流相对示教点的时序关系;实时反馈要再往后看
### 13.3 实时反馈最接近示教点,出现在触发后 9 到 10 个状态周期
对每个触发点,以 `paired_status_sequence = trigger_sequence - 8` 为起点,在前后 `±20` 个状态周期里搜索“与示教点最接近”的状态反馈帧,结果统一得到:
- 最佳点相对 `paired_status` 晚 `9` 或 `10` 个状态周期
- 统计分布:`9 周期 = 9` 次,`10 周期 = 8` 次
折算成触发命令时刻:
- 最佳点相对触发命令晚 `1` 或 `2` 个状态周期
- 统计分布:`+1 周期 = 9` 次,`+2 周期 = 8` 次
- 对应抓包时间差约 `71.838 ~ 79.964 ms`
- 平均约 `75.661 ms`
这里看起来像是 `1~2` 周期,但要注意基准不同:
- 触发命令本身领先当前状态 `8` 个周期
- 所以“触发后约 `72~80ms` 才最接近示教点”与“状态流从当前反馈再往后走 `9~10` 个周期才到位”本质上是同一件事
### 13.4 实时反馈视角的逐点结果
| 触发序号 | waypoint_index | trigger_frame | trigger_seq | paired_status_frame | paired_status_seq | 触发当下最大误差轴 | 触发当下最大误差(deg) | 触发当下RMS(deg) | 最佳状态seq | 最佳状态相对当前反馈晚几周期 | 最佳状态相对触发时间(ms) | 最佳状态最大误差(deg) | 最佳状态RMS(deg) |
| --- | --- | ---: | ---: | ---: | ---: | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
| 1 | 1 | 1955 | 1381124 | 1954 | 1381116 | J1 | 6.588242 | 3.822544 | 1381126 | 10 | 79.853 | 0.207590 | 0.122397 |
| 2 | 2 | 2151 | 1381198 | 2150 | 1381190 | J1 | 2.335315 | 1.382754 | 1381199 | 9 | 71.912 | 0.082134 | 0.046161 |
| 3 | 3 | 2223 | 1381226 | 2222 | 1381218 | J6 | 16.123934 | 8.852377 | 1381228 | 10 | 79.929 | 0.569692 | 0.323964 |
| 4 | 4 | 2336 | 1381269 | 2335 | 1381261 | J6 | 4.901883 | 3.244056 | 1381271 | 10 | 79.909 | 0.094704 | 0.065805 |
| 5 | 5 | 2477 | 1381321 | 2476 | 1381313 | J6 | 9.219438 | 4.621321 | 1381323 | 10 | 79.833 | 0.056622 | 0.033575 |
| 6 | 6 | 2665 | 1381391 | 2664 | 1381383 | J6 | 6.146461 | 3.047841 | 1381392 | 9 | 71.904 | 0.176974 | 0.085631 |
| 7 | 7 | 2811 | 1381447 | 2810 | 1381439 | J6 | 13.151718 | 7.782024 | 1381448 | 9 | 71.873 | 0.306587 | 0.182107 |
| 8 | 8 | 2908 | 1381483 | 2907 | 1381475 | J6 | 7.436279 | 5.127041 | 1381484 | 9 | 71.838 | 0.060748 | 0.027507 |
| 9 | 9 | 3112 | 1381561 | 3111 | 1381553 | J6 | 5.585605 | 3.277206 | 1381562 | 9 | 71.873 | 0.152260 | 0.089150 |
| 10 | 10 | 3170 | 1381583 | 3169 | 1381575 | J2 | 4.165077 | 2.128984 | 1381585 | 10 | 79.856 | 0.073440 | 0.043286 |
| 11 | 11 | 3350 | 1381651 | 3349 | 1381643 | J2 | 1.830953 | 1.169340 | 1381652 | 9 | 71.898 | 0.077668 | 0.042465 |
| 12 | 12 | 3470 | 1381697 | 3469 | 1381689 | J4 | 4.822353 | 2.861125 | 1381698 | 9 | 71.950 | 0.021077 | 0.014936 |
| 13 | 13 | 3627 | 1381756 | 3626 | 1381748 | J4 | 8.152508 | 5.168659 | 1381757 | 9 | 71.855 | 0.145543 | 0.069960 |
| 14 | 14 | 3710 | 1381787 | 3709 | 1381779 | J6 | 10.404419 | 5.679811 | 1381789 | 10 | 79.922 | 0.119453 | 0.058360 |
| 15 | 15 | 3796 | 1381820 | 3795 | 1381812 | J2 | 2.616108 | 1.591175 | 1381822 | 10 | 79.926 | 0.149803 | 0.087169 |
| 16 | 16 | 3870 | 1381848 | 3869 | 1381840 | J2 | 4.682474 | 2.206740 | 1381850 | 10 | 79.964 | 0.172956 | 0.104194 |
| 17 | 18 | 4138 | 1381950 | 4137 | 1381942 | J6 | 1.813960 | 1.153699 | 1381951 | 9 | 71.948 | 0.074923 | 0.038345 |
### 13.4.1 手工核对专用三时刻对照
如果你后面要拿着导出的触发序列人工对比,最推荐直接看下面这张表。
对应单独导出文件:
- `analysis/2026042802-1/2026042802-1_trigger_manual_compare.csv`
它把每个触发点压缩成 3 个时刻:
- `触发命令时刻`
- 你真正发出去、同时带着 IO 触发的那条命令
- `触发当下实时反馈`
- 按 `buffer_size=8` 回推到当时机器人真实反馈所对应的状态包
- `最接近示教点的实时反馈`
- 在状态流中真正最贴近示教点的那一帧
可以直接把它理解成一句话:
- “我在第几条命令上触发了”
- “触发那一刻机器人实际还差多少”
- “再过多久机器人才真正到最接近示教点的位置”
| 触发序号 | waypoint_index | 触发命令seq | 触发命令frame | 触发当下状态seq | 触发当下状态frame | 命令领先状态(周期) | 触发当下最大误差(deg) | 最接近示教点状态seq | 最接近示教点状态frame | 从触发当下反馈再晚几周期 | 相对触发命令再晚几周期 | 相对触发命令再晚多久(ms) | 最接近示教点最大误差(deg) |
| --- | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
| 1 | 1 | 1381124 | 1955 | 1381116 | 1954 | 8 | 6.588242 | 1381126 | 1977 | 10 | 2 | 79.853 | 0.207590 |
| 2 | 2 | 1381198 | 2151 | 1381190 | 2150 | 8 | 2.335315 | 1381199 | 2174 | 9 | 1 | 71.912 | 0.082134 |
| 3 | 3 | 1381226 | 2223 | 1381218 | 2222 | 8 | 16.123934 | 1381228 | 2248 | 10 | 2 | 79.929 | 0.569692 |
| 4 | 4 | 1381269 | 2336 | 1381261 | 2335 | 8 | 4.901883 | 1381271 | 2361 | 10 | 2 | 79.909 | 0.094704 |
| 5 | 5 | 1381321 | 2477 | 1381313 | 2476 | 8 | 9.219438 | 1381323 | 2502 | 10 | 2 | 79.833 | 0.056622 |
| 6 | 6 | 1381391 | 2665 | 1381383 | 2664 | 8 | 6.146461 | 1381392 | 2687 | 9 | 1 | 71.904 | 0.176974 |
| 7 | 7 | 1381447 | 2811 | 1381439 | 2810 | 8 | 13.151718 | 1381448 | 2833 | 9 | 1 | 71.873 | 0.306587 |
| 8 | 8 | 1381483 | 2908 | 1381475 | 2907 | 8 | 7.436279 | 1381484 | 2930 | 9 | 1 | 71.838 | 0.060748 |
| 9 | 9 | 1381561 | 3112 | 1381553 | 3111 | 8 | 5.585605 | 1381562 | 3134 | 9 | 1 | 71.873 | 0.152260 |
| 10 | 10 | 1381583 | 3170 | 1381575 | 3169 | 8 | 4.165077 | 1381585 | 3195 | 10 | 2 | 79.856 | 0.073440 |
| 11 | 11 | 1381651 | 3350 | 1381643 | 3349 | 8 | 1.830953 | 1381652 | 3372 | 9 | 1 | 71.898 | 0.077668 |
| 12 | 12 | 1381697 | 3470 | 1381689 | 3469 | 8 | 4.822353 | 1381698 | 3492 | 9 | 1 | 71.950 | 0.021077 |
| 13 | 13 | 1381756 | 3627 | 1381748 | 3626 | 8 | 8.152508 | 1381757 | 3649 | 9 | 1 | 71.855 | 0.145543 |
| 14 | 14 | 1381787 | 3710 | 1381779 | 3709 | 8 | 10.404419 | 1381789 | 3735 | 10 | 2 | 79.922 | 0.119453 |
| 15 | 15 | 1381820 | 3796 | 1381812 | 3795 | 8 | 2.616108 | 1381822 | 3821 | 10 | 2 | 79.926 | 0.149803 |
| 16 | 16 | 1381848 | 3870 | 1381840 | 3869 | 8 | 4.682474 | 1381850 | 3895 | 10 | 2 | 79.964 | 0.172956 |
| 17 | 18 | 1381950 | 4138 | 1381942 | 4137 | 8 | 1.813960 | 1381951 | 4160 | 9 | 1 | 71.948 | 0.074923 |
这张表最适合回答两个实际问题:
1. 触发到底是不是已经“跑过点”才发生的
- 不是
- 因为触发当下对应的实时反馈误差仍然明显存在
2. 真正最接近示教点的实时位置,距离触发还差多久
- 大部分点还要再过 `1` 或 `2` 个命令周期
- 如果换成状态反馈基准,就是再过 `9` 或 `10` 个状态周期
- 时间量级稳定在 `72~80ms`
### 13.4.2 可以优先人工盯的异常点
如果你不想一开始就看 17 个点,建议优先盯下面几类:
- `trigger 3`
- 触发当下实时反馈最大误差 `16.123934 deg`
- 即便到“最佳反馈时刻”,最大误差也还有 `0.569692 deg`
- 它是 17 个点里最值得优先复核的一个
- `trigger 7`
- 触发当下实时反馈最大误差 `13.151718 deg`
- 到位后仍有 `0.306587 deg`
- `trigger 14`
- 触发当下实时反馈最大误差 `10.404419 deg`
- 属于第二档明显偏大的点
- `trigger 1`
- 触发当下误差 `6.588242 deg`,而且最大误差轴是 `J1`
- 和前面很多 `J6` 主导的点不一样,适合拿来对比不同类型
如果你想最快抓住旧程序补偿逻辑,建议先人工对照:
- `trigger 3`
- `trigger 7`
- `trigger 14`
- `trigger 1`
这 4 个点已经能覆盖:
- `J6` 主导的大偏差
- `J1` 主导的特殊点
- `+1` 周期到位与 `+2` 周期到位两种情况
### 13.5 对“为什么触发时间看起来在我运动到这个位置之后”的解释
现在可以把两种视角彻底分开:
1. 命令流视角
- 触发命令帧前 `6` 个周期的目标点,和示教点已经非常接近
- 所以如果只看“发给机器人什么目标”,会误以为触发发生在“经过示教点之后”
2. 实时反馈视角
- 由于 `buffer_size=8`,触发命令帧本身就领先当前状态 `8` 个周期
- 触发时刻对应的实时反馈,其实还没到示教点附近
- 再往后走 `9~10` 个状态周期,反馈才最接近示教点
因此更准确的表述应该是:
- 旧程序不是“机器人已经跑过示教点才开始触发”
- 而是“触发命令是按一个提前缓存的目标序列发出去的,命令序列本身比实时反馈领先 8 个周期”
- 叠加旧程序里触发标志在目标序列上的布置位置,就会出现:
- 命令流看起来:最接近示教点在触发前 `6` 个周期
- 实时反馈看起来:最接近示教点在触发后 `9~10` 个状态周期
两者并不矛盾,基准不同而已。
### 13.6 本节结论
1. 这份旧抓包的 `60015` 状态流已经直接证明:下发命令序列稳定领先当前状态序列 `8` 个周期。
2. 因此不能再把“命令流中的前 6 周期最接近示教点”误解成“机械臂真实位置已经过点后才触发”。
3. 按实时反馈看,17 个触发点在触发当下都还没有到示教点附近;最接近示教点的反馈统一出现在触发后约 `72~80ms`。
4. 如果后续要反推旧程序拍照补偿逻辑,优先应该建模成:
- `命令目标序列` 提前缓存 `8` 个周期
- `触发标志` 在目标序列上又相对示教点提前约 `6` 个周期
- 两者叠加后,实时反馈到位时刻落在触发之后约 `1~2` 个命令周期
## 14. TriggerSampleIndexOffsetCycles 取值 6 / 7 / 8 的对照
在代码里把触发绑定后移做成显式配置之后,进一步对 `6 / 7 / 8` 三档做了离线对照。
本节的比较口径是:
- 先找“最接近示教点”的最佳命令 sample
- 再分别向后偏移 `6 / 7 / 8` 个命令周期
- 比较这三档各自落到的命令帧,与旧抓包真实触发帧的误差大小
新增文件:
- `analysis/2026042802-1/2026042802-1_trigger_offset_6_7_8_compare.csv`
- `analysis/2026042802-1/2026042802-1_trigger_offset_6_7_8_summary.json`
- `analysis/build_trigger_offset_compare_6_7_8.py`
### 14.1 汇总结论
三档比较结果非常明确:
- `6` 周期胜出 `17 / 17`
- `7` 周期胜出 `0 / 17`
- `8` 周期胜出 `0 / 17`
平均误差也呈现单调变差:
| 偏移周期 | 平均最大单轴误差(deg) | 平均RMS误差(deg) |
| --- | ---: | ---: |
| `6` | `4.241584` | `2.540614` |
| `7` | `5.068092` | `3.021297` |
| `8` | `5.931684` | `3.520176` |
这说明:
- 如果目标是“尽量复刻旧抓包里的真实命令触发点”,`TriggerSampleIndexOffsetCycles` 应该选 `6`
- 当前写入配置的 `7`,更偏向“再往后推一拍”,但它并不更接近这份旧抓包
### 14.2 逐点对照表
| 触发序号 | waypoint_index | 最佳sample frame | 偏移6 frame | 偏移6最大误差 | 偏移6RMS | 偏移7 frame | 偏移7最大误差 | 偏移7RMS | 偏移8 frame | 偏移8最大误差 | 偏移8RMS | 本点最优偏移 |
| --- | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | --- |
| 1 | 1 | 1941 | 1955 | 3.406527 | 2.468439 | 1958 | 4.180932 | 2.935578 | 1961 | 5.004552 | 3.418447 | `6` |
| 2 | 2 | 2135 | 2151 | 2.816723 | 1.656246 | 2153 | 3.526760 | 2.060948 | 2155 | 4.301479 | 2.503393 | `6` |
| 3 | 3 | 2207 | 2223 | 11.130744 | 5.955608 | 2227 | 13.165517 | 7.035338 | 2229 | 15.212756 | 8.119717 | `6` |
| 4 | 4 | 2321 | 2336 | 2.311684 | 1.373158 | 2339 | 3.083186 | 1.743718 | 2341 | 3.952016 | 2.158078 | `6` |
| 5 | 5 | 2457 | 2477 | 2.171828 | 1.404704 | 2479 | 2.585320 | 1.667010 | 2483 | 3.004922 | 1.950746 | `6` |
| 6 | 6 | 2649 | 2665 | 1.408994 | 0.842403 | 2667 | 1.770170 | 1.066919 | 2669 | 2.303903 | 1.330975 | `6` |
| 7 | 7 | 2795 | 2811 | 7.194107 | 4.618545 | 2814 | 8.248680 | 5.331516 | 2817 | 9.275559 | 6.035874 | `6` |
| 8 | 8 | 2892 | 2908 | 5.939884 | 3.533133 | 2910 | 6.922169 | 4.090128 | 2912 | 7.916950 | 4.649087 | `6` |
| 9 | 9 | 3096 | 3112 | 2.524071 | 1.630593 | 3114 | 2.830116 | 1.857618 | 3118 | 3.101311 | 2.072647 | `6` |
| 10 | 10 | 3154 | 3170 | 3.392997 | 2.105986 | 3172 | 4.172522 | 2.562682 | 3175 | 4.999007 | 3.042228 | `6` |
| 11 | 11 | 3334 | 3350 | 2.984824 | 2.013493 | 3352 | 3.668105 | 2.458222 | 3355 | 4.397696 | 2.932320 | `6` |
| 12 | 12 | 3455 | 3470 | 4.000958 | 1.990246 | 3474 | 4.929097 | 2.393668 | 3476 | 5.929425 | 2.827363 | `6` |
| 13 | 13 | 3611 | 3627 | 6.656209 | 4.109282 | 3629 | 7.722974 | 4.798702 | 3633 | 8.793828 | 5.501948 | `6` |
| 14 | 14 | 3694 | 3710 | 2.935180 | 1.567983 | 3712 | 3.224395 | 1.721797 | 3714 | 3.454963 | 1.844232 | `6` |
| 15 | 15 | 3780 | 3796 | 2.573147 | 1.686690 | 3798 | 3.074261 | 2.037358 | 3801 | 3.579433 | 2.398059 | `6` |
| 16 | 16 | 3854 | 3870 | 3.810522 | 2.197783 | 3872 | 4.756826 | 2.710410 | 3874 | 5.783033 | 3.263711 | `6` |
| 17 | 18 | 4121 | 4138 | 6.848522 | 4.036140 | 4140 | 8.296536 | 4.890438 | 4142 | 9.827793 | 5.794164 | `6` |
### 14.3 推荐
因此这里可以分成两个不同目标:
1. 如果目标是“尽量复刻旧程序抓包”
- 推荐:`TriggerSampleIndexOffsetCycles = 6`
2. 如果目标是“主观上想往实时反馈更靠后推一点,再试效果”
- 可以继续保留 `7`
- 但要明确:这已经不是“最接近旧抓包”的取值,而是主动做新的补偿尝试
当前基于旧抓包证据,我更推荐:
- 先把配置从 `7` 改回 `6`
- 再拿新程序实际导出的 `ActualSendJointTraj.txt + ShotEvents.json + 现场图像效果` 做下一轮闭环

View File

@@ -0,0 +1,125 @@
# 相机触发实际偏差抓包触发点记录
## 数据来源
- 抓包文件:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/Rvbust/相机触发实际偏差.pcap`
- 对照配置:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/flyshot-replacement/Config/RobotConfig.json` 中 `flying_shots.UTTC_MS11`
- 提取口径:只看 `192.168.10.5:50843 -> 192.168.10.11:60015` 的 64B J519 命令帧。
- 触发判定:`write_io_mask > 0 && write_io_value > 0` 视为 IO 置位。
- 去重规则:由于 `io_keep_cycles=2`,每次触发会连续保持两帧高电平;本文档每组只记录第一帧,最终得到 17 次真实触发。
- 关节单位:抓包中的 J519 目标关节为 `deg`。
## 统计
- 真实触发次数:17
- 平均单点最大单轴误差:0.142224 deg
- 最大单轴误差:0.309618 deg
- 平均 RMS 误差:0.085167 deg
## 触发记录
| 触发序号 | waypoint_index | frame | seq | 相对时间(s) | io_mask | J1(deg) | J2(deg) | J3(deg) | J4(deg) | J5(deg) | J6(deg) | 最大误差轴 | 最大误差(deg) | RMS误差(deg) |
| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |
| 1 | 1 | 2445 | 4641 | 7.536586 | 10 | 48.879890 | 2.237859 | -11.024109 | 0.101012 | 6.253663 | 2.604609 | J6 | 0.309618 | 0.179380 |
| 2 | 2 | 2689 | 4739 | 8.320342 | 14 | 55.367146 | 11.804994 | -7.005956 | -70.935097 | 6.014911 | 74.180328 | J4 | 0.079235 | 0.043714 |
| 3 | 3 | 2779 | 4769 | 8.560284 | 14 | 55.135155 | 8.783901 | -8.500860 | -41.282757 | 10.067441 | 42.164478 | J6 | 0.207545 | 0.110824 |
| 4 | 4 | 2882 | 4811 | 8.896328 | 10 | 43.623173 | -1.579988 | -17.651855 | 6.007176 | 32.012070 | -22.635672 | J5 | 0.159648 | 0.078436 |
| 5 | 5 | 3004 | 4860 | 9.288442 | 10 | 64.717529 | 4.395389 | -15.566289 | -29.885765 | 29.651941 | 45.777378 | J5 | 0.198499 | 0.137260 |
| 6 | 6 | 3169 | 4921 | 9.776381 | 12 | 60.502796 | 23.212698 | -5.056696 | 36.065346 | 5.528813 | -31.387259 | J2 | 0.143916 | 0.073287 |
| 7 | 7 | 3286 | 4968 | 10.152455 | 12 | 70.484123 | 16.315718 | -13.455399 | -28.008068 | 14.532272 | 31.833994 | J6 | 0.122061 | 0.075811 |
| 8 | 8 | 3360 | 4998 | 10.392386 | 10 | 69.575752 | -17.207413 | -8.691541 | -58.617191 | 7.602061 | 64.591766 | J6 | 0.154034 | 0.096014 |
| 9 | 9 | 3542 | 5063 | 10.912377 | 10 | 73.584053 | -6.468980 | -9.609648 | -128.917969 | 14.704389 | 140.109039 | J4 | 0.109321 | 0.066535 |
| 10 | 10 | 3585 | 5081 | 11.056321 | 10 | 75.474472 | -14.468846 | -7.325953 | -131.159668 | 17.608482 | 142.113190 | J4 | 0.236635 | 0.165806 |
| 11 | 11 | 3739 | 5137 | 11.504390 | 10 | 86.261429 | -14.617929 | -13.661033 | -69.765862 | 26.736444 | 88.871201 | J1 | 0.167931 | 0.107930 |
| 12 | 12 | 3835 | 5176 | 11.816490 | 10 | 61.744911 | -4.247795 | -9.792937 | -108.367577 | 22.170429 | 118.159653 | J1 | 0.024179 | 0.016111 |
| 13 | 13 | 3961 | 5227 | 12.224448 | 12 | 80.019569 | 4.632708 | -15.454964 | -55.831528 | 26.270802 | 52.205910 | J1 | 0.262486 | 0.159680 |
| 14 | 14 | 4031 | 5255 | 12.448371 | 10 | 109.016167 | 1.357394 | -33.634285 | -48.365601 | 44.002415 | 85.139641 | J6 | 0.100380 | 0.053296 |
| 15 | 15 | 4119 | 5285 | 12.688396 | 10 | 110.607498 | -3.936621 | -32.371201 | -51.189194 | 44.234520 | 87.651657 | J4 | 0.076603 | 0.047511 |
| 16 | 16 | 4183 | 5311 | 12.896459 | 10 | 118.107513 | -17.420839 | -31.061703 | -59.539654 | 48.746391 | 94.106598 | J2 | 0.044452 | 0.024439 |
| 17 | 18 | 4466 | 5419 | 13.760487 | 12 | 60.277737 | -22.937906 | -10.334841 | 77.472717 | 35.583603 | -69.647720 | J6 | 0.021260 | 0.011799 |
## 逐点示教差值
| 触发序号 | waypoint_index | diff_j1(deg) | diff_j2(deg) | diff_j3(deg) | diff_j4(deg) | diff_j5(deg) | diff_j6(deg) |
| --- | --- | --- | --- | --- | --- | --- | --- |
| 1 | 1 | -0.006920 | 0.038873 | -0.003092 | -0.309197 | 0.005281 | 0.309618 |
| 2 | 2 | 0.019390 | -0.002046 | 0.003139 | 0.079235 | 0.002846 | -0.069204 |
| 3 | 3 | 0.025347 | 0.024404 | 0.017357 | -0.165506 | -0.041047 | 0.207545 |
| 4 | 4 | -0.030420 | 0.049671 | 0.063898 | 0.011967 | -0.159648 | -0.061698 |
| 5 | 5 | 0.135085 | 0.132409 | 0.102928 | 0.067162 | -0.198499 | 0.150851 |
| 6 | 6 | 0.023314 | 0.143916 | -0.045432 | -0.041509 | 0.003132 | -0.084695 |
| 7 | 7 | 0.008286 | -0.078131 | 0.001549 | -0.115749 | -0.003390 | 0.122061 |
| 8 | 8 | -0.006712 | -0.101699 | 0.019049 | -0.141497 | -0.028529 | 0.154034 |
| 9 | 9 | 0.012794 | -0.039135 | 0.018932 | -0.109321 | 0.033306 | 0.106991 |
| 10 | 10 | -0.094914 | 0.210460 | -0.031797 | -0.236635 | -0.079879 | 0.219682 |
| 11 | 11 | 0.167931 | -0.119596 | 0.020478 | 0.102434 | -0.006237 | -0.128213 |
| 12 | 12 | 0.024179 | -0.015005 | -0.008514 | 0.016506 | 0.009657 | 0.017589 |
| 13 | 13 | 0.262486 | -0.007523 | -0.143778 | 0.203785 | 0.137664 | 0.053653 |
| 14 | 14 | 0.060616 | -0.013366 | -0.039860 | 0.001760 | 0.039011 | 0.100380 |
| 15 | 15 | 0.022650 | -0.075722 | 0.025207 | -0.076603 | 0.005360 | 0.027657 |
| 16 | 16 | 0.011561 | -0.044452 | 0.007298 | 0.020885 | 0.013816 | -0.028173 |
| 17 | 18 | -0.004746 | 0.000174 | -0.001593 | -0.018925 | 0.000225 | 0.021260 |
## 说明
- 本文档只记录“抓包中真实发 IO 的那一帧”的关节坐标。
- 如果后续要和旧程序抓包或 `ShotEvents.json` 对比,应继续沿用同一口径:优先比较真实 IO 置位帧,而不是只比较 sample_index。
## 新旧程序真实触发点逐点对比
- 新抓包:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/Rvbust/相机触发实际偏差.pcap`
- 旧抓包:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/Rvbust/uttc-20260428/2026042802-1.pcap`
- 对比口径:两边都只取真实 IO 置位高电平的第一帧,共 17 次触发。
### 对比统计
- 新旧都成功提取到 17 次真实触发。
- 平均单点最大单轴关节差:4.185609 deg
- 最大单轴关节差:11.338289 deg
- 平均 RMS 关节差:2.515726 deg
- 平均相对首触发时间漂移:0.220309 s
- 最大相对首触发时间漂移:0.432218 s
### 新旧实际触发记录
| 触发序号 | 新frame | 新seq | 新相对时间(s) | 新io_mask | 新J1 | 新J2 | 新J3 | 新J4 | 新J5 | 新J6 | 旧frame | 旧seq | 旧相对时间(s) | 旧io_mask | 旧J1 | 旧J2 | 旧J3 | 旧J4 | 旧J5 | 旧J6 | 时间差(s) | 相对首点漂移(s) | 最大差轴 | 最大关节差(deg) | RMS差(deg) |
| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |
| 1 | 2445 | 4641 | 7.536586 | 10 | 48.879890 | 2.237859 | -11.024109 | 0.101012 | 6.253663 | 2.604609 | 1955 | 1381124 | 5.888271 | 10 | 45.863270 | 2.972479 | -13.609342 | -2.185114 | 7.582361 | 5.701518 | 1.648315 | 0.000000 | J6 | 3.096909 | 2.341857 |
| 2 | 2689 | 4739 | 8.320342 | 14 | 55.367146 | 11.804994 | -7.005956 | -70.935097 | 6.014911 | 74.180328 | 2151 | 1381198 | 6.480252 | 14 | 56.343365 | 11.667239 | -6.600924 | -68.197609 | 6.058437 | 71.539284 | 1.840090 | 0.191775 | J4 | 2.737488 | 1.612807 |
| 3 | 2779 | 4769 | 8.560284 | 14 | 55.135155 | 8.783901 | -8.500860 | -41.282757 | 10.067441 | 42.164478 | 2223 | 1381226 | 6.704225 | 14 | 53.508320 | 7.412223 | -9.629740 | -32.305714 | 12.488306 | 30.826189 | 1.856059 | 0.207744 | J6 | 11.338289 | 6.066399 |
| 4 | 2882 | 4811 | 8.896328 | 10 | 43.623173 | -1.579988 | -17.651855 | 6.007176 | 32.012070 | -22.635672 | 2336 | 1381269 | 7.048266 | 10 | 44.433006 | -2.115629 | -18.397774 | 4.837199 | 33.993725 | -20.262289 | 1.848062 | 0.199747 | J6 | 2.373383 | 1.439206 |
| 5 | 3004 | 4860 | 9.288442 | 10 | 64.717529 | 4.395389 | -15.566289 | -29.885765 | 29.651941 | 45.777378 | 2477 | 1381321 | 7.464289 | 10 | 65.849670 | 5.695003 | -14.563637 | -28.693426 | 27.678612 | 46.437157 | 1.824153 | 0.175838 | J5 | 1.973330 | 1.273254 |
| 6 | 3169 | 4921 | 9.776381 | 12 | 60.502796 | 23.212698 | -5.056696 | 36.065346 | 5.528813 | -31.387259 | 2665 | 1381391 | 8.024277 | 12 | 60.847530 | 24.477776 | -5.617044 | 34.804432 | 5.786255 | -31.225348 | 1.752104 | 0.103789 | J2 | 1.265078 | 0.786940 |
| 7 | 3286 | 4968 | 10.152455 | 12 | 70.484123 | 16.315718 | -13.455399 | -28.008068 | 14.532272 | 31.833994 | 2811 | 1381447 | 8.472317 | 12 | 70.825050 | 10.952600 | -13.136046 | -34.684597 | 14.022421 | 38.906040 | 1.680138 | 0.031823 | J6 | 7.072046 | 4.542963 |
| 8 | 3360 | 4998 | 10.392386 | 10 | 69.575752 | -17.207413 | -8.691541 | -58.617191 | 7.602061 | 64.591766 | 2908 | 1381483 | 8.760361 | 10 | 69.405167 | -20.066782 | -8.152936 | -63.932354 | 6.775912 | 70.377617 | 1.632025 | -0.016290 | J6 | 5.785851 | 3.437653 |
| 9 | 3542 | 5063 | 10.912377 | 10 | 73.584053 | -6.468980 | -9.609648 | -128.917969 | 14.704389 | 140.109039 | 3112 | 1381561 | 9.384332 | 10 | 73.905930 | -8.016125 | -9.002426 | -131.332718 | 15.526207 | 142.416687 | 1.528045 | -0.120270 | J4 | 2.414749 | 1.565121 |
| 10 | 3585 | 5081 | 11.056321 | 10 | 75.474472 | -14.468846 | -7.325953 | -131.159668 | 17.608482 | 142.113190 | 3170 | 1381583 | 9.560358 | 10 | 76.726883 | -16.518726 | -7.151471 | -127.530037 | 18.537889 | 138.789749 | 1.495963 | -0.152352 | J4 | 3.629631 | 2.268783 |
| 11 | 3739 | 5137 | 11.504390 | 10 | 86.261429 | -14.617929 | -13.661033 | -69.765862 | 26.736444 | 88.871201 | 3350 | 1381651 | 10.104322 | 10 | 83.716621 | -13.186573 | -13.738809 | -72.703178 | 26.667589 | 91.984238 | 1.400068 | -0.248247 | J6 | 3.113037 | 2.115595 |
| 12 | 3835 | 5176 | 11.816490 | 10 | 61.744911 | -4.247795 | -9.792937 | -108.367577 | 22.170429 | 118.159653 | 3470 | 1381697 | 10.472333 | 10 | 59.789558 | -2.709417 | -8.933828 | -108.248573 | 21.177492 | 114.141106 | 1.344157 | -0.304158 | J6 | 4.018547 | 2.003201 |
| 13 | 3961 | 5227 | 12.224448 | 12 | 80.019569 | 4.632708 | -15.454964 | -55.831528 | 26.270802 | 52.205910 | 3627 | 1381756 | 10.944371 | 12 | 86.413292 | 4.266173 | -19.159878 | -52.240280 | 29.853724 | 55.869072 | 1.280077 | -0.368238 | J1 | 6.393723 | 3.955873 |
| 14 | 4031 | 5255 | 12.448371 | 10 | 109.016167 | 1.357394 | -33.634285 | -48.365601 | 44.002415 | 85.139641 | 3710 | 1381787 | 11.192339 | 10 | 110.752319 | 0.875189 | -34.756535 | -48.143326 | 45.086559 | 87.974442 | 1.256032 | -0.392283 | J6 | 2.834801 | 1.514764 |
| 15 | 4119 | 5285 | 12.688396 | 10 | 110.607498 | -3.936621 | -32.371201 | -51.189194 | 44.234520 | 87.651657 | 3796 | 1381820 | 11.456361 | 10 | 111.791779 | -6.225441 | -31.958988 | -53.685738 | 44.761509 | 89.334549 | 1.232035 | -0.416280 | J4 | 2.496544 | 1.640818 |
| 16 | 4183 | 5311 | 12.896459 | 10 | 118.107513 | -17.420839 | -31.061703 | -59.539654 | 48.746391 | 94.106598 | 3870 | 1381848 | 11.680362 | 10 | 117.874741 | -19.607178 | -30.329103 | -56.600250 | 49.086555 | 90.324249 | 1.216097 | -0.432218 | J6 | 3.782349 | 2.176883 |
| 17 | 4466 | 5419 | 13.760487 | 12 | 60.277737 | -22.937906 | -10.334841 | 77.472717 | 35.583603 | -69.647720 | 4138 | 1381950 | 12.496365 | 12 | 59.935894 | -21.102140 | -9.684239 | 70.643120 | 32.941307 | -63.348557 | 1.264122 | -0.384193 | J4 | 6.829597 | 4.025228 |
### 新减旧逐轴关节差
| 触发序号 | diff_j1(deg) | diff_j2(deg) | diff_j3(deg) | diff_j4(deg) | diff_j5(deg) | diff_j6(deg) |
| --- | --- | --- | --- | --- | --- | --- |
| 1 | 3.016621 | -0.734620 | 2.585233 | 2.286127 | -1.328699 | -3.096909 |
| 2 | -0.976219 | 0.137754 | -0.405033 | -2.737488 | -0.043527 | 2.641045 |
| 3 | 1.626835 | 1.371678 | 1.128880 | -8.977043 | -2.420865 | 11.338289 |
| 4 | -0.809834 | 0.535641 | 0.745918 | 1.169977 | -1.981655 | -2.373383 |
| 5 | -1.132141 | -1.299614 | -1.002652 | -1.192339 | 1.973330 | -0.659779 |
| 6 | -0.344734 | -1.265078 | 0.560348 | 1.260914 | -0.257442 | -0.161911 |
| 7 | -0.340927 | 5.363118 | -0.319352 | 6.676529 | 0.509851 | -7.072046 |
| 8 | 0.170586 | 2.859369 | -0.538605 | 5.315163 | 0.826149 | -5.785851 |
| 9 | -0.321877 | 1.547145 | -0.607222 | 2.414749 | -0.821818 | -2.307648 |
| 10 | -1.252411 | 2.049880 | -0.174482 | -3.629631 | -0.929407 | 3.323441 |
| 11 | 2.544807 | -1.431356 | 0.077776 | 2.937317 | 0.068855 | -3.113037 |
| 12 | 1.955353 | -1.538378 | -0.859109 | -0.119003 | 0.992937 | 4.018547 |
| 13 | -6.393723 | 0.366534 | 3.704914 | -3.591248 | -3.582922 | -3.663162 |
| 14 | -1.736153 | 0.482206 | 1.122250 | -0.222275 | -1.084145 | -2.834801 |
| 15 | -1.184280 | 2.288819 | -0.412212 | 2.496544 | -0.526989 | -1.682892 |
| 16 | 0.232773 | 2.186338 | -0.732599 | -2.939404 | -0.340164 | 3.782349 |
| 17 | 0.341843 | -1.835766 | -0.650601 | 6.829597 | 2.642296 | -6.299164 |

View File

@@ -130,11 +130,11 @@
以下内容是当前新实现的状态,不反推为旧系统事实:
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务,不启动 `50001/TCP+JSON` 监听。
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹,再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹,通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
- `ExecuteTrajectory` 会先通过默认 Rust `method="icsp"` 后端规划普通轨迹,再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹,通过已注册的规划后端生成轨迹,并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
- HTTP 控制器已经按公开文档和抓包补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache, wait)` 参数,并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
- 规划阶段会继续消费旧配置中的 `acc_limit / jerk_limit`。如果现场需要复现旧服务端不可见的保守约束,replacement 设计上使用内部 `planning_acceleration_scale` 限制规划加速度;该字段不属于旧 `ControllerClient` 公开 API,也不会通过 `50001` JSON 下发。
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器;`method="doubles"` 会被识别但返回显式未实现,不会静默降级成 ICSP。
- `method="icsp"` 已接入默认 Rust 后端;`method="icsp-csharp"` 与 `method="self-adapt-icsp"` 只有显式注册 `Flyshot.CSharp.Planning` 后端时可用;`method="doubles"` 会被识别但返回显式未实现,不会静默降级成 ICSP。
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码,并使用逆向抓包样本覆盖最小测试;`10010` 当前现场确认固定 90B。
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路;稠密轨迹下发已按 `speed_ratio` 推进轨迹时间,并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。
- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`,`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包;实际执行不发送 464 行导出点,而是在 8ms 实发周期上按 `t_traj = t_send * speed_ratio` 采样,包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。

View File

@@ -7,32 +7,27 @@
推荐使用聚合端点完成当前现场的一次性初始化:
```bash
POST /init_mpc_robt
POST /init_mpc_robot
{
"server_ip": "127.0.0.1",
"port": 50001,
"robot_name": "FANUC_LR_Mate_200iD",
"robot_ip": "192.168.10.11",
"sim": false
"robot_ip": "192.168.10.11"
}
```
该端点内部顺序:
1. `ConnectServer(server_ip, port)`:兼容旧参数形状,仅记录服务连接语义。
2. `SetUpRobot(robot_name)`:加载机器人配置、关节限制和伺服周期。
3. `SetActiveController(sim)`:选择仿真或 FANUC 真机运行时。
4. `Connect(robot_ip)`:真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
5. `EnableRobot(2)`:真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`,随后允许 J519 在收到机器人 UDP status 包后回发下一帧命令。
1. `SetUpRobot(robot_name)`:加载机器人配置、关节限制和伺服周期,状态进入 `OfflineReady`。
2. `Connect(robot_ip)`:依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
3. `EnableRobot(8)`:启动 `RVBUSTSM` 和 J519 运动循环,成功后进入 `RealReady`。
连接或使能失败时接口返回 400,但模型保持加载,状态进入 `OfflineReady`;只有模型加载失败才进入 `ModelLoadFailed`。
也可以使用拆分端点按同样顺序调用:
```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
GET /enable_robot/?buffer_size=8
```
## 2. 参数设置
@@ -98,6 +93,8 @@ POST /move_joint/
## 4. 飞拍轨迹
飞拍资产固定保存在 `Config/RobotConfig.json`。上传、列出和删除不要求模型已加载,也不要求真机连接;`setup_robot` 不会清空已上传轨迹。
上传:
```bash
@@ -122,6 +119,18 @@ POST /is_flyShotTrajValid/
}
```
离线生成最终发送轨迹:
```bash
POST /save_traj_info/
{
"name": "UTTC_MS11",
"method": "self-adapt-icsp"
}
```
该接口只要求模型已加载,不要求连接或使能。响应包含规划时长、最终发送时长、关键/稠密点数量、完整最终 8ms 点列、IO mask/value、ShotEvents、触发采样点绑定和全部导出文件绝对路径。`/debug` 中的“离线飞拍规划”面板提供同一能力,并明确不会向机器人发送命令。
执行:
```bash
@@ -142,7 +151,7 @@ POST /execute_flyshot/
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹;规划阶段先应用 `acc_limit / jerk_limit`,再应用 replacement-only 的规划加速度校准参数。
3. 生成 `TrajectoryDoEvent`,把拍照触发绑定到轨迹时间。
4. 若 `move_to_start=true`,先从运行时当前关节位置生成临时 PTP 稠密轨迹移动到规划轨迹起点,并等待运行时 `IsInMotion=false` 后再启动飞拍轨迹,避免第一帧 J519 目标从当前位置跳到起点。
5. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 轨迹时间步长重采样并转成 `deg`,命令实际发包由机器人 UDP status 包驱动。
5. 把规划输出的 `rad` 连续轨迹构建为最终 8ms 发送队列并转成 `deg`;离线规划和真机执行复用同一份结果。
6. 若 `wait=true`,正式飞拍轨迹启动后继续等待运行时 `IsInMotion=false`,机器人执行完整条飞拍轨迹后 HTTP 才返回;`wait=false` 时启动后立即返回。
7. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪,则拒绝执行。
8. 周期命令中嵌入 IO 脉冲;当前 UTTC 抓包确认 mask 集合为 `10/12/14`,共 17 个 set 脉冲和 17 个 clear 帧。
@@ -157,9 +166,10 @@ GET /disable_robot/
POST /disconnect_robot/
```
真机模式下:
真机运行时:
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 状态包驱动发送。
- 正常飞拍、MoveJoint、MovePose 结束时,默认在 J519 队列末尾追加 `LastData=1` 结束帧;结束帧发出后进入仅监控状态,继续接收 J519 状态包,不再回发保姿命令。
- `StopMove()` 取消当前稠密轨迹生成任务并走强制停止路径,可停止 J519 状态包驱动发送。
- `DisableRobot()` 发送 J519 packet type 2 状态输出停止包,然后 `StopProg("RVBUSTSM")`。
- `Disconnect()` 关闭状态、命令和 J519 三条通道,并清理本地运行状态。
@@ -178,5 +188,5 @@ POST /disconnect_robot/
- 主运行窗口命令序号连续,无重复 seq;J519 客户端单元测试覆盖按最新 status sequence 回发命令。
- 响应 `status=15` 段覆盖主运行窗口,响应相对命令滞后 2 到 8 帧。
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`,最大绝对误差小于 `0.07deg`。
- `lastData=0`,结束运动当前依赖 J519 packet type 2 状态输出停止包;`../j519 协议.pcap` 中另有 1 个 `LastData=1` 后紧跟 type 2 的样本,停止语义后续单独验证。
- 历史抓包主运行窗口 `lastData=0`;当前实现默认在生成的新 J519 队列末尾追加一帧 `LastData=1` 结束命令,随后保留状态监控且不发送 packet type 2。packet type 2 仅用于禁用、异常停止、断开等强制停止路径;如现场需要旧保持行为,可通过内部开关关闭结束帧路径。
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。

View File

@@ -82,7 +82,7 @@
文档把它定义为“停止状态包输出”的控制包,不是命令流正常终止的首选动作。命令流结束应通过 command packet 的 `Last data=1` 表达。
当前实现:`FanucJ519Client.StopMotionAsync()` 当前会停止状态包驱动发送并发送 packet type `2`,而稠密轨迹执行期间保持 `LastData=0`。这是与 FANUC 文档最明显的语义差异之一;已有多数 UTTC 抓包显示主运行窗口 `LastData=0`,但 `../j519 协议.pcap` 中存在 1 个 `LastData=1` 后紧跟 packet type `2` 的样本,后续应单独校准停止语义。
当前实现:`FanucJ519Client.StopMotionAsync()` 仍保留 packet type `2` 作为禁用、异常中断或断开前的强制停止路径。正常稠密运动结束由内部开关 `EndCommandStreamAfterQueueDrained` 控制,默认会在队列末尾追加一帧 `LastData=1` 的结束命令;该帧发出后客户端进入仅监控状态,继续接收 J519 状态包用于位置监控,不再持续回发保姿命令。关闭该开关时保留旧行为:队列结束后继续保持最后目标或反馈位置。`LastData` 是单帧 command packet 字段,不是全局状态;下一轮运动重新装载的新队列会从 `LastData=0` 的普通命令帧开始。
## 4. 通信时序重点
@@ -100,7 +100,7 @@
1. `FanucJ519Client` 已改为收到机器人 132B status 包后立即回发当前命令,不再由上位机本地固定 8ms 发送循环主动发包。
2. 命令包 sequence 已按刚收到的 status packet sequence 写入,避免第一帧从本地 `0` 起步。
3. `FanucControllerRuntime.ExecuteTrajectory()` 启动前会检查已有 J519 响应中的 `AcceptsCommand` 和 `SystemReady`;但如果还没收到状态包,则会放行,后续命令仍要等第一帧 status 到达才会发出。
4. 当前稠密轨迹结束不发送 `LastData=1`,而是依赖停止 J519 状态包驱动发送和 packet type `2` stop 控制包。
4. 默认新路径会在稠密轨迹队列末尾追加独立的 `LastData=1` 结束帧;结束后只监控状态,不发送 packet type `2`。旧保持路径仍可通过内部开关回退。
序号和节拍已经按手册方向校准;停止语义仍需在真实 R30iB 联调中继续确认。
@@ -220,13 +220,13 @@
主要差异/风险:
1. 当前未实现命令缓冲预填,也未读取 `$PKT_STACK / $START_MOVE`。
2. 当前停止运动依赖 packet type `2` stop 控制包,没有稳定发送 `LastData=1` 的最后 command packet;这与手册标准结束语义不同。
2. 默认正常运动结束已经通过追加 `LastData=1` 结束帧靠近手册语义;packet type `2` 仍仅作为强制停止状态输出路径。
3. 当前未实现 packet type `3` 的速度/加速度/jerk 限制表查询,也未实现 payload/speed 20 档动态限制算法。
4. 当前没有自动校验 `AUTO / 100% OVERRIDE / brake control / resume offset / payload` 等控制器前置状态。
5. 当前没有报警码读取和 `MOTN-* / PRIO-*` 映射。
建议后续联调优先级:
1. 验证运动结束是否必须补 `LastData=1`;如果当前 stop 控制包能稳定工作,也应在文档中标为现场兼容路径,而不是手册标准路径。
1. 联调验证 `LastData=1` 后机器人是否稳定退出命令消费并继续输出状态包;如现场程序仍要求持续命令包,可通过内部开关回退旧保持路径。
2. 抓一次报警现场包,确认 `MOTN-600/603/617` 等是否能从包序号与状态位直接定位。
3. 如果后续追求更稳的真实机运行,补 packet type `3` 限制表查询,并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。

View File

@@ -0,0 +1,138 @@
<svg xmlns="http://www.w3.org/2000/svg" width="1600" height="900" viewBox="0 0 1600 900" role="img" aria-labelledby="title desc">
<title id="title">飞拍轨迹规划逻辑</title>
<desc id="desc">用于 PPT 展示的飞拍轨迹规划逻辑图,展示输入、三次样条、约束检查、逐段缩放迭代、输出时间轴与采样层关系。</desc>
<defs>
<marker id="arrow-blue" markerWidth="14" markerHeight="14" refX="11" refY="7" orient="auto" markerUnits="strokeWidth">
<path d="M2,2 L12,7 L2,12 Z" fill="#2563eb" />
</marker>
<marker id="arrow-green" markerWidth="14" markerHeight="14" refX="11" refY="7" orient="auto" markerUnits="strokeWidth">
<path d="M2,2 L12,7 L2,12 Z" fill="#059669" />
</marker>
<marker id="arrow-amber" markerWidth="14" markerHeight="14" refX="11" refY="7" orient="auto" markerUnits="strokeWidth">
<path d="M2,2 L12,7 L2,12 Z" fill="#d97706" />
</marker>
<linearGradient id="title-grad" x1="0" x2="1" y1="0" y2="0">
<stop offset="0" stop-color="#1d4ed8" />
<stop offset="0.55" stop-color="#0f766e" />
<stop offset="1" stop-color="#7c3aed" />
</linearGradient>
<filter id="soft-shadow" x="-10%" y="-10%" width="120%" height="130%">
<feDropShadow dx="0" dy="8" stdDeviation="8" flood-color="#0f172a" flood-opacity="0.12" />
</filter>
<style>
.bg { fill: #f8fafc; }
.title { font: 700 42px "Microsoft YaHei", "Segoe UI", Arial, sans-serif; fill: #0f172a; letter-spacing: 0; }
.subtitle { font: 400 23px "Microsoft YaHei", "Segoe UI", Arial, sans-serif; fill: #475569; letter-spacing: 0; }
.section-title { font: 700 25px "Microsoft YaHei", "Segoe UI", Arial, sans-serif; fill: #0f172a; letter-spacing: 0; }
.label { font: 600 21px "Microsoft YaHei", "Segoe UI", Arial, sans-serif; fill: #0f172a; letter-spacing: 0; }
.small { font: 400 18px "Microsoft YaHei", "Segoe UI", Arial, sans-serif; fill: #334155; letter-spacing: 0; }
.tiny { font: 400 15px "Microsoft YaHei", "Segoe UI", Arial, sans-serif; fill: #475569; letter-spacing: 0; }
.formula { font: 500 20px "Consolas", "Microsoft YaHei", monospace; fill: #0f172a; letter-spacing: 0; }
.math { font: 500 25px "Cambria Math", "Times New Roman", "Microsoft YaHei", serif; fill: #0f172a; letter-spacing: 0; }
.math-small { font: 500 21px "Cambria Math", "Times New Roman", "Microsoft YaHei", serif; fill: #0f172a; letter-spacing: 0; }
.box { fill: #ffffff; stroke: #cbd5e1; stroke-width: 2; rx: 18; filter: url(#soft-shadow); }
.box-blue { fill: #eff6ff; stroke: #93c5fd; stroke-width: 2; rx: 18; }
.box-green { fill: #ecfdf5; stroke: #86efac; stroke-width: 2; rx: 18; }
.box-amber { fill: #fffbeb; stroke: #fcd34d; stroke-width: 2; rx: 18; }
.box-purple { fill: #f5f3ff; stroke: #c4b5fd; stroke-width: 2; rx: 18; }
.pill { rx: 22; }
.line { fill: none; stroke: #2563eb; stroke-width: 4; marker-end: url(#arrow-blue); }
.line-green { fill: none; stroke: #059669; stroke-width: 4; marker-end: url(#arrow-green); }
.line-amber { fill: none; stroke: #d97706; stroke-width: 4; marker-end: url(#arrow-amber); }
.loop { fill: none; stroke: #d97706; stroke-width: 4; stroke-dasharray: 10 8; marker-end: url(#arrow-amber); }
.axis { stroke: #64748b; stroke-width: 2; }
.grid { stroke: #e2e8f0; stroke-width: 1; }
.curve { fill: none; stroke: #2563eb; stroke-width: 5; stroke-linecap: round; }
.curve2 { fill: none; stroke: #0f766e; stroke-width: 4; stroke-linecap: round; }
.dot { fill: #1d4ed8; stroke: #ffffff; stroke-width: 4; }
.dot-green { fill: #059669; stroke: #ffffff; stroke-width: 4; }
.accent-bar { fill: url(#title-grad); rx: 4; }
.divider { stroke: #cbd5e1; stroke-width: 2; stroke-dasharray: 8 8; }
</style>
</defs>
<rect class="bg" width="1600" height="900" />
<rect class="accent-bar" x="80" y="58" width="540" height="8" />
<text class="title" x="80" y="120">飞拍轨迹规划逻辑</text>
<text class="subtitle" x="80" y="158">用三次样条连接示教点,通过反复拉长超限段,让整条轨迹满足关节约束。</text>
<!-- 主流程卡片 -->
<rect class="box-blue" x="80" y="215" width="255" height="180" />
<text class="section-title" x="112" y="258">输入</text>
<text class="small" x="112" y="312">示教点</text>
<text class="small" x="112" y="346">关节约束</text>
<path class="line" d="M335 305 H420" />
<rect class="box" x="420" y="215" width="285" height="180" />
<text class="section-title" x="452" y="258">初始时间轴</text>
<text class="small" x="452" y="318">按相邻点距离</text>
<text class="small" x="452" y="352">估算每段时长</text>
<path class="line" d="M705 305 H790" />
<rect class="box-green" x="790" y="215" width="305" height="180" />
<text class="section-title" x="822" y="258">三次样条</text>
<text class="small" x="822" y="318">连接所有示教点</text>
<text class="small" x="822" y="352">形成连续轨迹</text>
<path class="line" d="M1095 305 H1180" />
<rect class="box-amber" x="1180" y="215" width="340" height="180" />
<text class="section-title" x="1212" y="258">约束检查</text>
<text class="small" x="1212" y="312">速度 / 加速度 / jerk</text>
<text class="small" x="1212" y="346">找出超限段</text>
<!-- 曲线示意 -->
<rect class="box" x="105" y="455" width="585" height="285" />
<text class="section-title" x="135" y="497">轨迹形状:经过原始示教点</text>
<line class="grid" x1="150" y1="680" x2="650" y2="680" />
<line class="grid" x1="150" y1="620" x2="650" y2="620" />
<line class="grid" x1="150" y1="560" x2="650" y2="560" />
<line class="axis" x1="150" y1="690" x2="650" y2="690" />
<line class="axis" x1="150" y1="690" x2="150" y2="535" />
<path class="curve" d="M165 665 C205 618, 240 565, 285 585 C340 610, 375 695, 425 645 C475 595, 500 515, 558 548 C595 570, 620 625, 640 600" />
<circle class="dot" cx="165" cy="665" r="9" />
<circle class="dot" cx="285" cy="585" r="9" />
<circle class="dot" cx="425" cy="645" r="9" />
<circle class="dot" cx="558" cy="548" r="9" />
<circle class="dot" cx="640" cy="600" r="9" />
<text class="tiny" x="160" y="720">q0</text>
<text class="tiny" x="277" y="720">q1</text>
<text class="tiny" x="417" y="720">q2</text>
<text class="tiny" x="550" y="720">q3</text>
<text class="tiny" x="630" y="720">qn</text>
<text class="small" x="135" y="777">点位不变,主要调整每段用时。</text>
<!-- 核心公式卡片 -->
<rect class="box-purple" x="735" y="455" width="785" height="285" />
<text class="section-title" x="765" y="497">迭代逻辑</text>
<text class="label" x="780" y="560">1. 计算每段是否超限</text>
<text class="label" x="780" y="620">2. 超限段按比例拉长时间</text>
<text class="label" x="780" y="680">3. 重新生成样条,再检查一次</text>
<text class="math-small" x="1195" y="590">
<tspan>scale</tspan><tspan baseline-shift="sub" font-size="14">i</tspan>
<tspan> = max(s</tspan><tspan baseline-shift="sub" font-size="14">v</tspan><tspan>, s</tspan><tspan baseline-shift="sub" font-size="14">a</tspan><tspan>, s</tspan><tspan baseline-shift="sub" font-size="14">j</tspan><tspan>)</tspan>
</text>
<text class="math-small" x="1195" y="650">
<tspan>T</tspan><tspan baseline-shift="sub" font-size="14">i</tspan>
<tspan> &#8592; T</tspan><tspan baseline-shift="sub" font-size="14">i</tspan>
<tspan> * scale</tspan><tspan baseline-shift="sub" font-size="14">i</tspan>
</text>
<!-- 迭代回路 -->
<path class="loop" d="M1350 395 C1350 430, 1270 435, 1190 425 C1075 410, 988 410, 945 397" />
<text class="small" x="1015" y="435" fill="#92400e">未收敛就继续拉长、重算</text>
<!-- 输出层 -->
<rect class="box-green" x="80" y="805" width="330" height="58" />
<text class="label" x="110" y="842">输出时间轴</text>
<path class="line-green" d="M410 834 H515" />
<rect class="box" x="515" y="790" width="430" height="88" />
<text class="label" x="545" y="826">按周期采样</text>
<text class="tiny" x="545" y="856">后续模块生成稠密轨迹点</text>
<path class="line-green" d="M945 834 H1050" />
<rect class="box-amber" x="1050" y="790" width="470" height="88" />
<text class="label" x="1080" y="826">运行时下发</text>
<text class="tiny" x="1080" y="856">进入飞拍执行链路</text>
</svg>

After

Width:  |  Height:  |  Size: 8.3 KiB

View File

@@ -0,0 +1,460 @@
# 飞拍任务 `speedRatio` 执行侧调速设计
## 1. 背景
当前系统同时存在两个速度相关参数:
- `planning_speed_scale`
- 规划阶段参数。
- 作用是让规划结果在时长与整体动力学上更接近现场旧系统或当前配置要求。
- 启动后固定,不希望在运行中再修改语义。
- `speedRatio`
- 运行阶段参数。
- 期望成为统一在线调速入口。
- 本次范围内只要求对“下一次开始执行的飞拍任务”生效,不要求对正在执行中的任务中途热切换。
本次任务已经明确收敛:
- 只修改飞拍任务链路。
- 不修改 `move_joint`。
- 不通过放宽阈值、关闭校验或只看平均速度来规避问题。
## 2. 现状问题
当前飞拍执行链路的核心语义是:
1. 规划层先生成 `DenseJointTrajectory`。
2. 运行时保持 `8ms` 物理发送周期不变。
3. 对第 `k` 个发送点,按 `trajectoryTime = sendTime * speedRatio` 回映射到规划轨迹时间。
4. 再在原始稠密轨迹上做线性插值,得到实际要发的关节目标。
5. 最后再用离散差分检查 `vel/acc/jerk`。
该结构在 `speedRatio < 1` 时有天然风险:
- 物理发送周期固定为 `8ms`,但轨迹时间推进被压缩。
- 线性插值会把规划轨迹中原本“看起来连续”的局部变化映射成新的离散点列。
- 这些新点列在三阶后向差分上容易形成尖峰。
- 该风险在 `speedRatio = 0.5` 时已经暴露为明显的 Jerk 超限告警。
问题本质不是“平均速度是否合理”,而是:
- 最终 `8ms` 发送点列本身没有先被当作一等公民建模。
- `speedRatio` 的执行语义落在“发送前临场回采样”阶段。
- 离散约束校验发生得太晚,只能报错或事后分析。
## 3. 目标
本设计目标如下:
- 保持 `planning_speed_scale` 只属于规划层。
- `speedRatio` 是运行时统一执行倍率,可影响下一次启动的飞拍、普通轨迹和 `MoveJoint`。
- 飞拍链路在任意有效 `speedRatio` 下,最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
- 不依赖“插值后自然会平滑”的经验假设。
- 当请求 `speedRatio` 下的最终发送队列不满足约束时,拒绝执行并输出超限诊断;不在执行侧偷偷改写本次请求倍率。
- 不破坏现有 `shot timeline / sample offset` 语义。
- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。
## 4. 非目标
本次设计明确不包含以下内容:
- 不恢复旧的发送阶段临场回采样机制;普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段折算。
- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。
- 不修改 `planning_speed_scale` 的含义。
- 不在任务执行中途支持 `speedRatio` 热切换。
- 不通过调大容差、降低限值检查强度或直接跳过 Jerk 检查来“解决问题”。
## 5. 设计总览
飞拍执行链路调整为“两段式”:
1. 规划段
- 维持当前实现。
- 仍由 `planning_speed_scale` 参与规划时长和规划层轨迹生成。
- 输出 `TrajectoryResult.DenseJointTrajectory`、`TriggerTimeline`、`ShotEvents` 等规划结果。
2. 执行准备段
- 新增“飞拍执行侧最终发送序列生成器”。
- 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。
- 生成最终 `8ms` 发送队列。
- 对最终队列做逐周期 `vel/acc/jerk` 校验。
- 如果不通过,则拒绝执行,提示降低 `planning_speed_scale` 重新规划或显式设置更低的 `speedRatio`。
3. 发送段
- 运行时只消费已经准备好的最终 `8ms` 队列。
- 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。
核心变化是:
- `speedRatio` 不再直接驱动“发送前临场插值”。
- `speedRatio` 改为驱动“执行侧最终队列构建”。
- 最终队列一旦生成并校验通过,运行时只负责按 J519 状态包节奏出队发送。
## 6. 统一语义
本次以飞拍任务为主线,但 `speedRatio` 允许作为运行时统一执行倍率影响普通轨迹和 `MoveJoint`,需要把三类入口的边界说清楚,避免再次混淆规划倍率和执行倍率。
### 6.1 `planning_speed_scale`
- 只影响规划结果。
- 影响 `DenseJointTrajectory` 的时间轴与规划层速度/加速度/Jerk 使用方式。
- 飞拍开始执行前就已经固定。
- 在线设置 `speedRatio` 时不应反向修改它。
### 6.2 `speedRatio`
- 影响下一次启动的运动任务,不在任务执行中途热切换。
- 对飞拍任务,表示用户期望的执行层时间推进速度,可前移到规划/执行准备阶段生成最终发送队列。
- 对普通轨迹和 `MoveJoint`,允许在规划或临时 PTP 轨迹生成阶段折算进有效关节限位,使运行时只消费已经生成好的 `8ms` 稠密点列。
- `speedRatio = 0.8` 表示执行节奏接近把同几何轨迹时间轴整体拉长 `1 / 0.8`。
- 例如 `planning_speed_scale = 0.9` 且 `speedRatio = 0.8` 时,最终执行节奏应接近同几何轨迹在 `planning_speed_scale ≈ 0.72` 下的结果。
也就是说:
- `speedRatio` 是确定的执行倍率,不是校验失败后的自动搜索变量。
- `finalSpeedRatio` 正常必须等于请求值。
- 若该倍率下最终队列超限,问题暴露给调用方,而不是由执行侧自动改成另一个倍率。
## 7. 最终发送序列生成
### 7.1 新增执行侧生成器
建议新增一个飞拍专用生成器,命名可类似:
- `FlyshotExecutionSendSequenceBuilder`
- 或 `FlyshotExecutionQueueBuilder`
职责:
- 输入规划层连续 `PlannedTrajectory`,优先使用经过执行侧定时处理后的 `ExecutionTrajectory`。
- 输入 `servoPeriodSeconds`、`speedRatio`。
- 输出最终真实发送的 `8ms` 点列。
- 输出与该点列一致的触发绑定结果。
- 输出请求倍率、最终倍率和历史倍率改写次数等诊断信息;当前语义下最终倍率等于请求倍率、改写次数为 0。
### 7.2 最终请求倍率队列
最终队列按固定时间轴语义生成:
- `sendTime = sampleIndex * 0.008`
- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)`
- 关节目标从连续样条直接取点,并从 `rad` 转为 J519 需要的 `deg`
该队列就是本次请求倍率下的最终发送队列,后续只做校验和触发绑定,不再进行二次滤波或倍率搜索。
### 7.3 校验失败策略
当最终队列的离散 `vel/acc/jerk` 校验失败时:
- 不修改规划层轨迹。
- 不修改 `planning_speed_scale`。
- 不放宽校验阈值。
- 不降低本次 `speedRatio`。
- 直接拒绝执行并保留首个超限诊断。
这样可以避免把 `speedRatio` 误实现成安全优化器。现场若希望从 `planning_speed_scale = 0.9` 的 10s 轨迹降到接近 `planning_speed_scale = 0.7` 的执行效果,应显式设置类似 `speedRatio = 0.8`,而不是让系统在失败后自动猜一个更低倍率。
## 8. 逐周期约束校验
最终发送队列必须在执行前通过离散检查。
### 8.1 校验对象
校验对象必须是最终真实发送的 `8ms` 点列,而不是:
- 规划层原始稠密轨迹,
- 中间候选插值轨迹,
- 或平均时长推导。
### 8.2 校验指标
保留现有逐周期离散差分口径:
- 速度
- 加速度
- Jerk
并继续使用当前机器人模型和运行时限值:
- `velocity_eff = velocity_base`
- `acceleration_eff = acceleration_base * acc_limit`
- `jerk_eff = jerk_base * jerk_limit`
### 8.3 校验失败的处理
飞拍链路按当前修正语义采用“确定倍率 + 失败拒绝”:
- 若请求倍率下失败,立即拒绝执行。
- 不进行执行侧自动倍率搜索。
- 调用方可以选择降低 `planning_speed_scale` 重新规划,或显式设置更低的 `speedRatio` 后再次执行。
拒绝执行时必须输出:
- 首次失败的轴
- 时间窗
- 指标类型
- `actual / limit / ratio`
- 请求 `speedRatio`
- 当前 `finalSpeedRatio`,正常等于请求值
## 9. 触发时序与绑定
### 9.1 保持现有语义
以下语义不变:
- `TriggerTimeline`
- `ShotEvents`
- `TriggerSampleIndexOffsetCycles`
- `io_keep_cycles`
### 9.2 绑定对象改为最终发送队列
当前实现中,触发通常绑定到由回采样生成的 `samples`。
本次改为:
- 先生成最终发送队列。
- 再用该最终队列做 `TriggerSampleBinder.Bind(...)`。
这样可以保证:
- 触发绑定和真实发送完全一致。
- 导出工件中的 `ShotEvents.json` 与真实执行时序一致。
- 触发点可被精确追溯到本次请求倍率下的最终发送点索引。
### 9.3 关于触发数量与顺序
请求倍率改变最终发送时长时:
- 触发数量不能变。
- 触发顺序不能变。
- 触发在最终发送队列中的绑定索引应随时间轴拉长后移或保持可解释等价。
## 10. 运行时职责调整
本次需要显式收紧 `FanucControllerRuntime` 的职责。
### 10.1 改造前
`FanucControllerRuntime.SendDenseTrajectory(...)` 当前同时负责:
- 读取 `DenseJointTrajectory`
- 根据 `_speedRatio` 回采样
- 绑定触发
- 生成命令队列
- 写出 `ActualSend*` 工件
### 10.2 改造后
`FanucControllerRuntime` 只负责:
- 接收已经准备好的最终发送队列
- 接收与该队列一致的触发绑定结果
- 预构建命令队列并交给 J519 客户端
- 记录真实发送过程与导出工件
不再负责:
- 按 `_speedRatio` 从 `DenseJointTrajectory` 临场生成发送点
这样做的目的是让:
- 运行时负责发送。
- 执行准备层负责离散稳定性。
两层边界更清晰,也更利于测试。
## 11. 导出工件与日志
### 11.1 旧规划导出和实发诊断必须分开
`saveTrajectory` 目录中存在两类不同用途的文件,不能再混成同一条时间轴:
第一类是旧格式规划导出,用于和旧 RVBUST 导出的规划轨迹、离线分析脚本做对比。它们只受 `planning_speed_scale` 和规划/整形参数影响,不受运行时 `speedRatio` 影响:
- `JointTraj.txt`
- `JointDetialTraj.txt`
- `CartTraj.txt`
- `CartDetialTraj.txt`
- `JointDetialTraj.analysis.txt`
第二类是实发诊断,用于观察当前 `speedRatio` 下真实准备发送的 8ms 队列和触发绑定。它们必须全部从最终发送队列生成:
- `ActualSendJointTraj.txt`
- `ActualSendTiming.txt`
- `ActualSendJerkStats.txt`
- `ShotEvents.json`
不允许再出现:
- 旧规划导出被运行时 `speedRatio` 改写,
- `ActualSend*` 基于一套样本,
- 真实发送队列又基于另一套样本。
### 11.2 推荐新增日志字段
每次飞拍执行建议至少记录:
- 请求 `speedRatio`
- 规划层轨迹时长
- 最终发送队列点数
- 最终发送队列是否通过校验
- 若失败,首个失败窗口的:
- 关节轴
- 时间区间
- 指标类型
- `actual`
- `limit`
- `ratio`
- 倍率改写次数,当前正常为 0
- 最终采用的执行时长
- 最终发送队列点数
- 最终触发绑定数量
- 最终校验通过结论
### 11.3 日志级别建议
- `Information`
- 记录执行请求、最终采用结果、最终通过结论
- `Warning`
- 记录请求倍率下最终发送队列校验失败
- `Debug`
- 记录最终发送队列的时间映射与详细差分统计
## 12. 验收口径
### 12.1 功能验收
- `speedRatio` 可在线设置。
- 该值对下一次启动的飞拍、普通轨迹或 `MoveJoint` 生效。
- 不需要修改 `planning_speed_scale`。
### 12.2 约束验收
在 `speedRatio = {1.0, 0.8, 0.5}` 下:
- 最终发送队列逐周期 `vel/acc/jerk` 不超限。
- 不出现固定区间正负交替的大幅 Jerk 尖峰。
### 12.3 工件与日志验收
- `JointTraj.txt / JointDetialTraj.txt / CartTraj.txt / CartDetialTraj.txt` 不随运行时 `speedRatio` 改变。
- `ActualSend*` 文件能反映最终真实发送点位与时间映射。
- 日志能定位请求倍率、最终发送队列和校验失败窗口。
### 12.4 回归验收
- `speedRatio = 1.0` 时不退化。
- 触发事件数量不异常漂移。
- 触发绑定顺序不异常漂移。
## 13. 测试方案
### 13.1 单元测试
建议新增:
- 飞拍执行侧最终发送序列生成器测试
- `speedRatio` 非法边界值测试
- 请求倍率下队列超限时拒绝执行测试
- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试
- `planning_speed_scale = 0.9` 加 `speedRatio = 0.8` 的执行时间轴等价于约 `0.72` 规划倍率的回归测试
- 普通轨迹和 `MoveJoint` 在倍率乘积相同时生成等价或可解释等价的稠密执行轨迹
- 触发绑定始终与最终发送队列一致的测试
### 13.2 编排测试
建议补充:
- `ExecuteFlyShotTraj` 进入运行时前,已经拿到最终发送队列
- `FanucControllerRuntime` 不再自行按 `_speedRatio` 对飞拍轨迹回采样
- `ExecuteTrajectory` 和 `MoveJoint` 的调速在规划/生成阶段完成,运行时不再做临场倍率重采样
- `SaveTrajectoryInfo` 的旧格式规划导出不随运行时 `speedRatio` 改变,而 `ActualSend*` 仍随运行时 `speedRatio` 改变
### 13.3 集成与黄金样本
建议至少覆盖:
- `UTTC_MS11`
- `speedRatio = 1.0`
- `speedRatio = 0.8`
- `speedRatio = 0.5`
检查项:
- 最终发送点数
- 最终发送时长
- `ActualSendTiming`
- `ActualSendJointTraj`
- `ActualSendJerkStats`
- `ShotEvents` 绑定结果
- 是否消除当前已知 Jerk 尖峰
## 14. 代码落点建议
建议实现边界如下:
### 14.1 `src/Flyshot.Core.Planning/Sampling/`
新增飞拍执行侧最终发送序列生成器及相关结果类型,负责:
- 候选队列构建
- 离散校验
- 触发绑定输入准备
### 14.2 `src/Flyshot.ControllerClientCompat/`
在飞拍执行准备阶段调用该生成器,把最终发送队列放入执行结果或新的执行上下文对象。
普通轨迹规划入口和 `MoveJoint` 临时轨迹生成入口可以读取当前 `speedRatio`,并在进入运行时前折算成已调速的稠密轨迹。
### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
调整 `SendDenseTrajectory(...)`:
- 不再自行根据 `_speedRatio` 从 `DenseJointTrajectory` 临场重采样
- 直接消费已经构建好的最终发送队列
### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs`
`FlyshotTrajectoryArtifactWriter` 接收两份视图:
- 旧格式规划导出视图:写 `JointTraj.txt`、`JointDetialTraj.txt`、`CartTraj.txt`、`CartDetialTraj.txt`
- 实发诊断视图:写 `ActualSendJointTraj.txt`、`ActualSendTiming.txt`、`ActualSendJerkStats.txt`、`ShotEvents.json`
### 14.5 `tests/Flyshot.Core.Tests/`
补飞拍专项单测、UTTC 回归和工件一致性测试。
## 15. 风险与注意事项
### 15.1 不能把 speedRatio 做成安全优化器
对外语义必须保持:
- 用户请求的是某个 `speedRatio`
- 系统按这个 `speedRatio` 构建最终发送队列
- 若队列超限,则拒绝执行并要求调用方显式选择新的规划倍率或执行倍率
不能因为校验失败就在执行侧自动把 `0.8` 改成 `0.2`;这会让 `speedRatio` 和 `planning_speed_scale` 的关系失去可解释性。
### 15.2 不能让触发绑定脱离最终队列
只要最终发送点数变化,触发绑定就必须重新基于最终队列计算,否则工件与执行会再次分叉。
### 15.3 `speedRatio = 1.0` 必须优先做回归保护
这是最容易被“执行侧重构”误伤的路径,必须作为首要回归项。
## 16. 结论
本次飞拍调速设计的核心结论是:
- `planning_speed_scale` 继续只属于规划层。
- `speedRatio` 是运行时统一执行倍率,并且只对下一次启动的运动任务生效。
- 普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段,运行时只发送已生成好的 `8ms` 点列。
- 飞拍执行必须先生成最终 `8ms` 发送队列,再对该队列做逐周期 `vel/acc/jerk` 校验。
- `speedRatio` 按 `trajectoryTime = sendTime * speedRatio` 确定性拉长执行时间轴,`finalSpeedRatio` 不应被自动改写。
- 若请求倍率下最终队列不满足约束,则拒绝执行并提示降低 `planning_speed_scale` 重新规划或显式设置更低 `speedRatio`。
- 运行时不再对飞拍轨迹做发送前临场回采样。
- 旧格式规划导出必须保持不受运行时 `speedRatio` 影响;`ActualSend*` 和 `ShotEvents.json` 必须统一到真实发送队列上。
该方案可以在不放宽校验阈值、不中断现有触发语义的前提下,把 `speedRatio` 调速从“发送阶段经验插值”收敛为“规划/准备阶段完成、运行时确定发送”的执行机制。

View File

@@ -0,0 +1,257 @@
# 智能视觉质检系统宣传文稿
## 一句话定位
智能视觉质检系统是一套覆盖“边缘取图、AI 检测、人工复核、第三方对接、结果闭环”的工业视觉质量管理平台,帮助产线把图像采集、缺陷识别和复检工作流整合为一条连续、可追踪、可协同的质检链路。
## 宣传主文案
在工业视觉检测现场,真正影响效率的不只是模型能否识别缺陷,还包括图片如何稳定采集、AI 结果如何进入业务系统、人工如何快速复核、第三方系统如何查询结果,以及异常任务如何被追踪和处理。
智能视觉质检系统围绕这一完整链路设计:前端由边缘飞拍系统负责在机器人运动过程中完成连续取图;中间由 AI 检测服务对图片进行模型推理,输出 OK/NG、缺陷区域和结构化检测信息;上层由复检工作流平台承接图片、AI 结果和人工判断,完成任务管理、复核流转、结果查询和第三方接口对接。
这不是单个检测工具,而是一整套面向产线落地的视觉质检系统。它把“取图、识别、复核、上传、查询、归档”串成统一流程,让现场从分散的图片文件、模型结果和人工沟通,升级为可管理、可复核、可追溯的质量工作流。
## 系统组成
### 1. 边缘飞拍取图层
飞拍系统负责在机器人运动过程中完成边缘段取图。它根据检测路径和拍照节拍,在连续运动中触发相机采集图片,减少停顿拍照对产线节拍的影响。
核心作用:
- 支持机器人运动中连续取图。
- 支持按路径、节拍或检测点触发拍照。
- 适合边缘、焊缝、孔位、外观等路径型检测场景。
- 为后续 AI 检测提供稳定、连续、带位置语义的图片输入。
### 2. AI 检测服务层
AI 检测服务负责接收图片并执行模型推理。系统支持通过接口提交图片,可按业务需要调用单模型或多模型检测,并返回结构化结果。
核心作用:
- 接收产线图片进行 AI 检测。
- 输出 OK/NG、缺陷区域、模型结果和处理信息。
- 支持多模型组合检测,适配不同检测面、不同缺陷类型。
- 通过标准接口与上层复检平台、现场系统集成。
### 3. 人工复核与工作流层
复检工作流平台负责承接 AI 检测结果和图片数据,形成可操作的人工复核流程。它提供任务管理、图片浏览、AI 结果展示、人工判定、历史查询、第三方接口对接和结果归档能力。
核心作用:
- 管理待复核、已完成、历史任务。
- 支持原图、AI 标注图和检测元数据统一展示。
- 支持人工 OK/NG 判断与批量提交。
- 支持第三方系统上传任务、查询最新复检结果。
- 形成从图片到判定结果的完整追溯链。
## 端到端业务闭环
```text
机器人/相机飞拍取图
↓
图片进入 AI 检测服务
↓
AI 输出 OK/NG 与缺陷信息
↓
复检平台生成审核任务
↓
人工查看原图与 AI 标注图
↓
人工确认最终结果
↓
第三方系统查询或接收复检结果
↓
质检数据归档与追溯
```
这条链路把现场设备、AI 推理、人工经验和业务系统连接在一起,使视觉检测不再停留在单点识别,而是进入可运行的质量管理流程。
## 核心能力
### 1. 连续取图能力
系统支持机器人边运动边拍照,适合路径型工件和多点位检测。通过拍照节拍配置,可根据产线速度、相机曝光和检测需求调整采集频率。
### 2. AI 自动检测能力
系统支持图片接口化检测,能够将模型推理结果结构化输出。AI 检测结果可直接进入复检平台,作为人工判断的重要依据。
### 3. 人工复核能力
复检平台提供面向操作员的图片审核界面,可集中查看任务、图片、AI 标注图、缺陷信息和历史记录。人工可快速完成 OK/NG 判定,减少线下沟通和文件查找。
### 4. 第三方接口能力
系统可对接第三方业务系统,支持任务上传、结果查询和复检状态同步。产线系统、PLC 上位系统、MES 或其他业务平台可以通过接口获取复检结果。
### 5. 工作流闭环能力
系统将图片采集、AI 检测、人工复核和结果回传组织成统一流程。每个任务都有状态,每张图片都有来源和判定,每次操作都可追踪。
### 6. 多场景扩展能力
整体架构可适配不同检测产品、不同图片来源、不同 AI 模型和不同复核策略。既可以服务单工位,也可以扩展到多工位、多模型、多业务线。
## 展示重点
### 系统能力总览
| 层级 | 作用 | 对外表达 |
| --- | --- | --- |
| 边缘取图 | 机器人运动中连续采集图片 | 让检测图片稳定进入系统 |
| AI 检测 | 对图片进行模型推理 | 自动识别缺陷并输出结构化结果 |
| 人工复核 | 对 AI 结果进行确认 | 让关键判定有人工闭环 |
| 第三方接口 | 任务上传与结果查询 | 与产线和业务系统打通 |
| 工作流平台 | 任务、图片、结果统一管理 | 形成可追踪的质量流程 |
### 拍照节拍展示
对外展示时,可以用“每秒拍照数量”表达边缘取图能力,同时避免写成固定承诺。
| 模式 | 每秒拍照数量示意 | 适用场景 |
| --- | ---: | --- |
| 稳定检测模式 | 5-10 张/秒 | 常规外观、定位、尺寸检测 |
| 高速检测模式 | 10-20 张/秒 | 连续路径、多点位检测 |
| 高密度采集模式 | 20+ 张/秒 | 短路径密集取像、节拍优化展示 |
备注:实际拍照数量取决于机器人速度、相机曝光时间、光源响应、图像处理耗时和现场安全策略。
### 时序图展示
```text
机器人运动轨迹 ────────────────────────────────>
边缘飞拍触发 ▲ ▲ ▲ ▲ ▲
图1 图2 图3 图4 图5
AI 检测服务 检测 检测 检测 检测 检测
复检工作流 生成任务 → 图片审核 → 人工判定 → 结果回传
```
配套说明文案:
系统将取图、检测和复核组织在同一条业务时间线上。边缘端负责稳定采集,AI 服务负责自动识别,复检平台负责人工确认和结果流转,最终形成完整的质检闭环。
## 建议 PPT 结构
### 1. 封面
标题:智能视觉质检系统
副标题:从边缘取图到 AI 检测,再到人工复核与结果闭环
### 2. 行业痛点
标题:视觉检测难点不只在 AI 模型
要点:
- 图片来源分散,取图节拍难统一。
- AI 结果需要人工确认和业务留痕。
- 产线系统需要查询最终复检结果。
- 图片、模型结果、人工判断缺少统一工作流。
### 3. 系统总体架构
标题:三层协同,形成端到端质检链路
建议图形:
```text
边缘飞拍取图层
↓
AI 检测服务层
↓
人工复核与工作流层
↓
第三方系统 / 产线系统
```
### 4. 边缘取图能力
标题:机器人运动中连续采集检测图片
展示内容:
- 运动轨迹。
- 拍照触发点。
- 每秒拍照数量示意。
- 取图与检测位置绑定。
### 5. AI 检测能力
标题:接口化 AI 检测,支持多模型结果输出
展示内容:
- 图片输入。
- 模型推理。
- OK/NG 结果。
- 缺陷区域信息。
- 处理后标注图。
### 6. 人工复核能力
标题:AI 结果进入复检工作台
展示内容:
- 当前待复核任务。
- 原图与 AI 标注图对比。
- 人工 OK/NG 判定。
- 批量提交与历史查询。
### 7. 第三方接口与业务闭环
标题:与产线系统打通最终复检结果
展示内容:
- 第三方上传任务。
- 查询最新复检结果。
- 状态同步。
- 结果归档。
### 8. 典型应用场景
标题:适用于多类工业视觉质检场景
要点:
- 焊缝与边缘检测。
- PCB / 工件外观检测。
- 孔位与装配位置检测。
- 多点位连续取像。
- AI 初判 + 人工终判场景。
### 9. 收尾页
标题:让视觉检测从单点识别走向完整工作流
推荐短句:
智能视觉质检系统把边缘取图、AI 检测、人工复核和第三方对接整合为统一流程,让质量判断更高效、更清晰、更可追踪。
## 展会讲解词
这套智能视觉质检系统不是单独展示一个 AI 模型,也不是只展示一个拍照工具,而是展示从现场取图到最终复检结果的完整链路。
在边缘端,机器人通过飞拍方式在运动过程中采集检测图片,适合边缘段、路径型、多点位的工业检测场景。图片采集完成后,进入 AI 检测服务,由模型自动判断 OK/NG,并输出缺陷区域、标注图和结构化结果。
随后,这些图片和 AI 结果会进入复检工作流平台。操作员可以在界面中查看原图、AI 标注图和检测信息,完成最终人工判断。对于产线或第三方业务系统,平台提供任务上传和结果查询能力,使复检结果能够回到业务流程中。
因此,客户看到的是一整套系统能力:前端能稳定取图,中间能自动检测,上层能人工复核,外部系统还能查询最终结果。它解决的是工业视觉检测从“拍到图、看出缺陷”到“形成可追踪质量结论”的完整问题。
## 对外短标题
- 智能视觉质检系统
- 从边缘取图到复检闭环
- AI 检测 + 人工复核一体化
- 让视觉检测进入可追踪工作流
- 机器人飞拍取图,AI 自动识别,人工确认结果
- 面向工业产线的端到端视觉质量平台

View File

@@ -0,0 +1,657 @@
<!doctype html>
<html lang="zh-CN">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>状态页摇杆点动 Demo</title>
<style>
:root {
color-scheme: light;
--bg: #f5f7fb;
--surface: #ffffff;
--line: #d8dee9;
--text: #172033;
--muted: #5b667a;
--accent: #007c89;
--accent-dark: #005d67;
--steel: #2f3b4f;
--steel-soft: #e7ebf1;
--good: #12805c;
--warn: #b7791f;
}
* {
box-sizing: border-box;
}
body {
margin: 0;
min-height: 100vh;
background: var(--bg);
color: var(--text);
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
font-size: 15px;
letter-spacing: 0;
}
header {
border-bottom: 1px solid var(--line);
background: var(--surface);
}
.topbar {
display: flex;
align-items: center;
justify-content: space-between;
gap: 16px;
width: min(1180px, calc(100% - 32px));
margin: 0 auto;
padding: 18px 0;
}
h1 {
margin: 0;
font-size: 22px;
font-weight: 650;
}
.status-pill {
display: inline-flex;
align-items: center;
gap: 8px;
min-height: 36px;
padding: 0 12px;
border: 1px solid rgba(18, 128, 92, 0.28);
border-radius: 999px;
background: rgba(18, 128, 92, 0.08);
color: var(--good);
font-weight: 650;
}
.dot {
width: 10px;
height: 10px;
border-radius: 999px;
background: var(--good);
}
main {
width: min(1180px, calc(100% - 32px));
margin: 22px auto;
}
.summary {
display: grid;
grid-template-columns: repeat(4, minmax(0, 1fr));
gap: 12px;
margin-bottom: 16px;
}
.metric,
.panel {
border: 1px solid var(--line);
border-radius: 8px;
background: var(--surface);
}
.metric {
min-height: 86px;
padding: 14px;
}
.label {
color: var(--muted);
font-size: 13px;
}
.value {
margin-top: 8px;
overflow-wrap: anywhere;
font-size: 24px;
font-weight: 650;
}
.panel h2 {
margin: 0;
padding: 14px 16px;
border-bottom: 1px solid var(--line);
font-size: 16px;
font-weight: 650;
}
.jog-panel {
overflow: hidden;
}
.jog-body {
display: grid;
grid-template-columns: minmax(0, 1fr) minmax(0, 1fr);
gap: 18px;
padding: 16px;
}
.jog-toolbar {
display: grid;
grid-template-columns: repeat(2, minmax(180px, 1fr)) minmax(220px, 1fr);
gap: 12px;
padding: 16px;
border-bottom: 1px solid var(--line);
background: #fbfcfe;
}
.step-field {
display: grid;
gap: 6px;
color: var(--muted);
font-size: 13px;
}
.step-field input {
min-height: 38px;
width: 100%;
padding: 0 10px;
border: 1px solid var(--line);
border-radius: 6px;
background: #ffffff;
color: var(--text);
font: inherit;
}
.jog-readout {
display: flex;
align-items: center;
justify-content: flex-end;
min-height: 38px;
color: var(--muted);
font-family: Consolas, "Cascadia Mono", monospace;
text-align: right;
}
.stick-card {
min-width: 0;
border: 1px solid #ccd4df;
border-radius: 8px;
background: linear-gradient(180deg, #ffffff 0%, #f3f6fa 100%);
box-shadow: inset 0 1px 0 rgba(255, 255, 255, 0.9);
}
.stick-head {
display: flex;
align-items: center;
justify-content: space-between;
gap: 12px;
padding: 14px 16px 0;
}
.stick-title {
margin: 0;
font-size: 16px;
font-weight: 650;
}
.stick-unit {
color: var(--muted);
font-size: 13px;
font-family: Consolas, "Cascadia Mono", monospace;
}
.stick-console {
display: grid;
grid-template-columns: 72px 1fr 72px;
grid-template-rows: 72px 72px 72px;
gap: 8px;
width: min(100%, 344px);
margin: 18px auto 16px;
padding: 14px;
border: 1px solid #c2cad7;
border-radius: 999px;
background:
radial-gradient(circle at 50% 50%, #ffffff 0 26%, #e6ebf2 27% 48%, #cdd5e0 49% 50%, #edf1f6 51% 100%);
}
.axis-button {
min-width: 0;
min-height: 64px;
border: 1px solid #99a8ba;
border-radius: 999px;
background: linear-gradient(180deg, #ffffff 0%, #e5ebf2 100%);
color: var(--steel);
font: inherit;
font-weight: 750;
cursor: pointer;
touch-action: none;
user-select: none;
box-shadow: 0 2px 0 #b7c1ce;
}
.axis-button:hover,
.axis-button.active {
border-color: var(--accent);
background: linear-gradient(180deg, #10a4b3 0%, var(--accent) 100%);
color: #ffffff;
box-shadow: 0 2px 0 var(--accent-dark);
}
.axis-button:focus-visible {
outline: 3px solid rgba(0, 124, 137, 0.3);
outline-offset: 2px;
}
.axis-button.up {
grid-column: 2;
grid-row: 1;
}
.axis-button.left {
grid-column: 1;
grid-row: 2;
}
.axis-button.right {
grid-column: 3;
grid-row: 2;
}
.axis-button.down {
grid-column: 2;
grid-row: 3;
}
.center-stack {
display: grid;
grid-column: 2;
grid-row: 2;
grid-template-columns: 1fr 1fr;
gap: 6px;
place-items: center;
padding: 8px;
border-radius: 999px;
background: rgba(47, 59, 79, 0.08);
}
.mini-button {
min-width: 44px;
min-height: 44px;
border: 1px solid #99a8ba;
border-radius: 999px;
background: var(--surface);
color: var(--steel);
font: inherit;
font-weight: 750;
cursor: pointer;
touch-action: none;
user-select: none;
}
.mini-button:hover,
.mini-button.active {
border-color: var(--warn);
background: #b7791f;
color: #ffffff;
}
.hint-row {
display: grid;
grid-template-columns: repeat(3, minmax(0, 1fr));
gap: 8px;
padding: 0 16px 16px;
color: var(--muted);
font-size: 13px;
text-align: center;
}
.hint-row span {
min-height: 28px;
padding: 5px 8px;
border-radius: 999px;
background: var(--steel-soft);
white-space: nowrap;
}
.log-panel {
margin-top: 16px;
overflow: hidden;
}
.log-head {
display: flex;
align-items: center;
justify-content: space-between;
gap: 12px;
padding: 14px 16px;
border-bottom: 1px solid var(--line);
}
.log-head h2 {
margin: 0;
padding: 0;
border-bottom: 0;
}
.clear-button {
min-height: 34px;
padding: 0 12px;
border: 1px solid var(--line);
border-radius: 6px;
background: #ffffff;
color: var(--muted);
font: inherit;
cursor: pointer;
}
.clear-button:hover {
border-color: var(--accent);
color: var(--accent);
}
.request-log {
display: grid;
gap: 8px;
max-height: 300px;
margin: 0;
padding: 12px 16px 16px;
overflow: auto;
background: #fbfcfe;
font-family: Consolas, "Cascadia Mono", monospace;
font-size: 13px;
}
.log-entry {
display: grid;
grid-template-columns: 86px 96px minmax(0, 1fr);
gap: 10px;
align-items: start;
padding: 9px 10px;
border: 1px solid #e1e7ef;
border-radius: 6px;
background: #ffffff;
}
.log-entry:first-child {
border-color: rgba(0, 124, 137, 0.35);
background: rgba(0, 124, 137, 0.04);
}
.log-time,
.log-axis {
color: var(--muted);
white-space: nowrap;
}
.log-payload {
overflow-wrap: anywhere;
color: var(--text);
}
@media (max-width: 920px) {
.summary,
.jog-body,
.jog-toolbar {
grid-template-columns: 1fr;
}
.jog-readout {
justify-content: flex-start;
text-align: left;
}
}
@media (max-width: 520px) {
.topbar {
align-items: flex-start;
flex-direction: column;
}
.stick-console {
grid-template-columns: 60px 1fr 60px;
grid-template-rows: 60px 60px 60px;
padding: 10px;
}
.axis-button {
min-height: 56px;
}
.log-entry {
grid-template-columns: 1fr;
}
}
</style>
</head>
<body>
<header>
<div class="topbar">
<h1>Flyshot Replacement 状态监控</h1>
<div class="status-pill"><span class="dot"></span><span>Connected</span></div>
</div>
</header>
<main>
<div class="summary">
<div class="metric">
<div class="label">机器人</div>
<div class="value">FANUC</div>
</div>
<div class="metric">
<div class="label">速度倍率</div>
<div class="value">0.70</div>
</div>
<div class="metric">
<div class="label">运动中</div>
<div class="value">否</div>
</div>
<div class="metric">
<div class="label">TCP 位姿</div>
<div class="value" style="font-size: 16px;">120.0, 80.0, 320.0</div>
</div>
</div>
<section class="panel jog-panel">
<h2>直角坐标点动</h2>
<div class="jog-toolbar">
<label class="step-field" for="linear-step">
<span>平移步长 mm</span>
<input id="linear-step" type="number" min="0.1" max="10" step="0.1" value="1">
</label>
<label class="step-field" for="angular-step">
<span>姿态步长 deg</span>
<input id="angular-step" type="number" min="0.1" max="10" step="0.1" value="1">
</label>
<div id="readout" class="jog-readout">等待点动输入</div>
</div>
<div class="jog-body">
<article class="stick-card">
<div class="stick-head">
<h3 class="stick-title">XYZ 平移摇杆</h3>
<span class="stick-unit">mm</span>
</div>
<div class="stick-console" aria-label="XYZ 平移摇杆">
<button class="axis-button up" type="button" data-axis="y" data-direction="1">Y+</button>
<button class="axis-button left" type="button" data-axis="x" data-direction="-1">X-</button>
<div class="center-stack">
<button class="mini-button" type="button" data-axis="z" data-direction="1">Z+</button>
<button class="mini-button" type="button" data-axis="z" data-direction="-1">Z-</button>
</div>
<button class="axis-button right" type="button" data-axis="x" data-direction="1">X+</button>
<button class="axis-button down" type="button" data-axis="y" data-direction="-1">Y-</button>
</div>
<div class="hint-row">
<span>X 左右</span>
<span>Y 上下</span>
<span>Z 中心</span>
</div>
</article>
<article class="stick-card">
<div class="stick-head">
<h3 class="stick-title">WPR 姿态摇杆</h3>
<span class="stick-unit">deg</span>
</div>
<div class="stick-console" aria-label="WPR 姿态摇杆">
<button class="axis-button up" type="button" data-axis="p" data-direction="1">P+</button>
<button class="axis-button left" type="button" data-axis="w" data-direction="-1">W-</button>
<div class="center-stack">
<button class="mini-button" type="button" data-axis="r" data-direction="-1">R-</button>
<button class="mini-button" type="button" data-axis="r" data-direction="1">R+</button>
</div>
<button class="axis-button right" type="button" data-axis="w" data-direction="1">W+</button>
<button class="axis-button down" type="button" data-axis="p" data-direction="-1">P-</button>
</div>
<div class="hint-row">
<span>W 左右</span>
<span>P 上下</span>
<span>R 中心</span>
</div>
</article>
</div>
</section>
<section class="panel log-panel">
<div class="log-head">
<h2>请求日志</h2>
<button id="clear-log" class="clear-button" type="button">清空</button>
</div>
<div id="request-log" class="request-log" aria-live="polite">
<div class="log-entry">
<span class="log-time">--:--:--</span>
<span class="log-axis">等待输入</span>
<span class="log-payload">按住任意摇杆方向后,这里会记录每次模拟 MovePose 请求。</span>
</div>
</div>
</section>
</main>
<script>
const readout = document.getElementById("readout");
const buttons = Array.from(document.querySelectorAll("[data-axis]"));
const linearStepInput = document.getElementById("linear-step");
const angularStepInput = document.getElementById("angular-step");
const requestLog = document.getElementById("request-log");
const clearLog = document.getElementById("clear-log");
const axisIndexes = { x: 0, y: 1, z: 2, w: 3, p: 4, r: 5 };
const poseKeys = ["x", "y", "z", "w", "p", "r"];
const maxLogCount = 80;
let timer = null;
let activeButton = null;
let requestCount = 0;
let currentPose = {
x: 120.0,
y: 80.0,
z: 320.0,
w: 0.0,
p: 45.0,
r: 0.0
};
function labelFor(button) {
const axis = button.dataset.axis.toUpperCase();
const sign = Number(button.dataset.direction) > 0 ? "+" : "-";
return axis + sign;
}
function clampStep(input, min, max) {
const value = Number(input.value);
if (!Number.isFinite(value)) {
input.value = String(min);
return min;
}
const clamped = Math.min(Math.max(value, min), max);
input.value = String(clamped);
return clamped;
}
function stepFor(axis) {
return axis === "x" || axis === "y" || axis === "z"
? clampStep(linearStepInput, 0.1, 10)
: clampStep(angularStepInput, 0.1, 10);
}
function buildMovePoseBody(button) {
const axis = button.dataset.axis;
const direction = Number(button.dataset.direction);
const key = poseKeys[axisIndexes[axis]];
currentPose[key] = Number((currentPose[key] + direction * stepFor(axis)).toFixed(6));
return {
x: currentPose.x,
y: currentPose.y,
z: currentPose.z,
w: currentPose.w,
p: currentPose.p,
r: currentPose.r
};
}
function appendLog(button, body) {
requestCount += 1;
if (requestCount === 1) {
requestLog.replaceChildren();
}
const row = document.createElement("div");
row.className = "log-entry";
const time = document.createElement("span");
time.className = "log-time";
time.textContent = new Date().toLocaleTimeString("zh-CN", { hour12: false });
const axis = document.createElement("span");
axis.className = "log-axis";
axis.textContent = "#" + String(requestCount).padStart(3, "0") + " " + labelFor(button);
const payload = document.createElement("span");
payload.className = "log-payload";
payload.textContent = "POST /move_pose/ " + JSON.stringify(body);
row.append(time, axis, payload);
requestLog.prepend(row);
while (requestLog.children.length > maxLogCount) {
requestLog.lastElementChild.remove();
}
}
function tick(button) {
const body = buildMovePoseBody(button);
readout.textContent = "模拟发送: " + labelFor(button) + " -> " + JSON.stringify(body);
appendLog(button, body);
}
function stop() {
if (timer !== null) {
window.clearInterval(timer);
timer = null;
}
if (activeButton) {
activeButton.classList.remove("active");
activeButton = null;
}
}
buttons.forEach(function (button) {
button.addEventListener("pointerdown", function (event) {
event.preventDefault();
stop();
activeButton = button;
button.classList.add("active");
tick(button);
timer = window.setInterval(function () {
tick(button);
}, 250);
});
button.addEventListener("pointerup", stop);
button.addEventListener("pointerleave", stop);
button.addEventListener("pointercancel", stop);
});
clearLog.addEventListener("click", function () {
requestCount = 0;
requestLog.innerHTML = '<div class="log-entry"><span class="log-time">--:--:--</span><span class="log-axis">已清空</span><span class="log-payload">新的点动请求会继续记录在这里。</span></div>';
readout.textContent = "等待点动输入";
});
window.addEventListener("blur", stop);
</script>
</body>
</html>

View File

@@ -0,0 +1,55 @@
# Flyshot 跨平台混淆发布 Implementation Plan
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
**Goal:** 为全部 `Flyshot.*` 自研程序集建立依赖环境 Obfuscar 的跨平台框架依赖混淆发布流程。
**Architecture:** 使用一个 PowerShell 入口编排 `dotnet publish`、绝对路径 Obfuscar XML 生成、混淆执行、产物完整性检查和映射文件隔离。发布脚本不修改项目运行时代码,也不引入混淆器包依赖。
**Tech Stack:** PowerShell 7、.NET 8 SDK、Obfuscar.GlobalTool 2.2.50、ASP.NET Core 自包含发布。
---
### Task 1: 发布脚本契约测试
**Files:**
- Create: `tests/Publishing/Publish-Obfuscated.Tests.ps1`
- [ ] 编写测试夹具,创建包含八个 `Flyshot.*.dll` 和一个第三方 DLL 的临时发布目录。
- [ ] 调用尚不存在的配置生成入口,断言测试因缺少 `scripts/Publish-Obfuscated.ps1` 而失败。
- [ ] 保留测试入口,使后续实现可以在 `-GenerateConfigOnly` 模式下验证配置内容而不执行真实发布。
### Task 2: 双平台混淆发布脚本
**Files:**
- Create: `scripts/Publish-Obfuscated.ps1`
- [ ] 定义带完整注释的参数、静态程序集清单和辅助函数。
- [ ] 检查 PowerShell、`dotnet` 与 `obfuscar.console` 环境命令。
- [ ] 使用 `dotnet publish -c Release -r <rid> --self-contained true -p:PublishSingleFile=false` 生成原始产物。
- [ ] 为八个 `Flyshot.*.dll` 生成使用绝对路径的 Obfuscar XML。
- [ ] 执行 Obfuscar,并检查每个输入模块都有混淆输出。
- [ ] 将混淆 DLL/PDB 替换进发布目录,并将 `Mapping.txt` 隔离到 `outputs/obfuscation-maps/<rid>/`。
- [ ] 不传 RID,生成一份 Windows/Linux 共用的 .NET 8 框架依赖发布包。
### Task 3: 发布任务与文档
**Files:**
- Modify: `.vscode/tasks.json`
- Modify: `README.md`
- [ ] 将 VS Code `publish` 任务改为调用 PowerShell 混淆发布脚本。
- [ ] 在 README 发布章节记录环境安装、发布命令、输出目录和保护边界。
### Task 4: 验证
**Files:**
- Modify: `docs/superpowers/plans/2026-06-22-cross-platform-obfuscation.md`
- [ ] 运行脚本契约测试并确认通过。
- [ ] 运行 `dotnet test FlyshotReplacement.sln -c Release -v minimal`。
- [ ] 运行 `dotnet build FlyshotReplacement.sln -c Release --no-restore -v minimal`。
- [ ] 环境安装 Obfuscar 后执行 `pwsh -File scripts/Publish-Obfuscated.ps1`。
- [ ] 启动 Windows 混淆产物并请求 `/healthz`。
- [ ] 检查 Linux 混淆产物结构;存在 WSL 时启动并请求 `/healthz`。
- [ ] 检查 `git diff`,确认没有第三方程序集或映射文件进入部署包。

View File

@@ -0,0 +1,330 @@
# Rust ICSP Native Implementation Plan
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
**Goal:** 为现有 `ICspPlanner` 增加一套基于 Rust `cdylib` 的原生 ICSP 规划实现,并通过 C ABI + C# P/Invoke 完成可验证的对拍接入,同时不改变当前默认生产路径。
**Architecture:** 新增 `native/flyshot-native-planning` Rust 原生库承载 clamped-zero 三次样条与 ICSP 迭代核心;新增 `src/Flyshot.Native.Planning` 托管互操作项目负责库加载、数组编排、错误转换和结果复制。现有 `Flyshot.Core.Planning.ICspPlanner` 保持不变,`RustIcspPlanner` 仅作为显式新入口并在测试中与托管实现逐项对拍。
**Tech Stack:** Rust `cdylib`、C ABI、.NET 8、`LibraryImport`、`SafeHandle`、xUnit、PowerShell、Cargo
---
### Task 1: 建立原生与托管骨架
**Files:**
- Create: `native/flyshot-native-planning/Cargo.toml`
- Create: `native/flyshot-native-planning/src/lib.rs`
- Create: `native/flyshot-native-planning/src/error.rs`
- Create: `native/flyshot-native-planning/src/ffi.rs`
- Create: `native/flyshot-native-planning/src/icsp.rs`
- Create: `native/flyshot-native-planning/src/spline.rs`
- Create: `src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj`
- Create: `src/Flyshot.Native.Planning/NativeIcspMethods.cs`
- Create: `src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs`
- Create: `src/Flyshot.Native.Planning/NativePlanningException.cs`
- Create: `src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs`
- Modify: `FlyshotReplacement.sln`
- Modify: `tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj`
- [x] **Step 1: 先写托管侧失败测试,锁定缺少原生库时的报错边界**
```csharp
[Fact]
public void RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing()
{
var planner = new RustIcspPlanner(
options: NativeIcspPlannerOptions.Default,
libraryResolver: new NativePlanningLibraryResolver(_ => "missing-library"));
var exception = Assert.Throws<NativePlanningException>(() => planner.Plan(CreateSimpleRequest()));
Assert.Contains("missing-library", exception.Message);
}
```
- [x] **Step 2: 运行单测确认当前失败**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing -v minimal`
Expected: FAIL,提示 `RustIcspPlanner` 或互操作类型不存在
- [x] **Step 3: 创建 Rust `cdylib` 和托管互操作项目骨架**
```toml
[package]
name = "flyshot_planning"
version = "0.1.0"
edition = "2021"
[lib]
crate-type = ["cdylib"]
[profile.release]
lto = true
codegen-units = 1
strip = "symbols"
panic = "abort"
```
```xml
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
</ItemGroup>
</Project>
```
- [x] **Step 4: 添加最小库解析与异常包装实现**
```csharp
public sealed class NativePlanningException : Exception
{
public NativePlanningException(string message, int errorCode, Exception? innerException = null)
: base(message, innerException)
{
ErrorCodeValue = errorCode;
}
public int ErrorCodeValue { get; }
}
```
```csharp
public sealed class NativePlanningLibraryResolver
{
private readonly Func<RuntimeInformation, string> _resolveLibraryPath;
}
```
- [x] **Step 5: 重新运行单测,确认错误已变为清晰的原生库缺失异常**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing -v minimal`
Expected: PASS
### Task 2: 先做样条基础能力
**Files:**
- Modify: `native/flyshot-native-planning/src/spline.rs`
- Modify: `native/flyshot-native-planning/src/lib.rs`
- Create: `native/flyshot-native-planning/tests/spline_tests.rs`
- [x] **Step 1: 先写 Rust 失败测试,锁定 clamped-zero 样条边界和值/导数接口**
```rust
#[test]
fn clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity() {
let times = [0.0, 1.0, 2.0, 3.0];
let values = [0.0, 1.0, 0.5, 2.0];
let spline = CubicSpline1D::new_clamped_zero(&times, &values).unwrap();
assert_abs_diff_eq!(spline.position(0.0), 0.0, epsilon = 1e-12);
assert_abs_diff_eq!(spline.position(3.0), 2.0, epsilon = 1e-12);
assert_abs_diff_eq!(spline.velocity(0.0), 0.0, epsilon = 1e-12);
assert_abs_diff_eq!(spline.velocity(3.0), 0.0, epsilon = 1e-12);
}
```
- [x] **Step 2: 运行 Rust 测试确认失败**
Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml spline_tests::clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity -- --exact`
Expected: FAIL,提示样条类型或构造函数不存在
- [x] **Step 3: 实现三对角求解、样条系数和每段导数峰值接口**
```rust
pub struct CubicSpline1D {
knots: Vec<f64>,
coefficients: Vec<[f64; 4]>,
}
impl CubicSpline1D {
pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result<Self, NativePlanningError> { /* ... */ }
pub fn position(&self, time: f64) -> f64 { /* ... */ }
pub fn velocity(&self, time: f64) -> f64 { /* ... */ }
pub fn acceleration(&self, time: f64) -> f64 { /* ... */ }
pub fn jerk(&self, time: f64) -> f64 { /* ... */ }
pub fn segment_peak_derivatives(&self, segment_index: usize) -> [f64; 3] { /* ... */ }
}
```
- [x] **Step 4: 重新运行 Rust 样条测试**
Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml spline_tests::clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity -- --exact`
Expected: PASS
### Task 3: 实现纯 Rust ICSP 核心
**Files:**
- Modify: `native/flyshot-native-planning/src/icsp.rs`
- Create: `native/flyshot-native-planning/tests/icsp_tests.rs`
- [x] **Step 1: 先写 Rust 失败测试,锁定初始段时长、scale 计算和未收敛路径**
```rust
#[test]
fn icsp_returns_same_iteration_count_as_managed_baseline_fixture() {
let input = test_fixture_three_segment_case();
let result = plan_icsp(&input).unwrap();
assert_eq!(result.iterations, 2);
assert_abs_diff_eq!(result.threshold, 0.0, epsilon = 1e-12);
}
```
- [x] **Step 2: 运行 Rust ICSP 测试确认失败**
Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml icsp_tests::icsp_returns_same_iteration_count_as_managed_baseline_fixture -- --exact`
Expected: FAIL,提示 `plan_icsp` 或结果结构不存在
- [x] **Step 3: 实现最小 ICSP 纯算法结构,逐项复刻 C# 收敛语义**
```rust
pub struct IcspInput<'a> {
pub waypoints: &'a [f64],
pub waypoint_count: usize,
pub dof: usize,
pub velocity_limits: &'a [f64],
pub acceleration_limits: &'a [f64],
pub jerk_limits: &'a [f64],
pub options: IcspOptions,
}
pub fn plan_icsp(input: &IcspInput<'_>) -> Result<IcspPlanResult, NativePlanningError> { /* ... */ }
```
- [x] **Step 4: 重新运行 Rust ICSP 测试**
Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml icsp_tests::icsp_returns_same_iteration_count_as_managed_baseline_fixture -- --exact`
Expected: PASS
### Task 4: 接通 C ABI 和托管结果复制
**Files:**
- Modify: `native/flyshot-native-planning/src/ffi.rs`
- Modify: `native/flyshot-native-planning/src/error.rs`
- Modify: `src/Flyshot.Native.Planning/NativeIcspMethods.cs`
- Modify: `src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs`
- Create: `src/Flyshot.Native.Planning/NativeIcspInteropModels.cs`
- Create: `src/Flyshot.Native.Planning/RustIcspPlanner.cs`
- Create: `tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs`
- [x] **Step 1: 先写托管失败测试,锁定数组布局、结果复制和释放路径**
```csharp
[Fact]
public void RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult()
{
var planner = CreateRustPlannerAgainstBuiltNativeLibrary();
var result = planner.Plan(CreateSimpleRequest());
Assert.Equal(PlanningMethod.Icsp, result.Method);
Assert.Equal(result.PlannedWaypointCount, result.WaypointTimes.Count);
}
```
- [x] **Step 2: 运行单测确认失败**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult -v minimal`
Expected: FAIL,提示 P/Invoke 签名、结果结构或释放逻辑未实现
- [x] **Step 3: 实现 C ABI 入口、线程局部错误文本和 `SafeHandle` 释放**
```rust
#[no_mangle]
pub extern "C" fn flyshot_icsp_plan(/* ... */) -> i32 { /* ... */ }
#[no_mangle]
pub extern "C" fn flyshot_icsp_result_free(result: *mut FlyshotIcspResult) { /* ... */ }
#[no_mangle]
pub extern "C" fn flyshot_last_error_message() -> *const c_char { /* ... */ }
```
```csharp
[LibraryImport(NativeLibraryName, EntryPoint = "flyshot_icsp_plan")]
internal static unsafe partial int FlyshotIcspPlan(/* ... */);
```
- [x] **Step 4: 重新运行托管互操作测试**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult -v minimal`
Expected: PASS
### Task 5: 增加 C#/Rust 对拍测试和构建复制脚本
**Files:**
- Modify: `tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs`
- Create: `tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs`
- Create: `build/Copy-NativePlanningArtifacts.ps1`
- Modify: `README.md`
- [x] **Step 1: 先写失败测试,锁定 C# 与 Rust 在基础样本上的逐项一致**
```csharp
[Fact]
public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture()
{
var request = CreateCompatibilityRequest();
var managed = new ICspPlanner().Plan(request);
var native = CreateRustPlannerAgainstBuiltNativeLibrary().Plan(request);
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
}
```
- [x] **Step 2: 运行对拍测试确认失败**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture -v minimal`
Expected: FAIL,提示数值未对齐或原生库复制失败
- [x] **Step 3: 补齐原生库复制脚本、测试输出复制规则和对拍辅助断言**
```powershell
param(
[string]$Configuration = "Release"
)
cargo build --manifest-path native/flyshot-native-planning/Cargo.toml --release
Copy-Item native/flyshot-native-planning/target/release/flyshot_planning.dll tests/Flyshot.Core.Tests/bin/$Configuration/net8.0/ -Force
```
- [x] **Step 4: 重新运行对拍测试**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture -v minimal`
Expected: PASS
### Task 6: 解决方案级验证
**Files:**
- Modify: `README.md`
- Modify: `docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md`
- [x] **Step 1: 运行 Rust 全量测试**
Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml`
Expected: PASS
- [ ] **Step 2: 运行 Core 测试**
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal`
Expected: PASS
当前验证:新增 Rust 相关测试全部通过;Core 全套仍保留实施前已存在的两项
`FlyshotExecutionSendSequenceBuilder` speed-ratio 失败,本计划未修改该行为。
- [x] **Step 3: 运行解决方案构建**
Run: `dotnet build FlyshotReplacement.sln -v minimal`
Expected: PASS
- [x] **Step 4: 更新文档,记录本地构建、库复制和当前上线边界**
```markdown
- Rust 规划器当前仅显式使用,不替换默认 `ICspPlanner`
- Windows 需要 `flyshot_planning.dll`
- Linux 需要 `libflyshot_planning.so`
```

View File

@@ -0,0 +1,32 @@
# 真机优先与离线飞拍规划改造计划
## 已实施目标
- 启动及 `/init_mpc_robot` 固定执行“加载模型 → 连接真机 → `EnableRobot(8)`”。
- 模型加载成功但连接或使能失败时进入 `OfflineReady`,HTTP 宿主继续运行。
- 飞拍资产固定保存到 `Config/RobotConfig.json`,上传、列出、删除不依赖模型或真机状态。
- `/save_traj_info/` 只要求模型已加载,返回完整最终发送点列、触发绑定和导出文件绝对路径。
- `/execute_flyshot/` 仍要求 `RealReady`,离线规划不会伪造机器人连接或使能。
- `/debug` 提供模型加载、轨迹上传/选择、离线生成、分页点列和导出路径查看。
## 状态约定
- `LoadingModel`:正在加载机器人模型。
- `Connecting`:模型已加载,正在连接或使能真机。
- `RealReady`:真机已连接且使能,允许真实运动和 IO。
- `OfflineReady`:模型已加载但真机不可用,允许离线规划。
- `ModelLoadFailed`:机器人模型不可用,禁止规划和执行。
## HTTP 约定
- 保留 `/setup_robot/`,仅加载模型,不连接、不使能,也不清空飞拍资产。
- 新增 `/init_mpc_robot`,请求体只包含 `robot_name`、`robot_ip`。
- 删除 `/init_mpc_robt`、`/connect_server/`、版本查询、环境文件、TCP 显示、`is_setup` 和仿真控制器切换路由。
- `/upload_flyshot/` 在上传阶段只校验资产结构和有限数值;自由度与模型限制在规划阶段校验。
- `/save_traj_info/` 返回规划摘要、关键/稠密轨迹、最终发送点、ShotEvents、触发绑定和导出路径。
## 验证
- 核心测试覆盖离线资产生命周期、模型重载保留资产、连接失败降级和完整离线规划结果。
- 集成测试覆盖删除路由 404、新初始化接口、状态快照和 Debug 离线规划页面。
- 最终执行核心测试、集成测试和解决方案构建。

View File

@@ -0,0 +1,44 @@
# Flyshot 跨平台混淆发布设计
## 目标
为全部 `Flyshot.*` 自研程序集增加统一的 Obfuscar 混淆发布流程,生成一份 Windows 与 Linux 共用的 .NET 8 框架依赖部署包。混淆流程必须显式失败,不能在工具缺失或混淆失败时留下看似成功的未保护发布包。
## 工具与环境边界
- 使用环境中安装的 Obfuscar 全局工具,命令名为 `obfuscar.console`。
- 仓库不增加 Obfuscar NuGet `PackageReference`,也不增加 `.config/dotnet-tools.json`。
- 构建环境需预先执行 `dotnet tool install --global Obfuscar.GlobalTool --version 2.2.50`。
- 发布脚本只依赖 PowerShell 7、.NET 8 SDK 和环境中的 Obfuscar。
## 发布结构
发布脚本不接受运行平台或 RID,固定执行一次框架依赖发布:
1. 使用 `dotnet publish` 生成自包含、多文件的原始发布目录。
2. 动态生成包含绝对路径的 Obfuscar XML 配置。
3. 校验当前八个核心程序集不可缺失,并将发布目录中的全部 `Flyshot.*.dll` 加入混淆模块列表。
4. Obfuscar 输出到独立临时目录。
5. 仅在混淆成功且输出程序集完整时,用混淆程序集替换原始发布目录中的对应文件。
6. 将映射文件移入不随部署包交付的 `outputs/obfuscation-maps/<rid>/`。
7. 最终部署包输出到 `outputs/obfuscated/framework-dependent/`。
## 混淆边界
- `KeepPublicApi=true`:保留公共 API 名称,保护 ASP.NET Core 控制器发现、配置绑定、DI、JSON DTO、Swagger 和程序集间调用。
- `HidePrivateApi=true`:重命名内部类型、私有方法和字段。
- `RenameProperties=false`、`RenameEvents=false`:避免配置绑定、JSON 序列化和框架约定受属性或事件重命名影响。
- 不处理 `Microsoft.*`、`System.*`、NLog、Swashbuckle 等第三方程序集。
- 所有 `Flyshot.*.dll` 必须被发现并加入配置;缺少任何当前核心程序集都视为发布失败,未来新增模块会自动纳入。
## 验证
- 脚本契约测试验证:RID 列表、环境工具缺失失败、全部自研程序集进入配置、第三方程序集不进入配置、映射文件不进入部署目录。
- 执行现有解决方案测试与 Release 构建。
- 真实执行框架依赖发布及混淆,并分别在 Windows 与 Linux 的 .NET 8 运行时上启动。
- Windows 包实际启动并请求 `/healthz`。
- Linux 包至少验证 ELF 启动文件、依赖文件和全部混淆程序集完整;环境存在 WSL 时再实际启动并请求 `/healthz`。
## 风险与限制
Obfuscar 能显著提高普通反编译的阅读成本,但不能绝对阻止有经验的逆向分析。保留公共 API 是当前 ASP.NET Core 稳定运行所需的安全边界;若未来需要更强的控制流混淆、字符串加密或防篡改,应评估商业混淆器。

View File

@@ -0,0 +1,271 @@
# Rust ICSP 原生规划核心设计
## 1. 目标
在不改变现有上层业务接口的前提下,使用 Rust 重新实现当前 C# 版本的 ICSP 和 clamped-zero 三次样条算法,并通过稳定的 C ABI 供 .NET 8 调用。
第一阶段必须实现:
- Rust `cdylib` 中的 clamped-zero 三次样条系数求解。
- 样条位置求值和每段一至三阶导数绝对值峰值解析计算。
- ICSP 初始分段时长、迭代缩放、最优结果保存和最终收敛判定。
- C# P/Invoke 适配层。
- Rust 与现有 C# 算法的自动对拍测试。
- Windows x64 和 Linux x64 原生库加载边界。
第一阶段不实现:
- `SelfAdaptIcspPlanner` 的补中点逻辑。
- 轨迹稠密采样、J519 发送采样和触发绑定。
- 正运动学、MoveJoint 和 MovePose。
- FANUC 协议、Socket 和运行时状态机。
- 删除或替换现有 C# ICSP 实现。
## 2. 总体架构
新增原生算法模块和 C# 适配模块:
```text
ControllerClientTrajectoryOrchestrator
|
ICSP 规划接口
/ \
C# ICspPlanner RustIcspPlanner
|
Flyshot.Native.Interop
|
C ABI
|
flyshot_planning
Rust cdylib
```
现有 `ICspPlanner` 保持不变,继续作为行为基准和回退实现。Rust 规划器返回的数据仍由 C# 组装成现有 `PlannedTrajectory`,因此领域层、兼容层和运行时不感知原生对象。
## 3. Rust 模块边界
原生模块建议位于:
```text
native/
flyshot-native-planning/
Cargo.toml
src/
lib.rs
error.rs
ffi.rs
icsp.rs
spline.rs
```
各文件职责:
- `spline.rs`:三对角方程求解、样条系数、位置求值、解析导数峰值。
- `icsp.rs`:纯 Rust ICSP 输入、迭代规划和结果模型。
- `ffi.rs`:C ABI 数据结构、指针校验、结果分配与释放。
- `error.rs`:原生错误分类和 UTF-8 错误信息。
- `lib.rs`:只导出稳定 ABI,不导出内部算法符号。
Rust 内部算法不依赖 .NET 类型、JSON、日志框架、网络或文件系统。
## 4. C ABI 设计
ABI 使用固定宽度整数、C 布局结构和连续 `double` 数组。路点输入采用行优先扁平布局:
```text
[waypoint0_joint0, waypoint0_joint1, ...,
waypoint1_joint0, waypoint1_joint1, ...]
```
主要入口:
```c
int32_t flyshot_icsp_plan(
const double* waypoints,
int32_t waypoint_count,
int32_t dof,
const double* velocity_limits,
const double* acceleration_limits,
const double* jerk_limits,
const FlyshotIcspOptions* options,
FlyshotIcspResult** result);
void flyshot_icsp_result_free(FlyshotIcspResult* result);
const char* flyshot_last_error_message(void);
```
`FlyshotIcspOptions` 包含:
- `threshold`
- `max_iterations`
- `enforce_final_scale`
- `final_scale_tolerance`
`FlyshotIcspResult` 包含:
- `waypoint_times` 及长度
- `segment_durations` 及长度
- `segment_scales` 及长度
- `iterations`
- `threshold`
调用成功返回 `0`。输入非法、数值计算失败、未收敛和内部异常使用不同错误码。错误文本由线程局部存储保存,只保证在当前线程下一次原生调用前有效。
所有结果内存都由 Rust 分配,并且只能通过 `flyshot_icsp_result_free` 释放。C# 不直接释放 Rust 分配的数组。
所有可预期失败必须在 FFI 内转换为错误码,FFI 路径不得依赖 panic 表达业务错误。Debug 和测试构建使用 `catch_unwind` 检测遗漏的 panic;Release 使用 `panic=abort`,此时 `catch_unwind` 不能恢复 panic,因此实现必须通过边界校验避免索引越界、空指针解引用和显式 `unwrap`。任何构建模式都不得让 Rust unwind 穿过 C ABI。
## 5. C# 适配层
新增一个只负责原生互操作的 C# 项目,建议命名为:
```text
src/Flyshot.Native.Planning/
```
包含:
- `NativeIcspMethods`:使用 `LibraryImport` 声明 C ABI。
- `NativeIcspSafeHandle`:封装 Rust 结果指针和释放函数。
- `RustIcspPlanner`:把 `TrajectoryRequest` 转换为连续数组,再把结果转换为 `PlannedTrajectory`。
- `NativePlanningLibraryResolver`:按当前操作系统和体系结构解析原生库。
- `NativePlanningException`:携带原生错误码和错误文本。
原生库逻辑名称固定为 `flyshot_planning`:
- Windows:`flyshot_planning.dll`
- Linux:`libflyshot_planning.so`
C# 适配层负责:
- 检查输入自由度和数组长度。
- 固定数组,避免调用期间被 GC 移动。
- 将 Rust 返回数据复制到托管数组。
- 无论成功或失败都正确释放结果句柄。
- 将原生错误转换为包含诊断信息的 C# 异常。
`RustIcspPlanner` 的构造参数和 `Plan` 行为应与 `ICspPlanner` 对齐,但第一阶段不自动替换现有依赖注入注册。
## 6. 数值一致性
Rust 实现必须逐项复刻当前 C# 行为:
- 初始段时长使用相邻路点的关节空间欧氏距离。
- 使用 clamped-zero 边界条件求三次样条。
- 每轮解析计算各段速度、加速度和 jerk 峰值。
- 分段倍率分别使用一次方、平方根和立方根。
- 收敛指标为 `sum(abs(scale - 1))`。
- 始终保存历史最小收敛指标对应的结果。
- 迭代范围与当前 C# 保持一致,即包含第 `max_iterations` 轮。
- 最终按 `global_scale > 1 + tolerance` 判定是否失败。
第一阶段采用以下对拍容差:
- 路点时间:绝对误差不超过 `1e-9`。
- 段时长:绝对误差不超过 `1e-9`。
- 段缩放:绝对误差不超过 `1e-9`。
- 最终 threshold:绝对误差不超过 `1e-9`。
- 迭代次数:必须完全一致。
如果真实样本因浮点指令或编译器优化无法满足 `1e-9`,必须先定位首个分歧计算步骤,再基于证据调整容差,不直接放宽整个测试。
## 7. 测试策略
遵循 TDD,先增加失败测试,再实现 Rust 和 C# 适配。
测试分四层:
1. Rust 单元测试
- 三次样条边界值。
- 一至三阶导数。
- 解析峰值。
- ICSP 收敛与不收敛。
- 非法指针之外的纯算法输入校验。
2. Rust FFI 测试
- 正常调用和结果释放。
- 空指针、错误维度、非有限数。
- 错误码与错误文本。
- 重复释放由 C# `SafeHandle` 防止,不把未定义行为包装成可接受用法。
3. C# 互操作测试
- 原生库加载。
- 托管数组到 C ABI 的转换。
- 结果复制和释放。
- 原生错误到 C# 异常的转换。
4. C#/Rust 对拍测试
- 仓库中的稳定规划样本。
- 当前 `PlanningCompatibilityTests` 使用的基础输入。
- 多自由度、多路点和极限参数。
- `maxIterations=0` 的未收敛路径。
- 至少一个父目录真实样本固化后的 golden case。
对拍测试必须同时调用现有 `ICspPlanner` 和 `RustIcspPlanner`,不能把预期值只写成 Rust 自身输出。
## 8. 构建与发布
本地构建脚本负责:
- 执行 `cargo build --release`。
- 根据目标平台复制原生库到 C# 测试和发布输出目录。
- Windows x64 构建 Rust `x86_64-pc-windows-msvc`。
- Linux x64 构建 Rust `x86_64-unknown-linux-gnu`。
第一阶段不要求在 Windows 主机上完成 Linux 交叉编译;Linux 原生库可以在 Linux 构建环境生成。仓库必须提供对应命令和 CI/离线构建说明。
发布时原生库与托管程序集放在同一部署目录。现有 Obfuscar 继续处理 `Flyshot.*.dll` 托管程序集,不处理 Rust 原生库。
Rust Release 配置启用:
```toml
[profile.release]
lto = true
codegen-units = 1
strip = "symbols"
panic = "abort"
```
## 9. 回退与上线边界
第一阶段只提供显式的 Rust 规划器,不改变生产默认算法。完成全部对拍和跨平台验证后,再单独设计配置切换:
```text
Planning:Backend = Managed | Rust
```
原生库缺失、架构不匹配或加载失败时,显式选择 Rust 后应返回清晰错误,不静默回退。静默回退会掩盖部署缺陷,也无法确认现场实际使用的是哪套算法。
保留 C# 实现至少经过一个完整现场验证周期。后续是否删除托管算法,需要独立评审,不属于本设计范围。
## 10. 验收标准
- Rust 单元测试全部通过。
- C# 原生互操作测试全部通过。
- 选定样本的 C#/Rust 对拍全部通过。
- `dotnet test` 和解决方案构建通过。
- Windows x64 实际加载 `.dll` 并完成规划。
- Linux x64 实际加载 `.so` 并完成规划。
- 缺少原生库时能得到明确且可诊断的错误。
- 现有 C# `ICspPlanner` 和上层运行链路行为不变。
## 11. 第一阶段落地状态(2026-06-23)
当前已落地:
- `native/flyshot-native-planning` Rust `cdylib`,包含 clamped-zero 样条、
一至三阶导数解析峰值、ICSP 迭代和稳定 C ABI。
- `src/Flyshot.Native.Planning` 托管互操作项目,包含显式平台解析、
`LibraryImport`、`SafeHandle`、错误码转换和结果复制。
- Rust 样条、ICSP、FFI 测试,以及 C#/Rust `1e-9` 自动对拍测试。
- `build/Copy-NativePlanningArtifacts.ps1`,用于初始化 Windows MSVC 环境、
构建原生库并复制到测试或发布目录。
- Windows x64 已实际生成并加载 `flyshot_planning.dll` 完成规划。
当前上线边界保持不变:
- 默认生产路径继续使用 C# `ICspPlanner`。
- Rust 规划器只允许显式选择,库缺失、路径不一致或架构不支持时返回明确错误。
- Linux x64 的源码、库名和构建脚本边界已落地,但 `.so` 的实际加载验证必须在
Linux x64 构建环境执行后才能标记完成。

View File

@@ -0,0 +1,151 @@
# UTTC_MS11 跳过 flyshot jerk 阻断的结论记录
## 背景
2026-05-11 对 `UTTC_MS11` 做了两轮独立 `ConfigRoot` 重新生成与对比:
1. 保持 flyshot 离散校验中的 `vel/acc/jerk` 全部生效。
2. 临时只对 flyshot 链路跳过 `jerk` 阻断,仍保留速度与加速度校验。
两轮都使用当前程序重新生成:
- `POST /set_speedRatio/`
- `POST /save_traj_info/`
并与以下旧抓包侧结果做对比:
- `../Rvbust/uttc-20260428/2026042802-1_joint_segments/segment_02/JointDetialTraj.txt`
- `../Rvbust/uttc-20260428/2026042802-0.5_joint_segments/segment_02/JointDetialTraj.txt`
- `../Rvbust/uttc-20260428/1倍速度 角度坐标点/真实轨迹JointDetialTraj.txt`
## 直接结论
对当前 `UTTC_MS11` 现场样本,flyshot 链路若继续用离散 `jerk` 作为阻断条件,会触发大量时间轴 stretch,使新结果远慢于旧抓包。
当 flyshot 链路只保留速度、加速度校验并跳过 `jerk` 阻断后:
- 新 `1.0x` 实发时长从 `16.576s` 回落到 `7.552s`
- 新 `0.5x` 实发时长从 `33.152s` 回落到 `15.104s`
- 与旧抓包主运动段相比,只剩很小的时长差异,可接受并忽略
因此,当前项目对 `UTTC_MS11` 的实际结论定为:
- flyshot 链路临时不再把离散 `jerk` 作为阻断条件
- 仍保留速度、加速度校验
- `jerk` 诊断文件继续保留,用于分析,不再作为该路径是否可运行的唯一否决条件
## 证据
### 1. 开启 jerk 阻断时的现象
重新生成结果目录:
- `../temp/regen-20260511-110950/`
关键结果:
- `1.0x ActualSendTiming.txt`:`2073` 行,`send_duration=16.576s`
- `0.5x ActualSendTiming.txt`:`4145` 行,`send_duration=33.152s`
对比旧抓包主段:
- 旧 `1.0x`:`7.408109s`
- 旧 `0.5x`:`14.808342s`
可见新结果明显被拉长。
### 2. 触发 stretch 的第一条失败证据
日志文件:
- `../temp/regen-20260511-110950/host.log`
第一条离散失败不是加速度,而是开头窗口的 `Joint1 Jerk`:
- `轨迹 UTTC_MS11 第 3 行 Joint1 Jerk超限`
- `time=0.008000->0.016000s`
- `actual=-1732.421875`
- `limit=91.698044`
- `ratio=18.8927`
后续 `CreateLimitCompliantDenseTrajectory(...)` 按 `1.01` 倍持续拉长时间轴,直到通过离散校验,最终把总时长推到约 `16.57s`。
### 3. 只跳过 flyshot jerk 阻断后的结果
重新生成结果目录:
- `../temp/regen-nojerk-20260511-113258/`
关键结果:
- `1.0x ActualSendTiming.txt`:`945` 行,`send_duration=7.552s`
- `0.5x ActualSendTiming.txt`:`1889` 行,`send_duration=15.104s`
与旧抓包主段对比:
| 项目 | 旧抓包时长 | 新结果时长 | 差值 |
| --- | ---: | ---: | ---: |
| `1.0x` | `7.408109s` | `7.552s` | `0.143891s` |
| `0.5x` | `14.808342s` | `15.104s` | `0.295658s` |
该差异已经很小,当前按“可忽略”处理。
## 轨迹误差变化
在同一套对比脚本口径下:
### 开启 jerk 阻断时
- `JointDetialTraj` 新 vs 旧真实轨迹:全局 RMS 约 `46.664 deg`
- `ActualSendJointTraj 1.0x` 新 vs 旧抓包:全局 RMS 约 `46.665 deg`
- `ActualSendJointTraj 0.5x` 新 vs 旧抓包:全局 RMS 约 `46.667 deg`
### 跳过 flyshot jerk 阻断后
- `JointDetialTraj` 新 vs 旧真实轨迹:全局 RMS 约 `8.411 deg`
- `ActualSendJointTraj 1.0x` 新 vs 旧抓包:全局 RMS 约 `8.412 deg`
- `ActualSendJointTraj 0.5x` 新 vs 旧抓包:全局 RMS 约 `8.412 deg`
说明这次差异收敛主要来自去掉 flyshot jerk 阻断后的 stretch,而不是 `speed_ratio` 链路本身被重新设计。
## 对 jerk 的解释
这次调整不等于“现场 jerk 完全合规”,而是明确区分:
1. `jerk` 仍然继续计算和导出
2. `jerk` 仍然可作为诊断指标
3. 但对当前 `UTTC_MS11` flyshot 现场样本,不再把 `jerk` 作为阻断执行的唯一判据
重新生成后的分析文件仍显示:
- 加速度 ratio 明显在限值内
- `jerk` ratio 依然可能大于 `1`
这与旧抓包整理结果的观察一致:当前现场样本里,“加速度合规但跃度超限”是可接受现象,不能单凭 jerk 超限就否决 flyshot 轨迹。
## 当前代码决策范围
本次代码调整范围刻意收窄为:
- 只对 flyshot 链路关闭 `jerk` 阻断
- 保留 flyshot 的速度、加速度校验
- 不修改 `move_joint` 的 jerk 约束逻辑
对应入口包括:
- `ControllerClientTrajectoryOrchestrator.CreateLimitCompliantDenseTrajectory(...)`
- `FlyshotExecutionSendSequenceBuilder.Build(...)`
- `FlyshotTrajectoryArtifactWriter.WriteActualSendArtifacts(...)`
`move_joint` 仍保持原先的 jerk 受限实现,不共享这次豁免策略。
## 当前验收判断
对 `UTTC_MS11`:
- 只保留 flyshot 速度/加速度校验
- 跳过 flyshot jerk 阻断
- 新 `1.0x / 0.5x` 与旧抓包主段的剩余时长差异已经很小
- 当前判断:这点差异可以忽略
后续除非现场再次提供新的反证,否则本结论作为当前项目实现与验收的记录保留。

View File

@@ -1,6 +1,6 @@
{
"sdk": {
"version": "8.0.420",
"rollForward": "disable"
"version": "8.0.422",
"rollForward": "latestPatch"
}
}

View File

@@ -0,0 +1,7 @@
# This file is automatically @generated by Cargo.
# It is not intended for manual editing.
version = 4
[[package]]
name = "flyshot_planning"
version = "0.1.0"

View File

@@ -0,0 +1,13 @@
[package]
name = "flyshot_planning"
version = "0.1.0"
edition = "2021"
[lib]
crate-type = ["cdylib"]
[profile.release]
lto = true
codegen-units = 1
strip = "symbols"
panic = "abort"

View File

@@ -0,0 +1,33 @@
//! 原生规划错误定义。
use std::fmt::{Display, Formatter};
/// 表示 Rust 规划核心中可预期、可转换为 C ABI 错误码的失败。
#[derive(Debug, Clone, PartialEq)]
pub enum NativePlanningError {
/// 表示输入参数不满足算法前置条件。
InvalidInput(String),
/// 表示数值计算无法继续。
NumericalFailure(String),
/// 表示规划迭代结束后仍未满足约束。
NotConverged(String),
}
impl Display for NativePlanningError {
/// 将错误格式化为可供托管侧诊断的中文文本。
///
/// # 参数
/// - `formatter`: 接收格式化文本的输出缓冲区。
///
/// # 返回值
/// 格式化成功时返回 `Ok(())`,否则返回底层格式化错误。
fn fmt(&self, formatter: &mut Formatter<'_>) -> std::fmt::Result {
match self {
Self::InvalidInput(message) => write!(formatter, "输入非法:{message}"),
Self::NumericalFailure(message) => write!(formatter, "数值计算失败:{message}"),
Self::NotConverged(message) => write!(formatter, "ICSP 未收敛:{message}"),
}
}
}
impl std::error::Error for NativePlanningError {}

View File

@@ -0,0 +1,436 @@
//! C ABI 边界定义。
use std::cell::RefCell;
use std::ffi::{c_char, CString};
use std::panic::{catch_unwind, AssertUnwindSafe};
use std::ptr;
use std::slice;
use crate::error::NativePlanningError;
use crate::icsp::{plan_icsp, IcspInput, IcspOptions};
/// 原生调用成功时返回的固定错误码。
pub const FLYSHOT_SUCCESS: i32 = 0;
/// 输入指针、维度或数值非法时返回的错误码。
pub const FLYSHOT_ERROR_INVALID_INPUT: i32 = 1;
/// 样条或 ICSP 数值计算无法继续时返回的错误码。
pub const FLYSHOT_ERROR_NUMERICAL_FAILURE: i32 = 2;
/// 迭代完成但最终 scale 仍超限时返回的错误码。
pub const FLYSHOT_ERROR_NOT_CONVERGED: i32 = 3;
/// FFI 捕获到未预期 panic 时返回的错误码。
pub const FLYSHOT_ERROR_INTERNAL: i32 = 4;
thread_local! {
/// 保存当前线程最近一次原生错误文本,指针仅在下一次原生调用前有效。
static LAST_ERROR_MESSAGE: RefCell<CString> = RefCell::new(CString::default());
}
/// 表示 C ABI 输入的 ICSP 迭代选项。
#[repr(C)]
#[derive(Debug, Clone, Copy)]
pub struct FlyshotIcspOptions {
/// 收敛指标停止阈值。
pub threshold: f64,
/// 最大迭代索引,必须为非负数。
pub max_iterations: i32,
/// 非零表示启用最终 scale 检查。
pub enforce_final_scale: i32,
/// 最终 scale 允许的非负容差。
pub final_scale_tolerance: f64,
}
impl Default for FlyshotIcspOptions {
/// 返回与托管 ICSP 默认行为一致的 C ABI 选项。
///
/// # 返回值
/// 可直接传入 `flyshot_icsp_plan` 的默认选项。
fn default() -> Self {
let options = IcspOptions::default();
Self {
threshold: options.threshold,
max_iterations: options.max_iterations as i32,
enforce_final_scale: i32::from(options.enforce_final_scale),
final_scale_tolerance: options.final_scale_tolerance,
}
}
}
/// 表示 Rust 分配并通过 C ABI 返回的 ICSP 结果。
#[repr(C)]
#[derive(Debug)]
pub struct FlyshotIcspResult {
/// 路点绝对时间连续数组指针。
pub waypoint_times: *mut f64,
/// 路点时间数组长度。
pub waypoint_times_len: i32,
/// 分段时长连续数组指针。
pub segment_durations: *mut f64,
/// 分段时长数组长度。
pub segment_durations_len: i32,
/// 分段缩放连续数组指针。
pub segment_scales: *mut f64,
/// 分段缩放数组长度。
pub segment_scales_len: i32,
/// 历史最优结果对应的迭代轮数。
pub iterations: i32,
/// 历史最小收敛指标。
pub threshold: f64,
}
/// 通过稳定 C ABI 执行 ICSP 规划。
///
/// # Safety
/// 调用方必须保证所有非空输入指针在调用期间指向满足声明长度的可读连续数组,
/// `result` 必须指向可写的结果指针槽位;返回的结果只能交给
/// `flyshot_icsp_result_free` 释放。
#[no_mangle]
pub unsafe extern "C" fn flyshot_icsp_plan(
waypoints: *const f64,
waypoint_count: i32,
dof: i32,
velocity_limits: *const f64,
acceleration_limits: *const f64,
jerk_limits: *const f64,
options: *const FlyshotIcspOptions,
result: *mut *mut FlyshotIcspResult,
) -> i32 {
clear_last_error();
// Debug/测试构建可恢复遗漏 panic;Release panic=abort 下仍依赖边界校验避免 panic。
match catch_unwind(AssertUnwindSafe(|| unsafe {
flyshot_icsp_plan_impl(
waypoints,
waypoint_count,
dof,
velocity_limits,
acceleration_limits,
jerk_limits,
options,
result,
)
})) {
Ok(code) => code,
Err(_) => {
set_last_error("Rust ICSP FFI 捕获到未预期的内部 panic");
FLYSHOT_ERROR_INTERNAL
}
}
}
/// 释放由 `flyshot_icsp_plan` 返回的结果及其全部连续数组。
///
/// # Safety
/// `result` 必须为空指针,或是尚未释放且由本库当前版本分配的结果指针。
#[no_mangle]
pub unsafe extern "C" fn flyshot_icsp_result_free(result: *mut FlyshotIcspResult) {
if result.is_null() {
return;
}
// 先收回结果结构所有权,再按原始长度重建 boxed slice 归还数组内存。
let result = unsafe { Box::from_raw(result) };
unsafe {
free_f64_array(result.waypoint_times, result.waypoint_times_len);
free_f64_array(result.segment_durations, result.segment_durations_len);
free_f64_array(result.segment_scales, result.segment_scales_len);
}
}
/// 返回当前线程最近一次原生错误的 UTF-8 C 字符串。
///
/// # 返回值
/// 指针在当前线程下一次原生调用前有效;没有错误时指向空字符串。
#[no_mangle]
pub extern "C" fn flyshot_last_error_message() -> *const c_char {
LAST_ERROR_MESSAGE.with(|message| message.borrow().as_ptr())
}
/// 执行已通过 panic 边界保护的实际 FFI 参数校验和规划调用。
///
/// # Safety
/// 参数约束与公开 `flyshot_icsp_plan` 相同,本方法会在解引用前检查空指针和维度。
#[allow(clippy::too_many_arguments)]
unsafe fn flyshot_icsp_plan_impl(
waypoints: *const f64,
waypoint_count: i32,
dof: i32,
velocity_limits: *const f64,
acceleration_limits: *const f64,
jerk_limits: *const f64,
options: *const FlyshotIcspOptions,
result: *mut *mut FlyshotIcspResult,
) -> i32 {
if result.is_null() {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "result 输出指针不能为空");
}
unsafe {
*result = ptr::null_mut();
}
if waypoints.is_null() {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "waypoints 输入指针不能为空");
}
if velocity_limits.is_null() || acceleration_limits.is_null() || jerk_limits.is_null() {
return fail(
FLYSHOT_ERROR_INVALID_INPUT,
"velocity/acceleration/jerk 限值指针不能为空",
);
}
if options.is_null() {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "options 输入指针不能为空");
}
if waypoint_count < 4 || dof <= 0 {
return fail(
FLYSHOT_ERROR_INVALID_INPUT,
"waypoint_count 必须至少为 4,dof 必须大于零",
);
}
let Ok(waypoint_count_usize) = usize::try_from(waypoint_count) else {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "waypoint_count 转换失败");
};
let Ok(dof_usize) = usize::try_from(dof) else {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "dof 转换失败");
};
let Some(waypoint_value_count) = waypoint_count_usize.checked_mul(dof_usize) else {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "路点数组长度计算溢出");
};
let ffi_options = unsafe { *options };
if ffi_options.max_iterations < 0 {
return fail(FLYSHOT_ERROR_INVALID_INPUT, "max_iterations 不能为负数");
}
// 所有原始指针都已完成非空及长度上界校验,此处才建立只读切片。
let input = IcspInput {
waypoints: unsafe { slice::from_raw_parts(waypoints, waypoint_value_count) },
waypoint_count: waypoint_count_usize,
dof: dof_usize,
velocity_limits: unsafe { slice::from_raw_parts(velocity_limits, dof_usize) },
acceleration_limits: unsafe { slice::from_raw_parts(acceleration_limits, dof_usize) },
jerk_limits: unsafe { slice::from_raw_parts(jerk_limits, dof_usize) },
options: IcspOptions {
threshold: ffi_options.threshold,
max_iterations: ffi_options.max_iterations as usize,
enforce_final_scale: ffi_options.enforce_final_scale != 0,
final_scale_tolerance: ffi_options.final_scale_tolerance,
},
};
match plan_icsp(&input) {
Ok(plan) => {
let Ok(iterations) = i32::try_from(plan.iterations) else {
return fail(FLYSHOT_ERROR_INTERNAL, "迭代次数超出 C ABI i32 范围");
};
let (waypoint_times, waypoint_times_len) = leak_f64_array(plan.waypoint_times);
let (segment_durations, segment_durations_len) = leak_f64_array(plan.segment_durations);
let (segment_scales, segment_scales_len) = leak_f64_array(plan.segment_scales);
let native_result = Box::new(FlyshotIcspResult {
waypoint_times,
waypoint_times_len,
segment_durations,
segment_durations_len,
segment_scales,
segment_scales_len,
iterations,
threshold: plan.threshold,
});
unsafe {
*result = Box::into_raw(native_result);
}
FLYSHOT_SUCCESS
}
Err(error) => {
let code = error_code(&error);
set_last_error(&error.to_string());
code
}
}
}
/// 将 Rust 规划错误映射为稳定 C ABI 错误码。
///
/// # 参数
/// - `error`: 纯算法层返回的错误。
///
/// # 返回值
/// 与错误类别一一对应的固定整数。
fn error_code(error: &NativePlanningError) -> i32 {
match error {
NativePlanningError::InvalidInput(_) => FLYSHOT_ERROR_INVALID_INPUT,
NativePlanningError::NumericalFailure(_) => FLYSHOT_ERROR_NUMERICAL_FAILURE,
NativePlanningError::NotConverged(_) => FLYSHOT_ERROR_NOT_CONVERGED,
}
}
/// 写入错误文本并返回指定错误码。
///
/// # 参数
/// - `code`: 需要返回给调用方的稳定错误码。
/// - `message`: 当前线程可读取的诊断文本。
///
/// # 返回值
/// 原样返回 `code`,便于校验分支直接结束。
fn fail(code: i32, message: &str) -> i32 {
set_last_error(message);
code
}
/// 清空当前线程的最近错误文本。
fn clear_last_error() {
set_last_error("");
}
/// 将诊断文本保存到当前线程;内部 NUL 会替换为空格以保持合法 C 字符串。
///
/// # 参数
/// - `message`: 需要保存到线程局部缓冲区的 UTF-8 文本。
fn set_last_error(message: &str) {
let sanitized = message.replace('\0', " ");
// FFI 路径明确不使用 unwrap;即使替换逻辑未来变化,也保持无 panic 回退。
#[allow(clippy::manual_unwrap_or_default)]
let c_string = match CString::new(sanitized) {
Ok(value) => value,
Err(_) => CString::default(),
};
LAST_ERROR_MESSAGE.with(|slot| {
*slot.borrow_mut() = c_string;
});
}
/// 将 `Vec<f64>` 转为由 C ABI 结果持有的裸指针和 i32 长度。
///
/// # 参数
/// - `values`: 需要转移给结果对象的连续数组。
///
/// # 返回值
/// 数组裸指针及其长度;当前输入长度来源于 i32 路点维度,因此不会溢出。
fn leak_f64_array(values: Vec<f64>) -> (*mut f64, i32) {
let mut values = values.into_boxed_slice();
let length = i32::try_from(values.len()).unwrap_or(i32::MAX);
let pointer = values.as_mut_ptr();
std::mem::forget(values);
(pointer, length)
}
/// 按原始长度重建并释放 Rust 分配的 `f64` boxed slice。
///
/// # Safety
/// 指针和长度必须来自 `leak_f64_array` 且尚未释放。
unsafe fn free_f64_array(pointer: *mut f64, length: i32) {
if pointer.is_null() || length <= 0 {
return;
}
let Ok(length) = usize::try_from(length) else {
return;
};
let slice_pointer = ptr::slice_from_raw_parts_mut(pointer, length);
drop(unsafe { Box::from_raw(slice_pointer) });
}
#[cfg(test)]
mod tests {
use std::ffi::CStr;
use std::ptr;
use super::{
flyshot_icsp_plan, flyshot_icsp_result_free, flyshot_last_error_message,
FlyshotIcspOptions, FlyshotIcspResult, FLYSHOT_ERROR_INVALID_INPUT, FLYSHOT_SUCCESS,
};
/// 验证空输入指针会返回输入错误,并在线程局部错误文本中保留可诊断信息。
#[test]
fn ffi_rejects_null_waypoint_pointer_with_error_message() {
let options = FlyshotIcspOptions::default();
let mut result: *mut FlyshotIcspResult = ptr::null_mut();
let error_code = unsafe {
flyshot_icsp_plan(
ptr::null(),
4,
1,
[2.0].as_ptr(),
[4.0].as_ptr(),
[8.0].as_ptr(),
&options,
&mut result,
)
};
assert_eq!(error_code, FLYSHOT_ERROR_INVALID_INPUT);
assert!(result.is_null());
let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) }
.to_string_lossy()
.into_owned();
assert!(message.contains("waypoints"));
}
/// 验证正常 C ABI 调用会返回 Rust 分配的结果,并可通过唯一释放函数归还。
#[test]
fn ffi_returns_and_frees_successful_plan_result() {
let waypoints = [0.0, 0.5, 1.0, 1.5];
let velocity_limits = [2.0];
let acceleration_limits = [4.0];
let jerk_limits = [8.0];
let options = FlyshotIcspOptions {
enforce_final_scale: 0,
..FlyshotIcspOptions::default()
};
let mut result: *mut FlyshotIcspResult = ptr::null_mut();
let error_code = unsafe {
flyshot_icsp_plan(
waypoints.as_ptr(),
4,
1,
velocity_limits.as_ptr(),
acceleration_limits.as_ptr(),
jerk_limits.as_ptr(),
&options,
&mut result,
)
};
let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) }
.to_string_lossy()
.into_owned();
assert_eq!(error_code, FLYSHOT_SUCCESS, "{message}");
assert!(!result.is_null());
unsafe {
assert_eq!((*result).waypoint_times_len, 4);
assert_eq!((*result).segment_durations_len, 3);
assert_eq!((*result).segment_scales_len, 3);
flyshot_icsp_result_free(result);
}
}
/// 验证包含非有限数的路点会在 FFI 内转换为输入错误而不是触发 panic。
#[test]
fn ffi_rejects_non_finite_waypoint_values() {
let waypoints = [0.0, 0.5, f64::NAN, 1.5];
let velocity_limits = [2.0];
let acceleration_limits = [4.0];
let jerk_limits = [8.0];
let options = FlyshotIcspOptions::default();
let mut result: *mut FlyshotIcspResult = ptr::null_mut();
let error_code = unsafe {
flyshot_icsp_plan(
waypoints.as_ptr(),
4,
1,
velocity_limits.as_ptr(),
acceleration_limits.as_ptr(),
jerk_limits.as_ptr(),
&options,
&mut result,
)
};
assert_eq!(error_code, FLYSHOT_ERROR_INVALID_INPUT);
assert!(result.is_null());
let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) }
.to_string_lossy()
.into_owned();
assert!(message.contains("有限数"));
}
}

View File

@@ -0,0 +1,367 @@
//! 纯 Rust ICSP 规划算法。
use crate::error::NativePlanningError;
use crate::spline::CubicSpline1D;
/// 描述 ICSP 迭代和最终约束检查选项。
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct IcspOptions {
/// 收敛指标 `sum(abs(scale - 1))` 的停止阈值。
pub threshold: f64,
/// 最大迭代索引;算法与托管实现一致,会执行 `0..=max_iterations`。
pub max_iterations: usize,
/// 是否在历史最优结果仍有 scale 超限时返回失败。
pub enforce_final_scale: bool,
/// 最终 scale 检查允许的非负容差。
pub final_scale_tolerance: f64,
}
impl Default for IcspOptions {
/// 返回与现有 C# `ICspPlanner` 一致的默认参数。
///
/// # 返回值
/// threshold=1e-3、max_iterations=1000、启用最终 scale 检查且容差为 1e-2。
fn default() -> Self {
Self {
threshold: 1e-3,
max_iterations: 1000,
enforce_final_scale: true,
final_scale_tolerance: 1e-2,
}
}
}
/// 描述一次纯 Rust ICSP 调用的扁平输入。
#[derive(Debug, Clone, Copy)]
pub struct IcspInput<'a> {
/// 按路点行优先排列的连续关节位置数组。
pub waypoints: &'a [f64],
/// 路点数量。
pub waypoint_count: usize,
/// 每个路点的关节自由度。
pub dof: usize,
/// 每个关节的正速度上限。
pub velocity_limits: &'a [f64],
/// 每个关节的正加速度上限。
pub acceleration_limits: &'a [f64],
/// 每个关节的正 jerk 上限。
pub jerk_limits: &'a [f64],
/// 本次规划使用的迭代选项。
pub options: IcspOptions,
}
/// 表示 ICSP 历史最优迭代对应的完整规划结果。
#[derive(Debug, Clone, PartialEq)]
pub struct IcspPlanResult {
/// 每个路点对应的绝对时间,首项固定为零。
pub waypoint_times: Vec<f64>,
/// 历史最优结果的逐段时长。
pub segment_durations: Vec<f64>,
/// 历史最优结果的逐段约束缩放因子。
pub segment_scales: Vec<f64>,
/// 历史最优结果产生时已执行的迭代轮数。
pub iterations: usize,
/// 历史最小 `sum(abs(scale - 1))`。
pub threshold: f64,
}
/// 执行 ICSP 迭代规划并返回历史最优结果。
///
/// # 参数
/// - `input`: 行优先路点、逐关节约束和迭代选项。
///
/// # 返回值
/// 输入合法并通过最终 scale 检查时返回规划结果;否则返回明确错误。
pub fn plan_icsp(input: &IcspInput<'_>) -> Result<IcspPlanResult, NativePlanningError> {
validate_input(input)?;
let mut segment_durations = compute_initial_durations(input)?;
let segment_count = segment_durations.len();
let mut best_result: Option<IcspPlanResult> = None;
let mut best_threshold = f64::INFINITY;
// 迭代范围必须包含 max_iterations 对应轮次,以复刻托管实现的 `<=` 语义。
for iteration in 0..=input.options.max_iterations {
let waypoint_times = cumulative_times(&segment_durations)?;
let splines = build_joint_splines(input, &waypoint_times)?;
let mut segment_scales = vec![0.0; segment_count];
for (segment, segment_scale) in segment_scales.iter_mut().enumerate() {
let mut scale = 0.0_f64;
for (joint, spline) in splines.iter().enumerate() {
let [max_velocity, max_acceleration, max_jerk] =
spline.segment_peak_derivatives(segment)?;
let velocity_scale = (max_velocity / input.velocity_limits[joint]).abs();
let acceleration_scale = (max_acceleration / input.acceleration_limits[joint])
.abs()
.sqrt();
let jerk_scale = (max_jerk / input.jerk_limits[joint]).abs().cbrt();
scale = scale.max(velocity_scale.max(acceleration_scale.max(jerk_scale)));
}
if !scale.is_finite() {
return Err(NativePlanningError::NumericalFailure(format!(
"段 {segment} 的缩放因子不是有限数"
)));
}
*segment_scale = scale;
}
let current_threshold = segment_scales
.iter()
.map(|scale| (scale - 1.0).abs())
.sum::<f64>();
if !current_threshold.is_finite() {
return Err(NativePlanningError::NumericalFailure(
"ICSP 收敛指标不是有限数".to_owned(),
));
}
// 始终保存历史最小指标对应的数据,不能只保留最后一轮。
if current_threshold < best_threshold {
best_threshold = current_threshold;
best_result = Some(IcspPlanResult {
waypoint_times: waypoint_times.clone(),
segment_durations: segment_durations.clone(),
segment_scales: segment_scales.clone(),
iterations: iteration + 1,
threshold: current_threshold,
});
}
if current_threshold < input.options.threshold {
break;
}
for (duration, scale) in segment_durations.iter_mut().zip(segment_scales.iter()) {
*duration *= *scale;
}
}
let Some(result) = best_result else {
return Err(NativePlanningError::NumericalFailure(
"ICSP 未产生任何有效迭代结果".to_owned(),
));
};
let global_scale = result
.segment_scales
.iter()
.copied()
.fold(0.0_f64, f64::max);
if input.options.enforce_final_scale && global_scale > 1.0 + input.options.final_scale_tolerance
{
return Err(NativePlanningError::NotConverged(format!(
"global_scale={global_scale:.6} > {:.6}",
1.0 + input.options.final_scale_tolerance
)));
}
Ok(result)
}
/// 校验 ICSP 输入维度、有限性和正约束。
///
/// # 参数
/// - `input`: 待校验的纯算法输入。
///
/// # 返回值
/// 全部约束满足时返回 `Ok(())`,否则返回输入错误。
fn validate_input(input: &IcspInput<'_>) -> Result<(), NativePlanningError> {
if input.waypoint_count < 4 {
return Err(NativePlanningError::InvalidInput(
"ICSP 至少需要 4 个示教点".to_owned(),
));
}
if input.dof == 0 {
return Err(NativePlanningError::InvalidInput(
"自由度必须大于零".to_owned(),
));
}
let expected_waypoint_values = input
.waypoint_count
.checked_mul(input.dof)
.ok_or_else(|| NativePlanningError::InvalidInput("路点维度乘积溢出".to_owned()))?;
if input.waypoints.len() != expected_waypoint_values {
return Err(NativePlanningError::InvalidInput(format!(
"路点扁平数组长度应为 {expected_waypoint_values},实际为 {}",
input.waypoints.len()
)));
}
if input.velocity_limits.len() != input.dof
|| input.acceleration_limits.len() != input.dof
|| input.jerk_limits.len() != input.dof
{
return Err(NativePlanningError::InvalidInput(
"速度、加速度和 jerk 限值长度必须等于自由度".to_owned(),
));
}
if input.waypoints.iter().any(|value| !value.is_finite()) {
return Err(NativePlanningError::InvalidInput(
"路点位置必须全部为有限数".to_owned(),
));
}
if input
.velocity_limits
.iter()
.chain(input.acceleration_limits.iter())
.chain(input.jerk_limits.iter())
.any(|limit| !limit.is_finite() || *limit <= 0.0)
{
return Err(NativePlanningError::InvalidInput(
"速度、加速度和 jerk 限值必须全部为有限正数".to_owned(),
));
}
if !input.options.threshold.is_finite() || input.options.threshold <= 0.0 {
return Err(NativePlanningError::InvalidInput(
"收敛阈值必须为有限正数".to_owned(),
));
}
if !input.options.final_scale_tolerance.is_finite() || input.options.final_scale_tolerance < 0.0
{
return Err(NativePlanningError::InvalidInput(
"最终 scale 容差必须为有限非负数".to_owned(),
));
}
Ok(())
}
/// 计算相邻路点在关节空间的欧氏距离,作为初始段时长。
///
/// # 参数
/// - `input`: 已通过维度校验的 ICSP 输入。
///
/// # 返回值
/// 每对相邻路点对应的正段时长。
fn compute_initial_durations(input: &IcspInput<'_>) -> Result<Vec<f64>, NativePlanningError> {
let mut durations = Vec::with_capacity(input.waypoint_count - 1);
for segment in 0..input.waypoint_count - 1 {
let start_offset = segment * input.dof;
let end_offset = (segment + 1) * input.dof;
let mut squared_distance = 0.0;
for joint in 0..input.dof {
let difference =
input.waypoints[end_offset + joint] - input.waypoints[start_offset + joint];
squared_distance += difference * difference;
}
let duration = squared_distance.sqrt();
if !duration.is_finite() || duration <= 0.0 {
return Err(NativePlanningError::InvalidInput(format!(
"相邻路点段 {segment} 的欧氏距离必须为有限正数"
)));
}
durations.push(duration);
}
Ok(durations)
}
/// 将逐段时长累加为首项为零的绝对时间节点。
///
/// # 参数
/// - `segment_durations`: 当前迭代的逐段时长。
///
/// # 返回值
/// 长度比分段时长多一的严格递增时间轴。
fn cumulative_times(segment_durations: &[f64]) -> Result<Vec<f64>, NativePlanningError> {
let mut times = Vec::with_capacity(segment_durations.len() + 1);
times.push(0.0);
let mut current = 0.0;
for (segment, duration) in segment_durations.iter().copied().enumerate() {
if !duration.is_finite() || duration <= 0.0 {
return Err(NativePlanningError::NumericalFailure(format!(
"段 {segment} 的迭代时长必须为有限正数"
)));
}
current += duration;
if !current.is_finite() {
return Err(NativePlanningError::NumericalFailure(
"累计时间轴发生溢出".to_owned(),
));
}
times.push(current);
}
Ok(times)
}
/// 为每个关节独立构造 clamped-zero 样条。
///
/// # 参数
/// - `input`: 行优先路点及自由度信息。
/// - `waypoint_times`: 当前迭代的绝对时间节点。
///
/// # 返回值
/// 按关节顺序排列的单自由度样条集合。
fn build_joint_splines(
input: &IcspInput<'_>,
waypoint_times: &[f64],
) -> Result<Vec<CubicSpline1D>, NativePlanningError> {
let mut splines = Vec::with_capacity(input.dof);
for joint in 0..input.dof {
let mut values = Vec::with_capacity(input.waypoint_count);
for waypoint in 0..input.waypoint_count {
values.push(input.waypoints[waypoint * input.dof + joint]);
}
splines.push(CubicSpline1D::new_clamped_zero(waypoint_times, &values)?);
}
Ok(splines)
}
#[cfg(test)]
mod tests {
use super::{plan_icsp, IcspInput, IcspOptions};
use crate::error::NativePlanningError;
/// 验证 ICSP 对基础四路点样本返回完整时间轴、分段结果和有限收敛信息。
#[test]
fn icsp_returns_complete_result_for_basic_fixture() {
let waypoints = [0.0, 0.0, 1.0, 0.5, 2.0, 1.0, 3.0, 1.5];
let velocity_limits = [2.0, 2.0];
let acceleration_limits = [4.0, 4.0];
let jerk_limits = [8.0, 8.0];
let input = IcspInput {
waypoints: &waypoints,
waypoint_count: 4,
dof: 2,
velocity_limits: &velocity_limits,
acceleration_limits: &acceleration_limits,
jerk_limits: &jerk_limits,
options: IcspOptions::default(),
};
let result = plan_icsp(&input).unwrap();
assert_eq!(result.waypoint_times.len(), 4);
assert_eq!(result.segment_durations.len(), 3);
assert_eq!(result.segment_scales.len(), 3);
assert!(result.iterations >= 1);
assert!(result.threshold.is_finite());
assert!(result
.waypoint_times
.windows(2)
.all(|pair| pair[1] > pair[0]));
}
/// 验证禁用迭代且最终 scale 明显超限时,纯 Rust 核心返回未收敛错误。
#[test]
fn icsp_reports_not_converged_when_zero_iterations_cannot_meet_limits() {
let waypoints = [0.0, 1.0, 2.0, 3.0];
let velocity_limits = [0.01];
let acceleration_limits = [0.01];
let jerk_limits = [0.01];
let input = IcspInput {
waypoints: &waypoints,
waypoint_count: 4,
dof: 1,
velocity_limits: &velocity_limits,
acceleration_limits: &acceleration_limits,
jerk_limits: &jerk_limits,
options: IcspOptions {
max_iterations: 0,
..IcspOptions::default()
},
};
let error = plan_icsp(&input).unwrap_err();
assert!(matches!(error, NativePlanningError::NotConverged(_)));
}
}

View File

@@ -0,0 +1,6 @@
//! Flyshot ICSP 原生规划库的稳定入口。
mod error;
mod ffi;
mod icsp;
mod spline;

View File

@@ -0,0 +1,277 @@
//! clamped-zero 三次样条算法。
use crate::error::NativePlanningError;
/// 表示单自由度 clamped-zero 三次样条及其逐段多项式系数。
#[derive(Debug, Clone)]
pub struct CubicSpline1D {
knots: Vec<f64>,
coefficients: Vec<[f64; 4]>,
}
// 位置和逐阶导数求值是第一阶段要求的原生能力,当前 ICSP 热路径只直接使用解析峰值。
#[allow(dead_code)]
impl CubicSpline1D {
/// 使用严格递增时间节点和对应值构造首尾速度为零的三次样条。
///
/// # 参数
/// - `times`: 严格递增且全部有限的时间节点,长度至少为 2。
/// - `values`: 与时间节点一一对应的有限位置值。
///
/// # 返回值
/// 输入合法且三对角方程可解时返回样条,否则返回输入或数值错误。
pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result<Self, NativePlanningError> {
if times.len() < 2 {
return Err(NativePlanningError::InvalidInput(
"时间节点至少需要 2 个".to_owned(),
));
}
if values.len() != times.len() {
return Err(NativePlanningError::InvalidInput(
"位置值数量必须与时间节点数量一致".to_owned(),
));
}
if times.iter().any(|value| !value.is_finite())
|| values.iter().any(|value| !value.is_finite())
{
return Err(NativePlanningError::InvalidInput(
"时间节点和位置值必须全部为有限数".to_owned(),
));
}
let segment_count = times.len() - 1;
let mut durations = Vec::with_capacity(segment_count);
for segment in 0..segment_count {
let duration = times[segment + 1] - times[segment];
if duration <= 0.0 || !duration.is_finite() {
return Err(NativePlanningError::InvalidInput(format!(
"时间节点必须严格递增,段 {segment} 的时长为 {duration}"
)));
}
durations.push(duration);
}
// 与托管实现保持一致:先组装 clamped-zero 边界下的三对角方程右端项。
let node_count = times.len();
let mut alpha = vec![0.0; node_count];
alpha[0] = 6.0 * ((values[1] - values[0]) / durations[0]);
alpha[node_count - 1] = 6.0
* (0.0 - (values[node_count - 1] - values[node_count - 2]) / durations[node_count - 2]);
for index in 1..node_count - 1 {
alpha[index] = 6.0
* ((values[index + 1] - values[index]) / durations[index]
- (values[index] - values[index - 1]) / durations[index - 1]);
}
let mut diagonal = vec![0.0; node_count];
let mut upper_ratio = vec![0.0; node_count];
let mut solved_rhs = vec![0.0; node_count];
diagonal[0] = 2.0 * durations[0];
upper_ratio[0] = 0.5;
solved_rhs[0] = alpha[0] / diagonal[0];
// Thomas 消元逐行处理内部节点,避免建立完整稠密矩阵。
for index in 1..node_count - 1 {
diagonal[index] = 2.0 * (durations[index - 1] + durations[index])
- durations[index - 1] * upper_ratio[index - 1];
if diagonal[index].abs() <= f64::EPSILON || !diagonal[index].is_finite() {
return Err(NativePlanningError::NumericalFailure(format!(
"三对角方程在节点 {index} 出现无效主元"
)));
}
upper_ratio[index] = durations[index] / diagonal[index];
solved_rhs[index] =
(alpha[index] - durations[index - 1] * solved_rhs[index - 1]) / diagonal[index];
}
diagonal[node_count - 1] = 2.0 * durations[node_count - 2]
- durations[node_count - 2] * upper_ratio[node_count - 2];
if diagonal[node_count - 1].abs() <= f64::EPSILON || !diagonal[node_count - 1].is_finite() {
return Err(NativePlanningError::NumericalFailure(
"三对角方程末端出现无效主元".to_owned(),
));
}
solved_rhs[node_count - 1] = (alpha[node_count - 1]
- durations[node_count - 2] * solved_rhs[node_count - 2])
/ diagonal[node_count - 1];
// 回代得到各节点二阶导数,再转换为每段局部三次多项式系数。
let mut second_derivatives = vec![0.0; node_count];
second_derivatives[node_count - 1] = solved_rhs[node_count - 1];
for index in (0..node_count - 1).rev() {
second_derivatives[index] =
solved_rhs[index] - upper_ratio[index] * second_derivatives[index + 1];
}
let mut coefficients = Vec::with_capacity(segment_count);
for segment in 0..segment_count {
let duration = durations[segment];
let cubic =
(second_derivatives[segment + 1] - second_derivatives[segment]) / (6.0 * duration);
let quadratic = second_derivatives[segment] / 2.0;
let linear = (values[segment + 1] - values[segment]) / duration
- duration * (2.0 * second_derivatives[segment] + second_derivatives[segment + 1])
/ 6.0;
coefficients.push([cubic, quadratic, linear, values[segment]]);
}
Ok(Self {
knots: times.to_vec(),
coefficients,
})
}
/// 在指定绝对时间求样条位置;超出范围时使用首段或末段外推点。
///
/// # 参数
/// - `time`: 需要求值的绝对时间。
///
/// # 返回值
/// 对应样条位置。
pub fn position(&self, time: f64) -> f64 {
let (segment, local_time) = self.segment_and_local_time(time);
let [cubic, quadratic, linear, constant] = self.coefficients[segment];
cubic * local_time * local_time * local_time
+ quadratic * local_time * local_time
+ linear * local_time
+ constant
}
/// 在指定绝对时间求一阶导数。
///
/// # 参数
/// - `time`: 需要求值的绝对时间。
///
/// # 返回值
/// 对应样条速度。
pub fn velocity(&self, time: f64) -> f64 {
let (segment, local_time) = self.segment_and_local_time(time);
let [cubic, quadratic, linear, _] = self.coefficients[segment];
3.0 * cubic * local_time * local_time + 2.0 * quadratic * local_time + linear
}
/// 在指定绝对时间求二阶导数。
///
/// # 参数
/// - `time`: 需要求值的绝对时间。
///
/// # 返回值
/// 对应样条加速度。
pub fn acceleration(&self, time: f64) -> f64 {
let (segment, local_time) = self.segment_and_local_time(time);
let [cubic, quadratic, _, _] = self.coefficients[segment];
6.0 * cubic * local_time + 2.0 * quadratic
}
/// 在指定绝对时间求三阶导数。
///
/// # 参数
/// - `time`: 需要求值的绝对时间。
///
/// # 返回值
/// 对应样条 jerk。
pub fn jerk(&self, time: f64) -> f64 {
let (segment, _) = self.segment_and_local_time(time);
6.0 * self.coefficients[segment][0]
}
/// 解析计算指定段的一至三阶导数绝对值峰值。
///
/// # 参数
/// - `segment_index`: 从零开始的段索引。
///
/// # 返回值
/// `[速度峰值, 加速度峰值, jerk 峰值]`;索引越界时返回输入错误。
pub fn segment_peak_derivatives(
&self,
segment_index: usize,
) -> Result<[f64; 3], NativePlanningError> {
let Some([cubic, quadratic, linear, _]) = self.coefficients.get(segment_index).copied()
else {
return Err(NativePlanningError::InvalidInput(format!(
"样条段索引 {segment_index} 越界"
)));
};
let duration = self.knots[segment_index + 1] - self.knots[segment_index];
// 一阶导为二次函数,绝对值峰值只可能出现在端点或段内顶点。
let mut velocity_peak = linear
.abs()
.max((3.0 * cubic * duration * duration + 2.0 * quadratic * duration + linear).abs());
if cubic.abs() > 1e-12 {
let critical = -quadratic / (3.0 * cubic);
if critical > 0.0 && critical < duration {
velocity_peak = velocity_peak.max(
(3.0 * cubic * critical * critical + 2.0 * quadratic * critical + linear).abs(),
);
}
}
let acceleration_peak = (2.0 * quadratic)
.abs()
.max((6.0 * cubic * duration + 2.0 * quadratic).abs());
let jerk_peak = (6.0 * cubic).abs();
Ok([velocity_peak, acceleration_peak, jerk_peak])
}
/// 定位时间所在样条段并计算相对段起点的局部时间。
///
/// # 参数
/// - `time`: 需要定位的绝对时间。
///
/// # 返回值
/// `(段索引, 局部时间)`;时间超界时钳制到首尾节点。
fn segment_and_local_time(&self, time: f64) -> (usize, f64) {
let last_segment = self.coefficients.len() - 1;
if time <= self.knots[0] {
return (0, 0.0);
}
let last_knot = self.knots[self.knots.len() - 1];
if time >= last_knot {
return (
last_segment,
self.knots[last_segment + 1] - self.knots[last_segment],
);
}
let insertion = self.knots.partition_point(|knot| *knot <= time);
let segment = insertion.saturating_sub(1).min(last_segment);
(segment, time - self.knots[segment])
}
}
#[cfg(test)]
mod tests {
use super::CubicSpline1D;
/// 验证 clamped-zero 样条严格命中首尾路点,并满足两端零速度边界。
#[test]
fn clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity() {
let times = [0.0, 1.0, 2.0, 3.0];
let values = [0.0, 1.0, 0.5, 2.0];
let spline = CubicSpline1D::new_clamped_zero(&times, &values).unwrap();
assert!((spline.position(0.0) - 0.0).abs() <= 1e-12);
assert!((spline.position(3.0) - 2.0).abs() <= 1e-12);
assert!(spline.velocity(0.0).abs() <= 1e-12);
assert!(spline.velocity(3.0).abs() <= 1e-12);
assert!(spline.acceleration(1.5).is_finite());
assert!(spline.jerk(1.5).is_finite());
}
/// 验证每段速度、加速度和 jerk 的解析峰值不会漏掉段内速度极值。
#[test]
fn segment_peak_derivatives_include_internal_velocity_extremum() {
let times = [0.0, 1.0, 2.0, 3.0];
let values = [0.0, 1.0, 0.5, 2.0];
let spline = CubicSpline1D::new_clamped_zero(&times, &values).unwrap();
let peaks = spline.segment_peak_derivatives(0).unwrap();
assert!(peaks[0] >= spline.velocity(0.0).abs());
assert!(peaks[0] >= spline.velocity(1.0).abs());
assert!(peaks[1].is_finite() && peaks[1] > 0.0);
assert!(peaks[2].is_finite() && peaks[2] > 0.0);
}
}

View File

@@ -0,0 +1,642 @@
<#
.SYNOPSIS
发布并混淆 Flyshot 跨平台框架依赖部署包。
.DESCRIPTION
脚本先通过 dotnet publish 生成不区分运行平台的框架依赖产物,再调用环境中安装的
Obfuscar 全局工具混淆全部 Flyshot.* 自研程序集。第三方程序集不会进入混淆模块,
映射文件会保存到独立目录,不会随部署包交付。目标机器需要安装 .NET 8 运行时。
.PARAMETER OutputRoot
最终混淆部署包目录。
.PARAMETER MappingRoot
Obfuscar 映射文件的保存根目录。该目录不得作为部署包交付。
.PARAMETER GenerateConfigOnly
仅生成 Obfuscar 配置,供自动化测试和问题诊断使用,不执行发布或混淆。
.PARAMETER InputDirectory
GenerateConfigOnly 模式下的程序集输入目录。
.PARAMETER ObfuscatedDirectory
GenerateConfigOnly 模式下的混淆输出目录。
.PARAMETER ConfigPath
GenerateConfigOnly 模式下生成的 Obfuscar XML 文件路径。
.PARAMETER ObfuscateExistingPublish
仅混淆已经由 Visual Studio 或 dotnet publish 生成的现有发布目录,不再次执行发布。
.PARAMETER ExistingPublishDirectory
ObfuscateExistingPublish 模式下需要原地替换自研程序集的发布目录。
#>
[CmdletBinding()]
param(
[string]$OutputRoot,
[string]$MappingRoot,
[switch]$GenerateConfigOnly,
[string]$InputDirectory,
[string]$ObfuscatedDirectory,
[string]$ConfigPath,
[switch]$ObfuscateExistingPublish,
[string]$ExistingPublishDirectory
)
$ErrorActionPreference = 'Stop'
Set-StrictMode -Version Latest
# 该清单固定了当前解决方案必须保护的全部自研程序集,避免新增或丢失模块时静默发布。
$script:FlyshotAssemblyNames = @(
'Flyshot.Server.Host.dll'
'Flyshot.ControllerClientCompat.dll'
'Flyshot.Core.Config.dll'
'Flyshot.Core.Domain.dll'
'Flyshot.Core.Planning.dll'
'Flyshot.Core.Triggering.dll'
'Flyshot.Runtime.Fanuc.dll'
'Flyshot.Runtime.Common.dll'
)
<#
.SYNOPSIS
将路径转换为规范化绝对路径。
.PARAMETER Path
需要规范化的文件或目录路径,可以是相对路径或绝对路径。
.OUTPUTS
返回由 System.IO.Path 解析后的绝对路径字符串。
#>
function ConvertTo-AbsolutePath
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$Path
)
return [System.IO.Path]::GetFullPath($Path)
}
<#
.SYNOPSIS
检查发布目录是否包含全部预期的 Flyshot 自研程序集。
.PARAMETER Directory
待检查的发布输入目录。
.OUTPUTS
按固定清单顺序返回每个程序集的绝对路径。
.EXCEPTION
当目录不存在或任一预期程序集缺失时抛出异常,阻止生成未完整保护的发布包。
#>
function Get-FlyshotAssemblyPaths
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$Directory
)
$absoluteDirectory = ConvertTo-AbsolutePath -Path $Directory
if (-not (Test-Path -LiteralPath $absoluteDirectory -PathType Container))
{
throw "程序集输入目录不存在:$absoluteDirectory"
}
foreach ($assemblyName in $script:FlyshotAssemblyNames)
{
$assemblyPath = Join-Path $absoluteDirectory $assemblyName
if (-not (Test-Path -LiteralPath $assemblyPath -PathType Leaf))
{
throw "发布目录缺少必须混淆的自研程序集:$assemblyName"
}
}
# 固定清单负责阻止当前核心模块缺失,通配发现负责自动覆盖未来新增的 Flyshot 自研程序集。
$assemblyPaths = Get-ChildItem -LiteralPath $absoluteDirectory -Filter 'Flyshot.*.dll' -File |
Sort-Object Name |
ForEach-Object { ConvertTo-AbsolutePath -Path $_.FullName }
return @($assemblyPaths)
}
<#
.SYNOPSIS
发现当前环境中与项目目标框架匹配的 .NET 8 共享框架目录。
.DESCRIPTION
框架依赖发布不会把 Microsoft.NETCore.App 与 Microsoft.AspNetCore.App 的程序集复制到发布目录。
Obfuscar 仍需要这些目录解析继承关系,因此通过 dotnet --list-runtimes 动态发现实际安装位置。
.OUTPUTS
返回 Microsoft.NETCore.App 与 Microsoft.AspNetCore.App 最新 .NET 8 运行时的绝对目录。
.EXCEPTION
当环境未安装所需的 .NET 8 共享框架时抛出异常,避免生成无法解析依赖的混淆配置。
#>
function Get-DotNetSharedFrameworkSearchPaths
{
[CmdletBinding()]
param()
$runtimeEntries = & dotnet --list-runtimes
if ($LASTEXITCODE -ne 0)
{
throw "无法查询已安装的 .NET 运行时,dotnet --list-runtimes 退出码为 $LASTEXITCODE。"
}
$requiredFrameworks = @(
'Microsoft.NETCore.App'
'Microsoft.AspNetCore.App'
)
$searchPaths = foreach ($frameworkName in $requiredFrameworks)
{
$candidates = foreach ($runtimeEntry in $runtimeEntries)
{
if ($runtimeEntry -match '^(?<name>\S+)\s+(?<version>8\.\d+\.\d+)\s+\[(?<root>.+)\]$')
{
if ($Matches.name -eq $frameworkName)
{
[pscustomobject]@{
Version = [version]$Matches.version
Path = Join-Path $Matches.root $Matches.version
}
}
}
}
$selectedRuntime = $candidates |
Sort-Object Version -Descending |
Select-Object -First 1
if ($null -eq $selectedRuntime -or -not (Test-Path -LiteralPath $selectedRuntime.Path -PathType Container))
{
throw "环境缺少 Obfuscar 解析依赖所需的 .NET 8 共享框架:$frameworkName"
}
ConvertTo-AbsolutePath -Path $selectedRuntime.Path
}
return @($searchPaths)
}
<#
.SYNOPSIS
生成仅包含 Flyshot 自研程序集的 Obfuscar XML 配置。
.PARAMETER SourceDirectory
包含发布程序集及其依赖项的输入目录。
.PARAMETER TargetDirectory
Obfuscar 写入混淆程序集的独立输出目录。
.PARAMETER DestinationPath
要写入的 Obfuscar XML 配置文件路径。
.OUTPUTS
返回生成后的 Obfuscar XML 配置绝对路径。
#>
function New-ObfuscarConfiguration
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$SourceDirectory,
[Parameter(Mandatory)]
[string]$TargetDirectory,
[Parameter(Mandatory)]
[string]$DestinationPath
)
$absoluteSourceDirectory = ConvertTo-AbsolutePath -Path $SourceDirectory
$absoluteTargetDirectory = ConvertTo-AbsolutePath -Path $TargetDirectory
$absoluteDestinationPath = ConvertTo-AbsolutePath -Path $DestinationPath
$assemblyPaths = Get-FlyshotAssemblyPaths -Directory $absoluteSourceDirectory
$assemblySearchPaths = Get-DotNetSharedFrameworkSearchPaths
$destinationParent = Split-Path -Parent $absoluteDestinationPath
New-Item -ItemType Directory -Path $destinationParent -Force | Out-Null
New-Item -ItemType Directory -Path $absoluteTargetDirectory -Force | Out-Null
$settings = [ordered]@{
InPath = $absoluteSourceDirectory
OutPath = $absoluteTargetDirectory
KeepPublicApi = 'true'
HidePrivateApi = 'true'
RenameProperties = 'false'
RenameEvents = 'false'
RenameFields = 'true'
SkipGenerated = 'true'
SkipSpecialName = 'true'
}
$xmlSettings = [System.Xml.XmlWriterSettings]::new()
$xmlSettings.Indent = $true
$xmlSettings.Encoding = [System.Text.UTF8Encoding]::new($false)
$writer = [System.Xml.XmlWriter]::Create($absoluteDestinationPath, $xmlSettings)
try
{
$writer.WriteStartDocument()
$writer.WriteStartElement('Obfuscator')
foreach ($setting in $settings.GetEnumerator())
{
$writer.WriteStartElement('Var')
$writer.WriteAttributeString('name', [string]$setting.Key)
$writer.WriteAttributeString('value', [string]$setting.Value)
$writer.WriteEndElement()
}
foreach ($assemblySearchPath in $assemblySearchPaths)
{
# 框架依赖发布不携带共享框架 DLL,Obfuscar 通过这些目录解析 ASP.NET Core 与运行时依赖。
$writer.WriteStartElement('AssemblySearchPath')
$writer.WriteAttributeString('path', $assemblySearchPath)
$writer.WriteEndElement()
}
foreach ($assemblyPath in $assemblyPaths)
{
# 使用明确的绝对路径,兼容 Obfuscar 当前版本并避免工作目录差异。
$writer.WriteStartElement('Module')
$writer.WriteAttributeString('file', $assemblyPath)
# 匿名类型会被 ASP.NET Core 的 System.Text.Json 反射序列化,必须完整保留类型及构造参数元数据。
$writer.WriteStartElement('SkipType')
$writer.WriteAttributeString('name', '^<>f__AnonymousType.*')
$writer.WriteAttributeString('skipMethods', 'true')
$writer.WriteAttributeString('skipFields', 'true')
$writer.WriteAttributeString('skipProperties', 'true')
$writer.WriteAttributeString('skipEvents', 'true')
$writer.WriteEndElement()
$writer.WriteEndElement()
}
$writer.WriteEndElement()
$writer.WriteEndDocument()
}
finally
{
$writer.Dispose()
}
return $absoluteDestinationPath
}
<#
.SYNOPSIS
确认发布依赖的外部命令已安装并可从 PATH 调用。
.PARAMETER CommandName
要检查的命令名称。
.PARAMETER InstallHint
命令缺失时展示给构建人员的安装提示。
#>
function Assert-EnvironmentCommand
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$CommandName,
[Parameter(Mandatory)]
[string]$InstallHint
)
if (-not (Get-Command $CommandName -ErrorAction SilentlyContinue))
{
throw "环境缺少命令 '$CommandName'。$InstallHint"
}
}
<#
.SYNOPSIS
执行外部命令并在非零退出码时终止发布。
.PARAMETER FilePath
需要执行的外部命令。
.PARAMETER ArgumentList
按顺序传入外部命令的参数数组。
#>
function Invoke-CheckedCommand
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$FilePath,
[Parameter(Mandatory)]
[string[]]$ArgumentList
)
& $FilePath @ArgumentList
if ($LASTEXITCODE -ne 0)
{
throw "命令执行失败,退出码为 $LASTEXITCODE:$FilePath $($ArgumentList -join ' ')"
}
}
<#
.SYNOPSIS
将 Obfuscar 输出安全替换进发布目录并隔离映射文件。
.PARAMETER PublishedDirectory
dotnet publish 生成的最终部署目录。
.PARAMETER ObfuscatedOutputDirectory
Obfuscar 独立输出目录。
.PARAMETER RuntimeMappingDirectory
框架依赖发布对应的映射文件保存目录。
#>
function Install-ObfuscatedAssemblies
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$PublishedDirectory,
[Parameter(Mandatory)]
[string]$ObfuscatedOutputDirectory,
[Parameter(Mandatory)]
[string]$RuntimeMappingDirectory
)
$assemblyNames = Get-FlyshotAssemblyPaths -Directory $PublishedDirectory |
ForEach-Object { [System.IO.Path]::GetFileName($_) }
foreach ($assemblyName in $assemblyNames)
{
$obfuscatedAssemblyPath = Join-Path $ObfuscatedOutputDirectory $assemblyName
if (-not (Test-Path -LiteralPath $obfuscatedAssemblyPath -PathType Leaf))
{
throw "Obfuscar 未生成预期程序集:$assemblyName"
}
}
foreach ($assemblyName in $assemblyNames)
{
Copy-Item `
-LiteralPath (Join-Path $ObfuscatedOutputDirectory $assemblyName) `
-Destination (Join-Path $PublishedDirectory $assemblyName) `
-Force
$pdbName = [System.IO.Path]::ChangeExtension($assemblyName, '.pdb')
$obfuscatedPdbPath = Join-Path $ObfuscatedOutputDirectory $pdbName
if (Test-Path -LiteralPath $obfuscatedPdbPath -PathType Leaf)
{
Copy-Item `
-LiteralPath $obfuscatedPdbPath `
-Destination (Join-Path $PublishedDirectory $pdbName) `
-Force
}
}
$mappingPath = Join-Path $ObfuscatedOutputDirectory 'Mapping.txt'
if (-not (Test-Path -LiteralPath $mappingPath -PathType Leaf))
{
throw 'Obfuscar 未生成 Mapping.txt,无法确认混淆映射结果。'
}
New-Item -ItemType Directory -Path $RuntimeMappingDirectory -Force | Out-Null
Copy-Item -LiteralPath $mappingPath -Destination (Join-Path $RuntimeMappingDirectory 'Mapping.txt') -Force
# 部署目录不得包含符号映射,避免把重命名关系直接交付给最终用户。
$publishedMappingPath = Join-Path $PublishedDirectory 'Mapping.txt'
if (Test-Path -LiteralPath $publishedMappingPath)
{
Remove-Item -LiteralPath $publishedMappingPath -Force
}
# XML 文档包含源码 /// 注释,PDB 包含调试符号;生产混淆包不得向反编译器暴露这些辅助信息。
Get-ChildItem -LiteralPath $PublishedDirectory -File |
Where-Object {
$_.Name -like 'Flyshot*.xml' -or
$_.Name -like 'Flyshot*.pdb'
} |
Remove-Item -Force
# Rust ICSP 原生库必须随框架依赖包一起交付,否则默认 method=icsp 会在运行时加载失败。
$nativePlanningLibraryPath = Join-Path $PublishedDirectory 'flyshot_planning.dll'
if (-not (Test-Path -LiteralPath $nativePlanningLibraryPath -PathType Leaf))
{
throw "发布目录缺少 Rust 原生规划库 flyshot_planning.dll:$nativePlanningLibraryPath"
}
}
<#
.SYNOPSIS
发布并混淆 Flyshot 框架依赖主程序。
.PARAMETER RepositoryRoot
Flyshot Replacement 仓库根目录。
.PARAMETER PublishedOutputDirectory
最终混淆部署包目录。
.PARAMETER MappingOutputRoot
全部混淆映射文件的根目录。
#>
function Publish-ObfuscatedFrameworkDependent
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$RepositoryRoot,
[Parameter(Mandatory)]
[string]$PublishedOutputDirectory,
[Parameter(Mandatory)]
[string]$MappingOutputRoot
)
$hostProject = Join-Path $RepositoryRoot 'src\Flyshot.Server.Host\Flyshot.Server.Host.csproj'
$workingDirectory = Join-Path $RepositoryRoot '.tmp\obfuscation\framework-dependent'
$obfuscatedOutputDirectory = Join-Path $workingDirectory 'obfuscated'
$configurationPath = Join-Path $workingDirectory 'obfuscar.xml'
$runtimeMappingDirectory = Join-Path $MappingOutputRoot 'framework-dependent'
# 每次发布使用干净目录,避免旧程序集或旧映射掩盖本轮失败。
foreach ($directory in @($PublishedOutputDirectory, $workingDirectory, $runtimeMappingDirectory))
{
if (Test-Path -LiteralPath $directory)
{
Remove-Item -LiteralPath $directory -Recurse -Force
}
}
New-Item -ItemType Directory -Path $PublishedOutputDirectory -Force | Out-Null
Write-Host '正在发布跨平台框架依赖包 ...'
Invoke-CheckedCommand -FilePath 'dotnet' -ArgumentList @(
'publish'
$hostProject
'-c'
'Release'
'--self-contained'
'false'
'-p:PublishSingleFile=false'
'-p:UseAppHost=false'
'-p:DebugType=none'
'-o'
$PublishedOutputDirectory
)
New-ObfuscarConfiguration `
-SourceDirectory $PublishedOutputDirectory `
-TargetDirectory $obfuscatedOutputDirectory `
-DestinationPath $configurationPath | Out-Null
Write-Host '正在混淆框架依赖包中的 Flyshot 自研程序集 ...'
Invoke-CheckedCommand -FilePath 'obfuscar.console' -ArgumentList @($configurationPath)
Install-ObfuscatedAssemblies `
-PublishedDirectory $PublishedOutputDirectory `
-ObfuscatedOutputDirectory $obfuscatedOutputDirectory `
-RuntimeMappingDirectory $runtimeMappingDirectory
Write-Host "已生成跨平台框架依赖混淆包:$PublishedOutputDirectory"
}
<#
.SYNOPSIS
混淆 Visual Studio 已生成的现有发布目录。
.PARAMETER PublishedDirectory
包含全部发布文件的目录;混淆成功后会原地替换其中的 Flyshot 自研程序集。
.PARAMETER RepositoryRoot
Flyshot Replacement 仓库根目录。
.PARAMETER MappingOutputRoot
混淆映射文件的根目录;映射文件不会进入发布目录。
#>
function Protect-ExistingPublishedDirectory
{
[CmdletBinding()]
param(
[Parameter(Mandatory)]
[string]$PublishedDirectory,
[Parameter(Mandatory)]
[string]$RepositoryRoot,
[Parameter(Mandatory)]
[string]$MappingOutputRoot
)
$absolutePublishedDirectory = ConvertTo-AbsolutePath -Path $PublishedDirectory
$workingDirectory = Join-Path $RepositoryRoot '.tmp\obfuscation\visual-studio\framework-dependent'
$obfuscatedOutputDirectory = Join-Path $workingDirectory 'obfuscated'
$configurationPath = Join-Path $workingDirectory 'obfuscar.xml'
$runtimeMappingDirectory = Join-Path $MappingOutputRoot 'framework-dependent'
if (-not (Test-Path -LiteralPath $absolutePublishedDirectory -PathType Container))
{
throw "Visual Studio 发布目录不存在:$absolutePublishedDirectory"
}
# 后处理必须使用独立输出目录,避免 Obfuscar 读写同一个程序集导致文件锁或半成品。
foreach ($directory in @($workingDirectory, $runtimeMappingDirectory))
{
if (Test-Path -LiteralPath $directory)
{
Remove-Item -LiteralPath $directory -Recurse -Force
}
}
New-ObfuscarConfiguration `
-SourceDirectory $absolutePublishedDirectory `
-TargetDirectory $obfuscatedOutputDirectory `
-DestinationPath $configurationPath | Out-Null
Write-Host "正在混淆 Visual Studio 发布目录中的 Flyshot 自研程序集:$absolutePublishedDirectory"
Invoke-CheckedCommand -FilePath 'obfuscar.console' -ArgumentList @($configurationPath)
Install-ObfuscatedAssemblies `
-PublishedDirectory $absolutePublishedDirectory `
-ObfuscatedOutputDirectory $obfuscatedOutputDirectory `
-RuntimeMappingDirectory $runtimeMappingDirectory
Write-Host "Visual Studio 发布目录混淆完成:$absolutePublishedDirectory"
}
try
{
if ($GenerateConfigOnly)
{
foreach ($requiredValue in @{
InputDirectory = $InputDirectory
ObfuscatedDirectory = $ObfuscatedDirectory
ConfigPath = $ConfigPath
}.GetEnumerator())
{
if ([string]::IsNullOrWhiteSpace([string]$requiredValue.Value))
{
throw "GenerateConfigOnly 模式缺少参数:$($requiredValue.Key)"
}
}
New-ObfuscarConfiguration `
-SourceDirectory $InputDirectory `
-TargetDirectory $ObfuscatedDirectory `
-DestinationPath $ConfigPath | Out-Null
exit 0
}
$repositoryRoot = (Resolve-Path (Join-Path $PSScriptRoot '..')).Path
if ([string]::IsNullOrWhiteSpace($OutputRoot))
{
$OutputRoot = Join-Path $repositoryRoot 'outputs\obfuscated\framework-dependent'
}
if ([string]::IsNullOrWhiteSpace($MappingRoot))
{
$MappingRoot = Join-Path $repositoryRoot 'outputs\obfuscation-maps'
}
Assert-EnvironmentCommand -CommandName 'dotnet' -InstallHint '请安装仓库 global.json 指定的 .NET SDK。'
Assert-EnvironmentCommand `
-CommandName 'obfuscar.console' `
-InstallHint '请执行:dotnet tool install --global Obfuscar.GlobalTool --version 2.2.50'
if ($ObfuscateExistingPublish)
{
if ([string]::IsNullOrWhiteSpace($ExistingPublishDirectory))
{
throw 'ObfuscateExistingPublish 模式缺少参数:ExistingPublishDirectory'
}
Protect-ExistingPublishedDirectory `
-PublishedDirectory $ExistingPublishDirectory `
-RepositoryRoot $repositoryRoot `
-MappingOutputRoot (ConvertTo-AbsolutePath -Path $MappingRoot)
exit 0
}
Publish-ObfuscatedFrameworkDependent `
-RepositoryRoot $repositoryRoot `
-PublishedOutputDirectory (ConvertTo-AbsolutePath -Path $OutputRoot) `
-MappingOutputRoot (ConvertTo-AbsolutePath -Path $MappingRoot)
Write-Host 'Flyshot 跨平台框架依赖混淆发布包已生成。'
exit 0
}
catch
{
Write-Error $_
exit 1
}

View File

@@ -0,0 +1,44 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 将拆分后的 C# ICSP 规划器暴露为可选规划后端。
/// </summary>
public sealed class CSharpIcspPlanningBackend : IPlanningBackend
{
private readonly ICspPlanner _planner;
/// <summary>
/// 初始化 C# ICSP 后端。
/// </summary>
/// <param name="logger">C# ICSP 规划器日志记录器;允许为空。</param>
public CSharpIcspPlanningBackend(ILogger<ICspPlanner>? logger = null)
{
_planner = new ICspPlanner(logger: logger);
}
/// <summary>
/// 获取外部调用 C# ICSP 后端时使用的准确方法名。
/// </summary>
public string MethodName => "icsp-csharp";
/// <summary>
/// 获取 C# ICSP 后端写入结果对象的规划方法。
/// </summary>
public PlanningMethod ResultMethod => PlanningMethod.IcspCSharp;
/// <summary>
/// 执行 C# ICSP 规划,并将结果标记为 C# 后端。
/// </summary>
/// <param name="request">包含机器人约束和示教路点的规划请求。</param>
/// <param name="context">规划上下文;普通 C# ICSP 当前不使用该参数。</param>
/// <returns>由 C# ICSP 规划器生成的轨迹结果。</returns>
public PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context)
{
ArgumentNullException.ThrowIfNull(context);
return _planner.Plan(request);
}
}

View File

@@ -0,0 +1,24 @@
using Flyshot.Core.Planning;
using Microsoft.Extensions.DependencyInjection;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 提供可选 C# 规划算法后端的依赖注入注册入口。
/// </summary>
public static class CSharpPlanningServiceCollectionExtensions
{
/// <summary>
/// 显式注册 C# ICSP 与 self-adapt-icsp 后端。
/// </summary>
/// <param name="services">当前宿主服务集合。</param>
/// <returns>同一服务集合,便于链式调用。</returns>
public static IServiceCollection AddCSharpPlanningBackends(this IServiceCollection services)
{
ArgumentNullException.ThrowIfNull(services);
services.AddSingleton<IPlanningBackend, CSharpIcspPlanningBackend>();
services.AddSingleton<IPlanningBackend, SelfAdaptIcspPlanningBackend>();
return services;
}
}

View File

@@ -0,0 +1,13 @@
<Project Sdk="Microsoft.NET.Sdk">
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
</ItemGroup>
<ItemGroup>
<PackageReference Include="Microsoft.Extensions.DependencyInjection.Abstractions" Version="8.0.2" />
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
</ItemGroup>
</Project>

View File

@@ -1,6 +1,6 @@
# ICSP 算法说明(`ICspPlanner`)
本文档用于解释 `Flyshot.Core.Planning.ICspPlanner` 当前实现的 **ICSP 规划算法**在本仓库中的真实含义与计算逻辑,便于与逆向结论对照、以及指导后续改造(例如“按约束生成中间点位”)。
本文档用于解释 `Flyshot.CSharp.Planning.ICspPlanner` 当前实现的 **C# ICSP 规划算法**在本仓库中的真实含义与计算逻辑,便于与逆向结论对照、以及指导后续改造(例如“按约束生成中间点位”)。
> 适用范围:本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代(retiming)”** 版本。
> 重要澄清:`ICspPlanner` 的主要输出是 **时间轴**(每个示教点的时间戳),而不是直接输出固定周期的稠密点序列;稠密点在后续采样层生成。
@@ -186,9 +186,8 @@ globalScale > 1 + \text{finalScaleTolerance}
## 7. 关联实现位置(便于跳转)
- 算法入口:`src/Flyshot.Core.Planning/ICspPlanner.cs`
- 自适应补点:`src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs`
- 算法入口:`src/Flyshot.CSharp.Planning/ICspPlanner.cs`
- 自适应补点:`src/Flyshot.CSharp.Planning/SelfAdaptIcspPlanner.cs`
- 三次样条实现(clamped-zero + 解析导峰值):`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs`
- 规划层稠密采样:`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs`
- J519 实发重采样:`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs`

View File

@@ -1,7 +1,8 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Planning;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。
@@ -196,7 +197,7 @@ public sealed class ICspPlanner
waypointTimes: bestWaypointTimes,
segmentDurations: bestDurations,
segmentScales: bestScales,
method: PlanningMethod.Icsp,
method: PlanningMethod.IcspCSharp,
iterations: bestIterations,
threshold: bestThreshold);
}

View File

@@ -1,7 +1,8 @@
using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Planning;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 在 ICSP 外层包裹补中点策略,实现 self-adapt-icsp 行为。

View File

@@ -0,0 +1,44 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 将 C# self-adapt-icsp 规划器暴露为可选规划后端。
/// </summary>
public sealed class SelfAdaptIcspPlanningBackend : IPlanningBackend
{
private readonly SelfAdaptIcspPlanner _planner;
/// <summary>
/// 初始化 C# self-adapt-icsp 后端。
/// </summary>
/// <param name="logger">self-adapt-icsp 规划器日志记录器;允许为空。</param>
public SelfAdaptIcspPlanningBackend(ILogger<SelfAdaptIcspPlanner>? logger = null)
{
_planner = new SelfAdaptIcspPlanner(logger);
}
/// <summary>
/// 获取外部调用 self-adapt-icsp 后端时使用的准确方法名。
/// </summary>
public string MethodName => "self-adapt-icsp";
/// <summary>
/// 获取 self-adapt-icsp 后端写入结果对象的规划方法。
/// </summary>
public PlanningMethod ResultMethod => PlanningMethod.SelfAdaptIcsp;
/// <summary>
/// 执行 C# 自适应 ICSP 规划,按上下文使用配置中的补点次数。
/// </summary>
/// <param name="request">包含机器人约束、示教路点和触发元数据的规划请求。</param>
/// <param name="context">包含 self-adapt-icsp 最大补点次数的规划上下文。</param>
/// <returns>由 self-adapt-icsp 规划器生成的轨迹结果。</returns>
public PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context)
{
ArgumentNullException.ThrowIfNull(context);
return _planner.Plan(request, context.AdaptIcspTryNum);
}
}

View File

@@ -20,6 +20,11 @@ public sealed class ControllerClientCompatOptions
/// </summary>
public string? WorkspaceRoot { get; set; }
/// <summary>
/// 获取或设置服务启动时自动初始化机器人的配置;为 null 时跳过自动初始化。
/// </summary>
public AutoInitOptions? AutoInit { get; set; }
/// <summary>
/// 解析运行配置根目录,确保运行时默认不再依赖源码仓库位置。
/// </summary>
@@ -44,3 +49,19 @@ public sealed class ControllerClientCompatOptions
: Path.GetFullPath(WorkspaceRoot);
}
}
/// <summary>
/// 表示服务启动时自动初始化机器人的配置项。
/// </summary>
public sealed class AutoInitOptions
{
/// <summary>
/// 获取或设置机器人名称,用于加载机器人配置文件(如 LR_Mate_200iD)。
/// </summary>
public string RobotName { get; set; } = string.Empty;
/// <summary>
/// 获取或设置机器人 IP 地址,用于连接真实 FANUC 控制器。
/// </summary>
public string RobotIp { get; set; } = string.Empty;
}

View File

@@ -1,4 +1,6 @@
using Flyshot.Core.Config;
using Flyshot.Core.Planning;
using Flyshot.Native.Planning;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
using Microsoft.Extensions.Configuration;
@@ -31,6 +33,7 @@ public static class ControllerClientCompatServiceCollectionExtensions
services.AddSingleton<RobotModelLoader>();
services.AddSingleton<RobotConfigLoader>();
services.AddSingleton<ControllerClientCompatRobotCatalog>();
services.AddSingleton<IPlanningBackend, RustIcspPlanningBackend>();
services.AddSingleton<ControllerClientTrajectoryOrchestrator>();
services.AddSingleton<FlyshotTrajectoryArtifactWriter>();
services.AddSingleton<JsonFlyshotTrajectoryStore>();

View File

@@ -0,0 +1,102 @@
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// 保存状态页需要的 ControllerClient 兼容层元数据快照,避免状态页连续读取多个受保护字段。
/// </summary>
public sealed record ControllerClientStatusSnapshotMetadata
{
/// <summary>
/// 初始化一份兼容层状态元数据快照。
/// </summary>
/// <param name="isSetup">当前是否已经完成机器人初始化。</param>
/// <param name="robotName">当前机器人名称;未初始化时为空。</param>
/// <param name="degreesOfFreedom">当前机器人自由度;未初始化时为 0。</param>
/// <param name="uploadedTrajectories">当前已上传轨迹名称快照。</param>
/// <param name="serverVersion">兼容服务端版本。</param>
/// <param name="clientVersion">兼容客户端版本。</param>
public ControllerClientStatusSnapshotMetadata(
bool isSetup,
string? robotName,
int degreesOfFreedom,
IReadOnlyList<string> uploadedTrajectories,
string serverVersion,
string clientVersion,
string operatingMode = "ModelLoadFailed",
string initializationStage = "NotStarted",
string? robotIp = null,
string? lastConnectionError = null,
DateTimeOffset? lastConnectionErrorAt = null,
DateTimeOffset? lastInitializationAttemptAt = null,
bool canPlanOffline = false,
bool canExecuteRealMotion = false)
{
IsSetup = isSetup;
RobotName = robotName;
DegreesOfFreedom = degreesOfFreedom;
UploadedTrajectories = uploadedTrajectories.ToArray();
ServerVersion = serverVersion;
ClientVersion = clientVersion;
OperatingMode = operatingMode;
InitializationStage = initializationStage;
RobotIp = robotIp;
LastConnectionError = lastConnectionError;
LastConnectionErrorAt = lastConnectionErrorAt;
LastInitializationAttemptAt = lastInitializationAttemptAt;
CanPlanOffline = canPlanOffline;
CanExecuteRealMotion = canExecuteRealMotion;
}
/// <summary>
/// 获取当前是否已经完成机器人初始化。
/// </summary>
public bool IsSetup { get; }
/// <summary>
/// 获取当前机器人名称;未初始化时为空。
/// </summary>
public string? RobotName { get; }
/// <summary>
/// 获取当前机器人自由度;未初始化时为 0。
/// </summary>
public int DegreesOfFreedom { get; }
/// <summary>
/// 获取已上传轨迹名称数组快照。
/// </summary>
public IReadOnlyList<string> UploadedTrajectories { get; }
/// <summary>
/// 获取兼容服务端版本。
/// </summary>
public string ServerVersion { get; }
/// <summary>
/// 获取兼容客户端版本。
/// </summary>
public string ClientVersion { get; }
/// <summary>获取当前运行模式。</summary>
public string OperatingMode { get; }
/// <summary>获取当前初始化阶段。</summary>
public string InitializationStage { get; }
/// <summary>获取最近尝试连接的机器人 IP。</summary>
public string? RobotIp { get; }
/// <summary>获取最后一次真机连接或使能错误。</summary>
public string? LastConnectionError { get; }
/// <summary>获取最后一次真机连接或使能错误发生时间。</summary>
public DateTimeOffset? LastConnectionErrorAt { get; }
/// <summary>获取最后一次初始化尝试时间。</summary>
public DateTimeOffset? LastInitializationAttemptAt { get; }
/// <summary>获取当前是否允许离线规划。</summary>
public bool CanPlanOffline { get; }
/// <summary>获取当前是否允许真机执行。</summary>
public bool CanExecuteRealMotion { get; }
}

View File

@@ -3,6 +3,7 @@ using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Flyshot.Native.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat;
@@ -22,8 +23,13 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary>
private const double DenseLimitStretchFactor = 1.01;
private readonly ICspPlanner _icspPlanner;
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
/// <summary>
/// 平滑起停重定时混合系数:0 表示不平滑,1 表示完全使用平滑时间律。
/// 当前取值用于“弱平滑”,仅轻微拉伸首尾时间段,避免起停过慢。
/// </summary>
private const double SmoothStartStopBlend = 0.60;
private readonly IReadOnlyDictionary<string, IPlanningBackend> _planningBackends;
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;
@@ -31,15 +37,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <summary>
/// 初始化轨迹编排器。
/// </summary>
/// <param name="planningBackends">显式注册的规划后端;为空时仅启用默认 Rust icsp 后端。</param>
/// <param name="logger">日志记录器;允许 null。</param>
/// <param name="loggerFactory">日志工厂;允许 null。</param>
public ControllerClientTrajectoryOrchestrator(
IEnumerable<IPlanningBackend>? planningBackends = null,
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
ILoggerFactory? loggerFactory = null)
{
_logger = logger;
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>());
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
_planningBackends = BuildPlanningBackendMap(planningBackends);
}
/// <summary>
@@ -47,21 +54,26 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary>
/// <param name="robot">当前机器人配置。</param>
/// <param name="waypoints">普通轨迹关节路点。</param>
/// <param name="options">执行参数。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次执行速度倍率;普通轨迹在规划阶段折算进有效限速倍率。</param>
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanOrdinaryTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> waypoints,
TrajectoryExecutionOptions? options = null,
double planningSpeedScale = 1.0)
double planningSpeedScale = 1.0,
double speedRatio = 1.0)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions();
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(planningSpeedScale, speedRatio, "普通轨迹");
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale);
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale, speedRatio, effectivePlanningSpeedScale);
var program = CreateProgram(
name: "ordinary-trajectory",
@@ -70,25 +82,31 @@ public sealed class ControllerClientTrajectoryOrchestrator
offsetValues: Enumerable.Repeat(0, waypoints.Count),
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
var method = ParseOrdinaryMethod(options.Method);
var methodName = ParseOrdinaryMethod(options.Method);
var backend = ResolvePlanningBackend(methodName);
var request = new TrajectoryRequest(
robot: planningRobot,
program: program,
method: method,
method: backend.ResultMethod,
saveTrajectoryArtifacts: options.SaveTrajectory);
var plannedTrajectory = PlanByMethod(request, method);
var plannedTrajectory = PlanByBackend(request, backend);
var executionTrajectory = plannedTrajectory;
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
var result = CreateResult(executionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
var result = CreateResult(
executionTrajectory,
shotTimeline,
denseJointTrajectory,
usedCache: false,
triggerSampleIndexOffsetCycles: 0);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
result.Duration.TotalSeconds,
result.DenseJointTrajectory?.Count ?? 0);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: executionTrajectory);
}
/// <summary>
@@ -96,24 +114,38 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary>
/// <param name="robot">当前机器人配置。</param>
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
/// <param name="options">执行参数。</param>
/// <param name="settings">兼容层机器人设置。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次飞拍使用的速度倍率;为空时使用配置默认值。</param>
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanUploadedFlyshot(
RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions? options = null,
CompatibilityRobotSettings? settings = null,
double? planningSpeedScale = null)
double? planningSpeedScale = null,
double? speedRatio = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(uploaded);
options ??= new FlyshotExecutionOptions();
settings ??= CreateDefaultRobotSettings();
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var requestedSpeedRatio = speedRatio ?? settings.SpeedRatio;
var configuredPlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(configuredPlanningSpeedScale, requestedSpeedRatio, "飞拍");
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale, settings.SmoothStartStopTiming);
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
uploaded.Name,
uploaded.Waypoints.Count,
options.Method,
options.UseCache,
configuredPlanningSpeedScale,
requestedSpeedRatio,
effectivePlanningSpeedScale,
settings.SmoothStartStopTiming);
var program = CreateProgram(
name: uploaded.Name,
@@ -122,33 +154,26 @@ public sealed class ControllerClientTrajectoryOrchestrator
offsetValues: uploaded.OffsetValues,
addressGroups: uploaded.AddressGroups);
var method = ParseFlyshotMethod(options.Method);
var methodName = ParseFlyshotMethod(options.Method);
var backend = ResolvePlanningBackend(methodName);
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
//if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
//{
// _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
// var executionTrajectory = ApplyExecutionTiming(cachedBundle.PlannedTrajectory, settings);
// var executionTimeline = _shotTimelineBuilder.Build(
// executionTrajectory,
// holdCycles: settings.IoKeepCycles,
// samplePeriod: planningRobot.ServoPeriod,
// useDo: settings.UseDo);
// // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志,规划轨迹和触发时间轴保持不可变复用。
// return new PlannedExecutionBundle(
// cachedBundle.PlannedTrajectory,
// executionTimeline,
// CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false));
//}
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
{
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志,规划轨迹和触发时间轴保持不可变复用。
return cachedBundle;
}
var request = new TrajectoryRequest(
robot: planningRobot,
program: program,
method: method,
method: backend.ResultMethod,
moveToStart: options.MoveToStart,
saveTrajectoryArtifacts: options.SaveTrajectory,
useCache: options.UseCache);
var plannedTrajectory = PlanByMethod(request, method, settings);
var plannedTrajectory = PlanByBackend(request, backend, settings);
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = _shotTimelineBuilder.Build(
@@ -156,8 +181,13 @@ public sealed class ControllerClientTrajectoryOrchestrator
holdCycles: settings.IoKeepCycles,
samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo);
var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
var result = CreateResult(
smoothedExecutionTrajectory,
shotTimeline,
denseJointTrajectory,
usedCache: false,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: smoothedExecutionTrajectory);
_logger?.LogInformation(
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
@@ -176,13 +206,15 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary>
/// <param name="method">旧 SDK 传入的方法名。</param>
/// <returns>领域层规划方法。</returns>
private static PlanningMethod ParseOrdinaryMethod(string method)
private static string ParseOrdinaryMethod(string method)
{
var normalized = NormalizeMethod(method);
return normalized switch
{
"icsp" => PlanningMethod.Icsp,
"doubles" => PlanningMethod.Doubles,
"icsp" => normalized,
"icsp-csharp" => normalized,
"icsp_rust" => throw new ArgumentException("method=\"icsp_rust\" 已改名为 method=\"icsp\"。", nameof(method)),
"doubles" => normalized,
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
};
}
@@ -192,33 +224,80 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary>
/// <param name="method">旧 SDK 传入的方法名。</param>
/// <returns>领域层规划方法。</returns>
private static PlanningMethod ParseFlyshotMethod(string method)
private static string ParseFlyshotMethod(string method)
{
var normalized = NormalizeMethod(method);
return normalized switch
{
"icsp" => PlanningMethod.Icsp,
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
"doubles" => PlanningMethod.Doubles,
"icsp" => normalized,
"icsp-csharp" => normalized,
"icsp_rust" => throw new ArgumentException("method=\"icsp_rust\" 已改名为 method=\"icsp\"。", nameof(method)),
"self-adapt-icsp" => normalized,
"doubles" => normalized,
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
};
}
/// <summary>
/// 按领域枚举分派到当前已经落地的规划器。
/// 使用显式注册的规划后端执行规划。
/// </summary>
/// <param name="request">规划请求。</param>
/// <param name="method">规划方法。</param>
/// <param name="backend">已解析的规划后端。</param>
/// <param name="settings">当前机器人兼容配置;为空时使用默认补点次数。</param>
/// <returns>规划轨迹。</returns>
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
private static PlannedTrajectory PlanByBackend(
TrajectoryRequest request,
IPlanningBackend backend,
CompatibilityRobotSettings? settings = null)
{
return method switch
var context = new PlanningBackendContext(settings?.AdaptIcspTryNum ?? 5);
return backend.Plan(request, context);
}
/// <summary>
/// 从已注册后端中解析 method 对应的规划实现。
/// </summary>
/// <param name="methodName">已经归一化的外部 method 名称。</param>
/// <returns>可执行该 method 的规划后端。</returns>
private IPlanningBackend ResolvePlanningBackend(string methodName)
{
if (methodName == "doubles")
{
PlanningMethod.Icsp => _icspPlanner.Plan(request),
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
};
throw new NotSupportedException("doubles 轨迹规划尚未落地。");
}
if (_planningBackends.TryGetValue(methodName, out var backend))
{
return backend;
}
throw new NotSupportedException($"轨迹规划方法 {methodName} 未注册对应后端。");
}
/// <summary>
/// 构造规划后端查找表,未显式注册时只启用默认 Rust icsp 后端。
/// </summary>
/// <param name="planningBackends">调用方显式注册的规划后端集合。</param>
/// <returns>按 method 名称索引的后端表。</returns>
private static IReadOnlyDictionary<string, IPlanningBackend> BuildPlanningBackendMap(IEnumerable<IPlanningBackend>? planningBackends)
{
var backends = planningBackends?.ToArray() ?? [];
if (backends.Length == 0)
{
backends = [new RustIcspPlanningBackend()];
}
var map = new Dictionary<string, IPlanningBackend>(StringComparer.Ordinal);
foreach (var backend in backends)
{
var methodName = NormalizeMethod(backend.MethodName);
if (!map.TryAdd(methodName, backend))
{
throw new InvalidOperationException($"规划后端 method={methodName} 被重复注册。");
}
}
return map;
}
/// <summary>
@@ -259,6 +338,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
hash.Add(options.SaveTrajectory);
hash.Add(settings.UseDo);
hash.Add(settings.IoKeepCycles);
hash.Add(settings.TriggerSampleIndexOffsetCycles);
hash.Add(settings.AdaptIcspTryNum);
hash.Add(settings.SmoothStartStopTiming);
@@ -309,11 +389,29 @@ public sealed class ControllerClientTrajectoryOrchestrator
useDo: true,
ioAddresses: Array.Empty<int>(),
ioKeepCycles: 0,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
}
/// <summary>
/// 解析飞拍生成轨迹时真正使用的规划速度倍率。
/// </summary>
/// <param name="planningSpeedScale">配置或调用方传入的规划倍率。</param>
/// <param name="speedRatio">本次执行速度倍率。</param>
/// <returns>用于缩放规划关节限制的有效倍率。</returns>
private static double ResolveEffectivePlanningSpeedScale(double planningSpeedScale, double speedRatio, string trajectoryKind)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), $"{trajectoryKind}速度倍率必须是有限正数。");
}
// 生成轨迹时把执行倍率折算进规划限制,保证 planning_speed_scale * speed_ratio 相同的配置产出同一条轨迹。
return planningSpeedScale * speedRatio;
}
/// <summary>
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
/// </summary>
@@ -346,7 +444,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
return robot;
}
// RVBUST 规划阶段会用独立限速倍率缩放有效限制;运行时 speedRatio 仍只负责 J519 下发重采样。
// speed_ratio 已在调用方折算到规划倍率中;运行时只消费已经生成好的 8ms 稠密轨迹。
var scaledLimits = robot.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
@@ -399,7 +497,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
PlannedTrajectory plannedTrajectory,
ShotTimeline shotTimeline,
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
bool usedCache)
bool usedCache,
int triggerSampleIndexOffsetCycles)
{
return new TrajectoryResult(
programName: plannedTrajectory.OriginalProgram.Name,
@@ -413,6 +512,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
usedCache: usedCache,
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
triggerSampleIndexOffsetCycles: triggerSampleIndexOffsetCycles,
denseJointTrajectory: denseJointTrajectory);
}
@@ -435,7 +535,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
executionTrajectory.Robot,
denseJointTrajectory,
trajectoryName: executionTrajectory.OriginalProgram.Name);
trajectoryName: executionTrajectory.OriginalProgram.Name,
validateJerk: false);
return denseJointTrajectory;
}
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
@@ -476,36 +577,51 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
/// <summary>
/// 为飞拍执行生成一条平滑起停的时间轴。
/// 保持路点位置不变,只重映射路点时刻,让起点和终点附近的速度自然收敛。
/// 为飞拍执行生成平滑起停时间轴(仅重定时,不改几何路点)。
/// 该方法保持 <see cref="PlannedTrajectory.PlannedWaypoints"/> 不变,只重映射 <see cref="PlannedTrajectory.WaypointTimes"/>,
/// 让轨迹在起点和终点附近具有更柔和的速度变化,从而降低“突然起步/突然收尾”带来的离散差分尖峰。
/// </summary>
/// <param name="plannedTrajectory">规划器输出的原始轨迹(通常是线性时间轴)。</param>
/// <returns>
/// 若满足平滑条件,则返回新的重定时轨迹;若路点数量不足或总时长无效,则直接返回输入轨迹。
/// </returns>
private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
{
var originalTimes = plannedTrajectory.WaypointTimes;
// 至少需要“起点-中间点-终点”三类点,才有可平滑的中间区间。
if (originalTimes.Count < 3)
{
return plannedTrajectory;
}
var totalDuration = originalTimes[^1];
// 轨迹总时长必须为正,避免后续归一化进度出现除零或无意义映射。
if (totalDuration <= 0.0)
{
return plannedTrajectory;
}
var smoothedTimes = new double[originalTimes.Count];
// 强制固定边界:起点仍在 0,终点仍在总时长,保证任务总耗时不变。
smoothedTimes[0] = 0.0;
smoothedTimes[^1] = totalDuration;
for (var index = 1; index < originalTimes.Count - 1; index++)
{
// 把原始时刻归一化到 [0, 1],用于统一时间律变换。
var normalizedProgress = originalTimes[index] / totalDuration;
smoothedTimes[index] = totalDuration * InvertSmoothStartStopProgress(normalizedProgress);
// 先求完整平滑时间律对应的时刻,再与原线性时刻按比例混合。
// 这里采用弱平滑(blend=0.35):保留大部分原节奏,仅轻微降低首尾突变。
var smoothedProgress = InvertSmoothStartStopProgress(normalizedProgress);
var blendedProgress = ((1.0 - SmoothStartStopBlend) * normalizedProgress)
+ (SmoothStartStopBlend * smoothedProgress);
smoothedTimes[index] = totalDuration * blendedProgress;
}
var segmentDurations = new double[smoothedTimes.Length - 1];
for (var index = 0; index < segmentDurations.Length; index++)
{
// 重建每段持续时间,供后续稠密采样和限幅检查使用。
segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
}
@@ -522,17 +638,24 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
/// <summary>
/// 反解 7 次 smootherstep 的时间进度,用二分法把原始线性进度映射成平滑时间轴。
/// 反解平滑时间律进度:给定目标进度 p,求满足 f(t)=p 的 t。
/// 其中 f(t) 由 <see cref="EvaluateSmoothStartStopProgress"/> 给出,是单调递增的 7 次 smootherstep 曲线,
/// 通过二分法可稳定得到对应的归一化时间,避免显式求高次方程根带来的复杂性和数值不稳定。
/// </summary>
/// <param name="normalizedProgress">目标归一化进度,理论范围 [0, 1]。</param>
/// <returns>与目标进度对应的归一化时间,范围 [0, 1]。</returns>
private static double InvertSmoothStartStopProgress(double normalizedProgress)
{
// 防御式裁剪,避免上游浮点误差将进度推到区间外。
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
var low = 0.0;
var high = 1.0;
// 固定迭代次数确保耗时可预测;40 次足以把区间收敛到工程可用精度。
for (var iteration = 0; iteration < 40; iteration++)
{
var middle = (low + high) / 2.0;
var progress = EvaluateSmoothStartStopProgress(middle);
// f(middle) 小于目标,说明根在右半区间;否则在左半区间。
if (progress < target)
{
low = middle;
@@ -547,10 +670,15 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
/// <summary>
/// 计算 7 次 smootherstep 进度值,用于整段平滑起停时间律。
/// 计算 7 次 smootherstep 的进度函数值 f(u)。
/// 该函数在 u=0 和 u=1 处具有更高阶导数连续性,可让起停段速度变化更平滑,
/// 适合用于飞拍轨迹的时间重参数化,减少离散导数在边界处的突变。
/// </summary>
/// <param name="normalizedTime">归一化时间 u,理论范围 [0, 1]。</param>
/// <returns>归一化进度 f(u),范围 [0, 1]。</returns>
private static double EvaluateSmoothStartStopProgress(double normalizedTime)
{
// 先裁剪再计算多项式,确保数值稳定且不受外部越界输入影响。
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
var u2 = u * u;
var u3 = u2 * u;

View File

@@ -5,6 +5,7 @@
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
<ProjectReference Include="..\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
<ProjectReference Include="..\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj" />
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
<ProjectReference Include="..\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
</ItemGroup>

View File

@@ -9,7 +9,7 @@ public sealed class FlyshotExecutionOptions
/// 初始化飞拍轨迹执行参数。
/// </summary>
/// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param>
/// <param name="method">轨迹生成方法,支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
/// <param name="method">轨迹生成方法,支持默认 Rust `icsp`、可选 C# `icsp-csharp`、`self-adapt-icsp` 或 `doubles`。</param>
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
/// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param>

View File

@@ -49,9 +49,27 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="bundle">规划结果包。</param>
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
/// <param name="bundle">规划结果包;旧规划导出和实发诊断使用同一份视图。</param>
/// <param name="speedRatio">请求速度倍率,仅作为诊断列保留;最终采样倍率已在规划阶段处理。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
{
WriteUploadedFlyshot(trajectoryName, robot, bundle, bundle, speedRatio);
}
/// <summary>
/// 将飞拍规划结果导出到 Config/Data/name,并显式区分旧规划导出视图和实发诊断视图。
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="exportPlanningBundle">旧格式规划导出使用的结果包,不应包含运行时 speedRatio 折算。</param>
/// <param name="executionBundle">实发诊断使用的结果包,应与运行时最终发送队列保持一致。</param>
/// <param name="speedRatio">请求速度倍率,仅作为 ActualSend 诊断列保留。</param>
public void WriteUploadedFlyshot(
string trajectoryName,
RobotProfile robot,
PlannedExecutionBundle exportPlanningBundle,
PlannedExecutionBundle executionBundle,
double speedRatio = 1.0)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
@@ -59,32 +77,32 @@ public sealed class FlyshotTrajectoryArtifactWriter
}
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(bundle);
ArgumentNullException.ThrowIfNull(exportPlanningBundle);
ArgumentNullException.ThrowIfNull(executionBundle);
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
Directory.CreateDirectory(outputDir);
if (bundle.Result.DenseJointTrajectory is null)
if (exportPlanningBundle.Result.DenseJointTrajectory is null)
{
throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。");
throw new InvalidOperationException("导出飞拍旧格式规划工件前必须先生成规划明细轨迹。");
}
// 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”,避免再次从 PlannedTrajectory 生成另一条轨迹。
// 旧格式规划文件固定使用不含运行时 speedRatio 的规划视图,避免和 ActualSend 实发诊断混在一起。
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
var jointTrajectory = BuildJointRows(exportPlanningBundle.PlannedTrajectory);
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
var executionDenseTrajectory = bundle.Result.DenseJointTrajectory;
var executionDenseTrajectory = exportPlanningBundle.Result.DenseJointTrajectory;
var jointDetailTrajectory = DownsampleDenseRows(
executionDenseTrajectory,
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
var cartTrajectory = BuildCartesianRows(exportPlanningBundle.PlannedTrajectory, kinematicsModel);
var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents);
WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio);
WriteActualSendArtifacts(outputDir, robot, executionBundle.Result, speedRatio);
_logger?.LogInformation(
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
@@ -102,37 +120,52 @@ public sealed class FlyshotTrajectoryArtifactWriter
{
ArgumentNullException.ThrowIfNull(robot);
if (result.DenseJointTrajectory is null)
var preparedExecution = result.PreparedFlyshotExecution;
if (result.DenseJointTrajectory is null && preparedExecution is null)
{
return;
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
var samples = preparedExecution is null
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio: 1.0)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
sample.TrajectoryTime,
sample.SpeedRatio,
sample.JointsDegrees)).ToArray();
var triggerBindings = preparedExecution is null
? TriggerSampleBinder.Bind(
result.TriggerTimeline,
samples,
trajectoryName: result.ProgramName);
}
catch (Exception e)
{
_logger?.LogError(e, "ValidateJ519SendSamples 失败,program={ProgramName}", result.ProgramName);
}
result.TriggerSampleIndexOffsetCycles)
: preparedExecution.TriggerBindings.Select(static binding =>
new TriggerSampleBinding(
binding.Trigger,
new J519SendSample(
binding.Sample.SampleIndex,
binding.Sample.SendTime,
binding.Sample.TrajectoryTime,
binding.Sample.SpeedRatio,
binding.Sample.JointsDegrees),
binding.SampleIndex,
binding.FoundInWindow)).ToArray();
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName,
validateJerk: false);
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
var timingRows = new List<IReadOnlyList<double>>(samples.Count);
var jerkRows = new List<IReadOnlyList<double>>();
List<IReadOnlyList<double>> timingRows = preparedExecution is null
? new List<IReadOnlyList<double>>(samples.Count)
: preparedExecution.TimingRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
List<IReadOnlyList<double>> jerkRows = preparedExecution is null
? []
: preparedExecution.JerkRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
double? previousSendTime = null;
double[]? previousJoints = null;
double[]? previousVelocity = null;
@@ -141,17 +174,20 @@ public sealed class FlyshotTrajectoryArtifactWriter
foreach (var sample in samples)
{
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
if (previousSendTime is not null && previousJoints is not null)
if (preparedExecution is null)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sample.SendTime,
previousJoints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample, speedRatio, stretchIterationCount: 0));
if (previousSendTime is not null && previousJoints is not null)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sample.SendTime,
previousJoints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
}
}
previousSendTime = sample.SendTime;
@@ -161,6 +197,7 @@ public sealed class FlyshotTrajectoryArtifactWriter
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), jointRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), timingRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), jerkRows);
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
}
/// <summary>

View File

@@ -34,14 +34,14 @@ public sealed class JsonFlyshotTrajectoryStore
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="settings">当前机器人级兼容配置。</param>
/// <param name="trajectory">要保存的已上传轨迹。</param>
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
public void Save(CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{
ArgumentNullException.ThrowIfNull(settings);
ArgumentNullException.ThrowIfNull(trajectory);
_logger?.LogInformation(
"RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
robotName,
"fixed-config",
trajectory.Name,
trajectory.Waypoints.Count);
@@ -82,19 +82,30 @@ public sealed class JsonFlyshotTrajectoryStore
_logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
}
/// <summary>
/// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。
/// </summary>
/// <param name="robotName">仅用于兼容的机器人名称。</param>
/// <param name="settings">机器人级兼容配置。</param>
/// <param name="trajectory">待保存轨迹。</param>
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{
Save(settings, trajectory);
}
/// <summary>
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="trajectoryName">要删除的轨迹名称。</param>
public void Delete(string robotName, string trajectoryName)
public void Delete(string trajectoryName)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
}
_logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName);
_logger?.LogInformation("RobotConfig 删除轨迹: name={TrajectoryName}", trajectoryName);
var path = ResolveStorePath();
if (!File.Exists(path))
@@ -131,13 +142,23 @@ public sealed class JsonFlyshotTrajectoryStore
}
}
/// <summary>
/// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。
/// </summary>
/// <param name="robotName">仅用于兼容的机器人名称。</param>
/// <param name="trajectoryName">待删除轨迹名称。</param>
public void Delete(string robotName, string trajectoryName)
{
Delete(trajectoryName);
}
/// <summary>
/// 加载统一 RobotConfig.json 中的所有轨迹,并回传机器人配置。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="settings">输出 RobotConfig.json 中的机器人配置;若文件不存在或解析失败则为 null。</param>
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(out CompatibilityRobotSettings? settings)
{
var path = ResolveStorePath();
if (!File.Exists(path))
@@ -168,7 +189,7 @@ public sealed class JsonFlyshotTrajectoryStore
_logger?.LogInformation(
"RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
robotName,
"fixed-config",
dict.Count,
settings?.UseDo,
settings?.IoKeepCycles);
@@ -183,6 +204,19 @@ public sealed class JsonFlyshotTrajectoryStore
}
}
/// <summary>
/// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。
/// </summary>
/// <param name="robotName">仅用于兼容的机器人名称。</param>
/// <param name="settings">输出机器人配置。</param>
/// <returns>固定 RobotConfig.json 中的全部飞拍资产。</returns>
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(
string robotName,
out CompatibilityRobotSettings? settings)
{
return LoadAll(out settings);
}
/// <summary>
/// 把机器人兼容配置序列化为 JSON 对象,字段名与旧版 RobotConfig.json 一致。
/// </summary>
@@ -193,10 +227,12 @@ public sealed class JsonFlyshotTrajectoryStore
["use_do"] = JsonValue.Create(settings.UseDo),
["io_addr"] = JsonSerializer.SerializeToNode(settings.IoAddresses),
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
["trigger_sample_index_offset_cycles"] = JsonValue.Create(settings.TriggerSampleIndexOffsetCycles),
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
["speed_ratio"] = JsonValue.Create(settings.SpeedRatio),
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
};
}

View File

@@ -17,25 +17,6 @@ public interface IControllerClientCompatService
/// </summary>
bool IsSetUp { get; }
/// <summary>
/// 保存当前调用方期望连接的 replacement 服务端地址。
/// </summary>
/// <param name="serverIp">客户端传入的服务端 IP。</param>
/// <param name="port">客户端传入的服务端端口。</param>
void ConnectServer(string serverIp, int port);
/// <summary>
/// 获取兼容服务端版本号。
/// </summary>
/// <returns>服务端版本号。</returns>
string GetServerVersion();
/// <summary>
/// 获取兼容客户端版本号。
/// </summary>
/// <returns>客户端版本号。</returns>
string GetClientVersion();
/// <summary>
/// 根据旧客户端使用的机器人名称完成机器人初始化。
/// </summary>
@@ -43,24 +24,12 @@ public interface IControllerClientCompatService
void SetUpRobot(string robotName);
/// <summary>
/// 根据旧客户端传入的环境文件完成机器人初始化。
/// 按“加载模型、连接真机、使能机器人”的固定顺序执行初始化。
/// </summary>
/// <param name="envFile">环境文件路径。</param>
void SetUpRobotFromEnv(string envFile);
/// <summary>
/// 设置是否显示 TCP 坐标轴。
/// </summary>
/// <param name="isShow">是否显示 TCP。</param>
/// <param name="axisLength">坐标轴长度。</param>
/// <param name="axisSize">坐标轴线宽。</param>
void SetShowTcp(bool isShow, double axisLength, int axisSize);
/// <summary>
/// 记录当前激活的控制器类型。
/// </summary>
/// <param name="sim">是否为仿真控制器。</param>
void SetActiveController(bool sim);
/// <param name="robotName">机器人名称。</param>
/// <param name="robotIp">机器人控制器 IP。</param>
/// <param name="bufferSize">J519 发送缓冲区大小。</param>
void InitializeRobot(string robotName, string robotIp, int bufferSize = 8);
/// <summary>
/// 记录当前控制器已经建立连接。
@@ -95,6 +64,12 @@ public interface IControllerClientCompatService
/// <returns>控制器运行时状态快照。</returns>
ControllerStateSnapshot GetControllerSnapshot();
/// <summary>
/// 读取状态页所需的兼容层元数据快照。
/// </summary>
/// <returns>兼容层元数据快照。</returns>
ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata();
/// <summary>
/// 获取当前速度倍率。
/// </summary>
@@ -157,6 +132,12 @@ public interface IControllerClientCompatService
/// <param name="jointPositions">目标关节位置。</param>
void MoveJoint(IReadOnlyList<double> jointPositions);
/// <summary>
/// 按直角坐标移动到目标 TCP 位姿。
/// </summary>
/// <param name="pose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
void MovePose(IReadOnlyList<double> pose);
/// <summary>
/// 执行普通轨迹。
/// </summary>
@@ -194,7 +175,7 @@ public interface IControllerClientCompatService
/// </summary>
/// <param name="name">轨迹名称。</param>
/// <param name="method">轨迹生成方法。</param>
void SaveTrajectoryInfo(string name, string method = "icsp");
OfflineFlyshotPlanningResult SaveTrajectoryInfo(string name, string method = "icsp");
/// <summary>
/// 检查指定飞拍轨迹是否可执行。

View File

@@ -53,7 +53,7 @@ internal static class MoveJointTrajectoryGenerator
///
/// 处理流程:
/// 1. 根据关节限位计算连续时间律理论最短时长
/// 2. 按 speedRatio 换算轨迹采样周期,并将时长对齐到整数个采样间隔
/// 2. 按 speedRatio 缩放规划限位,并将时长对齐到固定伺服采样间隔
/// 3. 用 7 阶平滑点到点时间律生成稠密轨迹点
/// 4. 按离散点反查速度、加速度和 jerk,必要时拉长时长重算
/// 5. 封装为 TrajectoryResult 返回
@@ -61,7 +61,7 @@ internal static class MoveJointTrajectoryGenerator
/// <param name="robot">机器人配置,含自由度数和关节限位。</param>
/// <param name="startJoints">起始关节角(弧度)。</param>
/// <param name="targetJoints">目标关节角(弧度)。</param>
/// <param name="speedRatio">速度倍率,必须大于 0;当前链路中用于换算轨迹采样周期。</param>
/// <param name="speedRatio">速度倍率,必须大于 0;在生成阶段折算进有效关节限位。</param>
/// <param name="logger">可选的诊断日志。</param>
public static TrajectoryResult CreateResult(
RobotProfile robot,
@@ -84,13 +84,14 @@ internal static class MoveJointTrajectoryGenerator
throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
}
var requestedDurationSeconds = ResolveDurationSeconds(robot, startJoints, targetJoints);
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds * speedRatio;
var planningRobot = ApplySpeedRatioToJointLimits(robot, speedRatio);
var requestedDurationSeconds = ResolveDurationSeconds(planningRobot, startJoints, targetJoints);
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds;
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
var stretchCount = 0;
while (!SatisfiesDiscreteJointLimits(robot, denseJointTrajectory))
while (!SatisfiesDiscreteJointLimits(planningRobot, denseJointTrajectory))
{
stretchCount++;
if (stretchCount > MaxDiscreteLimitStretchIterations)
@@ -127,6 +128,34 @@ internal static class MoveJointTrajectoryGenerator
denseJointTrajectory: denseJointTrajectory);
}
/// <summary>
/// 将执行速度倍率折算为 MoveJoint 规划限位,保持最终稠密轨迹仍按物理 8ms 周期生成。
/// </summary>
private static RobotProfile ApplySpeedRatioToJointLimits(RobotProfile robot, double speedRatio)
{
if (Math.Abs(speedRatio - 1.0) < 1e-12)
{
return robot;
}
var scaledLimits = robot.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
limit.VelocityLimit * speedRatio,
limit.AccelerationLimit * speedRatio * speedRatio,
limit.JerkLimit * speedRatio * speedRatio * speedRatio))
.ToArray();
return new RobotProfile(
name: robot.Name,
modelPath: robot.ModelPath,
degreesOfFreedom: robot.DegreesOfFreedom,
jointLimits: scaledLimits,
jointCouplings: robot.JointCouplings,
servoPeriod: robot.ServoPeriod,
triggerPeriod: robot.TriggerPeriod);
}
/// <summary>
/// 根据 7 阶平滑点到点时间律和每轴限位,计算 MoveJoint 理论最短时长。
///

View File

@@ -0,0 +1,353 @@
using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// MovePose 直角坐标轨迹生成器。
/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动,按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。
/// </summary>
internal static class MovePoseTrajectoryGenerator
{
/// <summary>
/// 直角位置轴最大速度,单位为 mm/s。
/// </summary>
private const double LinearVelocityLimit = 50.0;
/// <summary>
/// 直角位置轴最大加速度,单位为 mm/s^2。
/// </summary>
private const double LinearAccelerationLimit = 250.0;
/// <summary>
/// 直角位置轴最大 jerk,单位为 mm/s^3。
/// </summary>
private const double LinearJerkLimit = 1250.0;
/// <summary>
/// 姿态轴最大速度,单位为 deg/s。
/// </summary>
private const double AngularVelocityLimit = 10.0;
/// <summary>
/// 姿态轴最大加速度,单位为 deg/s^2。
/// </summary>
private const double AngularAccelerationLimit = 50.0;
/// <summary>
/// 姿态轴最大 jerk,单位为 deg/s^3。
/// </summary>
private const double AngularJerkLimit = 250.0;
/// <summary>
/// 7 阶平滑点到点时间律的一阶导数最大值。
/// </summary>
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
/// <summary>
/// 7 阶平滑点到点时间律的二阶导数最大值。
/// </summary>
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
/// <summary>
/// 7 阶平滑点到点时间律的三阶导数最大值。
/// </summary>
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 单次 MovePose 最大采样点数上限,避免极低速度倍率生成过大的队列。
/// </summary>
private const int MaxMovePoseSampleCount = 1_000_000;
/// <summary>
/// 离散限位校验允许的浮点容差。
/// </summary>
private const double DiscreteLimitTolerance = 1.000001;
/// <summary>
/// 离散限位校验失败时最多拉长的采样周期次数。
/// </summary>
private const int MaxDiscreteLimitStretchIterations = 10_000;
/// <summary>
/// 计算 MovePose 轨迹的完整结果。
/// </summary>
/// <param name="startPose">起始位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="servoPeriod">J519 伺服发送周期。</param>
/// <param name="speedRatio">速度倍率,必须大于 0。</param>
/// <param name="logger">可选诊断日志。</param>
/// <returns>包含稠密直角轨迹的规划结果。</returns>
public static TrajectoryResult CreateResult(
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose,
TimeSpan servoPeriod,
double speedRatio,
ILogger? logger = null)
{
ArgumentNullException.ThrowIfNull(startPose);
ArgumentNullException.ThrowIfNull(targetPose);
EnsurePoseVector(startPose, nameof(startPose));
EnsurePoseVector(targetPose, nameof(targetPose));
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose execution.");
}
var samplePeriodSeconds = servoPeriod.TotalSeconds;
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
{
throw new InvalidOperationException("MovePose servo period must be a finite positive duration.");
}
var requestedDurationSeconds = ResolveDurationSeconds(startPose, targetPose, speedRatio);
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
var denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
var stretchCount = 0;
while (!SatisfiesDefaultCartesianLimits(denseCartesianTrajectory, speedRatio))
{
stretchCount++;
if (stretchCount > MaxDiscreteLimitStretchIterations)
{
throw new InvalidOperationException("MovePose duration cannot be stretched enough to satisfy Cartesian limits.");
}
// 离散差分以真实下发点为准;不满足时逐周期拉长后重采样。
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
}
logger?.LogDebug(
"MovePoseTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
requestedDurationSeconds,
durationSeconds,
speedRatio,
samplePeriodSeconds,
denseCartesianTrajectory.Count,
stretchCount);
return new TrajectoryResult(
programName: "move-pose",
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: denseCartesianTrajectory.Count,
denseCartesianTrajectory: denseCartesianTrajectory);
}
/// <summary>
/// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。
/// </summary>
/// <param name="rows">稠密轨迹行,每行为 [time,x,y,z,w,p,r]。</param>
/// <param name="speedRatio">生成该轨迹时使用的速度倍率。</param>
/// <returns>满足限制时返回 true,否则返回 false。</returns>
public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList<IReadOnlyList<double>> rows, double speedRatio)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose limit validation.");
}
double? previousTime = null;
double[]? previousPositions = null;
double[]? previousVelocities = null;
double[]? previousAccelerations = null;
foreach (var row in rows)
{
if (row.Count != 7)
{
throw new InvalidOperationException("MovePose dense trajectory rows must contain time plus six pose values.");
}
var currentTime = row[0];
var currentPositions = row.Skip(1).Take(6).ToArray();
if (previousTime is not null && previousPositions is not null)
{
var dt = currentTime - previousTime.Value;
if (dt <= 0.0)
{
throw new InvalidOperationException("MovePose dense trajectory timestamps must be strictly increasing.");
}
var velocities = new double[6];
var accelerations = new double[6];
for (var index = 0; index < 6; index++)
{
var limits = GetAxisLimits(index, speedRatio);
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
if (Math.Abs(velocities[index]) > limits.Velocity * DiscreteLimitTolerance)
{
return false;
}
accelerations[index] = previousVelocities is null
? 0.0
: (velocities[index] - previousVelocities[index]) / dt;
if (Math.Abs(accelerations[index]) > limits.Acceleration * DiscreteLimitTolerance)
{
return false;
}
if (previousAccelerations is not null)
{
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
if (Math.Abs(jerk) > limits.Jerk * DiscreteLimitTolerance)
{
return false;
}
}
}
previousVelocities = velocities;
previousAccelerations = accelerations;
}
previousTime = currentTime;
previousPositions = currentPositions;
}
return true;
}
/// <summary>
/// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。
/// </summary>
private static double ResolveDurationSeconds(IReadOnlyList<double> startPose, IReadOnlyList<double> targetPose, double speedRatio)
{
var duration = 0.0;
for (var index = 0; index < 6; index++)
{
var distance = Math.Abs(targetPose[index] - startPose[index]);
if (distance <= 0.0)
{
continue;
}
var limits = GetAxisLimits(index, speedRatio);
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limits.Velocity;
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limits.Acceleration);
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limits.Jerk);
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
/// <summary>
/// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose,
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, startPose, targetPose));
}
return rows;
}
/// <summary>
/// 将请求时长向上对齐到整数个伺服周期。
/// </summary>
private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
{
return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds;
}
/// <summary>
/// 计算时长对应的采样间隔数,轨迹至少包含起点和终点两帧。
/// </summary>
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
{
var rawIntervals = durationSeconds / samplePeriodSeconds;
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
{
throw new InvalidOperationException("MovePose sample count is not representable.");
}
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
intervals = Math.Max(1, intervals);
if (intervals + 1 > MaxMovePoseSampleCount)
{
throw new InvalidOperationException($"MovePose sample count must be between 2 and {MaxMovePoseSampleCount}.");
}
return intervals;
}
/// <summary>
/// 构造单个轨迹行:[time_seconds,x,y,z,w,p,r]。
/// </summary>
private static IReadOnlyList<double> CreateRow(
double timeSeconds,
double durationSeconds,
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose)
{
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
var scale = MoveJointTrajectoryGenerator.EvaluateSmoothPtpPositionScale(u);
var row = new double[7];
row[0] = Math.Round(timeSeconds, 9);
for (var index = 0; index < 6; index++)
{
row[index + 1] = startPose[index] + ((targetPose[index] - startPose[index]) * scale);
}
return row;
}
/// <summary>
/// 获取指定直角轴在当前速度倍率下的有效限制。
/// </summary>
private static CartesianAxisLimit GetAxisLimits(int index, double speedRatio)
{
var linearAxis = index < 3;
var velocity = linearAxis ? LinearVelocityLimit : AngularVelocityLimit;
var acceleration = linearAxis ? LinearAccelerationLimit : AngularAccelerationLimit;
var jerk = linearAxis ? LinearJerkLimit : AngularJerkLimit;
return new CartesianAxisLimit(
velocity * speedRatio,
acceleration * speedRatio * speedRatio,
jerk * speedRatio * speedRatio * speedRatio);
}
/// <summary>
/// 校验位姿向量必须为六维有限数。
/// </summary>
private static void EnsurePoseVector(IReadOnlyList<double> pose, string parameterName)
{
if (pose.Count != 6)
{
throw new ArgumentException("MovePose expects pose [x,y,z,w,p,r].", parameterName);
}
if (pose.Any(static value => double.IsNaN(value) || double.IsInfinity(value)))
{
throw new ArgumentException("MovePose pose values must be finite.", parameterName);
}
}
/// <summary>
/// 表示单个直角轴的有效速度、加速度和 jerk 限制。
/// </summary>
private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk);
}

View File

@@ -0,0 +1,174 @@
using Flyshot.Core.Domain;
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// 表示离线飞拍规划返回给 HTTP、Debug 页面和导出链路的完整结果。
/// </summary>
public sealed class OfflineFlyshotPlanningResult
{
/// <summary>
/// 初始化一份离线飞拍规划结果。
/// </summary>
/// <param name="trajectoryName">轨迹名称。</param>
/// <param name="method">规划方法。</param>
/// <param name="isValid">规划结果是否有效。</param>
/// <param name="planningDurationSeconds">连续规划轨迹时长,单位为秒。</param>
/// <param name="finalSendDurationSeconds">最终发送时长,单位为秒。</param>
/// <param name="originalWaypointCount">原始示教路点数量。</param>
/// <param name="plannedWaypointCount">规划关键路点数量。</param>
/// <param name="densePointCount">稠密轨迹点数量。</param>
/// <param name="speedRatio">本次最终发送使用的速度倍率。</param>
/// <param name="servoPeriodSeconds">最终发送采样周期,单位为秒。</param>
/// <param name="plannedJointTrajectory">关键关节轨迹,每行为时间加弧度制关节值。</param>
/// <param name="denseJointTrajectory">稠密关节轨迹,每行为时间加弧度制关节值。</param>
/// <param name="cartesianTrajectory">稠密笛卡尔轨迹。</param>
/// <param name="finalSendPoints">最终 8ms 发送点列。</param>
/// <param name="shotEvents">理论拍照事件。</param>
/// <param name="triggerBindings">触发事件与最终发送点的绑定结果。</param>
/// <param name="artifactPaths">导出文件名与绝对路径。</param>
/// <param name="diagnostics">最终发送点列的速度、加速度和跃度统计。</param>
public OfflineFlyshotPlanningResult(
string trajectoryName,
string method,
bool isValid,
double planningDurationSeconds,
double finalSendDurationSeconds,
int originalWaypointCount,
int plannedWaypointCount,
int densePointCount,
double speedRatio,
double servoPeriodSeconds,
IEnumerable<IReadOnlyList<double>> plannedJointTrajectory,
IEnumerable<IReadOnlyList<double>> denseJointTrajectory,
IEnumerable<IReadOnlyList<double>> cartesianTrajectory,
IEnumerable<OfflineFlyshotSendPoint> finalSendPoints,
IEnumerable<ShotEvent> shotEvents,
IEnumerable<OfflineFlyshotTriggerBinding> triggerBindings,
IReadOnlyDictionary<string, string> artifactPaths,
OfflineFlyshotDiagnostics diagnostics)
{
TrajectoryName = trajectoryName;
Method = method;
IsValid = isValid;
PlanningDurationSeconds = planningDurationSeconds;
FinalSendDurationSeconds = finalSendDurationSeconds;
OriginalWaypointCount = originalWaypointCount;
PlannedWaypointCount = plannedWaypointCount;
DensePointCount = densePointCount;
SpeedRatio = speedRatio;
ServoPeriodSeconds = servoPeriodSeconds;
PlannedJointTrajectory = plannedJointTrajectory.Select(static row => row.ToArray()).ToArray();
DenseJointTrajectory = denseJointTrajectory.Select(static row => row.ToArray()).ToArray();
CartesianTrajectory = cartesianTrajectory.Select(static row => row.ToArray()).ToArray();
FinalSendPoints = finalSendPoints.ToArray();
ShotEvents = shotEvents.ToArray();
TriggerBindings = triggerBindings.ToArray();
ArtifactPaths = new Dictionary<string, string>(artifactPaths, StringComparer.Ordinal);
Diagnostics = diagnostics;
}
/// <summary>获取轨迹名称。</summary>
public string TrajectoryName { get; }
/// <summary>获取规划方法。</summary>
public string Method { get; }
/// <summary>获取规划结果是否有效。</summary>
public bool IsValid { get; }
/// <summary>获取连续规划轨迹时长,单位为秒。</summary>
public double PlanningDurationSeconds { get; }
/// <summary>获取最终发送时长,单位为秒。</summary>
public double FinalSendDurationSeconds { get; }
/// <summary>获取原始示教路点数量。</summary>
public int OriginalWaypointCount { get; }
/// <summary>获取规划关键路点数量。</summary>
public int PlannedWaypointCount { get; }
/// <summary>获取稠密轨迹点数量。</summary>
public int DensePointCount { get; }
/// <summary>获取最终发送点数量。</summary>
public int FinalSendPointCount => FinalSendPoints.Count;
/// <summary>获取本次最终发送使用的速度倍率。</summary>
public double SpeedRatio { get; }
/// <summary>获取最终发送采样周期,单位为秒。</summary>
public double ServoPeriodSeconds { get; }
/// <summary>获取关键关节轨迹。</summary>
public IReadOnlyList<IReadOnlyList<double>> PlannedJointTrajectory { get; }
/// <summary>获取稠密关节轨迹。</summary>
public IReadOnlyList<IReadOnlyList<double>> DenseJointTrajectory { get; }
/// <summary>获取稠密笛卡尔轨迹。</summary>
public IReadOnlyList<IReadOnlyList<double>> CartesianTrajectory { get; }
/// <summary>获取最终 8ms 发送点列。</summary>
public IReadOnlyList<OfflineFlyshotSendPoint> FinalSendPoints { get; }
/// <summary>获取理论拍照事件。</summary>
public IReadOnlyList<ShotEvent> ShotEvents { get; }
/// <summary>获取触发事件与最终发送点的绑定结果。</summary>
public IReadOnlyList<OfflineFlyshotTriggerBinding> TriggerBindings { get; }
/// <summary>获取导出文件名与绝对路径。</summary>
public IReadOnlyDictionary<string, string> ArtifactPaths { get; }
/// <summary>获取最终发送点列的速度、加速度和跃度统计。</summary>
public OfflineFlyshotDiagnostics Diagnostics { get; }
}
/// <summary>
/// 表示离线规划结果中的单个最终 J519 发送点。
/// </summary>
/// <param name="SampleIndex">发送周期序号。</param>
/// <param name="SendTime">物理发送时间,单位为秒。</param>
/// <param name="TrajectoryTime">映射到连续轨迹的采样时间,单位为秒。</param>
/// <param name="JointsDegrees">角度制关节目标。</param>
/// <param name="IoMask">该点写出的 IO 掩码。</param>
/// <param name="IoValue">该点写出的 IO 值。</param>
public sealed record OfflineFlyshotSendPoint(
long SampleIndex,
double SendTime,
double TrajectoryTime,
IReadOnlyList<double> JointsDegrees,
ushort IoMask,
ushort IoValue);
/// <summary>
/// 表示离线规划结果中的触发采样点绑定。
/// </summary>
/// <param name="WaypointIndex">原始示教路点索引。</param>
/// <param name="TriggerTime">理论触发时间,单位为秒。</param>
/// <param name="SampleIndex">最终发送点数组索引。</param>
/// <param name="SampleSequence">最终发送序号。</param>
/// <param name="FoundInWindow">是否在理论搜索窗口内完成绑定。</param>
/// <param name="IoMask">触发 IO 掩码。</param>
/// <param name="IoValue">触发 IO 值。</param>
public sealed record OfflineFlyshotTriggerBinding(
int WaypointIndex,
double TriggerTime,
int SampleIndex,
long SampleSequence,
bool FoundInWindow,
ushort IoMask,
ushort IoValue);
/// <summary>
/// 表示最终发送点列的离散运动学诊断统计。
/// </summary>
/// <param name="MaxAbsVelocityDegreesPerSecond">最大绝对速度,单位 deg/s。</param>
/// <param name="MaxAbsAccelerationDegreesPerSecondSquared">最大绝对加速度,单位 deg/s²。</param>
/// <param name="MaxAbsJerkDegreesPerSecondCubed">最大绝对跃度,单位 deg/s³。</param>
public sealed record OfflineFlyshotDiagnostics(
double MaxAbsVelocityDegreesPerSecond,
double MaxAbsAccelerationDegreesPerSecondSquared,
double MaxAbsJerkDegreesPerSecondCubed);

View File

@@ -12,21 +12,35 @@ public sealed class PlannedExecutionBundle
/// <summary>
/// 初始化一份执行规划结果包。
/// </summary>
/// <param name="plannedTrajectory">规划后的轨迹。</param>
/// <param name="plannedTrajectory">规划后的原始连续轨迹。</param>
/// <param name="shotTimeline">飞拍触发时间轴。</param>
/// <param name="result">对运行时和监控层暴露的规划结果。</param>
public PlannedExecutionBundle(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, TrajectoryResult result)
/// <param name="preparedExecution">飞拍链路预先准备好的最终发送队列。</param>
/// <param name="executionTrajectory">与运行时结果同口径的连续执行轨迹;为空时复用 plannedTrajectory。</param>
public PlannedExecutionBundle(
PlannedTrajectory plannedTrajectory,
ShotTimeline shotTimeline,
TrajectoryResult result,
FlyshotPreparedExecution? preparedExecution = null,
PlannedTrajectory? executionTrajectory = null)
{
PlannedTrajectory = plannedTrajectory ?? throw new ArgumentNullException(nameof(plannedTrajectory));
ShotTimeline = shotTimeline ?? throw new ArgumentNullException(nameof(shotTimeline));
Result = result ?? throw new ArgumentNullException(nameof(result));
PreparedExecution = preparedExecution;
ExecutionTrajectory = executionTrajectory ?? plannedTrajectory;
}
/// <summary>
/// 获取规划后的轨迹。
/// 获取规划后的原始连续轨迹。
/// </summary>
public PlannedTrajectory PlannedTrajectory { get; }
/// <summary>
/// 获取与运行时结果同口径的连续执行轨迹,用于构建最终 8ms 发送队列。
/// </summary>
public PlannedTrajectory ExecutionTrajectory { get; }
/// <summary>
/// 获取飞拍触发时间轴。
/// </summary>
@@ -36,4 +50,9 @@ public sealed class PlannedExecutionBundle
/// 获取运行时可消费的规划结果。
/// </summary>
public TrajectoryResult Result { get; }
/// <summary>
/// 获取飞拍链路预先准备好的最终发送队列;普通轨迹与 move_joint 为 null。
/// </summary>
public FlyshotPreparedExecution? PreparedExecution { get; }
}

View File

@@ -8,7 +8,7 @@ public sealed class TrajectoryExecutionOptions
/// <summary>
/// 初始化普通轨迹执行参数。
/// </summary>
/// <param name="method">轨迹生成方法,支持 `icsp` 或 `doubles`。</param>
/// <param name="method">轨迹生成方法,支持默认 Rust `icsp`、可选 C# `icsp-csharp` 或 `doubles`。</param>
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false)
{

View File

@@ -16,10 +16,12 @@ public sealed class CompatibilityRobotSettings
bool useDo,
IEnumerable<int> ioAddresses,
int ioKeepCycles,
int triggerSampleIndexOffsetCycles,
double accLimitScale,
double jerkLimitScale,
int adaptIcspTryNum,
double planningSpeedScale = 1.0,
double speedRatio = 1.0,
bool smoothStartStopTiming = true)
{
ArgumentNullException.ThrowIfNull(ioAddresses);
@@ -29,6 +31,11 @@ public sealed class CompatibilityRobotSettings
throw new ArgumentOutOfRangeException(nameof(ioKeepCycles), "IO 保持周期不能为负数。");
}
if (triggerSampleIndexOffsetCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(triggerSampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
}
if (accLimitScale <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
@@ -44,6 +51,11 @@ public sealed class CompatibilityRobotSettings
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
}
if (speedRatio <= 0.0 || speedRatio > 1.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "运行时速度倍率必须在 (0, 1] 范围内。");
}
if (adaptIcspTryNum < 0)
{
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
@@ -59,10 +71,12 @@ public sealed class CompatibilityRobotSettings
UseDo = useDo;
IoAddresses = copiedIoAddresses;
IoKeepCycles = ioKeepCycles;
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
AccLimitScale = accLimitScale;
JerkLimitScale = jerkLimitScale;
AdaptIcspTryNum = adaptIcspTryNum;
PlanningSpeedScale = planningSpeedScale;
SpeedRatio = speedRatio;
SmoothStartStopTiming = smoothStartStopTiming;
}
@@ -81,6 +95,12 @@ public sealed class CompatibilityRobotSettings
/// </summary>
public int IoKeepCycles { get; }
/// <summary>
/// 获取触发绑定到最佳 sample 后,还要再向后偏移的命令周期数。
/// 该值作用在 J519 命令 sample 时间轴,不是 60015 状态反馈周期。
/// </summary>
public int TriggerSampleIndexOffsetCycles { get; }
/// <summary>
/// 获取加速度全局倍率。
/// </summary>
@@ -96,6 +116,11 @@ public sealed class CompatibilityRobotSettings
/// </summary>
public double PlanningSpeedScale { get; }
/// <summary>
/// 获取运行时默认速度倍率,会在 SetUpRobot 时同步到控制器运行时,供 MoveJoint 和后续执行链路使用。
/// </summary>
public double SpeedRatio { get; }
/// <summary>
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
/// </summary>
@@ -184,10 +209,12 @@ public sealed class RobotConfigLoader
useDo: ReadBoolean(robotElement, "use_do", defaultValue: false),
ioAddresses: ReadIntArray(robotElement, "io_addr"),
ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0),
triggerSampleIndexOffsetCycles: ReadInt(robotElement, "trigger_sample_index_offset_cycles", defaultValue: 7),
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0),
planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0),
speedRatio: ReadDouble(robotElement, "speed_ratio", defaultValue: 1.0),
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
@@ -199,8 +226,8 @@ public sealed class RobotConfigLoader
}
_logger?.LogInformation(
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SpeedRatio, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
return new LoadedRobotConfig(
sourcePath: resolvedConfigPath,

View File

@@ -0,0 +1,202 @@
namespace Flyshot.Core.Domain;
/// <summary>
/// 表示飞拍链路在进入运行时之前就已经准备完成的最终发送结果。
/// </summary>
public sealed class FlyshotPreparedExecution
{
/// <summary>
/// 初始化一份飞拍最终发送结果。
/// </summary>
/// <param name="samples">最终 8ms 发送点列。</param>
/// <param name="triggerBindings">与最终发送点列对齐的触发绑定结果。</param>
/// <param name="timingRows">与最终发送点列一致的时间映射诊断行。</param>
/// <param name="jerkRows">与最终发送点列一致的跃度诊断行。</param>
/// <param name="requestSpeedRatio">请求的执行倍率。</param>
/// <param name="finalSpeedRatio">最终发送队列采用的采样倍率;请求倍率已折算进规划时为 1。</param>
/// <param name="finalDurationSeconds">最终发送总时长,单位为秒。</param>
/// <param name="stretchIterationCount">历史诊断字段;当前执行侧不自动改写 speed_ratio,因此正常为 0。</param>
public FlyshotPreparedExecution(
IEnumerable<FlyshotPreparedSample> samples,
IEnumerable<FlyshotPreparedTriggerBinding> triggerBindings,
IEnumerable<IReadOnlyList<double>> timingRows,
IEnumerable<IReadOnlyList<double>> jerkRows,
double requestSpeedRatio,
double finalSpeedRatio,
double finalDurationSeconds,
int stretchIterationCount)
{
ArgumentNullException.ThrowIfNull(samples);
ArgumentNullException.ThrowIfNull(triggerBindings);
ArgumentNullException.ThrowIfNull(timingRows);
ArgumentNullException.ThrowIfNull(jerkRows);
if (requestSpeedRatio <= 0.0 || double.IsNaN(requestSpeedRatio) || double.IsInfinity(requestSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(requestSpeedRatio), "请求速度倍率必须是有限正数。");
}
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(finalSpeedRatio), "最终速度倍率必须是有限正数。");
}
if (finalDurationSeconds < 0.0 || double.IsNaN(finalDurationSeconds) || double.IsInfinity(finalDurationSeconds))
{
throw new ArgumentOutOfRangeException(nameof(finalDurationSeconds), "最终发送总时长必须是有限非负数。");
}
if (stretchIterationCount < 0)
{
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写迭代次数必须是非负整数。");
}
Samples = samples.Select(static sample => sample).ToArray();
TriggerBindings = triggerBindings.Select(static binding => binding).ToArray();
TimingRows = timingRows.Select(static row => row.ToArray()).ToArray();
JerkRows = jerkRows.Select(static row => row.ToArray()).ToArray();
RequestSpeedRatio = requestSpeedRatio;
FinalSpeedRatio = finalSpeedRatio;
FinalDurationSeconds = finalDurationSeconds;
StretchIterationCount = stretchIterationCount;
}
/// <summary>
/// 获取最终 8ms 发送点列。
/// </summary>
public IReadOnlyList<FlyshotPreparedSample> Samples { get; }
/// <summary>
/// 获取与最终发送点列对齐的触发绑定结果。
/// </summary>
public IReadOnlyList<FlyshotPreparedTriggerBinding> TriggerBindings { get; }
/// <summary>
/// 获取与最终发送点列一致的时间映射诊断行。
/// </summary>
public IReadOnlyList<IReadOnlyList<double>> TimingRows { get; }
/// <summary>
/// 获取与最终发送点列一致的跃度诊断行。
/// </summary>
public IReadOnlyList<IReadOnlyList<double>> JerkRows { get; }
/// <summary>
/// 获取请求的执行倍率。
/// </summary>
public double RequestSpeedRatio { get; }
/// <summary>
/// 获取最终发送队列采用的采样倍率;请求倍率保留在 <see cref="RequestSpeedRatio"/> 中用于诊断。
/// </summary>
public double FinalSpeedRatio { get; }
/// <summary>
/// 获取最终发送总时长,单位为秒。
/// </summary>
public double FinalDurationSeconds { get; }
/// <summary>
/// 获取历史诊断字段;当前执行侧不自动改写 speed_ratio,因此正常为 0。
/// </summary>
public int StretchIterationCount { get; }
}
/// <summary>
/// 表示飞拍最终发送队列中的一个 8ms 发送点。
/// </summary>
public sealed class FlyshotPreparedSample
{
/// <summary>
/// 初始化一条最终发送点。
/// </summary>
/// <param name="sampleIndex">发送周期序号。</param>
/// <param name="sendTime">物理发送时间,单位为秒。</param>
/// <param name="trajectoryTime">回映射到规划轨迹的采样时间,单位为秒。</param>
/// <param name="speedRatio">生成该发送点时采用的队列采样倍率,已规划调速路径通常为 1。</param>
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
public FlyshotPreparedSample(
long sampleIndex,
double sendTime,
double trajectoryTime,
double speedRatio,
IReadOnlyList<double> jointsDegrees)
{
ArgumentNullException.ThrowIfNull(jointsDegrees);
SampleIndex = sampleIndex;
SendTime = sendTime;
TrajectoryTime = trajectoryTime;
SpeedRatio = speedRatio;
JointsDegrees = jointsDegrees.ToArray();
}
/// <summary>
/// 获取发送周期序号。
/// </summary>
public long SampleIndex { get; }
/// <summary>
/// 获取物理发送时间,单位为秒。
/// </summary>
public double SendTime { get; }
/// <summary>
/// 获取回映射到规划轨迹的采样时间,单位为秒。
/// </summary>
public double TrajectoryTime { get; }
/// <summary>
/// 获取生成该发送点时采用的队列采样倍率。
/// </summary>
public double SpeedRatio { get; }
/// <summary>
/// 获取 J519 下发使用的角度制关节目标。
/// </summary>
public IReadOnlyList<double> JointsDegrees { get; }
}
/// <summary>
/// 表示理论触发事件最终绑定到哪个发送点的结果。
/// </summary>
public sealed class FlyshotPreparedTriggerBinding
{
/// <summary>
/// 初始化一条最终触发绑定结果。
/// </summary>
/// <param name="trigger">理论触发事件。</param>
/// <param name="sample">最终绑定的发送点。</param>
/// <param name="sampleIndex">最终绑定到的发送点索引。</param>
/// <param name="foundInWindow">是否在理论搜索窗口内找到该发送点。</param>
public FlyshotPreparedTriggerBinding(
TrajectoryDoEvent trigger,
FlyshotPreparedSample sample,
int sampleIndex,
bool foundInWindow)
{
Trigger = trigger ?? throw new ArgumentNullException(nameof(trigger));
Sample = sample ?? throw new ArgumentNullException(nameof(sample));
SampleIndex = sampleIndex;
FoundInWindow = foundInWindow;
}
/// <summary>
/// 获取理论触发事件。
/// </summary>
public TrajectoryDoEvent Trigger { get; }
/// <summary>
/// 获取最终绑定的发送点。
/// </summary>
public FlyshotPreparedSample Sample { get; }
/// <summary>
/// 获取最终绑定到的发送点索引。
/// </summary>
public int SampleIndex { get; }
/// <summary>
/// 获取是否在理论搜索窗口内完成绑定。
/// </summary>
public bool FoundInWindow { get; }
}

View File

@@ -70,10 +70,15 @@ public sealed class TrajectoryRequest
public enum PlanningMethod
{
/// <summary>
/// Uses the cubic-spline ICSP planner.
/// 使用正式 icsp 后端;当前默认实现为 Rust 原生 ICSP。
/// </summary>
Icsp,
/// <summary>
/// 使用拆分到可选程序集中的 C# ICSP 规划器。
/// </summary>
IcspCSharp,
/// <summary>
/// Uses the adaptive ICSP planner that may insert additional points.
/// </summary>

View File

@@ -22,7 +22,10 @@ public sealed class TrajectoryResult
bool usedCache,
int originalWaypointCount,
int plannedWaypointCount,
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null)
int triggerSampleIndexOffsetCycles = 0,
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null,
FlyshotPreparedExecution? preparedFlyshotExecution = null,
IEnumerable<IReadOnlyList<double>>? denseCartesianTrajectory = null)
{
if (string.IsNullOrWhiteSpace(programName))
{
@@ -44,6 +47,11 @@ public sealed class TrajectoryResult
throw new ArgumentOutOfRangeException(nameof(plannedWaypointCount), "Planned waypoint count must be greater than or equal to the original waypoint count.");
}
if (triggerSampleIndexOffsetCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(triggerSampleIndexOffsetCycles), "Trigger sample index offset cycles must be zero or positive.");
}
ArgumentNullException.ThrowIfNull(shotEvents);
ArgumentNullException.ThrowIfNull(triggerTimeline);
ArgumentNullException.ThrowIfNull(artifacts);
@@ -53,6 +61,7 @@ public sealed class TrajectoryResult
var copiedTriggerTimeline = triggerTimeline.ToArray();
var copiedArtifacts = artifacts.ToArray();
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
var copiedDenseCartesianTrajectory = denseCartesianTrajectory?.Select(static row => row.ToArray()).ToArray();
ProgramName = programName;
Method = method;
@@ -65,7 +74,10 @@ public sealed class TrajectoryResult
UsedCache = usedCache;
OriginalWaypointCount = originalWaypointCount;
PlannedWaypointCount = plannedWaypointCount;
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
DenseJointTrajectory = copiedDenseJointTrajectory;
DenseCartesianTrajectory = copiedDenseCartesianTrajectory;
PreparedFlyshotExecution = preparedFlyshotExecution;
}
/// <summary>
@@ -134,12 +146,31 @@ public sealed class TrajectoryResult
[JsonPropertyName("plannedWaypointCount")]
public int PlannedWaypointCount { get; }
/// <summary>
/// Gets the configured command-sample offset applied after trigger binding picks the best sample.
/// </summary>
[JsonPropertyName("triggerSampleIndexOffsetCycles")]
public int TriggerSampleIndexOffsetCycles { get; }
/// <summary>
/// Gets the dense joint trajectory samples where each row is [time, j1, j2, ...].
/// Null when dense sampling was not performed (e.g. simulation fallback).
/// </summary>
[JsonPropertyName("denseJointTrajectory")]
public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; }
/// <summary>
/// 获取稠密直角坐标轨迹采样点,每行格式为 [time, x, y, z, w, p, r]。
/// Null 表示本结果不是直角流式运动。
/// </summary>
[JsonPropertyName("denseCartesianTrajectory")]
public IReadOnlyList<IReadOnlyList<double>>? DenseCartesianTrajectory { get; }
/// <summary>
/// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence.
/// </summary>
[JsonIgnore]
public FlyshotPreparedExecution? PreparedFlyshotExecution { get; }
}
/// <summary>
@@ -150,7 +181,13 @@ public sealed class TrajectoryDoEvent
/// <summary>
/// Initializes a validated runtime trigger event.
/// </summary>
public TrajectoryDoEvent(int waypointIndex, double triggerTime, int offsetCycles, int holdCycles, IoAddressGroup addressGroup)
public TrajectoryDoEvent(
int waypointIndex,
double triggerTime,
int offsetCycles,
int holdCycles,
IoAddressGroup addressGroup,
IEnumerable<double>? referenceJointsDegrees = null)
{
if (waypointIndex < 0)
{
@@ -172,6 +209,7 @@ public sealed class TrajectoryDoEvent
OffsetCycles = offsetCycles;
HoldCycles = holdCycles;
AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup));
ReferenceJointsDegrees = referenceJointsDegrees?.ToArray();
}
/// <summary>
@@ -203,6 +241,13 @@ public sealed class TrajectoryDoEvent
/// </summary>
[JsonPropertyName("addressGroup")]
public IoAddressGroup AddressGroup { get; }
/// <summary>
/// Gets the teach waypoint joints converted to degrees.
/// This is used to select the closest real send sample inside the trigger time window.
/// </summary>
[JsonPropertyName("referenceJointsDegrees")]
public IReadOnlyList<double>? ReferenceJointsDegrees { get; }
}
/// <summary>

View File

@@ -1,6 +1,7 @@
using System.Text;
using System.Text.Json;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Sampling;
namespace Flyshot.Core.Planning.Export;
@@ -52,13 +53,51 @@ public static class TrajectoryExporter
/// </summary>
public static void WriteShotEvents(string path, IReadOnlyList<ShotEvent> events)
{
WriteShotEvents(path, events, bindings: null);
}
/// <summary>
/// 导出触发事件到 JSON 文件,并可选附带最终绑定到的实际 sample 信息。
/// </summary>
public static void WriteShotEvents(
string path,
IReadOnlyList<ShotEvent> events,
IReadOnlyList<TriggerSampleBinding>? bindings)
{
var bindingByWaypointIndex = bindings?
.GroupBy(static binding => binding.Trigger.WaypointIndex)
.ToDictionary(static group => group.Key, static group => group.First());
var payload = events.Select(e => new
{
waypoint_index = e.WaypointIndex,
trigger_time = Math.Round(e.TriggerTime, 6),
sample_index = e.SampleIndex,
sample_time = Math.Round(e.SampleTime, 6),
addrs = e.AddressGroup.Addresses.ToList()
addrs = e.AddressGroup.Addresses.ToList(),
trigger_window_seconds = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out var binding)
? Math.Round(TriggerSampleBinder.TriggerBindingToleranceSeconds, 6)
: (double?)null,
selected_sample_index = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
? binding.SampleIndex
: (int?)null,
selected_send_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
? Math.Round(binding.Sample.SendTime, 6)
: (double?)null,
selected_trajectory_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
? Math.Round(binding.Sample.TrajectoryTime, 6)
: (double?)null,
io_mask = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
? ComputeIoValue(binding.Trigger.AddressGroup)
: (ushort?)null,
reference_joints_deg = bindingByWaypointIndex is not null
&& bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
&& binding.Trigger.ReferenceJointsDegrees is { Count: > 0 }
? binding.Trigger.ReferenceJointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
: null,
trigger_joints_deg = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
? binding.Sample.JointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
: null
}).ToList();
var json = JsonSerializer.Serialize(payload, new JsonSerializerOptions
@@ -79,4 +118,21 @@ public static class TrajectoryExporter
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
}
/// <summary>
/// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码,和运行时 / saveTrajectory 导出保持一致。
/// </summary>
private static ushort ComputeIoValue(IoAddressGroup group)
{
ushort value = 0;
foreach (var addr in group.Addresses)
{
if (addr is >= 1 and <= 16)
{
value |= (ushort)(1 << (addr - 1));
}
}
return value;
}
}

View File

@@ -0,0 +1,27 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning;
/// <summary>
/// 定义可由兼容层按 method 字符串分派的轨迹规划后端。
/// </summary>
public interface IPlanningBackend
{
/// <summary>
/// 获取外部 HTTP/ControllerClient method 参数使用的准确方法名。
/// </summary>
string MethodName { get; }
/// <summary>
/// 获取该后端成功规划后写入结果对象的领域规划方法。
/// </summary>
PlanningMethod ResultMethod { get; }
/// <summary>
/// 执行轨迹规划。
/// </summary>
/// <param name="request">包含机器人约束、轨迹程序和调用标志的规划请求。</param>
/// <param name="context">兼容层传入的规划上下文参数。</param>
/// <returns>规划后的轨迹时间轴、路点和收敛信息。</returns>
PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context);
}

View File

@@ -0,0 +1,21 @@
namespace Flyshot.Core.Planning;
/// <summary>
/// 表示规划后端执行时由兼容层补充的运行参数。
/// </summary>
public sealed class PlanningBackendContext
{
/// <summary>
/// 初始化规划后端上下文。
/// </summary>
/// <param name="adaptIcspTryNum">self-adapt-icsp 允许的最大补点重试次数。</param>
public PlanningBackendContext(int adaptIcspTryNum = 5)
{
AdaptIcspTryNum = adaptIcspTryNum;
}
/// <summary>
/// 获取 self-adapt-icsp 允许的最大补点重试次数。
/// </summary>
public int AdaptIcspTryNum { get; }
}

View File

@@ -0,0 +1,147 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列,并执行发送口径的离散限幅校验。
/// </summary>
public static class FlyshotExecutionSendSequenceBuilder
{
/// <summary>
/// 根据连续规划轨迹构建最终发送队列;请求 speedRatio 只作为诊断信息保留。
/// </summary>
/// <param name="robot">机器人关节限值配置。</param>
/// <param name="plannedTrajectory">规划阶段产出的连续轨迹。</param>
/// <param name="result">规划结果。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
/// <param name="requestedSpeedRatio">请求的执行倍率,必须已经在规划阶段折算进轨迹。</param>
/// <param name="samplingSpeedRatio">最终队列采样倍率;为空时使用 1.0,表示直接采样已调速后的规划轨迹。</param>
/// <returns>通过离散校验后的飞拍最终发送结果。</returns>
public static FlyshotPreparedExecution Build(
RobotProfile robot,
PlannedTrajectory plannedTrajectory,
TrajectoryResult result,
double servoPeriodSeconds,
double requestedSpeedRatio,
double? samplingSpeedRatio = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(plannedTrajectory);
ArgumentNullException.ThrowIfNull(result);
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。");
}
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
{
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。");
}
// 未显式指定独立采样倍率时,最终发送队列必须原样使用请求倍率。
var finalSpeedRatio = samplingSpeedRatio ?? requestedSpeedRatio;
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。");
}
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
plannedTrajectory,
servoPeriodSeconds,
finalSpeedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName,
validateJerk: true,
strictJerkTolerance: true);
}
catch (InvalidOperationException exception)
{
throw new InvalidOperationException(
$"飞拍最终发送队列在请求 speed_ratio={requestedSpeedRatio:F6} 下离散限幅校验失败。" +
"请降低 planning_speed_scale 或 speed_ratio 后重新规划;执行侧不会在发送阶段临场重采样。",
exception);
}
return BuildPreparedExecution(
result,
samples,
requestedSpeedRatio,
finalSpeedRatio,
stretchIterationCount: 0);
}
/// <summary>
/// 将通过校验的 J519 采样点和触发绑定结果封装为稳定的飞拍执行结果。
/// </summary>
private static FlyshotPreparedExecution BuildPreparedExecution(
TrajectoryResult result,
IReadOnlyList<J519SendSample> samples,
double requestedSpeedRatio,
double finalSpeedRatio,
int stretchIterationCount)
{
var preparedSamples = samples.Select(static sample =>
new FlyshotPreparedSample(
sample.SampleIndex,
sample.SendTime,
sample.TrajectoryTime,
sample.SpeedRatio,
sample.JointsDegrees)).ToArray();
var sampleLookup = preparedSamples.ToDictionary(static sample => sample.SampleIndex);
var triggerBindings = TriggerSampleBinder.Bind(
result.TriggerTimeline,
samples,
result.TriggerSampleIndexOffsetCycles)
.Select(binding =>
new FlyshotPreparedTriggerBinding(
binding.Trigger,
sampleLookup[binding.Sample.SampleIndex],
binding.SampleIndex,
binding.FoundInWindow))
.ToArray();
var timingRows = samples
.Select(sample => J519SendTrajectorySampler.BuildTimingRow(sample, requestedSpeedRatio, stretchIterationCount))
.ToArray();
var jerkRows = BuildJerkRows(samples);
var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;
return new FlyshotPreparedExecution(
preparedSamples,
triggerBindings,
timingRows,
jerkRows,
requestedSpeedRatio,
finalSpeedRatio,
finalDurationSeconds,
stretchIterationCount);
}
/// <summary>
/// 为最终发送点列构建跃度诊断行,保证导出工件和运行时发送复用同一份数据。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> BuildJerkRows(IReadOnlyList<J519SendSample> samples)
{
var jerkRows = new List<IReadOnlyList<double>>(Math.Max(0, samples.Count - 1));
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
for (var index = 1; index < samples.Count; index++)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
samples[index - 1].SendTime,
samples[index].SendTime,
samples[index - 1].JointsDegrees,
samples[index].JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
}
return jerkRows;
}
}

View File

@@ -1,242 +1,352 @@
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责把规划层稠密关节轨迹重采样为 J519 物理发送周期上的角度制目标。
/// <para>
/// 算法约定:
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN],time 为规划轨迹时间,关节单位为弧度;
/// 输出的 J519 采样点按物理伺服周期排列,关节单位转换为角度制,供 UDP 60015 实时下发和离线 ActualSend 文件共用。
/// </para>
/// <para>
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算:
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
/// 末尾额外保留一个终点钳制周期,确保轨迹时长不是周期整数倍时仍会输出最终点。
/// </para>
/// <para>
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod;
/// speedRatio 不改变物理发送周期,只用于把发送时间映射回规划轨迹时间:
/// trajectoryTime = min(sendTime * speedRatio, duration)。
/// 之后在原始稠密关节轨迹上按 trajectoryTime 做线性插值,并把每个关节从 rad 转为 deg。
/// </para>
/// <para>
/// 诊断行也在这里统一生成:Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio;
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分,依次近似速度、加速度和跃度,格式为
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
/// </para>
/// </summary>
public static class J519SendTrajectorySampler
{
/// <summary>
/// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
/// </summary>
/// <param name="denseJointTrajectory">规划层稠密关节轨迹,每行格式为 [time, j1..jN],关节单位为弧度。</param>
/// <param name="durationSeconds">规划轨迹总时长,单位为秒。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
/// <param name="speedRatio">速度倍率;只缩放轨迹采样时间,不改变物理发送周期。</param>
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double durationSeconds,
double servoPeriodSeconds,
double speedRatio)
{
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
var segmentIndex = 0;
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
{
// J519 物理周期固定,speed_ratio 只用于把发送时间映射回原始轨迹时间。
var sendTime = sampleIndex * servoPeriodSeconds;
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
}
return samples;
}
/// <summary>
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
/// </summary>
/// <param name="durationSeconds">规划轨迹总时长,单位为秒。</param>
/// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长,单位为秒。</param>
/// <returns>包含终点钳制周期的采样点数量。</returns>
public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
{
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
}
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
{
throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
}
// 非周期整数倍时多保留一个终点钳制周期,和真实 J519 下发序列保持一致。
return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
}
/// <summary>
/// 构造实发时间映射文本行,格式为 sample_index + send_time + trajectory_time + speed_ratio。
/// </summary>
/// <param name="sample">待写出的 J519 实发采样点。</param>
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
{
ArgumentNullException.ThrowIfNull(sample);
return
[
sample.SampleIndex,
Math.Round(sample.SendTime, 6),
Math.Round(sample.TrajectoryTime, 6),
Math.Round(sample.SpeedRatio, 6)
];
}
/// <summary>
/// 构造相邻发送点之间的角度制跃度统计行。
/// </summary>
/// <param name="previousTime">上一帧发送时间,单位为秒。</param>
/// <param name="currentTime">当前帧发送时间,单位为秒。</param>
/// <param name="previousJoints">上一帧角度制关节目标。</param>
/// <param name="currentJoints">当前帧角度制关节目标。</param>
/// <param name="previousVelocity">上一帧关节速度,调用后更新为当前帧速度。</param>
/// <param name="previousAcceleration">上一帧关节加速度,调用后更新为当前帧加速度。</param>
/// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
public static IReadOnlyList<double> BuildJerkRow(
double previousTime,
double currentTime,
IReadOnlyList<double> previousJoints,
IReadOnlyList<double> currentJoints,
ref double[]? previousVelocity,
ref double[]? previousAcceleration)
{
ArgumentNullException.ThrowIfNull(previousJoints);
ArgumentNullException.ThrowIfNull(currentJoints);
var dt = currentTime - previousTime;
if (dt <= 0.0)
{
dt = 1e-9;
}
var jointCount = currentJoints.Count;
var currentVelocity = new double[jointCount];
var currentAcceleration = new double[jointCount];
var currentJerk = new double[jointCount];
var maxAbsJerk = 0.0;
for (var index = 0; index < jointCount; index++)
{
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
if (previousVelocity is not null)
{
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
}
if (previousAcceleration is not null)
{
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
}
}
previousVelocity = currentVelocity;
previousAcceleration = currentAcceleration;
var row = new double[jointCount + 4];
row[0] = Math.Round(previousTime, 6);
row[1] = Math.Round(currentTime, 6);
row[2] = Math.Round(dt, 6);
row[3] = Math.Round(maxAbsJerk, 6);
for (var index = 0; index < jointCount; index++)
{
row[index + 4] = Math.Round(currentJerk[index], 6);
}
return row;
}
/// <summary>
/// 在稠密关节轨迹上按时间线性插值,并转换成 J519 下发使用的角度制目标。
/// </summary>
private static double[] SampleDenseJointTrajectoryDegrees(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double trajectoryTime,
ref int segmentIndex)
{
if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
{
return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
}
var lastIndex = denseJointTrajectory.Count - 1;
if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
{
return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
}
while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
{
segmentIndex++;
}
var start = denseJointTrajectory[segmentIndex];
var end = denseJointTrajectory[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 joints = new double[start.Count - 1];
for (var index = 0; index < joints.Length; index++)
{
joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
}
return joints;
}
/// <summary>
/// 校验 J519 实发采样的基础输入,避免错误时间轴进入运行时链路。
/// </summary>
private static void ValidateInputs(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double durationSeconds,
double servoPeriodSeconds,
double speedRatio)
{
if (denseJointTrajectory.Count == 0)
{
throw new InvalidOperationException("稠密关节轨迹为空。");
}
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
}
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
{
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
}
/// <summary>
/// 角度单位转换:rad -> deg。
/// </summary>
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}
using Flyshot.Core.Planning;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责把规划层关节轨迹重采样为 J519 物理发送周期上的角度制目标。
/// <para>
/// 算法约定:
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN],time 为规划轨迹时间,关节单位为弧度;
/// 输出的 J519 采样点按物理伺服周期排列,关节单位转换为角度制,供 UDP 60015 实时下发和离线 ActualSend 文件共用。
/// </para>
/// <para>
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算:
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
/// 末尾额外保留一个终点钳制周期,确保轨迹时长不是周期整数倍时仍会输出最终点。
/// </para>
/// <para>
/// 飞拍最终发送队列优先从 <see cref="PlannedTrajectory"/> 重建连续三次样条并直接取点;
/// 旧稠密点入口仅保留给运行时 fallback 和兼容导出。
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod;
/// speedRatio 不改变物理发送周期,只用于把发送时间映射回规划轨迹时间:
/// trajectoryTime = min(sendTime * speedRatio, duration)。
/// 之后在连续样条或旧稠密轨迹上求取关节,并把每个关节从 rad 转为 deg。
/// </para>
/// <para>
/// 诊断行也在这里统一生成:Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio;
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分,依次近似速度、加速度和跃度,格式为
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
/// </para>
/// </summary>
public static class J519SendTrajectorySampler
{
/// <summary>
/// 根据连续规划样条和 J519 伺服周期生成完整实发采样序列。
/// </summary>
/// <param name="trajectory">规划阶段产出的连续轨迹信息。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
/// <param name="speedRatio">速度倍率;只缩放轨迹采样时间,不改变物理发送周期。</param>
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
public static IReadOnlyList<J519SendSample> SamplePlannedTrajectory(
PlannedTrajectory trajectory,
double servoPeriodSeconds,
double speedRatio)
{
ArgumentNullException.ThrowIfNull(trajectory);
ValidatePlannedTrajectoryInputs(trajectory, servoPeriodSeconds, speedRatio);
var durationSeconds = trajectory.WaypointTimes[^1];
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
var spline = RebuildSpline(trajectory);
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
{
// 直接在连续样条上按 trajectoryTime 取点,避免先离散再线性回采样引入局部跃度尖峰。
var sendTime = sampleIndex * servoPeriodSeconds;
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
var joints = spline.Evaluate(trajectoryTime).Select(RadiansToDegrees).ToArray();
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
}
return samples;
}
/// <summary>
/// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
/// </summary>
/// <param name="denseJointTrajectory">规划层稠密关节轨迹,每行格式为 [time, j1..jN],关节单位为弧度。</param>
/// <param name="durationSeconds">规划轨迹总时长,单位为秒。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
/// <param name="speedRatio">速度倍率;只缩放轨迹采样时间,不改变物理发送周期。</param>
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double durationSeconds,
double servoPeriodSeconds,
double speedRatio)
{
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
var segmentIndex = 0;
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
{
// J519 物理周期固定,speed_ratio 只用于把发送时间映射回原始轨迹时间。
var sendTime = sampleIndex * servoPeriodSeconds;
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
}
return samples;
}
/// <summary>
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
/// </summary>
/// <param name="durationSeconds">规划轨迹总时长,单位为秒。</param>
/// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长,单位为秒。</param>
/// <returns>包含终点钳制周期的采样点数量。</returns>
public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
{
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
}
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
{
throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
}
// 非周期整数倍时多保留一个终点钳制周期,和真实 J519 下发序列保持一致。
return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
}
/// <summary>
/// 构造实发时间映射文本行,格式为 sample_index + send_time + trajectory_time + final_speed_ratio。
/// </summary>
/// <param name="sample">待写出的 J519 实发采样点。</param>
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
{
ArgumentNullException.ThrowIfNull(sample);
return
[
sample.SampleIndex,
Math.Round(sample.SendTime, 6),
Math.Round(sample.TrajectoryTime, 6),
Math.Round(sample.SpeedRatio, 6)
];
}
/// <summary>
/// 构造带倍率诊断信息的实发时间映射文本行。
/// </summary>
/// <param name="sample">待写出的 J519 实发采样点。</param>
/// <param name="requestedSpeedRatio">用户请求的执行倍率。</param>
/// <param name="stretchIterationCount">历史诊断字段;当前执行侧不自动改写 speed_ratio,因此正常为 0。</param>
/// <returns>前 4 列兼容 ActualSendTiming.txt,后续列记录请求倍率和倍率改写次数。</returns>
public static IReadOnlyList<double> BuildTimingRow(
J519SendSample sample,
double requestedSpeedRatio,
int stretchIterationCount)
{
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "请求 speed_ratio 必须是有限正数。");
}
if (stretchIterationCount < 0)
{
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写次数不能为负数。");
}
var legacyRow = BuildTimingRow(sample);
return
[
legacyRow[0],
legacyRow[1],
legacyRow[2],
legacyRow[3],
Math.Round(requestedSpeedRatio, 6),
stretchIterationCount
];
}
/// <summary>
/// 构造相邻发送点之间的角度制跃度统计行。
/// </summary>
/// <param name="previousTime">上一帧发送时间,单位为秒。</param>
/// <param name="currentTime">当前帧发送时间,单位为秒。</param>
/// <param name="previousJoints">上一帧角度制关节目标。</param>
/// <param name="currentJoints">当前帧角度制关节目标。</param>
/// <param name="previousVelocity">上一帧关节速度,调用后更新为当前帧速度。</param>
/// <param name="previousAcceleration">上一帧关节加速度,调用后更新为当前帧加速度。</param>
/// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
public static IReadOnlyList<double> BuildJerkRow(
double previousTime,
double currentTime,
IReadOnlyList<double> previousJoints,
IReadOnlyList<double> currentJoints,
ref double[]? previousVelocity,
ref double[]? previousAcceleration)
{
ArgumentNullException.ThrowIfNull(previousJoints);
ArgumentNullException.ThrowIfNull(currentJoints);
var dt = currentTime - previousTime;
if (dt <= 0.0)
{
dt = 1e-9;
}
var jointCount = currentJoints.Count;
var currentVelocity = new double[jointCount];
var currentAcceleration = new double[jointCount];
var currentJerk = new double[jointCount];
var maxAbsJerk = 0.0;
for (var index = 0; index < jointCount; index++)
{
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
if (previousVelocity is not null)
{
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
}
if (previousAcceleration is not null)
{
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
}
}
previousVelocity = currentVelocity;
previousAcceleration = currentAcceleration;
var row = new double[jointCount + 4];
row[0] = Math.Round(previousTime, 6);
row[1] = Math.Round(currentTime, 6);
row[2] = Math.Round(dt, 6);
row[3] = Math.Round(maxAbsJerk, 6);
for (var index = 0; index < jointCount; index++)
{
row[index + 4] = Math.Round(currentJerk[index], 6);
}
return row;
}
/// <summary>
/// 在稠密关节轨迹上按时间线性插值,并转换成 J519 下发使用的角度制目标。
/// </summary>
private static double[] SampleDenseJointTrajectoryDegrees(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double trajectoryTime,
ref int segmentIndex)
{
if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
{
return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
}
var lastIndex = denseJointTrajectory.Count - 1;
if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
{
return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
}
while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
{
segmentIndex++;
}
var start = denseJointTrajectory[segmentIndex];
var end = denseJointTrajectory[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 joints = new double[start.Count - 1];
for (var index = 0; index < joints.Length; index++)
{
joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
}
return joints;
}
/// <summary>
/// 校验 J519 实发采样的基础输入,避免错误时间轴进入运行时链路。
/// </summary>
private static void ValidateInputs(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double durationSeconds,
double servoPeriodSeconds,
double speedRatio)
{
if (denseJointTrajectory.Count == 0)
{
throw new InvalidOperationException("稠密关节轨迹为空。");
}
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
}
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
{
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
}
/// <summary>
/// 校验连续规划轨迹采样入口的基础输入,避免异常时间轴进入最终发送队列。
/// </summary>
private static void ValidatePlannedTrajectoryInputs(
PlannedTrajectory trajectory,
double servoPeriodSeconds,
double speedRatio)
{
if (trajectory.WaypointTimes.Count < 2)
{
throw new InvalidOperationException("规划轨迹至少需要两个路点才能重建连续样条。");
}
if (trajectory.WaypointTimes[^1] < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(trajectory), "规划轨迹总时长不能为负数。");
}
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
{
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
}
/// <summary>
/// 从规划轨迹重建连续三次样条,供飞拍最终发送队列直接采样。
/// </summary>
private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
{
var times = trajectory.WaypointTimes.ToArray();
var values = trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray();
return new CubicSplineInterpolator(times, values);
}
/// <summary>
/// 角度单位转换:rad -> deg。
/// </summary>
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}

View File

@@ -19,16 +19,18 @@ public static class TrajectoryLimitValidator
/// <param name="rows">稠密轨迹行,格式为 time + 关节弧度。</param>
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
/// <param name="trajectoryName">诊断用轨迹名称。</param>
/// <param name="validateJerk">是否校验离散 jerk;飞拍链路可临时关闭,仅保留速度/加速度约束。</param>
public static void ValidateDenseJointTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> rows,
double toleranceMultiplier = DefaultLimitTolerance,
string? trajectoryName = null)
string? trajectoryName = null,
bool validateJerk = true)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(rows);
ValidateTolerance(toleranceMultiplier);
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory");
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory", validateJerk);
}
/// <summary>
@@ -38,11 +40,15 @@ public static class TrajectoryLimitValidator
/// <param name="samples">J519 发送采样点,关节单位为角度。</param>
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
/// <param name="trajectoryName">诊断用轨迹名称。</param>
/// <param name="validateJerk">是否校验离散 jerk;飞拍链路可临时关闭,仅保留速度/加速度约束。</param>
/// <param name="strictJerkTolerance">是否按原始 jerk 限值做硬约束;false 时保留历史宽容口径。</param>
public static void ValidateJ519SendSamples(
RobotProfile robot,
IReadOnlyList<J519SendSample> samples,
double toleranceMultiplier = DefaultLimitTolerance,
string? trajectoryName = null)
string? trajectoryName = null,
bool validateJerk = false,
bool strictJerkTolerance = true)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(samples);
@@ -61,7 +67,13 @@ public static class TrajectoryLimitValidator
rows.Add(row);
}
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory");
ValidateRows(
robot,
rows,
toleranceMultiplier,
trajectoryName ?? "j519-send-trajectory",
validateJerk,
strictJerkTolerance);
}
/// <summary>
@@ -82,7 +94,9 @@ public static class TrajectoryLimitValidator
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> rows,
double toleranceMultiplier,
string trajectoryName)
string trajectoryName,
bool validateJerk,
bool strictJerkTolerance = false)
{
double? previousTime = null;
double[]? previousPositions = null;
@@ -145,7 +159,7 @@ public static class TrajectoryLimitValidator
jointLimit.AccelerationLimit,
toleranceMultiplier);
if (previousAccelerations is not null)
if (validateJerk && previousAccelerations is not null)
{
var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
ThrowIfExceeded(
@@ -157,7 +171,7 @@ public static class TrajectoryLimitValidator
"Jerk",
jerk,
jointLimit.JerkLimit,
toleranceMultiplier*4);
strictJerkTolerance ? toleranceMultiplier : toleranceMultiplier * 4.0);
}
}

View File

@@ -0,0 +1,134 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责把理论触发事件绑定到实际 J519 发送 sample。
/// 绑定规则为:先在理论触发时间前后固定时间窗内筛候选点,再优先选择关节差最小的 sample。
/// </summary>
public static class TriggerSampleBinder
{
/// <summary>
/// 当前触发绑定使用的固定近时窗半宽,单位为秒。
/// 即:以理论触发时间为中心,在前后各 100ms 的范围内寻找最优 sample。
/// </summary>
public const double TriggerBindingToleranceSeconds = 0.1;
/// <summary>
/// 把一组触发事件绑定到采样后的 J519 sample。
/// </summary>
/// <param name="triggers">待绑定的触发事件。</param>
/// <param name="samples">J519 实发 sample。</param>
/// <param name="sampleIndexOffsetCycles">在最佳绑定 sample 基础上继续向后偏移的命令周期数。</param>
/// <returns>按最终 sampleIndex 排序的绑定结果。</returns>
public static IReadOnlyList<TriggerSampleBinding> Bind(
IReadOnlyList<TrajectoryDoEvent> triggers,
IReadOnlyList<J519SendSample> samples,
int sampleIndexOffsetCycles = 0)
{
ArgumentNullException.ThrowIfNull(triggers);
ArgumentNullException.ThrowIfNull(samples);
if (sampleIndexOffsetCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(sampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
}
var bindings = new List<TriggerSampleBinding>(triggers.Count);
if (triggers.Count == 0 || samples.Count == 0)
{
return bindings;
}
var lastSampleIndex = samples.Count - 1;
foreach (var trigger in triggers)
{
var fallbackIndex = FindClosestTrajectoryTimeSampleIndex(samples, trigger.TriggerTime);
var candidateStart = Math.Max(0, fallbackIndex - 1);
var candidateEnd = Math.Min(lastSampleIndex, fallbackIndex + 1);
var bestIndex = fallbackIndex;
var bestScore = double.PositiveInfinity;
var foundInWindow = false;
for (var sampleIndex = candidateStart; sampleIndex <= candidateEnd; sampleIndex++)
{
var sample = samples[sampleIndex];
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) > TriggerBindingToleranceSeconds)
{
continue;
}
foundInWindow = true;
var score = trigger.ReferenceJointsDegrees is { Count: > 0 }
? ComputeJointDifferenceScore(trigger.ReferenceJointsDegrees, sample.JointsDegrees)
: Math.Abs(sample.TrajectoryTime - trigger.TriggerTime);
if (score < bestScore)
{
bestScore = score;
bestIndex = sampleIndex;
}
}
var finalIndex = Math.Min(lastSampleIndex, bestIndex + sampleIndexOffsetCycles);
bindings.Add(new TriggerSampleBinding(trigger, samples[finalIndex], finalIndex, foundInWindow));
}
return bindings
.OrderBy(static binding => binding.SampleIndex)
.ThenBy(static binding => binding.Trigger.WaypointIndex)
.ToArray();
}
/// <summary>
/// 按轨迹时间找到最接近理论触发时刻的 sample。
/// </summary>
public static int FindClosestTrajectoryTimeSampleIndex(IReadOnlyList<J519SendSample> samples, double triggerTime)
{
ArgumentNullException.ThrowIfNull(samples);
var bestIndex = 0;
var bestDelta = double.PositiveInfinity;
for (var sampleIndex = 0; sampleIndex < samples.Count; sampleIndex++)
{
var delta = Math.Abs(samples[sampleIndex].TrajectoryTime - triggerTime);
if (delta < bestDelta)
{
bestDelta = delta;
bestIndex = sampleIndex;
}
}
return bestIndex;
}
/// <summary>
/// 计算参考示教点与候选 sample 的关节差评分,使用平方和即可稳定排序。
/// </summary>
public static double ComputeJointDifferenceScore(
IReadOnlyList<double>? referenceJointsDegrees,
IReadOnlyList<double> sampleJointsDegrees)
{
if (referenceJointsDegrees is null || referenceJointsDegrees.Count == 0)
{
return 0.0;
}
var jointCount = Math.Min(referenceJointsDegrees.Count, sampleJointsDegrees.Count);
var score = 0.0;
for (var index = 0; index < jointCount; index++)
{
var delta = sampleJointsDegrees[index] - referenceJointsDegrees[index];
score += delta * delta;
}
return score;
}
}
/// <summary>
/// 表示一个理论触发事件最终绑定到哪个 J519 sample 的结果。
/// </summary>
public sealed record TriggerSampleBinding(
TrajectoryDoEvent Trigger,
J519SendSample Sample,
int SampleIndex,
bool FoundInWindow);

View File

@@ -8,9 +8,22 @@ namespace Flyshot.Core.Triggering;
/// 根据规划轨迹和飞拍配置生成触发时间轴,把示教点上的 shot_flags / offset_values / addr
/// 映射成带理论时间和离散化时间的 ShotEvent,以及可直接注入伺服流的 TrajectoryDoEvent。
/// </summary>
/// <remarks>
/// 本构建器只负责“时间轴映射”和“事件打包”,不负责轨迹规划与运行时发送。
/// 其中:
/// - ShotEvent 用于诊断、导出和日志观察;
/// - TrajectoryDoEvent 用于运行时伺服链路注入 DO 脉冲。
/// </remarks>
public sealed class ShotTimelineBuilder
{
/// <summary>
/// 将原始示教点索引映射到规划后时间轴上的时间戳解析器。
/// </summary>
private readonly WaypointTimestampResolver _resolver;
/// <summary>
/// 可选日志记录器,用于输出时间轴构建统计信息。
/// </summary>
private readonly ILogger<ShotTimelineBuilder>? _logger;
/// <summary>
@@ -32,6 +45,17 @@ public sealed class ShotTimelineBuilder
/// <param name="samplePeriod">稠密采样周期,用于离散化 sample_index 和 sample_time。</param>
/// <param name="useDo">是否生成可注入伺服流的 DO 事件。</param>
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
/// <exception cref="ArgumentNullException">当 <paramref name="trajectory"/> 为 null 时抛出。</exception>
/// <exception cref="ArgumentOutOfRangeException">
/// 当 <paramref name="holdCycles"/> 小于 0,或 <paramref name="samplePeriod"/> 小于等于 0 时抛出。
/// </exception>
/// <remarks>
/// 时间计算规则:
/// - 理论触发时间:timestamp[i] + offset_values[i] * trigger_period;
/// - 离散采样索引:Round(trigger_time / sample_period);
/// - 离散采样时间:sample_index * sample_period。
/// 该离散化与运行时 8ms 周期下发模型一致,可用于对齐导出与诊断。
/// </remarks>
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod, bool useDo = true)
{
ArgumentNullException.ThrowIfNull(trajectory);
@@ -55,6 +79,7 @@ public sealed class ShotTimelineBuilder
var shotEvents = new List<ShotEvent>();
var triggerTimeline = new List<TrajectoryDoEvent>();
// 遍历原始示教点,仅对 shot_flag=true 的点生成触发事件。
for (int i = 0; i < program.Waypoints.Count; i++)
{
if (!program.ShotFlags[i])
@@ -62,6 +87,7 @@ public sealed class ShotTimelineBuilder
continue;
}
// 先得到连续时间上的理论触发时刻,再映射到离散采样周期。
double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds;
int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds);
double sampleTime = sampleIndex * samplePeriodSeconds;
@@ -83,7 +109,8 @@ public sealed class ShotTimelineBuilder
triggerTime: triggerTime,
offsetCycles: program.OffsetValues[i],
holdCycles: holdCycles,
addressGroup: addressGroup));
addressGroup: addressGroup,
referenceJointsDegrees: program.Waypoints[i].Positions.Select(static radians => radians * 180.0 / Math.PI)));
}
}

View File

@@ -0,0 +1,15 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1,77 @@
using System.Runtime.InteropServices;
namespace Flyshot.Native.Planning;
/// <summary>
/// 与 Rust <c>FlyshotIcspOptions</c> 保持逐字段一致的 C ABI 选项结构。
/// </summary>
[StructLayout(LayoutKind.Sequential)]
internal struct NativeIcspOptions
{
/// <summary>
/// 收敛指标停止阈值。
/// </summary>
internal double Threshold;
/// <summary>
/// 最大迭代索引。
/// </summary>
internal int MaxIterations;
/// <summary>
/// 非零表示启用最终 scale 检查。
/// </summary>
internal int EnforceFinalScale;
/// <summary>
/// 最终 scale 允许的非负容差。
/// </summary>
internal double FinalScaleTolerance;
}
/// <summary>
/// 与 Rust <c>FlyshotIcspResult</c> 保持逐字段一致的 C ABI 结果结构。
/// </summary>
[StructLayout(LayoutKind.Sequential)]
internal struct NativeIcspResult
{
/// <summary>
/// 路点绝对时间连续数组指针。
/// </summary>
internal nint WaypointTimes;
/// <summary>
/// 路点绝对时间数组长度。
/// </summary>
internal int WaypointTimesLength;
/// <summary>
/// 分段时长连续数组指针。
/// </summary>
internal nint SegmentDurations;
/// <summary>
/// 分段时长数组长度。
/// </summary>
internal int SegmentDurationsLength;
/// <summary>
/// 分段缩放连续数组指针。
/// </summary>
internal nint SegmentScales;
/// <summary>
/// 分段缩放数组长度。
/// </summary>
internal int SegmentScalesLength;
/// <summary>
/// 历史最优结果对应的迭代轮数。
/// </summary>
internal int Iterations;
/// <summary>
/// 历史最小收敛指标。
/// </summary>
internal double Threshold;
}

View File

@@ -0,0 +1,130 @@
using System.Runtime.InteropServices;
namespace Flyshot.Native.Planning;
/// <summary>
/// 声明 Rust 原生规划库导出的稳定 C ABI 方法。
/// </summary>
internal static partial class NativeIcspMethods
{
/// <summary>
/// 保护原生库句柄首次初始化,避免并发规划重复加载。
/// </summary>
private static readonly object LibraryLoadLock = new();
/// <summary>
/// 保存已加载原生库句柄,使 P/Invoke 生命周期覆盖整个进程。
/// </summary>
private static nint _libraryHandle;
/// <summary>
/// 保存当前进程已绑定到 P/Invoke 逻辑名称的实际库路径。
/// </summary>
private static string? _loadedLibraryPath;
/// <summary>
/// 初始化程序集级 DllImport 解析器,把逻辑库名绑定到显式加载句柄。
/// </summary>
static NativeIcspMethods()
{
NativeLibrary.SetDllImportResolver(
typeof(NativeIcspMethods).Assembly,
ResolveImportedLibrary);
}
/// <summary>
/// 使用指定解析器确保原生规划库已加载。
/// </summary>
/// <param name="resolver">负责生成平台路径并加载原生库的解析器。</param>
internal static void EnsureLibraryLoaded(NativePlanningLibraryResolver resolver)
{
ArgumentNullException.ThrowIfNull(resolver);
string requestedPath = resolver.ResolveLibraryPath();
lock (LibraryLoadLock)
{
if (_libraryHandle != nint.Zero)
{
StringComparison comparison = OperatingSystem.IsWindows()
? StringComparison.OrdinalIgnoreCase
: StringComparison.Ordinal;
if (!string.Equals(_loadedLibraryPath, requestedPath, comparison))
{
throw new NativePlanningException(
$"Rust 原生规划库已绑定到“{_loadedLibraryPath}”,不能在同一进程中改为“{requestedPath}”。",
errorCode: -1008);
}
return;
}
_libraryHandle = resolver.LoadLibrary();
_loadedLibraryPath = requestedPath;
}
}
/// <summary>
/// 调用 Rust ICSP 入口并返回错误码和结果指针。
/// </summary>
/// <param name="waypoints">行优先连续路点数组。</param>
/// <param name="waypointCount">路点数量。</param>
/// <param name="degreesOfFreedom">每个路点的关节自由度。</param>
/// <param name="velocityLimits">逐关节速度上限。</param>
/// <param name="accelerationLimits">逐关节加速度上限。</param>
/// <param name="jerkLimits">逐关节 jerk 上限。</param>
/// <param name="options">与 Rust C ABI 对齐的规划选项。</param>
/// <param name="result">接收 Rust 分配的结果指针。</param>
/// <returns>零表示成功,非零表示固定类别的原生错误。</returns>
[LibraryImport(
NativePlanningLibraryResolver.LogicalLibraryName,
EntryPoint = "flyshot_icsp_plan")]
internal static unsafe partial int FlyshotIcspPlan(
double* waypoints,
int waypointCount,
int degreesOfFreedom,
double* velocityLimits,
double* accelerationLimits,
double* jerkLimits,
NativeIcspOptions* options,
out nint result);
/// <summary>
/// 释放由 Rust 分配的 ICSP 结果对象。
/// </summary>
/// <param name="result">Rust 返回的结果对象指针;允许为空指针。</param>
[LibraryImport(
NativePlanningLibraryResolver.LogicalLibraryName,
EntryPoint = "flyshot_icsp_result_free")]
internal static partial void FlyshotIcspResultFree(nint result);
/// <summary>
/// 获取当前线程最近一次 Rust 调用保存的 UTF-8 错误文本。
/// </summary>
/// <returns>指向线程局部 C 字符串的只读指针。</returns>
[LibraryImport(
NativePlanningLibraryResolver.LogicalLibraryName,
EntryPoint = "flyshot_last_error_message")]
internal static partial nint FlyshotLastErrorMessage();
/// <summary>
/// 将固定逻辑库名解析为已经显式加载的系统句柄。
/// </summary>
/// <param name="libraryName">P/Invoke 请求的逻辑库名。</param>
/// <param name="assembly">发起请求的托管程序集。</param>
/// <param name="searchPath">调用方提供的可选搜索路径。</param>
/// <returns>逻辑名称匹配时返回已加载句柄,否则返回零交给系统继续解析。</returns>
private static nint ResolveImportedLibrary(
string libraryName,
System.Reflection.Assembly assembly,
DllImportSearchPath? searchPath)
{
_ = assembly;
_ = searchPath;
return string.Equals(
libraryName,
NativePlanningLibraryResolver.LogicalLibraryName,
StringComparison.Ordinal)
? _libraryHandle
: nint.Zero;
}
}

View File

@@ -0,0 +1,24 @@
namespace Flyshot.Native.Planning;
/// <summary>
/// 描述 Rust ICSP 规划器使用的收敛和最终约束选项。
/// </summary>
/// <param name="Threshold">收敛指标 <c>sum(abs(scale - 1))</c> 的停止阈值。</param>
/// <param name="MaxIterations">最大迭代索引;执行范围包含该索引对应轮次。</param>
/// <param name="EnforceFinalScale">是否拒绝历史最优结果中仍然超限的段。</param>
/// <param name="FinalScaleTolerance">最终段缩放因子相对 1.0 的非负容差。</param>
public sealed record NativeIcspPlannerOptions(
double Threshold,
int MaxIterations,
bool EnforceFinalScale,
double FinalScaleTolerance)
{
/// <summary>
/// 获取与现有托管 <c>ICspPlanner</c> 一致的默认选项。
/// </summary>
public static NativeIcspPlannerOptions Default { get; } = new(
Threshold: 1e-3,
MaxIterations: 1000,
EnforceFinalScale: true,
FinalScaleTolerance: 1e-2);
}

View File

@@ -0,0 +1,37 @@
using Microsoft.Win32.SafeHandles;
namespace Flyshot.Native.Planning;
/// <summary>
/// 封装 Rust 分配的 ICSP 结果对象,确保托管异常路径也会调用原生释放函数。
/// </summary>
internal sealed class NativeIcspSafeHandle : SafeHandleZeroOrMinusOneIsInvalid
{
/// <summary>
/// 初始化一个尚未持有原生结果的安全句柄。
/// </summary>
public NativeIcspSafeHandle()
: base(ownsHandle: true)
{
}
/// <summary>
/// 初始化并接管 Rust 返回的非零结果指针。
/// </summary>
/// <param name="result">由 <c>flyshot_icsp_plan</c> 分配的结果指针。</param>
internal NativeIcspSafeHandle(nint result)
: this()
{
SetHandle(result);
}
/// <summary>
/// 调用 Rust 导出的释放函数归还结果及其连续数组内存。
/// </summary>
/// <returns>释放函数调用完成后返回 <see langword="true"/>。</returns>
protected override bool ReleaseHandle()
{
NativeIcspMethods.FlyshotIcspResultFree(handle);
return true;
}
}

View File

@@ -0,0 +1,24 @@
namespace Flyshot.Native.Planning;
/// <summary>
/// 表示原生规划库加载、调用或数值计算失败。
/// </summary>
public sealed class NativePlanningException : Exception
{
/// <summary>
/// 初始化包含原生错误码和底层异常的规划异常。
/// </summary>
/// <param name="message">供调用方诊断的错误说明。</param>
/// <param name="errorCode">Rust C ABI 返回的错误码;托管加载错误使用负数。</param>
/// <param name="innerException">触发本异常的底层异常;没有时为 <see langword="null"/>。</param>
public NativePlanningException(string message, int errorCode, Exception? innerException = null)
: base(message, innerException)
{
ErrorCodeValue = errorCode;
}
/// <summary>
/// 获取 Rust C ABI 错误码或托管互操作错误码。
/// </summary>
public int ErrorCodeValue { get; }
}

View File

@@ -0,0 +1,104 @@
using System.Runtime.InteropServices;
namespace Flyshot.Native.Planning;
/// <summary>
/// 描述当前进程加载原生规划库时使用的平台信息。
/// </summary>
/// <param name="OperatingSystem">当前操作系统名称。</param>
/// <param name="Architecture">当前进程体系结构。</param>
public readonly record struct NativePlanningPlatform(string OperatingSystem, Architecture Architecture);
/// <summary>
/// 按当前操作系统和体系结构解析并加载 Rust 原生规划库。
/// </summary>
public sealed class NativePlanningLibraryResolver
{
/// <summary>
/// 原生规划库的逻辑名称。
/// </summary>
public const string LogicalLibraryName = "flyshot_planning";
private readonly Func<NativePlanningPlatform, string> _resolveLibraryPath;
/// <summary>
/// 初始化使用默认部署目录解析规则的原生库解析器。
/// </summary>
public NativePlanningLibraryResolver()
: this(ResolveDefaultLibraryPath)
{
}
/// <summary>
/// 初始化使用自定义路径解析规则的原生库解析器。
/// </summary>
/// <param name="resolveLibraryPath">根据当前平台返回原生库路径或名称的函数。</param>
public NativePlanningLibraryResolver(Func<NativePlanningPlatform, string> resolveLibraryPath)
{
_resolveLibraryPath = resolveLibraryPath ?? throw new ArgumentNullException(nameof(resolveLibraryPath));
}
/// <summary>
/// 解析当前平台应加载的原生库路径。
/// </summary>
/// <returns>原生库路径或可被系统加载器识别的库名称。</returns>
public string ResolveLibraryPath()
{
string operatingSystem = OperatingSystem.IsWindows()
? "Windows"
: OperatingSystem.IsLinux()
? "Linux"
: "Unsupported";
return _resolveLibraryPath(new NativePlanningPlatform(operatingSystem, RuntimeInformation.ProcessArchitecture));
}
/// <summary>
/// 加载当前平台的原生规划库并返回系统句柄。
/// </summary>
/// <returns>由 <see cref="NativeLibrary"/> 管理的非零库句柄。</returns>
/// <exception cref="NativePlanningException">库缺失、架构不匹配或系统不受支持时抛出。</exception>
public nint LoadLibrary()
{
string path = ResolveLibraryPath();
try
{
return NativeLibrary.Load(path);
}
catch (Exception exception) when (
exception is DllNotFoundException
or BadImageFormatException
or FileLoadException)
{
throw new NativePlanningException(
$"无法加载 Rust 原生规划库“{path}”。请确认库文件存在且与 {RuntimeInformation.ProcessArchitecture} 架构匹配。",
errorCode: -1001,
exception);
}
}
/// <summary>
/// 根据当前平台生成与托管程序集同目录的默认原生库路径。
/// </summary>
/// <param name="platform">当前操作系统和进程体系结构。</param>
/// <returns>默认原生库绝对路径。</returns>
private static string ResolveDefaultLibraryPath(NativePlanningPlatform platform)
{
if (platform.Architecture != Architecture.X64)
{
throw new NativePlanningException(
$"Rust 原生规划库第一阶段仅支持 x64,当前进程架构为 {platform.Architecture}。",
errorCode: -1002);
}
string fileName = platform.OperatingSystem switch
{
"Windows" => "flyshot_planning.dll",
"Linux" => "libflyshot_planning.so",
_ => throw new NativePlanningException(
$"Rust 原生规划库不支持当前操作系统:{platform.OperatingSystem}。",
errorCode: -1003)
};
return Path.Combine(AppContext.BaseDirectory, fileName);
}
}

View File

@@ -0,0 +1,220 @@
using System.Runtime.InteropServices;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Native.Planning;
/// <summary>
/// 通过稳定 C ABI 调用 Rust ICSP 核心,并把结果转换为现有托管轨迹模型。
/// </summary>
public sealed class RustIcspPlanner
{
private readonly NativeIcspPlannerOptions _options;
private readonly NativePlanningLibraryResolver _libraryResolver;
/// <summary>
/// 初始化 Rust ICSP 规划器。
/// </summary>
/// <param name="options">与托管 ICSP 对齐的收敛及最终约束选项。</param>
/// <param name="libraryResolver">负责解析 Windows/Linux x64 原生库的加载器。</param>
public RustIcspPlanner(
NativeIcspPlannerOptions? options = null,
NativePlanningLibraryResolver? libraryResolver = null)
{
_options = options ?? NativeIcspPlannerOptions.Default;
_libraryResolver = libraryResolver ?? new NativePlanningLibraryResolver();
ValidateOptions(_options);
}
/// <summary>
/// 使用 Rust 原生核心规划给定关节路点。
/// </summary>
/// <param name="request">包含机器人约束和示教路点的规划请求。</param>
/// <returns>与现有托管规划链路兼容的轨迹结果。</returns>
/// <exception cref="NativePlanningException">原生库加载或规划调用失败时抛出。</exception>
public unsafe PlannedTrajectory Plan(TrajectoryRequest request)
{
ArgumentNullException.ThrowIfNull(request);
ValidateRequest(request);
NativeIcspMethods.EnsureLibraryLoaded(_libraryResolver);
int waypointCount = request.Program.Waypoints.Count;
int degreesOfFreedom = request.Robot.DegreesOfFreedom;
double[] waypoints = FlattenWaypoints(request, waypointCount, degreesOfFreedom);
double[] velocityLimits = request.Robot.JointLimits
.Select(limit => limit.VelocityLimit)
.ToArray();
double[] accelerationLimits = request.Robot.JointLimits
.Select(limit => limit.AccelerationLimit)
.ToArray();
double[] jerkLimits = request.Robot.JointLimits
.Select(limit => limit.JerkLimit)
.ToArray();
var nativeOptions = new NativeIcspOptions
{
Threshold = _options.Threshold,
MaxIterations = _options.MaxIterations,
EnforceFinalScale = _options.EnforceFinalScale ? 1 : 0,
FinalScaleTolerance = _options.FinalScaleTolerance
};
fixed (double* waypointsPointer = waypoints)
fixed (double* velocityLimitsPointer = velocityLimits)
fixed (double* accelerationLimitsPointer = accelerationLimits)
fixed (double* jerkLimitsPointer = jerkLimits)
{
int errorCode = NativeIcspMethods.FlyshotIcspPlan(
waypointsPointer,
waypointCount,
degreesOfFreedom,
velocityLimitsPointer,
accelerationLimitsPointer,
jerkLimitsPointer,
&nativeOptions,
out nint resultPointer);
if (errorCode != 0)
{
string errorMessage = ReadLastErrorMessage();
throw new NativePlanningException(
$"Rust ICSP 规划失败,错误码={errorCode}:{errorMessage}",
errorCode);
}
if (resultPointer == nint.Zero)
{
throw new NativePlanningException(
"Rust ICSP 返回成功,但结果指针为空。",
errorCode: -1005);
}
using var resultHandle = new NativeIcspSafeHandle(resultPointer);
NativeIcspResult nativeResult = Marshal.PtrToStructure<NativeIcspResult>(resultPointer);
double[] waypointTimes = CopyNativeArray(
nativeResult.WaypointTimes,
nativeResult.WaypointTimesLength,
"waypoint_times");
double[] segmentDurations = CopyNativeArray(
nativeResult.SegmentDurations,
nativeResult.SegmentDurationsLength,
"segment_durations");
double[] segmentScales = CopyNativeArray(
nativeResult.SegmentScales,
nativeResult.SegmentScalesLength,
"segment_scales");
return new PlannedTrajectory(
robot: request.Robot,
originalProgram: request.Program,
plannedWaypoints: request.Program.Waypoints,
waypointTimes: waypointTimes,
segmentDurations: segmentDurations,
segmentScales: segmentScales,
method: PlanningMethod.Icsp,
iterations: nativeResult.Iterations,
threshold: nativeResult.Threshold);
}
}
/// <summary>
/// 校验托管规划选项,防止非法数值穿过 C ABI。
/// </summary>
/// <param name="options">待校验的原生规划选项。</param>
private static void ValidateOptions(NativeIcspPlannerOptions options)
{
if (!double.IsFinite(options.Threshold) || options.Threshold <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(options), "收敛阈值必须为有限正数。");
}
if (options.MaxIterations < 0)
{
throw new ArgumentOutOfRangeException(nameof(options), "最大迭代次数不能为负数。");
}
if (!double.IsFinite(options.FinalScaleTolerance) || options.FinalScaleTolerance < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(options), "最终 scale 容差必须为有限非负数。");
}
}
/// <summary>
/// 校验规划请求与机器人自由度,确保扁平数组布局可安全建立。
/// </summary>
/// <param name="request">待传入 Rust 的规划请求。</param>
private static void ValidateRequest(TrajectoryRequest request)
{
if (request.Program.Waypoints.Count < 4)
{
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
}
int degreesOfFreedom = request.Robot.DegreesOfFreedom;
if (request.Program.Waypoints.Any(waypoint => waypoint.Positions.Count != degreesOfFreedom))
{
throw new ArgumentException("所有路点维度必须与机器人自由度一致。", nameof(request));
}
}
/// <summary>
/// 将托管路点按行优先顺序复制为连续数组。
/// </summary>
/// <param name="request">包含路点的规划请求。</param>
/// <param name="waypointCount">已校验的路点数量。</param>
/// <param name="degreesOfFreedom">已校验的机器人自由度。</param>
/// <returns>长度为路点数量乘自由度的连续数组。</returns>
private static double[] FlattenWaypoints(
TrajectoryRequest request,
int waypointCount,
int degreesOfFreedom)
{
var flattened = new double[checked(waypointCount * degreesOfFreedom)];
for (int waypoint = 0; waypoint < waypointCount; waypoint++)
{
for (int joint = 0; joint < degreesOfFreedom; joint++)
{
flattened[waypoint * degreesOfFreedom + joint] =
request.Program.Waypoints[waypoint].Positions[joint];
}
}
return flattened;
}
/// <summary>
/// 将 Rust 连续数组复制到独立托管数组,避免结果释放后继续引用原生内存。
/// </summary>
/// <param name="pointer">Rust 结果中的连续数组指针。</param>
/// <param name="length">数组元素数量。</param>
/// <param name="fieldName">用于异常诊断的 ABI 字段名。</param>
/// <returns>与原生结果生命周期解耦的托管数组。</returns>
private static double[] CopyNativeArray(nint pointer, int length, string fieldName)
{
if (length < 0)
{
throw new NativePlanningException(
$"Rust ICSP 结果字段 {fieldName} 的长度为负数:{length}。",
errorCode: -1006);
}
if (length > 0 && pointer == nint.Zero)
{
throw new NativePlanningException(
$"Rust ICSP 结果字段 {fieldName} 长度为 {length},但指针为空。",
errorCode: -1007);
}
var values = new double[length];
if (length > 0)
{
Marshal.Copy(pointer, values, 0, length);
}
return values;
}
/// <summary>
/// 读取 Rust 当前线程保存的最近错误文本。
/// </summary>
/// <returns>UTF-8 错误文本;空指针时返回占位诊断。</returns>
private static string ReadLastErrorMessage()
{
nint messagePointer = NativeIcspMethods.FlyshotLastErrorMessage();
return messagePointer == nint.Zero
? "原生库未提供错误文本"
: Marshal.PtrToStringUTF8(messagePointer) ?? "原生错误文本不是有效 UTF-8";
}
}

View File

@@ -0,0 +1,43 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Native.Planning;
/// <summary>
/// 将 Rust ICSP 规划器暴露为默认正式 icsp 后端。
/// </summary>
public sealed class RustIcspPlanningBackend : IPlanningBackend
{
private readonly RustIcspPlanner _planner;
/// <summary>
/// 初始化 Rust ICSP 后端。
/// </summary>
/// <param name="planner">可选的 Rust ICSP 规划器实例;为空时使用默认原生库解析规则。</param>
public RustIcspPlanningBackend(RustIcspPlanner? planner = null)
{
_planner = planner ?? new RustIcspPlanner();
}
/// <summary>
/// 获取外部调用 Rust ICSP 后端时使用的正式方法名。
/// </summary>
public string MethodName => "icsp";
/// <summary>
/// 获取 Rust ICSP 后端写入结果对象的规划方法。
/// </summary>
public PlanningMethod ResultMethod => PlanningMethod.Icsp;
/// <summary>
/// 执行 Rust ICSP 规划。
/// </summary>
/// <param name="request">包含机器人约束和示教路点的规划请求。</param>
/// <param name="context">规划上下文;Rust ICSP 当前不使用该参数。</param>
/// <returns>由 Rust 原生库生成的轨迹结果。</returns>
public PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context)
{
ArgumentNullException.ThrowIfNull(context);
return _planner.Plan(request);
}
}

View File

@@ -14,12 +14,6 @@ public interface IControllerRuntime
/// <param name="robotName">兼容层传入的机器人名称。</param>
void ResetRobot(RobotProfile robot, string robotName);
/// <summary>
/// 选择当前活动控制器类型。
/// </summary>
/// <param name="sim">是否使用仿真控制器。</param>
void SetActiveController(bool sim);
/// <summary>
/// 建立到控制器 IP 的连接。
/// </summary>
@@ -113,4 +107,11 @@ public interface IControllerRuntime
/// <param name="result">规划结果。</param>
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
/// <summary>
/// 执行一条已经完成规划的直角坐标轨迹,并更新最终 TCP 位姿。
/// </summary>
/// <param name="result">规划结果,必须包含稠密直角坐标轨迹。</param>
/// <param name="finalPose">轨迹执行结束后的 [x,y,z,w,p,r] 位姿。</param>
void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose);
}

View File

@@ -1,6 +1,7 @@
using System.Diagnostics;
using System.Text;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc.Protocol;
@@ -11,7 +12,6 @@ namespace Flyshot.Runtime.Fanuc;
/// <summary>
/// FANUC 控制器运行时,将上层兼容层指令转换为三条真实 Socket 通道的交互。
/// 仿真模式下仍保持内存桩行为,便于离线测试与回退。
/// </summary>
public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
@@ -24,7 +24,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
private RobotProfile? _robot;
private string? _robotName;
private bool? _activeControllerIsSimulation;
private string? _connectedRobotIp;
private bool _isEnabled;
private bool _isInMotion;
@@ -87,7 +86,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
DisconnectClients();
_robot = robot;
_robotName = robotName;
_activeControllerIsSimulation = null;
_connectedRobotIp = null;
_isEnabled = false;
_isInMotion = false;
@@ -100,21 +98,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <inheritdoc />
public void SetActiveController(bool sim)
{
_logger?.LogInformation("SetActiveController: sim={Sim}", sim);
lock (_stateLock)
{
EnsureRobotSetup();
DisconnectClients();
_activeControllerIsSimulation = sim;
_connectedRobotIp = null;
_isEnabled = false;
_isInMotion = false;
}
}
/// <inheritdoc />
public void Connect(string robotIp)
{
@@ -123,21 +106,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
}
_logger?.LogInformation("Connect 开始: robotIp={RobotIp}, 仿真={IsSim}", robotIp, _activeControllerIsSimulation);
_logger?.LogInformation("Connect 开始: robotIp={RobotIp}", robotIp);
lock (_stateLock)
{
EnsureActiveControllerSelected();
if (_activeControllerIsSimulation!.Value)
{
_connectedRobotIp = robotIp;
_isEnabled = false;
_isInMotion = false;
_logger?.LogInformation("Connect 完成(仿真): robotIp={RobotIp}", robotIp);
return;
}
// 真机模式:顺序建立三条通道 — 状态 → 命令 → 运动。
EnsureRobotSetup();
// 顺序建立三条真机通道:状态、命令、运动。
_stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult();
_commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult();
_j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult();
@@ -174,7 +148,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
throw new ArgumentOutOfRangeException(nameof(bufferSize), "buffer_size 必须大于 0。");
}
_logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}, 仿真={IsSim}", bufferSize, _activeControllerIsSimulation);
_logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}", bufferSize);
lock (_stateLock)
{
@@ -182,14 +156,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
_bufferSize = bufferSize;
_j519Client.SetSequenceBufferSize(bufferSize);
if (IsSimulationMode)
{
_isEnabled = true;
_logger?.LogInformation("EnableRobot 完成(仿真)");
return;
}
// 真机模式:按 all-reconnect.pcap 的重连序列启动 RVBUSTSM,暂不发送 StopProg。
// 按 all-reconnect.pcap 的重连序列启动 RVBUSTSM。
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
_commandClient.ResetRobotAsync().GetAwaiter().GetResult();
//_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
@@ -210,11 +177,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
EnsureRobotSetup();
CancelSendTaskLocked();
if (!IsSimulationMode)
{
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
}
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
_isEnabled = false;
_isInMotion = false;
@@ -230,10 +194,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
EnsureRobotSetup();
CancelSendTaskLocked();
if (!IsSimulationMode)
{
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
}
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
_isInMotion = false;
}
@@ -246,11 +207,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureConnected();
if (!IsSimulationMode)
{
var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult();
_speedRatio = response.Ratio;
}
var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult();
_speedRatio = response.Ratio;
return _speedRatio;
}
@@ -270,11 +228,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
//EnsureConnected();
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
if (!IsSimulationMode)
{
//_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
}
_speedRatio = clampedRatio;
}
@@ -287,7 +240,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureRobotSetup();
if (_activeControllerIsSimulation is false && !string.IsNullOrWhiteSpace(_connectedRobotIp))
if (!string.IsNullOrWhiteSpace(_connectedRobotIp))
{
var response = _commandClient.GetTcpAsync(1).GetAwaiter().GetResult();
_tcp = response.Pose.Take(3).ToArray();
@@ -303,11 +256,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureRobotSetup();
if (_activeControllerIsSimulation is false)
{
EnsureConnected();
_commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult();
}
EnsureConnected();
_commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult();
_tcp = [x, y, z];
}
@@ -326,14 +276,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureConnected();
if (!IsSimulationMode)
{
var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult();
_ioValues[(normalizedIoType, port)] = response.Value;
return response.Value;
}
return _ioValues.TryGetValue((normalizedIoType, port), out var value) && value;
var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult();
_ioValues[(normalizedIoType, port)] = response.Value;
return response.Value;
}
}
@@ -350,10 +295,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureEnabled();
if (!IsSimulationMode)
{
_commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult();
}
_commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult();
_ioValues[(normalizedIoType, port)] = value;
}
@@ -365,7 +307,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureRobotSetup();
if (!IsSimulationMode)
if (!string.IsNullOrWhiteSpace(_connectedRobotIp))
{
var frame = GetFreshStateFrame();
if (frame?.JointRadians.Count >= _jointPositions.Length)
@@ -384,7 +326,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureRobotSetup();
if (!IsSimulationMode)
if (!string.IsNullOrWhiteSpace(_connectedRobotIp))
{
var frame = GetFreshStateFrame();
if (frame?.CartesianPose.Count >= 6)
@@ -413,7 +355,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
bool? j519SystemReady = null;
bool? j519RobotInMotion = null;
if (!IsSimulationMode)
if (!string.IsNullOrWhiteSpace(_connectedRobotIp))
{
var frame = GetFreshStateFrame();
if (frame is not null)
@@ -484,16 +426,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
EnsureJointCount(finalJointPositions.Count);
CancelSendTaskLocked();
if (!IsSimulationMode && result.DenseJointTrajectory is not null)
if (result.DenseJointTrajectory is not null)
{
if (_speedRatio <= 0.0)
{
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
}
EnsureJ519ReadyForDenseExecution();
// 真机模式且存在稠密路点:准备可被 StopMove 取消的同步发送任务。
// 存在稠密路点时,准备可被 StopMove 取消的同步发送任务。
_isInMotion = true;
_sendCts = new CancellationTokenSource();
denseSendCancellationSource = _sendCts;
@@ -504,14 +441,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
else
{
if (!IsSimulationMode)
{
// 真机模式无稠密路点:回退到单点收敛。
var command = new FanucJ519Command(
sequence: 0,
targetJoints: finalJointPositions.Select(j => (double)j).ToArray());
_j519Client.UpdateCommand(command);
}
// 无稠密路点时回退到单点收敛。
var command = new FanucJ519Command(
sequence: 0,
targetJoints: finalJointPositions.Select(j => (double)j).ToArray());
_j519Client.UpdateCommand(command);
_isInMotion = true;
_jointPositions = finalJointPositions.ToArray();
@@ -542,6 +476,75 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
ArgumentNullException.ThrowIfNull(result);
ArgumentNullException.ThrowIfNull(finalPose);
CancellationToken denseSendCancellationToken = default;
CancellationTokenSource? denseSendCancellationSource = null;
var shouldRunDenseTrajectory = false;
_logger?.LogInformation(
"ExecuteCartesianTrajectory 开始: program={ProgramName}, 时长={Duration}s, 稠密采样={HasDense}, speedRatio={SpeedRatio}",
result.ProgramName, result.Duration.TotalSeconds, result.DenseCartesianTrajectory is not null, _speedRatio);
lock (_stateLock)
{
EnsureEnabled();
EnsureValidTrajectory(result);
EnsurePoseCount(finalPose.Count);
CancelSendTaskLocked();
if (result.DenseCartesianTrajectory is not null)
{
EnsureJ519ReadyForDenseExecution();
// 直角模式同样预装完整队列,由机器人状态包节拍驱动出队。
_isInMotion = true;
_sendCts = new CancellationTokenSource();
denseSendCancellationSource = _sendCts;
denseSendCancellationToken = _sendCts.Token;
shouldRunDenseTrajectory = true;
_logger?.LogInformation("ExecuteCartesianTrajectory 开始同步直角稠密发送任务");
}
else
{
var command = new FanucJ519Command(
sequence: 0,
targetValues: BuildCartesianTargetValues(finalPose),
dataStyle: 0);
_j519Client.UpdateCommand(command);
_isInMotion = true;
_pose = MergeFinalCartesianPose(finalPose);
_isInMotion = false;
_logger?.LogInformation("ExecuteCartesianTrajectory 完成(单点模式)");
}
}
if (shouldRunDenseTrajectory)
{
try
{
SendDenseCartesianTrajectory(result, finalPose, denseSendCancellationToken);
_logger?.LogInformation("ExecuteCartesianTrajectory 完成(稠密模式)");
}
finally
{
lock (_stateLock)
{
if (ReferenceEquals(_sendCts, denseSendCancellationSource))
{
denseSendCancellationSource?.Dispose();
_sendCts = null;
}
}
}
}
}
/// <summary>
/// 释放运行时持有的所有 Socket 客户端。
/// </summary>
@@ -561,21 +564,26 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
/// <summary>
/// 稠密轨迹发送任务:预生成完整 J519 命令队列,并等待机器人状态包按 speed_ratio 推进到执行完成。
/// 稠密轨迹发送任务:预生成完整 J519 命令队列,并等待机器人状态包驱动队列执行完成。
/// </summary>
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
{
var triggers = result.TriggerTimeline;
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
var speedRatio = _speedRatio;
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
var durationSeconds = result.Duration.TotalSeconds;
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
durationSeconds,
servoPeriodSeconds,
speedRatio);
var preparedExecution = result.PreparedFlyshotExecution;
var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds;
var samples = preparedExecution is null
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
durationSeconds,
servoPeriodSeconds,
speedRatio: 1.0)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
sample.TrajectoryTime,
sample.SpeedRatio,
sample.JointsDegrees)).ToArray();
TrajectoryLimitValidator.ValidateJ519SendSamples(
_robot!,
samples,
@@ -583,8 +591,25 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
var sampleCount = samples.Count;
_logger?.LogInformation(
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, runtimeSpeedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
result.ProgramName, sampleCount, durationSeconds, _speedRatio, servoPeriodSeconds * 1000, triggers.Count);
var triggerBindings = preparedExecution is null
? TriggerSampleBinder.Bind(
triggers,
samples,
result.TriggerSampleIndexOffsetCycles)
: preparedExecution.TriggerBindings.Select(static binding =>
new TriggerSampleBinding(
binding.Trigger,
new J519SendSample(
binding.Sample.SampleIndex,
binding.Sample.SendTime,
binding.Sample.TrajectoryTime,
binding.Sample.SpeedRatio,
binding.Sample.JointsDegrees),
binding.SampleIndex,
binding.FoundInWindow)).ToArray();
var stopwatch = Stopwatch.StartNew();
ushort ioValue = 0;
@@ -592,6 +617,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
int holdRemaining = -1;
long logInterval = Math.Max(1, sampleCount / 10);
int triggerFiredCount = 0;
var nextBindingIndex = 0;
var commands = new List<FanucJ519Command>(sampleCount);
var sentJointRows = new List<IReadOnlyList<double>>(sampleCount);
var sentTimingRows = new List<IReadOnlyList<double>>(sampleCount);
@@ -601,6 +627,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
double[]? previousJoints = null;
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
var preparedJerkRows = preparedExecution?.JerkRows;
var diagnosticRequestedSpeedRatio = _speedRatio > 0.0 && !double.IsNaN(_speedRatio) && !double.IsInfinity(_speedRatio)
? _speedRatio
: 1.0;
try
{
@@ -624,19 +654,30 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
// 检查当前周期是否有新的触发事件。
if (holdRemaining < 0 && !clearMaskAfterSend)
{
foreach (var trigger in triggers)
while (nextBindingIndex < triggerBindings.Count
&& triggerBindings[nextBindingIndex].SampleIndex < sample.SampleIndex)
{
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
{
ioMask = ComputeIoValue(trigger.AddressGroup);
ioValue = ioMask;
holdRemaining = Math.Max(trigger.HoldCycles - 1, 0);
triggerFiredCount++;
_logger?.LogInformation(
"J519 IO触发: time={Time:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
sample.TrajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
break;
}
nextBindingIndex++;
}
if (nextBindingIndex < triggerBindings.Count
&& triggerBindings[nextBindingIndex].SampleIndex == sample.SampleIndex)
{
var binding = triggerBindings[nextBindingIndex];
ioMask = ComputeIoValue(binding.Trigger.AddressGroup);
ioValue = ioMask;
holdRemaining = Math.Max(binding.Trigger.HoldCycles - 1, 0);
triggerFiredCount++;
nextBindingIndex++;
_logger?.LogInformation(
"J519 IO触发: waypointIndex={WaypointIndex}, theoryTime={TheoryTime:F4}s, sampleIndex={SampleIndex}, sampleSendTime={SampleSendTime:F4}s, sampleTrajectoryTime={SampleTrajectoryTime:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
binding.Trigger.WaypointIndex,
binding.Trigger.TriggerTime,
binding.SampleIndex,
binding.Sample.SendTime,
binding.Sample.TrajectoryTime,
string.Join(",", binding.Trigger.AddressGroup.Addresses),
binding.Trigger.HoldCycles);
}
}
@@ -650,9 +691,21 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
commands.Add(command);
sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue));
sentTimingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
sentTimingRows.Add(preparedExecution is null
? J519SendTrajectorySampler.BuildTimingRow(sample, diagnosticRequestedSpeedRatio, stretchIterationCount: 0)
: J519SendTrajectorySampler.BuildTimingRow(
sample,
preparedExecution.RequestSpeedRatio,
preparedExecution.StretchIterationCount));
if (previousSendTime is not null && previousJoints is not null)
if (preparedJerkRows is not null)
{
if (sample.SampleIndex > 0 && sample.SampleIndex - 1 < preparedJerkRows.Count)
{
sentJerkRows.Add(preparedJerkRows[(int)sample.SampleIndex - 1]);
}
}
else if (previousSendTime is not null && previousJoints is not null)
{
var jerkRow = J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
@@ -684,7 +737,19 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
sample.TrajectoryTime);
}
}
TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentTimingRows, sentJerkRows);
if (_j519Client.EndCommandStreamAfterQueueDrained && commands.Count > 0)
{
// 正常结束命令流时追加独立结束帧,避免把最后一个真实轨迹采样点和结束语义混在一起。
commands.Add(new FanucJ519Command(
sequence: 0,
targetJoints: commands[^1].TargetJoints,
lastData: 1,
writeIoType: 2,
writeIoIndex: 1,
writeIoMask: 0,
writeIoValue: 0));
}
TryWriteDenseSendArtifacts(result, outputDir, sentJointRows, sentTimingRows, sentJerkRows, triggerBindings);
// 上层只负责生成完整目标序列,真实出队节拍交给 J519 状态包驱动。
_j519Client.LoadCommandQueue(commands);
@@ -713,6 +778,84 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 直角稠密轨迹发送任务:预生成 Data format=0 的 J519 命令队列,并等待状态包驱动执行完成。
/// </summary>
private void SendDenseCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose, CancellationToken cancellationToken)
{
var rows = result.DenseCartesianTrajectory ?? throw new InvalidOperationException("Cartesian trajectory requires dense Cartesian samples.");
var commands = new List<FanucJ519Command>(rows.Count);
var sentPoseRows = new List<IReadOnlyList<double>>(rows.Count);
var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
var stopwatch = Stopwatch.StartNew();
try
{
foreach (var row in rows)
{
cancellationToken.ThrowIfCancellationRequested();
if (row.Count != 7)
{
throw new InvalidOperationException("Cartesian dense trajectory rows must contain time plus six pose values.");
}
var pose = row.Skip(1).Take(6).ToArray();
var command = new FanucJ519Command(
sequence: 0,
targetValues: BuildCartesianTargetValues(pose),
dataStyle: 0,
writeIoType: 2,
writeIoIndex: 1,
writeIoMask: 0,
writeIoValue: 0);
commands.Add(command);
sentPoseRows.Add(BuildDenseSendPoseRow(row[0], pose));
}
if (_j519Client.EndCommandStreamAfterQueueDrained && commands.Count > 0)
{
// 直角轨迹结束帧沿用最终目标位姿,并明确清空 IO 输出。
commands.Add(new FanucJ519Command(
sequence: 0,
targetValues: commands[^1].TargetValues,
lastData: 1,
dataStyle: 0,
writeIoType: 2,
writeIoIndex: 1,
writeIoMask: 0,
writeIoValue: 0));
}
TryWriteDenseCartesianSendArtifacts(outputDir, sentPoseRows);
_j519Client.LoadCommandQueue(commands);
if (_j519Client.IsConnected)
{
_j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
}
_logger?.LogInformation(
"SendDenseCartesianTrajectory 正常完成: 采样数={SampleCount}, 队列装载耗时={ElapsedMs}ms",
commands.Count,
stopwatch.ElapsedMilliseconds);
}
catch (OperationCanceledException)
{
_logger?.LogWarning(
"SendDenseCartesianTrajectory 被取消: 已生成 {Current}/{Total} 条命令",
commands.Count,
rows.Count);
}
finally
{
lock (_stateLock)
{
_isInMotion = false;
_pose = MergeFinalCartesianPose(finalPose);
}
}
}
/// <summary>
/// 若已有 J519 响应,则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
/// </summary>
@@ -734,7 +877,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
/// <param name="getLatestResponse">读取最新 J519 状态的委托。</param>
/// <param name="retryEnableRobot">状态未就绪时触发一次 EnableRobot 重试的委托。</param>
/// <param name="waitAfterRetry">重试后等待状态刷新的委托。</param>
internal static void EnsureJ519ReadyForDenseExecutionCore(
internal void EnsureJ519ReadyForDenseExecutionCore(
Func<FanucJ519Response?> getLatestResponse,
Action retryEnableRobot,
Action waitAfterRetry)
@@ -744,13 +887,22 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
ArgumentNullException.ThrowIfNull(waitAfterRetry);
var response = getLatestResponse();
if (response is null || IsJ519ReadyForDenseExecution(response))
{
return;
}
retryEnableRobot();
waitAfterRetry();
_logger?.LogInformation("当前机器人不满足执行条件,重新启用机器人");
try
{
retryEnableRobot();
waitAfterRetry();
}
catch (InvalidOperationException exception)
{
// 重试链路自身失败时,优先保留 J519 状态位诊断,底层连接异常作为 inner exception 继续上抛。
throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response), exception);
}
response = getLatestResponse();
if (response is null || IsJ519ReadyForDenseExecution(response))
@@ -824,26 +976,65 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
return row;
}
/// <summary>
/// 构造实际发送直角位姿文本行,格式为 send_time + X/Y/Z/W/P/R。
/// </summary>
private static IReadOnlyList<double> BuildDenseSendPoseRow(double sendTime, IReadOnlyList<double> pose)
{
var row = new double[pose.Count + 1];
row[0] = Math.Round(sendTime, 6);
for (var index = 0; index < pose.Count; index++)
{
row[index + 1] = Math.Round(pose[index], 6);
}
return row;
}
/// <summary>
/// 构造 J519 直角命令的 9 个目标槽位,当前现场无扩展轴时 E1/E2/E3 补零。
/// </summary>
private static double[] BuildCartesianTargetValues(IReadOnlyList<double> pose)
{
EnsurePoseCount(pose.Count);
return [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], 0.0, 0.0, 0.0];
}
/// <summary>
/// 将六维 WPR 直角目标合并回运行时缓存,保留 GetPose 的 7 维兼容外形。
/// </summary>
private double[] MergeFinalCartesianPose(IReadOnlyList<double> pose)
{
EnsurePoseCount(pose.Count);
return _pose.Length >= 7
? [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], _pose[6]]
: pose.ToArray();
}
/// <summary>
/// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件;若落盘失败,只记录日志,不影响运动主流程。
/// </summary>
private void TryWriteDenseSendArtifacts(
TrajectoryResult result,
string outputDir,
IReadOnlyList<IReadOnlyList<double>> sentJointRows,
IReadOnlyList<IReadOnlyList<double>> sentTimingRows,
IReadOnlyList<IReadOnlyList<double>> sentJerkRows)
IReadOnlyList<IReadOnlyList<double>> sentJerkRows,
IReadOnlyList<TriggerSampleBinding> triggerBindings)
{
try
{
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), sentJointRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), sentTimingRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), sentJerkRows);
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
_logger?.LogInformation(
"SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, timingRows={TimingRows}, jerkRows={JerkRows}",
"SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, timingRows={TimingRows}, jerkRows={JerkRows}, triggerBindings={TriggerBindings}",
outputDir,
sentJointRows.Count,
sentTimingRows.Count,
sentJerkRows.Count);
sentJerkRows.Count,
triggerBindings.Count);
}
catch (Exception exception)
{
@@ -851,6 +1042,25 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 尝试把直角实际发送点位写入文本文件;若落盘失败,只记录日志,不影响运动主流程。
/// </summary>
private void TryWriteDenseCartesianSendArtifacts(string outputDir, IReadOnlyList<IReadOnlyList<double>> sentPoseRows)
{
try
{
WriteDenseRows(Path.Combine(outputDir, "ActualSendPoseTraj.txt"), sentPoseRows);
_logger?.LogInformation(
"SendDenseCartesianTrajectory 已写出实际发送记录: outputDir={OutputDir}, poseRows={PoseRows}",
outputDir,
sentPoseRows.Count);
}
catch (Exception exception)
{
_logger?.LogWarning(exception, "SendDenseCartesianTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
}
}
/// <summary>
/// 以旧轨迹文本兼容的空格分隔格式写出数值行。
/// </summary>
@@ -888,37 +1098,22 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
/// <summary>
/// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码。
/// 手册约定 writeIoIndex=1 时,LSB 对应第 1 个 IO 点,因此地址 N 对应 bit(N-1)。
/// </summary>
internal static ushort ComputeIoValue(IoAddressGroup group)
{
ushort value = 0;
foreach (var addr in group.Addresses)
{
if (addr is >= 0 and < 16)
if (addr is >= 1 and <= 16)
{
value |= (ushort)(1 << addr);
value |= (ushort)(1 << (addr - 1));
}
}
return value;
}
/// <summary>
/// 判断当前是否处于仿真模式;若尚未选择控制器则抛出异常。
/// </summary>
private bool IsSimulationMode
{
get
{
if (_activeControllerIsSimulation is null)
{
return false;
}
return _activeControllerIsSimulation.Value;
}
}
/// <summary>
/// 断开所有真实 Socket 通道,不影响内存状态。
/// </summary>
@@ -976,9 +1171,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
return "Disconnected";
}
return _activeControllerIsSimulation is false
? ResolveRealConnectionState(_stateClient.GetStatus())
: "Connected";
return ResolveRealConnectionState(_stateClient.GetStatus());
}
/// <summary>
@@ -1029,6 +1222,17 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 校验直角位姿固定为 [x,y,z,w,p,r] 六维。
/// </summary>
private static void EnsurePoseCount(int poseCount)
{
if (poseCount != 6)
{
throw new InvalidOperationException($"Expected 6 Cartesian pose values but received {poseCount}.");
}
}
/// <summary>
/// 校验机器人已经完成初始化。
/// </summary>
@@ -1040,24 +1244,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 校验活动控制器已经被选择。
/// </summary>
private void EnsureActiveControllerSelected()
{
EnsureRobotSetup();
if (_activeControllerIsSimulation is null)
{
throw new InvalidOperationException("Active controller has not been selected.");
}
}
/// <summary>
/// 校验控制器已经建立连接。
/// </summary>
private void EnsureConnected()
{
EnsureActiveControllerSelected();
EnsureRobotSetup();
if (string.IsNullOrWhiteSpace(_connectedRobotIp))
{
throw new InvalidOperationException("Controller has not been connected.");

View File

@@ -12,6 +12,10 @@ public sealed class FanucJ519Client : IDisposable
private readonly object _commandLock = new();
private readonly object _responseLock = new();
private readonly ILogger<FanucJ519Client>? _logger;
/// <summary>
/// 指示预装队列发出 LastData 结束帧后是否进入仅监控状态,而不是继续发送保姿命令。
/// </summary>
private readonly bool _endCommandStreamAfterQueueDrained;
private UdpClient? _udpClient;
private CancellationTokenSource? _cts;
private Thread? _receiveThread;
@@ -25,8 +29,14 @@ public sealed class FanucJ519Client : IDisposable
private long _slowSendCount;
private long _maxReceiveToSendTicks;
private uint _sequenceBufferSize;
// 标记 StartMotion 前是否刚装载过新目标,用于区分新命令和上次运动残留目标。
/// <summary>
/// 在 StartMotion 前是否有待发送的新命令;如果为 true 则 StartMotion 不会清空当前目标,而是继续发送新目标。
/// </summary>
private bool _hasPendingCommandForStart;
/// <summary>
/// 标记命令流已通过 LastData 正常结束;该状态下仅接收状态包,不继续回发 hold 命令。
/// </summary>
private bool _monitorOnlyAfterLastData;
private bool _motionStarted;
private bool _disposed;
@@ -35,13 +45,22 @@ public sealed class FanucJ519Client : IDisposable
/// </summary>
public bool IsConnected => _udpClient is not null;
/// <summary>
/// 获取正常队列结束时是否通过 LastData 结束命令流并保留状态监控。
/// </summary>
internal bool EndCommandStreamAfterQueueDrained => _endCommandStreamAfterQueueDrained;
/// <summary>
/// 初始化 FANUC J519 客户端。
/// </summary>
/// <param name="logger">日志记录器;允许 null,供无日志场景使用。</param>
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
/// <param name="endCommandStreamAfterQueueDrained">队列结束帧发出后是否停止命令流并保留状态监控。</param>
public FanucJ519Client(
ILogger<FanucJ519Client>? logger = null,
bool endCommandStreamAfterQueueDrained = true)
{
_logger = logger;
_endCommandStreamAfterQueueDrained = endCommandStreamAfterQueueDrained;
}
/// <summary>
@@ -113,6 +132,7 @@ public sealed class FanucJ519Client : IDisposable
}
_hasPendingCommandForStart = false;
_monitorOnlyAfterLastData = false;
_motionStarted = true;
}
@@ -156,6 +176,7 @@ public sealed class FanucJ519Client : IDisposable
{
_motionStarted = false;
_hasPendingCommandForStart = false;
_monitorOnlyAfterLastData = false;
CompleteCommandQueueLocked();
}
@@ -178,6 +199,7 @@ public sealed class FanucJ519Client : IDisposable
CompleteCommandQueueLocked();
_currentCommand = command;
_hasPendingCommandForStart = true;
_monitorOnlyAfterLastData = false;
_commandHistoryForTests?.Add(command);
}
@@ -211,6 +233,8 @@ public sealed class FanucJ519Client : IDisposable
// 队列耗尽后继续保持最后一帧目标,避免运动结束后回落到旧目标或空目标。
_currentCommand = commands[^1];
_hasPendingCommandForStart = true;
_monitorOnlyAfterLastData = false;
_motionStarted = true;
_commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
_commandHistoryForTests?.AddRange(commands);
}
@@ -337,6 +361,7 @@ public sealed class FanucJ519Client : IDisposable
CompleteCommandQueueLocked();
_commandHistoryForTests = null;
_hasPendingCommandForStart = false;
_monitorOnlyAfterLastData = false;
_motionStarted = false;
}
@@ -375,6 +400,7 @@ public sealed class FanucJ519Client : IDisposable
{
CompleteCommandQueueLocked();
_hasPendingCommandForStart = false;
_monitorOnlyAfterLastData = false;
}
}
@@ -526,9 +552,15 @@ public sealed class FanucJ519Client : IDisposable
{
FanucJ519Command? command;
var willDrainQueue = false;
var suppressHoldCommand = false;
lock (_commandLock)
{
if (!_motionStarted)
if (_monitorOnlyAfterLastData)
{
command = null;
suppressHoldCommand = true;
}
else if (!_motionStarted)
{
command = null;
}
@@ -546,7 +578,7 @@ public sealed class FanucJ519Client : IDisposable
}
}
if (command is null)
if (command is null && !suppressHoldCommand)
{
command = TryBuildHoldCommandFromLatestResponse(response);
}
@@ -574,7 +606,14 @@ public sealed class FanucJ519Client : IDisposable
lock (_commandLock)
{
_lastSentCommand = command;
if (willDrainQueue && _commandQueue is { Count: 0 })
if (_endCommandStreamAfterQueueDrained && command.LastData == 1)
{
// LastData 是命令流的正常结束语义;结束后保留状态接收,但不继续回发保姿包。
_monitorOnlyAfterLastData = true;
_motionStarted = false;
CompleteCommandQueueLocked();
}
else if (willDrainQueue && _commandQueue is { Count: 0 })
{
CompleteCommandQueueLocked();
}

View File

@@ -7,7 +7,7 @@ namespace Flyshot.Runtime.Fanuc.Protocol;
/// </summary>
public sealed class FanucJ519Command
{
private readonly double[] _targetJoints;
private readonly double[] _targetValues;
/// <summary>
/// 初始化 J519 命令数据。
@@ -35,11 +35,53 @@ public sealed class FanucJ519Command
ushort writeIoIndex = 1,
ushort writeIoMask = 0,
ushort writeIoValue = 0)
: this(
sequence,
targetValues: targetJoints,
lastData,
readIoType,
readIoIndex,
readIoMask,
dataStyle,
writeIoType,
writeIoIndex,
writeIoMask,
writeIoValue)
{
ArgumentNullException.ThrowIfNull(targetJoints);
if (targetJoints.Count is <= 0 or > 9)
}
/// <summary>
/// 初始化 J519 命令数据。
/// </summary>
/// <param name="sequence">命令序号。</param>
/// <param name="targetValues">J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3,关节模式为 J1..J9。</param>
/// <param name="lastData">是否为最后一帧数据。</param>
/// <param name="readIoType">读取 IO 类型。</param>
/// <param name="readIoIndex">读取 IO 起始索引。</param>
/// <param name="readIoMask">读取 IO 掩码。</param>
/// <param name="dataStyle">目标数据类型,0 为直角坐标,1 为关节坐标。</param>
/// <param name="writeIoType">写入 IO 类型。</param>
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
/// <param name="writeIoMask">写入 IO 掩码。</param>
/// <param name="writeIoValue">写入 IO 数值。</param>
public FanucJ519Command(
uint sequence,
IEnumerable<double> targetValues,
byte lastData = 0,
byte readIoType = 2,
ushort readIoIndex = 1,
ushort readIoMask = 255,
byte dataStyle = 1,
byte writeIoType = 2,
ushort writeIoIndex = 1,
ushort writeIoMask = 0,
ushort writeIoValue = 0)
{
ArgumentNullException.ThrowIfNull(targetValues);
var copiedTargetValues = targetValues.ToArray();
if (copiedTargetValues.Length is <= 0 or > 9)
{
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
throw new ArgumentOutOfRangeException(nameof(targetValues), "J519 目标数据必须包含 1 到 9 个槽位。");
}
Sequence = sequence;
@@ -52,7 +94,7 @@ public sealed class FanucJ519Command
WriteIoIndex = writeIoIndex;
WriteIoMask = writeIoMask;
WriteIoValue = writeIoValue;
_targetJoints = targetJoints.ToArray();
_targetValues = copiedTargetValues;
}
/// <summary>
@@ -108,7 +150,12 @@ public sealed class FanucJ519Command
/// <summary>
/// 获取目标关节或扩展轴数据。
/// </summary>
public IReadOnlyList<double> TargetJoints => _targetJoints;
public IReadOnlyList<double> TargetJoints => _targetValues;
/// <summary>
/// 获取 J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3,关节模式为 J1..J9。
/// </summary>
public IReadOnlyList<double> TargetValues => _targetValues;
}
/// <summary>

View File

@@ -0,0 +1,13 @@
{
"version": 1,
"isRoot": true,
"tools": {
"dotnet-ef": {
"version": "10.0.7",
"commands": [
"dotnet-ef"
],
"rollForward": false
}
}
}

View File

@@ -25,59 +25,6 @@ public sealed class LegacyHttpApiController : ControllerBase
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
}
/// <summary>
/// 兼容旧根路径探活接口。
/// </summary>
/// <returns>旧 HTTP 服务约定的 Hello World 响应。</returns>
[HttpGet("/")]
public IActionResult Root()
{
return Ok(new { message = "Hello World" });
}
/// <summary>
/// 兼容旧 `/connect_server/` 路由;在 replacement 宿主中仅记录调用方期望连接的地址。
/// </summary>
/// <param name="server_ip">旧客户端传入的服务端 IP。</param>
/// <param name="port">旧客户端传入的服务端端口。</param>
/// <returns>与旧 FastAPI 层一致的状态响应。</returns>
[HttpPost("/connect_server/")]
public IActionResult ConnectServer([FromQuery] string server_ip, [FromQuery] int port)
{
_logger.LogInformation("ConnectServer 调用: server_ip={ServerIp}, port={Port}", server_ip, port);
try
{
_compatService.ConnectServer(server_ip, port);
_logger.LogInformation("ConnectServer 成功: server_ip={ServerIp}, port={Port}", server_ip, port);
return Ok(new { status = "connected" });
}
catch (Exception exception)
{
_logger.LogError(exception, "ConnectServer 失败: server_ip={ServerIp}, port={Port}", server_ip, port);
return LegacyBadRequest("Connect Server failed");
}
}
/// <summary>
/// 兼容旧 `GetServerVersion` 版本查询语义。
/// </summary>
/// <returns>服务端版本号。</returns>
[HttpGet("/get_server_version/")]
public IActionResult GetServerVersion()
{
return Ok(new { server_version = _compatService.GetServerVersion() });
}
/// <summary>
/// 兼容旧 `GetClientVersion` 版本查询语义。
/// </summary>
/// <returns>客户端版本号。</returns>
[HttpGet("/get_client_version/")]
public IActionResult GetClientVersion()
{
return Ok(new { client_version = _compatService.GetClientVersion() });
}
/// <summary>
/// 兼容旧 `/setup_robot/` 路由。
/// </summary>
@@ -100,59 +47,6 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 兼容旧 `SetUpRobotFromEnv(env_file)` 参数形状。
/// </summary>
/// <param name="env_file">环境文件路径。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/setup_robot_from_env/")]
public IActionResult SetupRobotFromEnv([FromQuery] string env_file)
{
try
{
_compatService.SetUpRobotFromEnv(env_file);
return Ok(new { status = "robot setup" });
}
catch
{
return LegacyBadRequest("SetUpRobotFromEnv failed");
}
}
/// <summary>
/// 兼容旧 `SetShowTCP(is_show, axis_length, axis_size)` 参数形状。
/// </summary>
/// <param name="is_show">是否显示 TCP。</param>
/// <param name="axis_length">坐标轴长度。</param>
/// <param name="axis_size">坐标轴线宽。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/set_show_tcp/")]
public IActionResult SetShowTcp(
[FromQuery] bool is_show = true,
[FromQuery] double axis_length = 0.1,
[FromQuery] int axis_size = 2)
{
try
{
_compatService.SetShowTcp(is_show, axis_length, axis_size);
return Ok(new { status = "show TCP set" });
}
catch
{
return LegacyBadRequest("SetShowTCP failed");
}
}
/// <summary>
/// 兼容旧 `/is_setup/` 路由。
/// </summary>
/// <returns>当前机器人是否完成初始化。</returns>
[HttpGet("/is_setup/")]
public IActionResult IsSetup()
{
return Ok(new { is_setup = _compatService.IsSetUp });
}
/// <summary>
/// 兼容旧 `EnableRobot(buffer_size=4)` 参数形状。
/// </summary>
@@ -217,28 +111,6 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 兼容旧 `/set_active_controller/` 路由。
/// </summary>
/// <param name="sim">是否切到仿真控制器。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/set_active_controller/")]
public IActionResult SetActiveController([FromQuery] bool sim)
{
_logger.LogInformation("SetActiveController 调用: sim={Sim}", sim);
try
{
_compatService.SetActiveController(sim);
_logger.LogInformation("SetActiveController 成功: sim={Sim}", sim);
return Ok(new { status = "active controller set" });
}
catch (Exception exception)
{
_logger.LogError(exception, "SetActiveController 失败: sim={Sim}", sim);
return LegacyBadRequest("SetActiveController failed");
}
}
/// <summary>
/// 兼容旧 `/connect_robot/` 路由。
/// </summary>
@@ -431,6 +303,32 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 以直角坐标 `[x,y,z,w,p,r]` 执行点到点移动。
/// </summary>
/// <param name="pose_data">直角位姿请求体。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/move_pose/")]
public IActionResult MovePose([FromBody] JsonElement pose_data)
{
try
{
var poseRequest = LegacyCartesianPoseRequest.FromJson(pose_data);
var pose = poseRequest.ToPoseArray();
_logger.LogInformation("MovePose 调用: x={X}, y={Y}, z={Z}, w={W}, p={P}, r={R}",
poseRequest.x, poseRequest.y, poseRequest.z, poseRequest.w, poseRequest.p, poseRequest.r);
_compatService.MovePose(pose);
_logger.LogInformation("MovePose 成功");
return Ok(new { status = "robot moved" });
}
catch (Exception exception)
{
_logger.LogError(exception, "MovePose 失败");
return LegacyBadRequest("MovePose failed");
}
}
/// <summary>
/// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。
/// </summary>
@@ -461,11 +359,6 @@ public sealed class LegacyHttpApiController : ControllerBase
public IActionResult ListFlyshotTrajectories()
{
var names = _compatService.ListTrajectoryNames();
if (names.Count == 0)
{
return LegacyBadRequest("ListFlyShotTraj failed");
}
return Ok(new { flyshot_trajs = names });
}
@@ -597,9 +490,9 @@ public sealed class LegacyHttpApiController : ControllerBase
_logger.LogInformation("SaveTrajectoryInfo 调用: name={Name}, method={Method}", request.name, request.method);
try
{
_compatService.SaveTrajectoryInfo(request.name, request.method);
var result = _compatService.SaveTrajectoryInfo(request.name, request.method);
_logger.LogInformation("SaveTrajectoryInfo 成功: name={Name}", request.name);
return Ok(new { status = "trajectory info saved", success = true });
return Ok(result);
}
catch (NotSupportedException exception)
{
@@ -697,36 +590,26 @@ public sealed class LegacyHttpApiController : ControllerBase
}
/// <summary>
/// 兼容旧 `/init_mpc_robt` 路由,保留历史拼写。
/// 使用机器人名称和 IP 执行真机优先初始化。
/// </summary>
/// <param name="data">初始化请求体。</param>
/// <returns>旧 FastAPI 层风格的初始化结果。</returns>
[HttpPost("/init_mpc_robt")]
public IActionResult InitMpcRobot([FromBody] LegacyInitMpcRobotRequest data)
[HttpPost("/init_mpc_robot")]
public IActionResult InitMpcRobot([FromBody] InitMpcRobotRequest data)
{
_logger.LogInformation(
"InitMpcRobot 调用: robot_name={RobotName}, robot_ip={RobotIp}, sim={Sim}, server={ServerIp}:{Port}",
data.robot_name, data.robot_ip, data.sim, data.server_ip, data.port);
"InitMpcRobot 调用: robot_name={RobotName}, robot_ip={RobotIp}",
data.robot_name, data.robot_ip);
try
{
_compatService.SetUpRobot(data.robot_name);
if (!_compatService.IsSetUp)
{
_logger.LogWarning("InitMpcRobot 失败: Robot not setup");
return LegacyBadRequest("Robot not setup");
}
_compatService.SetActiveController(data.sim);
_compatService.Connect(data.robot_ip);
_compatService.EnableRobot(8);
_compatService.InitializeRobot(data.robot_name, data.robot_ip, bufferSize: 8);
_logger.LogInformation("InitMpcRobot 成功: robot_name={RobotName}", data.robot_name);
return Ok(new { message = "init_Success", returnCode = 0 });
}
catch (Exception exception)
{
_logger.LogError(exception, "InitMpcRobot 失败");
return LegacyBadRequest("Connect Server failed");
return BadRequest(new { detail = exception.Message });
}
}
@@ -874,6 +757,136 @@ public sealed class LegacyJointPositionRequest
public List<double> joints { get; init; } = [];
}
/// <summary>
/// 表示 `/move_pose/` 路由使用的直角位姿请求体。
/// </summary>
public sealed class LegacyCartesianPoseRequest
{
/// <summary>
/// MovePose 第一版入口硬限制:TCP X/Y 最大绝对值,单位 mm。
/// </summary>
private const double MaxHorizontalMillimeters = 1000.0;
/// <summary>
/// MovePose 第一版入口硬限制:TCP Z 最小值,单位 mm。
/// </summary>
private const double MinZMillimeters = 0.0;
/// <summary>
/// MovePose 第一版入口硬限制:TCP Z 最大值,单位 mm。
/// </summary>
private const double MaxZMillimeters = 1200.0;
/// <summary>
/// MovePose 第一版入口硬限制:W/R 姿态角最大绝对值,单位 deg。
/// </summary>
private const double MaxRollYawDegrees = 180.0;
/// <summary>
/// MovePose 第一版入口硬限制:P 姿态角最大绝对值,单位 deg。
/// </summary>
private const double MaxPitchDegrees = 90.0;
/// <summary>
/// 获取或设置 TCP X,单位为 mm。
/// </summary>
public double x { get; init; }
/// <summary>
/// 获取或设置 TCP Y,单位为 mm。
/// </summary>
public double y { get; init; }
/// <summary>
/// 获取或设置 TCP Z,单位为 mm。
/// </summary>
public double z { get; init; }
/// <summary>
/// 获取或设置姿态 W,单位为 deg。
/// </summary>
public double w { get; init; }
/// <summary>
/// 获取或设置姿态 P,单位为 deg。
/// </summary>
public double p { get; init; }
/// <summary>
/// 获取或设置姿态 R,单位为 deg。
/// </summary>
public double r { get; init; }
/// <summary>
/// 从原始 JSON 请求体解析直角位姿,并显式拒绝缺字段、null 和非有限数。
/// </summary>
/// <param name="json">原始 JSON 请求体。</param>
/// <returns>解析后的直角位姿请求。</returns>
public static LegacyCartesianPoseRequest FromJson(JsonElement json)
{
if (json.ValueKind != JsonValueKind.Object)
{
throw new ArgumentException("MovePose request body must be an object.");
}
// 旧接口要求请求体必须完整提供 x/y/z/w/p/r,不能让模型绑定把缺字段静默补 0。
return new LegacyCartesianPoseRequest
{
x = ReadRequiredFiniteDouble(json, "x", -MaxHorizontalMillimeters, MaxHorizontalMillimeters),
y = ReadRequiredFiniteDouble(json, "y", -MaxHorizontalMillimeters, MaxHorizontalMillimeters),
z = ReadRequiredFiniteDouble(json, "z", MinZMillimeters, MaxZMillimeters),
w = ReadRequiredFiniteDouble(json, "w", -MaxRollYawDegrees, MaxRollYawDegrees),
p = ReadRequiredFiniteDouble(json, "p", -MaxPitchDegrees, MaxPitchDegrees),
r = ReadRequiredFiniteDouble(json, "r", -MaxRollYawDegrees, MaxRollYawDegrees)
};
}
/// <summary>
/// 转换为兼容层使用的六维位姿数组。
/// </summary>
/// <returns>[x,y,z,w,p,r] 数组。</returns>
public IReadOnlyList<double> ToPoseArray()
{
return [x, y, z, w, p, r];
}
/// <summary>
/// 读取必填有限数值字段。
/// </summary>
/// <param name="json">请求体 JSON 对象。</param>
/// <param name="propertyName">字段名。</param>
/// <param name="minInclusive">允许的最小值。</param>
/// <param name="maxInclusive">允许的最大值。</param>
/// <returns>字段对应的有限 double 数值。</returns>
private static double ReadRequiredFiniteDouble(
JsonElement json,
string propertyName,
double minInclusive,
double maxInclusive)
{
if (!json.TryGetProperty(propertyName, out var property) || property.ValueKind != JsonValueKind.Number)
{
throw new ArgumentException($"MovePose request field '{propertyName}' is required and must be a number.");
}
var value = property.GetDouble();
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(propertyName, "MovePose request values must be finite.");
}
if (value < minInclusive || value > maxInclusive)
{
throw new ArgumentOutOfRangeException(
propertyName,
value,
$"MovePose request field '{propertyName}' must be between {minInclusive} and {maxInclusive}.");
}
return value;
}
}
/// <summary>
/// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。
/// </summary>
@@ -1017,20 +1030,10 @@ public sealed class LegacySpeedRatioRequest
}
/// <summary>
/// 表示旧 `/init_mpc_robt` 路由使用的初始化请求体。
/// 表示 `/init_mpc_robot` 路由使用的真机初始化请求体。
/// </summary>
public sealed class LegacyInitMpcRobotRequest
public sealed class InitMpcRobotRequest
{
/// <summary>
/// 获取或设置目标服务端 IP。
/// </summary>
public string server_ip { get; init; } = string.Empty;
/// <summary>
/// 获取或设置目标服务端端口。
/// </summary>
public int port { get; init; }
/// <summary>
/// 获取或设置机器人名称。
/// </summary>
@@ -1041,8 +1044,4 @@ public sealed class LegacyInitMpcRobotRequest
/// </summary>
public string robot_ip { get; init; } = string.Empty;
/// <summary>
/// 获取或设置是否使用仿真控制器;默认 false 连接真机。
/// </summary>
public bool sim { get; init; }
}

View File

@@ -49,23 +49,27 @@ public sealed class StatusController : ControllerBase
public IActionResult GetSnapshot()
{
var snapshot = _compatService.GetControllerSnapshot();
var isSetup = _compatService.IsSetUp;
// 状态页需要在机器人未初始化时仍能打开,因此只有初始化后才读取机器人元数据。
var robotName = isSetup ? _compatService.GetRobotName() : null;
var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0;
var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty<string>();
var metadata = _compatService.GetStatusSnapshotMetadata();
return Ok(new
{
Status = "ok",
Service = "flyshot-server-host",
ServerVersion = _compatService.GetServerVersion(),
ClientVersion = _compatService.GetClientVersion(),
IsSetup = isSetup,
RobotName = robotName,
DegreesOfFreedom = degreesOfFreedom,
UploadedTrajectories = uploadedTrajectories,
ServerVersion = metadata.ServerVersion,
ClientVersion = metadata.ClientVersion,
IsSetup = metadata.IsSetup,
RobotName = metadata.RobotName,
DegreesOfFreedom = metadata.DegreesOfFreedom,
UploadedTrajectories = metadata.UploadedTrajectories,
OperatingMode = metadata.OperatingMode,
InitializationStage = metadata.InitializationStage,
ModelLoaded = metadata.IsSetup,
RobotIp = metadata.RobotIp,
LastConnectionError = metadata.LastConnectionError,
LastConnectionErrorAt = metadata.LastConnectionErrorAt,
LastInitializationAttemptAt = metadata.LastInitializationAttemptAt,
CanPlanOffline = metadata.CanPlanOffline,
CanExecuteRealMotion = metadata.CanExecuteRealMotion,
Snapshot = snapshot
});
}

View File

@@ -1,7 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk.Web">
<PropertyGroup>
<!-- 生成 XML 文档以便 Swashbuckle 把控制器/DTO 上的 /// summary 注释注入 OpenAPI 文档,
调试页和 Swagger UI 的端点标题都依赖这一份文档。1591 抑制掉 “缺失 XML 注释” 的噪音。 -->
<!-- 生成 XML 文档以便 Swashbuckle 把控制器/DTO 上的 /// summary 注释注入 OpenAPI 文档;
调试页说明文本已经静态内嵌在 debug.html 中,这份 XML 现在只服务 Swagger UI。1591 抑制掉 “缺失 XML 注释” 的噪音。 -->
<GenerateDocumentationFile>true</GenerateDocumentationFile>
<NoWarn>$(NoWarn);1591</NoWarn>
</PropertyGroup>
@@ -16,6 +16,9 @@
<Content Include="..\..\NLog.config" Link="NLog.config" CopyToOutputDirectory="PreserveNewest" />
<!-- 运行时配置根目录固定为输出目录 Config,调试和发布都复制仓库内固化配置。 -->
<Content Include="..\..\Config\**\*" Link="Config\%(RecursiveDir)%(Filename)%(Extension)" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" />
<!-- Rust ICSP 原生规划库:从 src/Libs 固定复制到运行与发布目录,供默认 method=icsp 加载。 -->
<Content Include="..\Libs\flyshot_planning.dll" Link="flyshot_planning.dll" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" Condition="Exists('..\Libs\flyshot_planning.dll')" />
<Content Include="..\Libs\libflyshot_planning.so" Link="libflyshot_planning.so" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" Condition="Exists('..\Libs\libflyshot_planning.so')" />
</ItemGroup>
<ItemGroup>

View File

@@ -1,6 +1,7 @@
using Flyshot.ControllerClientCompat;
using Flyshot.Server.Host;
using Flyshot.Server.Host.Middleware;
using Flyshot.Server.Host.Services;
using Microsoft.Extensions.Options;
using Microsoft.OpenApi.Models;
using NLog.Web;
@@ -23,6 +24,10 @@ try
builder.Services.Configure<HostSwaggerOptions>(builder.Configuration.GetSection("Swagger"));
builder.Services.Configure<HostCorsOptions>(builder.Configuration.GetSection("Cors"));
builder.Services.AddControllerClientCompat(builder.Configuration);
// 注册服务启动时自动初始化机器人的托管服务;配置缺失时自动跳过。
builder.Services.AddHostedService<AutoInitRobotHostedService>();
builder.Services.AddControllers();
builder.Services.AddEndpointsApiExplorer();
@@ -35,7 +40,8 @@ try
Version = swaggerOptions.Version
});
// 把控制器与 DTO 上的 /// summary 注释纳入 OpenAPI 文档;调试页据此渲染端点标题。
// 把控制器与 DTO 上的 /// summary 注释纳入 OpenAPI 文档;当前主要服务 Swagger UI,
// 调试页的人类可读说明已经静态内嵌在 debug.html 中,不再依赖发布目录中的 XML 文档。
var xmlDocumentationPath = Path.Combine(AppContext.BaseDirectory, $"{typeof(Program).Assembly.GetName().Name}.xml");
if (File.Exists(xmlDocumentationPath))
{

View File

@@ -0,0 +1,80 @@
using Flyshot.ControllerClientCompat;
using Microsoft.Extensions.Options;
namespace Flyshot.Server.Host.Services;
/// <summary>
/// 服务启动时自动初始化机器人的托管服务。
/// 从配置文件读取 robot_name 和 robot_ip,执行统一的真机优先初始化流程。
/// 初始化失败不会阻止服务启动,仅记录警告日志。
/// </summary>
public sealed class AutoInitRobotHostedService : IHostedService
{
private readonly IControllerClientCompatService _compatService;
private readonly ControllerClientCompatOptions _options;
private readonly ILogger<AutoInitRobotHostedService> _logger;
/// <summary>
/// 初始化自动初始化机器人托管服务的实例。
/// </summary>
/// <param name="compatService">控制器兼容服务,用于执行机器人初始化操作。</param>
/// <param name="options">控制器兼容层配置,包含自动初始化参数。</param>
/// <param name="logger">日志记录器。</param>
public AutoInitRobotHostedService(
IControllerClientCompatService compatService,
IOptions<ControllerClientCompatOptions> options,
ILogger<AutoInitRobotHostedService> logger)
{
_compatService = compatService;
_options = options.Value;
_logger = logger;
}
/// <summary>
/// 服务启动时执行自动初始化。若配置中未指定 AutoInit 或参数为空,则跳过。
/// </summary>
/// <param name="cancellationToken">取消令牌。</param>
/// <returns>异步任务。</returns>
public Task StartAsync(CancellationToken cancellationToken)
{
var autoInit = _options.AutoInit;
// 未配置 AutoInit 或参数为空时,跳过自动初始化
if (autoInit == null
|| string.IsNullOrWhiteSpace(autoInit.RobotName)
|| string.IsNullOrWhiteSpace(autoInit.RobotIp))
{
_logger.LogInformation("未配置 AutoInit,跳过服务启动自动初始化。");
return Task.CompletedTask;
}
_logger.LogInformation(
"开始自动初始化机器人: robot_name={RobotName}, robot_ip={RobotIp}",
autoInit.RobotName, autoInit.RobotIp);
try
{
// 启动与人工初始化复用同一编排,确保状态阶段和失败降级语义一致。
_compatService.InitializeRobot(autoInit.RobotName, autoInit.RobotIp, bufferSize: 8);
_logger.LogInformation("自动初始化成功: robot_name={RobotName}", autoInit.RobotName);
}
catch (Exception ex)
{
// 初始化失败不阻止服务启动,仅记录警告
_logger.LogWarning(ex, "自动初始化机器人失败,服务将继续启动。robot_name={RobotName}", autoInit.RobotName);
}
return Task.CompletedTask;
}
/// <summary>
/// 服务停止时的清理操作。当前无需额外清理。
/// </summary>
/// <param name="cancellationToken">取消令牌。</param>
/// <returns>异步任务。</returns>
public Task StopAsync(CancellationToken cancellationToken)
{
return Task.CompletedTask;
}
}

View File

@@ -25,5 +25,12 @@
"AllowedHeaders": [
"*"
]
},
"Kestrel": {
"Endpoints": {
"Http": {
"Url": "http://0.0.0.0:5190"
}
}
}
}

View File

@@ -6,7 +6,11 @@
}
},
"ControllerClientCompat": {
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0"
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0",
"AutoInit": {
"RobotName": "LR_Mate_200iD",
"RobotIp": "127.0.0.1"
}
},
"Swagger": {
"Enabled": true,
@@ -30,5 +34,12 @@
"*"
]
},
"Kestrel": {
"Endpoints": {
"Http": {
"Url": "http://0.0.0.0:5000"
}
}
},
"AllowedHosts": "*"
}

Some files were not shown because too many files have changed in this diff Show More