Compare commits
18 Commits
milestone/
...
feat/rust-
| Author | SHA1 | Date | |
|---|---|---|---|
| bcbf8017ad | |||
| 537c6b5320 | |||
| 32cc740df5 | |||
| 9c56ee825e | |||
| bb21e0923d | |||
| 9a01ce851d | |||
| 5193f537b0 | |||
| 070d2ba5c0 | |||
| ac41d04a54 | |||
| 5c5eaf850d | |||
| 4aae6219cc | |||
| 35bdb08d11 | |||
| 0952958af7 | |||
| 2cd42f04e5 | |||
| d120ef2a39 | |||
| d82128da4a | |||
| 74761bb5da | |||
| f7e2bb0e7b |
8
.gitignore
vendored
8
.gitignore
vendored
@@ -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
15
.vscode/tasks.json
vendored
@@ -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": []
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -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": {
|
||||
|
||||
@@ -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
|
||||
|
||||
83
README.md
83
README.md
@@ -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。
|
||||
|
||||
21
analysis/2026042802-1/2026042802-1_analysis_summary.json
Normal file
21
analysis/2026042802-1/2026042802-1_analysis_summary.json
Normal 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
|
||||
}
|
||||
1789
analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv
Normal file
1789
analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv
Normal file
File diff suppressed because it is too large
Load Diff
1790
analysis/2026042802-1/2026042802-1_j519_status_feedback_all.csv
Normal file
1790
analysis/2026042802-1/2026042802-1_j519_status_feedback_all.csv
Normal file
File diff suppressed because it is too large
Load Diff
18
analysis/2026042802-1/2026042802-1_j519_trigger_frames.csv
Normal file
18
analysis/2026042802-1/2026042802-1_j519_trigger_frames.csv
Normal 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
|
||||
|
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
|
@@ -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
|
||||
|
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
@@ -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
|
||||
|
363
analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py
Normal file
363
analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py
Normal 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()
|
||||
250
analysis/analyze_2026042802_1_trigger_vs_teach_points.py
Normal file
250
analysis/analyze_2026042802_1_trigger_vs_teach_points.py
Normal 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()
|
||||
137
analysis/build_trigger_offset_compare_6_7_8.py
Normal file
137
analysis/build_trigger_offset_compare_6_7_8.py
Normal 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()
|
||||
200
analysis/read_fanuc_allowable_limits.py
Normal file
200
analysis/read_fanuc_allowable_limits.py
Normal 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())
|
||||
76
build/Copy-NativePlanningArtifacts.ps1
Normal file
76
build/Copy-NativePlanningArtifacts.ps1
Normal 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
BIN
docs/0.7x.pcap
Normal file
Binary file not shown.
526
docs/2026042802-1-trigger-vs-teach-point-analysis.md
Normal file
526
docs/2026042802-1-trigger-vs-teach-point-analysis.md
Normal 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 + 现场图像效果` 做下一轮闭环
|
||||
125
docs/camera-trigger-actual-offset-pcap-record-20260508.md
Normal file
125
docs/camera-trigger-actual-offset-pcap-record-20260508.md
Normal 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 |
|
||||
@@ -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`。
|
||||
|
||||
@@ -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` 与抓包一致。
|
||||
|
||||
@@ -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 手册算法靠近。
|
||||
|
||||
138
docs/figures/icsp-algorithm-ppt.svg
Normal file
138
docs/figures/icsp-algorithm-ppt.svg
Normal 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> ← 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 |
460
docs/flyshot-speedratio-execution-design-20260509.md
Normal file
460
docs/flyshot-speedratio-execution-design-20260509.md
Normal 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` 调速从“发送阶段经验插值”收敛为“规划/准备阶段完成、运行时确定发送”的执行机制。
|
||||
257
docs/marketing/飞拍控制系统宣传文稿.md
Normal file
257
docs/marketing/飞拍控制系统宣传文稿.md
Normal 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 自动识别,人工确认结果
|
||||
- 面向工业产线的端到端视觉质量平台
|
||||
657
docs/status-jog-joystick-demo.html
Normal file
657
docs/status-jog-joystick-demo.html
Normal 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>
|
||||
@@ -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`,确认没有第三方程序集或映射文件进入部署包。
|
||||
@@ -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(×, &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`
|
||||
```
|
||||
@@ -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 离线规划页面。
|
||||
- 最终执行核心测试、集成测试和解决方案构建。
|
||||
@@ -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 稳定运行所需的安全边界;若未来需要更强的控制流混淆、字符串加密或防篡改,应评估商业混淆器。
|
||||
271
docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md
Normal file
271
docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md
Normal 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 构建环境执行后才能标记完成。
|
||||
151
docs/uttc-ms11-jerk-validation-decision-20260511.md
Normal file
151
docs/uttc-ms11-jerk-validation-decision-20260511.md
Normal 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` 与旧抓包主段的剩余时长差异已经很小
|
||||
- 当前判断:这点差异可以忽略
|
||||
|
||||
后续除非现场再次提供新的反证,否则本结论作为当前项目实现与验收的记录保留。
|
||||
@@ -1,6 +1,6 @@
|
||||
{
|
||||
"sdk": {
|
||||
"version": "8.0.420",
|
||||
"rollForward": "disable"
|
||||
"version": "8.0.422",
|
||||
"rollForward": "latestPatch"
|
||||
}
|
||||
}
|
||||
|
||||
7
native/flyshot-native-planning/Cargo.lock
generated
Normal file
7
native/flyshot-native-planning/Cargo.lock
generated
Normal 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"
|
||||
13
native/flyshot-native-planning/Cargo.toml
Normal file
13
native/flyshot-native-planning/Cargo.toml
Normal 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"
|
||||
33
native/flyshot-native-planning/src/error.rs
Normal file
33
native/flyshot-native-planning/src/error.rs
Normal 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 {}
|
||||
436
native/flyshot-native-planning/src/ffi.rs
Normal file
436
native/flyshot-native-planning/src/ffi.rs
Normal 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("有限数"));
|
||||
}
|
||||
}
|
||||
367
native/flyshot-native-planning/src/icsp.rs
Normal file
367
native/flyshot-native-planning/src/icsp.rs
Normal 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(_)));
|
||||
}
|
||||
}
|
||||
6
native/flyshot-native-planning/src/lib.rs
Normal file
6
native/flyshot-native-planning/src/lib.rs
Normal file
@@ -0,0 +1,6 @@
|
||||
//! Flyshot ICSP 原生规划库的稳定入口。
|
||||
|
||||
mod error;
|
||||
mod ffi;
|
||||
mod icsp;
|
||||
mod spline;
|
||||
277
native/flyshot-native-planning/src/spline.rs
Normal file
277
native/flyshot-native-planning/src/spline.rs
Normal 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(×, &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(×, &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);
|
||||
}
|
||||
}
|
||||
642
scripts/Publish-Obfuscated.ps1
Normal file
642
scripts/Publish-Obfuscated.ps1
Normal 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
|
||||
}
|
||||
44
src/Flyshot.CSharp.Planning/CSharpIcspPlanningBackend.cs
Normal file
44
src/Flyshot.CSharp.Planning/CSharpIcspPlanningBackend.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
13
src/Flyshot.CSharp.Planning/Flyshot.CSharp.Planning.csproj
Normal file
13
src/Flyshot.CSharp.Planning/Flyshot.CSharp.Planning.csproj
Normal 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>
|
||||
@@ -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`
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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>
|
||||
/// 在 ICSP 外层包裹补中点策略,实现 self-adapt-icsp 行为。
|
||||
44
src/Flyshot.CSharp.Planning/SelfAdaptIcspPlanningBackend.cs
Normal file
44
src/Flyshot.CSharp.Planning/SelfAdaptIcspPlanningBackend.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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>();
|
||||
|
||||
@@ -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; }
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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)
|
||||
};
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
/// 检查指定飞拍轨迹是否可执行。
|
||||
|
||||
@@ -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 理论最短时长。
|
||||
///
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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);
|
||||
@@ -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; }
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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,
|
||||
|
||||
202
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
Normal file
202
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
Normal 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; }
|
||||
}
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
27
src/Flyshot.Core.Planning/IPlanningBackend.cs
Normal file
27
src/Flyshot.Core.Planning/IPlanningBackend.cs
Normal 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);
|
||||
}
|
||||
21
src/Flyshot.Core.Planning/PlanningBackendContext.cs
Normal file
21
src/Flyshot.Core.Planning/PlanningBackendContext.cs
Normal 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; }
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -1,7 +1,9 @@
|
||||
using Flyshot.Core.Planning;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把规划层稠密关节轨迹重采样为 J519 物理发送周期上的角度制目标。
|
||||
/// 负责把规划层关节轨迹重采样为 J519 物理发送周期上的角度制目标。
|
||||
/// <para>
|
||||
/// 算法约定:
|
||||
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN],time 为规划轨迹时间,关节单位为弧度;
|
||||
@@ -13,10 +15,12 @@ namespace Flyshot.Core.Planning.Sampling;
|
||||
/// 末尾额外保留一个终点钳制周期,确保轨迹时长不是周期整数倍时仍会输出最终点。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 飞拍最终发送队列优先从 <see cref="PlannedTrajectory"/> 重建连续三次样条并直接取点;
|
||||
/// 旧稠密点入口仅保留给运行时 fallback 和兼容导出。
|
||||
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod;
|
||||
/// speedRatio 不改变物理发送周期,只用于把发送时间映射回规划轨迹时间:
|
||||
/// trajectoryTime = min(sendTime * speedRatio, duration)。
|
||||
/// 之后在原始稠密关节轨迹上按 trajectoryTime 做线性插值,并把每个关节从 rad 转为 deg。
|
||||
/// 之后在连续样条或旧稠密轨迹上求取关节,并把每个关节从 rad 转为 deg。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 诊断行也在这里统一生成:Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio;
|
||||
@@ -26,6 +30,39 @@ namespace Flyshot.Core.Planning.Sampling;
|
||||
/// </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>
|
||||
@@ -83,7 +120,7 @@ public static class J519SendTrajectorySampler
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造实发时间映射文本行,格式为 sample_index + send_time + trajectory_time + speed_ratio。
|
||||
/// 构造实发时间映射文本行,格式为 sample_index + send_time + trajectory_time + final_speed_ratio。
|
||||
/// </summary>
|
||||
/// <param name="sample">待写出的 J519 实发采样点。</param>
|
||||
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
|
||||
@@ -99,6 +136,40 @@ public static class J519SendTrajectorySampler
|
||||
];
|
||||
}
|
||||
|
||||
/// <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>
|
||||
@@ -232,6 +303,45 @@ public static class J519SendTrajectorySampler
|
||||
}
|
||||
}
|
||||
|
||||
/// <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>
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
134
src/Flyshot.Core.Planning/Sampling/TriggerSampleBinder.cs
Normal file
134
src/Flyshot.Core.Planning/Sampling/TriggerSampleBinder.cs
Normal 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);
|
||||
@@ -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)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
15
src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj
Normal file
15
src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj
Normal 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>
|
||||
77
src/Flyshot.Native.Planning/NativeIcspInteropModels.cs
Normal file
77
src/Flyshot.Native.Planning/NativeIcspInteropModels.cs
Normal 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;
|
||||
}
|
||||
130
src/Flyshot.Native.Planning/NativeIcspMethods.cs
Normal file
130
src/Flyshot.Native.Planning/NativeIcspMethods.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
24
src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs
Normal file
24
src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs
Normal 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);
|
||||
}
|
||||
37
src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs
Normal file
37
src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
24
src/Flyshot.Native.Planning/NativePlanningException.cs
Normal file
24
src/Flyshot.Native.Planning/NativePlanningException.cs
Normal 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; }
|
||||
}
|
||||
104
src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs
Normal file
104
src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
220
src/Flyshot.Native.Planning/RustIcspPlanner.cs
Normal file
220
src/Flyshot.Native.Planning/RustIcspPlanner.cs
Normal 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";
|
||||
}
|
||||
}
|
||||
43
src/Flyshot.Native.Planning/RustIcspPlanningBackend.cs
Normal file
43
src/Flyshot.Native.Planning/RustIcspPlanningBackend.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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.");
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
|
||||
13
src/Flyshot.Server.Host/.config/dotnet-tools.json
Normal file
13
src/Flyshot.Server.Host/.config/dotnet-tools.json
Normal file
@@ -0,0 +1,13 @@
|
||||
{
|
||||
"version": 1,
|
||||
"isRoot": true,
|
||||
"tools": {
|
||||
"dotnet-ef": {
|
||||
"version": "10.0.7",
|
||||
"commands": [
|
||||
"dotnet-ef"
|
||||
],
|
||||
"rollForward": false
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
|
||||
@@ -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
|
||||
});
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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))
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -25,5 +25,12 @@
|
||||
"AllowedHeaders": [
|
||||
"*"
|
||||
]
|
||||
},
|
||||
"Kestrel": {
|
||||
"Endpoints": {
|
||||
"Http": {
|
||||
"Url": "http://0.0.0.0:5190"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
Reference in New Issue
Block a user