From 4eeaa3fef37db007ca7cf157aeeced68da22b8ea Mon Sep 17 00:00:00 2001 From: "yunxiao.zhu" Date: Thu, 23 Apr 2026 17:35:37 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat:=20=E5=88=9D=E5=A7=8B=E5=8C=96?= =?UTF-8?q?=E9=A3=9E=E6=8B=8D=E6=9B=BF=E6=8D=A2=E6=96=B9=E6=A1=88=E4=BB=93?= =?UTF-8?q?=E5=BA=93=E9=AA=A8=E6=9E=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 建立 .NET 8 解决方案及分层项目结构 * 添加 Flyshot.Core.Domain 领域模型（机器人、轨迹、运动学） * 添加 Flyshot.Core.Planning 规划层（ICSP、CubicSpline、采样器） * 添加 Flyshot.Core.Triggering 触发时序与 IO 时间轴 * 添加 Flyshot.Core.Config 配置兼容与 .robot 解析 * 添加 Flyshot.Server.Host 最小宿主及 /healthz 端点 * 补充单元测试与集成测试项目 * 添加 CLAUDE.md、AGENTS.md、README.md 项目规范 --- .gitignore | 398 ++++++++++++++++++ AGENTS.md | 150 +++++++ CLAUDE.md | 150 +++++++ Directory.Build.props | 8 + FlyshotReplacement.sln | 146 +++++++ README.md | 24 ++ .../Flyshot.Core.Config.csproj | 13 + src/Flyshot.Core.Config/PathCompatibility.cs | 137 ++++++ src/Flyshot.Core.Config/RobotConfigLoader.cs | 278 ++++++++++++ src/Flyshot.Core.Config/RobotModelLoader.cs | 216 ++++++++++ .../ControllerStateSnapshot.cs | 95 +++++ .../Flyshot.Core.Domain.csproj | 9 + src/Flyshot.Core.Domain/FlyshotProgram.cs | 175 ++++++++ .../RobotKinematicsModel.cs | 172 ++++++++ src/Flyshot.Core.Domain/RobotProfile.cs | 226 ++++++++++ src/Flyshot.Core.Domain/RuntimeAlarm.cs | 88 ++++ src/Flyshot.Core.Domain/ShotEvent.cs | 71 ++++ src/Flyshot.Core.Domain/TrajectoryRequest.cs | 86 ++++ src/Flyshot.Core.Domain/TrajectoryResult.cs | 272 ++++++++++++ .../CubicSplineInterpolator.cs | 298 +++++++++++++ .../Export/TrajectoryExporter.cs | 66 +++ .../Flyshot.Core.Planning.csproj | 13 + src/Flyshot.Core.Planning/ICspPlanner.cs | 190 +++++++++ src/Flyshot.Core.Planning/Kinematics/Mat4.cs | 83 ++++ .../Kinematics/RobotKinematics.cs | 214 ++++++++++ .../PlannedTrajectory.cs | 130 ++++++ .../Sampling/TrajectorySampler.cs | 106 +++++ .../SelfAdaptIcspPlanner.cs | 182 ++++++++ .../Flyshot.Core.Triggering.csproj | 14 + src/Flyshot.Core.Triggering/ShotTimeline.cs | 31 ++ .../ShotTimelineBuilder.cs | 82 ++++ .../WaypointTimestampResolver.cs | 78 ++++ .../Flyshot.Runtime.Common.csproj | 9 + .../Flyshot.Server.Host.csproj | 7 + src/Flyshot.Server.Host/Program.cs | 13 + .../Properties/launchSettings.json | 38 ++ .../appsettings.Development.json | 8 + src/Flyshot.Server.Host/appsettings.json | 9 + .../ConfigCompatibilityTests.cs | 176 ++++++++ tests/Flyshot.Core.Tests/DomainModelTests.cs | 207 +++++++++ .../Flyshot.Core.Tests.csproj | 32 ++ tests/Flyshot.Core.Tests/GlobalUsings.cs | 1 + tests/Flyshot.Core.Tests/OfflinePlanTests.cs | 154 +++++++ .../PlanningCompatibilityTests.cs | 227 ++++++++++ .../Flyshot.Server.IntegrationTests.csproj | 30 ++ .../GlobalUsings.cs | 1 + .../HealthEndpointTests.cs | 27 ++ 47 files changed, 5140 insertions(+) create mode 100644 .gitignore create mode 100644 AGENTS.md create mode 100644 CLAUDE.md create mode 100644 Directory.Build.props create mode 100644 FlyshotReplacement.sln create mode 100644 README.md create mode 100644 src/Flyshot.Core.Config/Flyshot.Core.Config.csproj create mode 100644 src/Flyshot.Core.Config/PathCompatibility.cs create mode 100644 src/Flyshot.Core.Config/RobotConfigLoader.cs create mode 100644 src/Flyshot.Core.Config/RobotModelLoader.cs create mode 100644 src/Flyshot.Core.Domain/ControllerStateSnapshot.cs create mode 100644 src/Flyshot.Core.Domain/Flyshot.Core.Domain.csproj create mode 100644 src/Flyshot.Core.Domain/FlyshotProgram.cs create mode 100644 src/Flyshot.Core.Domain/RobotKinematicsModel.cs create mode 100644 src/Flyshot.Core.Domain/RobotProfile.cs create mode 100644 src/Flyshot.Core.Domain/RuntimeAlarm.cs create mode 100644 src/Flyshot.Core.Domain/ShotEvent.cs create mode 100644 src/Flyshot.Core.Domain/TrajectoryRequest.cs create mode 100644 src/Flyshot.Core.Domain/TrajectoryResult.cs create mode 100644 src/Flyshot.Core.Planning/CubicSplineInterpolator.cs create mode 100644 src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs create mode 100644 src/Flyshot.Core.Planning/Flyshot.Core.Planning.csproj create mode 100644 src/Flyshot.Core.Planning/ICspPlanner.cs create mode 100644 src/Flyshot.Core.Planning/Kinematics/Mat4.cs create mode 100644 src/Flyshot.Core.Planning/Kinematics/RobotKinematics.cs create mode 100644 src/Flyshot.Core.Planning/PlannedTrajectory.cs create mode 100644 src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs create mode 100644 src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs create mode 100644 src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj create mode 100644 src/Flyshot.Core.Triggering/ShotTimeline.cs create mode 100644 src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs create mode 100644 src/Flyshot.Core.Triggering/WaypointTimestampResolver.cs create mode 100644 src/Flyshot.Runtime.Common/Flyshot.Runtime.Common.csproj create mode 100644 src/Flyshot.Server.Host/Flyshot.Server.Host.csproj create mode 100644 src/Flyshot.Server.Host/Program.cs create mode 100644 src/Flyshot.Server.Host/Properties/launchSettings.json create mode 100644 src/Flyshot.Server.Host/appsettings.Development.json create mode 100644 src/Flyshot.Server.Host/appsettings.json create mode 100644 tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs create mode 100644 tests/Flyshot.Core.Tests/DomainModelTests.cs create mode 100644 tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj create mode 100644 tests/Flyshot.Core.Tests/GlobalUsings.cs create mode 100644 tests/Flyshot.Core.Tests/OfflinePlanTests.cs create mode 100644 tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs create mode 100644 tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj create mode 100644 tests/Flyshot.Server.IntegrationTests/GlobalUsings.cs create mode 100644 tests/Flyshot.Server.IntegrationTests/HealthEndpointTests.cs diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8a30d25 --- /dev/null +++ b/.gitignore @@ -0,0 +1,398 @@ +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +## +## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore + +# User-specific files +*.rsuser +*.suo +*.user +*.userosscache +*.sln.docstates + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Mono auto generated files +mono_crash.* + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +[Ww][Ii][Nn]32/ +[Aa][Rr][Mm]/ +[Aa][Rr][Mm]64/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ +[Ll]ogs/ + +# Visual Studio 2015/2017 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# Visual Studio 2017 auto generated files +Generated\ Files/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUnit +*.VisualState.xml +TestResult.xml +nunit-*.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# Benchmark Results +BenchmarkDotNet.Artifacts/ + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ + +# ASP.NET Scaffolding +ScaffoldingReadMe.txt + +# StyleCop +StyleCopReport.xml + +# Files built by Visual Studio +*_i.c +*_p.c +*_h.h +*.ilk +*.meta +*.obj +*.iobj +*.pch +*.pdb +*.ipdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*_wpftmp.csproj +*.log +*.tlog +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.sdf +*.cachefile +*.VC.db +*.VC.VC.opendb + +# Visual Studio profiler +*.psess +*.vsp +*.vspx +*.sap + +# Visual Studio Trace Files +*.e2e + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# AxoCover is a Code Coverage Tool +.axoCover/* +!.axoCover/settings.json + +# Coverlet is a free, cross platform Code Coverage Tool +coverage*.json +coverage*.xml +coverage*.info + +# Visual Studio code coverage results +*.coverage +*.coveragexml + +# NCrunch +_NCrunch_* +.*crunch*.local.xml +nCrunchTemp_* + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +# Note: Comment the next line if you want to checkin your web deploy settings, +# but database connection strings (with potential passwords) will be unencrypted +*.pubxml +*.publishproj + +# Microsoft Azure Web App publish settings. Comment the next line if you want to +# checkin your Azure Web App publish settings, but sensitive information contained +# in these scripts will be unencrypted +PublishScripts/ + +# NuGet Packages +*.nupkg +# NuGet Symbol Packages +*.snupkg +# The packages folder can be ignored because of Package Restore +**/[Pp]ackages/* +# except build/, which is used as an MSBuild target. +!**/[Pp]ackages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/[Pp]ackages/repositories.config +# NuGet v3's project.json files produces more ignorable files +*.nuget.props +*.nuget.targets + +# Microsoft Azure Build Output +csx/ +*.build.csdef + +# Microsoft Azure Emulator +ecf/ +rcf/ + +# Windows Store app package directories and files +AppPackages/ +BundleArtifacts/ +Package.StoreAssociation.xml +_pkginfo.txt +*.appx +*.appxbundle +*.appxupload + +# Visual Studio cache files +# files ending in .cache can be ignored +*.[Cc]ache +# but keep track of directories ending in .cache +!?*.[Cc]ache/ + +# Others +ClientBin/ +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.jfm +*.pfx +*.publishsettings +orleans.codegen.cs + +# Including strong name files can present a security risk +# (https://github.com/github/gitignore/pull/2483#issue-259490424) +#*.snk + +# Since there are multiple workflows, uncomment next line to ignore bower_components +# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) +#bower_components/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm +ServiceFabricBackup/ +*.rptproj.bak + +# SQL Server files +*.mdf +*.ldf +*.ndf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings +*.rptproj.rsuser +*- [Bb]ackup.rdl +*- [Bb]ackup ([0-9]).rdl +*- [Bb]ackup ([0-9][0-9]).rdl + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Visual Studio 6 build log +*.plg + +# Visual Studio 6 workspace options file +*.opt + +# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) +*.vbw + +# Visual Studio 6 auto-generated project file (contains which files were open etc.) +*.vbp + +# Visual Studio 6 workspace and project file (working project files containing files to include in project) +*.dsw +*.dsp + +# Visual Studio 6 technical files +*.ncb +*.aps + +# Visual Studio LightSwitch build output +**/*.HTMLClient/GeneratedArtifacts +**/*.DesktopClient/GeneratedArtifacts +**/*.DesktopClient/ModelManifest.xml +**/*.Server/GeneratedArtifacts +**/*.Server/ModelManifest.xml +_Pvt_Extensions + +# Paket dependency manager +.paket/paket.exe +paket-files/ + +# FAKE - F# Make +.fake/ + +# CodeRush personal settings +.cr/personal + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Tabs Studio +*.tss + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs + +# OpenCover UI analysis results +OpenCover/ + +# Azure Stream Analytics local run output +ASALocalRun/ + +# MSBuild Binary and Structured Log +*.binlog + +# NVidia Nsight GPU debugger configuration file +*.nvuser + +# MFractors (Xamarin productivity tool) working folder +.mfractor/ + +# Local History for Visual Studio +.localhistory/ + +# Visual Studio History (VSHistory) files +.vshistory/ + +# BeatPulse healthcheck temp database +healthchecksdb + +# Backup folder for Package Reference Convert tool in Visual Studio 2017 +MigrationBackup/ + +# Ionide (cross platform F# VS Code tools) working folder +.ionide/ + +# Fody - auto-generated XML schema +FodyWeavers.xsd + +# VS Code files for those working on multiple tools +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +# Local History for Visual Studio Code +.history/ + +# Windows Installer files from build outputs +*.cab +*.msi +*.msix +*.msm +*.msp + +# JetBrains Rider +*.sln.iml diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..16cbc09 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,150 @@ +# Flyshot Replacement 仓库规范 + +## 1. 仓库目标 + +本仓库用于重写并替代现有 RVBUST/FANUC 飞拍服务端链路。 + +第一版目标固定为： + +- 使用 `C# + .NET 8` +- 提供跨平台独立服务端 +- 兼容现有 `50001/TCP+JSON` 上层接入语义 +- 重写轨迹生成、触发时序、FANUC 控制链路和状态监控 +- Windows / Linux 都能运行完整服务端 +- 只支持当前现场这套组合 + +明确不做： + +- GUI 桌面程序 +- 多机器人同时控制 +- 面向多控制柜的通用平台化框架 + +## 2. 代码与资料边界 + +本仓库是新的独立实现仓库，不复用旧的 RVBUST 二进制作为运行时依赖。 + +允许依赖的内容： + +- 旧仓库中的逆向分析文档 +- 旧仓库中的协议样本、轨迹样本和配置样本 +- 旧仓库中的 `ControllerClient.h` 公开接口 + +不允许把旧服务端实现直接包装成“新系统”。 + +## 3. 当前目录结构 + +```text +flyshot-replacement/ + ├─ src/ + │ ├─ Flyshot.Server.Host/ + │ ├─ Flyshot.Core.Domain/ + │ └─ Flyshot.Runtime.Common/ + ├─ tests/ + │ ├─ Flyshot.Server.IntegrationTests/ + │ └─ Flyshot.Core.Tests/ + ├─ FlyshotReplacement.sln + ├─ Directory.Build.props + ├─ README.md + └─ AGENTS.md +``` + +后续新增模块时，优先保持以下边界： + +- `Flyshot.Core.Domain` + - 纯领域对象 + - 不依赖网络、文件系统、HTTP、UI、操作系统 +- `Flyshot.Core.Config` + - 配置兼容 + - `.robot` 解析 + - 路径兼容 +- `Flyshot.Core.Planning` + - `icsp` + - `self-adapt-icsp` + - `doubles` +- `Flyshot.Core.Triggering` + - `TrajectoryDO` 等价时间轴 + - `shot_flags / offset_values / addr` 解析 +- `Flyshot.LegacyGateway` + - `50001/TCP+JSON` 兼容接入 +- `Flyshot.Runtime.Fanuc` + - `10010 / 10012 / 60015` +- `Flyshot.Web.Status` + - 状态查看 + - 诊断页 + - 不直接处理底层协议细节 + +## 4. 开发规范 + +### 4.1 通用要求 + +- 正式文档默认使用中文。 +- 不要把临时验证脚本直接塞进正式运行时代码。 +- 兼容性优先级高于“重新发明接口”。 +- 第一版默认围绕当前现场组合实现，不提前做泛化设计。 + +### 4.2 实现约束 + +- 旧协议兼容以“语义兼容”为主，不追求二进制逐字节一致。 +- 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。 +- 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。 +- 网页只做状态监控，不把复杂控制流程放进前端。 +- 多机型切换通过不同 `.robot` 文件加载，不在第一版做插件化机型框架。 + +### 4.3 测试要求 + +- 默认采用 TDD。 +- 至少先写失败测试，再写最小实现。 +- 每完成一个阶段，要补最小可运行验证，而不是只看代码编译是否“像是对的”。 + +### 4.4 注释要求 + +- 所有生成代码都必须带中文注释，不可缺漏。 +- 所有类定义都必须在类头提供 XML 注释。 +- 所有静态变量都必须提供 XML 注释。 +- 关键代码块必须补充单行注释，说明该段逻辑为什么存在、在做什么，不允许只写空泛注释。 + +## 5. 构建与验证命令 + +在当前环境中，推荐使用下面两条命令： + +```bash +/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal' +/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal' +``` + +说明： + +- 第一条命令当前已经验证通过，可用于检查最小宿主和 `/healthz`。 +- 第二条命令当前已经验证通过，可用于检查解决方案骨架是否完整。 + +后续新增模块时，继续补充： + +- `Flyshot.Core.Tests` +- 协议回放测试 +- golden sample 对拍测试 +- 端到端集成测试 + +## 6. 修改前优先查看的资料 + +本仓库上层是独立实现，但上下文仍然依赖父目录中的逆向资料。开始重要改动前，优先阅读： + +- `../analysis/ControllerServer_analysis.md` +- `../analysis/ICSP_algorithm_reverse_analysis.md` +- `../analysis/CommonMsg_protocol_analysis.md` +- `../analysis/J519_stream_motion_analysis.md` +- `../analysis/FANUC_realtime_comm_analysis.md` +- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h` + +## 7. 任务推进方式 + +- `README.md` 中的 Todo 需要随着阶段推进同步更新。 +- 如果实现范围发生收敛或扩展，先更新设计或计划，再继续改代码。 +- 如果发现计划漏了关键对象或模块，直接补到文档里，不要把缺口留到后面。 + +## 8. 当前已验证状态 + +- 独立仓库已初始化。 +- `dotnet 8` 解决方案骨架已建立。 +- `Flyshot.Server.Host` 已提供最小 `/healthz`。 +- 最小集成测试已通过。 +- 解决方案构建已通过。 diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..16cbc09 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,150 @@ +# Flyshot Replacement 仓库规范 + +## 1. 仓库目标 + +本仓库用于重写并替代现有 RVBUST/FANUC 飞拍服务端链路。 + +第一版目标固定为： + +- 使用 `C# + .NET 8` +- 提供跨平台独立服务端 +- 兼容现有 `50001/TCP+JSON` 上层接入语义 +- 重写轨迹生成、触发时序、FANUC 控制链路和状态监控 +- Windows / Linux 都能运行完整服务端 +- 只支持当前现场这套组合 + +明确不做： + +- GUI 桌面程序 +- 多机器人同时控制 +- 面向多控制柜的通用平台化框架 + +## 2. 代码与资料边界 + +本仓库是新的独立实现仓库，不复用旧的 RVBUST 二进制作为运行时依赖。 + +允许依赖的内容： + +- 旧仓库中的逆向分析文档 +- 旧仓库中的协议样本、轨迹样本和配置样本 +- 旧仓库中的 `ControllerClient.h` 公开接口 + +不允许把旧服务端实现直接包装成“新系统”。 + +## 3. 当前目录结构 + +```text +flyshot-replacement/ + ├─ src/ + │ ├─ Flyshot.Server.Host/ + │ ├─ Flyshot.Core.Domain/ + │ └─ Flyshot.Runtime.Common/ + ├─ tests/ + │ ├─ Flyshot.Server.IntegrationTests/ + │ └─ Flyshot.Core.Tests/ + ├─ FlyshotReplacement.sln + ├─ Directory.Build.props + ├─ README.md + └─ AGENTS.md +``` + +后续新增模块时，优先保持以下边界： + +- `Flyshot.Core.Domain` + - 纯领域对象 + - 不依赖网络、文件系统、HTTP、UI、操作系统 +- `Flyshot.Core.Config` + - 配置兼容 + - `.robot` 解析 + - 路径兼容 +- `Flyshot.Core.Planning` + - `icsp` + - `self-adapt-icsp` + - `doubles` +- `Flyshot.Core.Triggering` + - `TrajectoryDO` 等价时间轴 + - `shot_flags / offset_values / addr` 解析 +- `Flyshot.LegacyGateway` + - `50001/TCP+JSON` 兼容接入 +- `Flyshot.Runtime.Fanuc` + - `10010 / 10012 / 60015` +- `Flyshot.Web.Status` + - 状态查看 + - 诊断页 + - 不直接处理底层协议细节 + +## 4. 开发规范 + +### 4.1 通用要求 + +- 正式文档默认使用中文。 +- 不要把临时验证脚本直接塞进正式运行时代码。 +- 兼容性优先级高于“重新发明接口”。 +- 第一版默认围绕当前现场组合实现，不提前做泛化设计。 + +### 4.2 实现约束 + +- 旧协议兼容以“语义兼容”为主，不追求二进制逐字节一致。 +- 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。 +- 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。 +- 网页只做状态监控，不把复杂控制流程放进前端。 +- 多机型切换通过不同 `.robot` 文件加载，不在第一版做插件化机型框架。 + +### 4.3 测试要求 + +- 默认采用 TDD。 +- 至少先写失败测试，再写最小实现。 +- 每完成一个阶段，要补最小可运行验证，而不是只看代码编译是否“像是对的”。 + +### 4.4 注释要求 + +- 所有生成代码都必须带中文注释，不可缺漏。 +- 所有类定义都必须在类头提供 XML 注释。 +- 所有静态变量都必须提供 XML 注释。 +- 关键代码块必须补充单行注释，说明该段逻辑为什么存在、在做什么，不允许只写空泛注释。 + +## 5. 构建与验证命令 + +在当前环境中，推荐使用下面两条命令： + +```bash +/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal' +/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal' +``` + +说明： + +- 第一条命令当前已经验证通过，可用于检查最小宿主和 `/healthz`。 +- 第二条命令当前已经验证通过，可用于检查解决方案骨架是否完整。 + +后续新增模块时，继续补充： + +- `Flyshot.Core.Tests` +- 协议回放测试 +- golden sample 对拍测试 +- 端到端集成测试 + +## 6. 修改前优先查看的资料 + +本仓库上层是独立实现，但上下文仍然依赖父目录中的逆向资料。开始重要改动前，优先阅读： + +- `../analysis/ControllerServer_analysis.md` +- `../analysis/ICSP_algorithm_reverse_analysis.md` +- `../analysis/CommonMsg_protocol_analysis.md` +- `../analysis/J519_stream_motion_analysis.md` +- `../analysis/FANUC_realtime_comm_analysis.md` +- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h` + +## 7. 任务推进方式 + +- `README.md` 中的 Todo 需要随着阶段推进同步更新。 +- 如果实现范围发生收敛或扩展，先更新设计或计划，再继续改代码。 +- 如果发现计划漏了关键对象或模块，直接补到文档里，不要把缺口留到后面。 + +## 8. 当前已验证状态 + +- 独立仓库已初始化。 +- `dotnet 8` 解决方案骨架已建立。 +- `Flyshot.Server.Host` 已提供最小 `/healthz`。 +- 最小集成测试已通过。 +- 解决方案构建已通过。 diff --git a/Directory.Build.props b/Directory.Build.props new file mode 100644 index 0000000..65410cf --- /dev/null +++ b/Directory.Build.props @@ -0,0 +1,8 @@ + + + net8.0 + enable + enable + 12.0 + + diff --git a/FlyshotReplacement.sln b/FlyshotReplacement.sln new file mode 100644 index 0000000..0e0216d --- /dev/null +++ b/FlyshotReplacement.sln @@ -0,0 +1,146 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.0.31903.59 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "src", "src", "{64EABE09-B1E0-4476-A213-32C93E46E7C3}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Server.Host", "src\Flyshot.Server.Host\Flyshot.Server.Host.csproj", "{8A744541-C680-41D4-96D7-2E8C2E59D038}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Domain", "src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj", "{85691BA7-BBD1-4A5B-A141-D354200299AF}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Config", "src\Flyshot.Core.Config\Flyshot.Core.Config.csproj", "{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Common", "src\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj", "{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}" +EndProject +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "tests", "tests", "{CB517CF5-2EF6-43A8-B335-ABD3A6FCE3BE}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Server.IntegrationTests", "tests\Flyshot.Server.IntegrationTests\Flyshot.Server.IntegrationTests.csproj", "{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Tests", "tests\Flyshot.Core.Tests\Flyshot.Core.Tests.csproj", "{6CC8418D-2A13-4D70-8F94-585CD71F0B74}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Planning", "src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj", "{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Triggering", "src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj", "{E4DDC34C-9AB6-4050-A927-3DF69804708A}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|Any CPU.Build.0 = Debug|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x64.ActiveCfg = Debug|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x64.Build.0 = Debug|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x86.ActiveCfg = Debug|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x86.Build.0 = Debug|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|Any CPU.ActiveCfg = Release|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|Any CPU.Build.0 = Release|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x64.ActiveCfg = Release|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x64.Build.0 = Release|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x86.ActiveCfg = Release|Any CPU + {8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x86.Build.0 = Release|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|Any CPU.Build.0 = Debug|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x64.ActiveCfg = Debug|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x64.Build.0 = Debug|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x86.ActiveCfg = Debug|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x86.Build.0 = Debug|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|Any CPU.ActiveCfg = Release|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|Any CPU.Build.0 = Release|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x64.ActiveCfg = Release|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x64.Build.0 = Release|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x86.ActiveCfg = Release|Any CPU + {85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x86.Build.0 = Release|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x64.ActiveCfg = Debug|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x64.Build.0 = Debug|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x86.ActiveCfg = Debug|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x86.Build.0 = Debug|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|Any CPU.Build.0 = Release|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x64.ActiveCfg = Release|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x64.Build.0 = Release|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x86.ActiveCfg = Release|Any CPU + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x86.Build.0 = Release|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x64.ActiveCfg = Debug|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x64.Build.0 = Debug|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x86.ActiveCfg = Debug|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x86.Build.0 = Debug|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|Any CPU.Build.0 = Release|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x64.ActiveCfg = Release|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x64.Build.0 = Release|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x86.ActiveCfg = Release|Any CPU + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x86.Build.0 = Release|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|Any CPU.Build.0 = Debug|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x64.ActiveCfg = Debug|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x64.Build.0 = Debug|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x86.ActiveCfg = Debug|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x86.Build.0 = Debug|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|Any CPU.ActiveCfg = Release|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|Any CPU.Build.0 = Release|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x64.ActiveCfg = Release|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x64.Build.0 = Release|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x86.ActiveCfg = Release|Any CPU + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x86.Build.0 = Release|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|Any CPU.Build.0 = Debug|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x64.ActiveCfg = Debug|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x64.Build.0 = Debug|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x86.ActiveCfg = Debug|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x86.Build.0 = Debug|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|Any CPU.ActiveCfg = Release|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|Any CPU.Build.0 = Release|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x64.ActiveCfg = Release|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x64.Build.0 = Release|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x86.ActiveCfg = Release|Any CPU + {6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x86.Build.0 = Release|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|Any CPU.Build.0 = Debug|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x64.ActiveCfg = Debug|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x64.Build.0 = Debug|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x86.ActiveCfg = Debug|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x86.Build.0 = Debug|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|Any CPU.ActiveCfg = Release|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|Any CPU.Build.0 = Release|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x64.ActiveCfg = Release|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x64.Build.0 = Release|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x86.ActiveCfg = Release|Any CPU + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x86.Build.0 = Release|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x64.ActiveCfg = Debug|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x64.Build.0 = Debug|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x86.ActiveCfg = Debug|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x86.Build.0 = Debug|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|Any CPU.Build.0 = Release|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x64.ActiveCfg = Release|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x64.Build.0 = Release|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x86.ActiveCfg = Release|Any CPU + {E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x86.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(NestedProjects) = preSolution + {8A744541-C680-41D4-96D7-2E8C2E59D038} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + {85691BA7-BBD1-4A5B-A141-D354200299AF} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + {D5DB99C2-D58A-49D0-8A7E-5D6D6273E550} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + {B7E1F1B5-96FF-4C60-8AEE-A98D2278452C} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + {7ADC4C1B-53B6-4456-8E6A-81DB38F13E05} = {CB517CF5-2EF6-43A8-B335-ABD3A6FCE3BE} + {6CC8418D-2A13-4D70-8F94-585CD71F0B74} = {CB517CF5-2EF6-43A8-B335-ABD3A6FCE3BE} + {154CA299-80D8-4BE2-B1C9-4BC133FA8B28} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + {E4DDC34C-9AB6-4050-A927-3DF69804708A} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + EndGlobalSection +EndGlobal diff --git a/README.md b/README.md new file mode 100644 index 0000000..98be7c4 --- /dev/null +++ b/README.md @@ -0,0 +1,24 @@ +# Flyshot Replacement + +基于 `.NET 8` 的跨平台飞拍服务端重写项目。 + +当前目标： + +- 兼容现有 `50001/TCP+JSON` 上层接入语义 +- 重写轨迹生成、触发时序和 FANUC 实时控制链路 +- 提供 Web 状态监控页面 +- 在 Windows 和 Linux 上运行完整后台服务 + +说明： + +- 这是长期运行的无头后台服务，不是 GUI 桌面程序。 +- 第一版仅面向当前现场组合，后续再扩展机型与控制柜适配。 + +当前 Todo： + +- [x] 初始化独立仓库 +- [x] 创建 `dotnet 8` 解决方案骨架 +- [x] 打通最小宿主与 `/healthz` +- [x] 建立领域模型与模块边界 +- [x] 落地配置兼容与机器人模型解析 +- [ ] 落地轨迹规划、实时控制和 Web 状态页 diff --git a/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj b/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj new file mode 100644 index 0000000..1ccc45d --- /dev/null +++ b/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj @@ -0,0 +1,13 @@ + + + + net8.0 + enable + enable + + + + + + + diff --git a/src/Flyshot.Core.Config/PathCompatibility.cs b/src/Flyshot.Core.Config/PathCompatibility.cs new file mode 100644 index 0000000..18a95ef --- /dev/null +++ b/src/Flyshot.Core.Config/PathCompatibility.cs @@ -0,0 +1,137 @@ +namespace Flyshot.Core.Config; + +/// +/// 标识需要生成哪一种平台风格的兼容路径。 +/// +public enum CompatibilityPathStyle +{ + /// + /// 使用 Linux/Unix 风格路径。 + /// + Posix, + + /// + /// 使用 Windows 风格路径。 + /// + Windows +} + +/// +/// 提供旧配置与新服务端之间的路径兼容策略。 +/// +public static class PathCompatibility +{ + /// + /// 按旧系统常见目录约定解析配置文件路径。 + /// + /// 调用方传入的原始配置路径。 + /// 当前兼容搜索的仓库根目录。 + /// 命中的绝对配置路径。 + public static string ResolveConfigPath(string configPath, string repoRoot) + { + if (string.IsNullOrWhiteSpace(configPath)) + { + throw new ArgumentException("配置路径不能为空。", nameof(configPath)); + } + + if (string.IsNullOrWhiteSpace(repoRoot)) + { + throw new ArgumentException("仓库根目录不能为空。", nameof(repoRoot)); + } + + var rawPath = configPath.Trim(); + if (Path.IsPathRooted(rawPath)) + { + return File.Exists(rawPath) + ? Path.GetFullPath(rawPath) + : throw new FileNotFoundException($"未找到配置文件: {rawPath}", rawPath); + } + + var normalizedRepoRoot = Path.GetFullPath(repoRoot); + var fileName = Path.GetFileName(rawPath); + var checkedPaths = new List(); + + // 先按最常见的候选路径顺序尝试，保持与旧工具链相近的定位逻辑。 + foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath, fileName)) + { + var fullCandidate = Path.GetFullPath(candidate); + if (checkedPaths.Contains(fullCandidate, StringComparer.OrdinalIgnoreCase)) + { + continue; + } + + checkedPaths.Add(fullCandidate); + if (File.Exists(fullCandidate)) + { + return fullCandidate; + } + } + + // 最后一层兜底按文件名全仓库搜索，但只接受唯一命中，避免同名配置误判。 + var matches = Directory + .EnumerateFiles(normalizedRepoRoot, fileName, SearchOption.AllDirectories) + .Select(Path.GetFullPath) + .Distinct(StringComparer.OrdinalIgnoreCase) + .ToArray(); + + if (matches.Length == 1) + { + return matches[0]; + } + + throw new FileNotFoundException( + $"未找到配置文件 '{configPath}'。已检查: {string.Join(", ", checkedPaths)}", + configPath); + } + + /// + /// 构造当前平台约定的用户数据根目录。 + /// + /// 用户主目录。 + /// 目标平台风格。 + /// 兼容旧系统的用户数据目录。 + public static string BuildUserDataRoot(string homeDirectory, CompatibilityPathStyle pathStyle) + { + if (string.IsNullOrWhiteSpace(homeDirectory)) + { + throw new ArgumentException("用户目录不能为空。", nameof(homeDirectory)); + } + + return pathStyle switch + { + CompatibilityPathStyle.Posix => JoinPosix(homeDirectory, ".Rvbust", "Data"), + CompatibilityPathStyle.Windows => JoinWindows(homeDirectory, ".Rvbust", "Data"), + _ => throw new ArgumentOutOfRangeException(nameof(pathStyle), pathStyle, "不支持的路径风格。") + }; + } + + /// + /// 枚举旧系统中最常见的配置候选路径。 + /// + private static IEnumerable BuildConfigCandidates(string repoRoot, string rawPath, string fileName) + { + yield return Path.Combine(repoRoot, rawPath); + yield return Path.Combine(repoRoot, "Rvbust", "Data", fileName); + yield return Path.Combine(repoRoot, "Rvbust", "Install", "FlyingShot", "Config", fileName); + yield return Path.Combine(repoRoot, "Rvbust", fileName); + yield return Path.Combine(repoRoot, fileName); + } + + /// + /// 使用 Posix 风格拼接路径，便于在 Linux 下验证固定输出。 + /// + private static string JoinPosix(string root, params string[] segments) + { + var trimmedRoot = root.TrimEnd('/'); + return string.Join("/", new[] { trimmedRoot }.Concat(segments)); + } + + /// + /// 使用 Windows 风格拼接路径，避免在 Linux 上测试时被当前平台分隔符污染。 + /// + private static string JoinWindows(string root, params string[] segments) + { + var normalizedRoot = root.TrimEnd('\\', '/').Replace('/', '\\'); + return string.Join("\\", new[] { normalizedRoot }.Concat(segments)); + } +} diff --git a/src/Flyshot.Core.Config/RobotConfigLoader.cs b/src/Flyshot.Core.Config/RobotConfigLoader.cs new file mode 100644 index 0000000..a21906f --- /dev/null +++ b/src/Flyshot.Core.Config/RobotConfigLoader.cs @@ -0,0 +1,278 @@ +using System.Text.Json; +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Config; + +/// +/// 表示从旧版 RobotConfig.json 规范化后的机器人运行参数。 +/// +public sealed class CompatibilityRobotSettings +{ + /// + /// 初始化一份经过规范化的机器人兼容设置。 + /// + public CompatibilityRobotSettings( + bool useDo, + IEnumerable ioAddresses, + int ioKeepCycles, + double accLimitScale, + double jerkLimitScale, + int adaptIcspTryNum) + { + ArgumentNullException.ThrowIfNull(ioAddresses); + + if (ioKeepCycles < 0) + { + throw new ArgumentOutOfRangeException(nameof(ioKeepCycles), "IO 保持周期不能为负数。"); + } + + if (accLimitScale <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。"); + } + + if (jerkLimitScale <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。"); + } + + if (adaptIcspTryNum < 0) + { + throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。"); + } + + // 固化地址数组，避免上层代码在加载后原地篡改配置内容。 + var copiedIoAddresses = ioAddresses.ToArray(); + if (copiedIoAddresses.Any(address => address < 0)) + { + throw new ArgumentException("IO 地址不能为负数。", nameof(ioAddresses)); + } + + UseDo = useDo; + IoAddresses = copiedIoAddresses; + IoKeepCycles = ioKeepCycles; + AccLimitScale = accLimitScale; + JerkLimitScale = jerkLimitScale; + AdaptIcspTryNum = adaptIcspTryNum; + } + + /// + /// 获取是否启用伺服同步 IO。 + /// + public bool UseDo { get; } + + /// + /// 获取默认 IO 地址组。 + /// + public IReadOnlyList IoAddresses { get; } + + /// + /// 获取单次触发保持的伺服周期数。 + /// + public int IoKeepCycles { get; } + + /// + /// 获取加速度全局倍率。 + /// + public double AccLimitScale { get; } + + /// + /// 获取 Jerk 全局倍率。 + /// + public double JerkLimitScale { get; } + + /// + /// 获取自适应补点最大尝试次数。 + /// + public int AdaptIcspTryNum { get; } +} + +/// +/// 表示一次旧版 RobotConfig.json 的完整加载结果。 +/// +public sealed class LoadedRobotConfig +{ + /// + /// 初始化一份规范化后的旧配置文档。 + /// + public LoadedRobotConfig( + string sourcePath, + CompatibilityRobotSettings robot, + IReadOnlyDictionary programs) + { + if (string.IsNullOrWhiteSpace(sourcePath)) + { + throw new ArgumentException("配置源路径不能为空。", nameof(sourcePath)); + } + + SourcePath = sourcePath; + Robot = robot ?? throw new ArgumentNullException(nameof(robot)); + Programs = programs ?? throw new ArgumentNullException(nameof(programs)); + } + + /// + /// 获取实际命中的配置文件路径。 + /// + public string SourcePath { get; } + + /// + /// 获取机器人级兼容设置。 + /// + public CompatibilityRobotSettings Robot { get; } + + /// + /// 获取按名称索引的飞拍程序集合。 + /// + public IReadOnlyDictionary Programs { get; } +} + +/// +/// 负责读取旧版 RobotConfig.json 并转换成领域层可直接消费的结构。 +/// +public sealed class RobotConfigLoader +{ + /// + /// 加载一份旧版 RobotConfig.json。 + /// + /// 调用方传入的配置路径。 + /// 用于兼容搜索的仓库根目录；为空时按当前工作目录推断。 + /// 规范化后的配置文档。 + public LoadedRobotConfig Load(string configPath, string? repoRoot = null) + { + var resolvedRepoRoot = ResolveRepoRoot(repoRoot); + var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot); + + using var document = JsonDocument.Parse(File.ReadAllText(resolvedConfigPath)); + var root = document.RootElement; + var robotElement = root.GetProperty("robot"); + var flyingShotsElement = root.GetProperty("flying_shots"); + + var robot = new CompatibilityRobotSettings( + useDo: ReadBoolean(robotElement, "use_do", defaultValue: false), + ioAddresses: ReadIntArray(robotElement, "io_addr"), + ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0), + 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)); + + var programs = new Dictionary(StringComparer.Ordinal); + foreach (var programElement in flyingShotsElement.EnumerateObject()) + { + var programName = programElement.Name; + var program = ParseProgram(programName, programElement.Value); + programs.Add(programName, program); + } + + return new LoadedRobotConfig( + sourcePath: resolvedConfigPath, + robot: robot, + programs: programs); + } + + /// + /// 把单个 flying_shots 节点转成领域层 FlyshotProgram。 + /// + private static FlyshotProgram ParseProgram(string programName, JsonElement programElement) + { + var waypointRows = programElement.GetProperty("traj_waypoints").EnumerateArray().ToArray(); + var shotFlagElements = programElement.GetProperty("shot_flags").EnumerateArray().ToArray(); + + var waypoints = waypointRows + .Select(row => new JointWaypoint(row.EnumerateArray().Select(static value => value.GetDouble()))) + .ToArray(); + + var shotFlags = shotFlagElements + .Select(static value => value.ValueKind == JsonValueKind.True || (value.ValueKind == JsonValueKind.Number && value.GetInt32() != 0)) + .ToArray(); + + var waypointCount = waypoints.Length; + var offsetValues = programElement.TryGetProperty("offset_values", out var offsetValuesElement) + ? offsetValuesElement.EnumerateArray().Select(static value => value.GetInt32()).ToArray() + : Enumerable.Repeat(0, waypointCount).ToArray(); + + var addressGroups = programElement.TryGetProperty("addr", out var addressElement) + ? addressElement + .EnumerateArray() + .Select(group => new IoAddressGroup(group.EnumerateArray().Select(static value => value.GetInt32()))) + .ToArray() + : Enumerable.Range(0, waypointCount) + .Select(_ => new IoAddressGroup(Array.Empty())) + .ToArray(); + + return new FlyshotProgram( + name: programName, + waypoints: waypoints, + shotFlags: shotFlags, + offsetValues: offsetValues, + addressGroups: addressGroups); + } + + /// + /// 解析布尔或 0/1 风格的兼容字段。 + /// + private static bool ReadBoolean(JsonElement parent, string propertyName, bool defaultValue) + { + if (!parent.TryGetProperty(propertyName, out var property)) + { + return defaultValue; + } + + return property.ValueKind switch + { + JsonValueKind.True => true, + JsonValueKind.False => false, + JsonValueKind.Number => property.GetInt32() != 0, + _ => defaultValue + }; + } + + /// + /// 读取整数数组；字段不存在时返回空数组。 + /// + private static IReadOnlyList ReadIntArray(JsonElement parent, string propertyName) + { + return parent.TryGetProperty(propertyName, out var property) + ? property.EnumerateArray().Select(static value => value.GetInt32()).ToArray() + : Array.Empty(); + } + + /// + /// 读取整数兼容字段。 + /// + private static int ReadInt(JsonElement parent, string propertyName, int defaultValue) + { + return parent.TryGetProperty(propertyName, out var property) ? property.GetInt32() : defaultValue; + } + + /// + /// 读取浮点兼容字段。 + /// + private static double ReadDouble(JsonElement parent, string propertyName, double defaultValue) + { + return parent.TryGetProperty(propertyName, out var property) ? property.GetDouble() : defaultValue; + } + + /// + /// 推断仓库根目录，优先使用调用方显式传入的值。 + /// + private static string ResolveRepoRoot(string? repoRoot) + { + if (!string.IsNullOrWhiteSpace(repoRoot)) + { + return Path.GetFullPath(repoRoot); + } + + var current = new DirectoryInfo(Directory.GetCurrentDirectory()); + while (current is not null) + { + if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln"))) + { + return Path.GetFullPath(Path.Combine(current.FullName, "..")); + } + + current = current.Parent; + } + + return Directory.GetCurrentDirectory(); + } +} diff --git a/src/Flyshot.Core.Config/RobotModelLoader.cs b/src/Flyshot.Core.Config/RobotModelLoader.cs new file mode 100644 index 0000000..06771ad --- /dev/null +++ b/src/Flyshot.Core.Config/RobotModelLoader.cs @@ -0,0 +1,216 @@ +using System.Text; +using System.Text.Json; +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Config; + +/// +/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。 +/// +public sealed class RobotModelLoader +{ + private const uint JsonChunkType = 0x4E4F534A; + + /// + /// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。 + /// + /// .robot 文件路径。 + /// 加速度全局倍率。 + /// Jerk 全局倍率。 + /// 包含关节限制和 couple 信息的 RobotProfile。 + public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0) + { + if (string.IsNullOrWhiteSpace(modelPath)) + { + throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath)); + } + + if (accLimitScale <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。"); + } + + if (jerkLimitScale <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。"); + } + + var resolvedModelPath = Path.GetFullPath(modelPath); + var jsonText = ReadJsonChunk(resolvedModelPath); + using var document = JsonDocument.Parse(jsonText); + + var robotBody = FindRobotBody(document.RootElement); + var profileName = robotBody.TryGetProperty("name", out var nameElement) + ? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath) + : Path.GetFileNameWithoutExtension(resolvedModelPath); + + var jointLimits = new List(); + var jointCouplings = new List(); + + foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray()) + { + if (!IsPlanningJoint(jointElement)) + { + continue; + } + + var jointName = jointElement.GetProperty("name").GetString() ?? throw new InvalidDataException("关节缺少 name。"); + var limitElement = jointElement.GetProperty("limit"); + + jointLimits.Add(new JointLimit( + jointName: jointName, + velocityLimit: limitElement.GetProperty("velocity").GetDouble(), + accelerationLimit: limitElement.GetProperty("acceleration").GetDouble() * accLimitScale, + jerkLimit: limitElement.GetProperty("jerk").GetDouble() * jerkLimitScale)); + + if (jointElement.TryGetProperty("couple", out var coupleElement)) + { + var masterJointName = coupleElement.GetProperty("master_joint").GetString() + ?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。"); + + jointCouplings.Add(new JointCoupling( + slaveJointName: jointName, + masterJointName: masterJointName, + multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0, + offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0)); + } + } + + return new RobotProfile( + name: profileName, + modelPath: resolvedModelPath, + degreesOfFreedom: jointLimits.Count, + jointLimits: jointLimits, + jointCouplings: jointCouplings, + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + } + + /// + /// 从 GLB 文件中提取 JSON chunk 文本。 + /// + private static string ReadJsonChunk(string modelPath) + { + using var stream = File.OpenRead(modelPath); + using var reader = new BinaryReader(stream, Encoding.UTF8, leaveOpen: false); + + var magic = Encoding.ASCII.GetString(reader.ReadBytes(4)); + if (!string.Equals(magic, "glTF", StringComparison.Ordinal)) + { + throw new InvalidDataException($"{modelPath} 不是合法的 GLB 文件。"); + } + + var version = reader.ReadUInt32(); + if (version != 2) + { + throw new NotSupportedException($"当前仅支持 GLB 2.0，实际版本为 {version}。"); + } + + var totalLength = reader.ReadUInt32(); + while (stream.Position < totalLength) + { + var chunkLength = reader.ReadUInt32(); + var chunkType = reader.ReadUInt32(); + var chunkBytes = reader.ReadBytes((int)chunkLength); + if (chunkType == JsonChunkType) + { + return Encoding.UTF8.GetString(chunkBytes); + } + } + + throw new InvalidDataException($"{modelPath} 不包含 JSON chunk。"); + } + + /// + /// 在 robotics.bodies 中找到 type=2 的机器人主体。 + /// + private static JsonElement FindRobotBody(JsonElement root) + { + var bodies = root + .GetProperty("scenes")[0] + .GetProperty("extras") + .GetProperty("rvbust") + .GetProperty("robotics") + .GetProperty("bodies"); + + foreach (var body in bodies.EnumerateArray()) + { + if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2) + { + return body; + } + } + + throw new InvalidDataException("未在 .robot 文件中找到 type=2 的机器人主体。"); + } + + /// + /// 加载 .robot 文件并生成运动学侧需要的完整几何模型。 + /// + /// .robot 文件路径。 + /// 包含完整关节几何链的运动学模型。 + public RobotKinematicsModel LoadKinematicsModel(string modelPath) + { + if (string.IsNullOrWhiteSpace(modelPath)) + { + throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath)); + } + + var resolvedModelPath = Path.GetFullPath(modelPath); + var jsonText = ReadJsonChunk(resolvedModelPath); + using var document = JsonDocument.Parse(jsonText); + + var robotBody = FindRobotBody(document.RootElement); + var profileName = robotBody.TryGetProperty("name", out var nameElement) + ? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath) + : Path.GetFileNameWithoutExtension(resolvedModelPath); + + var joints = new List(); + foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray()) + { + var jointName = jointElement.GetProperty("name").GetString() + ?? throw new InvalidDataException("关节缺少 name。"); + var jointType = jointElement.TryGetProperty("type", out var typeElement) + ? typeElement.GetInt32() + : 0; + var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray(); + var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray(); + // axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。 + var axisVector = axis.Length >= 3 ? axis[^3..] : axis; + var originXyz = origin.Length >= 3 ? origin[..3] : origin; + var originQuat = origin.Length >= 7 ? origin[3..7] : new double[] { 0.0, 0.0, 0.0, 1.0 }; + + string? coupleMaster = null; + double coupleMultiplier = 0.0; + double coupleOffset = 0.0; + if (jointElement.TryGetProperty("couple", out var coupleElement)) + { + coupleMaster = coupleElement.GetProperty("master_joint").GetString(); + coupleMultiplier = coupleElement.TryGetProperty("multiplier", out var m) ? m.GetDouble() : 0.0; + coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0; + } + + joints.Add(new RobotJointGeometry( + name: jointName, + parent: jointElement.GetProperty("parent").GetString() ?? string.Empty, + child: jointElement.GetProperty("child").GetString() ?? string.Empty, + jointType: jointType, + axis: axisVector, + originXyz: originXyz, + originQuatXyzw: originQuat, + coupleMaster: coupleMaster, + coupleMultiplier: coupleMultiplier, + coupleOffset: coupleOffset)); + } + + return new RobotKinematicsModel(name: profileName, joints: joints); + } + + /// + /// 判断当前 joint 是否属于规划侧需要保留的旋转关节。 + /// + private static bool IsPlanningJoint(JsonElement jointElement) + { + return jointElement.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2; + } +} diff --git a/src/Flyshot.Core.Domain/ControllerStateSnapshot.cs b/src/Flyshot.Core.Domain/ControllerStateSnapshot.cs new file mode 100644 index 0000000..d25f8c1 --- /dev/null +++ b/src/Flyshot.Core.Domain/ControllerStateSnapshot.cs @@ -0,0 +1,95 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Represents the current high-level controller state consumed by web monitoring and APIs. +/// +public sealed class ControllerStateSnapshot +{ + /// + /// Initializes a controller state snapshot with safe default collections. + /// + public ControllerStateSnapshot( + DateTimeOffset capturedAt, + string connectionState, + bool isEnabled, + bool isInMotion, + double speedRatio, + IEnumerable? jointPositions = null, + IEnumerable? cartesianPose = null, + IEnumerable? activeAlarms = null) + { + if (string.IsNullOrWhiteSpace(connectionState)) + { + throw new ArgumentException("Connection state is required.", nameof(connectionState)); + } + + if (speedRatio < 0.0) + { + throw new ArgumentOutOfRangeException(nameof(speedRatio), "Speed ratio must be zero or positive."); + } + + // Default to empty immutable snapshots so disconnected states still serialize safely. + var copiedJointPositions = jointPositions?.ToArray() ?? Array.Empty(); + var copiedCartesianPose = cartesianPose?.ToArray() ?? Array.Empty(); + var copiedActiveAlarms = activeAlarms?.ToArray() ?? Array.Empty(); + + CapturedAt = capturedAt; + ConnectionState = connectionState; + IsEnabled = isEnabled; + IsInMotion = isInMotion; + SpeedRatio = speedRatio; + JointPositions = copiedJointPositions; + CartesianPose = copiedCartesianPose; + ActiveAlarms = copiedActiveAlarms; + } + + /// + /// Gets the capture time of the snapshot. + /// + [JsonPropertyName("capturedAt")] + public DateTimeOffset CapturedAt { get; } + + /// + /// Gets the high-level connection state label. + /// + [JsonPropertyName("connectionState")] + public string ConnectionState { get; } + + /// + /// Gets a value indicating whether the robot is enabled. + /// + [JsonPropertyName("isEnabled")] + public bool IsEnabled { get; } + + /// + /// Gets a value indicating whether the robot is currently moving. + /// + [JsonPropertyName("isInMotion")] + public bool IsInMotion { get; } + + /// + /// Gets the effective speed ratio reported by the controller. + /// + [JsonPropertyName("speedRatio")] + public double SpeedRatio { get; } + + /// + /// Gets the current joint positions when available. + /// + [JsonPropertyName("jointPositions")] + public IReadOnlyList JointPositions { get; } + + /// + /// Gets the current Cartesian pose when available. + /// + [JsonPropertyName("cartesianPose")] + public IReadOnlyList CartesianPose { get; } + + /// + /// Gets the active alarms visible at capture time. + /// + [JsonPropertyName("activeAlarms")] + public IReadOnlyList ActiveAlarms { get; } +} diff --git a/src/Flyshot.Core.Domain/Flyshot.Core.Domain.csproj b/src/Flyshot.Core.Domain/Flyshot.Core.Domain.csproj new file mode 100644 index 0000000..fa71b7a --- /dev/null +++ b/src/Flyshot.Core.Domain/Flyshot.Core.Domain.csproj @@ -0,0 +1,9 @@ + + + + net8.0 + enable + enable + + + diff --git a/src/Flyshot.Core.Domain/FlyshotProgram.cs b/src/Flyshot.Core.Domain/FlyshotProgram.cs new file mode 100644 index 0000000..c6cb428 --- /dev/null +++ b/src/Flyshot.Core.Domain/FlyshotProgram.cs @@ -0,0 +1,175 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Represents an uploaded flyshot program built from teach points and shot metadata. +/// +public sealed class FlyshotProgram +{ + /// + /// Initializes a validated flyshot program. + /// + public FlyshotProgram( + string name, + IEnumerable waypoints, + IEnumerable shotFlags, + IEnumerable offsetValues, + IEnumerable addressGroups) + { + if (string.IsNullOrWhiteSpace(name)) + { + throw new ArgumentException("Program name is required.", nameof(name)); + } + + ArgumentNullException.ThrowIfNull(waypoints); + ArgumentNullException.ThrowIfNull(shotFlags); + ArgumentNullException.ThrowIfNull(offsetValues); + ArgumentNullException.ThrowIfNull(addressGroups); + + // Freeze the uploaded data so later orchestration cannot silently drift from the request payload. + var copiedWaypoints = waypoints.ToArray(); + var copiedShotFlags = shotFlags.ToArray(); + var copiedOffsetValues = offsetValues.ToArray(); + var copiedAddressGroups = addressGroups.ToArray(); + + if (copiedWaypoints.Length == 0) + { + throw new ArgumentException("At least one waypoint is required.", nameof(waypoints)); + } + + if (copiedShotFlags.Length != copiedWaypoints.Length) + { + throw new ArgumentException("Shot flag count must match waypoint count.", nameof(shotFlags)); + } + + if (copiedOffsetValues.Length != copiedWaypoints.Length) + { + throw new ArgumentException("Offset value count must match waypoint count.", nameof(offsetValues)); + } + + if (copiedAddressGroups.Length != copiedWaypoints.Length) + { + throw new ArgumentException("Address group count must match waypoint count.", nameof(addressGroups)); + } + + var jointDimension = copiedWaypoints[0].Positions.Count; + if (copiedWaypoints.Any(waypoint => waypoint.Positions.Count != jointDimension)) + { + throw new ArgumentException("All waypoints must share the same joint dimension.", nameof(waypoints)); + } + + Name = name; + Waypoints = copiedWaypoints; + ShotFlags = copiedShotFlags; + OffsetValues = copiedOffsetValues; + AddressGroups = copiedAddressGroups; + JointDimension = jointDimension; + ShotWaypointCount = copiedShotFlags.Count(flag => flag); + } + + /// + /// Gets the program name used by cache and gateway lookups. + /// + [JsonPropertyName("name")] + public string Name { get; } + + /// + /// Gets the immutable teach waypoint list. + /// + [JsonPropertyName("waypoints")] + public IReadOnlyList Waypoints { get; } + + /// + /// Gets the per-waypoint shot trigger flags. + /// + [JsonPropertyName("shotFlags")] + public IReadOnlyList ShotFlags { get; } + + /// + /// Gets the per-waypoint trigger offset values in servo cycles. + /// + [JsonPropertyName("offsetValues")] + public IReadOnlyList OffsetValues { get; } + + /// + /// Gets the per-waypoint IO address groups. + /// + [JsonPropertyName("addressGroups")] + public IReadOnlyList AddressGroups { get; } + + /// + /// Gets the joint dimension shared by all waypoints. + /// + [JsonPropertyName("jointDimension")] + public int JointDimension { get; } + + /// + /// Gets the number of waypoints that request a shot trigger. + /// + [JsonPropertyName("shotWaypointCount")] + public int ShotWaypointCount { get; } +} + +/// +/// Represents a single teach waypoint in joint space. +/// +public sealed class JointWaypoint +{ + /// + /// Initializes a validated joint waypoint. + /// + public JointWaypoint(IEnumerable positions) + { + ArgumentNullException.ThrowIfNull(positions); + + // Copy the input once so the waypoint remains immutable after upload. + var copiedPositions = positions.ToArray(); + if (copiedPositions.Length == 0) + { + throw new ArgumentException("Joint waypoint must contain at least one joint value.", nameof(positions)); + } + + Positions = copiedPositions; + } + + /// + /// Gets the immutable joint value vector. + /// + [JsonPropertyName("positions")] + public IReadOnlyList Positions { get; } +} + +/// +/// Represents the list of IO addresses that should fire at one logical trigger point. +/// +public sealed class IoAddressGroup +{ + /// + /// Initializes a validated IO address group. + /// + public IoAddressGroup(IEnumerable addresses) + { + ArgumentNullException.ThrowIfNull(addresses); + + // Preserve address order because some upper-layer tooling expects exported order stability. + var copiedAddresses = addresses.ToArray(); + if (copiedAddresses.Length > 8) + { + throw new ArgumentException("A single trigger point can contain at most eight IO addresses.", nameof(addresses)); + } + + if (copiedAddresses.Any(address => address < 0)) + { + throw new ArgumentException("IO addresses must be zero or positive.", nameof(addresses)); + } + + Addresses = copiedAddresses; + } + + /// + /// Gets the immutable ordered IO address list. + /// + [JsonPropertyName("addresses")] + public IReadOnlyList Addresses { get; } +} diff --git a/src/Flyshot.Core.Domain/RobotKinematicsModel.cs b/src/Flyshot.Core.Domain/RobotKinematicsModel.cs new file mode 100644 index 0000000..c06c7f4 --- /dev/null +++ b/src/Flyshot.Core.Domain/RobotKinematicsModel.cs @@ -0,0 +1,172 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// 描述机器人运动学链所需的完整关节几何信息，从 .robot GLB 中提取。 +/// +/// 为什么与 RobotProfile 分开？ +/// --- +/// RobotProfile 只存规划侧需要的限速和 couple 元数据，是"规划约束视图"。 +/// RobotKinematicsModel 存的是几何链（origin、axis、变换顺序），是"运动学视图"。 +/// 两者生命周期和用途不同，分开可以避免规划层被迫依赖完整几何数据。 +/// +public sealed class RobotKinematicsModel +{ + /// + /// 初始化一份已验证的运动学模型。 + /// + public RobotKinematicsModel(string name, IEnumerable joints) + { + if (string.IsNullOrWhiteSpace(name)) + { + throw new ArgumentException("机器人名称不能为空。", nameof(name)); + } + + ArgumentNullException.ThrowIfNull(joints); + + var copiedJoints = joints.ToArray(); + if (copiedJoints.Length == 0) + { + throw new ArgumentException("关节列表不能为空。", nameof(joints)); + } + + Name = name; + Joints = copiedJoints; + } + + /// + /// 获取机器人名称。 + /// + [JsonPropertyName("name")] + public string Name { get; } + + /// + /// 获取按运动学链顺序排列的关节几何列表。 + /// + [JsonPropertyName("joints")] + public IReadOnlyList Joints { get; } +} + +/// +/// 描述单个关节的几何属性，用于正运动学计算。 +/// +public sealed class RobotJointGeometry +{ + /// + /// 初始化一份已验证的关节几何描述。 + /// + public RobotJointGeometry( + string name, + string parent, + string child, + int jointType, + double[] axis, + double[] originXyz, + double[] originQuatXyzw, + string? coupleMaster = null, + double coupleMultiplier = 0.0, + double coupleOffset = 0.0) + { + if (string.IsNullOrWhiteSpace(name)) + { + throw new ArgumentException("关节名称不能为空。", nameof(name)); + } + + if (string.IsNullOrWhiteSpace(parent)) + { + throw new ArgumentException("父节点名称不能为空。", nameof(parent)); + } + + if (string.IsNullOrWhiteSpace(child)) + { + throw new ArgumentException("子节点名称不能为空。", nameof(child)); + } + + if (axis is null || axis.Length != 3) + { + throw new ArgumentException("关节轴必须是长度为 3 的数组。", nameof(axis)); + } + + if (originXyz is null || originXyz.Length != 3) + { + throw new ArgumentException("原点平移必须是长度为 3 的数组。", nameof(originXyz)); + } + + if (originQuatXyzw is null || originQuatXyzw.Length != 4) + { + throw new ArgumentException("原点旋转四元数必须是长度为 4 的数组（xyzw）。", nameof(originQuatXyzw)); + } + + Name = name; + Parent = parent; + Child = child; + JointType = jointType; + Axis = (double[])axis.Clone(); + OriginXyz = (double[])originXyz.Clone(); + OriginQuatXyzw = (double[])originQuatXyzw.Clone(); + CoupleMaster = coupleMaster; + CoupleMultiplier = coupleMultiplier; + CoupleOffset = coupleOffset; + } + + /// + /// 获取关节名称。 + /// + [JsonPropertyName("name")] + public string Name { get; } + + /// + /// 获取父连杆名称。 + /// + [JsonPropertyName("parent")] + public string Parent { get; } + + /// + /// 获取子连杆名称。 + /// + [JsonPropertyName("child")] + public string Child { get; } + + /// + /// 获取关节类型：0=fixed, 1=prismatic, 2=revolute。 + /// + [JsonPropertyName("jointType")] + public int JointType { get; } + + /// + /// 获取关节旋转轴（单位向量）。 + /// + [JsonPropertyName("axis")] + public double[] Axis { get; } + + /// + /// 获取关节原点平移 [x, y, z]。 + /// + [JsonPropertyName("originXyz")] + public double[] OriginXyz { get; } + + /// + /// 获取关节原点旋转四元数 [x, y, z, w]。 + /// + [JsonPropertyName("originQuatXyzw")] + public double[] OriginQuatXyzw { get; } + + /// + /// 获取耦合主关节名称（如无则为 null）。 + /// + [JsonPropertyName("coupleMaster")] + public string? CoupleMaster { get; } + + /// + /// 获取耦合乘数。 + /// + [JsonPropertyName("coupleMultiplier")] + public double CoupleMultiplier { get; } + + /// + /// 获取耦合偏移。 + /// + [JsonPropertyName("coupleOffset")] + public double CoupleOffset { get; } +} diff --git a/src/Flyshot.Core.Domain/RobotProfile.cs b/src/Flyshot.Core.Domain/RobotProfile.cs new file mode 100644 index 0000000..05f92b8 --- /dev/null +++ b/src/Flyshot.Core.Domain/RobotProfile.cs @@ -0,0 +1,226 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Describes the robot model contract consumed by planning and runtime orchestration. +/// +public sealed class RobotProfile +{ + /// + /// Initializes a new robot profile with validated joint limits and coupling metadata. + /// + public RobotProfile( + string name, + string modelPath, + int degreesOfFreedom, + IEnumerable jointLimits, + IEnumerable jointCouplings, + TimeSpan servoPeriod, + TimeSpan triggerPeriod) + { + if (string.IsNullOrWhiteSpace(name)) + { + throw new ArgumentException("Robot profile name is required.", nameof(name)); + } + + if (string.IsNullOrWhiteSpace(modelPath)) + { + throw new ArgumentException("Robot profile model path is required.", nameof(modelPath)); + } + + if (degreesOfFreedom <= 0) + { + throw new ArgumentOutOfRangeException(nameof(degreesOfFreedom), "Degrees of freedom must be positive."); + } + + if (servoPeriod <= TimeSpan.Zero) + { + throw new ArgumentOutOfRangeException(nameof(servoPeriod), "Servo period must be positive."); + } + + if (triggerPeriod <= TimeSpan.Zero) + { + throw new ArgumentOutOfRangeException(nameof(triggerPeriod), "Trigger period must be positive."); + } + + ArgumentNullException.ThrowIfNull(jointLimits); + ArgumentNullException.ThrowIfNull(jointCouplings); + + // Snapshot the collections once so downstream layers cannot mutate domain state in place. + var copiedJointLimits = jointLimits.ToArray(); + var copiedJointCouplings = jointCouplings.ToArray(); + + if (copiedJointLimits.Length != degreesOfFreedom) + { + throw new ArgumentException("Joint limit count must match degrees of freedom.", nameof(jointLimits)); + } + + Name = name; + ModelPath = modelPath; + DegreesOfFreedom = degreesOfFreedom; + JointLimits = copiedJointLimits; + JointCouplings = copiedJointCouplings; + ServoPeriod = servoPeriod; + TriggerPeriod = triggerPeriod; + } + + /// + /// Gets the robot profile name exposed to the rest of the runtime. + /// + [JsonPropertyName("name")] + public string Name { get; } + + /// + /// Gets the source path of the robot model file. + /// + [JsonPropertyName("modelPath")] + public string ModelPath { get; } + + /// + /// Gets the active revolute degree-of-freedom count. + /// + [JsonPropertyName("degreesOfFreedom")] + public int DegreesOfFreedom { get; } + + /// + /// Gets the validated per-joint kinematic limits. + /// + [JsonPropertyName("jointLimits")] + public IReadOnlyList JointLimits { get; } + + /// + /// Gets optional joint coupling metadata parsed from the robot model. + /// + [JsonPropertyName("jointCouplings")] + public IReadOnlyList JointCouplings { get; } + + /// + /// Gets the servo scheduling period used by the runtime. + /// + [JsonPropertyName("servoPeriod")] + public TimeSpan ServoPeriod { get; } + + /// + /// Gets the trigger scheduling period used by shot-event alignment. + /// + [JsonPropertyName("triggerPeriod")] + public TimeSpan TriggerPeriod { get; } +} + +/// +/// Describes a single revolute joint limit set required by the planners. +/// +public sealed class JointLimit +{ + /// + /// Initializes a validated joint limit record. + /// + public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit) + { + if (string.IsNullOrWhiteSpace(jointName)) + { + throw new ArgumentException("Joint name is required.", nameof(jointName)); + } + + if (velocityLimit <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(velocityLimit), "Velocity limit must be positive."); + } + + if (accelerationLimit <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(accelerationLimit), "Acceleration limit must be positive."); + } + + if (jerkLimit <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(jerkLimit), "Jerk limit must be positive."); + } + + JointName = jointName; + VelocityLimit = velocityLimit; + AccelerationLimit = accelerationLimit; + JerkLimit = jerkLimit; + } + + /// + /// Gets the joint name associated with the limits. + /// + [JsonPropertyName("jointName")] + public string JointName { get; } + + /// + /// Gets the velocity limit in joint space units. + /// + [JsonPropertyName("velocityLimit")] + public double VelocityLimit { get; } + + /// + /// Gets the acceleration limit in joint space units. + /// + [JsonPropertyName("accelerationLimit")] + public double AccelerationLimit { get; } + + /// + /// Gets the jerk limit in joint space units. + /// + [JsonPropertyName("jerkLimit")] + public double JerkLimit { get; } +} + +/// +/// Describes a joint-coupling rule that must be applied before kinematics or planning. +/// +public sealed class JointCoupling +{ + /// + /// Initializes a validated joint-coupling description. + /// + public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset) + { + if (string.IsNullOrWhiteSpace(slaveJointName)) + { + throw new ArgumentException("Slave joint name is required.", nameof(slaveJointName)); + } + + if (string.IsNullOrWhiteSpace(masterJointName)) + { + throw new ArgumentException("Master joint name is required.", nameof(masterJointName)); + } + + if (string.Equals(slaveJointName, masterJointName, StringComparison.Ordinal)) + { + throw new ArgumentException("Slave and master joints must be different."); + } + + SlaveJointName = slaveJointName; + MasterJointName = masterJointName; + Multiplier = multiplier; + Offset = offset; + } + + /// + /// Gets the dependent joint name. + /// + [JsonPropertyName("slaveJointName")] + public string SlaveJointName { get; } + + /// + /// Gets the source joint name. + /// + [JsonPropertyName("masterJointName")] + public string MasterJointName { get; } + + /// + /// Gets the coupling multiplier applied to the master joint angle. + /// + [JsonPropertyName("multiplier")] + public double Multiplier { get; } + + /// + /// Gets the additive offset applied after the multiplier. + /// + [JsonPropertyName("offset")] + public double Offset { get; } +} diff --git a/src/Flyshot.Core.Domain/RuntimeAlarm.cs b/src/Flyshot.Core.Domain/RuntimeAlarm.cs new file mode 100644 index 0000000..c5e1382 --- /dev/null +++ b/src/Flyshot.Core.Domain/RuntimeAlarm.cs @@ -0,0 +1,88 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Represents an alarm surfaced by planning, gateway, or runtime subsystems. +/// +public sealed class RuntimeAlarm +{ + /// + /// Initializes a validated runtime alarm. + /// + public RuntimeAlarm(string code, string message, AlarmSeverity severity, DateTimeOffset raisedAt, bool isAcknowledged = false) + { + if (string.IsNullOrWhiteSpace(code)) + { + throw new ArgumentException("Alarm code is required.", nameof(code)); + } + + if (string.IsNullOrWhiteSpace(message)) + { + throw new ArgumentException("Alarm message is required.", nameof(message)); + } + + Code = code; + Message = message; + Severity = severity; + RaisedAt = raisedAt; + IsAcknowledged = isAcknowledged; + } + + /// + /// Gets the stable alarm code. + /// + [JsonPropertyName("code")] + public string Code { get; } + + /// + /// Gets the human-readable alarm message. + /// + [JsonPropertyName("message")] + public string Message { get; } + + /// + /// Gets the alarm severity level. + /// + [JsonPropertyName("severity")] + public AlarmSeverity Severity { get; } + + /// + /// Gets the time the alarm was raised. + /// + [JsonPropertyName("raisedAt")] + public DateTimeOffset RaisedAt { get; } + + /// + /// Gets a value indicating whether the alarm has been acknowledged. + /// + [JsonPropertyName("isAcknowledged")] + public bool IsAcknowledged { get; } +} + +/// +/// Identifies the severity level of a runtime alarm. +/// +[JsonConverter(typeof(JsonStringEnumConverter))] +public enum AlarmSeverity +{ + /// + /// Informational alarm or notice. + /// + Info, + + /// + /// Warning that does not yet block execution. + /// + Warning, + + /// + /// Error that blocks the current operation. + /// + Error, + + /// + /// Critical fault that requires immediate operator attention. + /// + Critical +} diff --git a/src/Flyshot.Core.Domain/ShotEvent.cs b/src/Flyshot.Core.Domain/ShotEvent.cs new file mode 100644 index 0000000..0b6b833 --- /dev/null +++ b/src/Flyshot.Core.Domain/ShotEvent.cs @@ -0,0 +1,71 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Describes the sampled shot-event mapping that downstream monitoring and reports consume. +/// +public sealed class ShotEvent +{ + /// + /// Initializes a validated shot-event mapping result. + /// + public ShotEvent(int waypointIndex, double triggerTime, int sampleIndex, double sampleTime, IoAddressGroup addressGroup) + { + if (waypointIndex < 0) + { + throw new ArgumentOutOfRangeException(nameof(waypointIndex), "Waypoint index must be zero or positive."); + } + + if (triggerTime < 0.0) + { + throw new ArgumentOutOfRangeException(nameof(triggerTime), "Trigger time must be zero or positive."); + } + + if (sampleIndex < 0) + { + throw new ArgumentOutOfRangeException(nameof(sampleIndex), "Sample index must be zero or positive."); + } + + if (sampleTime < 0.0) + { + throw new ArgumentOutOfRangeException(nameof(sampleTime), "Sample time must be zero or positive."); + } + + WaypointIndex = waypointIndex; + TriggerTime = triggerTime; + SampleIndex = sampleIndex; + SampleTime = sampleTime; + AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup)); + } + + /// + /// Gets the original teach-waypoint index. + /// + [JsonPropertyName("waypointIndex")] + public int WaypointIndex { get; } + + /// + /// Gets the mathematically resolved trigger time. + /// + [JsonPropertyName("triggerTime")] + public double TriggerTime { get; } + + /// + /// Gets the discrete sample index selected by the trigger scheduler. + /// + [JsonPropertyName("sampleIndex")] + public int SampleIndex { get; } + + /// + /// Gets the sampled trigger time used by runtime outputs. + /// + [JsonPropertyName("sampleTime")] + public double SampleTime { get; } + + /// + /// Gets the IO address group associated with the trigger. + /// + [JsonPropertyName("addressGroup")] + public IoAddressGroup AddressGroup { get; } +} diff --git a/src/Flyshot.Core.Domain/TrajectoryRequest.cs b/src/Flyshot.Core.Domain/TrajectoryRequest.cs new file mode 100644 index 0000000..ece4d1e --- /dev/null +++ b/src/Flyshot.Core.Domain/TrajectoryRequest.cs @@ -0,0 +1,86 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Represents a request to plan or execute a flyshot trajectory. +/// +public sealed class TrajectoryRequest +{ + /// + /// Initializes a trajectory request with compatibility-oriented defaults. + /// + public TrajectoryRequest( + RobotProfile robot, + FlyshotProgram program, + PlanningMethod method, + bool moveToStart = false, + bool saveTrajectoryArtifacts = false, + bool useCache = false) + { + Robot = robot ?? throw new ArgumentNullException(nameof(robot)); + Program = program ?? throw new ArgumentNullException(nameof(program)); + Method = method; + MoveToStart = moveToStart; + SaveTrajectoryArtifacts = saveTrajectoryArtifacts; + UseCache = useCache; + } + + /// + /// Gets the robot profile that constrains planning and runtime compatibility. + /// + [JsonPropertyName("robot")] + public RobotProfile Robot { get; } + + /// + /// Gets the uploaded program to be planned or executed. + /// + [JsonPropertyName("program")] + public FlyshotProgram Program { get; } + + /// + /// Gets the selected planning method. + /// + [JsonPropertyName("method")] + public PlanningMethod Method { get; } + + /// + /// Gets a value indicating whether the robot should move to the first waypoint before execution. + /// + [JsonPropertyName("moveToStart")] + public bool MoveToStart { get; } + + /// + /// Gets a value indicating whether trajectory artifacts should be exported. + /// + [JsonPropertyName("saveTrajectoryArtifacts")] + public bool SaveTrajectoryArtifacts { get; } + + /// + /// Gets a value indicating whether cached planning artifacts may be reused. + /// + [JsonPropertyName("useCache")] + public bool UseCache { get; } +} + +/// +/// Identifies the planning mode requested by compatibility APIs. +/// +[JsonConverter(typeof(JsonStringEnumConverter))] +public enum PlanningMethod +{ + /// + /// Uses the cubic-spline ICSP planner. + /// + Icsp, + + /// + /// Uses the adaptive ICSP planner that may insert additional points. + /// + SelfAdaptIcsp, + + /// + /// Uses the Double-S planner. + /// + Doubles +} diff --git a/src/Flyshot.Core.Domain/TrajectoryResult.cs b/src/Flyshot.Core.Domain/TrajectoryResult.cs new file mode 100644 index 0000000..e0ada1c --- /dev/null +++ b/src/Flyshot.Core.Domain/TrajectoryResult.cs @@ -0,0 +1,272 @@ +using System.Text.Json.Serialization; + +namespace Flyshot.Core.Domain; + +/// +/// Represents the stable planning result returned to orchestration, SDKs, and monitoring layers. +/// +public sealed class TrajectoryResult +{ + /// + /// Initializes a validated trajectory result. + /// + public TrajectoryResult( + string programName, + PlanningMethod method, + bool isValid, + TimeSpan duration, + IEnumerable shotEvents, + IEnumerable triggerTimeline, + IEnumerable artifacts, + string? failureReason, + bool usedCache, + int originalWaypointCount, + int plannedWaypointCount) + { + if (string.IsNullOrWhiteSpace(programName)) + { + throw new ArgumentException("Program name is required.", nameof(programName)); + } + + if (duration < TimeSpan.Zero) + { + throw new ArgumentOutOfRangeException(nameof(duration), "Duration must be zero or positive."); + } + + if (originalWaypointCount < 0) + { + throw new ArgumentOutOfRangeException(nameof(originalWaypointCount), "Original waypoint count must be zero or positive."); + } + + if (plannedWaypointCount < originalWaypointCount) + { + throw new ArgumentOutOfRangeException(nameof(plannedWaypointCount), "Planned waypoint count must be greater than or equal to the original waypoint count."); + } + + ArgumentNullException.ThrowIfNull(shotEvents); + ArgumentNullException.ThrowIfNull(triggerTimeline); + ArgumentNullException.ThrowIfNull(artifacts); + + // Materialize once so the result remains stable after the planner hands it off. + var copiedShotEvents = shotEvents.ToArray(); + var copiedTriggerTimeline = triggerTimeline.ToArray(); + var copiedArtifacts = artifacts.ToArray(); + + ProgramName = programName; + Method = method; + IsValid = isValid; + Duration = duration; + ShotEvents = copiedShotEvents; + TriggerTimeline = copiedTriggerTimeline; + Artifacts = copiedArtifacts; + FailureReason = failureReason; + UsedCache = usedCache; + OriginalWaypointCount = originalWaypointCount; + PlannedWaypointCount = plannedWaypointCount; + } + + /// + /// Gets the source program name. + /// + [JsonPropertyName("programName")] + public string ProgramName { get; } + + /// + /// Gets the method that produced the result. + /// + [JsonPropertyName("method")] + public PlanningMethod Method { get; } + + /// + /// Gets a value indicating whether the result can be executed. + /// + [JsonPropertyName("isValid")] + public bool IsValid { get; } + + /// + /// Gets the final trajectory duration. + /// + [JsonPropertyName("duration")] + public TimeSpan Duration { get; } + + /// + /// Gets the sampled shot events exported for monitoring and reports. + /// + [JsonPropertyName("shotEvents")] + public IReadOnlyList ShotEvents { get; } + + /// + /// Gets the trigger timeline that the runtime will inject into servo execution. + /// + [JsonPropertyName("triggerTimeline")] + public IReadOnlyList TriggerTimeline { get; } + + /// + /// Gets the exported trajectory artifacts associated with the result. + /// + [JsonPropertyName("artifacts")] + public IReadOnlyList Artifacts { get; } + + /// + /// Gets the failure reason when the result is invalid. + /// + [JsonPropertyName("failureReason")] + public string? FailureReason { get; } + + /// + /// Gets a value indicating whether the result reused cached planning data. + /// + [JsonPropertyName("usedCache")] + public bool UsedCache { get; } + + /// + /// Gets the teach waypoint count before adaptive point insertion. + /// + [JsonPropertyName("originalWaypointCount")] + public int OriginalWaypointCount { get; } + + /// + /// Gets the final waypoint count after planner preprocessing. + /// + [JsonPropertyName("plannedWaypointCount")] + public int PlannedWaypointCount { get; } +} + +/// +/// Describes a trigger event that the runtime must inject into the servo timeline. +/// +public sealed class TrajectoryDoEvent +{ + /// + /// Initializes a validated runtime trigger event. + /// + public TrajectoryDoEvent(int waypointIndex, double triggerTime, int offsetCycles, int holdCycles, IoAddressGroup addressGroup) + { + if (waypointIndex < 0) + { + throw new ArgumentOutOfRangeException(nameof(waypointIndex), "Waypoint index must be zero or positive."); + } + + if (triggerTime < 0.0) + { + throw new ArgumentOutOfRangeException(nameof(triggerTime), "Trigger time must be zero or positive."); + } + + if (holdCycles < 0) + { + throw new ArgumentOutOfRangeException(nameof(holdCycles), "Hold cycles must be zero or positive."); + } + + WaypointIndex = waypointIndex; + TriggerTime = triggerTime; + OffsetCycles = offsetCycles; + HoldCycles = holdCycles; + AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup)); + } + + /// + /// Gets the original teach-waypoint index that requested the event. + /// + [JsonPropertyName("waypointIndex")] + public int WaypointIndex { get; } + + /// + /// Gets the theoretical trigger time before discretization. + /// + [JsonPropertyName("triggerTime")] + public double TriggerTime { get; } + + /// + /// Gets the configured offset in servo cycles. + /// + [JsonPropertyName("offsetCycles")] + public int OffsetCycles { get; } + + /// + /// Gets the configured hold duration in servo cycles. + /// + [JsonPropertyName("holdCycles")] + public int HoldCycles { get; } + + /// + /// Gets the IO address group to fire. + /// + [JsonPropertyName("addressGroup")] + public IoAddressGroup AddressGroup { get; } +} + +/// +/// Describes an exported artifact generated from a planned trajectory. +/// +public sealed class TrajectoryArtifact +{ + /// + /// Initializes a validated trajectory artifact descriptor. + /// + public TrajectoryArtifact(TrajectoryArtifactKind kind, string logicalName, string relativePath) + { + if (string.IsNullOrWhiteSpace(logicalName)) + { + throw new ArgumentException("Logical artifact name is required.", nameof(logicalName)); + } + + if (string.IsNullOrWhiteSpace(relativePath)) + { + throw new ArgumentException("Relative artifact path is required.", nameof(relativePath)); + } + + Kind = kind; + LogicalName = logicalName; + RelativePath = relativePath; + } + + /// + /// Gets the exported artifact kind. + /// + [JsonPropertyName("kind")] + public TrajectoryArtifactKind Kind { get; } + + /// + /// Gets the logical artifact file name. + /// + [JsonPropertyName("logicalName")] + public string LogicalName { get; } + + /// + /// Gets the artifact path relative to the service output root. + /// + [JsonPropertyName("relativePath")] + public string RelativePath { get; } +} + +/// +/// Identifies the exported trajectory artifact category. +/// +[JsonConverter(typeof(JsonStringEnumConverter))] +public enum TrajectoryArtifactKind +{ + /// + /// Represents sparse joint teach points. + /// + JointTeachTrajectory, + + /// + /// Represents sparse Cartesian teach points. + /// + CartesianTeachTrajectory, + + /// + /// Represents dense joint trajectory samples. + /// + JointDenseTrajectory, + + /// + /// Represents dense Cartesian trajectory samples. + /// + CartesianDenseTrajectory, + + /// + /// Represents the exported shot-event mapping. + /// + ShotEventTimeline +} diff --git a/src/Flyshot.Core.Planning/CubicSplineInterpolator.cs b/src/Flyshot.Core.Planning/CubicSplineInterpolator.cs new file mode 100644 index 0000000..d8dc930 --- /dev/null +++ b/src/Flyshot.Core.Planning/CubicSplineInterpolator.cs @@ -0,0 +1,298 @@ +namespace Flyshot.Core.Planning; + +/// +/// clamps 到零速度边界条件的三次样条插值器，专用于 ICSP 规划核心。 +/// +/// 为什么自己实现而不是用现成库？ +/// --- +/// 逆向分析已经锁定原系统使用 scipy.interpolate.CubicSpline 的 clamped-zero 边界， +/// 且需要精确控制每段系数以便解析求导峰值。第三方库的边界条件语义和系数存储格式 +/// 可能与 scipy 不完全对齐，自己实现可以把数学公式与 Python 参考代码逐行对照， +/// 确保后续 golden-sample 对拍时误差来源只集中在算法本身，而不是插值器差异。 +/// +public sealed class CubicSplineInterpolator +{ + private readonly double[] _times; + private readonly double[][] _values; + private readonly int _n; // 节点数 + private readonly int _nseg; // 段数 = n - 1 + private readonly int _dof; // 自由度（关节数） + + // 每段每个关节的系数：S_i(t) = a*t^3 + b*t^2 + c*t + d，t 相对于段起点归一化到 [0, h] + // 数组维度 [_nseg][_dof] + private readonly double[][] _a; + private readonly double[][] _b; + private readonly double[][] _c; + private readonly double[][] _d; + private readonly double[] _h; // 每段时长 + + /// + /// 用 clamped-zero 边界构造三次样条。 + /// + /// 严格递增的时间节点，长度至少为 2。 + /// 与 times 一一对应的路点值，每个路点是一个 double[dof]。 + /// 输入维度不一致或时间节点非递增时抛出。 + public CubicSplineInterpolator(double[] times, double[][] values) + { + if (times.Length < 2) + { + throw new ArgumentException("时间节点至少需要 2 个。", nameof(times)); + } + + if (values.Length != times.Length) + { + throw new ArgumentException("路点数量必须与时间节点数量一致。", nameof(values)); + } + + _n = times.Length; + _nseg = _n - 1; + _dof = values[0].Length; + _times = new double[_n]; + Array.Copy(times, _times, _n); + _values = new double[_n][]; + for (int i = 0; i < _n; i++) + { + if (values[i].Length != _dof) + { + throw new ArgumentException($"第 {i} 个路点的维度与第一个路点不一致。"); + } + + _values[i] = new double[_dof]; + Array.Copy(values[i], _values[i], _dof); + } + + _h = new double[_nseg]; + for (int i = 0; i < _nseg; i++) + { + _h[i] = _times[i + 1] - _times[i]; + if (_h[i] <= 0.0) + { + throw new ArgumentException($"时间节点必须严格递增，但在索引 {i} 处发现 h={_h[i]}。"); + } + } + + _a = new double[_nseg][]; + _b = new double[_nseg][]; + _c = new double[_nseg][]; + _d = new double[_nseg][]; + for (int seg = 0; seg < _nseg; seg++) + { + _a[seg] = new double[_dof]; + _b[seg] = new double[_dof]; + _c[seg] = new double[_dof]; + _d[seg] = new double[_dof]; + } + + BuildCoefficients(); + } + + /// + /// 获取路点数量。 + /// + public int WaypointCount => _n; + + /// + /// 获取关节自由度。 + /// + public int DegreesOfFreedom => _dof; + + /// + /// 获取时间节点数组的副本。 + /// + public double[] Times + { + get + { + var copy = new double[_n]; + Array.Copy(_times, copy, _n); + return copy; + } + } + + /// + /// 在指定时刻求样条值（位置）。 + /// + public double[] Evaluate(double t) + { + int seg = FindSegment(t); + double local = t - _times[seg]; + double h = _h[seg]; + var result = new double[_dof]; + for (int d = 0; d < _dof; d++) + { + result[d] = _a[seg][d] * local * local * local + + _b[seg][d] * local * local + + _c[seg][d] * local + + _d[seg][d]; + } + + return result; + } + + /// + /// 在指定时刻求指定阶导数（order=1 速度，2 加速度，3 jerk）。 + /// + public double[] EvaluateDerivative(double t, int order) + { + if (order < 1 || order > 3) + { + throw new ArgumentOutOfRangeException(nameof(order), "仅支持 1~3 阶导数。"); + } + + int seg = FindSegment(t); + double local = t - _times[seg]; + var result = new double[_dof]; + for (int d = 0; d < _dof; d++) + { + result[d] = order switch + { + 1 => 3.0 * _a[seg][d] * local * local + 2.0 * _b[seg][d] * local + _c[seg][d], + 2 => 6.0 * _a[seg][d] * local + 2.0 * _b[seg][d], + 3 => 6.0 * _a[seg][d], + _ => 0.0 + }; + } + + return result; + } + + /// + /// 解析计算每段每个关节的 1/2/3 阶导绝对值峰值。 + /// + /// 为什么用解析法而不是逐点采样？ + /// --- + /// 三次样条的一阶导是二次函数、二阶导是线性函数、三阶导是常数，它们的极值点 + /// 可以通过解析公式精确计算。逐点采样会引入"漏检"风险，解析法能保证找到真正的最大值。 + /// + /// + /// 三元组：(max_dq[_nseg, _dof], max_ddq[_nseg, _dof], max_dddq[_nseg, _dof])。 + /// + public (double[,] maxDq, double[,] maxDdq, double[,] maxDddq) SegmentMaxAbsDerivatives() + { + var maxDq = new double[_nseg, _dof]; + var maxDdq = new double[_nseg, _dof]; + var maxDddq = new double[_nseg, _dof]; + + for (int seg = 0; seg < _nseg; seg++) + { + double h = _h[seg]; + for (int d = 0; d < _dof; d++) + { + double aa = _a[seg][d]; + double bb = _b[seg][d]; + double cc = _c[seg][d]; + + // 一阶导是二次函数 3a*t^2 + 2b*t + c，峰值在端点或顶点。 + var candidates = new List(3) + { + Math.Abs(cc), + Math.Abs(3.0 * aa * h * h + 2.0 * bb * h + cc), + }; + + if (Math.Abs(aa) > 1e-12) + { + // 二次函数顶点：导数的导数 = 6a*t + 2b = 0 => t = -b/(3a) + double critical = -bb / (3.0 * aa); + if (critical > 0.0 && critical < h) + { + candidates.Add(Math.Abs(3.0 * aa * critical * critical + 2.0 * bb * critical + cc)); + } + } + + maxDq[seg, d] = candidates.Max(); + + // 二阶导是线性函数 6a*t + 2b，最大值在端点。 + maxDdq[seg, d] = Math.Max(Math.Abs(2.0 * bb), Math.Abs(6.0 * aa * h + 2.0 * bb)); + + // 三阶导是常数 6a。 + maxDddq[seg, d] = Math.Abs(6.0 * aa); + } + } + + return (maxDq, maxDdq, maxDddq); + } + + /// + /// 定位给定时刻所属段索引，处理越界情况（ clamp 到首尾段）。 + /// + private int FindSegment(double t) + { + if (t <= _times[0]) + { + return 0; + } + + if (t >= _times[_n - 1]) + { + return _nseg - 1; + } + + // 线性扫描，因为路点数量通常很小（几十到几百个）。 + for (int i = 0; i < _nseg; i++) + { + if (t >= _times[i] && t < _times[i + 1]) + { + return i; + } + } + + return _nseg - 1; + } + + /// + /// 对每个关节独立建立 clamped-zero 边界的三次样条系数。 + /// + private void BuildCoefficients() + { + for (int d = 0; d < _dof; d++) + { + // 解三对角方程组求二阶导数 M_i。 + // 方程：h_{i-1}*M_{i-1} + 2(h_{i-1}+h_i)*M_i + h_i*M_{i+1} = 6*((y_{i+1}-y_i)/h_i - (y_i-y_{i-1})/h_{i-1}) + // 边界：2h_0*M_0 + h_0*M_1 = 6*((y_1-y_0)/h_0 - y'_0)，y'_0 = 0 + // h_{n-2}*M_{n-2} + 2h_{n-2}*M_{n-1} = 6*(y'_n - (y_n-y_{n-1})/h_{n-2})，y'_n = 0 + var alpha = new double[_n]; + alpha[0] = 6.0 * ((_values[1][d] - _values[0][d]) / _h[0] - 0.0); + alpha[_n - 1] = 6.0 * (0.0 - (_values[_n - 1][d] - _values[_n - 2][d]) / _h[_n - 2]); + for (int i = 1; i < _n - 1; i++) + { + alpha[i] = 6.0 * ((_values[i + 1][d] - _values[i][d]) / _h[i] - (_values[i][d] - _values[i - 1][d]) / _h[i - 1]); + } + + var l = new double[_n]; + var mu = new double[_n]; + var z = new double[_n]; + l[0] = 2.0 * _h[0]; + mu[0] = 0.5; + z[0] = alpha[0] / l[0]; + + for (int i = 1; i < _n - 1; i++) + { + l[i] = 2.0 * (_h[i - 1] + _h[i]) - _h[i - 1] * mu[i - 1]; + mu[i] = _h[i] / l[i]; + z[i] = (alpha[i] - _h[i - 1] * z[i - 1]) / l[i]; + } + + l[_n - 1] = 2.0 * _h[_n - 2] - _h[_n - 2] * mu[_n - 2]; + z[_n - 1] = (alpha[_n - 1] - _h[_n - 2] * z[_n - 2]) / l[_n - 1]; + + var m = new double[_n]; + m[_n - 1] = z[_n - 1]; + for (int j = _n - 2; j >= 0; j--) + { + m[j] = z[j] - mu[j] * m[j + 1]; + } + + // 计算每段系数 + for (int seg = 0; seg < _nseg; seg++) + { + double h = _h[seg]; + double yj = _values[seg][d]; + double yj1 = _values[seg + 1][d]; + _a[seg][d] = (m[seg + 1] - m[seg]) / (6.0 * h); + _b[seg][d] = m[seg] / 2.0; + _c[seg][d] = (yj1 - yj) / h - h * (2.0 * m[seg] + m[seg + 1]) / 6.0; + _d[seg][d] = yj; + } + } + } +} diff --git a/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs b/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs new file mode 100644 index 0000000..19f1a4f --- /dev/null +++ b/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs @@ -0,0 +1,66 @@ +using System.Text; +using System.Text.Json; +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning.Export; + +/// +/// 将规划结果导出为与 Python demo 格式兼容的文本文件。 +/// +/// 为什么用空格分隔而不是 CSV？ +/// --- +/// 空格分隔是最简单的列对齐格式，不需要额外引号处理，也方便用 numpy.loadtxt +/// 或 C++ 的 fscanf 直接读取。真机侧的状态日志通常也是空格分隔的纯文本， +/// 保持一致便于后续做自动化 diff。 +/// +public static class TrajectoryExporter +{ + /// + /// 导出稠密关节轨迹到文本文件。 + /// + public static void WriteJointDenseTrajectory(string path, IReadOnlyList> rows) + { + WriteDenseRows(path, rows); + } + + /// + /// 导出稠密笛卡尔轨迹到文本文件。 + /// + public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList> rows) + { + WriteDenseRows(path, rows); + } + + /// + /// 导出触发事件到 JSON 文件。 + /// + public static void WriteShotEvents(string path, IReadOnlyList events) + { + 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() + }).ToList(); + + var json = JsonSerializer.Serialize(payload, new JsonSerializerOptions + { + WriteIndented = true + }); + + File.WriteAllText(path, json, new UTF8Encoding(false)); + } + + private static void WriteDenseRows(string path, IReadOnlyList> rows) + { + var sb = new StringBuilder(); + foreach (var row in rows) + { + sb.AppendLine(string.Join(" ", row.Select(v => $"{v:F6}"))); + } + + File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false)); + } +} diff --git a/src/Flyshot.Core.Planning/Flyshot.Core.Planning.csproj b/src/Flyshot.Core.Planning/Flyshot.Core.Planning.csproj new file mode 100644 index 0000000..1ccc45d --- /dev/null +++ b/src/Flyshot.Core.Planning/Flyshot.Core.Planning.csproj @@ -0,0 +1,13 @@ + + + + net8.0 + enable + enable + + + + + + + diff --git a/src/Flyshot.Core.Planning/ICspPlanner.cs b/src/Flyshot.Core.Planning/ICspPlanner.cs new file mode 100644 index 0000000..89a3e83 --- /dev/null +++ b/src/Flyshot.Core.Planning/ICspPlanner.cs @@ -0,0 +1,190 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning; + +/// +/// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。 +/// +/// 算法核心逻辑（与逆向文档一致）： +/// 1. 初始段时长取关节空间欧氏距离； +/// 2. 每轮用当前时长构造 CubicSpline，解析求每段 1/2/3 阶导峰值； +/// 3. 按速度一次方、加速度平方根、jerk 立方根缩放时长； +/// 4. 以 sum(|scale_i - 1|) 为收敛指标，保存历史最优结果； +/// 5. 最终用最优时长构造 CubicSpline 并输出时间轴。 +/// +public sealed class ICspPlanner +{ + /// + /// 默认收敛阈值，对应原实现中的 1e-3。 + /// + public const double DefaultThreshold = 1e-3; + + /// + /// 默认最大迭代轮数，对应原实现中的 1000。 + /// + public const int DefaultMaxIterations = 1000; + + /// + /// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。 + /// + public PlannedTrajectory Plan(TrajectoryRequest request) + { + ArgumentNullException.ThrowIfNull(request); + + var waypoints = request.Program.Waypoints; + if (waypoints.Count < 4) + { + throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request)); + } + + var qs = WaypointsToArray(waypoints); + var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot); + + // 初始段时长直接取相邻示教点的关节空间欧氏距离。 + var segmentDurations = ComputeInitialDurations(qs); + int nseg = segmentDurations.Length; + int dof = qs[0].Length; + + double bestThreshold = double.PositiveInfinity; + double[]? bestDurations = null; + double[]? bestScales = null; + CubicSplineInterpolator? bestSpline = null; + int bestIterations = 0; + double[]? bestWaypointTimes = null; + + for (int iteration = 0; iteration <= DefaultMaxIterations; iteration++) + { + var waypointTimes = CumulativeTimes(segmentDurations); + var spline = new CubicSplineInterpolator(waypointTimes, qs); + var (maxDq, maxDdq, maxDddq) = spline.SegmentMaxAbsDerivatives(); + + var scales = new double[nseg]; + for (int seg = 0; seg < nseg; seg++) + { + double segScale = 0.0; + for (int d = 0; d < dof; d++) + { + double sv = Math.Abs(maxDq[seg, d] / velLimits[d]); + double sa = Math.Sqrt(Math.Abs(maxDdq[seg, d] / accLimits[d])); + double sj = Math.Cbrt(Math.Abs(maxDddq[seg, d] / jerkLimits[d])); + segScale = Math.Max(segScale, Math.Max(sv, Math.Max(sa, sj))); + } + + scales[seg] = segScale; + } + + double currentThreshold = 0.0; + for (int seg = 0; seg < nseg; seg++) + { + currentThreshold += Math.Abs(scales[seg] - 1.0); + } + + if (currentThreshold < bestThreshold) + { + bestThreshold = currentThreshold; + bestDurations = (double[])segmentDurations.Clone(); + bestScales = (double[])scales.Clone(); + bestSpline = spline; + bestIterations = iteration + 1; + bestWaypointTimes = (double[])waypointTimes.Clone(); + } + + if (currentThreshold < DefaultThreshold) + { + break; + } + + for (int seg = 0; seg < nseg; seg++) + { + segmentDurations[seg] *= scales[seg]; + } + } + + if (bestSpline is null || bestDurations is null || bestScales is null || bestWaypointTimes is null) + { + throw new InvalidOperationException("ICSP 规划未能产生有效结果。"); + } + + return new PlannedTrajectory( + robot: request.Robot, + originalProgram: request.Program, + plannedWaypoints: waypoints, + waypointTimes: bestWaypointTimes, + segmentDurations: bestDurations, + segmentScales: bestScales, + method: PlanningMethod.Icsp, + iterations: bestIterations, + threshold: bestThreshold); + } + + /// + /// 把领域层路点列表转换成 double[][]，方便数学运算。 + /// + private static double[][] WaypointsToArray(IReadOnlyList waypoints) + { + var result = new double[waypoints.Count][]; + for (int i = 0; i < waypoints.Count; i++) + { + result[i] = waypoints[i].Positions.ToArray(); + } + + return result; + } + + /// + /// 从机器人配置中提取速度/加速度/jerk 限值数组。 + /// + private static (double[] vel, double[] acc, double[] jerk) ExtractLimits(RobotProfile robot) + { + int dof = robot.DegreesOfFreedom; + var vel = new double[dof]; + var acc = new double[dof]; + var jerk = new double[dof]; + for (int d = 0; d < dof; d++) + { + vel[d] = robot.JointLimits[d].VelocityLimit; + acc[d] = robot.JointLimits[d].AccelerationLimit; + jerk[d] = robot.JointLimits[d].JerkLimit; + } + + return (vel, acc, jerk); + } + + /// + /// 计算初始段时长：取相邻路点在关节空间的欧氏距离。 + /// + private static double[] ComputeInitialDurations(double[][] qs) + { + int n = qs.Length; + var durations = new double[n - 1]; + for (int i = 0; i < n - 1; i++) + { + double sumSq = 0.0; + for (int d = 0; d < qs[i].Length; d++) + { + double diff = qs[i + 1][d] - qs[i][d]; + sumSq += diff * diff; + } + + durations[i] = Math.Sqrt(sumSq); + } + + return durations; + } + + /// + /// 由段时长累加得到绝对时间节点（首项为 0）。 + /// + private static double[] CumulativeTimes(double[] segmentDurations) + { + int nseg = segmentDurations.Length; + var times = new double[nseg + 1]; + times[0] = 0.0; + for (int i = 0; i < nseg; i++) + { + times[i + 1] = times[i] + segmentDurations[i]; + } + + return times; + } +} diff --git a/src/Flyshot.Core.Planning/Kinematics/Mat4.cs b/src/Flyshot.Core.Planning/Kinematics/Mat4.cs new file mode 100644 index 0000000..58be8a3 --- /dev/null +++ b/src/Flyshot.Core.Planning/Kinematics/Mat4.cs @@ -0,0 +1,83 @@ +namespace Flyshot.Core.Planning.Kinematics; + +/// +/// 轻量 4x4 矩阵，用于机器人运动学计算，与 numpy 的 4x4 矩阵行为一致（列向量约定）。 +/// +public struct Mat4 +{ + public double M11, M12, M13, M14; + public double M21, M22, M23, M24; + public double M31, M32, M33, M34; + public double M41, M42, M43, M44; + + public static Mat4 Identity => new() + { + M11 = 1.0, M22 = 1.0, M33 = 1.0, M44 = 1.0 + }; + + public static Mat4 operator *(Mat4 a, Mat4 b) + { + return new Mat4 + { + M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31 + a.M14 * b.M41, + M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32 + a.M14 * b.M42, + M13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33 + a.M14 * b.M43, + M14 = a.M11 * b.M14 + a.M12 * b.M24 + a.M13 * b.M34 + a.M14 * b.M44, + + M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31 + a.M24 * b.M41, + M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32 + a.M24 * b.M42, + M23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33 + a.M24 * b.M43, + M24 = a.M21 * b.M14 + a.M22 * b.M24 + a.M23 * b.M34 + a.M24 * b.M44, + + M31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31 + a.M34 * b.M41, + M32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32 + a.M34 * b.M42, + M33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33 + a.M34 * b.M43, + M34 = a.M31 * b.M14 + a.M32 * b.M24 + a.M33 * b.M34 + a.M34 * b.M44, + + M41 = a.M41 * b.M11 + a.M42 * b.M21 + a.M43 * b.M31 + a.M44 * b.M41, + M42 = a.M41 * b.M12 + a.M42 * b.M22 + a.M43 * b.M32 + a.M44 * b.M42, + M43 = a.M41 * b.M13 + a.M42 * b.M23 + a.M43 * b.M33 + a.M44 * b.M43, + M44 = a.M41 * b.M14 + a.M42 * b.M24 + a.M43 * b.M34 + a.M44 * b.M44, + }; + } + + public static double[] operator *(Mat4 m, double[] v) + { + if (v.Length != 4) + throw new ArgumentException("向量长度必须为 4。", nameof(v)); + + return new double[] + { + m.M11 * v[0] + m.M12 * v[1] + m.M13 * v[2] + m.M14 * v[3], + m.M21 * v[0] + m.M22 * v[1] + m.M23 * v[2] + m.M24 * v[3], + m.M31 * v[0] + m.M32 * v[1] + m.M33 * v[2] + m.M34 * v[3], + m.M41 * v[0] + m.M42 * v[1] + m.M43 * v[2] + m.M44 * v[3], + }; + } + + /// + /// 从 3x3 旋转矩阵构造 4x4 齐次旋转矩阵（无平移）。 + /// + public static Mat4 FromRotation(double[,] r) + { + return new Mat4 + { + M11 = r[0, 0], M12 = r[0, 1], M13 = r[0, 2], M14 = 0.0, + M21 = r[1, 0], M22 = r[1, 1], M23 = r[1, 2], M24 = 0.0, + M31 = r[2, 0], M32 = r[2, 1], M33 = r[2, 2], M34 = 0.0, + M41 = 0.0, M42 = 0.0, M43 = 0.0, M44 = 1.0, + }; + } + + /// + /// 构造平移矩阵。 + /// + public static Mat4 FromTranslation(double x, double y, double z) + { + var m = Identity; + m.M14 = x; + m.M24 = y; + m.M34 = z; + return m; + } +} diff --git a/src/Flyshot.Core.Planning/Kinematics/RobotKinematics.cs b/src/Flyshot.Core.Planning/Kinematics/RobotKinematics.cs new file mode 100644 index 0000000..90fa987 --- /dev/null +++ b/src/Flyshot.Core.Planning/Kinematics/RobotKinematics.cs @@ -0,0 +1,214 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning.Kinematics; + +/// +/// 机器人正运动学计算，包含关节耦合解析、四元数/旋转矩阵转换和齐次变换链。 +/// +/// 为什么自己写而不是用 System.Numerics？ +/// --- +/// System.Numerics.Matrix4x4 使用行向量约定（平移在 M41-M43），而 numpy 使用列向量 +/// 约定（平移在 M14-M24-M34）。两者是转置关系，如果混用会导致 FK 结果完全错误。 +/// 自己实现 Mat4 可以确保与 Python 参考代码的矩阵约定完全一致，便于 golden-sample 对拍。 +/// +public static class RobotKinematics +{ + /// + /// 执行正运动学，输出末端位姿 [x, y, z, qx, qy, qz, qw]。 + /// + public static double[] ForwardKinematics(RobotKinematicsModel model, double[] jointPositions) + { + var kinematicAngles = ResolveKinematicAngles(model, jointPositions); + var transform = Mat4.Identity; + int revoluteIndex = 0; + + foreach (var joint in model.Joints) + { + // 先叠加固定偏移 origin，再叠加关节旋转。 + transform = transform * TransformFromPose(joint.OriginXyz, joint.OriginQuatXyzw); + + if (joint.JointType == 2) + { + var rot = AxisAngleToMatrix(joint.Axis, kinematicAngles[revoluteIndex]); + var jointTransform = Mat4.FromRotation(rot); + transform = transform * jointTransform; + revoluteIndex++; + } + } + + var quat = MatrixToQuatXyzw(ExtractRotation(transform)); + return new double[] + { + transform.M14, transform.M24, transform.M34, + quat[0], quat[1], quat[2], quat[3] + }; + } + + /// + /// 解析关节耦合，把控制器关节角转换成运动学关节角。 + /// + /// 公式：q_kin[i] = q_raw[i] + multiplier * q_kin[master] + offset + /// + public static double[] ResolveKinematicAngles(RobotKinematicsModel model, double[] rawPositions) + { + var revoluteJoints = model.Joints.Where(j => j.JointType == 2).ToArray(); + if (rawPositions.Length != revoluteJoints.Length) + { + throw new ArgumentException( + $"关节位置数量 {rawPositions.Length} 与旋转关节数量 {revoluteJoints.Length} 不匹配。"); + } + + var nameToIndex = revoluteJoints + .Select((j, i) => (j.Name, i)) + .ToDictionary(x => x.Name, x => x.i); + + var kinematicValues = new double[rawPositions.Length]; + var resolved = new bool[rawPositions.Length]; + var resolving = new bool[rawPositions.Length]; + + double Resolve(int index) + { + if (resolved[index]) + return kinematicValues[index]; + if (resolving[index]) + throw new InvalidOperationException($"检测到循环关节耦合：{revoluteJoints[index].Name}"); + + resolving[index] = true; + var joint = revoluteJoints[index]; + double value = rawPositions[index]; + + if (!string.IsNullOrEmpty(joint.CoupleMaster)) + { + if (!nameToIndex.TryGetValue(joint.CoupleMaster, out var masterIndex)) + throw new InvalidOperationException($"关节 {joint.Name} 引用了未知的主关节 {joint.CoupleMaster}"); + value += joint.CoupleMultiplier * Resolve(masterIndex) + joint.CoupleOffset; + } + + kinematicValues[index] = value; + resolving[index] = false; + resolved[index] = true; + return value; + } + + for (int i = 0; i < revoluteJoints.Length; i++) + { + Resolve(i); + } + + return kinematicValues; + } + + /// + /// 从 Mat4 中提取 3x3 旋转矩阵。 + /// + private static double[,] ExtractRotation(Mat4 m) + { + return new double[,] + { + { m.M11, m.M12, m.M13 }, + { m.M21, m.M22, m.M23 }, + { m.M31, m.M32, m.M33 }, + }; + } + + /// + /// 把平移 + 四元数拼成 4x4 齐次变换矩阵。 + /// + private static Mat4 TransformFromPose(double[] xyz, double[] quatXyzw) + { + var rot = QuatToMatrix(quatXyzw); + var m = Mat4.Identity; + m.M11 = rot[0, 0]; m.M12 = rot[0, 1]; m.M13 = rot[0, 2]; + m.M21 = rot[1, 0]; m.M22 = rot[1, 1]; m.M23 = rot[1, 2]; + m.M31 = rot[2, 0]; m.M32 = rot[2, 1]; m.M33 = rot[2, 2]; + m.M14 = xyz[0]; + m.M24 = xyz[1]; + m.M34 = xyz[2]; + return m; + } + + /// + /// 四元数（xyzw 顺序）转旋转矩阵。 + /// + private static double[,] QuatToMatrix(double[] q) + { + double x = q[0], y = q[1], z = q[2], w = q[3]; + double xx = x * x, yy = y * y, zz = z * z; + double xy = x * y, xz = x * z, yz = y * z; + double wx = w * x, wy = w * y, wz = w * z; + + return new double[,] + { + { 1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy) }, + { 2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx) }, + { 2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy) }, + }; + } + + /// + /// 旋转矩阵转四元数（xyzw 顺序），使用 Shepperd 分支策略保证数值稳定。 + /// + private static double[] MatrixToQuatXyzw(double[,] r) + { + double trace = r[0, 0] + r[1, 1] + r[2, 2]; + double x, y, z, w; + + if (trace > 0.0) + { + double s = Math.Sqrt(trace + 1.0) * 2.0; + w = 0.25 * s; + x = (r[2, 1] - r[1, 2]) / s; + y = (r[0, 2] - r[2, 0]) / s; + z = (r[1, 0] - r[0, 1]) / s; + } + else if (r[0, 0] > r[1, 1] && r[0, 0] > r[2, 2]) + { + double s = Math.Sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0; + w = (r[2, 1] - r[1, 2]) / s; + x = 0.25 * s; + y = (r[0, 1] + r[1, 0]) / s; + z = (r[0, 2] + r[2, 0]) / s; + } + else if (r[1, 1] > r[2, 2]) + { + double s = Math.Sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0; + w = (r[0, 2] - r[2, 0]) / s; + x = (r[0, 1] + r[1, 0]) / s; + y = 0.25 * s; + z = (r[1, 2] + r[2, 1]) / s; + } + else + { + double s = Math.Sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0; + w = (r[1, 0] - r[0, 1]) / s; + x = (r[0, 2] + r[2, 0]) / s; + y = (r[1, 2] + r[2, 1]) / s; + z = 0.25 * s; + } + + double norm = Math.Sqrt(x * x + y * y + z * z + w * w); + return new double[] { x / norm, y / norm, z / norm, w / norm }; + } + + /// + /// Rodrigues 公式：绕单位向量 axis 旋转 angle 弧度。 + /// + private static double[,] AxisAngleToMatrix(double[] axis, double angle) + { + double norm = Math.Sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]); + if (norm == 0.0) + return new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; + + double ux = axis[0] / norm, uy = axis[1] / norm, uz = axis[2] / norm; + double c = Math.Cos(angle); + double s = Math.Sin(angle); + double oneC = 1.0 - c; + + return new double[,] + { + { c + ux * ux * oneC, ux * uy * oneC - uz * s, ux * uz * oneC + uy * s }, + { uy * ux * oneC + uz * s, c + uy * uy * oneC, uy * uz * oneC - ux * s }, + { uz * ux * oneC - uy * s, uz * uy * oneC + ux * s, c + uz * uz * oneC }, + }; + } +} diff --git a/src/Flyshot.Core.Planning/PlannedTrajectory.cs b/src/Flyshot.Core.Planning/PlannedTrajectory.cs new file mode 100644 index 0000000..8e29119 --- /dev/null +++ b/src/Flyshot.Core.Planning/PlannedTrajectory.cs @@ -0,0 +1,130 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning; + +/// +/// 表示一次规划完成后的轨迹结果，包含原始示教点、补中点后的完整路点、时间轴和收敛信息。 +/// +public sealed class PlannedTrajectory +{ + /// + /// 初始化一份已验证的规划轨迹。 + /// + public PlannedTrajectory( + RobotProfile robot, + FlyshotProgram originalProgram, + IReadOnlyList plannedWaypoints, + IReadOnlyList waypointTimes, + IReadOnlyList segmentDurations, + IReadOnlyList segmentScales, + PlanningMethod method, + int iterations, + double threshold) + { + ArgumentNullException.ThrowIfNull(robot); + ArgumentNullException.ThrowIfNull(originalProgram); + ArgumentNullException.ThrowIfNull(plannedWaypoints); + ArgumentNullException.ThrowIfNull(waypointTimes); + ArgumentNullException.ThrowIfNull(segmentDurations); + ArgumentNullException.ThrowIfNull(segmentScales); + + if (waypointTimes.Count != plannedWaypoints.Count) + { + throw new ArgumentException("路点时间轴长度必须与路点数量一致。", nameof(waypointTimes)); + } + + if (segmentDurations.Count != plannedWaypoints.Count - 1) + { + throw new ArgumentException("段时长数量必须等于路点数量减一。", nameof(segmentDurations)); + } + + if (segmentScales.Count != segmentDurations.Count) + { + throw new ArgumentException("段缩放因子数量必须与段时长数量一致。", nameof(segmentScales)); + } + + if (iterations < 0) + { + throw new ArgumentOutOfRangeException(nameof(iterations), "迭代次数不能为负数。"); + } + + if (threshold < 0.0) + { + throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值不能为负数。"); + } + + Robot = robot; + OriginalProgram = originalProgram; + PlannedWaypoints = plannedWaypoints; + WaypointTimes = waypointTimes; + SegmentDurations = segmentDurations; + SegmentScales = segmentScales; + Method = method; + Iterations = iterations; + Threshold = threshold; + + OriginalWaypointCount = originalProgram.Waypoints.Count; + InsertedWaypointCount = plannedWaypoints.Count - OriginalWaypointCount; + PlannedWaypointCount = plannedWaypoints.Count; + } + + /// + /// 获取规划时使用的机器人配置文件。 + /// + public RobotProfile Robot { get; } + + /// + /// 获取原始上传的飞拍程序（含示教点、shot_flags 等）。 + /// + public FlyshotProgram OriginalProgram { get; } + + /// + /// 获取规划后完整的路点列表（可能已补中点）。 + /// + public IReadOnlyList PlannedWaypoints { get; } + + /// + /// 获取每个路点对应的绝对时间（单位：秒）。 + /// + public IReadOnlyList WaypointTimes { get; } + + /// + /// 获取每段的规划时长（单位：秒）。 + /// + public IReadOnlyList SegmentDurations { get; } + + /// + /// 获取每段最终的速度/加速度/jerk 缩放因子。 + /// + public IReadOnlyList SegmentScales { get; } + + /// + /// 获取本次规划使用的方法。 + /// + public PlanningMethod Method { get; } + + /// + /// 获取 ICSP 实际执行的迭代轮数。 + /// + public int Iterations { get; } + + /// + /// 获取最终收敛指标（sum(|scale_i - 1|)）。 + /// + public double Threshold { get; } + + /// + /// 获取原始示教点数量（不含补中点）。 + /// + public int OriginalWaypointCount { get; } + + /// + /// 获取补中点插入的数量。 + /// + public int InsertedWaypointCount { get; } + + /// + /// 获取规划后的总路点数量（含补中点）。 + /// + public int PlannedWaypointCount { get; } +} diff --git a/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs b/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs new file mode 100644 index 0000000..35f682c --- /dev/null +++ b/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs @@ -0,0 +1,106 @@ +using Flyshot.Core.Domain; +using Flyshot.Core.Planning.Kinematics; + +namespace Flyshot.Core.Planning.Sampling; + +/// +/// 从规划轨迹生成稠密关节/笛卡尔采样序列。 +/// +/// 为什么采样周期默认 0.016s？ +/// --- +/// 真机伺服周期是 8ms（125Hz），但规划层通常以 16ms（约 60Hz）输出到上层。 +/// 0.016 是一个经验默认值，与真机观测到的稠密轨迹间隔一致。 +/// +public static class TrajectorySampler +{ + /// + /// 生成稠密关节轨迹采样，每行格式：[time, j1, j2, ..., jN]，保留 6 位小数。 + /// + public static IReadOnlyList> SampleJointTrajectory( + PlannedTrajectory trajectory, + double samplePeriod = 0.016, + int decimals = 6) + { + var spline = RebuildSpline(trajectory); + double duration = trajectory.WaypointTimes[^1]; + var times = GenerateSampleTimes(duration, samplePeriod); + var result = new List>(times.Count); + + foreach (var t in times) + { + var pos = spline.Evaluate(t); + var row = new List(pos.Length + 1); + row.Add(Math.Round(t, decimals)); + foreach (var value in pos) + { + row.Add(Math.Round(value, decimals)); + } + + result.Add(row); + } + + return result; + } + + /// + /// 生成稠密笛卡尔轨迹采样，每行格式：[time, x, y, z, qx, qy, qz, qw]，保留 6 位小数。 + /// + public static IReadOnlyList> SampleCartesianTrajectory( + PlannedTrajectory trajectory, + RobotKinematicsModel kinematicsModel, + double samplePeriod = 0.016, + int decimals = 6) + { + var spline = RebuildSpline(trajectory); + double duration = trajectory.WaypointTimes[^1]; + var times = GenerateSampleTimes(duration, samplePeriod); + var result = new List>(times.Count); + + foreach (var t in times) + { + var jointPos = spline.Evaluate(t); + var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos); + var row = new List(pose.Length + 1); + row.Add(Math.Round(t, decimals)); + foreach (var value in pose) + { + row.Add(Math.Round(value, decimals)); + } + + result.Add(row); + } + + return result; + } + + /// + /// 从规划轨迹重建 CubicSplineInterpolator 用于稠密采样。 + /// + private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory) + { + var times = trajectory.WaypointTimes.ToArray(); + var values = trajectory.PlannedWaypoints.Select(wp => wp.Positions.ToArray()).ToArray(); + return new CubicSplineInterpolator(times, values); + } + + /// + /// 生成采样时间点，强制包含终点。 + /// + private static IReadOnlyList GenerateSampleTimes(double duration, double samplePeriod) + { + var times = new List(); + double t = 0.0; + while (t < duration - 1e-12) + { + times.Add(t); + t += samplePeriod; + } + + if (times.Count == 0 || Math.Abs(times[^1] - duration) > 1e-12) + { + times.Add(duration); + } + + return times; + } +} diff --git a/src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs b/src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs new file mode 100644 index 0000000..f03b6a1 --- /dev/null +++ b/src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs @@ -0,0 +1,182 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning; + +/// +/// 在 ICSP 外层包裹补中点策略，实现 self-adapt-icsp 行为。 +/// +/// 为什么需要这层？ +/// --- +/// 逆向分析已经指出：原系统里普通 icsp 若仍有段 scale > 1，不会直接返回未收敛结果； +/// 配置中还明确存在 adapt_icsp_try_num。本层把“超限段统一插入中点后再重规划”的逻辑显式落地， +/// 补上 demo 缺失的失败恢复路径。 +/// +/// 补点策略： +/// --- +/// 对当前所有 scale > 1 + tolerance 的段统一插入关节空间中点，然后把新路点集交给 ICSPPlanner +/// 重新规划。这种"先把明显病灶都降一档，再整体重规划"的策略比逐段拆分更稳定， +/// 也更符合服务端 adapt_icsp_try_num 的意图。 +/// +public sealed class SelfAdaptIcspPlanner +{ + /// + /// 判定段是否超限的数值容差，过滤浮点噪声。 + /// + public const double ScaleTolerance = 5e-4; + + private readonly ICspPlanner _innerPlanner = new(); + + /// + /// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。 + /// + /// 轨迹规划请求。 + /// 最大补点重试次数（默认 5）。 + /// 规划后的轨迹结果。 + /// 超过最大重试次数仍未收敛时抛出。 + public PlannedTrajectory Plan(TrajectoryRequest request, int adaptIcspTryNum = 5) + { + ArgumentNullException.ThrowIfNull(request); + + var currentWaypoints = request.Program.Waypoints.ToArray(); + var currentShotFlags = request.Program.ShotFlags.ToArray(); + var currentOffsets = request.Program.OffsetValues.ToArray(); + var currentAddrs = request.Program.AddressGroups.ToArray(); + int originalWaypointCount = currentWaypoints.Length; + + if (originalWaypointCount < 4) + { + throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request)); + } + + var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs); + var currentRequest = new TrajectoryRequest( + robot: request.Robot, + program: currentProgram, + method: PlanningMethod.SelfAdaptIcsp, + moveToStart: request.MoveToStart, + saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts, + useCache: request.UseCache); + + PlannedTrajectory? lastTrajectory = null; + int maxAttempts = Math.Max(0, adaptIcspTryNum); + + for (int attempt = 0; attempt <= maxAttempts; attempt++) + { + var trajectory = _innerPlanner.Plan(currentRequest); + lastTrajectory = trajectory; + + var badSegments = new List(); + for (int seg = 0; seg < trajectory.SegmentScales.Count; seg++) + { + if (trajectory.SegmentScales[seg] > 1.0 + ScaleTolerance) + { + badSegments.Add(seg); + } + } + + if (badSegments.Count == 0) + { + // 所有段都满足约束，收敛成功。返回包含补中点后路点的轨迹。 + return new PlannedTrajectory( + robot: trajectory.Robot, + originalProgram: request.Program, + plannedWaypoints: currentWaypoints, + waypointTimes: trajectory.WaypointTimes, + segmentDurations: trajectory.SegmentDurations, + segmentScales: trajectory.SegmentScales, + method: PlanningMethod.SelfAdaptIcsp, + iterations: trajectory.Iterations, + threshold: trajectory.Threshold); + } + + if (attempt >= maxAttempts) + { + break; + } + + // 对超限段插入中点，并同步扩展 shot 元数据。 + (currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) = + InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments); + + currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs); + currentRequest = new TrajectoryRequest( + robot: request.Robot, + program: currentProgram, + method: PlanningMethod.SelfAdaptIcsp, + moveToStart: request.MoveToStart, + saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts, + useCache: request.UseCache); + } + + double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN; + throw new InvalidOperationException( + $"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛，最大段缩放因子={maxScale:F6}。"); + } + + /// + /// 用当前路点集和元数据构造 FlyshotProgram。 + /// + private static FlyshotProgram BuildProgram( + string name, + JointWaypoint[] waypoints, + bool[] shotFlags, + int[] offsets, + IoAddressGroup[] addrs) + { + return new FlyshotProgram( + name: name, + waypoints: waypoints, + shotFlags: shotFlags, + offsetValues: offsets, + addressGroups: addrs); + } + + /// + /// 对超限段统一插入关节空间中点，并同步扩展 shot 元数据。 + /// 新插入的路点默认 shotFlag=false、offset=0、addr=空。 + /// + private static (JointWaypoint[] waypoints, bool[] shotFlags, int[] offsets, IoAddressGroup[] addrs) + InsertSegmentMidpoints( + JointWaypoint[] waypoints, + bool[] shotFlags, + int[] offsets, + IoAddressGroup[] addrs, + List badSegments) + { + if (badSegments.Count == 0) + { + return (waypoints, shotFlags, offsets, addrs); + } + + var badSet = new HashSet(badSegments); + var newWaypoints = new List(waypoints.Length + badSegments.Count); + var newShotFlags = new List(waypoints.Length + badSegments.Count); + var newOffsets = new List(waypoints.Length + badSegments.Count); + var newAddrs = new List(waypoints.Length + badSegments.Count); + + newWaypoints.Add(waypoints[0]); + newShotFlags.Add(shotFlags[0]); + newOffsets.Add(offsets[0]); + newAddrs.Add(addrs[0]); + + for (int seg = 0; seg < waypoints.Length - 1; seg++) + { + if (badSet.Contains(seg)) + { + var mid = new JointWaypoint( + waypoints[seg].Positions.Zip(waypoints[seg + 1].Positions, (a, b) => (a + b) / 2.0)); + newWaypoints.Add(mid); + newShotFlags.Add(false); + newOffsets.Add(0); + newAddrs.Add(new IoAddressGroup(Array.Empty())); + } + + newWaypoints.Add(waypoints[seg + 1]); + newShotFlags.Add(shotFlags[seg + 1]); + newOffsets.Add(offsets[seg + 1]); + newAddrs.Add(addrs[seg + 1]); + } + + return (newWaypoints.ToArray(), newShotFlags.ToArray(), newOffsets.ToArray(), newAddrs.ToArray()); + } +} diff --git a/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj b/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj new file mode 100644 index 0000000..d3171ec --- /dev/null +++ b/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj @@ -0,0 +1,14 @@ + + + + net8.0 + enable + enable + + + + + + + + diff --git a/src/Flyshot.Core.Triggering/ShotTimeline.cs b/src/Flyshot.Core.Triggering/ShotTimeline.cs new file mode 100644 index 0000000..be8a139 --- /dev/null +++ b/src/Flyshot.Core.Triggering/ShotTimeline.cs @@ -0,0 +1,31 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Triggering; + +/// +/// 表示一次飞拍触发时间轴的完整计算结果，包含理论触发事件和运行时注入事件。 +/// +public sealed class ShotTimeline +{ + /// + /// 初始化一份已验证的触发时间轴。 + /// + public ShotTimeline(IEnumerable shotEvents, IEnumerable triggerTimeline) + { + ArgumentNullException.ThrowIfNull(shotEvents); + ArgumentNullException.ThrowIfNull(triggerTimeline); + + ShotEvents = shotEvents.ToArray(); + TriggerTimeline = triggerTimeline.ToArray(); + } + + /// + /// 获取所有拍照触发事件（含理论和离散化时间）。 + /// + public IReadOnlyList ShotEvents { get; } + + /// + /// 获取用于运行时伺服流注入的 DO 事件列表。 + /// + public IReadOnlyList TriggerTimeline { get; } +} diff --git a/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs b/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs new file mode 100644 index 0000000..f95778e --- /dev/null +++ b/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs @@ -0,0 +1,82 @@ +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; + +namespace Flyshot.Core.Triggering; + +/// +/// 根据规划轨迹和飞拍配置生成触发时间轴，把示教点上的 shot_flags / offset_values / addr +/// 映射成带理论时间和离散化时间的 ShotEvent，以及可直接注入伺服流的 TrajectoryDoEvent。 +/// +public sealed class ShotTimelineBuilder +{ + private readonly WaypointTimestampResolver _resolver; + + /// + /// 初始化 ShotTimelineBuilder，依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。 + /// + public ShotTimelineBuilder(WaypointTimestampResolver resolver) + { + _resolver = resolver ?? throw new ArgumentNullException(nameof(resolver)); + } + + /// + /// 为给定轨迹构建完整的触发时间轴。 + /// + /// 规划后的轨迹（含补中点信息和机器人配置）。 + /// IO 保持周期数（对应原系统的 io_keep_cycles）。 + /// 稠密采样周期，用于离散化 sample_index 和 sample_time。 + /// 包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。 + public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod) + { + ArgumentNullException.ThrowIfNull(trajectory); + + if (holdCycles < 0) + { + throw new ArgumentOutOfRangeException(nameof(holdCycles), "IO 保持周期数不能为负数。"); + } + + if (samplePeriod <= TimeSpan.Zero) + { + throw new ArgumentOutOfRangeException(nameof(samplePeriod), "采样周期必须大于零。"); + } + + var timestamps = _resolver.Resolve(trajectory); + var program = trajectory.OriginalProgram; + var robot = trajectory.Robot; + double triggerPeriodSeconds = robot.TriggerPeriod.TotalSeconds; + double samplePeriodSeconds = samplePeriod.TotalSeconds; + + var shotEvents = new List(); + var triggerTimeline = new List(); + + for (int i = 0; i < program.Waypoints.Count; i++) + { + if (!program.ShotFlags[i]) + { + continue; + } + + double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds; + int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds); + double sampleTime = sampleIndex * samplePeriodSeconds; + + var addressGroup = program.AddressGroups[i]; + + shotEvents.Add(new ShotEvent( + waypointIndex: i, + triggerTime: triggerTime, + sampleIndex: sampleIndex, + sampleTime: sampleTime, + addressGroup: addressGroup)); + + triggerTimeline.Add(new TrajectoryDoEvent( + waypointIndex: i, + triggerTime: triggerTime, + offsetCycles: program.OffsetValues[i], + holdCycles: holdCycles, + addressGroup: addressGroup)); + } + + return new ShotTimeline(shotEvents, triggerTimeline); + } +} diff --git a/src/Flyshot.Core.Triggering/WaypointTimestampResolver.cs b/src/Flyshot.Core.Triggering/WaypointTimestampResolver.cs new file mode 100644 index 0000000..5c8acb5 --- /dev/null +++ b/src/Flyshot.Core.Triggering/WaypointTimestampResolver.cs @@ -0,0 +1,78 @@ +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; + +namespace Flyshot.Core.Triggering; + +/// +/// 把补中点后的规划轨迹映射回原始示教点的时间戳。 +/// +/// 为什么需要单独拆一层？ +/// --- +/// self-adapt-icsp 会在原始示教点之间插入中点，导致 PlannedWaypoints 的数量和顺序 +/// 与 OriginalProgram.Waypoints 不一致。但触发时序（ShotEvent）必须绑定到原始示教点， +/// 而不是中点。这一层负责在补中点后的轨迹中“找回”每个原始示教点对应的精确时刻。 +/// +public sealed class WaypointTimestampResolver +{ + /// + /// 判定两个路点是否为同一示教点的关节空间距离容差。 + /// + public const double DefaultPositionTolerance = 1e-4; + + /// + /// 为原始示教点列表找回在规划轨迹中的对应时间戳。 + /// + /// 规划后的轨迹（可能已补中点）。 + /// 与原始示教点一一对应的时间戳数组（单位：秒）。 + /// 当某个原始示教点无法在当前路点列表中匹配时抛出。 + public double[] Resolve(PlannedTrajectory trajectory) + { + ArgumentNullException.ThrowIfNull(trajectory); + + var originalWaypoints = trajectory.OriginalProgram.Waypoints; + var plannedWaypoints = trajectory.PlannedWaypoints; + var waypointTimes = trajectory.WaypointTimes; + + var timestamps = new double[originalWaypoints.Count]; + int plannedIndex = 0; + + for (int originalIndex = 0; originalIndex < originalWaypoints.Count; originalIndex++) + { + bool matched = false; + for (; plannedIndex < plannedWaypoints.Count; plannedIndex++) + { + if (WaypointDistance(originalWaypoints[originalIndex], plannedWaypoints[plannedIndex]) <= DefaultPositionTolerance) + { + timestamps[originalIndex] = waypointTimes[plannedIndex]; + matched = true; + plannedIndex++; // 下一次匹配从当前位置之后开始，保证顺序 + break; + } + } + + if (!matched) + { + throw new InvalidOperationException( + $"无法在补中点后的轨迹中为原始示教点 {originalIndex} 找回对应时间戳。"); + } + } + + return timestamps; + } + + /// + /// 计算两个路点在关节空间的欧氏距离。 + /// + private static double WaypointDistance(JointWaypoint a, JointWaypoint b) + { + double sumSq = 0.0; + int count = Math.Min(a.Positions.Count, b.Positions.Count); + for (int i = 0; i < count; i++) + { + double diff = a.Positions[i] - b.Positions[i]; + sumSq += diff * diff; + } + + return Math.Sqrt(sumSq); + } +} diff --git a/src/Flyshot.Runtime.Common/Flyshot.Runtime.Common.csproj b/src/Flyshot.Runtime.Common/Flyshot.Runtime.Common.csproj new file mode 100644 index 0000000..fa71b7a --- /dev/null +++ b/src/Flyshot.Runtime.Common/Flyshot.Runtime.Common.csproj @@ -0,0 +1,9 @@ + + + + net8.0 + enable + enable + + + diff --git a/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj b/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj new file mode 100644 index 0000000..21720d3 --- /dev/null +++ b/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/Flyshot.Server.Host/Program.cs b/src/Flyshot.Server.Host/Program.cs new file mode 100644 index 0000000..38ded6e --- /dev/null +++ b/src/Flyshot.Server.Host/Program.cs @@ -0,0 +1,13 @@ +var builder = WebApplication.CreateBuilder(args); +var app = builder.Build(); + +app.MapGet("/", () => Results.Redirect("/healthz")); +app.MapGet("/healthz", () => Results.Ok(new +{ + status = "ok", + service = "flyshot-server-host" +})); + +app.Run(); + +public partial class Program; diff --git a/src/Flyshot.Server.Host/Properties/launchSettings.json b/src/Flyshot.Server.Host/Properties/launchSettings.json new file mode 100644 index 0000000..a6ca334 --- /dev/null +++ b/src/Flyshot.Server.Host/Properties/launchSettings.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json.schemastore.org/launchsettings.json", + "iisSettings": { + "windowsAuthentication": false, + "anonymousAuthentication": true, + "iisExpress": { + "applicationUrl": "http://localhost:43174", + "sslPort": 44344 + } + }, + "profiles": { + "http": { + "commandName": "Project", + "dotnetRunMessages": true, + "launchBrowser": true, + "applicationUrl": "http://localhost:5190", + "environmentVariables": { + "ASPNETCORE_ENVIRONMENT": "Development" + } + }, + "https": { + "commandName": "Project", + "dotnetRunMessages": true, + "launchBrowser": true, + "applicationUrl": "https://localhost:7167;http://localhost:5190", + "environmentVariables": { + "ASPNETCORE_ENVIRONMENT": "Development" + } + }, + "IIS Express": { + "commandName": "IISExpress", + "launchBrowser": true, + "environmentVariables": { + "ASPNETCORE_ENVIRONMENT": "Development" + } + } + } +} diff --git a/src/Flyshot.Server.Host/appsettings.Development.json b/src/Flyshot.Server.Host/appsettings.Development.json new file mode 100644 index 0000000..0c208ae --- /dev/null +++ b/src/Flyshot.Server.Host/appsettings.Development.json @@ -0,0 +1,8 @@ +{ + "Logging": { + "LogLevel": { + "Default": "Information", + "Microsoft.AspNetCore": "Warning" + } + } +} diff --git a/src/Flyshot.Server.Host/appsettings.json b/src/Flyshot.Server.Host/appsettings.json new file mode 100644 index 0000000..10f68b8 --- /dev/null +++ b/src/Flyshot.Server.Host/appsettings.json @@ -0,0 +1,9 @@ +{ + "Logging": { + "LogLevel": { + "Default": "Information", + "Microsoft.AspNetCore": "Warning" + } + }, + "AllowedHosts": "*" +} diff --git a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs new file mode 100644 index 0000000..ac3b989 --- /dev/null +++ b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs @@ -0,0 +1,176 @@ +using Flyshot.Core.Config; +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Tests; + +/// +/// 锁定 Task 3 的兼容输入行为，确保旧配置、.robot 元数据和路径策略都能被稳定加载。 +/// +public sealed class ConfigCompatibilityTests +{ + /// + /// 验证现有 RobotConfig.json 能被加载，并保持关键机器人参数与飞拍程序内容不变。 + /// + [Fact] + public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms() + { + var workspaceRoot = GetWorkspaceRoot(); + var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json"); + + var loaded = new RobotConfigLoader().Load(configPath); + + Assert.True(loaded.Robot.UseDo); + Assert.Equal([7, 8], loaded.Robot.IoAddresses); + Assert.Equal(2, loaded.Robot.IoKeepCycles); + Assert.Equal(1.0, loaded.Robot.AccLimitScale); + Assert.Equal(1.0, loaded.Robot.JerkLimitScale); + Assert.Equal(5, loaded.Robot.AdaptIcspTryNum); + + var program = Assert.Contains("001", loaded.Programs); + Assert.Equal("001", program.Name); + Assert.Equal(5, program.Waypoints.Count); + Assert.Equal(3, program.ShotWaypointCount); + Assert.Empty(program.AddressGroups[0].Addresses); + Assert.Equal([8, 7], program.AddressGroups[1].Addresses); + } + + /// + /// 验证旧配置缺少 offset_values 和 addr 字段时，会自动回填与旧系统一致的默认值。 + /// + [Fact] + public void RobotConfigLoader_FillsLegacyDefaults_WhenOptionalFieldsAreMissing() + { + var tempRoot = CreateTempDirectory(); + try + { + var configPath = Path.Combine(tempRoot, "legacy.json"); + File.WriteAllText( + configPath, + """ + { + "robot": { + "use_do": false, + "io_keep_cycles": 3, + "acc_limit": 0.5, + "jerk_limit": 0.25 + }, + "flying_shots": { + "demo": { + "traj_waypoints": [[0, 1], [2, 3], [4, 5]], + "shot_flags": [0, 1, 0] + } + } + } + """); + + var loaded = new RobotConfigLoader().Load(configPath); + var program = Assert.Contains("demo", loaded.Programs); + + Assert.False(loaded.Robot.UseDo); + Assert.Empty(loaded.Robot.IoAddresses); + Assert.Equal(3, loaded.Robot.IoKeepCycles); + Assert.Equal(0.5, loaded.Robot.AccLimitScale); + Assert.Equal(0.25, loaded.Robot.JerkLimitScale); + Assert.Equal([0, 0, 0], program.OffsetValues); + Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses)); + } + finally + { + Directory.Delete(tempRoot, recursive: true); + } + } + + /// + /// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。 + /// + [Fact] + public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling() + { + var workspaceRoot = GetWorkspaceRoot(); + var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot"); + + var profile = new RobotModelLoader().LoadProfile(modelPath); + + Assert.Equal("FANUC_LR_Mate_200iD_7L", profile.Name); + Assert.Equal(modelPath, profile.ModelPath); + Assert.Equal(6, profile.DegreesOfFreedom); + Assert.Equal(6.45, profile.JointLimits[0].VelocityLimit, precision: 2); + Assert.Equal(29.81, profile.JointLimits[2].AccelerationLimit, precision: 2); + + var coupling = Assert.Single(profile.JointCouplings); + Assert.Equal("Joint3", coupling.SlaveJointName); + Assert.Equal("Joint2", coupling.MasterJointName); + Assert.Equal(1.0, coupling.Multiplier); + Assert.Equal(0.0, coupling.Offset); + } + + /// + /// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。 + /// + [Fact] + public void RobotModelLoader_AppliesAccelerationAndJerkScales() + { + var workspaceRoot = GetWorkspaceRoot(); + var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot"); + + var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25); + + Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3); + Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3); + } + + /// + /// 验证路径兼容层既能补旧目录候选，也能按平台策略生成默认用户数据目录。 + /// + [Fact] + public void PathCompatibility_ResolvesLegacyCandidates_AndBuildsUserDataRoots() + { + var tempRoot = CreateTempDirectory(); + try + { + var legacyConfigPath = Path.Combine(tempRoot, "Rvbust", "Install", "FlyingShot", "Config", "sample.json"); + Directory.CreateDirectory(Path.GetDirectoryName(legacyConfigPath)!); + File.WriteAllText(legacyConfigPath, "{}"); + + var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot); + + Assert.Equal(legacyConfigPath, resolved); + Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix)); + Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows)); + } + finally + { + Directory.Delete(tempRoot, recursive: true); + } + } + + /// + /// 定位当前工作区根目录，便于复用父仓库中的真实样本。 + /// + private static string GetWorkspaceRoot() + { + var current = new DirectoryInfo(AppContext.BaseDirectory); + while (current is not null) + { + var slnPath = Path.Combine(current.FullName, "FlyshotReplacement.sln"); + if (File.Exists(slnPath)) + { + return Path.GetFullPath(Path.Combine(current.FullName, "..")); + } + + current = current.Parent; + } + + throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root."); + } + + /// + /// 创建当前测试专用的临时目录，避免不同测试之间相互污染。 + /// + private static string CreateTempDirectory() + { + var tempPath = Path.Combine(Path.GetTempPath(), "flyshot-config-tests", Guid.NewGuid().ToString("N")); + Directory.CreateDirectory(tempPath); + return tempPath; + } +} diff --git a/tests/Flyshot.Core.Tests/DomainModelTests.cs b/tests/Flyshot.Core.Tests/DomainModelTests.cs new file mode 100644 index 0000000..f094479 --- /dev/null +++ b/tests/Flyshot.Core.Tests/DomainModelTests.cs @@ -0,0 +1,207 @@ +using System.Text.Json; +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Tests; + +/// +/// Verifies the Task 2 domain contracts before planning and runtime code depend on them. +/// +public sealed class DomainModelTests +{ + /// + /// Ensures robot profiles keep a stable copy of joint limits and reject invalid dimensions. + /// + [Fact] + public void RobotProfile_CopiesJointLimits_AndRejectsMismatchedDof() + { + var jointLimits = new[] + { + new JointLimit("J1", 7.85, 32.72, 272.7), + new JointLimit("J2", 6.63, 27.63, 230.28) + }; + + var profile = new RobotProfile( + name: "LR_Mate_200iD_7L", + modelPath: "Models/LR_Mate_200iD_7L.robot", + degreesOfFreedom: 2, + jointLimits: jointLimits, + jointCouplings: Array.Empty(), + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + + Assert.Equal(2, profile.DegreesOfFreedom); + Assert.NotSame(jointLimits, profile.JointLimits); + + // The planner must not accept a profile whose DOF and limits disagree. + Assert.Throws(() => new RobotProfile( + name: "InvalidRobot", + modelPath: "Models/Invalid.robot", + degreesOfFreedom: 3, + jointLimits: jointLimits, + jointCouplings: Array.Empty(), + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8))); + } + + /// + /// Ensures uploaded flyshot programs keep their shot metadata aligned with teach waypoints. + /// + [Fact] + public void FlyshotProgram_RejectsMisalignedShotMetadata() + { + var waypoints = new[] + { + new JointWaypoint(new[] { 0.0, 1.0 }), + new JointWaypoint(new[] { 2.0, 3.0 }) + }; + + var validProgram = new FlyshotProgram( + name: "EOL10_EAU_0", + waypoints: waypoints, + shotFlags: new[] { true, false }, + offsetValues: new[] { 0, 1 }, + addressGroups: new[] + { + new IoAddressGroup(new[] { 100 }), + new IoAddressGroup(Array.Empty()) + }); + + Assert.Equal(1, validProgram.ShotWaypointCount); + + // The gateway cannot recover from count mismatches after this point, so fail fast here. + Assert.Throws(() => new FlyshotProgram( + name: "BrokenProgram", + waypoints: waypoints, + shotFlags: new[] { true }, + offsetValues: new[] { 0, 1 }, + addressGroups: new[] + { + new IoAddressGroup(new[] { 100 }), + new IoAddressGroup(Array.Empty()) + })); + } + + /// + /// Ensures execution requests start from predictable defaults for compatibility paths. + /// + [Fact] + public void TrajectoryRequest_UsesExpectedDefaults() + { + var request = new TrajectoryRequest( + robot: CreateRobotProfile(), + program: CreateProgram(), + method: PlanningMethod.Icsp); + + Assert.False(request.MoveToStart); + Assert.False(request.SaveTrajectoryArtifacts); + Assert.False(request.UseCache); + Assert.Equal(PlanningMethod.Icsp, request.Method); + } + + /// + /// Ensures runtime snapshots expose safe empty collections before the controller connects. + /// + [Fact] + public void ControllerStateSnapshot_InitializesEmptyRuntimeCollections() + { + var snapshot = new ControllerStateSnapshot( + capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"), + connectionState: "Disconnected", + isEnabled: false, + isInMotion: false, + speedRatio: 100.0); + + Assert.Empty(snapshot.JointPositions); + Assert.Empty(snapshot.CartesianPose); + Assert.Empty(snapshot.ActiveAlarms); + } + + /// + /// Ensures JSON payloads keep stable enum and property names for downstream SDKs. + /// + [Fact] + public void DomainObjects_SerializeStableContract() + { + var result = new TrajectoryResult( + programName: "EOL10_EAU_0", + method: PlanningMethod.SelfAdaptIcsp, + isValid: true, + duration: TimeSpan.FromSeconds(1.25), + shotEvents: new[] + { + new ShotEvent( + waypointIndex: 0, + triggerTime: 0.5, + sampleIndex: 62, + sampleTime: 0.496, + addressGroup: new IoAddressGroup(new[] { 100 })) + }, + triggerTimeline: new[] + { + new TrajectoryDoEvent( + waypointIndex: 0, + triggerTime: 0.5, + offsetCycles: 0, + holdCycles: 1, + addressGroup: new IoAddressGroup(new[] { 100 })) + }, + artifacts: new[] + { + new TrajectoryArtifact( + kind: TrajectoryArtifactKind.JointDenseTrajectory, + logicalName: "JointDetialTraj.txt", + relativePath: "artifacts/JointDetialTraj.txt") + }, + failureReason: null, + usedCache: true, + originalWaypointCount: 4, + plannedWaypointCount: 5); + + var json = JsonSerializer.Serialize(result); + + Assert.Contains("\"programName\":\"EOL10_EAU_0\"", json); + Assert.Contains("\"method\":\"SelfAdaptIcsp\"", json); + Assert.Contains("\"kind\":\"JointDenseTrajectory\"", json); + Assert.Contains("\"usedCache\":true", json); + } + + /// + /// Creates a representative robot profile for request-level domain tests. + /// + private static RobotProfile CreateRobotProfile() + { + return new RobotProfile( + name: "LR_Mate_200iD_7L", + modelPath: "Models/LR_Mate_200iD_7L.robot", + degreesOfFreedom: 2, + jointLimits: new[] + { + new JointLimit("J1", 7.85, 32.72, 272.7), + new JointLimit("J2", 6.63, 27.63, 230.28) + }, + jointCouplings: Array.Empty(), + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + } + + /// + /// Creates a representative uploaded program for request-level domain tests. + /// + private static FlyshotProgram CreateProgram() + { + return new FlyshotProgram( + name: "EOL10_EAU_0", + waypoints: new[] + { + new JointWaypoint(new[] { 0.0, 1.0 }), + new JointWaypoint(new[] { 2.0, 3.0 }) + }, + shotFlags: new[] { true, false }, + offsetValues: new[] { 0, 1 }, + addressGroups: new[] + { + new IoAddressGroup(new[] { 100 }), + new IoAddressGroup(Array.Empty()) + }); + } +} diff --git a/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj b/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj new file mode 100644 index 0000000..39acecc --- /dev/null +++ b/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj @@ -0,0 +1,32 @@ + + + + net8.0 + enable + enable + + false + true + + + + + + + runtime; build; native; contentfiles; analyzers; buildtransitive + all + + + runtime; build; native; contentfiles; analyzers; buildtransitive + all + + + + + + + + + + + diff --git a/tests/Flyshot.Core.Tests/GlobalUsings.cs b/tests/Flyshot.Core.Tests/GlobalUsings.cs new file mode 100644 index 0000000..8c927eb --- /dev/null +++ b/tests/Flyshot.Core.Tests/GlobalUsings.cs @@ -0,0 +1 @@ +global using Xunit; \ No newline at end of file diff --git a/tests/Flyshot.Core.Tests/OfflinePlanTests.cs b/tests/Flyshot.Core.Tests/OfflinePlanTests.cs new file mode 100644 index 0000000..a536cdd --- /dev/null +++ b/tests/Flyshot.Core.Tests/OfflinePlanTests.cs @@ -0,0 +1,154 @@ +using Flyshot.Core.Config; +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; +using Flyshot.Core.Planning.Export; +using Flyshot.Core.Planning.Kinematics; +using Flyshot.Core.Planning.Sampling; +using Flyshot.Core.Triggering; +using Xunit.Abstractions; + +namespace Flyshot.Core.Tests; + +/// +/// 离线轨迹生成入口，与 Python demo 的 CLI 对齐。 +/// +/// 为什么放在测试项目里？ +/// --- +/// 测试项目已经引用了所有需要的模块（Config、Domain、Planning、Triggering）， +/// 不需要额外创建 Console 项目。通过 dotnet test --filter 可以直接调用， +/// 也能利用 xUnit 的断言做结果验证。 +/// +public sealed class OfflinePlanTests +{ + /// + /// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。 + /// + [Theory] + [InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", false, 1.0)] + [InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", true, 0.9)] + [InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL10_EAU_0", false, 1.0)] + public void GenerateTrajectory_MatchesPythonDemo( + string configPath, + string robotModelPath, + string trajName, + bool useSelfAdapt, + double speedRatio) + { + var workspaceRoot = GetWorkspaceRoot(); + var outputDir = Path.Combine(workspaceRoot, "analysis", "output", "dotnet", $"{trajName}_sr{speedRatio:F2}_{(useSelfAdapt ? "adapt" : "icsp")}"); + Directory.CreateDirectory(outputDir); + + // 1. 加载配置和模型。 + var loadedConfig = new RobotConfigLoader().Load(configPath, repoRoot: workspaceRoot); + var program = loadedConfig.Programs[trajName]; + var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath); + var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale); + var kinematicsModel = new RobotModelLoader().LoadKinematicsModel(resolvedRobotModelPath); + + // 2. 应用 speed_ratio 缩放。 + var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio); + + // 3. 规划轨迹。 + var request = new TrajectoryRequest( + robot: scaledProfile, + program: program, + method: useSelfAdapt ? PlanningMethod.SelfAdaptIcsp : PlanningMethod.Icsp); + + PlannedTrajectory trajectory; + if (useSelfAdapt) + { + trajectory = new SelfAdaptIcspPlanner().Plan(request, loadedConfig.Robot.AdaptIcspTryNum); + } + else + { + trajectory = new ICspPlanner().Plan(request); + } + + // 4. 生成触发时间轴。 + var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver()) + .Build(trajectory, holdCycles: loadedConfig.Robot.IoKeepCycles, samplePeriod: TimeSpan.FromMilliseconds(16)); + + // 5. 稠密采样。 + double samplePeriod = 0.016; + var jointDense = TrajectorySampler.SampleJointTrajectory(trajectory, samplePeriod); + var cartDense = TrajectorySampler.SampleCartesianTrajectory(trajectory, kinematicsModel, samplePeriod); + + // 6. 导出文件。 + TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.demo.txt"), jointDense); + TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.demo.txt"), cartDense); + TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.demo.json"), timeline.ShotEvents); + + // 7. 打印统计信息到测试输出。 + _testOutputHelper.WriteLine($"traj_name={trajName}"); + _testOutputHelper.WriteLine($"speed_ratio={speedRatio:F6}"); + _testOutputHelper.WriteLine($"duration={trajectory.WaypointTimes[^1]:F6}"); + _testOutputHelper.WriteLine($"joint_dense_rows={jointDense.Count}"); + _testOutputHelper.WriteLine($"cart_dense_rows={cartDense.Count}"); + _testOutputHelper.WriteLine($"shot_events={timeline.ShotEvents.Count}"); + _testOutputHelper.WriteLine($"output_dir={outputDir}"); + + // 最小验证：输出文件存在且非空。 + Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.demo.txt"))); + Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.demo.txt"))); + Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.demo.json"))); + Assert.True(jointDense.Count > 0); + Assert.True(cartDense.Count > 0); + } + + private readonly ITestOutputHelper _testOutputHelper; + + public OfflinePlanTests(ITestOutputHelper testOutputHelper) + { + _testOutputHelper = testOutputHelper; + } + + /// + /// 按 speed_ratio 缩放机器人关节限制。 + /// + /// 缩放律： + /// - 速度按一次方：speedRatio^1 + /// - 加速度按平方：speedRatio^2 + /// - jerk 按立方：speedRatio^3 + /// + /// 为什么加速度是平方？ + /// --- + /// 如果把时间轴整体缩放 1/speedRatio 倍，速度按 speedRatio 缩放，加速度按 speedRatio^2 缩放。 + /// 所以降低 speed_ratio 意味着降低速度、更大幅度地降低加速度和 jerk，轨迹更平滑。 + /// + private static RobotProfile ScaleRobotProfile(RobotProfile source, double speedRatio) + { + return new RobotProfile( + name: source.Name, + modelPath: source.ModelPath, + degreesOfFreedom: source.DegreesOfFreedom, + jointLimits: source.JointLimits + .Select(limit => new JointLimit( + limit.JointName, + limit.VelocityLimit * speedRatio, + limit.AccelerationLimit * speedRatio * speedRatio, + limit.JerkLimit * speedRatio * speedRatio * speedRatio)) + .ToArray(), + jointCouplings: source.JointCouplings, + servoPeriod: source.ServoPeriod, + triggerPeriod: source.TriggerPeriod); + } + + /// + /// 定位父工作区根目录。 + /// + private static string GetWorkspaceRoot() + { + var current = new DirectoryInfo(AppContext.BaseDirectory); + while (current is not null) + { + if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln"))) + { + return Path.GetFullPath(Path.Combine(current.FullName, "..")); + } + + current = current.Parent; + } + + throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root."); + } +} diff --git a/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs b/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs new file mode 100644 index 0000000..3c59a09 --- /dev/null +++ b/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs @@ -0,0 +1,227 @@ +using Flyshot.Core.Config; +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; +using Flyshot.Core.Triggering; + +namespace Flyshot.Core.Tests; + +/// +/// 锁定 Task 4 的最小兼容面，覆盖 ICSP、自适应补点以及飞拍触发时间轴。 +/// +public sealed class PlanningCompatibilityTests +{ + /// + /// 验证 ICSP 规划至少会生成严格递增的 waypoint 时间轴。 + /// + [Fact] + public void ICspPlanner_ReturnsMonotonicWaypointTimes() + { + var request = new TrajectoryRequest( + robot: CreateRobotProfile([1, 1, 1, 1, 1, 1], [2, 2, 2, 2, 2, 2], [10, 10, 10, 10, 10, 10]), + program: CreateProgram( + new[] + { + new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, + new[] { 0.4, 0.1, 0.0, 0.0, 0.0, 0.0 }, + new[] { 0.8, 0.3, 0.0, 0.0, 0.0, 0.0 }, + new[] { 1.0, 0.2, 0.0, 0.0, 0.0, 0.0 } + }), + method: PlanningMethod.Icsp); + + var trajectory = new ICspPlanner().Plan(request); + + Assert.Equal(4, trajectory.WaypointTimes.Count); + Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First)); + } + + /// + /// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。 + /// + [Fact] + public void SelfAdaptIcspPlanner_InsertsMidpoints_ForEol9Case() + { + var workspaceRoot = GetWorkspaceRoot(); + var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json"); + var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot"); + + var config = new RobotConfigLoader().Load(configPath); + var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale); + var constrainedProfile = ScaleRobotProfile(baseProfile, velocityScale: 0.9, accelerationScale: 0.9 * 0.9, jerkScale: 0.9 * 0.9 * 0.9); + + var request = new TrajectoryRequest( + robot: constrainedProfile, + program: config.Programs["EOL9_EAU_90"], + method: PlanningMethod.SelfAdaptIcsp); + + var trajectory = new SelfAdaptIcspPlanner().Plan(request, adaptIcspTryNum: config.Robot.AdaptIcspTryNum); + + Assert.True(trajectory.InsertedWaypointCount > 0); + Assert.True(trajectory.PlannedWaypointCount > trajectory.OriginalWaypointCount); + Assert.True(trajectory.SegmentScales.Max() <= 1.0005); + } + + /// + /// 验证补点后仍然能按原始示教点顺序找回时间戳，而不是错误地绑定到新增中点。 + /// + [Fact] + public void WaypointTimestampResolver_UsesOriginalTeachPointsAfterInsertion() + { + var originalProgram = CreateProgram( + new[] + { + new[] { 0.0, 0.0 }, + new[] { 1.0, 0.0 }, + new[] { 2.0, 0.0 }, + new[] { 3.0, 0.0 } + }); + + var trajectory = new PlannedTrajectory( + robot: CreateRobotProfile([1, 1], [1, 1], [1, 1]), + originalProgram: originalProgram, + plannedWaypoints: + [ + new JointWaypoint([0.0, 0.0]), + new JointWaypoint([0.5, 0.0]), + new JointWaypoint([1.0, 0.0]), + new JointWaypoint([2.0, 0.0]), + new JointWaypoint([2.5, 0.0]), + new JointWaypoint([3.0, 0.0]) + ], + waypointTimes: [0.0, 0.25, 0.5, 1.0, 1.5, 2.0], + segmentDurations: [0.25, 0.25, 0.5, 0.5, 0.5], + segmentScales: [1.0, 1.0, 1.0, 1.0, 1.0], + method: PlanningMethod.SelfAdaptIcsp, + iterations: 3, + threshold: 0.0); + + var timestamps = new WaypointTimestampResolver().Resolve(trajectory); + + Assert.Equal([0.0, 0.5, 1.0, 2.0], timestamps); + } + + /// + /// 验证触发时间轴会使用原始 waypoint 时间、offset 周期和地址组生成 ShotEvent/TrajectoryDoEvent。 + /// + [Fact] + public void ShotTimelineBuilder_MapsOffsetsToShotEventsAndTriggerTimeline() + { + var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]); + var program = new FlyshotProgram( + name: "demo", + waypoints: + [ + new JointWaypoint([0.0, 0.0]), + new JointWaypoint([1.0, 0.0]), + new JointWaypoint([2.0, 0.0]) + ], + shotFlags: [false, true, false], + offsetValues: [0, 1, 0], + addressGroups: + [ + new IoAddressGroup(Array.Empty()), + new IoAddressGroup([2, 4]), + new IoAddressGroup(Array.Empty()) + ]); + + var trajectory = new PlannedTrajectory( + robot: robot, + originalProgram: program, + plannedWaypoints: program.Waypoints, + waypointTimes: [0.0, 0.5, 1.0], + segmentDurations: [0.5, 0.5], + segmentScales: [1.0, 1.0], + method: PlanningMethod.Icsp, + iterations: 1, + threshold: 0.0); + + var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver()) + .Build(trajectory, holdCycles: 2, samplePeriod: TimeSpan.FromMilliseconds(16)); + + var shotEvent = Assert.Single(timeline.ShotEvents); + Assert.Equal(0.508, shotEvent.TriggerTime, precision: 3); + Assert.Equal([2, 4], shotEvent.AddressGroup.Addresses); + + var doEvent = Assert.Single(timeline.TriggerTimeline); + Assert.Equal(1, doEvent.WaypointIndex); + Assert.Equal(1, doEvent.OffsetCycles); + Assert.Equal(2, doEvent.HoldCycles); + } + + /// + /// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。 + /// + private static RobotProfile CreateRobotProfile( + IReadOnlyList velocityLimits, + IReadOnlyList accelerationLimits, + IReadOnlyList jerkLimits) + { + return new RobotProfile( + name: "TestRobot", + modelPath: "Models/Test.robot", + degreesOfFreedom: velocityLimits.Count, + jointLimits: velocityLimits + .Select((velocity, index) => new JointLimit( + $"J{index + 1}", + velocity, + accelerationLimits[index], + jerkLimits[index])) + .ToArray(), + jointCouplings: Array.Empty(), + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + } + + /// + /// 构造一个默认不带拍照点的最小 FlyshotProgram。 + /// + private static FlyshotProgram CreateProgram(IEnumerable waypointRows) + { + var rows = waypointRows.ToArray(); + return new FlyshotProgram( + name: "demo", + waypoints: rows.Select(row => new JointWaypoint(row)).ToArray(), + shotFlags: Enumerable.Repeat(false, rows.Length).ToArray(), + offsetValues: Enumerable.Repeat(0, rows.Length).ToArray(), + addressGroups: Enumerable.Range(0, rows.Length).Select(_ => new IoAddressGroup(Array.Empty())).ToArray()); + } + + /// + /// 在不丢失 couple 元数据的前提下，按比例缩放机器人关节限制。 + /// + private static RobotProfile ScaleRobotProfile(RobotProfile source, double velocityScale, double accelerationScale, double jerkScale) + { + return new RobotProfile( + name: source.Name, + modelPath: source.ModelPath, + degreesOfFreedom: source.DegreesOfFreedom, + jointLimits: source.JointLimits + .Select(limit => new JointLimit( + limit.JointName, + limit.VelocityLimit * velocityScale, + limit.AccelerationLimit * accelerationScale, + limit.JerkLimit * jerkScale)) + .ToArray(), + jointCouplings: source.JointCouplings, + servoPeriod: source.ServoPeriod, + triggerPeriod: source.TriggerPeriod); + } + + /// + /// 定位父工作区根目录，读取真实的 EOL9 样本和机器人模型。 + /// + private static string GetWorkspaceRoot() + { + var current = new DirectoryInfo(AppContext.BaseDirectory); + while (current is not null) + { + if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln"))) + { + return Path.GetFullPath(Path.Combine(current.FullName, "..")); + } + + current = current.Parent; + } + + throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root."); + } +} diff --git a/tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj b/tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj new file mode 100644 index 0000000..0c8181e --- /dev/null +++ b/tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj @@ -0,0 +1,30 @@ + + + + net8.0 + enable + enable + + false + true + + + + + + + + runtime; build; native; contentfiles; analyzers; buildtransitive + all + + + runtime; build; native; contentfiles; analyzers; buildtransitive + all + + + + + + + + diff --git a/tests/Flyshot.Server.IntegrationTests/GlobalUsings.cs b/tests/Flyshot.Server.IntegrationTests/GlobalUsings.cs new file mode 100644 index 0000000..8c927eb --- /dev/null +++ b/tests/Flyshot.Server.IntegrationTests/GlobalUsings.cs @@ -0,0 +1 @@ +global using Xunit; \ No newline at end of file diff --git a/tests/Flyshot.Server.IntegrationTests/HealthEndpointTests.cs b/tests/Flyshot.Server.IntegrationTests/HealthEndpointTests.cs new file mode 100644 index 0000000..ad97cd2 --- /dev/null +++ b/tests/Flyshot.Server.IntegrationTests/HealthEndpointTests.cs @@ -0,0 +1,27 @@ +using System.Net; +using System.Text.Json; +using Microsoft.AspNetCore.Mvc.Testing; + +namespace Flyshot.Server.IntegrationTests; + +public sealed class FlyshotServerFactory : WebApplicationFactory; + +public sealed class HealthEndpointTests(FlyshotServerFactory factory) : IClassFixture +{ + [Fact] + public async Task GetHealthz_ReturnsOkPayload() + { + using var client = factory.CreateClient(); + + using var response = await client.GetAsync("/healthz"); + + Assert.Equal(HttpStatusCode.OK, response.StatusCode); + + await using var responseStream = await response.Content.ReadAsStreamAsync(); + using var jsonDocument = await JsonDocument.ParseAsync(responseStream); + var root = jsonDocument.RootElement; + + Assert.Equal("ok", root.GetProperty("status").GetString()); + Assert.Equal("flyshot-server-host", root.GetProperty("service").GetString()); + } +}