Compare commits
17 Commits
4eeaa3fef3
...
legacy-gat
| Author | SHA1 | Date | |
|---|---|---|---|
| 74761bb5da | |||
| f7e2bb0e7b | |||
| 1779067b5c | |||
| c6829d214a | |||
| 70b0ccd414 | |||
| 783716ff44 | |||
| b1710e5d01 | |||
| af65ca03a0 | |||
| 91c1494cde | |||
| a6579f1e5b | |||
| c38faddbf0 | |||
| 0724efebed | |||
| 0292e077ff | |||
| 69fa3edd89 | |||
| 390d066ece | |||
| a78e6761cb | |||
| 8a20d9f507 |
42
.claude/settings.local.json
Normal file
42
.claude/settings.local.json
Normal file
@@ -0,0 +1,42 @@
|
||||
{
|
||||
"permissions": {
|
||||
"allow": [
|
||||
"Bash(git commit -m ':*)",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln -v minimal 2>&1')",
|
||||
"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 2>&1')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test FlyshotReplacement.sln --no-build -v minimal 2>&1')",
|
||||
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal)",
|
||||
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj --no-build -v minimal)",
|
||||
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json'\\)\\); json.load\\(open\\('.vscode/tasks.json'\\)\\); print\\('JSON valid.'\\)\")",
|
||||
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json', encoding='utf-8'\\)\\); json.load\\(open\\('.vscode/tasks.json', encoding='utf-8'\\)\\); print\\('JSON valid.'\\)\")",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal')",
|
||||
"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')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1')",
|
||||
"Bash(taskkill /PID 120812 /F)",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build flyshot-replacement.sln --no-restore -v minimal 2>&1')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build /d/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/flyshot-replacement/FlyshotReplacement.sln --no-restore -v minimal 2>&1')",
|
||||
"Bash(capinfos \"20260430.pcap\")",
|
||||
"Bash(tshark -r \"20260430.pcap\" -T fields -e frame.number -e frame.time_relative -e ip.src -e ip.dst -e udp.srcport -e udp.dstport -e data.len)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e udp.srcport -e udp.length -e data.len)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.srcport==60015\" -T fields -e frame.number -e frame.time_relative -e udp.length)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e udp.length)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e ip.dst -e udp.srcport -e udp.dstport -e udp.length -e frame.protocols)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and frame.number==1\" -V)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.dstport!=60015\" -T fields -e frame.number -e frame.time_relative -e ip.dst -e udp.dstport)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e udp.length)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frj519.type -e frj519.seq -e frj519.status -e frj519.j1 -e frj519.j2 -e frj519.j3 -e frj519.j4 -e frj519.j5 -e frj519.j6 -e frj519.j7 -e frj519.j8 -e frj519.j9 -e frj519.timestamp)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frj519.type -e frj519.seq -e frj519.status -e frj519.j1 -e frj519.j2 -e frj519.j3 -e frj519.j4 -e frj519.j5 -e frj519.j6 -e frj519.j7 -e frj519.j8 -e frj519.j9 -e frj519.timestamp -e frj519.x -e frj519.y -e frj519.z -e frj519.w -e frj519.p -e frj519.r)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -E header=y)",
|
||||
"Bash(tshark -G fields)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frame.time_relative -e data)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e data)",
|
||||
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.srcport==60015\" -T fields -e frame.number -e frame.time_relative -e data)",
|
||||
"Bash(tshark -r \"20260430.pcap\" --disable-protocol frj519 -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e data)",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj --no-restore -v minimal 2>&1 | tail -20')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1 | tail -25')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1 | tail -15')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --no-restore -v minimal 2>&1 | tail -30')"
|
||||
]
|
||||
}
|
||||
}
|
||||
4
.gitignore
vendored
4
.gitignore
vendored
@@ -396,3 +396,7 @@ FodyWeavers.xsd
|
||||
|
||||
# JetBrains Rider
|
||||
*.sln.iml
|
||||
Config/Data/*
|
||||
.dotnet-home/*
|
||||
codex-dotnet-home/*
|
||||
.dotnet-sdk8/*
|
||||
|
||||
67
.vscode/launch.json
vendored
Normal file
67
.vscode/launch.json
vendored
Normal file
@@ -0,0 +1,67 @@
|
||||
{
|
||||
// VS Code 启动与调试配置
|
||||
// 依赖 C# 扩展(OmniSharp 或 C# Dev Kit)提供 coreclr 调试器。
|
||||
// 文档:https://code.visualstudio.com/docs/csharp/debugger-settings
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
{
|
||||
// 标准调试启动:编译并启动 Host,命中断点,浏览器自动打开首页
|
||||
"name": ".NET Core Launch (Host)",
|
||||
"type": "coreclr",
|
||||
"request": "launch",
|
||||
"program": "dotnet",
|
||||
"args": [
|
||||
"run",
|
||||
"--project",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"--no-launch-profile"
|
||||
],
|
||||
"cwd": "${workspaceFolder}",
|
||||
"env": {
|
||||
"ASPNETCORE_ENVIRONMENT": "Development",
|
||||
"ASPNETCORE_URLS": "http://localhost:5190"
|
||||
},
|
||||
"stopAtEntry": false,
|
||||
"console": "internalConsole",
|
||||
"preLaunchTask": "build",
|
||||
"serverReadyAction": {
|
||||
"action": "openExternally",
|
||||
"pattern": "\\bNow listening on:\\s+(https?://\\S+)",
|
||||
"uriFormat": "%s"
|
||||
}
|
||||
},
|
||||
{
|
||||
// 热重载调试启动:自动编译、自动重启、断点保留;迭代 Web / 控制器层时首选
|
||||
"name": ".NET Core Watch (Host)",
|
||||
"type": "coreclr",
|
||||
"request": "launch",
|
||||
"program": "dotnet",
|
||||
"args": [
|
||||
"watch",
|
||||
"run",
|
||||
"--project",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"--no-launch-profile"
|
||||
],
|
||||
"cwd": "${workspaceFolder}",
|
||||
"env": {
|
||||
"ASPNETCORE_ENVIRONMENT": "Development",
|
||||
"ASPNETCORE_URLS": "http://localhost:5190"
|
||||
},
|
||||
"stopAtEntry": false,
|
||||
"console": "integratedTerminal",
|
||||
"serverReadyAction": {
|
||||
"action": "openExternally",
|
||||
"pattern": "\\bNow listening on:\\s+(https?://\\S+)",
|
||||
"uriFormat": "%s"
|
||||
}
|
||||
},
|
||||
{
|
||||
// 附加到正在运行的 dotnet 进程(如已手动 `dotnet run` 或 Windows Service 模式)
|
||||
"name": ".NET Core Attach",
|
||||
"type": "coreclr",
|
||||
"request": "attach",
|
||||
"processId": "${command:pickProcess}"
|
||||
}
|
||||
]
|
||||
}
|
||||
160
.vscode/tasks.json
vendored
Normal file
160
.vscode/tasks.json
vendored
Normal file
@@ -0,0 +1,160 @@
|
||||
{
|
||||
// VS Code 任务配置
|
||||
// 文档:https://code.visualstudio.com/docs/editor/tasks
|
||||
"version": "2.0.0",
|
||||
"tasks": [
|
||||
{
|
||||
// 构建整个解决方案,是 launch.json 启动前的默认 preLaunchTask
|
||||
"label": "build",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"build",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln",
|
||||
"/property:GenerateFullPaths=true",
|
||||
"/consoleloggerparameters:NoSummary",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": {
|
||||
"kind": "build",
|
||||
"isDefault": true
|
||||
},
|
||||
"problemMatcher": "$msCompile",
|
||||
"presentation": {
|
||||
"reveal": "silent",
|
||||
"clear": true
|
||||
}
|
||||
},
|
||||
{
|
||||
// 仅构建宿主项目,迭代 Web 层时比整解决方案快
|
||||
"label": "build-host",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"build",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"/property:GenerateFullPaths=true",
|
||||
"/consoleloggerparameters:NoSummary",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": "build",
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 还原 NuGet 包,新增引用或克隆后第一次打开时使用
|
||||
"label": "restore",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"restore",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln"
|
||||
],
|
||||
"problemMatcher": []
|
||||
},
|
||||
{
|
||||
// 清理所有项目的 bin/obj
|
||||
"label": "clean",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"clean",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln"
|
||||
],
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 跑全部测试(领域 + 集成)
|
||||
"label": "test",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"test",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln",
|
||||
"--no-restore",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": {
|
||||
"kind": "test",
|
||||
"isDefault": true
|
||||
},
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 仅跑领域 / 算法层测试,迭代规划逻辑时使用
|
||||
"label": "test-core",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"test",
|
||||
"${workspaceFolder}/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": "test",
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 仅跑宿主集成测试,迭代 HTTP / 控制器层时使用
|
||||
"label": "test-integration",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"test",
|
||||
"${workspaceFolder}/tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": "test",
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 启动宿主,供 launch.json 的 watch 配置作为前置任务
|
||||
"label": "watch",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"watch",
|
||||
"run",
|
||||
"--project",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"--launch-profile",
|
||||
"http"
|
||||
],
|
||||
"isBackground": true,
|
||||
"problemMatcher": {
|
||||
"owner": "dotnet-watch",
|
||||
"pattern": [
|
||||
{
|
||||
"regexp": "^.*$",
|
||||
"file": 1,
|
||||
"location": 2,
|
||||
"message": 3
|
||||
}
|
||||
],
|
||||
"background": {
|
||||
"activeOnStart": true,
|
||||
"beginsPattern": "^.*Watch run started.*$",
|
||||
"endsPattern": "^.*Application started.*$"
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
// Release 配置发布到 publish/,用于现场部署包打包
|
||||
"label": "publish",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"publish",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"-c",
|
||||
"Release",
|
||||
"-o",
|
||||
"${workspaceFolder}/publish"
|
||||
],
|
||||
"problemMatcher": "$msCompile"
|
||||
}
|
||||
]
|
||||
}
|
||||
49
AGENTS.md
49
AGENTS.md
@@ -8,7 +8,7 @@
|
||||
|
||||
- 使用 `C# + .NET 8`
|
||||
- 提供跨平台独立服务端
|
||||
- 兼容现有 `50001/TCP+JSON` 上层接入语义
|
||||
- 以新的 ASP.NET Core HTTP API 作为唯一上层接口
|
||||
- 重写轨迹生成、触发时序、FANUC 控制链路和状态监控
|
||||
- Windows / Linux 都能运行完整服务端
|
||||
- 只支持当前现场这套组合
|
||||
@@ -18,6 +18,7 @@
|
||||
- GUI 桌面程序
|
||||
- 多机器人同时控制
|
||||
- 面向多控制柜的通用平台化框架
|
||||
- 恢复旧 `50001/TCP+JSON` 网关
|
||||
|
||||
## 2. 代码与资料边界
|
||||
|
||||
@@ -37,7 +38,12 @@
|
||||
flyshot-replacement/
|
||||
├─ src/
|
||||
│ ├─ Flyshot.Server.Host/
|
||||
│ ├─ Flyshot.ControllerClientCompat/
|
||||
│ ├─ Flyshot.Core.Config/
|
||||
│ ├─ Flyshot.Core.Domain/
|
||||
│ ├─ Flyshot.Core.Planning/
|
||||
│ ├─ Flyshot.Core.Triggering/
|
||||
│ ├─ Flyshot.Runtime.Fanuc/
|
||||
│ └─ Flyshot.Runtime.Common/
|
||||
├─ tests/
|
||||
│ ├─ Flyshot.Server.IntegrationTests/
|
||||
@@ -64,8 +70,10 @@ flyshot-replacement/
|
||||
- `Flyshot.Core.Triggering`
|
||||
- `TrajectoryDO` 等价时间轴
|
||||
- `shot_flags / offset_values / addr` 解析
|
||||
- `Flyshot.LegacyGateway`
|
||||
- `50001/TCP+JSON` 兼容接入
|
||||
- `Flyshot.ControllerClientCompat`
|
||||
- HTTP 控制器后端兼容服务
|
||||
- 旧 `ControllerClient` 语义适配
|
||||
- 不启动 `50001/TCP+JSON` 监听
|
||||
- `Flyshot.Runtime.Fanuc`
|
||||
- `10010 / 10012 / 60015`
|
||||
- `Flyshot.Web.Status`
|
||||
@@ -84,6 +92,7 @@ flyshot-replacement/
|
||||
|
||||
### 4.2 实现约束
|
||||
|
||||
- 旧 `ControllerClient` 资料只作为接口语义参考;运行时入口以新 HTTP API 为准,不恢复旧 `50001/TCP+JSON` 网关。
|
||||
- 旧协议兼容以“语义兼容”为主,不追求二进制逐字节一致。
|
||||
- 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。
|
||||
- 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。
|
||||
@@ -103,6 +112,15 @@ flyshot-replacement/
|
||||
- 所有静态变量都必须提供 XML 注释。
|
||||
- 关键代码块必须补充单行注释,说明该段逻辑为什么存在、在做什么,不允许只写空泛注释。
|
||||
|
||||
### 4.5 机器人模型字段约定
|
||||
|
||||
- 当任务涉及六轴 `velocity / acceleration / jerk` 来源时,默认先查看机器人模型文件中的 `joint.limit`,不要先从抓包、导出轨迹或聊天记录反推。
|
||||
- 当前仓库约定:`velocity_eff = velocity_base`,`acceleration_eff = acceleration_base * acc_limit`,`jerk_eff = jerk_base * jerk_limit`。
|
||||
- `acc_limit / jerk_limit` 来自运行时 `RobotConfig.json`,它们是全局倍率,不是每轴单独配置。
|
||||
- 模型里的 `limit.effort` 目前只能当静态模型字段记录,不能直接当现场真实电流。
|
||||
- 如果用户问“电流是不是从这个模型文件提取的”,默认先明确区分:模型里的 `effort` 不等于 J519 反馈里的电机电流。
|
||||
- 相关固定表格文档见 `docs/robot-joint-limit-table-20260505.md`。
|
||||
|
||||
## 5. 构建与验证命令
|
||||
|
||||
在当前环境中,推荐使用下面两条命令:
|
||||
@@ -132,9 +150,24 @@ flyshot-replacement/
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/CommonMsg_protocol_analysis.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/FANUC_realtime_comm_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
|
||||
### 6.1 父目录资料引用约定
|
||||
|
||||
- 日常开发、测试和 Codex 会话默认从 `flyshot-replacement/` 根目录启动。
|
||||
- 当前仓库内的 `@` 引用默认只覆盖本仓库文件,不要假设它能索引父目录资料。
|
||||
- 引用父目录资料时,统一直接写明确路径,优先使用相对路径,例如:
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/ControllerServer_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
- 当路径较长或跨工具复制时,可以使用绝对路径,但在文档和注释中优先保留相对路径写法,便于仓库整体搬迁。
|
||||
- 父目录中的 `analysis/`、`FlyingShot/`、`RobotController/`、`RPS/` 默认视为参考资料区,不在这些目录中继续落地新实现。
|
||||
- 新实现、测试、兼容层代码、设计文档和运行说明,都应优先写入 `flyshot-replacement/` 内部。
|
||||
- 如果父目录资料中的某段结论会长期影响本仓库实现,应在本仓库 `docs/` 中补充归纳说明,并标明来源路径,而不是要求后续开发反复回看聊天记录。
|
||||
- 如果需要引用父目录样本文件做测试输入,优先通过只读方式加载;只有在测试需要固化样本且样本已明确收敛时,才复制到本仓库测试数据目录。
|
||||
|
||||
## 7. 任务推进方式
|
||||
|
||||
- `README.md` 中的 Todo 需要随着阶段推进同步更新。
|
||||
@@ -148,3 +181,13 @@ flyshot-replacement/
|
||||
- `Flyshot.Server.Host` 已提供最小 `/healthz`。
|
||||
- 最小集成测试已通过。
|
||||
- 解决方案构建已通过。
|
||||
- 新 HTTP API / HTTP-only `ControllerClientCompat` 已覆盖旧 HTTP 控制器后端的主要兼容语义。
|
||||
- `Flyshot.Core.Planning` 已落地 `icsp` 与 `self-adapt-icsp`,并已完成旧系统导出轨迹对齐。
|
||||
- `Flyshot.Core.Triggering` 已能从 `shot_flags / offset_values / addr` 生成触发时间轴。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码,`10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段,并在状态快照中保留尾部状态字诊断信息。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`,而导出 `JointDetialTraj.txt` 为 `rad`;执行链路必须做单位转换。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸;本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`,不要把速度缩放只建模成单个机器人命令。J519 实发周期仍为 `t_send = k * 0.008`,原轨迹采样时间为 `t_traj = t_send * speed_ratio`,`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲,`io_keep_cycles=2` 对应约两周期清零。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路;稠密轨迹下发已按 `speed_ratio` 推进轨迹时间,J519 闭环状态判断与现场联调仍需补齐。
|
||||
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`,不再只是兼容层内存赋值。
|
||||
|
||||
11
CLAUDE.md
11
CLAUDE.md
@@ -64,8 +64,10 @@ flyshot-replacement/
|
||||
- `Flyshot.Core.Triggering`
|
||||
- `TrajectoryDO` 等价时间轴
|
||||
- `shot_flags / offset_values / addr` 解析
|
||||
- `Flyshot.LegacyGateway`
|
||||
- `50001/TCP+JSON` 兼容接入
|
||||
- `Flyshot.ControllerClientCompat`
|
||||
- HTTP 控制器后端兼容服务
|
||||
- 旧 `ControllerClient` 语义适配
|
||||
- 不启动 `50001/TCP+JSON` 监听
|
||||
- `Flyshot.Runtime.Fanuc`
|
||||
- `10010 / 10012 / 60015`
|
||||
- `Flyshot.Web.Status`
|
||||
@@ -132,6 +134,7 @@ flyshot-replacement/
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/CommonMsg_protocol_analysis.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/FANUC_realtime_comm_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
|
||||
@@ -148,3 +151,7 @@ flyshot-replacement/
|
||||
- `Flyshot.Server.Host` 已提供最小 `/healthz`。
|
||||
- 最小集成测试已通过。
|
||||
- 解决方案构建已通过。
|
||||
- `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`,而导出 `JointDetialTraj.txt` 为 `rad`;执行链路必须做单位转换。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸;本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。J519 实发周期仍为 `t_send = k * 0.008`,原轨迹采样时间为 `t_traj = t_send * speed_ratio`,`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲,`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
BIN
Config/Models/Expand/LR_Mate_200iD_7L_expand.robot
Normal file
BIN
Config/Models/Expand/LR_Mate_200iD_7L_expand.robot
Normal file
Binary file not shown.
BIN
Config/Models/Expand/LR_Mate_200iD_expand.robot
Normal file
BIN
Config/Models/Expand/LR_Mate_200iD_expand.robot
Normal file
Binary file not shown.
BIN
Config/Models/LR_Mate_200iD.robot
Normal file
BIN
Config/Models/LR_Mate_200iD.robot
Normal file
Binary file not shown.
BIN
Config/Models/LR_Mate_200iD_7L -.glb
Normal file
BIN
Config/Models/LR_Mate_200iD_7L -.glb
Normal file
Binary file not shown.
366
Config/Models/LR_Mate_200iD_7L.json
Normal file
366
Config/Models/LR_Mate_200iD_7L.json
Normal file
@@ -0,0 +1,366 @@
|
||||
{
|
||||
"scenes": [
|
||||
{
|
||||
"extras": {
|
||||
"rvbust": {
|
||||
"robotics": {
|
||||
"bodies": [
|
||||
{
|
||||
"active_manipulator_name": "ManipulatorName",
|
||||
"controller_info": {
|
||||
"analog_io": {
|
||||
"imax": 100,
|
||||
"imin": 0,
|
||||
"omax": 100,
|
||||
"omin": 0
|
||||
},
|
||||
"digital_io": {
|
||||
"imax": 100,
|
||||
"imin": 0,
|
||||
"omax": 100,
|
||||
"omin": 0
|
||||
},
|
||||
"name": "R30iB"
|
||||
},
|
||||
"generic_info": {
|
||||
"boundary_area": {
|
||||
"height": 0.8689,
|
||||
"length": 0.8194,
|
||||
"pose": [
|
||||
0.103025,
|
||||
0.0,
|
||||
0.10445,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"type": 2,
|
||||
"width": 0.2349
|
||||
},
|
||||
"materials": [
|
||||
{
|
||||
"color": [
|
||||
0.15,
|
||||
0.15,
|
||||
0.15,
|
||||
1.0
|
||||
],
|
||||
"name": "FANUC_Black",
|
||||
"texture_filename": ""
|
||||
},
|
||||
{
|
||||
"color": [
|
||||
1.0,
|
||||
1.0,
|
||||
1.0,
|
||||
1.0
|
||||
],
|
||||
"name": "FANUC_Generic",
|
||||
"texture_filename": ""
|
||||
},
|
||||
{
|
||||
"color": [
|
||||
0.278,
|
||||
0.278,
|
||||
0.278,
|
||||
1.0
|
||||
],
|
||||
"name": "FANUC_Grey",
|
||||
"texture_filename": ""
|
||||
},
|
||||
{
|
||||
"color": [
|
||||
1.0,
|
||||
1.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"name": "FANUC_Yellow",
|
||||
"texture_filename": ""
|
||||
}
|
||||
],
|
||||
"path_to_image": "./LR_Mate_200iD_7L.png"
|
||||
},
|
||||
"joints": [
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"child": "Link1",
|
||||
"curr_position": 0.0,
|
||||
"home_position": 0.0,
|
||||
"limit": {
|
||||
"acceleration": 26.9,
|
||||
"effort": 0.0,
|
||||
"jerk": 224.22,
|
||||
"lower": -2.96,
|
||||
"upper": 2.96,
|
||||
"velocity": 6.45
|
||||
},
|
||||
"name": "Joint1",
|
||||
"origin": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"parent": "BaseLink",
|
||||
"type": 2
|
||||
},
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0,
|
||||
0.0
|
||||
],
|
||||
"child": "Link2",
|
||||
"curr_position": 0.0,
|
||||
"home_position": 0.0,
|
||||
"limit": {
|
||||
"acceleration": 22.54,
|
||||
"effort": 0.0,
|
||||
"jerk": 187.86,
|
||||
"lower": -1.74,
|
||||
"upper": 2.52,
|
||||
"velocity": 5.41
|
||||
},
|
||||
"name": "Joint2",
|
||||
"origin": [
|
||||
0.05,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"parent": "Link1",
|
||||
"type": 2
|
||||
},
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-1.0,
|
||||
0.0
|
||||
],
|
||||
"child": "Link3",
|
||||
"couple": {
|
||||
"kin_lower": -1.22,
|
||||
"kin_upper": 3.71,
|
||||
"master_joint": "Joint2",
|
||||
"multiplier": 1.0,
|
||||
"offset": 0.0,
|
||||
"poly_boundary": [
|
||||
-1.745,
|
||||
0.524,
|
||||
-1.745,
|
||||
4.886,
|
||||
-1.132,
|
||||
4.832,
|
||||
2.53,
|
||||
1.1677,
|
||||
2.53067,
|
||||
-2.02841,
|
||||
1.3739,
|
||||
-2.574281
|
||||
]
|
||||
},
|
||||
"curr_position": 0.0,
|
||||
"home_position": 0.0,
|
||||
"limit": {
|
||||
"acceleration": 29.81,
|
||||
"effort": 0.0,
|
||||
"jerk": 248.46,
|
||||
"lower": -2.59,
|
||||
"upper": 4.88,
|
||||
"velocity": 7.15
|
||||
},
|
||||
"name": "Joint3",
|
||||
"origin": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.44,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"parent": "Link2",
|
||||
"type": 2
|
||||
},
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"child": "Link4",
|
||||
"curr_position": 0.0,
|
||||
"home_position": 0.0,
|
||||
"limit": {
|
||||
"acceleration": 39.99,
|
||||
"effort": 0.0,
|
||||
"jerk": 333.3,
|
||||
"lower": -3.31,
|
||||
"upper": 3.31,
|
||||
"velocity": 9.59
|
||||
},
|
||||
"name": "Joint4",
|
||||
"origin": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.035,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"parent": "Link3",
|
||||
"type": 2
|
||||
},
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-1.0,
|
||||
0.0
|
||||
],
|
||||
"child": "Link5",
|
||||
"curr_position": -1.5708,
|
||||
"home_position": -1.5708,
|
||||
"limit": {
|
||||
"acceleration": 39.63,
|
||||
"effort": 0.0,
|
||||
"jerk": 330.27,
|
||||
"lower": -2.18,
|
||||
"upper": 2.18,
|
||||
"velocity": 9.51
|
||||
},
|
||||
"name": "Joint5",
|
||||
"origin": [
|
||||
0.42,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"parent": "Link4",
|
||||
"type": 2
|
||||
},
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"child": "Link6",
|
||||
"curr_position": 0.0,
|
||||
"home_position": 0.0,
|
||||
"limit": {
|
||||
"acceleration": 72.72,
|
||||
"effort": 0.0,
|
||||
"jerk": 606.01,
|
||||
"lower": -6.28,
|
||||
"upper": 6.28,
|
||||
"velocity": 17.45
|
||||
},
|
||||
"name": "Joint6",
|
||||
"origin": [
|
||||
0.08,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"parent": "Link5",
|
||||
"type": 2
|
||||
},
|
||||
{
|
||||
"axis": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"child": "EffectorLink",
|
||||
"curr_position": 0.0,
|
||||
"home_position": 0.0,
|
||||
"limit": {
|
||||
"acceleration": 0.0,
|
||||
"effort": 0.0,
|
||||
"jerk": 0.0,
|
||||
"lower": 0.0,
|
||||
"upper": 0.0,
|
||||
"velocity": 0.0
|
||||
},
|
||||
"name": "JointEffector",
|
||||
"origin": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.7071067811865475,
|
||||
0.0,
|
||||
0.7071067811865475,
|
||||
0.0
|
||||
],
|
||||
"parent": "Link6",
|
||||
"type": 1
|
||||
}
|
||||
],
|
||||
"name": "FANUC_LR_Mate_200iD_7L",
|
||||
"other": {
|
||||
"base_transformation": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
},
|
||||
"robot_generic_info": {
|
||||
"payload": 7.0,
|
||||
"reach": 0.911,
|
||||
"repeat": 0.01,
|
||||
"vendor": "FANUC",
|
||||
"weight": 27.0
|
||||
},
|
||||
"rvdf_version": "0.1.0"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
BIN
Config/Models/LR_Mate_200iD_7L.robot
Normal file
BIN
Config/Models/LR_Mate_200iD_7L.robot
Normal file
Binary file not shown.
BIN
Config/Models/workpiece.stl
Normal file
BIN
Config/Models/workpiece.stl
Normal file
Binary file not shown.
304
Config/RobotConfig.json
Normal file
304
Config/RobotConfig.json
Normal file
@@ -0,0 +1,304 @@
|
||||
{
|
||||
"robot": {
|
||||
"use_do": true,
|
||||
"io_addr": [
|
||||
7,
|
||||
8
|
||||
],
|
||||
"io_keep_cycles": 2,
|
||||
"trigger_sample_index_offset_cycles": 6,
|
||||
"acc_limit": 1,
|
||||
"jerk_limit": 1,
|
||||
"adapt_icsp_try_num": 5,
|
||||
"planning_speed_scale": 0.74227,
|
||||
"smooth_start_stop_timing": false
|
||||
},
|
||||
"flying_shots": {
|
||||
"UTTC_MS11": {
|
||||
"traj_waypoints": [
|
||||
[
|
||||
1.056731,
|
||||
0.011664811,
|
||||
-0.017892333,
|
||||
-0.01516874,
|
||||
0.021492079,
|
||||
0.009567846
|
||||
],
|
||||
[
|
||||
0.8532358,
|
||||
0.03837953,
|
||||
-0.19235304,
|
||||
0.0071595116,
|
||||
0.109054826,
|
||||
0.040055145
|
||||
],
|
||||
[
|
||||
0.96600056,
|
||||
0.20607172,
|
||||
-0.12233179,
|
||||
-1.2394339,
|
||||
0.10493033,
|
||||
1.2958988
|
||||
],
|
||||
[
|
||||
0.9618476,
|
||||
0.15288207,
|
||||
-0.14867093,
|
||||
-0.7176314,
|
||||
0.1764264,
|
||||
0.73228663
|
||||
],
|
||||
[
|
||||
0.76189893,
|
||||
-0.028442925,
|
||||
-0.30919823,
|
||||
0.10463613,
|
||||
0.5615024,
|
||||
-0.39399016
|
||||
],
|
||||
[
|
||||
1.1271763,
|
||||
0.074403025,
|
||||
-0.27347943,
|
||||
-0.5227772,
|
||||
0.52098846,
|
||||
0.79633313
|
||||
],
|
||||
[
|
||||
1.0555661,
|
||||
0.4026262,
|
||||
-0.08746306,
|
||||
0.6301835,
|
||||
0.09644133,
|
||||
-0.5463328
|
||||
],
|
||||
[
|
||||
1.2300354,
|
||||
0.28612664,
|
||||
-0.23486805,
|
||||
-0.4868128,
|
||||
0.25369516,
|
||||
0.55347764
|
||||
],
|
||||
[
|
||||
1.2144431,
|
||||
-0.29855102,
|
||||
-0.15202847,
|
||||
-1.0205934,
|
||||
0.13317892,
|
||||
1.1246506
|
||||
],
|
||||
[
|
||||
1.2840607,
|
||||
-0.11222197,
|
||||
-0.16805042,
|
||||
-2.248135,
|
||||
0.2560587,
|
||||
2.4434967
|
||||
],
|
||||
[
|
||||
1.3189346,
|
||||
-0.25620222,
|
||||
-0.12730704,
|
||||
-2.285038,
|
||||
0.30872014,
|
||||
2.4765089
|
||||
],
|
||||
[
|
||||
1.502615,
|
||||
-0.25304365,
|
||||
-0.23878741,
|
||||
-1.2194318,
|
||||
0.46674785,
|
||||
1.5533328
|
||||
],
|
||||
[
|
||||
1.07723,
|
||||
-0.07387611,
|
||||
-0.1707704,
|
||||
-1.8916591,
|
||||
0.38677844,
|
||||
2.061968
|
||||
],
|
||||
[
|
||||
1.3920237,
|
||||
0.08098731,
|
||||
-0.2672306,
|
||||
-0.9780007,
|
||||
0.4561093,
|
||||
0.9102286
|
||||
],
|
||||
[
|
||||
1.9016331,
|
||||
0.023924276,
|
||||
-0.58633333,
|
||||
-0.8441697,
|
||||
0.76730615,
|
||||
1.4842151
|
||||
],
|
||||
[
|
||||
1.9300697,
|
||||
-0.06738541,
|
||||
-0.56542397,
|
||||
-0.892083,
|
||||
0.77194446,
|
||||
1.5293273
|
||||
],
|
||||
[
|
||||
2.0611632,
|
||||
-0.30327517,
|
||||
-0.54225636,
|
||||
-1.0395275,
|
||||
0.8505439,
|
||||
1.6429617
|
||||
],
|
||||
[
|
||||
1.0921186,
|
||||
-0.40034482,
|
||||
-0.1803499,
|
||||
1.3524796,
|
||||
0.6210477,
|
||||
-1.2159473
|
||||
],
|
||||
[
|
||||
1.0521278,
|
||||
-0.40034503,
|
||||
-0.1803492,
|
||||
1.3524843,
|
||||
0.6210471,
|
||||
-1.2159531
|
||||
],
|
||||
[
|
||||
1.056731,
|
||||
0.011664811,
|
||||
-0.017892333,
|
||||
-0.01516874,
|
||||
0.021492079,
|
||||
0.009567846
|
||||
]
|
||||
],
|
||||
"shot_flags": [
|
||||
false,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
false,
|
||||
true,
|
||||
false
|
||||
],
|
||||
"offset_values": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"addr": [
|
||||
[],
|
||||
[
|
||||
2,
|
||||
4
|
||||
],
|
||||
[
|
||||
3,
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
3,
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
3,
|
||||
4
|
||||
],
|
||||
[
|
||||
3,
|
||||
4
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
3
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
2
|
||||
],
|
||||
[
|
||||
4,
|
||||
3
|
||||
],
|
||||
[]
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -23,6 +23,10 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Planning", "sr
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Triggering", "src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj", "{E4DDC34C-9AB6-4050-A927-3DF69804708A}"
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.ControllerClientCompat", "src\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj", "{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}"
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Fanuc", "src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj", "{B705FA6C-19CA-44A8-882C-6CE26A5379C9}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|Any CPU = Debug|Any CPU
|
||||
@@ -129,6 +133,30 @@ Global
|
||||
{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
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x64.ActiveCfg = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x64.Build.0 = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x86.ActiveCfg = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x86.Build.0 = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|Any CPU.Build.0 = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x64.ActiveCfg = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x64.Build.0 = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x86.ActiveCfg = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x86.Build.0 = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x64.ActiveCfg = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x64.Build.0 = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x86.ActiveCfg = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x86.Build.0 = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|Any CPU.Build.0 = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.ActiveCfg = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.Build.0 = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.ActiveCfg = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.Build.0 = Release|Any CPU
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
@@ -142,5 +170,7 @@ Global
|
||||
{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}
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
|
||||
41
NLog.config
Normal file
41
NLog.config
Normal file
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0" encoding="utf-8" ?>
|
||||
<nlog xmlns="http://www.nlog-project.org/schemas/NLog.xsd"
|
||||
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
|
||||
xsi:schemaLocation="http://www.nlog-project.org/schemas/NLog.xsd NLog.xsd"
|
||||
autoReload="true"
|
||||
throwExceptions="false"
|
||||
internalLogLevel="Off" >
|
||||
|
||||
<!-- 环境变量配置:如果 ASPNETCORE_ENVIRONMENT 为空,则默认为 Production -->
|
||||
<variable name="env" value="${environment:ASPNETCORE_ENVIRONMENT:whenEmpty=Production}"/>
|
||||
|
||||
<!-- 文件目标:按日期分文件,单文件超过 4MB 自动归档,保留最近 50 个归档文件 -->
|
||||
<targets>
|
||||
<target name="logfile" xsi:type="File"
|
||||
fileName="${basedir}/logs/${shortdate}.log"
|
||||
layout="${longdate}|${level:uppercase=true}|${threadid}|${logger}|${message}${onexception:${newline} ${exception:format=tostring}}"
|
||||
archiveFileName="${basedir}/logs/${shortdate}.{#}.log"
|
||||
archiveAboveSize="4048576"
|
||||
archiveNumbering="Sequence"
|
||||
maxArchiveFiles="50"
|
||||
concurrentWrites="true"
|
||||
keepFileOpen="false"
|
||||
encoding="utf-8" />
|
||||
|
||||
<!-- 控制台目标:开发环境使用,带颜色高亮 -->
|
||||
<target name="logconsole" xsi:type="Console"
|
||||
layout="${longdate}|${level:uppercase=true}|${threadid}|${logger}|${message}${onexception:${newline} ${exception:format=tostring}}" />
|
||||
</targets>
|
||||
|
||||
<rules>
|
||||
<!-- 压制 ASP.NET Core 的常规信息日志,只保留 Error 及以上级别。 -->
|
||||
<logger name="Microsoft.AspNetCore.*" maxlevel="Warn" final="true" />
|
||||
<logger name="Microsoft.AspNetCore.*" minlevel="Error" writeTo="logconsole,logfile" />
|
||||
|
||||
<!-- 开发环境:显示控制台 + 详细文件,最低 Debug -->
|
||||
<logger name="*" minlevel="Debug" writeTo="logconsole,logfile" condition="equals('${var:env}','Development')" />
|
||||
|
||||
<!-- 生产环境:仅文件,最低 Info -->
|
||||
<logger name="*" minlevel="Info" writeTo="logfile" condition="not_equals('${var:env}','Development')" />
|
||||
</rules>
|
||||
</nlog>
|
||||
106
README.md
106
README.md
@@ -4,7 +4,7 @@
|
||||
|
||||
当前目标:
|
||||
|
||||
- 兼容现有 `50001/TCP+JSON` 上层接入语义
|
||||
- 以新的 ASP.NET Core HTTP API 作为唯一上层接口
|
||||
- 重写轨迹生成、触发时序和 FANUC 实时控制链路
|
||||
- 提供 Web 状态监控页面
|
||||
- 在 Windows 和 Linux 上运行完整后台服务
|
||||
@@ -13,12 +13,112 @@
|
||||
|
||||
- 这是长期运行的无头后台服务,不是 GUI 桌面程序。
|
||||
- 第一版仅面向当前现场组合,后续再扩展机型与控制柜适配。
|
||||
- 当前仓库不再恢复旧 `50001/TCP+JSON` 监听入口;旧 `ControllerClient` 逆向资料只作为接口语义参考,不作为运行时目标。
|
||||
- 宿主只保留 ASP.NET Core HTTP 控制器层,以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。
|
||||
- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路;Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。
|
||||
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐;`doubles` 仍未实现。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。
|
||||
- 2026-04-28 UTTC 抓包确认:UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`,`JointDetialTraj.txt` 为弧度制 `rad`,`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸;2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长,当前实际生成约 `7.4s` 轨迹。
|
||||
- 2026-04-30 本机 `50001/TCP+JSON` 抓包确认:`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数,不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路,新系统按 replacement-only 内部参数限制规划加速度。
|
||||
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送能力;稠密轨迹下发已按 `speed_ratio` 做执行时间缩放,并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。
|
||||
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹:当前关节到目标关节的关节空间直线,五次 smoothstep 起停,按 `status=15` 运动窗口复现 `40/55/77` 点,并由 J519 层完成 `rad -> deg` 下发。
|
||||
- 单程序只对应一台机器人,上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`,不再创建独立轨迹存储文件。
|
||||
|
||||
当前 Todo:
|
||||
单位约定总览:
|
||||
|
||||
- 规划层、`JointDetialTraj.txt` 和运行时内部关节轨迹,默认按弧度制 `rad` 理解。
|
||||
- `UDP 60015` J519 命令 `target[0..5]` 和响应关节反馈按角度制 `deg` 理解;运行时下发前必须显式执行 `rad -> deg` 转换。
|
||||
- `TCP 10010` 状态通道是混合单位:`pose[0..2]` 更像 `mm`,`pose[3..5]` 更像 `deg`,`joint_or_ext[0..5]` 当前现场抓包更支持按 `rad` 理解。
|
||||
- 不要把“关节角”默认当成统一单位;在规划、状态监控和 J519 执行三条链路之间必须明确标注 `rad/deg`。
|
||||
|
||||
当前现场主链路的单位流转可简化为:
|
||||
|
||||
| 位置 | 内容 | 当前更可信单位 |
|
||||
| --- | --- | --- |
|
||||
| 规划输入 / 轨迹算法 | 关节角 | `rad` |
|
||||
| `JointDetialTraj.txt` / `JointTraj.txt` | 关节角 | `rad` |
|
||||
| 运行时下发前内部轨迹 | 关节角 | `rad` |
|
||||
| `UDP 60015` 命令 `target[0..5]` | 关节目标 | `deg` |
|
||||
| `UDP 60015` 响应 `Joint` | 关节反馈 | `deg` |
|
||||
| `TCP 10010` `pose[0..2]` | `X/Y/Z` | `mm` |
|
||||
| `TCP 10010` `pose[3..5]` | 姿态角 | `deg` |
|
||||
| `TCP 10010` `joint_or_ext[0..5]` | 关节状态 | 更像 `rad` |
|
||||
| `TCP 10010` `joint_or_ext[6..8]` | 扩展轴槽位 | 当前样本为 `0` |
|
||||
|
||||
`TCP 10010` 的正式字段表、样例帧和已确认/待确认说明见 `docs/fanuc-10010-state-frame.md`。
|
||||
|
||||
开发约定:
|
||||
|
||||
- 建议从 `flyshot-replacement/` 根目录启动 IDE、终端和 Codex 会话。
|
||||
- 当前仓库内的 `@` 引用主要覆盖本仓库文件;引用父目录资料时,请直接写相对路径,如 `../analysis/ICSP_algorithm_reverse_analysis.md`。
|
||||
- 父目录中的 `analysis/`、`FlyingShot/`、`RobotController/`、`RPS/` 主要作为逆向参考资料和样本来源,新实现默认只落地在当前仓库。
|
||||
|
||||
当前已完成:
|
||||
|
||||
- [x] 初始化独立仓库
|
||||
- [x] 创建 `dotnet 8` 解决方案骨架
|
||||
- [x] 打通最小宿主与 `/healthz`
|
||||
- [x] 建立领域模型与模块边界
|
||||
- [x] 落地配置兼容与机器人模型解析
|
||||
- [ ] 落地轨迹规划、实时控制和 Web 状态页
|
||||
- [x] 落地 ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴
|
||||
- [x] 完成 ICSP 轨迹导出结果与旧系统对齐
|
||||
- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路
|
||||
- [x] 落地 Web 状态页
|
||||
- [x] 落地浏览器内 OpenAPI 自动驱动的接口调试页(`/debug`),与 `Swagger:Enabled` 同步可见
|
||||
- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码,确认 `10010` 状态帧为 90B
|
||||
- [x] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发
|
||||
- [x] 补齐 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 真机命令体与响应解析
|
||||
- [x] 保留新 HTTP 接口路线,明确不再实现旧 `50001/TCP+JSON` 网关
|
||||
- [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.json`
|
||||
|
||||
剩余 Todo:
|
||||
|
||||
1. 配置与测试基线
|
||||
- [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移:`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`,应改用稳定样本或更新断言。
|
||||
- [x] 将 `RobotConfig.json` 中的 `use_do`、`io_keep_cycles`、`acc_limit`、`jerk_limit`、`adapt_icsp_try_num` 全部贯通到规划和执行链路。
|
||||
- [x] 将上传飞拍轨迹统一保存到运行目录 `Config/RobotConfig.json` 的 `flying_shots` 节点。
|
||||
- [x] 为新 HTTP API 补一份当前现场调用顺序文档,替代旧 `ControllerClient` 工作流:见 `docs/fanuc-field-runtime-workflow.md`。
|
||||
|
||||
2. 轨迹规划
|
||||
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定,避免未收敛轨迹被当作有效结果执行。
|
||||
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
||||
- [ ] 新增 replacement-only 的 `planning_acceleration_scale` 规划加速度校准参数,用于复现旧服务端公开链路中抓不到的保守 effective limits;该参数只影响规划结果,不影响运行时 `speed_ratio`。
|
||||
- [ ] 如果现场仍需要 `method="doubles"`,实现 `TrajectoryDoubleS` 等价规划;否则在 HTTP 文档中明确标为不支持。
|
||||
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests,防止后续重构破坏轨迹一致性。
|
||||
- [x] 将 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本:输入为 `rad`,下发为 `deg`,并按 `speed_ratio` 拉伸时间轴;覆盖 `2026042802-0.5/0.7/1.pcap`。
|
||||
- [x] 补齐飞拍 `save_traj` / `SaveTrajInfo` 的规划结果导出,将关节关键点、稠密关节轨迹、笛卡尔关键点、稠密笛卡尔轨迹和 ShotEvents 写入 `Config/Data/<name>`。
|
||||
|
||||
3. FANUC TCP 10012 命令通道
|
||||
- [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetTCP` / `SetTCP` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetIO` / `SetIO` 真机命令体与响应解析。
|
||||
- [x] 所有命令响应必须检查 `result_code`,失败时返回可诊断错误,而不是只更新本地缓存。
|
||||
|
||||
4. FANUC TCP 10010 状态通道
|
||||
- [x] 用 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义,并映射到 `ControllerStateSnapshot`。
|
||||
- [x] 补充 `TCP 10010` 正式字段表与已确认/待确认说明:见 `docs/fanuc-10010-state-frame.md`。
|
||||
- [x] 补充断线清理和异常帧拒绝测试。
|
||||
- [x] 补充状态通道超时和重连策略,超时后标记陈旧状态并按退避策略自动重连。
|
||||
|
||||
5. FANUC UDP 60015 J519 运动链路
|
||||
- [x] 重新确认 J519 发送节拍与 `FanucControllerRuntime` 稠密轨迹循环的职责边界:`FanucJ519Client` 收到机器人 UDP status 后按该 status sequence 回发命令,`FanucControllerRuntime` 只按轨迹时间更新下一帧命令内容。
|
||||
- [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标,并按当前 `speed_ratio` 调整原轨迹采样时间尺度:第 `k` 个 J519 目标的实发时间为 `t_send = k * 0.008`,采样时间为 `t_traj = t_send * speed_ratio`,包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- [x] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位解析与启动前闭环检查;若已有 J519 响应且 `accept_cmd/sysrdy` 未就绪,则拒绝稠密轨迹执行。
|
||||
- [x] 校验序号递增、状态包 sequence 校准、响应滞后、丢包、停止包和最后一帧语义:UTTC golden tests 覆盖连续 seq、无重复 seq、响应滞后 2 到 8 帧、`lastData=0`;J519 客户端测试覆盖收到 status 后按 status sequence 回发命令和 type 2 状态输出停止包。
|
||||
- [x] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐;UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧,mask 集合为 `10/12/14`。
|
||||
- [x] 将 `MoveJoint` 从单点最终目标改为临时 PTP 稠密轨迹:按 `status=15` 运动窗口统计,speed=1 抓包 40 点,speed=0.7 抓包 55 点,speed=0.5 抓包 77 点,路径为关节空间直线 + smoothstep 进度。
|
||||
- [x] `ExecuteFlyShotTraj(move_to_start=true)` 复用临时 PTP 稠密轨迹移动到规划起点,并等待运行时完成后再启动飞拍轨迹,避免第一帧 J519 目标突变导致控制柜报警。
|
||||
- [x] `ExecuteFlyShotTraj(wait=true)` 等待正式飞拍轨迹执行完成后再返回;HTTP `/execute_flyshot/` 已接入旧抓包中的 `wait` 字段,默认值为 `true`。
|
||||
|
||||
6. 真机联调与运行安全
|
||||
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。
|
||||
- [x] 实体机复核运行速度对轨迹生成时间的影响:`speed_ratio` 不影响 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 生成的 `JointTraj.txt` 规划时长,当前实际生成约 `7.4s` 轨迹;运行阶段仅 J519 下发时长和包数按 `speed_ratio` 拉伸,UTTC_MS11 参考值为约 `7.4s` 与 `10.56s`。
|
||||
- [ ] 增加急停、伺服未就绪、程序未启动、网络断开、控制柜拒收命令等故障路径处理。
|
||||
- [ ] 给 HTTP 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。
|
||||
- [ ] 增加运行日志、协议摘要日志和状态快照导出,便于现场排查。
|
||||
|
||||
7. 发布与部署
|
||||
- [ ] 固化 Windows / Linux 启动脚本和 systemd 服务配置。
|
||||
- [ ] 补充生产配置模板、端口说明和现场部署检查表。
|
||||
- [ ] 给 Web 状态页增加程序状态和最近报警显示;J519 状态位已通过快照和状态页显示。
|
||||
|
||||
21
analysis/2026042802-1/2026042802-1_analysis_summary.json
Normal file
21
analysis/2026042802-1/2026042802-1_analysis_summary.json
Normal file
@@ -0,0 +1,21 @@
|
||||
{
|
||||
"pcap_path": "D:\\Dev\\Codes\\rvbust-code\\FlyingShotPkg_3.15_VDA\\Rvbust\\uttc-20260428\\2026042802-1.pcap",
|
||||
"all_command_count": 1788,
|
||||
"trigger_count": 17,
|
||||
"existing_runtime_actual_send_exists": true,
|
||||
"existing_runtime_actual_send_has_io_columns": false,
|
||||
"existing_shot_events_exists": true,
|
||||
"pcap_specific_combined_export_preexisting": false,
|
||||
"average_max_error_deg": 4.241583591714439,
|
||||
"max_error_deg": 11.130744251720401,
|
||||
"average_rms_error_deg": 2.5406136706026206,
|
||||
"max_error_axis_counter": {
|
||||
"J6": 9,
|
||||
"J4": 5,
|
||||
"J5": 1,
|
||||
"J2": 1,
|
||||
"J1": 1
|
||||
},
|
||||
"order_only_addr_mismatch_count": 14,
|
||||
"real_addr_mismatch_count": 0
|
||||
}
|
||||
1789
analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv
Normal file
1789
analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv
Normal file
File diff suppressed because it is too large
Load Diff
1790
analysis/2026042802-1/2026042802-1_j519_status_feedback_all.csv
Normal file
1790
analysis/2026042802-1/2026042802-1_j519_status_feedback_all.csv
Normal file
File diff suppressed because it is too large
Load Diff
18
analysis/2026042802-1/2026042802-1_j519_trigger_frames.csv
Normal file
18
analysis/2026042802-1/2026042802-1_j519_trigger_frames.csv
Normal file
@@ -0,0 +1,18 @@
|
||||
frame_number,time_relative_s,sequence,last_data,write_io_type,write_io_index,write_io_mask,write_io_value,io_addrs,j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,ext1_deg,ext2_deg,ext3_deg
|
||||
1955,5.888271,1381124,0,2,1,10,10,"[2, 4]",45.8632698059082,2.9724788665771484,-13.609341621398926,-2.1851141452789307,7.582361221313477,5.701517581939697,0.0,0.0,0.0
|
||||
2151,6.480252,1381198,0,2,1,14,14,"[2, 3, 4]",56.34336471557617,11.66723918914795,-6.600923538208008,-68.1976089477539,6.058437347412109,71.5392837524414,0.0,0.0,0.0
|
||||
2223,6.704225,1381226,0,2,1,14,14,"[2, 3, 4]",53.50831985473633,7.412222862243652,-9.629739761352539,-32.30571365356445,12.488306045532227,30.826189041137695,0.0,0.0,0.0
|
||||
2336,7.048266,1381269,0,2,1,10,10,"[2, 4]",44.433006286621094,-2.1156294345855713,-18.39777374267578,4.8371992111206055,33.99372482299805,-20.26228904724121,0.0,0.0,0.0
|
||||
2477,7.464289,1381321,0,2,1,10,10,"[2, 4]",65.84967041015625,5.695003032684326,-14.563636779785156,-28.69342613220215,27.678611755371094,46.437156677246094,0.0,0.0,0.0
|
||||
2665,8.024277,1381391,0,2,1,12,12,"[3, 4]",60.847530364990234,24.47777557373047,-5.617043972015381,34.8044319152832,5.786255359649658,-31.2253475189209,0.0,0.0,0.0
|
||||
2811,8.472317,1381447,0,2,1,12,12,"[3, 4]",70.8250503540039,10.95259952545166,-13.136046409606934,-34.68459701538086,14.022420883178711,38.90604019165039,0.0,0.0,0.0
|
||||
2908,8.760361,1381483,0,2,1,10,10,"[2, 4]",69.40516662597656,-20.066781997680664,-8.152935981750488,-63.93235397338867,6.775912284851074,70.37761688232422,0.0,0.0,0.0
|
||||
3112,9.384332,1381561,0,2,1,10,10,"[2, 4]",73.90592956542969,-8.016124725341797,-9.002426147460938,-131.3327178955078,15.526206970214844,142.41668701171875,0.0,0.0,0.0
|
||||
3170,9.560358,1381583,0,2,1,10,10,"[2, 4]",76.72688293457031,-16.518726348876953,-7.15147066116333,-127.53003692626953,18.53788948059082,138.7897491455078,0.0,0.0,0.0
|
||||
3350,10.104322,1381651,0,2,1,10,10,"[2, 4]",83.71662139892578,-13.186573028564453,-13.738808631896973,-72.70317840576172,26.66758918762207,91.98423767089844,0.0,0.0,0.0
|
||||
3470,10.472333,1381697,0,2,1,10,10,"[2, 4]",59.78955841064453,-2.7094168663024902,-8.933828353881836,-108.24857330322266,21.177492141723633,114.14110565185547,0.0,0.0,0.0
|
||||
3627,10.944371,1381756,0,2,1,12,12,"[3, 4]",86.41329193115234,4.266173362731934,-19.15987777709961,-52.24028015136719,29.853723526000977,55.86907196044922,0.0,0.0,0.0
|
||||
3710,11.192339,1381787,0,2,1,10,10,"[2, 4]",110.7523193359375,0.8751887679100037,-34.756534576416016,-48.14332580566406,45.0865592956543,87.97444152832031,0.0,0.0,0.0
|
||||
3796,11.456361,1381820,0,2,1,10,10,"[2, 4]",111.79177856445312,-6.225440502166748,-31.958988189697266,-53.68573760986328,44.76150894165039,89.33454895019531,0.0,0.0,0.0
|
||||
3870,11.680362,1381848,0,2,1,10,10,"[2, 4]",117.87474060058594,-19.607177734375,-30.329103469848633,-56.600250244140625,49.08655548095703,90.32424926757812,0.0,0.0,0.0
|
||||
4138,12.496365,1381950,0,2,1,12,12,"[3, 4]",59.93589401245117,-21.102140426635742,-9.684239387512207,70.64311981201172,32.941307067871094,-63.34855651855469,0.0,0.0,0.0
|
||||
|
@@ -0,0 +1,37 @@
|
||||
{
|
||||
"pcap_path": "D:\\Dev\\Codes\\rvbust-code\\FlyingShotPkg_3.15_VDA\\Rvbust\\uttc-20260428\\2026042802-1.pcap",
|
||||
"command_count": 1788,
|
||||
"trigger_count": 17,
|
||||
"command_minus_paired_status_sequence_counter": {
|
||||
"8": 17
|
||||
},
|
||||
"paired_status_average_max_error_deg": 6.469219290127501,
|
||||
"paired_status_max_error_deg": 16.123934474475888,
|
||||
"paired_status_max_error_axis_counter": {
|
||||
"J1": 2,
|
||||
"J6": 9,
|
||||
"J2": 4,
|
||||
"J4": 2
|
||||
},
|
||||
"best_status_average_max_error_deg": 0.14953970473465206,
|
||||
"best_status_max_error_deg": 0.5696919293327056,
|
||||
"best_status_max_error_axis_counter": {
|
||||
"J6": 10,
|
||||
"J1": 3,
|
||||
"J5": 1,
|
||||
"J2": 2,
|
||||
"J4": 1
|
||||
},
|
||||
"best_status_delta_from_paired_cycles_counter": {
|
||||
"10": 8,
|
||||
"9": 9
|
||||
},
|
||||
"best_status_delta_from_trigger_sequence_counter": {
|
||||
"2": 8,
|
||||
"1": 9
|
||||
},
|
||||
"best_status_time_after_trigger_ms_min": 71.83799999999962,
|
||||
"best_status_time_after_trigger_ms_max": 79.96399999999859,
|
||||
"best_status_time_after_trigger_ms_avg": 75.6613529411763,
|
||||
"search_window_cycles": 20
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
trigger_no,waypoint_index,trigger_command_sequence,trigger_command_frame,trigger_command_time_relative_s,trigger_current_status_sequence,trigger_current_status_frame,trigger_current_status_time_relative_s,command_leads_status_cycles,trigger_current_status_max_error_axis,trigger_current_status_max_error_deg,trigger_current_status_rms_error_deg,best_status_sequence,best_status_frame,best_status_time_relative_s,best_status_delay_from_current_status_cycles,best_status_delay_from_trigger_command_cycles,best_status_delay_from_trigger_command_ms,best_status_max_error_axis,best_status_max_error_deg,best_status_rms_error_deg
|
||||
1,1,1381124,1955,5.888271,1381116,1954,5.888166,8,J1,6.588241610414407,3.8225443630072706,1381126,1981,5.968124,10,2,79.85300000000083,J6,0.20758988641565335,0.12239685259209541
|
||||
2,2,1381198,2151,6.480252,1381190,2150,6.480138,8,J1,2.33531536749085,1.3827543088729335,1381199,2172,6.552164,9,1,71.9120000000002,J1,0.08213390997131853,0.04616054814443023
|
||||
3,3,1381226,2223,6.704225,1381218,2222,6.704166,8,J6,16.123934474475888,8.852376616648156,1381228,2248,6.784154,10,2,79.92899999999992,J6,0.5696919293327056,0.3239642953120798
|
||||
4,4,1381269,2336,7.048266,1381261,2335,7.048148,8,J6,4.901882807288516,3.2440556235708993,1381271,2362,7.128175,10,2,79.90899999999979,J6,0.09470430997406254,0.06580474324148583
|
||||
5,5,1381321,2477,7.464289,1381313,2476,7.464178,8,J6,9.21943820204428,4.621321121191458,1381323,2502,7.544122,10,2,79.83299999999983,J5,0.056622412878706285,0.03357478517689402
|
||||
6,6,1381391,2665,8.024277,1381383,2664,8.024156,8,J6,6.1464605154096255,3.0478405356855083,1381392,2687,8.096181,9,1,71.90399999999997,J6,0.17697427883735983,0.08563125303054821
|
||||
7,7,1381447,2811,8.472317,1381439,2810,8.472208,8,J6,13.15171783052326,7.78202379046887,1381448,2834,8.54419,9,1,71.87300000000008,J6,0.30658691011310424,0.18210669390591905
|
||||
8,8,1381483,2908,8.760361,1381475,2907,8.760245,8,J6,7.436279407197546,5.127041386597523,1381484,2929,8.832199,9,1,71.83799999999962,J2,0.06074767130727565,0.02750705896232285
|
||||
9,9,1381561,3112,9.384332,1381553,3111,9.384204,8,J6,5.585605293050492,3.277206017832995,1381562,3134,9.456205,9,1,71.87300000000008,J4,0.15225994654176134,0.089150370994325
|
||||
10,10,1381583,3170,9.560358,1381575,3169,9.56025,8,J2,4.165077087081428,2.128983861449047,1381585,3196,9.640214,10,2,79.85599999999948,J6,0.0734395372110157,0.043285948566531826
|
||||
11,11,1381651,3350,10.104322,1381643,3349,10.104215,8,J2,1.8309533977318093,1.169340011162825,1381652,3373,10.17622,9,1,71.8980000000009,J6,0.07766831894582538,0.04246508921571141
|
||||
12,12,1381697,3470,10.472333,1381689,3469,10.472191,8,J4,4.822353276363401,2.8611252897135864,1381698,3493,10.544283,9,1,71.94999999999929,J1,0.021077280093038553,0.014935517911187784
|
||||
13,13,1381756,3627,10.944371,1381748,3626,10.944251,8,J4,8.152507963730145,5.168659196117862,1381757,3651,11.016226,9,1,71.85499999999934,J1,0.14554304541260876,0.0699602863618738
|
||||
14,14,1381787,3710,11.192339,1381779,3709,11.19225,8,J6,10.40441920064211,5.679810575214033,1381789,3735,11.272261,10,2,79.92199999999983,J6,0.11945317484617135,0.05835950688086092
|
||||
15,15,1381820,3796,11.456361,1381812,3795,11.456253,8,J2,2.6161077284098373,1.5911754635072362,1381822,3821,11.536287,10,2,79.92600000000039,J2,0.14980253942005772,0.08716897465096986
|
||||
16,16,1381848,3870,11.680362,1381840,3869,11.680263,8,J2,4.682473812334727,2.2067399623329567,1381850,3895,11.760326,10,2,79.96399999999859,J6,0.17295643126782068,0.10419421346699378
|
||||
17,18,1381950,4138,12.496365,1381942,4137,12.496271,8,J6,1.8139599579791934,1.1536991630766555,1381951,4159,12.568313,9,1,71.94799999999901,J6,0.07492339792059965,0.03834497543010648
|
||||
|
@@ -0,0 +1,18 @@
|
||||
trigger_no,waypoint_index,best_sample_order,best_frame_number,best_sequence,best_time_relative_s,teach_j1_deg,teach_j2_deg,teach_j3_deg,teach_j4_deg,teach_j5_deg,teach_j6_deg,offset_6_frame_number,offset_6_sequence,offset_6_time_relative_s,offset_6_max_error_axis,offset_6_max_error_deg,offset_6_rms_error_deg,offset_6_delta_from_best_ms,offset_6_j1_actual_deg,offset_6_diff_j1_deg,offset_6_j2_actual_deg,offset_6_diff_j2_deg,offset_6_j3_actual_deg,offset_6_diff_j3_deg,offset_6_j4_actual_deg,offset_6_diff_j4_deg,offset_6_j5_actual_deg,offset_6_diff_j5_deg,offset_6_j6_actual_deg,offset_6_diff_j6_deg,offset_7_frame_number,offset_7_sequence,offset_7_time_relative_s,offset_7_max_error_axis,offset_7_max_error_deg,offset_7_rms_error_deg,offset_7_delta_from_best_ms,offset_7_j1_actual_deg,offset_7_diff_j1_deg,offset_7_j2_actual_deg,offset_7_diff_j2_deg,offset_7_j3_actual_deg,offset_7_diff_j3_deg,offset_7_j4_actual_deg,offset_7_diff_j4_deg,offset_7_j5_actual_deg,offset_7_diff_j5_deg,offset_7_j6_actual_deg,offset_7_diff_j6_deg,offset_8_frame_number,offset_8_sequence,offset_8_time_relative_s,offset_8_max_error_axis,offset_8_max_error_deg,offset_8_rms_error_deg,offset_8_delta_from_best_ms,offset_8_j1_actual_deg,offset_8_diff_j1_deg,offset_8_j2_actual_deg,offset_8_diff_j2_deg,offset_8_j3_actual_deg,offset_8_diff_j3_deg,offset_8_j4_actual_deg,offset_8_diff_j4_deg,offset_8_j5_actual_deg,offset_8_diff_j5_deg,offset_8_j6_actual_deg,offset_8_diff_j6_deg,best_of_6_7_8_offset
|
||||
1,1,730,1941,1381118,5.840207,48.886810269468405,2.1989850886957285,-11.021017368511105,0.4102097980549552,6.248381265333557,2.294990756284542,1955,1381124,5.888271,J6,3.4065268256551553,2.468438977201345,48.06399999999922,45.8632698059082,-3.0235404635602023,2.9724788665771484,0.7734937778814199,-13.609341621398926,-2.588324252887821,-2.1851141452789307,-2.5953239433338857,7.582361221313477,1.3339799559799195,5.701517581939697,3.4065268256551553,1958,1381125,5.896184,J6,4.180931828249149,2.9355776062802286,55.9769999999995,45.449710845947266,-3.43709942352114,3.117816925048828,0.9188318363530996,-13.963269233703613,-2.9422518651925085,-2.845515012741089,-3.255724810796044,7.768581867218018,1.5202006018844605,6.475922584533691,4.180931828249149,1961,1381126,5.904272,J6,5.0045521476156045,3.418446646752538,64.06499999999937,45.06731414794922,-3.8194961215191867,3.2660434246063232,1.0670583359105947,-14.290565490722656,-3.2695481222115514,-3.562589168548584,-3.972798966603539,7.9421586990356445,1.6937774337020874,7.2995429039001465,5.0045521476156045,6
|
||||
2,2,804,2135,1381192,6.432175,55.34775509527405,11.807039833001637,-7.0090952672806885,-71.01433145543973,6.012065051914967,74.24953191606797,2151,1381198,6.480252,J4,2.8167225076858244,1.6562461612538633,48.07700000000015,56.34336471557617,0.995609620302119,11.66723918914795,-0.13980064385368784,-6.600923538208008,0.40817172907268073,-68.1976089477539,2.8167225076858244,6.058437347412109,0.04637229549714217,71.5392837524414,-2.7102481636265594,2153,1381199,6.488246,J4,3.526759739131137,2.060948429836345,56.071000000000204,56.451515197753906,1.1037601024798533,11.618118286132812,-0.18892154686882456,-6.580313682556152,0.4287815847245362,-67.4875717163086,3.526759739131137,6.09804105758667,0.08597600567170272,70.84339141845703,-3.4061404976109344,2155,1381200,6.496271,J4,4.301478977412387,2.5033932349312007,64.09600000000015,56.54535675048828,1.1976016552142283,11.561552047729492,-0.24548778527214488,-6.571277618408203,0.4378176488724854,-66.71285247802734,4.301478977412387,6.147788047790527,0.13572299587556014,70.078857421875,-4.170674494192966,6
|
||||
3,3,832,2207,1381220,6.656264,55.109808014787404,8.759497374223619,-8.518216825284897,-41.11725046606459,10.108488114686867,41.956933292858096,2223,1381226,6.704225,J6,11.130744251720401,5.955607695749779,47.96099999999992,53.50831985473633,-1.6014881600510762,7.412222862243652,-1.3472745119799665,-9.629739761352539,-1.111522936067642,-32.30571365356445,8.811536812500137,12.488306045532227,2.379817930845359,30.826189041137695,-11.130744251720401,2227,1381227,6.712268,J6,13.165517124645206,7.03533795629365,56.00399999999972,53.18336486816406,-1.9264431466233418,7.154932975769043,-1.6045643984545759,-9.842713356018066,-1.3244965307331693,-30.750762939453125,10.366487526611465,12.961709976196289,2.8532218615094216,28.79141616821289,-13.165517124645206,2229,1381228,6.720286,J6,15.212756428234073,8.119717107623766,64.02199999999958,52.84844207763672,-2.2613659371506856,6.892857074737549,-1.86664029948607,-10.059968948364258,-1.5417521230793607,-29.200178146362305,11.917072319702285,13.449018478393555,3.340530363706687,26.744176864624023,-15.212756428234073,6
|
||||
4,4,875,2321,1381263,7.000291,43.65359310453334,-1.629659559507137,-17.715753611915318,5.995208633582219,32.17171770646655,-22.573973337684023,2336,1381269,7.048266,J6,2.3116842904428125,1.3731583485126422,47.9750000000001,44.433006286621094,0.779413182087751,-2.1156294345855713,-0.48596987507843425,-18.39777374267578,-0.6820201307604634,4.8371992111206055,-1.158009422461613,33.99372482299805,1.8220071165314948,-20.26228904724121,2.3116842904428125,2339,1381270,7.056293,J6,3.083185831580508,1.7437179916275436,56.00200000000033,44.65972137451172,1.006128269978376,-2.1617355346679688,-0.5320759751608317,-18.490602493286133,-0.774848881370815,4.406780242919922,-1.5884283906622967,34.24979019165039,2.0780724851838386,-19.490787506103516,3.083185831580508,2341,1381271,7.064278,J6,3.9520155587533594,2.15807761350517,63.987000000000016,44.91068649291992,1.2570933883865791,-2.196718215942383,-0.5670586564352458,-18.575654983520508,-0.85990137160519,3.9164717197418213,-2.0787369138403973,34.48752212524414,2.3158044187775886,-18.621957778930664,3.9520155587533594,6
|
||||
5,5,927,2457,1381315,7.416267,64.58244475717193,4.262979315506351,-15.669217122643433,-29.952927185666542,29.850439933020308,45.62652743544272,2477,1381321,7.464289,J5,2.171828177649214,1.404704060246151,48.021999999999565,65.84967041015625,1.2672256529843224,5.695003032684326,1.4320237171779748,-14.563636779785156,1.1055803428582767,-28.69342613220215,1.259501053464394,27.678611755371094,-2.171828177649214,46.437156677246094,0.810629241803376,2479,1381322,7.472265,J5,2.585320380408003,1.6670103454750829,55.99799999999977,66.01153564453125,1.4290908873593224,5.957343578338623,1.6943642628322717,-14.353693008422852,1.3155241142205814,-28.232824325561523,1.720102860105019,27.265119552612305,-2.585320380408003,46.24872589111328,0.6221984556705635,2483,1381323,7.480284,J5,3.0049218208376907,1.9507458917658702,64.01699999999977,66.15470886230469,1.57226410513276,6.221103191375732,1.958123875869381,-14.140877723693848,1.5283393989495853,-27.70670509338379,2.2462220922827534,26.845518112182617,-3.0049218208376907,45.96609115600586,0.33956372056314166,6
|
||||
6,6,997,2649,1381385,7.976283,60.47948252708421,23.068781981390185,-5.01126420129949,36.10685486878251,5.525681179628412,-31.3025636495649,2665,1381391,8.024277,J2,1.408993592340284,0.8424031169595625,47.99400000000009,60.847530364990234,0.36804783790602613,24.47777557373047,1.408993592340284,-5.617043972015381,-0.6057797707158912,34.8044319152832,-1.3024229534993097,5.786255359649658,0.260574180021246,-31.2253475189209,0.07721613064400046,2667,1381392,8.032258,J4,1.7701696881184503,1.0669192783996628,55.975000000000996,60.95240783691406,0.47292530982985426,24.69621467590332,1.6274326945131357,-5.75187873840332,-0.7406145371038306,34.33668518066406,-1.7701696881184503,5.885775089263916,0.3600939096355038,-30.920429229736328,0.38213441982857077,2669,1381393,8.040307,J4,2.303903056038372,1.3309750271252634,64.02400000000074,61.06847381591797,0.5889912888337605,24.908491134643555,1.83970915325337,-5.895160675048828,-0.8838964737493384,33.80295181274414,-2.303903056038372,5.999467372894287,0.4737861932658749,-30.538488388061523,0.7640752615033755,6
|
||||
7,7,1053,2795,1381441,8.424315,70.47583707168602,16.39384887825908,-13.456948007467595,-27.892318852946243,14.53566195089614,31.71193282686115,2811,1381447,8.472317,J6,7.1941073647892395,4.618545262978315,48.00200000000032,70.8250503540039,0.3492132823178906,10.95259952545166,-5.4412493528074215,-13.136046409606934,0.3209015978606615,-34.68459701538086,-6.7922781624346165,14.022420883178711,-0.5132410677174288,38.90604019165039,7.1941073647892395,2814,1381448,8.480253,J6,8.24868042387127,5.331515658278243,55.93799999999938,70.84622192382812,0.37038485214210937,9.979106903076172,-6.41474197518291,-13.042067527770996,0.414880479696599,-35.67851257324219,-7.786193720295945,13.882524490356445,-0.6531374605396945,39.96061325073242,8.24868042387127,2817,1381449,8.488291,J6,9.275558780804865,6.035874346469871,63.976000000000255,70.85871887207031,0.38288180038429687,8.986706733703613,-7.407142144555468,-12.9381742477417,0.5187737597258959,-36.64576721191406,-8.75344835896782,13.72922134399414,-0.8064406069019991,40.987491607666016,9.275558780804865,6
|
||||
8,8,1089,2892,1381477,8.71231,69.58246408878419,-17.10571341532583,-8.710589696831251,-58.47569441890704,7.63059003611043,64.43773280685575,2908,1381483,8.760361,J6,5.93988407546847,3.533132924843701,48.05099999999918,69.40516662597656,-0.1772974628076298,-20.066781997680664,-2.9610685823548337,-8.152935981750488,0.5576537150807628,-63.93235397338867,-5.456659554481632,6.775912284851074,-0.8546777512593557,70.37761688232422,5.93988407546847,2910,1381484,8.768294,J6,6.922168621366907,4.090127751125079,55.9839999999987,69.39288330078125,-0.1895807880029423,-20.37982177734375,-3.2741083620179197,-8.094615936279297,0.6159737605519542,-64.83366394042969,-6.357969521522648,6.682157039642334,-0.948432996468096,71.35990142822266,6.922168621366907,2912,1381485,8.77637,J6,7.916950115507532,4.649086823187336,64.05999999999956,69.38455200195312,-0.1979120868310673,-20.654869079589844,-3.5491556642640134,-8.043557167053223,0.6670325297780284,-65.74622344970703,-7.270529030799992,6.598722457885742,-1.0318675782246878,72.35468292236328,7.916950115507532,6
|
||||
9,9,1167,3096,1381555,9.336375,73.57125874861414,-6.42984524964374,-9.628579811400881,-128.80864727564332,14.671082817606491,140.00204816414424,3112,1381561,9.384332,J4,2.5240706198644887,1.630593251762862,47.95700000000025,73.90592956542969,0.3346708168155459,-8.016124725341797,-1.5862794756980572,-9.002426147460938,0.6261536639399434,-131.3327178955078,-2.5240706198644887,15.526206970214844,0.8551241526083526,142.41668701171875,2.4146388475745084,3114,1381562,9.392355,J4,2.830116152091051,1.8576181251506447,55.97999999999992,73.96361541748047,0.39235666886632714,-8.363268852233887,-1.933423602590147,-8.884164810180664,0.7444150012202169,-131.63876342773438,-2.830116152091051,15.65822696685791,0.987144149251419,142.69912719726562,2.6970790331213834,3118,1381563,9.400331,J4,3.1013106100988637,2.0726473342415157,63.955999999999236,74.02384948730469,0.4525907386905459,-8.728874206542969,-2.299028956899229,-8.76374626159668,0.8648335498042012,-131.9099578857422,-3.1013106100988637,15.788768768310547,1.1176859507040557,142.9464569091797,2.944408745035446,6
|
||||
10,10,1189,3154,1381577,9.512325,75.56938603377543,-14.67930590788221,-7.294156094303152,-130.92303342701462,17.688361072687904,141.89350789658602,3170,1381583,9.560358,J4,3.3929965007450846,2.1059862266772753,48.033000000000214,76.72688293457031,1.1574969007948823,-16.518726348876953,-1.8394204409947434,-7.15147066116333,0.14268543313982196,-127.53003692626953,3.3929965007450846,18.53788948059082,0.8495284079029162,138.7897491455078,-3.103758751078203,3172,1381584,9.568328,J4,4.172522257581022,2.562681778316925,56.00299999999869,76.96759033203125,1.3982042982558198,-16.79741096496582,-2.1181050570836106,-7.157340049743652,0.1368160445594997,-126.7505111694336,4.172522257581022,18.698274612426758,1.0099135397388537,138.08377075195312,-3.8097371446328907,3175,1381585,9.576354,J4,4.999006937756803,3.042227585289095,64.02899999999967,77.21700286865234,1.6476168348769136,-17.061315536499023,-2.3820096286168138,-7.1720428466796875,0.12211324762346454,-125.92402648925781,4.999006937756803,18.86079978942871,1.1724387167408068,137.33689880371094,-4.556609092875078,6
|
||||
11,11,1257,3334,1381645,10.056378,86.0934977330502,-14.498333177585573,-13.681510793859989,-69.8682955440411,26.742681901805224,88.9994136192388,3350,1381651,10.104322,J6,2.9848240516596434,2.0134925384056306,47.94399999999932,83.71662139892578,-2.376876334124418,-13.186573028564453,1.3117601490211204,-13.738808631896973,-0.05729783803698396,-72.70317840576172,-2.834882861720615,26.66758918762207,-0.07509271418315322,91.98423767089844,2.9848240516596434,3352,1381652,10.112375,J6,3.668104996483862,2.4582215610180698,55.99699999999963,83.25711822509766,-2.836379507952543,-12.97336483001709,1.5249683475684837,-13.721805572509766,-0.04029477864977693,-73.39217376708984,-3.52387822304874,26.632198333740234,-0.11048356806498916,92.66751861572266,3.668104996483862,3355,1381653,10.120334,J6,4.397696366112768,2.932320329432031,63.955999999999236,82.77881622314453,-3.314681509905668,-12.759296417236328,1.7390367603492454,-13.69810676574707,-0.016595971887081618,-74.13638305664062,-4.2680875125995215,26.590784072875977,-0.15189782892924697,93.39710998535156,4.397696366112768,6
|
||||
12,12,1303,3455,1381691,10.424364,61.720732564877665,-4.2327893098442155,-9.784423185760874,-108.38408270751574,22.16077221865394,118.14206389103131,3470,1381697,10.472333,J6,4.000958239175844,1.9902461373621054,47.96900000000015,59.78955841064453,-1.9311741542331333,-2.7094168663024902,1.5233724435417253,-8.933828353881836,0.8505948318790377,-108.24857330322266,0.13550940429308866,21.177492141723633,-0.9832800769303063,114.14110565185547,-4.000958239175844,3474,1381698,10.480323,J6,4.9290969720860005,2.3936680954994465,55.95899999999965,59.577754974365234,-2.14297759051243,-2.4748787879943848,1.7579105218498308,-8.810709953308105,0.9737132324527682,-107.96444702148438,0.4196356860313699,21.03158187866211,-1.1291903399918297,113.21296691894531,-4.9290969720860005,3476,1381699,10.488357,J6,5.929425036050844,2.82736270797146,63.99299999999997,59.389400482177734,-2.33133208269993,-2.241248369216919,1.9915409406272966,-8.690967559814453,1.0934556259464205,-107.61585998535156,0.7682227221641824,20.888513565063477,-1.2722586535904625,112.21263885498047,-5.929425036050844,6
|
||||
13,13,1362,3611,1381750,10.896324,79.75708299218505,4.6402310571176475,-15.311185536748695,-56.03531247084017,26.13313788666632,52.152257172101606,3627,1381756,10.944371,J1,6.656208938967296,4.109282348845555,48.047000000000395,86.41329193115234,6.656208938967296,4.266173362731934,-0.3740576943857139,-19.15987777709961,-3.848692240350914,-52.24028015136719,3.7950323194729805,29.853723526000977,3.720585639334658,55.86907196044922,3.7168147883476124,3629,1381757,10.952347,J1,7.722973770510265,4.798701518313248,56.02299999999971,87.48005676269531,7.722973770510265,4.181386947631836,-0.45884410948581156,-19.803678512573242,-4.492492975824547,-51.80745315551758,4.22785931532259,30.479778289794922,4.346640403128603,56.773921966552734,4.621664794451128,3633,1381758,10.960362,J1,8.793827957521984,5.501947903898078,64.03800000000004,88.55091094970703,8.793827957521984,4.09064245223999,-0.5495886048776573,-20.455745697021484,-5.144560160272789,-51.41057586669922,4.624736604140949,31.11459732055664,4.981459433890322,57.749176025390625,5.596918853289019,6
|
||||
14,14,1393,3694,1381781,11.144347,108.95555081237923,1.3707600427061273,-33.59442519685133,-48.36736100282485,43.963403989432074,85.03926111958742,3710,1381787,11.192339,J6,2.93518040873289,1.5679827735552678,47.9920000000007,110.7523193359375,1.7967685235582707,0.8751887679100037,-0.4955712747961236,-34.756534576416016,-1.1621093795646829,-48.14332580566406,0.2240351971607879,45.0865592956543,1.1231553062222233,87.97444152832031,2.93518040873289,3712,1381788,11.20037,J6,3.224395496623515,1.7217974200989363,56.02299999999971,110.92666625976562,1.9711154473863957,0.7935436964035034,-0.5772163463026239,-34.86042404174805,-1.265998844896714,-48.102813720703125,0.2645472821217254,45.1879768371582,1.2245728477261295,88.26365661621094,3.224395496623515,3714,1381789,11.208359,J6,3.4549634287524214,1.844232474600388,64.01199999999996,111.0645751953125,2.1090243829332707,0.7117024660110474,-0.6590575766950799,-34.93739700317383,-1.3429718063224954,-48.06473159790039,0.3026294049244598,45.2645149230957,1.3011109336636295,88.49422454833984,3.4549634287524214,6
|
||||
15,15,1426,3780,1381814,11.408361,110.58484797608095,-3.860899593758653,-32.39640711653168,-51.112590875369015,44.2291595765054,87.62399978413751,3796,1381820,11.456361,J4,2.573146734494266,1.686689503314435,48.00000000000004,111.79177856445312,1.206930588372174,-6.225440502166748,-2.364540908408095,-31.958988189697266,0.4374189268344111,-53.68573760986328,-2.573146734494266,44.76150894165039,0.532349365144988,89.33454895019531,1.7105491660578025,3798,1381821,11.464349,J4,3.0742606260958283,2.037357874022524,55.98800000000104,112.08367919921875,1.498831223137799,-6.6951823234558105,-2.8342827296971573,-31.910240173339844,0.48616694319183296,-54.186851501464844,-3.0742606260958283,44.907752990722656,0.6785934142172536,89.73998260498047,2.1159828208429587,3801,1381822,11.472359,J4,3.579433355588016,2.3980591339233284,63.998000000001554,112.39252471923828,1.8076767431573302,-7.177155494689941,-3.316255900931288,-31.86791229248047,0.528494824051208,-54.69202423095703,-3.579433355588016,45.06621170043945,0.8370521239340505,90.16300964355469,2.5390098594171775,6
|
||||
16,16,1454,3854,1381842,11.63236,118.09595224767921,-17.37638727211256,-31.06900084212659,-59.56053843778568,48.73257576059714,94.13477131163891,3870,1381848,11.680362,J6,3.8105220440607894,2.197782779718695,48.00200000000032,117.87474060058594,-0.2212116470932699,-19.607177734375,-2.2307904622624406,-30.329103469848633,0.7398973722779587,-56.600250244140625,2.960288193645056,49.08655548095703,0.353979720359888,90.32424926757812,-3.8105220440607894,3872,1381849,11.688367,J6,4.756826365349852,2.710409629634467,56.00699999999925,117.7159423828125,-0.3800098648667074,-19.938819885253906,-2.562432613141347,-30.167478561401367,0.9015222807252243,-55.84638595581055,3.714152481975134,49.0964241027832,0.3638483421860599,89.37794494628906,-4.756826365349852,3874,1381850,11.696354,J6,5.783033335564696,3.263710825457299,63.99399999999922,117.52371978759766,-0.5722324600815512,-20.255823135375977,-2.879435863263417,-29.996501922607422,1.0724989195191696,-55.02521514892578,4.5353232888598995,49.092247009277344,0.3596712486802005,88.35173797607422,-5.783033335564696,6
|
||||
17,18,1556,4121,1381944,12.448375,60.28248244838438,-22.938080568038327,-10.333247998560786,77.49164224770549,35.58337770883919,-69.66898071584893,4138,1381950,12.496365,J4,6.848522435693766,4.03614029285736,47.99000000000042,59.93589401245117,-0.34658843593320654,-21.102140426635742,1.8359401414025847,-9.684239387512207,0.6490086110485791,70.64311981201172,-6.848522435693766,32.941307067871094,-2.642070640968093,-63.34855651855469,6.320424197294244,4140,1381951,12.504369,J4,8.296535741357829,4.890437505980655,55.9940000000001,59.902915954589844,-0.37956649379453467,-20.689186096191406,2.2488944718469206,-9.52907943725586,0.8041685613049268,69.19510650634766,-8.296535741357829,32.343406677246094,-3.239971031593093,-62.032222747802734,7.636757968046197,4142,1381952,12.512395,J4,9.82779337075236,5.794164211715548,64.0199999999993,59.8752326965332,-0.4072497518511753,-20.24807357788086,2.6900069901574675,-9.361802101135254,0.9714458974255322,67.66384887695312,-9.82779337075236,31.704130172729492,-3.8792475361096947,-60.64377975463867,9.02520096121026,6
|
||||
|
@@ -0,0 +1,18 @@
|
||||
{
|
||||
"rows": 17,
|
||||
"best_offset_win_counts": {
|
||||
"6": 17,
|
||||
"7": 0,
|
||||
"8": 0
|
||||
},
|
||||
"average_max_error_deg": {
|
||||
"6": 4.241583591714439,
|
||||
"7": 5.068092118561757,
|
||||
"8": 5.9316839578472145
|
||||
},
|
||||
"average_rms_error_deg": {
|
||||
"6": 2.5406136706026206,
|
||||
"7": 3.021296972079456,
|
||||
"8": 3.5201758351699857
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
trigger_no,waypoint_index,trigger_frame_number,trigger_time_relative_s,trigger_sequence,paired_status_frame_number,paired_status_time_relative_s,paired_status_sequence,paired_status_timestamp,paired_status_to_trigger_sequence_delta,paired_status_to_trigger_time_ms,teach_j1_deg,teach_j2_deg,teach_j3_deg,teach_j4_deg,teach_j5_deg,teach_j6_deg,paired_status_j1_actual_deg,paired_status_diff_j1_deg,paired_status_j2_actual_deg,paired_status_diff_j2_deg,paired_status_j3_actual_deg,paired_status_diff_j3_deg,paired_status_j4_actual_deg,paired_status_diff_j4_deg,paired_status_j5_actual_deg,paired_status_diff_j5_deg,paired_status_j6_actual_deg,paired_status_diff_j6_deg,paired_status_max_error_axis,paired_status_max_error_deg,paired_status_rms_error_deg,best_status_frame_number,best_status_time_relative_s,best_status_sequence,best_status_timestamp,best_status_delta_from_paired_cycles,best_status_delta_from_trigger_sequence,best_status_time_after_trigger_ms,best_status_j1_actual_deg,best_status_diff_j1_deg,best_status_j2_actual_deg,best_status_diff_j2_deg,best_status_j3_actual_deg,best_status_diff_j3_deg,best_status_j4_actual_deg,best_status_diff_j4_deg,best_status_j5_actual_deg,best_status_diff_j5_deg,best_status_j6_actual_deg,best_status_diff_j6_deg,best_status_max_error_axis,best_status_max_error_deg,best_status_rms_error_deg
|
||||
1,1,1955,5.888271,1381124,1954,5.888166,1381116,624368380,-8,-0.10499999999957765,48.886810269468405,2.1989850886957285,-11.021017368511105,0.4102097980549552,6.248381265333557,2.294990756284542,55.47505187988281,6.588241610414407,1.2506256103515625,-0.948359478344166,-5.400295734405518,5.620721634105587,0.5259769558906555,0.1157671578357003,3.3612561225891113,-2.8871251427444458,0.44421958923339844,-1.8507711670511435,J1,6.588241610414407,3.8225443630072706,1981,5.968124,1381126,624368460,10,2,79.85300000000083,48.76339340209961,-0.12341686736879609,2.256481647491455,0.05749655879572657,-11.157909393310547,-0.13689202479944207,0.37860527634620667,-0.031604521708748556,6.3406662940979,0.0922850287643433,2.5025806427001953,0.20758988641565335,J6,0.20758988641565335,0.12239685259209541
|
||||
2,2,2151,6.480252,1381198,2150,6.480138,1381190,624368972,-8,-0.11399999999994748,55.34775509527405,11.807039833001637,-7.0090952672806885,-71.01433145543973,6.012065051914967,74.24953191606797,53.0124397277832,-2.33531536749085,11.506516456604004,-0.30052337639763316,-8.466276168823242,-1.4571809015425536,-69.63382720947266,1.3805042459670744,6.426647663116455,0.41458261120148787,72.93537902832031,-1.3141528877476532,J1,2.33531536749085,1.3827543088729335,2172,6.552164,1381199,624369044,9,1,71.9120000000002,55.265621185302734,-0.08213390997131853,11.803675651550293,-0.003364181451344095,-7.050373554229736,-0.04127828694904778,-71.06609344482422,-0.05176198938448806,6.026607990264893,0.014542938349925372,74.28738403320312,0.03785211713515935,J1,0.08213390997131853,0.04616054814443023
|
||||
3,3,2223,6.704225,1381226,2222,6.704166,1381218,624369196,-8,-0.059000000000253294,55.109808014787404,8.759497374223619,-8.518216825284897,-41.11725046606459,10.108488114686867,41.956933292858096,56.5781364440918,1.4683284293043926,10.494372367858887,1.7348749936352679,-7.114619255065918,1.4035975702189791,-55.112239837646484,-13.994989371581894,7.423236846923828,-2.6852512677630394,58.080867767333984,16.123934474475888,J6,16.123934474475888,8.852376616648156,2248,6.784154,1381228,624369276,10,2,79.92899999999992,55.00252151489258,-0.10728649989482619,8.69005012512207,-0.06944724910154854,-8.577027320861816,-0.05881049557691931,-40.59286117553711,0.5243892905274805,10.210430145263672,0.1019420305768044,41.38724136352539,-0.5696919293327056,J6,0.5696919293327056,0.3239642953120798
|
||||
4,4,2336,7.048266,1381269,2335,7.048148,1381261,624369540,-8,-0.11799999999961841,43.65359310453334,-1.629659559507137,-17.715753611915318,5.995208633582219,32.17171770646655,-22.573973337684023,44.479610443115234,0.8260173385818916,0.07192186266183853,1.7015814221689756,-15.996956825256348,1.7187967866589702,2.40028715133667,-3.5949214822455486,27.737812042236328,-4.433905664230224,-17.672090530395508,4.901882807288516,J6,4.901882807288516,3.2440556235708993,2362,7.128175,1381271,624369620,10,2,79.90899999999979,43.72715759277344,0.07356448824009476,-1.6386029720306396,-0.008943412523502614,-17.75617218017578,-0.04041856826046342,5.952699661254883,-0.04250897232733575,32.2616081237793,0.08989041731274483,-22.47926902770996,0.09470430997406254,J6,0.09470430997406254,0.06580474324148583
|
||||
5,5,2477,7.464289,1381321,2476,7.464178,1381313,624369956,-8,-0.11099999999952814,64.58244475717193,4.262979315506351,-15.669217122643433,-29.952927185666542,29.850439933020308,45.62652743544272,60.830230712890625,-3.7522140442813026,1.8808640241622925,-2.382115291344059,-17.297021865844727,-1.6278047432012936,-26.684900283813477,3.268026901853066,33.02191925048828,3.1714793174679734,36.40708923339844,-9.21943820204428,J6,9.21943820204428,4.621321121191458,2502,7.544122,1381323,624370036,10,2,79.83299999999983,64.5761947631836,-0.006249993988333813,4.283629417419434,0.020650101913082253,-15.615406036376953,0.05381108626647979,-29.94143295288086,0.011494232785683067,29.7938175201416,-0.056622412878706285,45.61851119995117,-0.008016235491545842,J5,0.056622412878706285,0.03357478517689402
|
||||
6,6,2665,8.024277,1381391,2664,8.024156,1381383,624370516,-8,-0.12100000000003774,60.47948252708421,23.068781981390185,-5.01126420129949,36.10685486878251,5.525681179628412,-31.3025636495649,60.89759063720703,0.418108110122823,20.716407775878906,-2.3523742055112784,-4.913214206695557,0.09804999460393304,32.72084045410156,-3.3860144146809503,6.405356407165527,0.8796752275371151,-25.156103134155273,6.1464605154096255,J6,6.1464605154096255,3.0478405356855083,2687,8.096181,1381392,624370588,9,1,71.90399999999997,60.54315185546875,0.06366932838454176,22.99761962890625,-0.07116235248393465,-5.037043571472168,-0.025779370172678284,36.056732177734375,-0.050122691048137824,5.545217037200928,0.019535857572515525,-31.12558937072754,0.17697427883735983,J6,0.17697427883735983,0.08563125303054821
|
||||
7,7,2811,8.472317,1381447,2810,8.472208,1381439,624370964,-8,-0.10900000000013677,70.47583707168602,16.39384887825908,-13.456948007467595,-27.892318852946243,14.53566195089614,31.71193282686115,69.13842010498047,-1.3374169667055469,22.202428817749023,5.808579939489942,-12.907807350158691,0.5491406573089037,-15.469873428344727,12.422445424601516,14.040997505187988,-0.4946644457081515,18.56021499633789,-13.15171783052326,J6,13.15171783052326,7.78202379046887,2834,8.54419,1381448,624371036,9,1,71.87300000000008,70.3967514038086,-0.07908566787742188,16.456680297851562,0.06283141959248084,-13.330730438232422,0.12621756923517324,-27.611600875854492,0.2807179770917507,14.542427062988281,0.006765112092141479,31.405345916748047,-0.30658691011310424,J6,0.30658691011310424,0.18210669390591905
|
||||
8,8,2908,8.760361,1381483,2907,8.760245,1381475,624371252,-8,-0.11600000000022703,69.58246408878419,-17.10571341532583,-8.710589696831251,-58.47569441890704,7.63059003611043,64.43773280685575,70.10379791259766,0.5213338238134639,-10.075156211853027,7.030557203472803,-9.98978042602539,-1.2791907291941396,-51.5866584777832,6.889035941123836,9.534638404846191,1.9040483687357614,57.0014533996582,-7.436279407197546,J6,7.436279407197546,5.127041386597523,2929,8.832199,1381484,624371324,9,1,71.83799999999962,69.59716033935547,0.014696250571276437,-17.044965744018555,0.06074767130727565,-8.703485488891602,0.007104207939649498,-58.473426818847656,0.0022676000593833123,7.6463942527771,0.015804216666669646,64.45584869384766,0.01811588699190736,J2,0.06074767130727565,0.02750705896232285
|
||||
9,9,3112,9.384332,1381561,3111,9.384204,1381553,624371876,-8,-0.128000000000128,73.57125874861414,-6.42984524964374,-9.628579811400881,-128.80864727564332,14.671082817606491,140.00204816414424,72.96144104003906,-0.6098177085750791,-6.3001484870910645,0.12969676255267526,-10.035088539123535,-0.40650872772265423,-123.29313659667969,5.515510678963636,13.165532112121582,-1.505550705484909,134.41644287109375,-5.585605293050492,J6,5.585605293050492,3.277206017832995,3134,9.456205,1381562,624371948,9,1,71.87300000000008,73.55095672607422,-0.02030202253992286,-6.441007614135742,-0.011162364492002474,-9.614203453063965,0.014376358336916084,-128.65638732910156,0.15225994654176134,14.635379791259766,-0.03570302634672551,139.8520965576172,-0.14995160652705408,J4,0.15225994654176134,0.089150370994325
|
||||
10,10,3170,9.560358,1381583,3169,9.56025,1381575,624372052,-8,-0.10800000000088517,75.56938603377543,-14.67930590788221,-7.294156094303152,-130.92303342701462,17.688361072687904,141.89350789658602,74.36400604248047,-1.2053799912949614,-10.514228820800781,4.165077087081428,-8.236283302307129,-0.9421272080039769,-132.6507110595703,-1.7276776325556966,16.38176155090332,-1.3065995217845838,143.5712890625,1.6777811659139843,J2,4.165077087081428,2.128983861449047,3196,9.640214,1381585,624372132,10,2,79.85599999999948,75.62290954589844,0.05352351212300732,-14.662798881530762,0.016507026351447962,-7.297325611114502,-0.003169516811349915,-130.88812255859375,0.03491086842086588,17.726865768432617,0.03850469574471305,141.820068359375,-0.0734395372110157,J6,0.0734395372110157,0.043285948566531826
|
||||
11,11,3350,10.104322,1381651,3349,10.104215,1381643,624372596,-8,-0.10699999999985721,86.0934977330502,-14.498333177585573,-13.681510793859989,-69.8682955440411,26.742681901805224,88.9994136192388,87.81878662109375,1.7252888880435506,-16.329286575317383,-1.8309533977318093,-13.073507308959961,0.6080034849000278,-70.97254180908203,-1.1042462650409277,26.366661071777344,-0.3760208300278798,89.37981414794922,0.3804005287104246,J2,1.8309533977318093,1.169340011162825,3373,10.17622,1381652,624372668,9,1,71.8980000000009,86.11857604980469,0.025078316754488128,-14.53605842590332,-0.03772524831774682,-13.651655197143555,0.029855596716434007,-69.8272933959961,0.04100214804500979,26.72992706298828,-0.012754838816942282,88.92174530029297,-0.07766831894582538,J6,0.07766831894582538,0.04246508921571141
|
||||
12,12,3470,10.472333,1381697,3469,10.472191,1381689,624372964,-8,-0.14200000000030855,61.720732564877665,-4.2327893098442155,-9.784423185760874,-108.38408270751574,22.16077221865394,118.14206389103131,65.91849517822266,4.197762613344992,-6.331206798553467,-2.0984174887092513,-11.008773803710938,-1.2243506179500638,-103.56172943115234,4.822353276363401,23.532543182373047,1.3717709637191078,117.46694946289062,-0.675114428140688,J4,4.822353276363401,2.8611252897135864,3493,10.544283,1381698,624373036,9,1,71.94999999999929,61.7418098449707,0.021077280093038553,-4.212831497192383,0.019957812651832718,-9.789913177490234,-0.00548999172936071,-108.36312103271484,0.020961674800901164,22.163673400878906,0.002901182224967158,118.13783264160156,-0.0042312494297505054,J1,0.021077280093038553,0.014935517911187784
|
||||
13,13,3627,10.944371,1381756,3626,10.944251,1381748,624373436,-8,-0.12000000000078614,79.75708299218505,4.6402310571176475,-15.311185536748695,-56.03531247084017,26.13313788666632,52.152257172101606,72.04009246826172,-7.716990523923329,4.5515007972717285,-0.08873025984591898,-11.385177612304688,3.926007924444008,-64.18782043457031,-8.152507963730145,22.412456512451172,-3.7206813742151468,54.39056396484375,2.2383067927421436,J4,8.152507963730145,5.168659196117862,3651,11.016226,1381757,624373508,9,1,71.85499999999934,79.90262603759766,0.14554304541260876,4.62429666519165,-0.015934391925997105,-15.370849609375,-0.05966407262630469,-55.99199676513672,0.043315705703449225,26.182939529418945,0.04980164275262666,52.15596008300781,0.003702910906206114,J1,0.14554304541260876,0.0699602863618738
|
||||
14,14,3710,11.192339,1381787,3709,11.19225,1381779,624373684,-8,-0.08900000000089392,108.95555081237923,1.3707600427061273,-33.59442519685133,-48.36736100282485,43.963403989432074,85.03926111958742,102.20675659179688,-6.748794220582354,2.5045764446258545,1.1338164019197272,-29.148040771484375,4.446384425366958,-48.76271438598633,-0.3953533831614777,39.65629959106445,-4.3071043983676205,74.63484191894531,-10.40441920064211,J6,10.40441920064211,5.679810575214033,3735,11.272261,1381789,624373764,10,2,79.92199999999983,108.94917297363281,-0.006377838746416842,1.3447811603546143,-0.025978882351513022,-33.561458587646484,0.032966609204848396,-48.376487731933594,-0.009126729108743348,44.02882766723633,0.06542367780425451,85.1587142944336,0.11945317484617135,J6,0.11945317484617135,0.05835950688086092
|
||||
15,15,3796,11.456361,1381820,3795,11.456253,1381812,624373948,-8,-0.10799999999910881,110.58484797608095,-3.860899593758653,-32.39640711653168,-51.112590875369015,44.2291595765054,87.62399978413751,110.58193969726562,-0.002908278815326071,-1.244791865348816,2.6161077284098373,-33.74542999267578,-1.3490228761441045,-48.585723876953125,2.5268669984158905,44.58126449584961,0.35210491934420673,87.75857543945312,0.134575655315615,J2,2.6161077284098373,1.5911754635072362,3821,11.536287,1381822,624374028,10,2,79.92600000000039,110.67292022705078,0.08807225096983018,-4.010702133178711,-0.14980253942005772,-32.37457275390625,0.021834362625426706,-51.22603225708008,-0.11344138171106266,44.249629974365234,0.020470397859831735,87.66435241699219,0.040352632854677495,J2,0.14980253942005772,0.08716897465096986
|
||||
16,16,3870,11.680362,1381848,3869,11.680263,1381840,624374172,-8,-0.09900000000051534,118.09595224767921,-17.37638727211256,-31.06900084212659,-59.56053843778568,48.73257576059714,94.13477131163891,115.98970031738281,-2.106251930296395,-12.693913459777832,4.682473812334727,-31.563575744628906,-0.49457490250231473,-59.300933837890625,0.2596045998950558,47.15116500854492,-1.5814107520522214,94.34331512451172,0.20854381287280432,J2,4.682473812334727,2.2067399623329567,3895,11.760326,1381850,624374252,10,2,79.96399999999859,118.06391906738281,-0.0320331802963949,-17.536325454711914,-0.15993818259935466,-31.00341796875,0.06558287337659152,-59.507286071777344,0.05325236600833705,48.77106475830078,0.038488997703638006,93.9618148803711,-0.17295643126782068,J6,0.17295643126782068,0.10419421346699378
|
||||
17,18,4138,12.496365,1381950,4137,12.496271,1381942,624374988,-8,-0.09400000000070463,60.28248244838438,-22.938080568038327,-10.333247998560786,77.49164224770549,35.58337770883919,-69.66898071584893,61.48033142089844,1.197848972514059,-23.1823787689209,-0.2442982008825716,-10.346473693847656,-0.01322569528687012,79.24906921386719,1.7574269661617024,35.91868591308594,0.33530820424675056,-71.48294067382812,-1.8139599579791934,J6,1.8139599579791934,1.1536991630766555,4159,12.568313,1381951,624375060,9,1,71.94799999999901,60.27695846557617,-0.00552398280820654,-22.918088912963867,0.019991655074459658,-10.347146987915039,-0.013898989354252933,77.54158782958984,0.0499455818843586,35.57386016845703,-0.009517540382155687,-69.74390411376953,-0.07492339792059965,J6,0.07492339792059965,0.03834497543010648
|
||||
|
@@ -0,0 +1,18 @@
|
||||
trigger_no,waypoint_index,frame_number,sequence,time_relative_s,write_io_value,io_addrs,config_addr,max_error_axis,max_error_deg,rms_error_deg,j1_actual_deg,j1_teach_deg,diff_j1_deg,j2_actual_deg,j2_teach_deg,diff_j2_deg,j3_actual_deg,j3_teach_deg,diff_j3_deg,j4_actual_deg,j4_teach_deg,diff_j4_deg,j5_actual_deg,j5_teach_deg,diff_j5_deg,j6_actual_deg,j6_teach_deg,diff_j6_deg
|
||||
1,1,1955,1381124,5.888271,10,"[2, 4]","[2, 4]",J6,3.4065268256551553,2.468438977201345,45.8632698059082,48.886810269468405,-3.0235404635602023,2.9724788665771484,2.1989850886957285,0.7734937778814199,-13.609341621398926,-11.021017368511105,-2.588324252887821,-2.1851141452789307,0.4102097980549552,-2.5953239433338857,7.582361221313477,6.248381265333557,1.3339799559799195,5.701517581939697,2.294990756284542,3.4065268256551553
|
||||
2,2,2151,1381198,6.480252,14,"[2, 3, 4]","[3, 4, 2]",J4,2.8167225076858244,1.6562461612538633,56.34336471557617,55.34775509527405,0.995609620302119,11.66723918914795,11.807039833001637,-0.13980064385368784,-6.600923538208008,-7.0090952672806885,0.40817172907268073,-68.1976089477539,-71.01433145543973,2.8167225076858244,6.058437347412109,6.012065051914967,0.04637229549714217,71.5392837524414,74.24953191606797,-2.7102481636265594
|
||||
3,3,2223,1381226,6.704225,14,"[2, 3, 4]","[3, 4, 2]",J6,11.130744251720401,5.955607695749779,53.50831985473633,55.109808014787404,-1.6014881600510762,7.412222862243652,8.759497374223619,-1.3472745119799665,-9.629739761352539,-8.518216825284897,-1.111522936067642,-32.30571365356445,-41.11725046606459,8.811536812500137,12.488306045532227,10.108488114686867,2.379817930845359,30.826189041137695,41.956933292858096,-11.130744251720401
|
||||
4,4,2336,1381269,7.048266,10,"[2, 4]","[4, 2]",J6,2.3116842904428125,1.3731583485126422,44.433006286621094,43.65359310453334,0.779413182087751,-2.1156294345855713,-1.629659559507137,-0.48596987507843425,-18.39777374267578,-17.715753611915318,-0.6820201307604634,4.8371992111206055,5.995208633582219,-1.158009422461613,33.99372482299805,32.17171770646655,1.8220071165314948,-20.26228904724121,-22.573973337684023,2.3116842904428125
|
||||
5,5,2477,1381321,7.464289,10,"[2, 4]","[4, 2]",J5,2.171828177649214,1.404704060246151,65.84967041015625,64.58244475717193,1.2672256529843224,5.695003032684326,4.262979315506351,1.4320237171779748,-14.563636779785156,-15.669217122643433,1.1055803428582767,-28.69342613220215,-29.952927185666542,1.259501053464394,27.678611755371094,29.850439933020308,-2.171828177649214,46.437156677246094,45.62652743544272,0.810629241803376
|
||||
6,6,2665,1381391,8.024277,12,"[3, 4]","[3, 4]",J2,1.408993592340284,0.8424031169595625,60.847530364990234,60.47948252708421,0.36804783790602613,24.47777557373047,23.068781981390185,1.408993592340284,-5.617043972015381,-5.01126420129949,-0.6057797707158912,34.8044319152832,36.10685486878251,-1.3024229534993097,5.786255359649658,5.525681179628412,0.260574180021246,-31.2253475189209,-31.3025636495649,0.07721613064400046
|
||||
7,7,2811,1381447,8.472317,12,"[3, 4]","[3, 4]",J6,7.1941073647892395,4.618545262978315,70.8250503540039,70.47583707168602,0.3492132823178906,10.95259952545166,16.39384887825908,-5.4412493528074215,-13.136046409606934,-13.456948007467595,0.3209015978606615,-34.68459701538086,-27.892318852946243,-6.7922781624346165,14.022420883178711,14.53566195089614,-0.5132410677174288,38.90604019165039,31.71193282686115,7.1941073647892395
|
||||
8,8,2908,1381483,8.760361,10,"[2, 4]","[4, 2]",J6,5.93988407546847,3.533132924843701,69.40516662597656,69.58246408878419,-0.1772974628076298,-20.066781997680664,-17.10571341532583,-2.9610685823548337,-8.152935981750488,-8.710589696831251,0.5576537150807628,-63.93235397338867,-58.47569441890704,-5.456659554481632,6.775912284851074,7.63059003611043,-0.8546777512593557,70.37761688232422,64.43773280685575,5.93988407546847
|
||||
9,9,3112,1381561,9.384332,10,"[2, 4]","[4, 2]",J4,2.5240706198644887,1.630593251762862,73.90592956542969,73.57125874861414,0.3346708168155459,-8.016124725341797,-6.42984524964374,-1.5862794756980572,-9.002426147460938,-9.628579811400881,0.6261536639399434,-131.3327178955078,-128.80864727564332,-2.5240706198644887,15.526206970214844,14.671082817606491,0.8551241526083526,142.41668701171875,140.00204816414424,2.4146388475745084
|
||||
10,10,3170,1381583,9.560358,10,"[2, 4]","[4, 2]",J4,3.3929965007450846,2.1059862266772753,76.72688293457031,75.56938603377543,1.1574969007948823,-16.518726348876953,-14.67930590788221,-1.8394204409947434,-7.15147066116333,-7.294156094303152,0.14268543313982196,-127.53003692626953,-130.92303342701462,3.3929965007450846,18.53788948059082,17.688361072687904,0.8495284079029162,138.7897491455078,141.89350789658602,-3.103758751078203
|
||||
11,11,3350,1381651,10.104322,10,"[2, 4]","[4, 2]",J6,2.9848240516596434,2.0134925384056306,83.71662139892578,86.0934977330502,-2.376876334124418,-13.186573028564453,-14.498333177585573,1.3117601490211204,-13.738808631896973,-13.681510793859989,-0.05729783803698396,-72.70317840576172,-69.8682955440411,-2.834882861720615,26.66758918762207,26.742681901805224,-0.07509271418315322,91.98423767089844,88.9994136192388,2.9848240516596434
|
||||
12,12,3470,1381697,10.472333,10,"[2, 4]","[4, 2]",J6,4.000958239175844,1.9902461373621054,59.78955841064453,61.720732564877665,-1.9311741542331333,-2.7094168663024902,-4.2327893098442155,1.5233724435417253,-8.933828353881836,-9.784423185760874,0.8505948318790377,-108.24857330322266,-108.38408270751574,0.13550940429308866,21.177492141723633,22.16077221865394,-0.9832800769303063,114.14110565185547,118.14206389103131,-4.000958239175844
|
||||
13,13,3627,1381756,10.944371,12,"[3, 4]","[4, 3]",J1,6.656208938967296,4.109282348845555,86.41329193115234,79.75708299218505,6.656208938967296,4.266173362731934,4.6402310571176475,-0.3740576943857139,-19.15987777709961,-15.311185536748695,-3.848692240350914,-52.24028015136719,-56.03531247084017,3.7950323194729805,29.853723526000977,26.13313788666632,3.720585639334658,55.86907196044922,52.152257172101606,3.7168147883476124
|
||||
14,14,3710,1381787,11.192339,10,"[2, 4]","[4, 2]",J6,2.93518040873289,1.5679827735552678,110.7523193359375,108.95555081237923,1.7967685235582707,0.8751887679100037,1.3707600427061273,-0.4955712747961236,-34.756534576416016,-33.59442519685133,-1.1621093795646829,-48.14332580566406,-48.36736100282485,0.2240351971607879,45.0865592956543,43.963403989432074,1.1231553062222233,87.97444152832031,85.03926111958742,2.93518040873289
|
||||
15,15,3796,1381820,11.456361,10,"[2, 4]","[4, 2]",J4,2.573146734494266,1.686689503314435,111.79177856445312,110.58484797608095,1.206930588372174,-6.225440502166748,-3.860899593758653,-2.364540908408095,-31.958988189697266,-32.39640711653168,0.4374189268344111,-53.68573760986328,-51.112590875369015,-2.573146734494266,44.76150894165039,44.2291595765054,0.532349365144988,89.33454895019531,87.62399978413751,1.7105491660578025
|
||||
16,16,3870,1381848,11.680362,10,"[2, 4]","[4, 2]",J6,3.8105220440607894,2.197782779718695,117.87474060058594,118.09595224767921,-0.2212116470932699,-19.607177734375,-17.37638727211256,-2.2307904622624406,-30.329103469848633,-31.06900084212659,0.7398973722779587,-56.600250244140625,-59.56053843778568,2.960288193645056,49.08655548095703,48.73257576059714,0.353979720359888,90.32424926757812,94.13477131163891,-3.8105220440607894
|
||||
17,18,4138,1381950,12.496365,12,"[3, 4]","[4, 3]",J4,6.848522435693766,4.03614029285736,59.93589401245117,60.28248244838438,-0.34658843593320654,-21.102140426635742,-22.938080568038327,1.8359401414025847,-9.684239387512207,-10.333247998560786,0.6490086110485791,70.64311981201172,77.49164224770549,-6.848522435693766,32.941307067871094,35.58337770883919,-2.642070640968093,-63.34855651855469,-69.66898071584893,6.320424197294244
|
||||
|
363
analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py
Normal file
363
analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py
Normal file
@@ -0,0 +1,363 @@
|
||||
#!/usr/bin/env python3
|
||||
"""提取 2026042802-1 抓包中的 60015 状态反馈,并和 UTTC_MS11 示教点对比。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
import subprocess
|
||||
from collections import Counter
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
DEFAULT_PCAP = REPO_ROOT.parent / "Rvbust" / "uttc-20260428" / "2026042802-1.pcap"
|
||||
DEFAULT_TSHARK = Path(r"D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe")
|
||||
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
|
||||
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
|
||||
SEARCH_WINDOW_CYCLES = 20
|
||||
|
||||
|
||||
def be_u32(data: bytes, offset: int) -> int:
|
||||
"""按大端读取 4 字节无符号整数。"""
|
||||
return struct.unpack(">I", data[offset : offset + 4])[0]
|
||||
|
||||
|
||||
def be_u16(data: bytes, offset: int) -> int:
|
||||
"""按大端读取 2 字节无符号整数。"""
|
||||
return struct.unpack(">H", data[offset : offset + 2])[0]
|
||||
|
||||
|
||||
def be_f32(data: bytes, offset: int) -> float:
|
||||
"""按大端读取 4 字节浮点数。"""
|
||||
return struct.unpack(">f", data[offset : offset + 4])[0]
|
||||
|
||||
|
||||
def load_udp_rows(pcap: Path, tshark: Path) -> list[list[str]]:
|
||||
"""提取 UDP 60015 原始字段,后续按方向和长度拆分命令与状态。"""
|
||||
command = [
|
||||
str(tshark),
|
||||
"-r",
|
||||
str(pcap),
|
||||
"-Y",
|
||||
"udp.port==60015",
|
||||
"-T",
|
||||
"fields",
|
||||
"-e",
|
||||
"frame.number",
|
||||
"-e",
|
||||
"frame.time_relative",
|
||||
"-e",
|
||||
"ip.src",
|
||||
"-e",
|
||||
"ip.dst",
|
||||
"-e",
|
||||
"udp.payload",
|
||||
]
|
||||
output = subprocess.check_output(command, text=True, encoding="utf-8", errors="ignore")
|
||||
rows: list[list[str]] = []
|
||||
for line in output.splitlines():
|
||||
if not line.strip():
|
||||
continue
|
||||
parts = line.split("\t")
|
||||
if len(parts) >= 5:
|
||||
rows.append(parts[:5])
|
||||
return rows
|
||||
|
||||
|
||||
def decode_command_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
|
||||
"""把 64B J519 命令帧解码成结构化记录。"""
|
||||
records: list[dict] = []
|
||||
for frame_no, time_rel, ip_src, ip_dst, payload_hex in rows:
|
||||
if ip_src != client_ip or ip_dst != robot_ip:
|
||||
continue
|
||||
|
||||
payload = bytes.fromhex(payload_hex)
|
||||
if len(payload) != 64:
|
||||
continue
|
||||
|
||||
records.append(
|
||||
{
|
||||
"frame_number": int(frame_no),
|
||||
"time_relative_s": float(time_rel),
|
||||
"sequence": be_u32(payload, 0x08),
|
||||
"write_io_value": be_u16(payload, 0x18),
|
||||
"j1_deg": be_f32(payload, 0x1C),
|
||||
"j2_deg": be_f32(payload, 0x20),
|
||||
"j3_deg": be_f32(payload, 0x24),
|
||||
"j4_deg": be_f32(payload, 0x28),
|
||||
"j5_deg": be_f32(payload, 0x2C),
|
||||
"j6_deg": be_f32(payload, 0x30),
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def decode_status_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
|
||||
"""把 132B J519 状态帧按运行时代码同口径解码。"""
|
||||
records: list[dict] = []
|
||||
for frame_no, time_rel, ip_src, ip_dst, payload_hex in rows:
|
||||
if ip_src != robot_ip or ip_dst != client_ip:
|
||||
continue
|
||||
|
||||
payload = bytes.fromhex(payload_hex)
|
||||
if len(payload) != 132:
|
||||
continue
|
||||
|
||||
status = payload[0x0C]
|
||||
joints = [be_f32(payload, 0x3C + index * 4) for index in range(6)]
|
||||
pose = [be_f32(payload, 0x18 + index * 4) for index in range(6)]
|
||||
records.append(
|
||||
{
|
||||
"frame_number": int(frame_no),
|
||||
"time_relative_s": float(time_rel),
|
||||
"sequence": be_u32(payload, 0x08),
|
||||
"status": status,
|
||||
"accepts_command": bool(status & 0b0001),
|
||||
"received_command": bool(status & 0b0010),
|
||||
"system_ready": bool(status & 0b0100),
|
||||
"robot_in_motion": bool(status & 0b1000),
|
||||
"read_io_value": be_u16(payload, 0x12),
|
||||
"timestamp": be_u32(payload, 0x14),
|
||||
"pose_x_mm": pose[0],
|
||||
"pose_y_mm": pose[1],
|
||||
"pose_z_mm": pose[2],
|
||||
"pose_w_deg": pose[3],
|
||||
"pose_p_deg": pose[4],
|
||||
"pose_r_deg": pose[5],
|
||||
"j1_deg": joints[0],
|
||||
"j2_deg": joints[1],
|
||||
"j3_deg": joints[2],
|
||||
"j4_deg": joints[3],
|
||||
"j5_deg": joints[4],
|
||||
"j6_deg": joints[5],
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def pick_trigger_first_high_frames(records: list[dict]) -> list[dict]:
|
||||
"""由于 io_keep_cycles=2,只保留每组高电平脉冲的第一帧。"""
|
||||
trigger_frames: list[dict] = []
|
||||
previous_high = False
|
||||
for record in records:
|
||||
current_high = record["write_io_value"] > 0
|
||||
if current_high and not previous_high:
|
||||
trigger_frames.append(record)
|
||||
previous_high = current_high
|
||||
return trigger_frames
|
||||
|
||||
|
||||
def load_uttc_ms11_config() -> dict:
|
||||
"""读取 UTTC_MS11 的示教点和触发配置。"""
|
||||
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
return config["flying_shots"]["UTTC_MS11"]
|
||||
|
||||
|
||||
def build_diff_row(prefix: str, actual_deg: list[float], teach_deg: list[float], row: dict) -> tuple[float, float, str]:
|
||||
"""向结果行写入逐轴误差,并返回聚合误差。"""
|
||||
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
|
||||
abs_diffs = [abs(value) for value in diffs]
|
||||
max_error = max(abs_diffs)
|
||||
max_error_axis = f"J{abs_diffs.index(max_error) + 1}"
|
||||
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
|
||||
|
||||
for joint_index in range(6):
|
||||
joint_no = joint_index + 1
|
||||
row[f"{prefix}_j{joint_no}_actual_deg"] = actual_deg[joint_index]
|
||||
row[f"{prefix}_diff_j{joint_no}_deg"] = diffs[joint_index]
|
||||
|
||||
row[f"{prefix}_max_error_axis"] = max_error_axis
|
||||
row[f"{prefix}_max_error_deg"] = max_error
|
||||
row[f"{prefix}_rms_error_deg"] = rms_error
|
||||
return max_error, rms_error, max_error_axis
|
||||
|
||||
|
||||
def build_trigger_status_rows(trigger_frames: list[dict], status_records: list[dict], shot_config: dict) -> list[dict]:
|
||||
"""按触发顺序对齐命令帧、当前状态帧以及最接近示教点的反馈状态帧。"""
|
||||
rows: list[dict] = []
|
||||
trigger_waypoint_indices = [index for index, flag in enumerate(shot_config["shot_flags"]) if flag]
|
||||
status_by_sequence = {record["sequence"]: record for record in status_records}
|
||||
status_sequence_set = set(status_by_sequence)
|
||||
|
||||
for trigger_no, (trigger_frame, waypoint_index) in enumerate(zip(trigger_frames, trigger_waypoint_indices), start=1):
|
||||
teach_deg = [math.degrees(value) for value in shot_config["traj_waypoints"][waypoint_index]]
|
||||
current_status_sequence = trigger_frame["sequence"] - 8
|
||||
current_status = status_by_sequence[current_status_sequence]
|
||||
|
||||
row = {
|
||||
"trigger_no": trigger_no,
|
||||
"waypoint_index": waypoint_index,
|
||||
"trigger_frame_number": trigger_frame["frame_number"],
|
||||
"trigger_time_relative_s": trigger_frame["time_relative_s"],
|
||||
"trigger_sequence": trigger_frame["sequence"],
|
||||
"paired_status_frame_number": current_status["frame_number"],
|
||||
"paired_status_time_relative_s": current_status["time_relative_s"],
|
||||
"paired_status_sequence": current_status["sequence"],
|
||||
"paired_status_timestamp": current_status["timestamp"],
|
||||
"paired_status_to_trigger_sequence_delta": current_status["sequence"] - trigger_frame["sequence"],
|
||||
"paired_status_to_trigger_time_ms": (current_status["time_relative_s"] - trigger_frame["time_relative_s"]) * 1000.0,
|
||||
}
|
||||
|
||||
for joint_index in range(6):
|
||||
joint_no = joint_index + 1
|
||||
row[f"teach_j{joint_no}_deg"] = teach_deg[joint_index]
|
||||
|
||||
build_diff_row(
|
||||
"paired_status",
|
||||
[current_status[f"j{joint_no}_deg"] for joint_no in range(1, 7)],
|
||||
teach_deg,
|
||||
row,
|
||||
)
|
||||
|
||||
best_candidate = None
|
||||
for delta_cycles in range(-SEARCH_WINDOW_CYCLES, SEARCH_WINDOW_CYCLES + 1):
|
||||
candidate_sequence = current_status_sequence + delta_cycles
|
||||
if candidate_sequence not in status_sequence_set:
|
||||
continue
|
||||
|
||||
candidate = status_by_sequence[candidate_sequence]
|
||||
diffs = [candidate[f"j{joint_no}_deg"] - teach_deg[joint_no - 1] for joint_no in range(1, 7)]
|
||||
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
|
||||
max_error = max(abs(value) for value in diffs)
|
||||
score = (rms_error, max_error, abs(delta_cycles))
|
||||
if best_candidate is None or score < best_candidate["score"]:
|
||||
best_candidate = {
|
||||
"score": score,
|
||||
"delta_cycles": delta_cycles,
|
||||
"record": candidate,
|
||||
}
|
||||
|
||||
if best_candidate is None:
|
||||
raise RuntimeError(f"Trigger {trigger_no} 未找到候选状态帧。")
|
||||
|
||||
best_status = best_candidate["record"]
|
||||
row["best_status_frame_number"] = best_status["frame_number"]
|
||||
row["best_status_time_relative_s"] = best_status["time_relative_s"]
|
||||
row["best_status_sequence"] = best_status["sequence"]
|
||||
row["best_status_timestamp"] = best_status["timestamp"]
|
||||
row["best_status_delta_from_paired_cycles"] = best_candidate["delta_cycles"]
|
||||
row["best_status_delta_from_trigger_sequence"] = best_status["sequence"] - trigger_frame["sequence"]
|
||||
row["best_status_time_after_trigger_ms"] = (best_status["time_relative_s"] - trigger_frame["time_relative_s"]) * 1000.0
|
||||
|
||||
build_diff_row(
|
||||
"best_status",
|
||||
[best_status[f"j{joint_no}_deg"] for joint_no in range(1, 7)],
|
||||
teach_deg,
|
||||
row,
|
||||
)
|
||||
|
||||
rows.append(row)
|
||||
|
||||
return rows
|
||||
|
||||
|
||||
def write_csv(path: Path, rows: list[dict]) -> None:
|
||||
"""把分析结果落成 UTF-8 CSV。"""
|
||||
if not rows:
|
||||
raise ValueError(f"No rows to write: {path}")
|
||||
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.DictWriter(handle, fieldnames=list(rows[0].keys()))
|
||||
writer.writeheader()
|
||||
writer.writerows(rows)
|
||||
|
||||
|
||||
def build_summary(command_records: list[dict], trigger_status_rows: list[dict]) -> dict:
|
||||
"""汇总命令序列偏移和状态反馈误差分布。"""
|
||||
sequence_offsets = [row["trigger_sequence"] - row["paired_status_sequence"] for row in trigger_status_rows]
|
||||
best_paired_cycle_offsets = [row["best_status_delta_from_paired_cycles"] for row in trigger_status_rows]
|
||||
best_trigger_sequence_offsets = [row["best_status_delta_from_trigger_sequence"] for row in trigger_status_rows]
|
||||
best_time_offsets = [row["best_status_time_after_trigger_ms"] for row in trigger_status_rows]
|
||||
paired_max_errors = [row["paired_status_max_error_deg"] for row in trigger_status_rows]
|
||||
best_max_errors = [row["best_status_max_error_deg"] for row in trigger_status_rows]
|
||||
paired_axes = Counter(row["paired_status_max_error_axis"] for row in trigger_status_rows)
|
||||
best_axes = Counter(row["best_status_max_error_axis"] for row in trigger_status_rows)
|
||||
|
||||
sequence_offset_counter = Counter()
|
||||
trigger_frames = pick_trigger_first_high_frames(command_records)
|
||||
status_pairs = {row["trigger_frame_number"]: row for row in trigger_status_rows}
|
||||
for trigger_frame in trigger_frames:
|
||||
sequence_offset_counter[trigger_frame["sequence"] - status_pairs[trigger_frame["frame_number"]]["paired_status_sequence"]] += 1
|
||||
|
||||
return {
|
||||
"pcap_path": str(DEFAULT_PCAP),
|
||||
"command_count": len(command_records),
|
||||
"trigger_count": len(trigger_status_rows),
|
||||
"command_minus_paired_status_sequence_counter": dict(sequence_offset_counter),
|
||||
"paired_status_average_max_error_deg": sum(paired_max_errors) / len(paired_max_errors),
|
||||
"paired_status_max_error_deg": max(paired_max_errors),
|
||||
"paired_status_max_error_axis_counter": dict(paired_axes),
|
||||
"best_status_average_max_error_deg": sum(best_max_errors) / len(best_max_errors),
|
||||
"best_status_max_error_deg": max(best_max_errors),
|
||||
"best_status_max_error_axis_counter": dict(best_axes),
|
||||
"best_status_delta_from_paired_cycles_counter": dict(Counter(best_paired_cycle_offsets)),
|
||||
"best_status_delta_from_trigger_sequence_counter": dict(Counter(best_trigger_sequence_offsets)),
|
||||
"best_status_time_after_trigger_ms_min": min(best_time_offsets),
|
||||
"best_status_time_after_trigger_ms_max": max(best_time_offsets),
|
||||
"best_status_time_after_trigger_ms_avg": sum(best_time_offsets) / len(best_time_offsets),
|
||||
"search_window_cycles": SEARCH_WINDOW_CYCLES,
|
||||
}
|
||||
|
||||
|
||||
def build_manual_compare_rows(trigger_status_rows: list[dict]) -> list[dict]:
|
||||
"""整理成便于人工逐点核对的三时刻对照表。"""
|
||||
rows: list[dict] = []
|
||||
for row in trigger_status_rows:
|
||||
rows.append(
|
||||
{
|
||||
"trigger_no": row["trigger_no"],
|
||||
"waypoint_index": row["waypoint_index"],
|
||||
"trigger_command_sequence": row["trigger_sequence"],
|
||||
"trigger_command_frame": row["trigger_frame_number"],
|
||||
"trigger_command_time_relative_s": row["trigger_time_relative_s"],
|
||||
"trigger_current_status_sequence": row["paired_status_sequence"],
|
||||
"trigger_current_status_frame": row["paired_status_frame_number"],
|
||||
"trigger_current_status_time_relative_s": row["paired_status_time_relative_s"],
|
||||
"command_leads_status_cycles": row["trigger_sequence"] - row["paired_status_sequence"],
|
||||
"trigger_current_status_max_error_axis": row["paired_status_max_error_axis"],
|
||||
"trigger_current_status_max_error_deg": row["paired_status_max_error_deg"],
|
||||
"trigger_current_status_rms_error_deg": row["paired_status_rms_error_deg"],
|
||||
"best_status_sequence": row["best_status_sequence"],
|
||||
"best_status_frame": row["best_status_frame_number"],
|
||||
"best_status_time_relative_s": row["best_status_time_relative_s"],
|
||||
"best_status_delay_from_current_status_cycles": row["best_status_delta_from_paired_cycles"],
|
||||
"best_status_delay_from_trigger_command_cycles": row["best_status_delta_from_trigger_sequence"],
|
||||
"best_status_delay_from_trigger_command_ms": row["best_status_time_after_trigger_ms"],
|
||||
"best_status_max_error_axis": row["best_status_max_error_axis"],
|
||||
"best_status_max_error_deg": row["best_status_max_error_deg"],
|
||||
"best_status_rms_error_deg": row["best_status_rms_error_deg"],
|
||||
}
|
||||
)
|
||||
return rows
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""执行状态反馈提取、触发对齐和摘要落盘。"""
|
||||
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
rows = load_udp_rows(DEFAULT_PCAP, DEFAULT_TSHARK)
|
||||
command_records = decode_command_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
|
||||
status_records = decode_status_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
|
||||
trigger_frames = pick_trigger_first_high_frames(command_records)
|
||||
shot_config = load_uttc_ms11_config()
|
||||
trigger_status_rows = build_trigger_status_rows(trigger_frames, status_records, shot_config)
|
||||
manual_compare_rows = build_manual_compare_rows(trigger_status_rows)
|
||||
summary = build_summary(command_records, trigger_status_rows)
|
||||
|
||||
write_csv(OUTPUT_DIR / "2026042802-1_j519_status_feedback_all.csv", status_records)
|
||||
write_csv(OUTPUT_DIR / "2026042802-1_trigger_status_feedback_vs_teach_points.csv", trigger_status_rows)
|
||||
write_csv(OUTPUT_DIR / "2026042802-1_trigger_manual_compare.csv", manual_compare_rows)
|
||||
(OUTPUT_DIR / "2026042802-1_status_feedback_summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2),
|
||||
encoding="utf-8",
|
||||
)
|
||||
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
250
analysis/analyze_2026042802_1_trigger_vs_teach_points.py
Normal file
250
analysis/analyze_2026042802_1_trigger_vs_teach_points.py
Normal file
@@ -0,0 +1,250 @@
|
||||
#!/usr/bin/env python3
|
||||
"""提取 2026042802-1 抓包中的真实 J519 发包,并对比 UTTC_MS11 示教点。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
import subprocess
|
||||
from collections import Counter
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
DEFAULT_PCAP = REPO_ROOT.parent / "Rvbust" / "uttc-20260428" / "2026042802-1.pcap"
|
||||
DEFAULT_TSHARK = Path(r"D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe")
|
||||
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
|
||||
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
|
||||
RUNTIME_DATA_DIR = REPO_ROOT / "Config" / "Data" / "UTTC_MS11"
|
||||
|
||||
|
||||
def be_u32(data: bytes, offset: int) -> int:
|
||||
"""按大端读取 4 字节无符号整数。"""
|
||||
return struct.unpack(">I", data[offset : offset + 4])[0]
|
||||
|
||||
|
||||
def be_u16(data: bytes, offset: int) -> int:
|
||||
"""按大端读取 2 字节无符号整数。"""
|
||||
return struct.unpack(">H", data[offset : offset + 2])[0]
|
||||
|
||||
|
||||
def be_f32(data: bytes, offset: int) -> float:
|
||||
"""按大端读取 4 字节浮点数。"""
|
||||
return struct.unpack(">f", data[offset : offset + 4])[0]
|
||||
|
||||
|
||||
def load_j519_command_rows(pcap: Path, tshark: Path) -> list[list[str]]:
|
||||
"""只提取 UDP 60015 的原始字段,后续再按 IP 方向筛选真实下发命令。"""
|
||||
command = [
|
||||
str(tshark),
|
||||
"-r",
|
||||
str(pcap),
|
||||
"-Y",
|
||||
"udp.port==60015",
|
||||
"-T",
|
||||
"fields",
|
||||
"-e",
|
||||
"frame.number",
|
||||
"-e",
|
||||
"frame.time_relative",
|
||||
"-e",
|
||||
"ip.src",
|
||||
"-e",
|
||||
"udp.srcport",
|
||||
"-e",
|
||||
"ip.dst",
|
||||
"-e",
|
||||
"udp.dstport",
|
||||
"-e",
|
||||
"udp.payload",
|
||||
]
|
||||
output = subprocess.check_output(command, text=True, encoding="utf-8", errors="ignore")
|
||||
rows: list[list[str]] = []
|
||||
for line in output.splitlines():
|
||||
if not line.strip():
|
||||
continue
|
||||
parts = line.split("\t")
|
||||
if len(parts) >= 7:
|
||||
rows.append(parts[:7])
|
||||
return rows
|
||||
|
||||
|
||||
def decode_command_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
|
||||
"""把抓包中的 64B J519 命令帧解码成带 IO 信息的结构化记录。"""
|
||||
records: list[dict] = []
|
||||
for frame_no, time_rel, ip_src, _udp_src, ip_dst, _udp_dst, payload_hex in rows:
|
||||
if ip_src != client_ip or ip_dst != robot_ip:
|
||||
continue
|
||||
|
||||
payload = bytes.fromhex(payload_hex)
|
||||
if len(payload) != 64:
|
||||
continue
|
||||
|
||||
io_value = be_u16(payload, 0x18)
|
||||
io_addrs = [bit + 1 for bit in range(16) if io_value & (1 << bit)]
|
||||
targets = [be_f32(payload, 0x1C + index * 4) for index in range(9)]
|
||||
records.append(
|
||||
{
|
||||
"frame_number": int(frame_no),
|
||||
"time_relative_s": float(time_rel),
|
||||
"sequence": be_u32(payload, 0x08),
|
||||
"last_data": payload[0x0C],
|
||||
"write_io_type": payload[0x13],
|
||||
"write_io_index": be_u16(payload, 0x14),
|
||||
"write_io_mask": be_u16(payload, 0x16),
|
||||
"write_io_value": io_value,
|
||||
"io_addrs": io_addrs,
|
||||
"j1_deg": targets[0],
|
||||
"j2_deg": targets[1],
|
||||
"j3_deg": targets[2],
|
||||
"j4_deg": targets[3],
|
||||
"j5_deg": targets[4],
|
||||
"j6_deg": targets[5],
|
||||
"ext1_deg": targets[6],
|
||||
"ext2_deg": targets[7],
|
||||
"ext3_deg": targets[8],
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def pick_trigger_first_high_frames(records: list[dict]) -> list[dict]:
|
||||
"""由于 io_keep_cycles=2,只记录每组高电平脉冲的第一帧。"""
|
||||
trigger_frames: list[dict] = []
|
||||
previous_high = False
|
||||
for record in records:
|
||||
current_high = record["write_io_value"] > 0
|
||||
if current_high and not previous_high:
|
||||
trigger_frames.append(record)
|
||||
previous_high = current_high
|
||||
return trigger_frames
|
||||
|
||||
|
||||
def load_uttc_ms11_config() -> dict:
|
||||
"""读取 UTTC_MS11 的示教点和触发配置。"""
|
||||
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
return config["flying_shots"]["UTTC_MS11"]
|
||||
|
||||
|
||||
def build_trigger_vs_teach_rows(trigger_frames: list[dict], shot_config: dict) -> list[dict]:
|
||||
"""按 shot_flags 为 true 的 waypoint 顺序,对齐抓包触发帧和示教点。"""
|
||||
rows: list[dict] = []
|
||||
trigger_waypoint_indices = [index for index, flag in enumerate(shot_config["shot_flags"]) if flag]
|
||||
|
||||
for trigger_no, (frame, waypoint_index) in enumerate(zip(trigger_frames, trigger_waypoint_indices), start=1):
|
||||
teach_rad = shot_config["traj_waypoints"][waypoint_index]
|
||||
teach_deg = [math.degrees(value) for value in teach_rad]
|
||||
actual_deg = [frame[f"j{joint_index}_deg"] for joint_index in range(1, 7)]
|
||||
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
|
||||
abs_diffs = [abs(value) for value in diffs]
|
||||
max_error = max(abs_diffs)
|
||||
max_error_axis = f"J{abs_diffs.index(max_error) + 1}"
|
||||
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
|
||||
|
||||
row = {
|
||||
"trigger_no": trigger_no,
|
||||
"waypoint_index": waypoint_index,
|
||||
"frame_number": frame["frame_number"],
|
||||
"sequence": frame["sequence"],
|
||||
"time_relative_s": frame["time_relative_s"],
|
||||
"write_io_value": frame["write_io_value"],
|
||||
"io_addrs": frame["io_addrs"],
|
||||
"config_addr": shot_config["addr"][waypoint_index],
|
||||
"max_error_axis": max_error_axis,
|
||||
"max_error_deg": max_error,
|
||||
"rms_error_deg": rms_error,
|
||||
}
|
||||
|
||||
for joint_index in range(6):
|
||||
joint_no = joint_index + 1
|
||||
row[f"j{joint_no}_actual_deg"] = actual_deg[joint_index]
|
||||
row[f"j{joint_no}_teach_deg"] = teach_deg[joint_index]
|
||||
row[f"diff_j{joint_no}_deg"] = diffs[joint_index]
|
||||
|
||||
rows.append(row)
|
||||
|
||||
return rows
|
||||
|
||||
|
||||
def write_csv(path: Path, rows: list[dict]) -> None:
|
||||
"""把分析结果落成 UTF-8 CSV,便于后续继续筛选和画图。"""
|
||||
if not rows:
|
||||
raise ValueError(f"No rows to write: {path}")
|
||||
|
||||
serializable_rows: list[dict] = []
|
||||
for row in rows:
|
||||
serializable_row: dict = {}
|
||||
for key, value in row.items():
|
||||
if isinstance(value, list):
|
||||
serializable_row[key] = json.dumps(value, ensure_ascii=False)
|
||||
else:
|
||||
serializable_row[key] = value
|
||||
serializable_rows.append(serializable_row)
|
||||
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.DictWriter(handle, fieldnames=list(serializable_rows[0].keys()))
|
||||
writer.writeheader()
|
||||
writer.writerows(serializable_rows)
|
||||
|
||||
|
||||
def build_summary(records: list[dict], trigger_rows: list[dict]) -> dict:
|
||||
"""汇总本次分析关心的导出状态和误差统计。"""
|
||||
max_errors = [float(row["max_error_deg"]) for row in trigger_rows]
|
||||
rms_errors = [float(row["rms_error_deg"]) for row in trigger_rows]
|
||||
axis_counter = Counter(str(row["max_error_axis"]) for row in trigger_rows)
|
||||
|
||||
order_only_addr_mismatch = 0
|
||||
real_addr_mismatch = 0
|
||||
for row in trigger_rows:
|
||||
io_addrs = list(row["io_addrs"])
|
||||
config_addr = list(row["config_addr"])
|
||||
if io_addrs != config_addr:
|
||||
if sorted(io_addrs) == sorted(config_addr):
|
||||
order_only_addr_mismatch += 1
|
||||
else:
|
||||
real_addr_mismatch += 1
|
||||
|
||||
return {
|
||||
"pcap_path": str(DEFAULT_PCAP),
|
||||
"all_command_count": len(records),
|
||||
"trigger_count": len(trigger_rows),
|
||||
"existing_runtime_actual_send_exists": (RUNTIME_DATA_DIR / "ActualSendJointTraj.txt").exists(),
|
||||
"existing_runtime_actual_send_has_io_columns": False,
|
||||
"existing_shot_events_exists": (RUNTIME_DATA_DIR / "ShotEvents.json").exists(),
|
||||
"pcap_specific_combined_export_preexisting": False,
|
||||
"average_max_error_deg": sum(max_errors) / len(max_errors),
|
||||
"max_error_deg": max(max_errors),
|
||||
"average_rms_error_deg": sum(rms_errors) / len(rms_errors),
|
||||
"max_error_axis_counter": dict(axis_counter),
|
||||
"order_only_addr_mismatch_count": order_only_addr_mismatch,
|
||||
"real_addr_mismatch_count": real_addr_mismatch,
|
||||
}
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""执行抓包提取、示教点对齐、CSV 导出和摘要落盘。"""
|
||||
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
rows = load_j519_command_rows(DEFAULT_PCAP, DEFAULT_TSHARK)
|
||||
records = decode_command_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
|
||||
trigger_frames = pick_trigger_first_high_frames(records)
|
||||
shot_config = load_uttc_ms11_config()
|
||||
trigger_rows = build_trigger_vs_teach_rows(trigger_frames, shot_config)
|
||||
summary = build_summary(records, trigger_rows)
|
||||
|
||||
write_csv(OUTPUT_DIR / "2026042802-1_j519_actual_send_all_with_io.csv", records)
|
||||
write_csv(OUTPUT_DIR / "2026042802-1_j519_trigger_frames.csv", trigger_frames)
|
||||
write_csv(OUTPUT_DIR / "2026042802-1_trigger_vs_teach_points.csv", trigger_rows)
|
||||
(OUTPUT_DIR / "2026042802-1_analysis_summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2),
|
||||
encoding="utf-8",
|
||||
)
|
||||
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
137
analysis/build_trigger_offset_compare_6_7_8.py
Normal file
137
analysis/build_trigger_offset_compare_6_7_8.py
Normal file
@@ -0,0 +1,137 @@
|
||||
#!/usr/bin/env python3
|
||||
"""生成 2026042802-1 抓包中触发偏移 6/7/8 周期的对照表。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||||
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
|
||||
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
|
||||
ACTUAL_SEND_CSV = OUTPUT_DIR / "2026042802-1_j519_actual_send_all_with_io.csv"
|
||||
TRIGGER_CSV = OUTPUT_DIR / "2026042802-1_j519_trigger_frames.csv"
|
||||
OUTPUT_CSV = OUTPUT_DIR / "2026042802-1_trigger_offset_6_7_8_compare.csv"
|
||||
OUTPUT_JSON = OUTPUT_DIR / "2026042802-1_trigger_offset_6_7_8_summary.json"
|
||||
|
||||
|
||||
def load_rows(path: Path) -> list[dict]:
|
||||
with path.open(encoding="utf-8") as handle:
|
||||
return list(csv.DictReader(handle))
|
||||
|
||||
|
||||
def to_float_list(record: dict, prefix: str = "j") -> list[float]:
|
||||
return [float(record[f"{prefix}{index}_deg"]) for index in range(1, 7)]
|
||||
|
||||
|
||||
def compute_diff_metrics(actual_deg: list[float], teach_deg: list[float]) -> tuple[list[float], float, float, str]:
|
||||
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
|
||||
abs_diffs = [abs(value) for value in diffs]
|
||||
max_error = max(abs_diffs)
|
||||
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
|
||||
max_axis = f"J{abs_diffs.index(max_error) + 1}"
|
||||
return diffs, max_error, rms_error, max_axis
|
||||
|
||||
|
||||
def main() -> None:
|
||||
actual_rows = load_rows(ACTUAL_SEND_CSV)
|
||||
trigger_rows = load_rows(TRIGGER_CSV)
|
||||
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))["flying_shots"]["UTTC_MS11"]
|
||||
trigger_waypoint_indices = [index for index, flag in enumerate(config["shot_flags"]) if flag]
|
||||
|
||||
actual_index_by_frame = {int(row["frame_number"]): row for row in actual_rows}
|
||||
actual_order_by_frame = {int(row["frame_number"]): idx for idx, row in enumerate(actual_rows)}
|
||||
|
||||
output_rows: list[dict] = []
|
||||
offset_win_counts = {6: 0, 7: 0, 8: 0}
|
||||
|
||||
for trigger_no, (trigger_row, waypoint_index) in enumerate(zip(trigger_rows, trigger_waypoint_indices), start=1):
|
||||
trigger_frame = int(trigger_row["frame_number"])
|
||||
trigger_order = actual_order_by_frame[trigger_frame]
|
||||
teach_deg = [math.degrees(value) for value in config["traj_waypoints"][waypoint_index]]
|
||||
|
||||
window_start = max(0, trigger_order - 20)
|
||||
window_end = min(len(actual_rows) - 1, trigger_order + 20)
|
||||
best_order = trigger_order
|
||||
best_rms = float("inf")
|
||||
best_max = float("inf")
|
||||
|
||||
for candidate_order in range(window_start, window_end + 1):
|
||||
candidate = actual_rows[candidate_order]
|
||||
_, max_error, rms_error, _ = compute_diff_metrics(to_float_list(candidate), teach_deg)
|
||||
score = (rms_error, max_error, abs(candidate_order - trigger_order))
|
||||
if (best_rms, best_max, abs(best_order - trigger_order)) > score:
|
||||
best_order = candidate_order
|
||||
best_rms = rms_error
|
||||
best_max = max_error
|
||||
|
||||
row = {
|
||||
"trigger_no": trigger_no,
|
||||
"waypoint_index": waypoint_index,
|
||||
"best_sample_order": best_order,
|
||||
"best_frame_number": int(actual_rows[best_order]["frame_number"]),
|
||||
"best_sequence": int(actual_rows[best_order]["sequence"]),
|
||||
"best_time_relative_s": float(actual_rows[best_order]["time_relative_s"]),
|
||||
}
|
||||
|
||||
for joint_index in range(6):
|
||||
row[f"teach_j{joint_index + 1}_deg"] = teach_deg[joint_index]
|
||||
|
||||
for offset in (6, 7, 8):
|
||||
target_order = min(len(actual_rows) - 1, best_order + offset)
|
||||
target = actual_rows[target_order]
|
||||
actual_deg = to_float_list(target)
|
||||
diffs, max_error, rms_error, max_axis = compute_diff_metrics(actual_deg, teach_deg)
|
||||
|
||||
row[f"offset_{offset}_frame_number"] = int(target["frame_number"])
|
||||
row[f"offset_{offset}_sequence"] = int(target["sequence"])
|
||||
row[f"offset_{offset}_time_relative_s"] = float(target["time_relative_s"])
|
||||
row[f"offset_{offset}_max_error_axis"] = max_axis
|
||||
row[f"offset_{offset}_max_error_deg"] = max_error
|
||||
row[f"offset_{offset}_rms_error_deg"] = rms_error
|
||||
row[f"offset_{offset}_delta_from_best_ms"] = (
|
||||
float(target["time_relative_s"]) - float(actual_rows[best_order]["time_relative_s"])
|
||||
) * 1000.0
|
||||
for joint_index in range(6):
|
||||
joint_no = joint_index + 1
|
||||
row[f"offset_{offset}_j{joint_no}_actual_deg"] = actual_deg[joint_index]
|
||||
row[f"offset_{offset}_diff_j{joint_no}_deg"] = diffs[joint_index]
|
||||
|
||||
best_offset = min(
|
||||
(6, 7, 8),
|
||||
key=lambda offset: (
|
||||
row[f"offset_{offset}_rms_error_deg"],
|
||||
row[f"offset_{offset}_max_error_deg"],
|
||||
),
|
||||
)
|
||||
row["best_of_6_7_8_offset"] = best_offset
|
||||
offset_win_counts[best_offset] += 1
|
||||
output_rows.append(row)
|
||||
|
||||
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
|
||||
with OUTPUT_CSV.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.DictWriter(handle, fieldnames=list(output_rows[0].keys()))
|
||||
writer.writeheader()
|
||||
writer.writerows(output_rows)
|
||||
|
||||
summary = {
|
||||
"rows": len(output_rows),
|
||||
"best_offset_win_counts": offset_win_counts,
|
||||
"average_max_error_deg": {
|
||||
str(offset): sum(row[f"offset_{offset}_max_error_deg"] for row in output_rows) / len(output_rows)
|
||||
for offset in (6, 7, 8)
|
||||
},
|
||||
"average_rms_error_deg": {
|
||||
str(offset): sum(row[f"offset_{offset}_rms_error_deg"] for row in output_rows) / len(output_rows)
|
||||
for offset in (6, 7, 8)
|
||||
},
|
||||
}
|
||||
OUTPUT_JSON.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
461
analysis/calc_jointdetail_acceleration.py
Normal file
461
analysis/calc_jointdetail_acceleration.py
Normal file
@@ -0,0 +1,461 @@
|
||||
#!/usr/bin/env python3
|
||||
"""计算 JointDetialTraj 类文件的速度 / 加速度 / 跃度峰值,并与当前生效轴限位对比。
|
||||
|
||||
输入格式:
|
||||
time joint1 joint2 ... jointN
|
||||
|
||||
本脚本采用的规则:
|
||||
1. 将轨迹按离散时间采样点读取,允许时间轴非等间隔。
|
||||
2. 自动推断角度单位:
|
||||
- 任一关节绝对值超过 2*pi*1.5,则按 degree 处理
|
||||
- 否则按 radian 处理
|
||||
3. 使用后向差分计算导数:
|
||||
v_i = (q_i - q_{i-1}) / dt_i
|
||||
a_i = (v_i - v_{i-1}) / dt_i
|
||||
j_i = (a_i - a_{i-1}) / dt_i
|
||||
4. 所有导数量统一换算成 rad 基单位,再与当前生效的机器人限值比较。
|
||||
|
||||
当前生效限值来源:
|
||||
.robot limit.velocity
|
||||
.robot limit.acceleration * RobotConfig.robot.acc_limit
|
||||
.robot limit.jerk * RobotConfig.robot.jerk_limit
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
|
||||
AUTO_DEG_THRESHOLD = 2.0 * math.pi * 1.5
|
||||
DEFAULT_VELOCITY_LIMITS = [6.45, 5.41, 7.15, 9.59, 9.51, 17.45]
|
||||
DEFAULT_ACCELERATION_LIMITS = [26.90, 22.54, 29.81, 39.99, 39.63, 72.72]
|
||||
DEFAULT_JERK_LIMITS = [224.22, 187.86, 248.46, 333.30, 330.27, 606.01]
|
||||
DEFAULT_JOINT_NAMES = [f"Joint{index}" for index in range(1, 7)]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointLimit:
|
||||
name: str
|
||||
velocity: float
|
||||
acceleration: float
|
||||
jerk: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PeakMetric:
|
||||
joint_name: str
|
||||
axis_index: int
|
||||
window_start: float
|
||||
window_end: float
|
||||
row_number: int
|
||||
metric_native: float
|
||||
metric_rad: float
|
||||
effective_limit_rad: float
|
||||
|
||||
@property
|
||||
def ratio_vs_limit(self) -> float:
|
||||
return abs(self.metric_rad) / self.effective_limit_rad
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EffectiveLimits:
|
||||
joints: list[JointLimit]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Calculate velocity / acceleration / jerk peaks from JointDetialTraj.txt and compare with built-in effective robot limits."
|
||||
)
|
||||
parser.add_argument("joint_detail", type=Path, help="Path to JointDetialTraj.txt")
|
||||
parser.add_argument(
|
||||
"--limit-csv",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="Optional CSV file with columns: Joint,Velocity,Acceleration,Jerk . If omitted, use built-in 1/1 effective limits.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--unit",
|
||||
choices=("auto", "rad", "deg"),
|
||||
default="auto",
|
||||
help="Input joint-angle unit. Default: auto.",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def resolve_path(path: Path) -> Path:
|
||||
return path if path.is_absolute() else (Path.cwd() / path).resolve()
|
||||
|
||||
|
||||
def read_joint_rows(path: Path) -> list[list[float]]:
|
||||
rows: list[list[float]] = []
|
||||
for raw_line in path.read_text(encoding="utf-8").splitlines():
|
||||
line = raw_line.strip()
|
||||
if not line:
|
||||
continue
|
||||
rows.append([float(part) for part in line.split()])
|
||||
|
||||
if len(rows) < 4:
|
||||
raise ValueError(f"{path} must contain at least 4 rows to calculate jerk.")
|
||||
|
||||
width = len(rows[0])
|
||||
if width < 3:
|
||||
raise ValueError(f"{path} must contain time + at least 2 joint columns.")
|
||||
|
||||
for index, row in enumerate(rows, start=1):
|
||||
if len(row) != width:
|
||||
raise ValueError(f"{path} line {index} has inconsistent column count.")
|
||||
|
||||
return rows
|
||||
|
||||
|
||||
def trim_rows_to_limit_count(rows: list[list[float]], limit_count: int) -> tuple[list[list[float]], str | None]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count == limit_count:
|
||||
return rows, None
|
||||
|
||||
if joint_count < limit_count:
|
||||
raise ValueError(f"Joint column count ({joint_count}) is smaller than robot limit count ({limit_count}).")
|
||||
|
||||
trimmed_rows = [row[: limit_count + 1] for row in rows]
|
||||
ignored_joint_count = joint_count - limit_count
|
||||
trim_note = (
|
||||
f"ignored_joint_columns={ignored_joint_count} "
|
||||
f"(using first {limit_count} joints out of {joint_count}; trailing columns treated as external axes/placeholders)"
|
||||
)
|
||||
return trimmed_rows, trim_note
|
||||
|
||||
|
||||
def infer_unit(rows: Iterable[list[float]], requested_unit: str) -> str:
|
||||
if requested_unit != "auto":
|
||||
return requested_unit
|
||||
|
||||
max_abs_joint = max(abs(value) for row in rows for value in row[1:])
|
||||
return "deg" if max_abs_joint > AUTO_DEG_THRESHOLD else "rad"
|
||||
|
||||
|
||||
def read_limit_csv(path: Path) -> list[JointLimit]:
|
||||
rows = [line.strip() for line in path.read_text(encoding="utf-8").splitlines() if line.strip()]
|
||||
if len(rows) < 2:
|
||||
raise ValueError(f"{path} must contain a header and at least one data row.")
|
||||
|
||||
header = [part.strip().lower() for part in rows[0].split(",")]
|
||||
expected = ["joint", "velocity", "acceleration", "jerk"]
|
||||
if header != expected:
|
||||
raise ValueError(f"{path} header must be: Joint,Velocity,Acceleration,Jerk")
|
||||
|
||||
limits: list[JointLimit] = []
|
||||
for row_index, row in enumerate(rows[1:], start=2):
|
||||
parts = [part.strip() for part in row.split(",")]
|
||||
if len(parts) != 4:
|
||||
raise ValueError(f"{path} line {row_index} must contain 4 columns.")
|
||||
limits.append(
|
||||
JointLimit(
|
||||
name=parts[0],
|
||||
velocity=float(parts[1]),
|
||||
acceleration=float(parts[2]),
|
||||
jerk=float(parts[3]),
|
||||
)
|
||||
)
|
||||
return limits
|
||||
|
||||
|
||||
def load_effective_limits(limit_csv_path: Path | None) -> EffectiveLimits:
|
||||
if limit_csv_path is not None:
|
||||
limits = read_limit_csv(resolve_path(limit_csv_path))
|
||||
else:
|
||||
limits = [
|
||||
JointLimit(
|
||||
name=name,
|
||||
velocity=velocity,
|
||||
acceleration=acceleration,
|
||||
jerk=jerk,
|
||||
)
|
||||
for name, velocity, acceleration, jerk in zip(
|
||||
DEFAULT_JOINT_NAMES,
|
||||
DEFAULT_VELOCITY_LIMITS,
|
||||
DEFAULT_ACCELERATION_LIMITS,
|
||||
DEFAULT_JERK_LIMITS,
|
||||
strict=True,
|
||||
)
|
||||
]
|
||||
|
||||
return EffectiveLimits(joints=limits)
|
||||
|
||||
|
||||
def to_radians(value: float, unit: str) -> float:
|
||||
return math.radians(value) if unit == "deg" else value
|
||||
|
||||
|
||||
def to_native_from_rad(value: float, unit: str) -> float:
|
||||
return math.degrees(value) if unit == "deg" else value
|
||||
|
||||
|
||||
def calculate_velocity_peaks(rows: list[list[float]], unit: str, limits: list[JointLimit]) -> list[PeakMetric]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count != len(limits):
|
||||
raise ValueError(f"Joint column count ({joint_count}) does not match robot limit count ({len(limits)}).")
|
||||
|
||||
peaks: list[PeakMetric | None] = [None] * joint_count
|
||||
|
||||
for row_index in range(1, len(rows)):
|
||||
previous = rows[row_index - 1]
|
||||
current = rows[row_index]
|
||||
dt = current[0] - previous[0]
|
||||
if dt <= 0.0:
|
||||
raise ValueError(f"Non-positive dt at line {row_index + 1}: {dt}")
|
||||
|
||||
for joint_index in range(joint_count):
|
||||
dq_native = current[joint_index + 1] - previous[joint_index + 1]
|
||||
dq_rad = to_radians(dq_native, unit)
|
||||
velocity_rad = dq_rad / dt
|
||||
velocity_native = to_native_from_rad(velocity_rad, unit)
|
||||
|
||||
candidate = PeakMetric(
|
||||
joint_name=limits[joint_index].name,
|
||||
axis_index=joint_index + 1,
|
||||
window_start=previous[0],
|
||||
window_end=current[0],
|
||||
row_number=row_index + 1,
|
||||
metric_native=velocity_native,
|
||||
metric_rad=velocity_rad,
|
||||
effective_limit_rad=limits[joint_index].velocity,
|
||||
)
|
||||
|
||||
current_peak = peaks[joint_index]
|
||||
if current_peak is None or abs(candidate.metric_rad) > abs(current_peak.metric_rad):
|
||||
peaks[joint_index] = candidate
|
||||
|
||||
return [peak for peak in peaks if peak is not None]
|
||||
|
||||
|
||||
def calculate_acceleration_peaks(rows: list[list[float]], unit: str, limits: list[JointLimit]) -> list[PeakMetric]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count != len(limits):
|
||||
raise ValueError(f"Joint column count ({joint_count}) does not match robot limit count ({len(limits)}).")
|
||||
|
||||
velocities_rad: list[list[float]] = []
|
||||
velocity_windows: list[tuple[float, float, int]] = []
|
||||
|
||||
for row_index in range(1, len(rows)):
|
||||
previous = rows[row_index - 1]
|
||||
current = rows[row_index]
|
||||
dt = current[0] - previous[0]
|
||||
if dt <= 0.0:
|
||||
raise ValueError(f"Non-positive dt at line {row_index + 1}: {dt}")
|
||||
|
||||
velocity_row = []
|
||||
for joint_index in range(joint_count):
|
||||
dq_native = current[joint_index + 1] - previous[joint_index + 1]
|
||||
dq_rad = to_radians(dq_native, unit)
|
||||
velocity_row.append(dq_rad / dt)
|
||||
|
||||
velocities_rad.append(velocity_row)
|
||||
velocity_windows.append((previous[0], current[0], row_index + 1))
|
||||
|
||||
peaks: list[PeakMetric | None] = [None] * joint_count
|
||||
|
||||
for velocity_index in range(1, len(velocities_rad)):
|
||||
dt = velocity_windows[velocity_index][1] - velocity_windows[velocity_index][0]
|
||||
for joint_index in range(joint_count):
|
||||
acceleration_rad = (velocities_rad[velocity_index][joint_index] - velocities_rad[velocity_index - 1][joint_index]) / dt
|
||||
acceleration_native = to_native_from_rad(acceleration_rad, unit)
|
||||
|
||||
candidate = PeakMetric(
|
||||
joint_name=limits[joint_index].name,
|
||||
axis_index=joint_index + 1,
|
||||
window_start=velocity_windows[velocity_index][0],
|
||||
window_end=velocity_windows[velocity_index][1],
|
||||
row_number=velocity_windows[velocity_index][2],
|
||||
metric_native=acceleration_native,
|
||||
metric_rad=acceleration_rad,
|
||||
effective_limit_rad=limits[joint_index].acceleration,
|
||||
)
|
||||
|
||||
current_peak = peaks[joint_index]
|
||||
if current_peak is None or abs(candidate.metric_rad) > abs(current_peak.metric_rad):
|
||||
peaks[joint_index] = candidate
|
||||
|
||||
return [peak for peak in peaks if peak is not None]
|
||||
|
||||
|
||||
def calculate_jerk_peaks(rows: list[list[float]], unit: str, limits: list[JointLimit]) -> list[PeakMetric]:
|
||||
joint_count = len(rows[0]) - 1
|
||||
if joint_count != len(limits):
|
||||
raise ValueError(f"Joint column count ({joint_count}) does not match robot limit count ({len(limits)}).")
|
||||
|
||||
velocities_rad: list[list[float]] = []
|
||||
velocity_windows: list[tuple[float, float, int]] = []
|
||||
for row_index in range(1, len(rows)):
|
||||
previous = rows[row_index - 1]
|
||||
current = rows[row_index]
|
||||
dt = current[0] - previous[0]
|
||||
if dt <= 0.0:
|
||||
raise ValueError(f"Non-positive dt at line {row_index + 1}: {dt}")
|
||||
|
||||
velocity_row = []
|
||||
for joint_index in range(joint_count):
|
||||
dq_native = current[joint_index + 1] - previous[joint_index + 1]
|
||||
dq_rad = to_radians(dq_native, unit)
|
||||
velocity_row.append(dq_rad / dt)
|
||||
|
||||
velocities_rad.append(velocity_row)
|
||||
velocity_windows.append((previous[0], current[0], row_index + 1))
|
||||
|
||||
accelerations_rad: list[list[float]] = []
|
||||
acceleration_windows: list[tuple[float, float, int]] = []
|
||||
for velocity_index in range(1, len(velocities_rad)):
|
||||
dt = velocity_windows[velocity_index][1] - velocity_windows[velocity_index][0]
|
||||
acceleration_row = []
|
||||
for joint_index in range(joint_count):
|
||||
acceleration_row.append((velocities_rad[velocity_index][joint_index] - velocities_rad[velocity_index - 1][joint_index]) / dt)
|
||||
accelerations_rad.append(acceleration_row)
|
||||
acceleration_windows.append((velocity_windows[velocity_index][0], velocity_windows[velocity_index][1], velocity_windows[velocity_index][2]))
|
||||
|
||||
peaks: list[PeakMetric | None] = [None] * joint_count
|
||||
|
||||
for acceleration_index in range(1, len(accelerations_rad)):
|
||||
dt = acceleration_windows[acceleration_index][1] - acceleration_windows[acceleration_index][0]
|
||||
for joint_index in range(joint_count):
|
||||
jerk_rad = (accelerations_rad[acceleration_index][joint_index] - accelerations_rad[acceleration_index - 1][joint_index]) / dt
|
||||
jerk_native = to_native_from_rad(jerk_rad, unit)
|
||||
|
||||
candidate = PeakMetric(
|
||||
joint_name=limits[joint_index].name,
|
||||
axis_index=joint_index + 1,
|
||||
window_start=acceleration_windows[acceleration_index][0],
|
||||
window_end=acceleration_windows[acceleration_index][1],
|
||||
row_number=acceleration_windows[acceleration_index][2],
|
||||
metric_native=jerk_native,
|
||||
metric_rad=jerk_rad,
|
||||
effective_limit_rad=limits[joint_index].jerk,
|
||||
)
|
||||
|
||||
current_peak = peaks[joint_index]
|
||||
if current_peak is None or abs(candidate.metric_rad) > abs(current_peak.metric_rad):
|
||||
peaks[joint_index] = candidate
|
||||
|
||||
return [peak for peak in peaks if peak is not None]
|
||||
|
||||
|
||||
def format_table(peaks: list[PeakMetric], native_unit: str, rad_unit: str, limit_header: str) -> str:
|
||||
lines = [
|
||||
f"{'Joint':<8} {'Window(s)':<20} {'Line':>6} {'Peak(' + native_unit + ')':>18} {'Peak(' + rad_unit + ')':>18} {limit_header:>20} {'Ratio':>10}",
|
||||
"-" * 108,
|
||||
]
|
||||
for peak in peaks:
|
||||
lines.append(
|
||||
f"{peak.joint_name:<8} "
|
||||
f"{peak.window_start:>7.6f}->{peak.window_end:<10.6f} "
|
||||
f"{peak.row_number:>6} "
|
||||
f"{peak.metric_native:>18.6f} "
|
||||
f"{peak.metric_rad:>18.6f} "
|
||||
f"{peak.effective_limit_rad:>20.6f} "
|
||||
f"{peak.ratio_vs_limit:>10.4f}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def print_metric_section(title: str, peaks: list[PeakMetric], unit: str, native_suffix: str, rad_suffix: str, limit_header: str) -> None:
|
||||
print(build_metric_section(title, peaks, unit, native_suffix, rad_suffix, limit_header))
|
||||
|
||||
|
||||
def build_metric_section(title: str, peaks: list[PeakMetric], unit: str, native_suffix: str, rad_suffix: str, limit_header: str) -> str:
|
||||
native_unit = f"deg/{native_suffix}" if unit == "deg" else f"rad/{native_suffix}"
|
||||
rad_unit = f"rad/{rad_suffix}"
|
||||
lines = [title, format_table(peaks, native_unit, rad_unit, limit_header)]
|
||||
worst = max(peaks, key=lambda item: item.ratio_vs_limit)
|
||||
metric_key = title.lower().replace(" ", "_")
|
||||
lines.append(
|
||||
f"worst_{metric_key}="
|
||||
f"{worst.joint_name}, window={worst.window_start:.6f}->{worst.window_end:.6f}, "
|
||||
f"peak_rad={worst.metric_rad:.6f}, limit_rad={worst.effective_limit_rad:.6f}, "
|
||||
f"ratio={worst.ratio_vs_limit:.4f}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def build_report_text(
|
||||
joint_detail_path: Path,
|
||||
rows: list[list[float]],
|
||||
unit: str,
|
||||
max_abs_joint: float,
|
||||
limit_source_text: str,
|
||||
trim_note: str | None,
|
||||
velocity_peaks: list[PeakMetric],
|
||||
acceleration_peaks: list[PeakMetric],
|
||||
jerk_peaks: list[PeakMetric],
|
||||
) -> str:
|
||||
lines = [
|
||||
f"joint_detail={joint_detail_path}",
|
||||
limit_source_text,
|
||||
f"row_count={len(rows)}",
|
||||
f"joint_count={len(rows[0]) - 1}",
|
||||
f"inferred_unit={unit}",
|
||||
f"max_abs_joint_value={max_abs_joint:.6f}",
|
||||
]
|
||||
if trim_note is not None:
|
||||
lines.append(trim_note)
|
||||
lines.extend(
|
||||
[
|
||||
"",
|
||||
build_metric_section("Velocity Peaks", velocity_peaks, unit, "s", "s", "VelLimit(rad/s)"),
|
||||
"",
|
||||
build_metric_section("Acceleration Peaks", acceleration_peaks, unit, "s^2", "s^2", "AccLimit(rad/s^2)"),
|
||||
"",
|
||||
build_metric_section("Jerk Peaks", jerk_peaks, unit, "s^3", "s^3", "JerkLimit(rad/s^3)"),
|
||||
"",
|
||||
]
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
joint_detail_path = resolve_path(args.joint_detail)
|
||||
rows = read_joint_rows(joint_detail_path)
|
||||
limits_info = load_effective_limits(args.limit_csv)
|
||||
rows, trim_note = trim_rows_to_limit_count(rows, len(limits_info.joints))
|
||||
unit = infer_unit(rows, args.unit)
|
||||
|
||||
velocity_peaks = calculate_velocity_peaks(rows, unit, limits_info.joints)
|
||||
acceleration_peaks = calculate_acceleration_peaks(rows, unit, limits_info.joints)
|
||||
jerk_peaks = calculate_jerk_peaks(rows, unit, limits_info.joints)
|
||||
|
||||
max_abs_joint = max(abs(value) for row in rows for value in row[1:])
|
||||
|
||||
if args.limit_csv is None:
|
||||
limit_source_text = "limit_source=built-in fixed effective limits (acc_limit=1, jerk_limit=1)"
|
||||
else:
|
||||
limit_source_text = f"limit_source_csv={resolve_path(args.limit_csv)}"
|
||||
|
||||
report_text = build_report_text(
|
||||
joint_detail_path=joint_detail_path,
|
||||
rows=rows,
|
||||
unit=unit,
|
||||
max_abs_joint=max_abs_joint,
|
||||
limit_source_text=limit_source_text,
|
||||
trim_note=trim_note,
|
||||
velocity_peaks=velocity_peaks,
|
||||
acceleration_peaks=acceleration_peaks,
|
||||
jerk_peaks=jerk_peaks,
|
||||
)
|
||||
|
||||
print(report_text, end="")
|
||||
|
||||
output_path = joint_detail_path.with_suffix(".analysis.txt")
|
||||
try:
|
||||
output_path.write_text(report_text, encoding="utf-8")
|
||||
print(f"saved_report={output_path}")
|
||||
except PermissionError as error:
|
||||
print(f"save_report_failed={output_path}")
|
||||
print(f"save_report_error={error}")
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
199
analysis/fanuc_10010_dissector.lua
Normal file
199
analysis/fanuc_10010_dissector.lua
Normal file
@@ -0,0 +1,199 @@
|
||||
local plugin_info = {
|
||||
version = "1.1.0",
|
||||
author = "OpenAI Codex",
|
||||
description = "FANUC TCP 10010 状态帧解析器",
|
||||
}
|
||||
|
||||
set_plugin_info(plugin_info)
|
||||
|
||||
local fanuc10010_proto = Proto("fanuc10010_state_frame", "FANUC 10010 State Frame")
|
||||
|
||||
local FRAME_LENGTH = 90
|
||||
local HEADER_LENGTH = 3
|
||||
local TRAILER_LENGTH = 3
|
||||
local LENGTH_OFFSET = 3
|
||||
local MESSAGE_ID_OFFSET = 7
|
||||
local POSE_OFFSET = 11
|
||||
local POSE_COUNT = 6
|
||||
local JOINT_OFFSET = 35
|
||||
local JOINT_COUNT = 9
|
||||
local TAIL_OFFSET = 71
|
||||
local TAIL_COUNT = 4
|
||||
|
||||
local POSE_NAMES = { "X", "Y", "Z", "W", "P", "R" }
|
||||
local JOINT_NAMES = { "J1", "J2", "J3", "J4", "J5", "J6", "Ext1", "Ext2", "Ext3" }
|
||||
local HEADER_MAGIC = "doz"
|
||||
local TRAILER_MAGIC = "zod"
|
||||
|
||||
local fields = {
|
||||
frame = ProtoField.bytes("fanuc10010.frame", "原始状态帧"),
|
||||
header = ProtoField.string("fanuc10010.header", "帧头 Magic"),
|
||||
declared_length = ProtoField.uint32("fanuc10010.length", "声明长度", base.DEC),
|
||||
message_id = ProtoField.uint32("fanuc10010.message_id", "消息号", base.DEC),
|
||||
pose = ProtoField.none("fanuc10010.pose", "笛卡尔位姿"),
|
||||
joints = ProtoField.none("fanuc10010.joints", "关节与扩展轴"),
|
||||
joint_degrees = ProtoField.none("fanuc10010.joint_degrees", "Joint Degrees"),
|
||||
tail = ProtoField.none("fanuc10010.tail", "尾部状态字"),
|
||||
trailer = ProtoField.string("fanuc10010.trailer", "帧尾 Magic"),
|
||||
expert_bad_length = ProtoField.string("fanuc10010.expert.bad_length", "长度异常"),
|
||||
expert_bad_magic = ProtoField.string("fanuc10010.expert.bad_magic", "Magic 异常"),
|
||||
}
|
||||
|
||||
for index = 1, POSE_COUNT do
|
||||
fields["pose_" .. index] = ProtoField.float(
|
||||
"fanuc10010.pose_" .. index,
|
||||
"Pose " .. POSE_NAMES[index])
|
||||
end
|
||||
|
||||
for index = 1, JOINT_COUNT do
|
||||
fields["joint_" .. index] = ProtoField.float(
|
||||
"fanuc10010.joint_" .. index,
|
||||
JOINT_NAMES[index] .. " (raw)")
|
||||
end
|
||||
|
||||
for index = 1, 6 do
|
||||
fields["joint_deg_" .. index] = ProtoField.float(
|
||||
"fanuc10010.joint_deg_" .. index,
|
||||
JOINT_NAMES[index] .. " (deg)")
|
||||
end
|
||||
|
||||
for index = 1, TAIL_COUNT do
|
||||
fields["tail_" .. index] = ProtoField.uint32(
|
||||
"fanuc10010.tail_" .. index,
|
||||
"Tail[" .. (index - 1) .. "]",
|
||||
base.DEC)
|
||||
end
|
||||
|
||||
fanuc10010_proto.fields = fields
|
||||
|
||||
local function read_f32_be(tvb, offset)
|
||||
return tvb(offset, 4):tvb():range(0, 4):float()
|
||||
end
|
||||
|
||||
local function read_u32_be(tvb, offset)
|
||||
return tvb(offset, 4):uint()
|
||||
end
|
||||
|
||||
local function read_ascii(tvb, offset, length)
|
||||
return tvb(offset, length):string()
|
||||
end
|
||||
|
||||
local function radians_to_degrees(value)
|
||||
return value * 180.0 / math.pi
|
||||
end
|
||||
|
||||
local function add_error(subtree, range, field, message)
|
||||
local item = subtree:add(field, range, message)
|
||||
item:add_expert_info(PI_MALFORMED, PI_ERROR, message)
|
||||
end
|
||||
|
||||
local function dissect_single_frame(tvb, pinfo, tree, frame_offset)
|
||||
local remaining = tvb:len() - frame_offset
|
||||
local candidate_length = math.min(remaining, FRAME_LENGTH)
|
||||
local frame_range = tvb(frame_offset, candidate_length)
|
||||
local subtree = tree:add(fanuc10010_proto, frame_range, "FANUC 10010 状态帧")
|
||||
subtree:add(fields.frame, frame_range)
|
||||
|
||||
if remaining < FRAME_LENGTH then
|
||||
add_error(subtree, frame_range, fields.expert_bad_length, "剩余字节不足 90B,无法组成完整状态帧")
|
||||
return remaining
|
||||
end
|
||||
|
||||
local header = read_ascii(tvb, frame_offset, HEADER_LENGTH)
|
||||
local declared_length = read_u32_be(tvb, frame_offset + LENGTH_OFFSET)
|
||||
local message_id = read_u32_be(tvb, frame_offset + MESSAGE_ID_OFFSET)
|
||||
local trailer = read_ascii(tvb, frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH)
|
||||
|
||||
subtree:add(fields.header, tvb(frame_offset, HEADER_LENGTH), header)
|
||||
subtree:add(fields.declared_length, tvb(frame_offset + LENGTH_OFFSET, 4), declared_length)
|
||||
subtree:add(fields.message_id, tvb(frame_offset + MESSAGE_ID_OFFSET, 4), message_id)
|
||||
|
||||
local pose_tree = subtree:add(fields.pose, tvb(frame_offset + POSE_OFFSET, POSE_COUNT * 4))
|
||||
local pose_values = {}
|
||||
for index = 1, POSE_COUNT do
|
||||
local field_offset = frame_offset + POSE_OFFSET + ((index - 1) * 4)
|
||||
local value = read_f32_be(tvb, field_offset)
|
||||
pose_values[index] = value
|
||||
pose_tree:add(fields["pose_" .. index], tvb(field_offset, 4), value)
|
||||
end
|
||||
|
||||
local joint_tree = subtree:add(fields.joints, tvb(frame_offset + JOINT_OFFSET, JOINT_COUNT * 4))
|
||||
local joint_values = {}
|
||||
for index = 1, JOINT_COUNT do
|
||||
local field_offset = frame_offset + JOINT_OFFSET + ((index - 1) * 4)
|
||||
local value = read_f32_be(tvb, field_offset)
|
||||
joint_values[index] = value
|
||||
joint_tree:add(fields["joint_" .. index], tvb(field_offset, 4), value)
|
||||
end
|
||||
|
||||
-- 单独保留一组角度显示,便于对照原始关节弧度值。
|
||||
local joint_degree_tree = subtree:add(fields.joint_degrees, tvb(frame_offset + JOINT_OFFSET, 6 * 4))
|
||||
for index = 1, 6 do
|
||||
local field_offset = frame_offset + JOINT_OFFSET + ((index - 1) * 4)
|
||||
joint_degree_tree:add(
|
||||
fields["joint_deg_" .. index],
|
||||
tvb(field_offset, 4),
|
||||
radians_to_degrees(joint_values[index]))
|
||||
end
|
||||
|
||||
local tail_tree = subtree:add(fields.tail, tvb(frame_offset + TAIL_OFFSET, TAIL_COUNT * 4))
|
||||
local tail_values = {}
|
||||
for index = 1, TAIL_COUNT do
|
||||
local field_offset = frame_offset + TAIL_OFFSET + ((index - 1) * 4)
|
||||
local value = read_u32_be(tvb, field_offset)
|
||||
tail_values[index] = value
|
||||
tail_tree:add(fields["tail_" .. index], tvb(field_offset, 4), value)
|
||||
end
|
||||
|
||||
subtree:add(fields.trailer, tvb(frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH), trailer)
|
||||
|
||||
if header ~= HEADER_MAGIC then
|
||||
add_error(subtree, tvb(frame_offset, HEADER_LENGTH), fields.expert_bad_magic, "帧头不是 doz")
|
||||
end
|
||||
|
||||
if trailer ~= TRAILER_MAGIC then
|
||||
add_error(subtree, tvb(frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH), fields.expert_bad_magic, "帧尾不是 zod")
|
||||
end
|
||||
|
||||
if declared_length ~= FRAME_LENGTH then
|
||||
add_error(subtree, tvb(frame_offset + LENGTH_OFFSET, 4), fields.expert_bad_length, "长度字段不是 90")
|
||||
end
|
||||
|
||||
local summary = string.format(
|
||||
"MsgId=%u X=%.3f Y=%.3f Z=%.3f J1=%.6f rad / %.3f deg J2=%.6f rad / %.3f deg Tail=[%u,%u,%u,%u]",
|
||||
message_id,
|
||||
pose_values[1], pose_values[2], pose_values[3],
|
||||
joint_values[1], radians_to_degrees(joint_values[1]),
|
||||
joint_values[2], radians_to_degrees(joint_values[2]),
|
||||
tail_values[1], tail_values[2], tail_values[3], tail_values[4])
|
||||
|
||||
subtree:set_text("FANUC 10010 状态帧, " .. summary)
|
||||
pinfo.cols.info:append(" | " .. summary)
|
||||
|
||||
return FRAME_LENGTH
|
||||
end
|
||||
|
||||
function fanuc10010_proto.dissector(tvb, pinfo, tree)
|
||||
if tvb:len() == 0 then
|
||||
return 0
|
||||
end
|
||||
|
||||
pinfo.cols.protocol = "FANUC10010"
|
||||
|
||||
local offset = 0
|
||||
while offset < tvb:len() do
|
||||
local consumed = dissect_single_frame(tvb, pinfo, tree, offset)
|
||||
if consumed <= 0 then
|
||||
break
|
||||
end
|
||||
|
||||
offset = offset + consumed
|
||||
if consumed < FRAME_LENGTH then
|
||||
break
|
||||
end
|
||||
end
|
||||
|
||||
return tvb:len()
|
||||
end
|
||||
|
||||
DissectorTable.get("tcp.port"):add(10010, fanuc10010_proto)
|
||||
7
analysis/segment_02_acc_jerk_summary.csv
Normal file
7
analysis/segment_02_acc_jerk_summary.csv
Normal file
@@ -0,0 +1,7 @@
|
||||
Axis,AccPeakRadPerS2,AccLimitRadPerS2,AccRatio,AccWindowStartS,AccWindowEndS,AccLine,JerkPeakRadPerS3,JerkLimitRadPerS3,JerkRatio,JerkWindowStartS,JerkWindowEndS,JerkLine
|
||||
Joint1,16.638678,26.900000,0.618538,0.128012,0.135939,18,902.687973,224.220000,4.025903,6.312082,6.320127,791
|
||||
Joint2,14.521836,22.540000,0.644270,3.088013,3.096086,388,888.335197,187.860000,4.728709,2.904052,2.912069,365
|
||||
Joint3,14.267221,29.810000,0.478605,0.128012,0.135939,18,728.505873,248.460000,2.932085,0.135939,0.143989,19
|
||||
Joint4,-34.694506,39.990000,0.867580,6.832125,6.840105,856,-2222.596524,333.300000,6.668456,6.312082,6.320127,791
|
||||
Joint5,-16.329775,39.630000,0.412056,6.840105,6.848111,857,842.738923,330.270000,2.551667,6.936077,6.944096,869
|
||||
Joint6,34.766065,72.720000,0.478081,1.392021,1.399995,176,2678.050822,606.010000,4.419153,6.312082,6.320127,791
|
||||
|
526
docs/2026042802-1-trigger-vs-teach-point-analysis.md
Normal file
526
docs/2026042802-1-trigger-vs-teach-point-analysis.md
Normal file
@@ -0,0 +1,526 @@
|
||||
# 2026042802-1 抓包触发点与示教点对比记录
|
||||
|
||||
## 1. 目的
|
||||
|
||||
- 使用 `../Rvbust/uttc-20260428/2026042802-1.pcap` 确认当前仓库是否已经导出过这份抓包对应的真实运动轨迹发包记录。
|
||||
- 如果现有导出不包含 IO 触发信息,则补一份“真实 J519 发包 + IO 标记”导出。
|
||||
- 将抓包里真实触发 IO 的关节坐标,与 `Config/RobotConfig.json` 中 `flying_shots.UTTC_MS11` 的示教点逐点对比。
|
||||
- 把结论固定成仓库文档,后续继续和新程序抓包、`ShotEvents.json`、运行时导出结果做同口径比较。
|
||||
|
||||
## 2. 数据来源与口径
|
||||
|
||||
- 抓包文件:`../Rvbust/uttc-20260428/2026042802-1.pcap`
|
||||
- 旧目录配置文件:`../Rvbust/uttc-20260428/RobotConfig.json`
|
||||
- 新版程序运行时配置文件:`Config/RobotConfig.json`
|
||||
- 对比对象:两份配置中的 `flying_shots.UTTC_MS11`
|
||||
- 协议口径:只看 `192.168.10.10 -> 192.168.10.11` 的 `UDP 60015` 64B J519 命令帧
|
||||
- 触发判定:`write_io_value > 0` 视为 IO 置位
|
||||
- 去重规则:由于 `io_keep_cycles=2`,每次触发只记录首个高电平帧
|
||||
- 关节单位:抓包 J519 目标关节为 `deg`;`RobotConfig.json` `traj_waypoints` 为 `rad`,分析时先转成 `deg`
|
||||
|
||||
补充说明:
|
||||
|
||||
- `../Rvbust/uttc-20260428/` 目录中的内容代表旧抓包及其同目录资料。
|
||||
- `Config/Data/UTTC_MS11/` 目录中的内容代表现在新版程序的运行时导出。
|
||||
- 本文不会把两者混成同一次运行的数据;这里只拿新版程序目录里的文件做“现状参照”,核心分析对象仍然是旧抓包 `2026042802-1.pcap`。
|
||||
|
||||
## 3. 现状核对
|
||||
|
||||
### 3.1 仓库里已经存在什么
|
||||
|
||||
- 旧抓包目录已有原始抓包:`../Rvbust/uttc-20260428/2026042802-1.pcap`
|
||||
- 旧抓包目录已有按运动段拆分的导出:`../Rvbust/uttc-20260428/2026042802-1_joint_segments/`
|
||||
- 旧抓包目录已有速度档位整理结果:`../Rvbust/uttc-20260428/1倍速度 角度坐标点/真实轨迹JointDetialTraj.txt`
|
||||
- 新版程序目录已有运行时导出:`Config/Data/UTTC_MS11/ActualSendJointTraj.txt`
|
||||
- 新版程序目录已有运行时导出:`Config/Data/UTTC_MS11/ActualSendTiming.txt`
|
||||
- 新版程序目录已有理论触发时间轴:`Config/Data/UTTC_MS11/ShotEvents.json`
|
||||
|
||||
### 3.2 当前缺的是什么
|
||||
|
||||
- 新版程序目录中的 `ActualSendJointTraj.txt` 是现在新版程序落盘的真实发送关节轨迹,但不带 IO 列,也不属于 `2026042802-1.pcap` 这次旧抓包本身。
|
||||
- 旧抓包目录中的 `2026042802-1_joint_segments/` 是按运动段拆开的 `JointDetialTraj.txt` 风格导出,也不包含整条抓包的 IO 标记。
|
||||
- 旧抓包目录中的 `1倍速度 角度坐标点/真实轨迹JointDetialTraj.txt` 提供了真实轨迹文本,但同样不带 IO 触发列。
|
||||
- 本次分析前,没有发现一份专门对应 `2026042802-1.pcap` 的“整条真实 J519 发包记录 + IO 标记 + 触发帧筛选”组合导出。
|
||||
|
||||
结论:
|
||||
旧抓包目录和新版程序目录里都已经有各自的轨迹资料,但在本次分析前,还没有一份专门对应 `2026042802-1.pcap` 的、带 IO 触发信息的整条实际发包记录。
|
||||
|
||||
## 4. 本次新增导出
|
||||
|
||||
本次补充生成了以下文件:
|
||||
|
||||
- `analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv`
|
||||
- `1788` 条客户端真实下发 J519 命令
|
||||
- 包含 `frame_number / time_relative_s / sequence / write_io_value / io_addrs / j1..j6`
|
||||
- `analysis/2026042802-1/2026042802-1_j519_trigger_frames.csv`
|
||||
- 只保留每次 IO 高电平脉冲的第一帧
|
||||
- 共 `17` 条,和 `ShotEvents.json` 的触发次数一致
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_vs_teach_points.csv`
|
||||
- 每个真实触发帧与 `UTTC_MS11` 示教点的逐点偏差表
|
||||
- `analysis/2026042802-1/2026042802-1_analysis_summary.json`
|
||||
- 本文档引用的汇总统计
|
||||
|
||||
对应复现脚本:
|
||||
|
||||
- `analysis/analyze_2026042802_1_trigger_vs_teach_points.py`
|
||||
|
||||
## 5. 触发与地址核对
|
||||
|
||||
- 抓包提取到的真实触发次数:`17`
|
||||
- `ShotEvents.json` 中的触发次数:`17`
|
||||
- `write_io_value` 解码出的 `io_addrs` 与 `UTTC_MS11.addr` 没有真实内容不一致
|
||||
- 其中有 `14` 处列表顺序不同,例如抓包里是 `[2, 3, 4]`,配置里写的是 `[3, 4, 2]`
|
||||
- 这 `14` 处只是顺序差异,不是触发地址集合差异
|
||||
- 真正的地址集合不匹配数:`0`
|
||||
|
||||
结论:
|
||||
`2026042802-1.pcap` 中真实发出的 IO 地址集合,与 `UTTC_MS11` 配置的 `addr` 集合是一致的。
|
||||
|
||||
## 6. 触发点与示教点误差统计
|
||||
|
||||
- 平均单点最大单轴误差:`4.241584 deg`
|
||||
- 最大单轴误差:`11.130744 deg`
|
||||
- 平均 RMS 误差:`2.540614 deg`
|
||||
- 最大误差轴分布:`J6=9`、`J4=5`、`J5=1`、`J2=1`、`J1=1`
|
||||
|
||||
这说明:
|
||||
|
||||
- 这份旧抓包里的真实触发点,并不是严格贴在配置示教点上触发
|
||||
- 误差主要集中在 `J6` 和 `J4`
|
||||
- 由于 `../Rvbust/uttc-20260428/RobotConfig.json` 和当前仓库 `Config/RobotConfig.json` 的 `UTTC_MS11` 四组关键字段完全一致,因此这里的“示教点”对旧抓包和当前配置是同一组数据
|
||||
- 如果用户的“对焦准确示教点”就是这组 `traj_waypoints`,那么旧程序实际触发时已经存在几度量级的偏移,而不是亚角度级别的完全重合
|
||||
|
||||
## 7. 最大偏差点
|
||||
|
||||
按最大单轴误差从大到小列出前 5 个触发点:
|
||||
|
||||
| 触发序号 | waypoint_index | 实际 io_addrs | 最大误差轴 | 最大误差(deg) | RMS误差(deg) | 主要差值 |
|
||||
| --- | --- | --- | --- | ---: | ---: | --- |
|
||||
| 3 | 3 | `[2, 3, 4]` | `J6` | `11.130744` | `5.955608` | `J4 +8.811537`,`J6 -11.130744` |
|
||||
| 7 | 7 | `[3, 4]` | `J6` | `7.194107` | `4.618545` | `J2 -5.441249`,`J4 -6.792278`,`J6 +7.194107` |
|
||||
| 17 | 18 | `[3, 4]` | `J4` | `6.848522` | `4.036140` | `J2 +1.835940`,`J4 -6.848522`,`J6 +6.320424` |
|
||||
| 13 | 13 | `[3, 4]` | `J1` | `6.656209` | `4.109282` | `J1 +6.656209`,`J3 -3.848692`,`J4 +3.795032` |
|
||||
| 8 | 8 | `[2, 4]` | `J6` | `5.939884` | `3.533133` | `J2 -2.961069`,`J4 -5.456660`,`J6 +5.939884` |
|
||||
|
||||
## 8. 逐点结果
|
||||
|
||||
完整逐点数据已写入:
|
||||
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_vs_teach_points.csv`
|
||||
|
||||
这里保留简表,方便直接看每次触发的最大偏差:
|
||||
|
||||
| 触发序号 | waypoint_index | frame | seq | time(s) | io_addrs | 最大误差轴 | 最大误差(deg) | RMS误差(deg) |
|
||||
| --- | --- | ---: | ---: | ---: | --- | --- | ---: | ---: |
|
||||
| 1 | 1 | 1955 | 1381124 | 5.888271 | `[2, 4]` | `J6` | `3.096909` | `2.341857` |
|
||||
| 2 | 2 | 2151 | 1381198 | 6.480252 | `[2, 3, 4]` | `J4` | `2.737488` | `1.612807` |
|
||||
| 3 | 3 | 2223 | 1381226 | 6.704225 | `[2, 3, 4]` | `J6` | `11.130744` | `5.955608` |
|
||||
| 4 | 4 | 2336 | 1381269 | 7.048266 | `[2, 4]` | `J6` | `2.373383` | `1.439206` |
|
||||
| 5 | 5 | 2477 | 1381321 | 7.464289 | `[2, 4]` | `J5` | `1.973330` | `1.273254` |
|
||||
| 6 | 6 | 2665 | 1381391 | 8.024277 | `[3, 4]` | `J2` | `1.265078` | `0.786940` |
|
||||
| 7 | 7 | 2811 | 1381447 | 8.472317 | `[3, 4]` | `J6` | `7.194107` | `4.618545` |
|
||||
| 8 | 8 | 2908 | 1381483 | 8.760361 | `[2, 4]` | `J6` | `5.939884` | `3.533133` |
|
||||
| 9 | 9 | 3112 | 1381561 | 9.384332 | `[2, 4]` | `J4` | `2.414749` | `1.565121` |
|
||||
| 10 | 10 | 3170 | 1381583 | 9.560358 | `[2, 4]` | `J4` | `3.629631` | `2.268783` |
|
||||
| 11 | 11 | 3350 | 1381651 | 10.104322 | `[2, 4]` | `J6` | `3.113037` | `2.115595` |
|
||||
| 12 | 12 | 3470 | 1381697 | 10.472333 | `[2, 4]` | `J6` | `4.018547` | `2.003201` |
|
||||
| 13 | 13 | 3627 | 1381756 | 10.944371 | `[3, 4]` | `J1` | `6.656209` | `4.109282` |
|
||||
| 14 | 14 | 3710 | 1381787 | 11.192339 | `[2, 4]` | `J6` | `2.834801` | `1.514764` |
|
||||
| 15 | 15 | 3796 | 1381820 | 11.456361 | `[2, 4]` | `J4` | `2.496544` | `1.640818` |
|
||||
| 16 | 16 | 3870 | 1381848 | 11.680362 | `[2, 4]` | `J6` | `3.782349` | `2.176883` |
|
||||
| 17 | 18 | 4138 | 1381950 | 12.496365 | `[3, 4]` | `J4` | `6.848522` | `4.036140` |
|
||||
|
||||
## 9. UTTC_MS11 示教点角度表
|
||||
|
||||
为了便于和导出的触发序列直接手工比对,下面把当前 `UTTC_MS11` 示教点从 `rad` 转成 `deg`。
|
||||
|
||||
说明:
|
||||
|
||||
- `waypoint_index` 使用配置里的原始下标。
|
||||
- `shot_flag=true` 表示该点在配置里是拍照点候选。
|
||||
- `addr` 保留原配置里的触发地址列表,便于和 `io_addrs` 对照。
|
||||
|
||||
| waypoint_index | shot_flag | addr | J1(deg) | J2(deg) | J3(deg) | J4(deg) | J5(deg) | J6(deg) |
|
||||
| --- | --- | --- | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| 0 | `false` | `[]` | 60.546226 | 0.668344 | -1.025155 | -0.869105 | 1.231405 | 0.548197 |
|
||||
| 1 | `true` | `[2, 4]` | 48.886810 | 2.198985 | -11.021017 | 0.410210 | 6.248381 | 2.294991 |
|
||||
| 2 | `true` | `[3, 4, 2]` | 55.347755 | 11.807040 | -7.009095 | -71.014331 | 6.012065 | 74.249532 |
|
||||
| 3 | `true` | `[3, 4, 2]` | 55.109808 | 8.759497 | -8.518217 | -41.117250 | 10.108488 | 41.956933 |
|
||||
| 4 | `true` | `[4, 2]` | 43.653593 | -1.629660 | -17.715754 | 5.995209 | 32.171718 | -22.573973 |
|
||||
| 5 | `true` | `[4, 2]` | 64.582445 | 4.262979 | -15.669217 | -29.952927 | 29.850440 | 45.626527 |
|
||||
| 6 | `true` | `[3, 4]` | 60.479483 | 23.068782 | -5.011264 | 36.106855 | 5.525681 | -31.302564 |
|
||||
| 7 | `true` | `[3, 4]` | 70.475837 | 16.393849 | -13.456948 | -27.892319 | 14.535662 | 31.711933 |
|
||||
| 8 | `true` | `[4, 2]` | 69.582464 | -17.105713 | -8.710590 | -58.475694 | 7.630590 | 64.437733 |
|
||||
| 9 | `true` | `[4, 2]` | 73.571259 | -6.429845 | -9.628580 | -128.808647 | 14.671083 | 140.002048 |
|
||||
| 10 | `true` | `[4, 2]` | 75.569386 | -14.679306 | -7.294156 | -130.923033 | 17.688361 | 141.893508 |
|
||||
| 11 | `true` | `[4, 2]` | 86.093498 | -14.498333 | -13.681511 | -69.868296 | 26.742682 | 88.999414 |
|
||||
| 12 | `true` | `[4, 2]` | 61.720733 | -4.232789 | -9.784423 | -108.384083 | 22.160772 | 118.142064 |
|
||||
| 13 | `true` | `[4, 3]` | 79.757083 | 4.640231 | -15.311186 | -56.035312 | 26.133138 | 52.152257 |
|
||||
| 14 | `true` | `[4, 2]` | 108.955551 | 1.370760 | -33.594425 | -48.367361 | 43.963404 | 85.039261 |
|
||||
| 15 | `true` | `[4, 2]` | 110.584848 | -3.860900 | -32.396407 | -51.112591 | 44.229160 | 87.624000 |
|
||||
| 16 | `true` | `[4, 2]` | 118.095952 | -17.376387 | -31.069001 | -59.560538 | 48.732576 | 94.134771 |
|
||||
| 17 | `false` | `[4, 2]` | 62.573787 | -22.938069 | -10.333288 | 77.491373 | 35.583412 | -69.668648 |
|
||||
| 18 | `true` | `[4, 3]` | 60.282482 | -22.938081 | -10.333248 | 77.491642 | 35.583378 | -69.668981 |
|
||||
| 19 | `false` | `[]` | 60.546226 | 0.668344 | -1.025155 | -0.869105 | 1.231405 | 0.548197 |
|
||||
|
||||
## 10. 结论
|
||||
|
||||
1. `../Rvbust/uttc-20260428/` 目录里已有旧抓包相关轨迹资料,`Config/Data/UTTC_MS11/` 目录里已有新版程序运行时导出,但在本次分析前,没有一份专门对应 `2026042802-1.pcap` 的“整条真实 J519 发包记录 + IO 标记”导出。
|
||||
2. 本次已经补齐该导出,文件位于 `analysis/2026042802-1/2026042802-1_j519_actual_send_all_with_io.csv`。
|
||||
3. `2026042802-1.pcap` 的真实 IO 触发次数是 `17`,与 `ShotEvents.json` 一致。
|
||||
4. 抓包中真实触发时使用的 IO 地址集合,与 `UTTC_MS11.addr` 一致;只有列表顺序差异,没有地址集合差异。
|
||||
5. 抓包中真实触发点相对示教点的平均最大单轴误差约 `4.24 deg`,最大达到 `11.13 deg`,主要偏在 `J6` 和 `J4`。
|
||||
6. 因此,如果后续要分析“为什么新程序拍照位置偏了”,不能把 `2026042802-1.pcap` 视为“与当前示教点完全重合的零误差基准”;它本身相对当前配置示教点就存在明显偏差。
|
||||
|
||||
## 11. 触发点前 6 个周期的实际关节坐标与示教点差值
|
||||
|
||||
在这份旧抓包里,17 个真实触发点对应的“最接近示教点”的实际采样点,统一出现在触发前 `6` 个周期,也就是前 `48ms`。
|
||||
|
||||
为了方便手工比对,这里把每个触发点前 `6` 个周期的实际关节坐标单独列出来,并和对应示教点逐轴做差。
|
||||
|
||||
说明:
|
||||
|
||||
- `pre6_frame` / `pre6_time` 是触发帧前 `6` 个 `8ms` 周期的实际 J519 发包点。
|
||||
- `pre6_j*` 是该时刻的实际关节角度,单位 `deg`。
|
||||
- `diff_j*` 定义为 `pre6_actual - teach_point`,单位 `deg`。
|
||||
- 这张表更适合你直接和导出的触发序列逐点人工核对。
|
||||
|
||||
| 触发序号 | waypoint_index | pre6_frame | pre6_time(s) | pre6_J1 | pre6_J2 | pre6_J3 | pre6_J4 | pre6_J5 | pre6_J6 | diff_J1 | diff_J2 | diff_J3 | diff_J4 | diff_J5 | diff_J6 | max_abs_diff(deg) | rms_diff(deg) |
|
||||
| --- | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| 1 | 1 | 1941 | 5.840207 | 49.036247 | 2.169637 | -10.893022 | 0.469577 | 6.183248 | 2.196172 | +0.149437 | -0.029348 | +0.127996 | +0.059368 | -0.065133 | -0.098818 | 0.149437 | 0.097560 |
|
||||
| 2 | 2 | 2135 | 6.432175 | 55.402473 | 11.806953 | -6.980721 | -70.955170 | 6.007696 | 74.193436 | +0.054718 | -0.000086 | +0.028375 | +0.059162 | -0.004369 | -0.056096 | 0.059162 | 0.041764 |
|
||||
| 3 | 3 | 2207 | 6.656264 | 55.185375 | 8.828770 | -8.461165 | -41.609406 | 9.991771 | 42.555542 | +0.075567 | +0.069272 | +0.057051 | -0.492155 | -0.116717 | +0.598609 | 0.598609 | 0.323506 |
|
||||
| 4 | 4 | 2321 | 7.000291 | 43.639214 | -1.593877 | -17.673382 | 5.988685 | 32.060623 | -22.587208 | -0.014380 | +0.035782 | +0.042372 | -0.006524 | -0.111095 | -0.013234 | 0.111095 | 0.051385 |
|
||||
| 5 | 5 | 2457 | 7.416267 | 64.456329 | 4.153820 | -15.749991 | -29.951208 | 30.008862 | 45.422455 | -0.126115 | -0.109159 | -0.080774 | +0.001719 | +0.158422 | -0.204073 | 0.204073 | 0.129802 |
|
||||
| 6 | 6 | 2649 | 7.976283 | 60.479656 | 23.070845 | -5.011842 | 36.106964 | 5.525600 | -31.304708 | +0.000174 | +0.002063 | -0.000578 | +0.000109 | -0.000081 | -0.002145 | 0.002145 | 0.001241 |
|
||||
| 7 | 7 | 2795 | 8.424315 | 70.489815 | 16.270372 | -13.455009 | -28.079985 | 14.531018 | 31.910471 | +0.013978 | -0.123476 | +0.001939 | -0.187666 | -0.004644 | +0.198538 | 0.198538 | 0.122545 |
|
||||
| 8 | 8 | 2892 | 8.712310 | 69.566750 | -17.333443 | -8.667440 | -58.787853 | 7.566183 | 64.777092 | -0.015715 | -0.227729 | +0.043149 | -0.312159 | -0.064407 | +0.339359 | 0.339359 | 0.212417 |
|
||||
| 9 | 9 | 3096 | 9.336375 | 73.577042 | -6.447144 | -9.620103 | -128.858627 | 14.686206 | 140.051056 | +0.005783 | -0.017298 | +0.008477 | -0.049980 | +0.015123 | +0.049008 | 0.049980 | 0.030367 |
|
||||
| 10 | 10 | 3154 | 9.512325 | 75.498016 | -14.523483 | -7.317341 | -131.103271 | 17.629053 | 142.061157 | -0.071370 | +0.155823 | -0.023185 | -0.180238 | -0.059308 | +0.167649 | 0.180238 | 0.125181 |
|
||||
| 11 | 11 | 3334 | 10.056378 | 86.017281 | -14.446944 | -13.689492 | -69.926414 | 26.744770 | 89.069130 | -0.076217 | +0.051389 | -0.007981 | -0.058119 | +0.002088 | +0.069716 | 0.076217 | 0.052846 |
|
||||
| 12 | 12 | 3455 | 10.424364 | 61.554295 | -4.126511 | -9.723098 | -108.476807 | 22.091070 | 117.986275 | -0.166438 | +0.106278 | +0.061325 | -0.092724 | -0.069702 | -0.155789 | 0.166438 | 0.115819 |
|
||||
| 13 | 13 | 3611 | 10.896324 | 80.232681 | 4.624288 | -15.574081 | -55.685375 | 26.385595 | 52.279755 | +0.475598 | -0.015943 | -0.262896 | +0.349937 | +0.252457 | +0.127497 | 0.475598 | 0.288100 |
|
||||
| 14 | 14 | 3694 | 11.144347 | 108.790268 | 1.405766 | -33.485344 | -48.378551 | 43.857502 | 84.771103 | -0.165283 | +0.035006 | +0.109081 | -0.011190 | -0.105902 | -0.268158 | 0.268158 | 0.143580 |
|
||||
| 15 | 15 | 3780 | 11.408361 | 110.543327 | -3.739877 | -32.432842 | -50.983562 | 44.216587 | 87.566269 | -0.041521 | +0.121023 | -0.036435 | +0.129028 | -0.012573 | -0.057731 | 0.129028 | 0.079412 |
|
||||
| 16 | 16 | 3854 | 11.632360 | 118.081940 | -17.294281 | -31.087101 | -59.614193 | 48.711964 | 94.208046 | -0.014013 | +0.082106 | -0.018100 | -0.053655 | -0.020612 | +0.073275 | 0.082106 | 0.051540 |
|
||||
| 17 | 18 | 4121 | 12.448375 | 60.258179 | -22.880474 | -10.315624 | 77.249168 | 35.501560 | -69.439224 | -0.024304 | +0.057606 | +0.017624 | -0.242474 | -0.081817 | +0.229756 | 0.242474 | 0.142884 |
|
||||
|
||||
这张表说明:
|
||||
|
||||
- 如果把旧程序的真实 IO 触发点整体前移 `6` 个周期,再去看对应关节坐标,它们和示教点已经非常接近。
|
||||
- 17 个点里,大多数前 `6` 周期采样点与示教点的最大单轴误差都在 `0.05 ~ 0.34 deg`。
|
||||
- 误差较大的两个点是:
|
||||
- `trigger 3`:`max_abs_diff = 0.598609 deg`
|
||||
- `trigger 13`:`max_abs_diff = 0.475598 deg`
|
||||
- 这进一步支持一个很强的现象:旧抓包里的真实 IO 触发时刻,相对“最接近示教点”的实际运动位置,整体滞后约 `48ms`。
|
||||
|
||||
## 12. 复现命令
|
||||
|
||||
在仓库根目录执行:
|
||||
|
||||
```powershell
|
||||
python analysis/analyze_2026042802_1_trigger_vs_teach_points.py
|
||||
python analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py
|
||||
```
|
||||
|
||||
脚本默认读取:
|
||||
|
||||
- `../Rvbust/uttc-20260428/2026042802-1.pcap`
|
||||
- `Config/RobotConfig.json`
|
||||
- `D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe`
|
||||
|
||||
如果后续 Wireshark 路径变化,需要先同步修改脚本中的 `DEFAULT_TSHARK`。
|
||||
|
||||
## 13. 60015 状态反馈视角复核
|
||||
|
||||
上面第 11 节的“前 6 个周期最接近示教点”,只是在看:
|
||||
|
||||
- `192.168.10.10 -> 192.168.10.11` 的 64B J519 下发命令序列
|
||||
|
||||
它说明的是“命令目标流”和示教点之间的相对时序,不能直接等价成“机械臂实时反馈已经过了示教点才触发”。
|
||||
|
||||
针对你补充的 `buffer_size` 线索,这里进一步直接解析了:
|
||||
|
||||
- `192.168.10.11 -> 192.168.10.10` 的 132B `UDP 60015` 状态包
|
||||
- 解码口径与仓库运行时代码 `src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs` 一致,全部按大端解析
|
||||
- 关节反馈字段使用状态包里的 `0x3C ~ 0x57` 六轴 `float32 deg`
|
||||
|
||||
本次新增文件:
|
||||
|
||||
- `analysis/2026042802-1/2026042802-1_j519_status_feedback_all.csv`
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_status_feedback_vs_teach_points.csv`
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_manual_compare.csv`
|
||||
- `analysis/2026042802-1/2026042802-1_status_feedback_summary.json`
|
||||
- `analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py`
|
||||
|
||||
### 13.1 序列号关系先确认
|
||||
|
||||
整条抓包里共提取到:
|
||||
|
||||
- 状态包 `1789` 条
|
||||
- 命令包 `1788` 条
|
||||
|
||||
对每一条命令帧,取它前一个状态帧做对齐,结果是:
|
||||
|
||||
- `命令 sequence - 前一条状态 sequence = 8`
|
||||
- 统计结果:`1788 / 1788` 全部成立
|
||||
|
||||
这说明在这份旧抓包里,`buffer_size=8` 不是偶发现象,而是整条 J519 流都稳定成立。
|
||||
|
||||
换句话说:
|
||||
|
||||
- 触发命令包上的 `sequence=1381124`
|
||||
- 它对应“当前实时反馈”的状态包 `sequence=1381116`
|
||||
- 中间固定隔着 `8` 个状态周期,也就是约 `64ms`
|
||||
|
||||
### 13.2 触发当下的实时反馈,并不在示教点附近
|
||||
|
||||
如果直接取“每个触发命令帧对应的当前状态包”,也就是:
|
||||
|
||||
- `paired_status_sequence = trigger_sequence - 8`
|
||||
|
||||
那么 17 个点相对示教点的误差反而更大:
|
||||
|
||||
- 平均最大单轴误差:`6.469219 deg`
|
||||
- 最大单轴误差:`16.123934 deg`
|
||||
- 最大误差轴分布:`J6=9`、`J2=4`、`J1=2`、`J4=2`
|
||||
|
||||
这说明:
|
||||
|
||||
- 机械臂在触发命令真正下发的那个时刻,对应的实时反馈位置,大多数情况下还没有跑到示教点附近
|
||||
- 所以“最接近示教点都出现在触发前 6 个周期”这个现象,不能理解成“机器人已经过了示教点才开始触发”
|
||||
- 那只是命令流相对示教点的时序关系;实时反馈要再往后看
|
||||
|
||||
### 13.3 实时反馈最接近示教点,出现在触发后 9 到 10 个状态周期
|
||||
|
||||
对每个触发点,以 `paired_status_sequence = trigger_sequence - 8` 为起点,在前后 `±20` 个状态周期里搜索“与示教点最接近”的状态反馈帧,结果统一得到:
|
||||
|
||||
- 最佳点相对 `paired_status` 晚 `9` 或 `10` 个状态周期
|
||||
- 统计分布:`9 周期 = 9` 次,`10 周期 = 8` 次
|
||||
|
||||
折算成触发命令时刻:
|
||||
|
||||
- 最佳点相对触发命令晚 `1` 或 `2` 个状态周期
|
||||
- 统计分布:`+1 周期 = 9` 次,`+2 周期 = 8` 次
|
||||
- 对应抓包时间差约 `71.838 ~ 79.964 ms`
|
||||
- 平均约 `75.661 ms`
|
||||
|
||||
这里看起来像是 `1~2` 周期,但要注意基准不同:
|
||||
|
||||
- 触发命令本身领先当前状态 `8` 个周期
|
||||
- 所以“触发后约 `72~80ms` 才最接近示教点”与“状态流从当前反馈再往后走 `9~10` 个周期才到位”本质上是同一件事
|
||||
|
||||
### 13.4 实时反馈视角的逐点结果
|
||||
|
||||
| 触发序号 | waypoint_index | trigger_frame | trigger_seq | paired_status_frame | paired_status_seq | 触发当下最大误差轴 | 触发当下最大误差(deg) | 触发当下RMS(deg) | 最佳状态seq | 最佳状态相对当前反馈晚几周期 | 最佳状态相对触发时间(ms) | 最佳状态最大误差(deg) | 最佳状态RMS(deg) |
|
||||
| --- | --- | ---: | ---: | ---: | ---: | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| 1 | 1 | 1955 | 1381124 | 1954 | 1381116 | J1 | 6.588242 | 3.822544 | 1381126 | 10 | 79.853 | 0.207590 | 0.122397 |
|
||||
| 2 | 2 | 2151 | 1381198 | 2150 | 1381190 | J1 | 2.335315 | 1.382754 | 1381199 | 9 | 71.912 | 0.082134 | 0.046161 |
|
||||
| 3 | 3 | 2223 | 1381226 | 2222 | 1381218 | J6 | 16.123934 | 8.852377 | 1381228 | 10 | 79.929 | 0.569692 | 0.323964 |
|
||||
| 4 | 4 | 2336 | 1381269 | 2335 | 1381261 | J6 | 4.901883 | 3.244056 | 1381271 | 10 | 79.909 | 0.094704 | 0.065805 |
|
||||
| 5 | 5 | 2477 | 1381321 | 2476 | 1381313 | J6 | 9.219438 | 4.621321 | 1381323 | 10 | 79.833 | 0.056622 | 0.033575 |
|
||||
| 6 | 6 | 2665 | 1381391 | 2664 | 1381383 | J6 | 6.146461 | 3.047841 | 1381392 | 9 | 71.904 | 0.176974 | 0.085631 |
|
||||
| 7 | 7 | 2811 | 1381447 | 2810 | 1381439 | J6 | 13.151718 | 7.782024 | 1381448 | 9 | 71.873 | 0.306587 | 0.182107 |
|
||||
| 8 | 8 | 2908 | 1381483 | 2907 | 1381475 | J6 | 7.436279 | 5.127041 | 1381484 | 9 | 71.838 | 0.060748 | 0.027507 |
|
||||
| 9 | 9 | 3112 | 1381561 | 3111 | 1381553 | J6 | 5.585605 | 3.277206 | 1381562 | 9 | 71.873 | 0.152260 | 0.089150 |
|
||||
| 10 | 10 | 3170 | 1381583 | 3169 | 1381575 | J2 | 4.165077 | 2.128984 | 1381585 | 10 | 79.856 | 0.073440 | 0.043286 |
|
||||
| 11 | 11 | 3350 | 1381651 | 3349 | 1381643 | J2 | 1.830953 | 1.169340 | 1381652 | 9 | 71.898 | 0.077668 | 0.042465 |
|
||||
| 12 | 12 | 3470 | 1381697 | 3469 | 1381689 | J4 | 4.822353 | 2.861125 | 1381698 | 9 | 71.950 | 0.021077 | 0.014936 |
|
||||
| 13 | 13 | 3627 | 1381756 | 3626 | 1381748 | J4 | 8.152508 | 5.168659 | 1381757 | 9 | 71.855 | 0.145543 | 0.069960 |
|
||||
| 14 | 14 | 3710 | 1381787 | 3709 | 1381779 | J6 | 10.404419 | 5.679811 | 1381789 | 10 | 79.922 | 0.119453 | 0.058360 |
|
||||
| 15 | 15 | 3796 | 1381820 | 3795 | 1381812 | J2 | 2.616108 | 1.591175 | 1381822 | 10 | 79.926 | 0.149803 | 0.087169 |
|
||||
| 16 | 16 | 3870 | 1381848 | 3869 | 1381840 | J2 | 4.682474 | 2.206740 | 1381850 | 10 | 79.964 | 0.172956 | 0.104194 |
|
||||
| 17 | 18 | 4138 | 1381950 | 4137 | 1381942 | J6 | 1.813960 | 1.153699 | 1381951 | 9 | 71.948 | 0.074923 | 0.038345 |
|
||||
|
||||
### 13.4.1 手工核对专用三时刻对照
|
||||
|
||||
如果你后面要拿着导出的触发序列人工对比,最推荐直接看下面这张表。
|
||||
|
||||
对应单独导出文件:
|
||||
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_manual_compare.csv`
|
||||
|
||||
它把每个触发点压缩成 3 个时刻:
|
||||
|
||||
- `触发命令时刻`
|
||||
- 你真正发出去、同时带着 IO 触发的那条命令
|
||||
- `触发当下实时反馈`
|
||||
- 按 `buffer_size=8` 回推到当时机器人真实反馈所对应的状态包
|
||||
- `最接近示教点的实时反馈`
|
||||
- 在状态流中真正最贴近示教点的那一帧
|
||||
|
||||
可以直接把它理解成一句话:
|
||||
|
||||
- “我在第几条命令上触发了”
|
||||
- “触发那一刻机器人实际还差多少”
|
||||
- “再过多久机器人才真正到最接近示教点的位置”
|
||||
|
||||
| 触发序号 | waypoint_index | 触发命令seq | 触发命令frame | 触发当下状态seq | 触发当下状态frame | 命令领先状态(周期) | 触发当下最大误差(deg) | 最接近示教点状态seq | 最接近示教点状态frame | 从触发当下反馈再晚几周期 | 相对触发命令再晚几周期 | 相对触发命令再晚多久(ms) | 最接近示教点最大误差(deg) |
|
||||
| --- | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| 1 | 1 | 1381124 | 1955 | 1381116 | 1954 | 8 | 6.588242 | 1381126 | 1977 | 10 | 2 | 79.853 | 0.207590 |
|
||||
| 2 | 2 | 1381198 | 2151 | 1381190 | 2150 | 8 | 2.335315 | 1381199 | 2174 | 9 | 1 | 71.912 | 0.082134 |
|
||||
| 3 | 3 | 1381226 | 2223 | 1381218 | 2222 | 8 | 16.123934 | 1381228 | 2248 | 10 | 2 | 79.929 | 0.569692 |
|
||||
| 4 | 4 | 1381269 | 2336 | 1381261 | 2335 | 8 | 4.901883 | 1381271 | 2361 | 10 | 2 | 79.909 | 0.094704 |
|
||||
| 5 | 5 | 1381321 | 2477 | 1381313 | 2476 | 8 | 9.219438 | 1381323 | 2502 | 10 | 2 | 79.833 | 0.056622 |
|
||||
| 6 | 6 | 1381391 | 2665 | 1381383 | 2664 | 8 | 6.146461 | 1381392 | 2687 | 9 | 1 | 71.904 | 0.176974 |
|
||||
| 7 | 7 | 1381447 | 2811 | 1381439 | 2810 | 8 | 13.151718 | 1381448 | 2833 | 9 | 1 | 71.873 | 0.306587 |
|
||||
| 8 | 8 | 1381483 | 2908 | 1381475 | 2907 | 8 | 7.436279 | 1381484 | 2930 | 9 | 1 | 71.838 | 0.060748 |
|
||||
| 9 | 9 | 1381561 | 3112 | 1381553 | 3111 | 8 | 5.585605 | 1381562 | 3134 | 9 | 1 | 71.873 | 0.152260 |
|
||||
| 10 | 10 | 1381583 | 3170 | 1381575 | 3169 | 8 | 4.165077 | 1381585 | 3195 | 10 | 2 | 79.856 | 0.073440 |
|
||||
| 11 | 11 | 1381651 | 3350 | 1381643 | 3349 | 8 | 1.830953 | 1381652 | 3372 | 9 | 1 | 71.898 | 0.077668 |
|
||||
| 12 | 12 | 1381697 | 3470 | 1381689 | 3469 | 8 | 4.822353 | 1381698 | 3492 | 9 | 1 | 71.950 | 0.021077 |
|
||||
| 13 | 13 | 1381756 | 3627 | 1381748 | 3626 | 8 | 8.152508 | 1381757 | 3649 | 9 | 1 | 71.855 | 0.145543 |
|
||||
| 14 | 14 | 1381787 | 3710 | 1381779 | 3709 | 8 | 10.404419 | 1381789 | 3735 | 10 | 2 | 79.922 | 0.119453 |
|
||||
| 15 | 15 | 1381820 | 3796 | 1381812 | 3795 | 8 | 2.616108 | 1381822 | 3821 | 10 | 2 | 79.926 | 0.149803 |
|
||||
| 16 | 16 | 1381848 | 3870 | 1381840 | 3869 | 8 | 4.682474 | 1381850 | 3895 | 10 | 2 | 79.964 | 0.172956 |
|
||||
| 17 | 18 | 1381950 | 4138 | 1381942 | 4137 | 8 | 1.813960 | 1381951 | 4160 | 9 | 1 | 71.948 | 0.074923 |
|
||||
|
||||
这张表最适合回答两个实际问题:
|
||||
|
||||
1. 触发到底是不是已经“跑过点”才发生的
|
||||
- 不是
|
||||
- 因为触发当下对应的实时反馈误差仍然明显存在
|
||||
|
||||
2. 真正最接近示教点的实时位置,距离触发还差多久
|
||||
- 大部分点还要再过 `1` 或 `2` 个命令周期
|
||||
- 如果换成状态反馈基准,就是再过 `9` 或 `10` 个状态周期
|
||||
- 时间量级稳定在 `72~80ms`
|
||||
|
||||
### 13.4.2 可以优先人工盯的异常点
|
||||
|
||||
如果你不想一开始就看 17 个点,建议优先盯下面几类:
|
||||
|
||||
- `trigger 3`
|
||||
- 触发当下实时反馈最大误差 `16.123934 deg`
|
||||
- 即便到“最佳反馈时刻”,最大误差也还有 `0.569692 deg`
|
||||
- 它是 17 个点里最值得优先复核的一个
|
||||
- `trigger 7`
|
||||
- 触发当下实时反馈最大误差 `13.151718 deg`
|
||||
- 到位后仍有 `0.306587 deg`
|
||||
- `trigger 14`
|
||||
- 触发当下实时反馈最大误差 `10.404419 deg`
|
||||
- 属于第二档明显偏大的点
|
||||
- `trigger 1`
|
||||
- 触发当下误差 `6.588242 deg`,而且最大误差轴是 `J1`
|
||||
- 和前面很多 `J6` 主导的点不一样,适合拿来对比不同类型
|
||||
|
||||
如果你想最快抓住旧程序补偿逻辑,建议先人工对照:
|
||||
|
||||
- `trigger 3`
|
||||
- `trigger 7`
|
||||
- `trigger 14`
|
||||
- `trigger 1`
|
||||
|
||||
这 4 个点已经能覆盖:
|
||||
|
||||
- `J6` 主导的大偏差
|
||||
- `J1` 主导的特殊点
|
||||
- `+1` 周期到位与 `+2` 周期到位两种情况
|
||||
|
||||
### 13.5 对“为什么触发时间看起来在我运动到这个位置之后”的解释
|
||||
|
||||
现在可以把两种视角彻底分开:
|
||||
|
||||
1. 命令流视角
|
||||
- 触发命令帧前 `6` 个周期的目标点,和示教点已经非常接近
|
||||
- 所以如果只看“发给机器人什么目标”,会误以为触发发生在“经过示教点之后”
|
||||
|
||||
2. 实时反馈视角
|
||||
- 由于 `buffer_size=8`,触发命令帧本身就领先当前状态 `8` 个周期
|
||||
- 触发时刻对应的实时反馈,其实还没到示教点附近
|
||||
- 再往后走 `9~10` 个状态周期,反馈才最接近示教点
|
||||
|
||||
因此更准确的表述应该是:
|
||||
|
||||
- 旧程序不是“机器人已经跑过示教点才开始触发”
|
||||
- 而是“触发命令是按一个提前缓存的目标序列发出去的,命令序列本身比实时反馈领先 8 个周期”
|
||||
- 叠加旧程序里触发标志在目标序列上的布置位置,就会出现:
|
||||
- 命令流看起来:最接近示教点在触发前 `6` 个周期
|
||||
- 实时反馈看起来:最接近示教点在触发后 `9~10` 个状态周期
|
||||
|
||||
两者并不矛盾,基准不同而已。
|
||||
|
||||
### 13.6 本节结论
|
||||
|
||||
1. 这份旧抓包的 `60015` 状态流已经直接证明:下发命令序列稳定领先当前状态序列 `8` 个周期。
|
||||
2. 因此不能再把“命令流中的前 6 周期最接近示教点”误解成“机械臂真实位置已经过点后才触发”。
|
||||
3. 按实时反馈看,17 个触发点在触发当下都还没有到示教点附近;最接近示教点的反馈统一出现在触发后约 `72~80ms`。
|
||||
4. 如果后续要反推旧程序拍照补偿逻辑,优先应该建模成:
|
||||
- `命令目标序列` 提前缓存 `8` 个周期
|
||||
- `触发标志` 在目标序列上又相对示教点提前约 `6` 个周期
|
||||
- 两者叠加后,实时反馈到位时刻落在触发之后约 `1~2` 个命令周期
|
||||
|
||||
## 14. TriggerSampleIndexOffsetCycles 取值 6 / 7 / 8 的对照
|
||||
|
||||
在代码里把触发绑定后移做成显式配置之后,进一步对 `6 / 7 / 8` 三档做了离线对照。
|
||||
|
||||
本节的比较口径是:
|
||||
|
||||
- 先找“最接近示教点”的最佳命令 sample
|
||||
- 再分别向后偏移 `6 / 7 / 8` 个命令周期
|
||||
- 比较这三档各自落到的命令帧,与旧抓包真实触发帧的误差大小
|
||||
|
||||
新增文件:
|
||||
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_offset_6_7_8_compare.csv`
|
||||
- `analysis/2026042802-1/2026042802-1_trigger_offset_6_7_8_summary.json`
|
||||
- `analysis/build_trigger_offset_compare_6_7_8.py`
|
||||
|
||||
### 14.1 汇总结论
|
||||
|
||||
三档比较结果非常明确:
|
||||
|
||||
- `6` 周期胜出 `17 / 17`
|
||||
- `7` 周期胜出 `0 / 17`
|
||||
- `8` 周期胜出 `0 / 17`
|
||||
|
||||
平均误差也呈现单调变差:
|
||||
|
||||
| 偏移周期 | 平均最大单轴误差(deg) | 平均RMS误差(deg) |
|
||||
| --- | ---: | ---: |
|
||||
| `6` | `4.241584` | `2.540614` |
|
||||
| `7` | `5.068092` | `3.021297` |
|
||||
| `8` | `5.931684` | `3.520176` |
|
||||
|
||||
这说明:
|
||||
|
||||
- 如果目标是“尽量复刻旧抓包里的真实命令触发点”,`TriggerSampleIndexOffsetCycles` 应该选 `6`
|
||||
- 当前写入配置的 `7`,更偏向“再往后推一拍”,但它并不更接近这份旧抓包
|
||||
|
||||
### 14.2 逐点对照表
|
||||
|
||||
| 触发序号 | waypoint_index | 最佳sample frame | 偏移6 frame | 偏移6最大误差 | 偏移6RMS | 偏移7 frame | 偏移7最大误差 | 偏移7RMS | 偏移8 frame | 偏移8最大误差 | 偏移8RMS | 本点最优偏移 |
|
||||
| --- | --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | --- |
|
||||
| 1 | 1 | 1941 | 1955 | 3.406527 | 2.468439 | 1958 | 4.180932 | 2.935578 | 1961 | 5.004552 | 3.418447 | `6` |
|
||||
| 2 | 2 | 2135 | 2151 | 2.816723 | 1.656246 | 2153 | 3.526760 | 2.060948 | 2155 | 4.301479 | 2.503393 | `6` |
|
||||
| 3 | 3 | 2207 | 2223 | 11.130744 | 5.955608 | 2227 | 13.165517 | 7.035338 | 2229 | 15.212756 | 8.119717 | `6` |
|
||||
| 4 | 4 | 2321 | 2336 | 2.311684 | 1.373158 | 2339 | 3.083186 | 1.743718 | 2341 | 3.952016 | 2.158078 | `6` |
|
||||
| 5 | 5 | 2457 | 2477 | 2.171828 | 1.404704 | 2479 | 2.585320 | 1.667010 | 2483 | 3.004922 | 1.950746 | `6` |
|
||||
| 6 | 6 | 2649 | 2665 | 1.408994 | 0.842403 | 2667 | 1.770170 | 1.066919 | 2669 | 2.303903 | 1.330975 | `6` |
|
||||
| 7 | 7 | 2795 | 2811 | 7.194107 | 4.618545 | 2814 | 8.248680 | 5.331516 | 2817 | 9.275559 | 6.035874 | `6` |
|
||||
| 8 | 8 | 2892 | 2908 | 5.939884 | 3.533133 | 2910 | 6.922169 | 4.090128 | 2912 | 7.916950 | 4.649087 | `6` |
|
||||
| 9 | 9 | 3096 | 3112 | 2.524071 | 1.630593 | 3114 | 2.830116 | 1.857618 | 3118 | 3.101311 | 2.072647 | `6` |
|
||||
| 10 | 10 | 3154 | 3170 | 3.392997 | 2.105986 | 3172 | 4.172522 | 2.562682 | 3175 | 4.999007 | 3.042228 | `6` |
|
||||
| 11 | 11 | 3334 | 3350 | 2.984824 | 2.013493 | 3352 | 3.668105 | 2.458222 | 3355 | 4.397696 | 2.932320 | `6` |
|
||||
| 12 | 12 | 3455 | 3470 | 4.000958 | 1.990246 | 3474 | 4.929097 | 2.393668 | 3476 | 5.929425 | 2.827363 | `6` |
|
||||
| 13 | 13 | 3611 | 3627 | 6.656209 | 4.109282 | 3629 | 7.722974 | 4.798702 | 3633 | 8.793828 | 5.501948 | `6` |
|
||||
| 14 | 14 | 3694 | 3710 | 2.935180 | 1.567983 | 3712 | 3.224395 | 1.721797 | 3714 | 3.454963 | 1.844232 | `6` |
|
||||
| 15 | 15 | 3780 | 3796 | 2.573147 | 1.686690 | 3798 | 3.074261 | 2.037358 | 3801 | 3.579433 | 2.398059 | `6` |
|
||||
| 16 | 16 | 3854 | 3870 | 3.810522 | 2.197783 | 3872 | 4.756826 | 2.710410 | 3874 | 5.783033 | 3.263711 | `6` |
|
||||
| 17 | 18 | 4121 | 4138 | 6.848522 | 4.036140 | 4140 | 8.296536 | 4.890438 | 4142 | 9.827793 | 5.794164 | `6` |
|
||||
|
||||
### 14.3 推荐
|
||||
|
||||
因此这里可以分成两个不同目标:
|
||||
|
||||
1. 如果目标是“尽量复刻旧程序抓包”
|
||||
- 推荐:`TriggerSampleIndexOffsetCycles = 6`
|
||||
|
||||
2. 如果目标是“主观上想往实时反馈更靠后推一点,再试效果”
|
||||
- 可以继续保留 `7`
|
||||
- 但要明确:这已经不是“最接近旧抓包”的取值,而是主动做新的补偿尝试
|
||||
|
||||
当前基于旧抓包证据,我更推荐:
|
||||
|
||||
- 先把配置从 `7` 改回 `6`
|
||||
- 再拿新程序实际导出的 `ActualSendJointTraj.txt + ShotEvents.json + 现场图像效果` 做下一轮闭环
|
||||
125
docs/camera-trigger-actual-offset-pcap-record-20260508.md
Normal file
125
docs/camera-trigger-actual-offset-pcap-record-20260508.md
Normal file
@@ -0,0 +1,125 @@
|
||||
# 相机触发实际偏差抓包触发点记录
|
||||
|
||||
## 数据来源
|
||||
|
||||
- 抓包文件:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/Rvbust/相机触发实际偏差.pcap`
|
||||
- 对照配置:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/flyshot-replacement/Config/RobotConfig.json` 中 `flying_shots.UTTC_MS11`
|
||||
- 提取口径:只看 `192.168.10.5:50843 -> 192.168.10.11:60015` 的 64B J519 命令帧。
|
||||
- 触发判定:`write_io_mask > 0 && write_io_value > 0` 视为 IO 置位。
|
||||
- 去重规则:由于 `io_keep_cycles=2`,每次触发会连续保持两帧高电平;本文档每组只记录第一帧,最终得到 17 次真实触发。
|
||||
- 关节单位:抓包中的 J519 目标关节为 `deg`。
|
||||
|
||||
## 统计
|
||||
|
||||
- 真实触发次数:17
|
||||
- 平均单点最大单轴误差:0.142224 deg
|
||||
- 最大单轴误差:0.309618 deg
|
||||
- 平均 RMS 误差:0.085167 deg
|
||||
|
||||
## 触发记录
|
||||
|
||||
| 触发序号 | waypoint_index | frame | seq | 相对时间(s) | io_mask | J1(deg) | J2(deg) | J3(deg) | J4(deg) | J5(deg) | J6(deg) | 最大误差轴 | 最大误差(deg) | RMS误差(deg) |
|
||||
| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |
|
||||
| 1 | 1 | 2445 | 4641 | 7.536586 | 10 | 48.879890 | 2.237859 | -11.024109 | 0.101012 | 6.253663 | 2.604609 | J6 | 0.309618 | 0.179380 |
|
||||
| 2 | 2 | 2689 | 4739 | 8.320342 | 14 | 55.367146 | 11.804994 | -7.005956 | -70.935097 | 6.014911 | 74.180328 | J4 | 0.079235 | 0.043714 |
|
||||
| 3 | 3 | 2779 | 4769 | 8.560284 | 14 | 55.135155 | 8.783901 | -8.500860 | -41.282757 | 10.067441 | 42.164478 | J6 | 0.207545 | 0.110824 |
|
||||
| 4 | 4 | 2882 | 4811 | 8.896328 | 10 | 43.623173 | -1.579988 | -17.651855 | 6.007176 | 32.012070 | -22.635672 | J5 | 0.159648 | 0.078436 |
|
||||
| 5 | 5 | 3004 | 4860 | 9.288442 | 10 | 64.717529 | 4.395389 | -15.566289 | -29.885765 | 29.651941 | 45.777378 | J5 | 0.198499 | 0.137260 |
|
||||
| 6 | 6 | 3169 | 4921 | 9.776381 | 12 | 60.502796 | 23.212698 | -5.056696 | 36.065346 | 5.528813 | -31.387259 | J2 | 0.143916 | 0.073287 |
|
||||
| 7 | 7 | 3286 | 4968 | 10.152455 | 12 | 70.484123 | 16.315718 | -13.455399 | -28.008068 | 14.532272 | 31.833994 | J6 | 0.122061 | 0.075811 |
|
||||
| 8 | 8 | 3360 | 4998 | 10.392386 | 10 | 69.575752 | -17.207413 | -8.691541 | -58.617191 | 7.602061 | 64.591766 | J6 | 0.154034 | 0.096014 |
|
||||
| 9 | 9 | 3542 | 5063 | 10.912377 | 10 | 73.584053 | -6.468980 | -9.609648 | -128.917969 | 14.704389 | 140.109039 | J4 | 0.109321 | 0.066535 |
|
||||
| 10 | 10 | 3585 | 5081 | 11.056321 | 10 | 75.474472 | -14.468846 | -7.325953 | -131.159668 | 17.608482 | 142.113190 | J4 | 0.236635 | 0.165806 |
|
||||
| 11 | 11 | 3739 | 5137 | 11.504390 | 10 | 86.261429 | -14.617929 | -13.661033 | -69.765862 | 26.736444 | 88.871201 | J1 | 0.167931 | 0.107930 |
|
||||
| 12 | 12 | 3835 | 5176 | 11.816490 | 10 | 61.744911 | -4.247795 | -9.792937 | -108.367577 | 22.170429 | 118.159653 | J1 | 0.024179 | 0.016111 |
|
||||
| 13 | 13 | 3961 | 5227 | 12.224448 | 12 | 80.019569 | 4.632708 | -15.454964 | -55.831528 | 26.270802 | 52.205910 | J1 | 0.262486 | 0.159680 |
|
||||
| 14 | 14 | 4031 | 5255 | 12.448371 | 10 | 109.016167 | 1.357394 | -33.634285 | -48.365601 | 44.002415 | 85.139641 | J6 | 0.100380 | 0.053296 |
|
||||
| 15 | 15 | 4119 | 5285 | 12.688396 | 10 | 110.607498 | -3.936621 | -32.371201 | -51.189194 | 44.234520 | 87.651657 | J4 | 0.076603 | 0.047511 |
|
||||
| 16 | 16 | 4183 | 5311 | 12.896459 | 10 | 118.107513 | -17.420839 | -31.061703 | -59.539654 | 48.746391 | 94.106598 | J2 | 0.044452 | 0.024439 |
|
||||
| 17 | 18 | 4466 | 5419 | 13.760487 | 12 | 60.277737 | -22.937906 | -10.334841 | 77.472717 | 35.583603 | -69.647720 | J6 | 0.021260 | 0.011799 |
|
||||
|
||||
## 逐点示教差值
|
||||
|
||||
| 触发序号 | waypoint_index | diff_j1(deg) | diff_j2(deg) | diff_j3(deg) | diff_j4(deg) | diff_j5(deg) | diff_j6(deg) |
|
||||
| --- | --- | --- | --- | --- | --- | --- | --- |
|
||||
| 1 | 1 | -0.006920 | 0.038873 | -0.003092 | -0.309197 | 0.005281 | 0.309618 |
|
||||
| 2 | 2 | 0.019390 | -0.002046 | 0.003139 | 0.079235 | 0.002846 | -0.069204 |
|
||||
| 3 | 3 | 0.025347 | 0.024404 | 0.017357 | -0.165506 | -0.041047 | 0.207545 |
|
||||
| 4 | 4 | -0.030420 | 0.049671 | 0.063898 | 0.011967 | -0.159648 | -0.061698 |
|
||||
| 5 | 5 | 0.135085 | 0.132409 | 0.102928 | 0.067162 | -0.198499 | 0.150851 |
|
||||
| 6 | 6 | 0.023314 | 0.143916 | -0.045432 | -0.041509 | 0.003132 | -0.084695 |
|
||||
| 7 | 7 | 0.008286 | -0.078131 | 0.001549 | -0.115749 | -0.003390 | 0.122061 |
|
||||
| 8 | 8 | -0.006712 | -0.101699 | 0.019049 | -0.141497 | -0.028529 | 0.154034 |
|
||||
| 9 | 9 | 0.012794 | -0.039135 | 0.018932 | -0.109321 | 0.033306 | 0.106991 |
|
||||
| 10 | 10 | -0.094914 | 0.210460 | -0.031797 | -0.236635 | -0.079879 | 0.219682 |
|
||||
| 11 | 11 | 0.167931 | -0.119596 | 0.020478 | 0.102434 | -0.006237 | -0.128213 |
|
||||
| 12 | 12 | 0.024179 | -0.015005 | -0.008514 | 0.016506 | 0.009657 | 0.017589 |
|
||||
| 13 | 13 | 0.262486 | -0.007523 | -0.143778 | 0.203785 | 0.137664 | 0.053653 |
|
||||
| 14 | 14 | 0.060616 | -0.013366 | -0.039860 | 0.001760 | 0.039011 | 0.100380 |
|
||||
| 15 | 15 | 0.022650 | -0.075722 | 0.025207 | -0.076603 | 0.005360 | 0.027657 |
|
||||
| 16 | 16 | 0.011561 | -0.044452 | 0.007298 | 0.020885 | 0.013816 | -0.028173 |
|
||||
| 17 | 18 | -0.004746 | 0.000174 | -0.001593 | -0.018925 | 0.000225 | 0.021260 |
|
||||
|
||||
## 说明
|
||||
|
||||
- 本文档只记录“抓包中真实发 IO 的那一帧”的关节坐标。
|
||||
- 如果后续要和旧程序抓包或 `ShotEvents.json` 对比,应继续沿用同一口径:优先比较真实 IO 置位帧,而不是只比较 sample_index。
|
||||
|
||||
## 新旧程序真实触发点逐点对比
|
||||
|
||||
- 新抓包:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/Rvbust/相机触发实际偏差.pcap`
|
||||
- 旧抓包:`D:/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/Rvbust/uttc-20260428/2026042802-1.pcap`
|
||||
- 对比口径:两边都只取真实 IO 置位高电平的第一帧,共 17 次触发。
|
||||
|
||||
### 对比统计
|
||||
|
||||
- 新旧都成功提取到 17 次真实触发。
|
||||
- 平均单点最大单轴关节差:4.185609 deg
|
||||
- 最大单轴关节差:11.338289 deg
|
||||
- 平均 RMS 关节差:2.515726 deg
|
||||
- 平均相对首触发时间漂移:0.220309 s
|
||||
- 最大相对首触发时间漂移:0.432218 s
|
||||
|
||||
### 新旧实际触发记录
|
||||
|
||||
| 触发序号 | 新frame | 新seq | 新相对时间(s) | 新io_mask | 新J1 | 新J2 | 新J3 | 新J4 | 新J5 | 新J6 | 旧frame | 旧seq | 旧相对时间(s) | 旧io_mask | 旧J1 | 旧J2 | 旧J3 | 旧J4 | 旧J5 | 旧J6 | 时间差(s) | 相对首点漂移(s) | 最大差轴 | 最大关节差(deg) | RMS差(deg) |
|
||||
| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |
|
||||
| 1 | 2445 | 4641 | 7.536586 | 10 | 48.879890 | 2.237859 | -11.024109 | 0.101012 | 6.253663 | 2.604609 | 1955 | 1381124 | 5.888271 | 10 | 45.863270 | 2.972479 | -13.609342 | -2.185114 | 7.582361 | 5.701518 | 1.648315 | 0.000000 | J6 | 3.096909 | 2.341857 |
|
||||
| 2 | 2689 | 4739 | 8.320342 | 14 | 55.367146 | 11.804994 | -7.005956 | -70.935097 | 6.014911 | 74.180328 | 2151 | 1381198 | 6.480252 | 14 | 56.343365 | 11.667239 | -6.600924 | -68.197609 | 6.058437 | 71.539284 | 1.840090 | 0.191775 | J4 | 2.737488 | 1.612807 |
|
||||
| 3 | 2779 | 4769 | 8.560284 | 14 | 55.135155 | 8.783901 | -8.500860 | -41.282757 | 10.067441 | 42.164478 | 2223 | 1381226 | 6.704225 | 14 | 53.508320 | 7.412223 | -9.629740 | -32.305714 | 12.488306 | 30.826189 | 1.856059 | 0.207744 | J6 | 11.338289 | 6.066399 |
|
||||
| 4 | 2882 | 4811 | 8.896328 | 10 | 43.623173 | -1.579988 | -17.651855 | 6.007176 | 32.012070 | -22.635672 | 2336 | 1381269 | 7.048266 | 10 | 44.433006 | -2.115629 | -18.397774 | 4.837199 | 33.993725 | -20.262289 | 1.848062 | 0.199747 | J6 | 2.373383 | 1.439206 |
|
||||
| 5 | 3004 | 4860 | 9.288442 | 10 | 64.717529 | 4.395389 | -15.566289 | -29.885765 | 29.651941 | 45.777378 | 2477 | 1381321 | 7.464289 | 10 | 65.849670 | 5.695003 | -14.563637 | -28.693426 | 27.678612 | 46.437157 | 1.824153 | 0.175838 | J5 | 1.973330 | 1.273254 |
|
||||
| 6 | 3169 | 4921 | 9.776381 | 12 | 60.502796 | 23.212698 | -5.056696 | 36.065346 | 5.528813 | -31.387259 | 2665 | 1381391 | 8.024277 | 12 | 60.847530 | 24.477776 | -5.617044 | 34.804432 | 5.786255 | -31.225348 | 1.752104 | 0.103789 | J2 | 1.265078 | 0.786940 |
|
||||
| 7 | 3286 | 4968 | 10.152455 | 12 | 70.484123 | 16.315718 | -13.455399 | -28.008068 | 14.532272 | 31.833994 | 2811 | 1381447 | 8.472317 | 12 | 70.825050 | 10.952600 | -13.136046 | -34.684597 | 14.022421 | 38.906040 | 1.680138 | 0.031823 | J6 | 7.072046 | 4.542963 |
|
||||
| 8 | 3360 | 4998 | 10.392386 | 10 | 69.575752 | -17.207413 | -8.691541 | -58.617191 | 7.602061 | 64.591766 | 2908 | 1381483 | 8.760361 | 10 | 69.405167 | -20.066782 | -8.152936 | -63.932354 | 6.775912 | 70.377617 | 1.632025 | -0.016290 | J6 | 5.785851 | 3.437653 |
|
||||
| 9 | 3542 | 5063 | 10.912377 | 10 | 73.584053 | -6.468980 | -9.609648 | -128.917969 | 14.704389 | 140.109039 | 3112 | 1381561 | 9.384332 | 10 | 73.905930 | -8.016125 | -9.002426 | -131.332718 | 15.526207 | 142.416687 | 1.528045 | -0.120270 | J4 | 2.414749 | 1.565121 |
|
||||
| 10 | 3585 | 5081 | 11.056321 | 10 | 75.474472 | -14.468846 | -7.325953 | -131.159668 | 17.608482 | 142.113190 | 3170 | 1381583 | 9.560358 | 10 | 76.726883 | -16.518726 | -7.151471 | -127.530037 | 18.537889 | 138.789749 | 1.495963 | -0.152352 | J4 | 3.629631 | 2.268783 |
|
||||
| 11 | 3739 | 5137 | 11.504390 | 10 | 86.261429 | -14.617929 | -13.661033 | -69.765862 | 26.736444 | 88.871201 | 3350 | 1381651 | 10.104322 | 10 | 83.716621 | -13.186573 | -13.738809 | -72.703178 | 26.667589 | 91.984238 | 1.400068 | -0.248247 | J6 | 3.113037 | 2.115595 |
|
||||
| 12 | 3835 | 5176 | 11.816490 | 10 | 61.744911 | -4.247795 | -9.792937 | -108.367577 | 22.170429 | 118.159653 | 3470 | 1381697 | 10.472333 | 10 | 59.789558 | -2.709417 | -8.933828 | -108.248573 | 21.177492 | 114.141106 | 1.344157 | -0.304158 | J6 | 4.018547 | 2.003201 |
|
||||
| 13 | 3961 | 5227 | 12.224448 | 12 | 80.019569 | 4.632708 | -15.454964 | -55.831528 | 26.270802 | 52.205910 | 3627 | 1381756 | 10.944371 | 12 | 86.413292 | 4.266173 | -19.159878 | -52.240280 | 29.853724 | 55.869072 | 1.280077 | -0.368238 | J1 | 6.393723 | 3.955873 |
|
||||
| 14 | 4031 | 5255 | 12.448371 | 10 | 109.016167 | 1.357394 | -33.634285 | -48.365601 | 44.002415 | 85.139641 | 3710 | 1381787 | 11.192339 | 10 | 110.752319 | 0.875189 | -34.756535 | -48.143326 | 45.086559 | 87.974442 | 1.256032 | -0.392283 | J6 | 2.834801 | 1.514764 |
|
||||
| 15 | 4119 | 5285 | 12.688396 | 10 | 110.607498 | -3.936621 | -32.371201 | -51.189194 | 44.234520 | 87.651657 | 3796 | 1381820 | 11.456361 | 10 | 111.791779 | -6.225441 | -31.958988 | -53.685738 | 44.761509 | 89.334549 | 1.232035 | -0.416280 | J4 | 2.496544 | 1.640818 |
|
||||
| 16 | 4183 | 5311 | 12.896459 | 10 | 118.107513 | -17.420839 | -31.061703 | -59.539654 | 48.746391 | 94.106598 | 3870 | 1381848 | 11.680362 | 10 | 117.874741 | -19.607178 | -30.329103 | -56.600250 | 49.086555 | 90.324249 | 1.216097 | -0.432218 | J6 | 3.782349 | 2.176883 |
|
||||
| 17 | 4466 | 5419 | 13.760487 | 12 | 60.277737 | -22.937906 | -10.334841 | 77.472717 | 35.583603 | -69.647720 | 4138 | 1381950 | 12.496365 | 12 | 59.935894 | -21.102140 | -9.684239 | 70.643120 | 32.941307 | -63.348557 | 1.264122 | -0.384193 | J4 | 6.829597 | 4.025228 |
|
||||
|
||||
### 新减旧逐轴关节差
|
||||
|
||||
| 触发序号 | diff_j1(deg) | diff_j2(deg) | diff_j3(deg) | diff_j4(deg) | diff_j5(deg) | diff_j6(deg) |
|
||||
| --- | --- | --- | --- | --- | --- | --- |
|
||||
| 1 | 3.016621 | -0.734620 | 2.585233 | 2.286127 | -1.328699 | -3.096909 |
|
||||
| 2 | -0.976219 | 0.137754 | -0.405033 | -2.737488 | -0.043527 | 2.641045 |
|
||||
| 3 | 1.626835 | 1.371678 | 1.128880 | -8.977043 | -2.420865 | 11.338289 |
|
||||
| 4 | -0.809834 | 0.535641 | 0.745918 | 1.169977 | -1.981655 | -2.373383 |
|
||||
| 5 | -1.132141 | -1.299614 | -1.002652 | -1.192339 | 1.973330 | -0.659779 |
|
||||
| 6 | -0.344734 | -1.265078 | 0.560348 | 1.260914 | -0.257442 | -0.161911 |
|
||||
| 7 | -0.340927 | 5.363118 | -0.319352 | 6.676529 | 0.509851 | -7.072046 |
|
||||
| 8 | 0.170586 | 2.859369 | -0.538605 | 5.315163 | 0.826149 | -5.785851 |
|
||||
| 9 | -0.321877 | 1.547145 | -0.607222 | 2.414749 | -0.821818 | -2.307648 |
|
||||
| 10 | -1.252411 | 2.049880 | -0.174482 | -3.629631 | -0.929407 | 3.323441 |
|
||||
| 11 | 2.544807 | -1.431356 | 0.077776 | 2.937317 | 0.068855 | -3.113037 |
|
||||
| 12 | 1.955353 | -1.538378 | -0.859109 | -0.119003 | 0.992937 | 4.018547 |
|
||||
| 13 | -6.393723 | 0.366534 | 3.704914 | -3.591248 | -3.582922 | -3.663162 |
|
||||
| 14 | -1.736153 | 0.482206 | 1.122250 | -0.222275 | -1.084145 | -2.834801 |
|
||||
| 15 | -1.184280 | 2.288819 | -0.412212 | 2.496544 | -0.526989 | -1.682892 |
|
||||
| 16 | 0.232773 | 2.186338 | -0.732599 | -2.939404 | -0.340164 | 3.782349 |
|
||||
| 17 | 0.341843 | -1.835766 | -0.650601 | 6.829597 | 2.642296 | -6.299164 |
|
||||
143
docs/controller-client-api-compatibility-requirements.md
Normal file
143
docs/controller-client-api-compatibility-requirements.md
Normal file
@@ -0,0 +1,143 @@
|
||||
# ControllerClient API 兼容逆向约束
|
||||
|
||||
> 记录时间:2026-04-24
|
||||
> 适用仓库:`flyshot-replacement`
|
||||
> 当前阶段:已落地 HTTP-only `ControllerClientCompat` 服务,并已将轨迹执行接入规划、触发时间轴和最小运行时骨架;不实现 `50001/TCP+JSON` 监听
|
||||
|
||||
## 1. 当前目标
|
||||
|
||||
本轮目标不是直接实现 `50001/TCP+JSON` 兼容网关,而是先把旧 `ControllerClient` 暴露的公开 API 做成可执行的逆向合同。
|
||||
|
||||
本轮交付物固定为两份文档:
|
||||
|
||||
- `docs/controller-client-api-compatibility-requirements.md`
|
||||
- `docs/controller-client-api-reverse-engineering.md`
|
||||
|
||||
后续继续扩展 `Flyshot.ControllerClientCompat` 的方法覆盖、兼容测试矩阵或真实控制器联动时,必须以这两份文档为准,不再重新口头约定接口语义。
|
||||
|
||||
## 2. 范围边界
|
||||
|
||||
本轮只覆盖 `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h` 中的 32 个公开方法。
|
||||
|
||||
分组如下:
|
||||
|
||||
- 传输与版本:`ConnectServer`、`GetServerVersion`、`GetClientVersion`
|
||||
- 机器人初始化:`SetUpRobot`、`SetUpRobotFromEnv`、`IsSetUp`、`SetShowTCP`、`GetName`、`GetDoF`
|
||||
- 控制器状态:`SetActiveController`、`Connect`、`Disconnect`、`EnableRobot`、`DisableRobot`、`StopMove`
|
||||
- 参数与 IO:`GetSpeedRatio`、`SetSpeedRatio`、`GetTCP`、`SetTCP`、`GetIO`、`SetIO`
|
||||
- 运动与求解:`GetJointPosition`、`GetPose`、`GetNearestIK`、`MoveJoint`、`ExecuteTrajectory`
|
||||
- 飞拍轨迹:`UploadFlyShotTraj`、`DeleteFlyShotTraj`、`ListFlyShotTraj`、`ExecuteFlyShotTraj`、`SaveTrajInfo`、`IsFlyShotTrajValid`
|
||||
|
||||
明确不在本轮范围内:
|
||||
|
||||
- `ControllerServer` 内部所有未公开 `_Xxx` 方法的完整实现复原
|
||||
- `50001` 网关代码、TCP server、JSON parser、命令路由实现
|
||||
- 真机 `10010 / 10012 / 60015` 联调
|
||||
- 抓包、hook、反汇编级协议完全坐实
|
||||
- 把当前 replacement 仓库的实现约束误写成旧系统事实
|
||||
|
||||
## 3. 证据源优先级
|
||||
|
||||
逆向结论必须按以下优先级交叉确认:
|
||||
|
||||
1. `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
2. `../FlyingShot/FlyingShot/Example/UseControllerClient.cpp`
|
||||
3. `../FlyingShot/FlyingShot/Example/UseControllerClient.py`
|
||||
4. `../FlyingShot/FlyingShot/Example/UseRealRobot.py`
|
||||
5. `../FlyingShot/FlyingShot/Docs/用户手册/FANUC飞拍软件及SDK用户手册.md`
|
||||
6. `../analysis/ControllerServer_analysis.md`
|
||||
7. `../analysis/CommonMsg_protocol_analysis.md`
|
||||
8. `../analysis/Trajectory_generation_algorithm_analysis.md`
|
||||
9. `../FlyingShot/FlyingShot/Lib/libControllerClient.so` 与 `../FlyingShot/FlyingShot/Python/ControllerServer/ControllerServer.cpython-37m-x86_64-linux-gnu.so` 的字符串证据
|
||||
|
||||
使用规则:
|
||||
|
||||
- 头文件负责定义“公开合同”:方法名、参数名、默认值、返回类型。
|
||||
- 示例和手册负责定义“典型调用方式”:调用顺序、Python 包装形态、用户侧常见用法。
|
||||
- 逆向分析文档和二进制字符串负责定义“服务端映射”和“协议线索”:`_Xxx` 方法名、错误文本、命令名、部分字段顺序。
|
||||
- 不能从当前 replacement 仓库的未来设计反推旧系统事实。
|
||||
|
||||
## 4. 文档填写规则
|
||||
|
||||
主归档文档 `docs/controller-client-api-reverse-engineering.md` 对每个 API 必须固定填写以下项目:
|
||||
|
||||
- C++ 公开签名
|
||||
- Python 包装形态
|
||||
- 服务端归属
|
||||
- 协议 / 命令线索
|
||||
- 返回值与默认值
|
||||
- 典型工作流位置
|
||||
- 证据来源
|
||||
- 置信度
|
||||
- 待确认点
|
||||
|
||||
填写约束:
|
||||
|
||||
- 只要精确 JSON 包结构还未恢复,就明确写成“命令名已知,JSON envelope 未恢复”。
|
||||
- 只要 Python 失败路径没有样例,就不能假装已经确认失败时的返回形态。
|
||||
- `GetServerVersion` 这类没有显式 `_GetServerVersion` 的接口,必须标成“协议分发层行为”,不能伪造一个服务端实现名。
|
||||
- `ConnectServer` 与 `GetClientVersion` 必须明确标成客户端侧行为,不写进服务端命令表。
|
||||
- 对 `GetJointPosition -> _GetJointPositions` 这种命名不一致,要单独注明。
|
||||
- 当同一结论同时来自头文件、示例和字符串时,置信度可标为“高”;只有示例间接体现时,最多“中”。
|
||||
|
||||
## 5. 验收条件
|
||||
|
||||
本轮逆向归档完成时,至少满足:
|
||||
|
||||
- 32 个公开方法全部覆盖,且每个方法只出现一次
|
||||
- 29 个服务端相关 API 有明确映射或明确写成协议分发层行为
|
||||
- `ConnectServer`、`GetClientVersion` 两个客户端侧行为被明确排除在服务端命令表外
|
||||
- 四条工作流附录完整:
|
||||
- 初始化工作流
|
||||
- 控制器状态工作流
|
||||
- 普通轨迹工作流
|
||||
- 飞拍轨迹工作流
|
||||
- 已知高置信协议字段至少记录:
|
||||
- `GetSpeedRatio`
|
||||
- `SetSpeedRatio`
|
||||
- `GetIO`
|
||||
- `SetIO`
|
||||
- 仍未恢复的部分必须进入“待确认问题”清单,不能被静默略过
|
||||
|
||||
## 6. 当前已确认摘要
|
||||
|
||||
当前已确认的高价值结论如下:
|
||||
|
||||
- `ControllerClient.h` 中共有 32 个公开方法。
|
||||
- 其中 29 个方法可与 `ControllerServer` 的公开 `_Xxx` 方法一一对齐。
|
||||
- `GetServerVersion` 能看到明确字符串证据,但未恢复到显式 `_GetServerVersion` 实现,更接近 `_ClientCB` / `_IsJsonValid` 所在的协议分发层。
|
||||
- `ConnectServer` 是客户端建立到 `127.0.0.1:50001` 的传输层动作。
|
||||
- `GetClientVersion` 更像客户端库自身版本查询,不进入服务端命令表。
|
||||
- `GetSpeedRatio` / `SetSpeedRatio`、`GetIO` / `SetIO` 已有较高置信度的底层字段顺序与 `MsgID` 证据。
|
||||
- `UploadFlyShotTraj`、`ListFlyShotTraj` 存在额外字符串线索:
|
||||
- `StartUploadFlyShotTraj`
|
||||
- `EndUploadFlyShotTraj`
|
||||
- `GetNextListFlyShotTraj`
|
||||
- `ExecuteFlyShotTraj`、`IsFlyShotTrajValid`、`SaveTrajInfo` 在工作流上有清晰分工,不能混成一个“执行轨迹”接口。
|
||||
|
||||
## 7. 下轮实现约束
|
||||
|
||||
当前 HTTP-only 兼容层已经可以承接公开 API 的主要服务端语义,并且 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已经进入 replacement 自身的规划与运行时链路。后续扩展必须遵守:
|
||||
|
||||
- 先以逆向合同建命令表,再写 `TCP + JSON` 入口
|
||||
- 先做兼容测试矩阵,再补最小命令桩
|
||||
- 区分“旧系统事实”和“replacement 当前策略”
|
||||
- 真机未接通前,允许实现层返回稳定错误或模拟状态,但不能反过来污染逆向文档
|
||||
- `50001/TCP+JSON` 抓包已经覆盖 `SetSpeedRatio` 与 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)`,请求中没有显式 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk` 字段;因此规划限制的补齐必须作为 replacement-only 策略记录,不能写成旧公开 API 合同。
|
||||
|
||||
## 8. 当前 replacement 实现状态
|
||||
|
||||
以下内容是当前新实现的状态,不反推为旧系统事实:
|
||||
|
||||
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务,不启动 `50001/TCP+JSON` 监听。
|
||||
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹,再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
|
||||
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹,通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
|
||||
- HTTP 控制器已经按公开文档和抓包补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache, wait)` 参数,并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
|
||||
- 规划阶段会继续消费旧配置中的 `acc_limit / jerk_limit`。如果现场需要复现旧服务端不可见的保守约束,replacement 设计上使用内部 `planning_acceleration_scale` 限制规划加速度;该字段不属于旧 `ControllerClient` 公开 API,也不会通过 `50001` JSON 下发。
|
||||
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器;`method="doubles"` 会被识别但返回显式未实现,不会静默降级成 ICSP。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码,并使用逆向抓包样本覆盖最小测试;`10010` 当前现场确认固定 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路;稠密轨迹下发已按 `speed_ratio` 推进轨迹时间,并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。
|
||||
- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`,`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包;实际执行不发送 464 行导出点,而是在 8ms 实发周期上按 `t_traj = t_send * speed_ratio` 采样,包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`,用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口,但状态写入已经统一经过运行时,而不是由兼容服务自己维护关节数组。
|
||||
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状,但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||
562
docs/controller-client-api-reverse-engineering.md
Normal file
562
docs/controller-client-api-reverse-engineering.md
Normal file
@@ -0,0 +1,562 @@
|
||||
# ControllerClient API 全量逆向归档
|
||||
|
||||
> 记录时间:2026-04-24
|
||||
> 适用仓库:`flyshot-replacement`
|
||||
> 目标:为后续 `50001/TCP+JSON` 兼容网关实现提供只读合同
|
||||
|
||||
## 1. 总览
|
||||
|
||||
本次归档覆盖 `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h` 中全部 32 个公开方法。
|
||||
|
||||
当前分类结果:
|
||||
|
||||
- 29 个 API 可与 `ControllerServer` 公开 `_Xxx` 方法一一映射
|
||||
- 1 个 API 属于协议分发层行为:`GetServerVersion`
|
||||
- 2 个 API 属于客户端侧行为:`ConnectServer`、`GetClientVersion`
|
||||
|
||||
本文档中的“协议 / 命令线索”有两个层次:
|
||||
|
||||
- 命令名:已从头文件、字符串或逆向文档坐实,例如 `SetUpRobot`、`GetIO`
|
||||
- JSON envelope:请求 JSON 的精确键名、必填规则、整体结构;当前大多仍未完全恢复
|
||||
|
||||
因此,除 `GetSpeedRatio` / `SetSpeedRatio` / `GetIO` / `SetIO` 等已在命令通道层坐实字段顺序的接口外,其余接口默认只写“命令名已知,精确 JSON 包结构待确认”。
|
||||
|
||||
置信度定义:
|
||||
|
||||
- 高:头文件 + 示例/手册 + 字符串/逆向文档三方交叉确认
|
||||
- 中:头文件 + 示例,或头文件 + 字符串,缺少第三方交叉
|
||||
- 低:仅有间接线索,未达到本轮归档主结论标准
|
||||
|
||||
## 2. 传输与版本
|
||||
|
||||
### `ConnectServer`
|
||||
|
||||
- C++ 公开签名:`bool ConnectServer(const std::string &server_ip = "127.0.0.1", unsigned port = 50001);`
|
||||
- Python 包装形态:`c.ConnectServer(server_ip="127.0.0.1", port=50001) -> bool`
|
||||
- 服务端归属:客户端传输层行为,不进入 `ControllerServer._Xxx` 命令表
|
||||
- 协议 / 命令线索:建立到 `127.0.0.1:50001` 的 TCP 连接;JSON 负载只发生在连接建立之后
|
||||
- 返回值与默认值:默认 `server_ip="127.0.0.1"`、`port=50001`;成功返回 `true`
|
||||
- 典型工作流位置:所有远程 API 的前置步骤
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`
|
||||
- 置信度:高
|
||||
- 待确认点:重连、超时、握手细节未恢复
|
||||
|
||||
### `GetServerVersion`
|
||||
|
||||
- C++ 公开签名:`bool GetServerVersion(std::string &version);`
|
||||
- Python 包装形态:样例中表现为 `controller.GetServerVersion() -> str`
|
||||
- 服务端归属:协议分发层行为;当前未恢复到显式 `_GetServerVersion`
|
||||
- 协议 / 命令线索:命令名字符串 `GetServerVersion` 已坐实;服务端存在 `GetServerVersion success: {}.` 日志;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:C++ 为 `bool + out string`;Python 样例把结果当作直接字符串使用
|
||||
- 典型工作流位置:`ConnectServer` 之后、机器人初始化前后均可读取的元信息接口
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`libControllerClient.so`、`ControllerServer.cpython-37m-x86_64-linux-gnu.so`
|
||||
- 置信度:高
|
||||
- 待确认点:Python 失败路径的返回形态未看到样例;请求 JSON 的精确字段未恢复
|
||||
|
||||
### `GetClientVersion`
|
||||
|
||||
- C++ 公开签名:`bool GetClientVersion(std::string &version);`
|
||||
- Python 包装形态:未见公开样例;从包装库字符串看接口存在
|
||||
- 服务端归属:客户端本地行为,不进入服务端命令表
|
||||
- 协议 / 命令线索:`libControllerClient.so` 与 `PyControllerClient` 都有 `GetClientVersion` 符号;未见 `ControllerServer` 对应 `_Xxx`
|
||||
- 返回值与默认值:C++ 为 `bool + out string`;Python 侧返回形态待确认
|
||||
- 典型工作流位置:客户端自检或 SDK 版本上报,不依赖服务端状态
|
||||
- 证据来源:`ControllerClient.h`、`libControllerClient.so`、`PyControllerClient.cpython-37m-x86_64-linux-gnu.so`
|
||||
- 置信度:高
|
||||
- 待确认点:实际版本字符串来源与 Python 包装失败行为未恢复
|
||||
|
||||
## 3. 机器人初始化
|
||||
|
||||
### `SetUpRobot`
|
||||
|
||||
- C++ 公开签名:`bool SetUpRobot(const std::string &robot_name);`
|
||||
- Python 包装形态:`c.SetUpRobot("FANUC_LR_Mate_200iD") -> bool`
|
||||
- 服务端归属:`ControllerServer._SetUpRobot`
|
||||
- 协议 / 命令线索:命令名 `SetUpRobot` 已坐实;参数高概率是 `robot_name`;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:创建客户端并连上 `50001` 后首先调用;旧说明明确它是最先执行的服务端初始化动作
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:请求字段键名是否为 `robot_name` 仍需抓包或 hook 坐实
|
||||
|
||||
### `SetUpRobotFromEnv`
|
||||
|
||||
- C++ 公开签名:`bool SetUpRobotFromEnv(const std::string &env_file);`
|
||||
- Python 包装形态:未见公开样例;服务端与客户端字符串均存在
|
||||
- 服务端归属:`ControllerServer._SetUpRobotFromEnv`
|
||||
- 协议 / 命令线索:命令名 `SetUpRobotFromEnv` 已坐实;参数高概率是环境文件绝对路径;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:与 `SetUpRobot` 二选一;当现场通过环境文件而不是型号名初始化时使用
|
||||
- 证据来源:`ControllerClient.h`、`ControllerServer_analysis.md`、`ControllerServer` 字符串、`libControllerClient.so`
|
||||
- 置信度:高
|
||||
- 待确认点:环境文件路径是否必须为绝对路径由注释可见,但服务端是否做额外归一化未恢复
|
||||
|
||||
### `IsSetUp`
|
||||
|
||||
- C++ 公开签名:`bool IsSetUp();`
|
||||
- Python 包装形态:`c.IsSetUp() -> bool`
|
||||
- 服务端归属:`ControllerServer._IsSetUp`
|
||||
- 协议 / 命令线索:命令名 `IsSetUp` 已坐实;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:直接返回布尔值
|
||||
- 典型工作流位置:`SetUpRobot` 或 `SetUpRobotFromEnv` 之后的状态确认
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:服务端返回是否仅受机器人对象存在性控制,还是还依赖模型/控制器更深层状态,当前未完全恢复
|
||||
|
||||
### `SetShowTCP`
|
||||
|
||||
- C++ 公开签名:`bool SetShowTCP(bool is_show = true, double axis_length = 0.1, size_t axis_size = 2);`
|
||||
- Python 包装形态:`c.SetShowTCP(is_show=True, axis_length=0.1, axis_size=2) -> bool`
|
||||
- 服务端归属:`ControllerServer._SetShowTCP`
|
||||
- 协议 / 命令线索:命令名 `SetShowTCP` 已坐实;字符串中可见 `SetShowTCP is_show success/failed`;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:默认 `is_show=true`、`axis_length=0.1`、`axis_size=2`
|
||||
- 典型工作流位置:初始化完成后、切换控制器前的仿真显示配置
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`ControllerServer` 字符串
|
||||
- 置信度:高
|
||||
- 待确认点:仅仿真控制器生效还是真实控制器也接受该命令,当前未见明确负例
|
||||
|
||||
### `GetName`
|
||||
|
||||
- C++ 公开签名:`std::string GetName();`
|
||||
- Python 包装形态:`c.GetName() -> str`
|
||||
- 服务端归属:`ControllerServer._GetName`
|
||||
- 协议 / 命令线索:命令名 `GetName` 已坐实;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:直接返回机器人名称字符串
|
||||
- 典型工作流位置:完成机器人初始化后读取名称确认
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:失败时是返回空串还是异常/默认值,Python 样例未覆盖
|
||||
|
||||
### `GetDoF`
|
||||
|
||||
- C++ 公开签名:`int GetDoF();`
|
||||
- Python 包装形态:`c.GetDoF() -> int`
|
||||
- 服务端归属:`ControllerServer._GetDoF`
|
||||
- 协议 / 命令线索:命令名 `GetDoF` 已坐实;精确 JSON envelope 未恢复
|
||||
- 返回值与默认值:直接返回自由度整数
|
||||
- 典型工作流位置:完成机器人初始化后读取自由度
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:失败时的客户端行为未恢复
|
||||
|
||||
## 4. 控制器状态
|
||||
|
||||
### `SetActiveController`
|
||||
|
||||
- C++ 公开签名:`bool SetActiveController(bool sim = true);`
|
||||
- Python 包装形态:`c.SetActiveController(sim=True) -> bool`;真机样例使用 `sim=False`
|
||||
- 服务端归属:`ControllerServer._SetActiveController`
|
||||
- 协议 / 命令线索:命令名 `SetActiveController` 已坐实;字符串中可见 `SetActiveController sim success/failed`
|
||||
- 返回值与默认值:默认 `sim=true`
|
||||
- 典型工作流位置:机器人初始化后、`Connect` 前,用于在仿真控制器和真实控制器之间切换
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:切换控制器时是否会隐式断开旧控制器,当前只有 `_DisconnectAll` 的间接线索
|
||||
|
||||
### `Connect`
|
||||
|
||||
- C++ 公开签名:`bool Connect(const std::string &robot_ip);`
|
||||
- Python 包装形态:`c.Connect("192.168.10.101") -> bool`
|
||||
- 服务端归属:`ControllerServer._Connect`
|
||||
- 协议 / 命令线索:命令名 `Connect` 已坐实;字符串中可见 `Connect ip success/failed`
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:选定活动控制器后,通知服务端连接真实机器人 IP
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 键名是否为 `ip` 或 `robot_ip` 未恢复;仿真模式下是否接受同一命令仍需实现侧验证
|
||||
|
||||
### `Disconnect`
|
||||
|
||||
- C++ 公开签名:`bool Disconnect();`
|
||||
- Python 包装形态:`c.Disconnect() -> bool`
|
||||
- 服务端归属:`ControllerServer._Disconnect`
|
||||
- 协议 / 命令线索:命令名 `Disconnect` 已坐实;字符串中可见 `Disconnect success/failed`
|
||||
- 返回值与默认值:直接返回布尔值
|
||||
- 典型工作流位置:真实控制器断开或工作流结束时调用
|
||||
- 证据来源:`ControllerClient.h`、`ControllerServer_analysis.md`、`ControllerServer` 字符串、`libControllerClient.so`
|
||||
- 置信度:高
|
||||
- 待确认点:是否同步清理状态通道 / 命令通道 / 伺服通道,当前只在 FANUC 分析文档看到对象链,并未完全坐实释放顺序
|
||||
|
||||
### `EnableRobot`
|
||||
|
||||
- C++ 公开签名:`bool EnableRobot(unsigned buffer_size = 2);`
|
||||
- Python 包装形态:`c.EnableRobot() -> bool`
|
||||
- 服务端归属:`ControllerServer._EnableRobot`
|
||||
- 协议 / 命令线索:命令名 `EnableRobot` 已坐实;字符串中可见 `EnableRobot success/failed`
|
||||
- 返回值与默认值:默认 `buffer_size=2`
|
||||
- 典型工作流位置:`Connect` 后使能机器人;多数运动 API 前置条件
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:`buffer_size` 在旧服务端具体如何映射到伺服缓冲区参数,当前未完全恢复
|
||||
|
||||
### `DisableRobot`
|
||||
|
||||
- C++ 公开签名:`bool DisableRobot();`
|
||||
- Python 包装形态:`c.DisableRobot() -> bool`
|
||||
- 服务端归属:`ControllerServer._DisableRobot`
|
||||
- 协议 / 命令线索:命令名 `DisableRobot` 已坐实;字符串中可见 `DisableRobot success/failed`
|
||||
- 返回值与默认值:直接返回布尔值
|
||||
- 典型工作流位置:停机或退出前关闭机器人使能
|
||||
- 证据来源:`ControllerClient.h`、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:与 `StopMove` 的先后顺序约束未恢复
|
||||
|
||||
### `StopMove`
|
||||
|
||||
- C++ 公开签名:`bool StopMove();`
|
||||
- Python 包装形态:`c.StopMove() -> bool`
|
||||
- 服务端归属:`ControllerServer._StopMove`
|
||||
- 协议 / 命令线索:命令名 `StopMove` 已坐实;字符串中可见 `StopMove success/failed`
|
||||
- 返回值与默认值:直接返回布尔值
|
||||
- 典型工作流位置:当前运动中止;Python 示例中调用后会再次 `EnableRobot()`
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:是否会清空控制器执行队列、是否必须重新使能,当前只从示例看到倾向性而非硬约束
|
||||
|
||||
## 5. 参数与 IO
|
||||
|
||||
### `GetSpeedRatio`
|
||||
|
||||
- C++ 公开签名:`double GetSpeedRatio();`
|
||||
- Python 包装形态:`c.GetSpeedRatio() -> float`
|
||||
- 服务端归属:`ControllerServer._GetSpeedRatio`
|
||||
- 协议 / 命令线索:命令通道 `MsgID = 0x2206`;响应字段为 `ratio_int` 与 `result_code`;成功后客户端将 `ratio_int / 100.0`
|
||||
- 返回值与默认值:直接返回 `0~1` 之间的倍率
|
||||
- 典型工作流位置:连机并使能后读取当前速度倍率
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`
|
||||
- 置信度:高
|
||||
- 待确认点:服务端 JSON 层到命令通道层的参数桥接结构未恢复
|
||||
|
||||
### `SetSpeedRatio`
|
||||
|
||||
- C++ 公开签名:`bool SetSpeedRatio(double ratio);`
|
||||
- Python 包装形态:`c.SetSpeedRatio(0.8) -> bool`
|
||||
- 服务端归属:`ControllerServer._SetSpeedRatio`
|
||||
- 协议 / 命令线索:命令通道 `MsgID = 0x2207`;请求字段为 `ratio_int_0_100`;输入 `double` 先乘 `100` 并夹到 `[0,100]`
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:连机并使能后修改控制器速度倍率
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:ratio 越界时客户端是裁剪还是直接失败,当前更偏向裁剪,但缺少公开负例
|
||||
|
||||
### `GetTCP`
|
||||
|
||||
- C++ 公开签名:`bool GetTCP(Pose &tcp);`
|
||||
- Python 包装形态:`res, tcp = c.GetTCP()`
|
||||
- 服务端归属:`ControllerServer._GetTCP`
|
||||
- 协议 / 命令线索:控制器命令通道底层接口 `GetTCP` 已有 `MsgID = 0x2200` 证据,但 `ControllerClient` JSON 层请求结构未完全恢复
|
||||
- 返回值与默认值:C++ 为 `bool + out Pose`;Python 为 `(bool, Pose)`
|
||||
- 典型工作流位置:连机后读取当前控制器 TCP
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 层是否支持多个 TCP ID,目前只看到底层命令通道有 `tcp_id`
|
||||
|
||||
### `SetTCP`
|
||||
|
||||
- C++ 公开签名:`bool SetTCP(const Pose &tcp);`
|
||||
- Python 包装形态:`c.SetTCP(tcp) -> bool`
|
||||
- 服务端归属:`ControllerServer._SetTCP`
|
||||
- 协议 / 命令线索:控制器命令通道底层接口 `SetTCP` 已有 `MsgID = 0x2201` 证据;JSON 层精确字段未恢复
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:连机后修改控制器 TCP
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 层是否也暴露 `tcp_id` 概念尚未恢复
|
||||
|
||||
### `GetIO`
|
||||
|
||||
- C++ 公开签名:`bool GetIO(unsigned port, bool &value, IOType type = kIOTypeDI);`
|
||||
- Python 包装形态:`res, value = c.GetIO(port=1, io_type=IOType.kIOTypeDI)`
|
||||
- 服务端归属:`ControllerServer._GetIO`
|
||||
- 协议 / 命令线索:命令通道 `MsgID = 0x2208`;请求字段顺序是 `io_type`、`io_index`;响应字段是 `result_code`、`io_value`
|
||||
- 返回值与默认值:默认 `type = kIOTypeDI`;C++ 为 `bool + out bool`;Python 为 `(bool, bool)`
|
||||
- 典型工作流位置:连机后读取 DI/DO/RI/RO 等 IO 值
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:高层 JSON 是否把字段命名为 `port` / `value` / `type`,还是 `io_index` / `io_type`,当前未抓到完整包
|
||||
|
||||
### `SetIO`
|
||||
|
||||
- C++ 公开签名:`bool SetIO(unsigned port, bool value, IOType type = kIOTypeDO);`
|
||||
- Python 包装形态:`c.SetIO(port=1, value=True, io_type=IOType.kIOTypeDO) -> bool`
|
||||
- 服务端归属:`ControllerServer._SetIO`
|
||||
- 协议 / 命令线索:命令通道 `MsgID = 0x2209`;请求字段顺序是 `io_type`、`io_index`、`io_value`
|
||||
- 返回值与默认值:默认 `type = kIOTypeDO`
|
||||
- 典型工作流位置:连机后设置数字输出;飞拍链路之外的普通 IO 调试也会用到
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:布尔值在高层 JSON 中是否以 `true/false` 传输、在命令通道中以 `float` 传输,当前只坐实了命令通道层
|
||||
|
||||
## 6. 运动与求解
|
||||
|
||||
### `GetJointPosition`
|
||||
|
||||
- C++ 公开签名:`bool GetJointPosition(JointPositions &joint_position);`
|
||||
- Python 包装形态:`res, joints = c.GetJointPosition()`
|
||||
- 服务端归属:`ControllerServer._GetJointPositions`
|
||||
- 协议 / 命令线索:命令名 `GetJointPositions` 已坐实;客户端公开 API 名与服务端方法名存在单复数差异
|
||||
- 返回值与默认值:C++ 为 `bool + out JointPositions`;Python 为 `(bool, JointPositions)`
|
||||
- 典型工作流位置:读取当前关节角,常作为 `GetNearestIK` 的 seed 或 `MoveJoint` 的基准
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 层返回数组字段键名未恢复
|
||||
|
||||
### `GetPose`
|
||||
|
||||
- C++ 公开签名:`bool GetPose(Pose &pose);`
|
||||
- Python 包装形态:`res, pose = c.GetPose()`
|
||||
- 服务端归属:`ControllerServer._GetPose`
|
||||
- 协议 / 命令线索:命令名 `GetPose` 已坐实;字符串中可见 `GetPose success/failed`
|
||||
- 返回值与默认值:C++ 为 `bool + out Pose`;Python 为 `(bool, Pose)`
|
||||
- 典型工作流位置:读取当前 TCP 位姿,常与 `GetNearestIK` 配套使用
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:位姿坐标系定义虽然在 `Types.h` 中体现为 7 元数组,但 JSON 层字段结构未恢复
|
||||
|
||||
### `GetNearestIK`
|
||||
|
||||
- C++ 公开签名:`bool GetNearestIK(const Pose &pose, const JointPositions &seed, JointPositions &ik);`
|
||||
- Python 包装形态:`res, ik = c.GetNearestIK(pose, joint_seed=joints)`
|
||||
- 服务端归属:`ControllerServer._GetNearestIK`
|
||||
- 协议 / 命令线索:命令名 `GetNearestIK` 已坐实;字符串中可见 `GetNearestIK success/failed`
|
||||
- 返回值与默认值:C++ 为 `bool + out JointPositions`;Python 为 `(bool, JointPositions)`
|
||||
- 典型工作流位置:先 `GetPose` 得到当前位姿,再构造目标位姿,并使用当前关节或邻近关节作为 seed 求最近 IK
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 层中 seed 参数键名在 Python 里表现为 `joint_seed`,但 C++ 注释写的是 `seed`;高层字段命名仍待确认
|
||||
|
||||
### `MoveJoint`
|
||||
|
||||
- C++ 公开签名:`bool MoveJoint(const JointPositions &joint_position);`
|
||||
- Python 包装形态:`c.MoveJoint(home_joint) -> bool`
|
||||
- 服务端归属:`ControllerServer._MoveJoint`
|
||||
- 协议 / 命令线索:命令名 `MoveJoint` 已坐实;字符串中可见 `MoveJoint waypoint success/failed`
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:点到点回零或移动到飞拍轨迹起点
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:移动速度/平滑参数是否固定在服务端内部,公开 API 未暴露
|
||||
|
||||
### `ExecuteTrajectory`
|
||||
|
||||
- C++ 公开签名:`bool ExecuteTrajectory(const std::vector<JointPositions> &waypoints, const std::string &method = "icsp", bool save_traj = false);`
|
||||
- Python 包装形态:`c.ExecuteTrajectory(waypoints=[...], method="icsp", save_traj=True) -> bool`
|
||||
- 服务端归属:`ControllerServer._ExecuteTrajectory`
|
||||
- 协议 / 命令线索:命令名 `ExecuteTrajectory` 已坐实;`method` 至少支持 `icsp` 与 `doubles`
|
||||
- 返回值与默认值:默认 `method="icsp"`、`save_traj=false`
|
||||
- 典型工作流位置:普通多点轨迹执行,不带飞拍 IO;输入是关节空间稀疏 waypoint,而不是笛卡尔点
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 中 waypoint 列表的键名和序列化结构未恢复;`doubles` 的失败语义未在公开样例中体现
|
||||
|
||||
## 7. 飞拍轨迹
|
||||
|
||||
### `UploadFlyShotTraj`
|
||||
|
||||
- C++ 公开签名:`bool UploadFlyShotTraj(const std::string &name, const std::vector<JointPositions> &waypoints, const std::vector<bool> &shot_flags, const std::vector<int> &offset_values, const std::vector<std::vector<int>> &addrs);`
|
||||
- Python 包装形态:`c.UploadFlyShotTraj(name="test_traj", waypoints=..., shot_flags=..., offset_values=..., addrs=...) -> bool`
|
||||
- 服务端归属:`ControllerServer._UploadFlyShotTraj`
|
||||
- 协议 / 命令线索:命令名 `UploadFlyShotTraj` 已坐实;客户端字符串中可见 `StartUploadFlyShotTraj` 与 `EndUploadFlyShotTraj`,说明上传阶段很可能分为开始 / 传输 / 结束三个子步骤
|
||||
- 返回值与默认值:成功返回 `true`;无默认参数
|
||||
- 典型工作流位置:运行时动态构造飞拍轨迹时,先把轨迹定义登记到服务端
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、`libControllerClient.so`、`ControllerServer` 字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 上传是否一次性发送完整 payload,还是像字符串暗示的那样分片上传,当前未完全恢复
|
||||
|
||||
### `DeleteFlyShotTraj`
|
||||
|
||||
- C++ 公开签名:`bool DeleteFlyShotTraj(const std::string &name);`
|
||||
- Python 包装形态:`c.DeleteFlyShotTraj(name="test_traj") -> bool`
|
||||
- 服务端归属:`ControllerServer._DeleteFlyShotTraj`
|
||||
- 协议 / 命令线索:命令名 `DeleteFlyShotTraj` 已坐实;字符串中可见 `DeleteFlyShotTraj {} success/failed`
|
||||
- 返回值与默认值:成功返回 `true`
|
||||
- 典型工作流位置:删除已上传的临时飞拍轨迹定义
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`PyControllerClient` 字符串、`ControllerServer` 字符串
|
||||
- 置信度:高
|
||||
- 待确认点:删除配置内固有轨迹名与删除运行时上传轨迹时是否走同一条路径,当前未恢复
|
||||
|
||||
### `ListFlyShotTraj`
|
||||
|
||||
- C++ 公开签名:`std::vector<std::string> ListFlyShotTraj();`
|
||||
- Python 包装形态:`c.ListFlyShotTraj() -> list[str]`
|
||||
- 服务端归属:`ControllerServer._ListFlyShotTraj`
|
||||
- 协议 / 命令线索:命令名 `ListFlyShotTraj` 已坐实;客户端与服务端字符串都出现 `GetNextListFlyShotTraj`,说明底层列举很可能是迭代式获取,而不是一次性返回整个数组
|
||||
- 返回值与默认值:直接返回轨迹名称列表
|
||||
- 典型工作流位置:查看当前服务端已登记的飞拍轨迹
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`libControllerClient.so`、`ControllerServer` 字符串
|
||||
- 置信度:高
|
||||
- 待确认点:高层 JSON 是否真的直接返回数组,还是客户端内部循环拉取后再拼成数组,当前未完全恢复
|
||||
|
||||
### `ExecuteFlyShotTraj`
|
||||
|
||||
- C++ 公开签名:`bool ExecuteFlyShotTraj(const std::string &name, const bool move_to_start = false, const std::string &method = "icsp", bool save_traj = false, bool use_cache = false);`
|
||||
- Python 包装形态:`c.ExecuteFlyShotTraj(name="002", move_to_start=True, method="icsp", save_traj=True, use_cache=False) -> bool`
|
||||
- 服务端归属:`ControllerServer._ExecuteFlyShotTraj`
|
||||
- 协议 / 命令线索:命令名 `ExecuteFlyShotTraj` 已坐实;字符串中出现 `move_to_start`、`use_cache`;伪代码级逆向已恢复其主流程
|
||||
- 返回值与默认值:默认 `move_to_start=false`、`method="icsp"`、`save_traj=false`、`use_cache=false`
|
||||
- 典型工作流位置:对已存在的飞拍轨迹定义执行“生成 + 挂接 DO 时间轴 + 执行”
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:JSON 包结构未恢复;`use_cache` 的缓存键是否只按 `name`,当前从伪代码判断是,但未见更底层证据
|
||||
|
||||
### `SaveTrajInfo`
|
||||
|
||||
- C++ 公开签名:`bool SaveTrajInfo(const std::string &name, const std::string &method = "icsp");`
|
||||
- Python 包装形态:未见运行样例,但包装库和服务端都有明确字符串;语义可由手册和分析文档确认
|
||||
- 服务端归属:`ControllerServer._SaveTrajInfo`
|
||||
- 协议 / 命令线索:命令名 `SaveTrajInfo` 已坐实;字符串中可见 `SaveTrajInfo {} success/failed`
|
||||
- 返回值与默认值:默认 `method="icsp"`;`self-adapt-icsp` 在执行时保存分析文件会回落成 `icsp` 导出语义
|
||||
- 典型工作流位置:按给定 `name + method` 生成并导出轨迹分析文件,例如 `JointTraj.txt`、`CartDetialTraj.txt`
|
||||
- 证据来源:`ControllerClient.h`、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:Python 包装是否直接暴露该方法并返回 `bool`,虽然字符串存在,但公开示例未调用
|
||||
|
||||
### `IsFlyShotTrajValid`
|
||||
|
||||
- C++ 公开签名:`bool IsFlyShotTrajValid(double &time, const std::string &name, const std::string &method = "icsp", bool save_traj = false);`
|
||||
- Python 包装形态:`valid, time_sec = c.IsFlyShotTrajValid("EOL9_EAU_90", "icsp", save_traj=True)`
|
||||
- 服务端归属:`ControllerServer._IsFlyShotTrajValid`
|
||||
- 协议 / 命令线索:命令名 `IsFlyShotTrajValid` 已坐实;作用是“生成 + 合法性检查 + 返回轨迹总时长”
|
||||
- 返回值与默认值:默认 `method="icsp"`、`save_traj=false`;C++ 为 `bool + out double`;Python 为 `(bool, float)`
|
||||
- 典型工作流位置:在执行飞拍轨迹前预校验;也可与 `save_traj=True` 结合导出分析文件
|
||||
- 证据来源:`ControllerClient.h`、`UseControllerClient.py` 中的注释样例、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度:高
|
||||
- 待确认点:`method="self-adapt-icsp"` 是否被该接口完整支持,分析文档显示公开手册更强调 `icsp` / `doubles`,而示例注释里又出现 `self-adapt-icsp`,这里仍需后续抓包或真环境验证
|
||||
|
||||
## 8. 四条旧 SDK 工作流
|
||||
|
||||
### 8.1 初始化工作流
|
||||
|
||||
```text
|
||||
ControllerClient()
|
||||
-> ConnectServer("127.0.0.1", 50001)
|
||||
-> SetUpRobot("FANUC_LR_Mate_200iD") 或 SetUpRobotFromEnv(...)
|
||||
-> IsSetUp()
|
||||
-> GetName()
|
||||
-> GetDoF()
|
||||
-> SetShowTCP(...)
|
||||
```
|
||||
|
||||
说明:
|
||||
|
||||
- 这是最常见的仿真或真机前置流程。
|
||||
- `SetUpRobot` 是旧说明里明确要求首先执行的服务端初始化动作。
|
||||
|
||||
### 8.2 控制器状态工作流
|
||||
|
||||
```text
|
||||
SetActiveController(sim=True/False)
|
||||
-> Connect(robot_ip)
|
||||
-> EnableRobot(buffer_size=2)
|
||||
-> GetSpeedRatio() / SetSpeedRatio(...)
|
||||
-> GetTCP() / SetTCP(...)
|
||||
-> GetIO() / SetIO(...)
|
||||
-> StopMove()
|
||||
-> DisableRobot()
|
||||
-> Disconnect()
|
||||
```
|
||||
|
||||
说明:
|
||||
|
||||
- 真机样例会使用 `sim=False`。
|
||||
- `StopMove()` 之后,Python 示例里会再次 `EnableRobot()`,说明停止运动后常伴随重新使能。
|
||||
|
||||
### 8.3 普通轨迹工作流
|
||||
|
||||
```text
|
||||
GetJointPosition()
|
||||
-> GetPose()
|
||||
-> GetNearestIK(pose, seed)
|
||||
-> MoveJoint(home_joint)
|
||||
-> ExecuteTrajectory(waypoints, method="icsp", save_traj=True)
|
||||
```
|
||||
|
||||
说明:
|
||||
|
||||
- 输入是关节空间稀疏 waypoint。
|
||||
- `ExecuteTrajectory` 至少支持 `icsp` 与 `doubles`。
|
||||
|
||||
### 8.4 飞拍轨迹工作流
|
||||
|
||||
#### 方案 A:轨迹名已在配置中存在
|
||||
|
||||
```text
|
||||
name
|
||||
-> IsFlyShotTrajValid(name, "icsp", save_traj=True)
|
||||
-> ExecuteFlyShotTraj(name, move_to_start=True, method="icsp", save_traj=True, use_cache=False)
|
||||
-> SaveTrajInfo(name, "icsp")
|
||||
```
|
||||
|
||||
#### 方案 B:客户端动态上传飞拍轨迹
|
||||
|
||||
```text
|
||||
UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)
|
||||
-> ListFlyShotTraj()
|
||||
-> IsFlyShotTrajValid(name, "icsp", save_traj=True)
|
||||
-> ExecuteFlyShotTraj(name, move_to_start=True, method="self-adapt-icsp", save_traj=True, use_cache=True)
|
||||
-> DeleteFlyShotTraj(name)
|
||||
```
|
||||
|
||||
说明:
|
||||
|
||||
- `UploadFlyShotTraj` 只负责登记轨迹定义。
|
||||
- `IsFlyShotTrajValid` 负责“生成 + 合法性检查”。
|
||||
- `ExecuteFlyShotTraj` 负责“生成 + 挂接 `TrajectoryDO` + 执行”。
|
||||
- `SaveTrajInfo` 负责导出分析文件。
|
||||
|
||||
## 9. 已知高置信协议线索
|
||||
|
||||
当前能直接用于后续实现的高置信协议结论如下:
|
||||
|
||||
- 传输层为 `TCP + JSON` 风格协议,不是 HTTP/gRPC。
|
||||
- JSON 层至少存在 `method` 字段,服务端存在 `_ClientCB` 与 `_IsJsonValid`。
|
||||
- 命令通道字段已确认:
|
||||
- `GetSpeedRatio`:`MsgID = 0x2206`
|
||||
- `SetSpeedRatio`:`MsgID = 0x2207`
|
||||
- `GetIO`:`MsgID = 0x2208`
|
||||
- `SetIO`:`MsgID = 0x2209`
|
||||
- 2026-04-28 `UTTC_MS11` 抓包中,`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来,但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`;兼容实现不能只依赖一次 10012 命令来表达执行倍率,还要在 J519 发送时间轴上应用当前倍率。J519 实发时间为 `t_send = k * 0.008`,原轨迹采样时间为 `t_traj = t_send * speed_ratio`,包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- 飞拍轨迹相关额外字符串线索:
|
||||
- `StartUploadFlyShotTraj`
|
||||
- `EndUploadFlyShotTraj`
|
||||
- `GetNextListFlyShotTraj`
|
||||
- `move_to_start`
|
||||
- `use_cache`
|
||||
- `shot_flags`
|
||||
- `offset_values`
|
||||
- `addr` / `addrs`
|
||||
|
||||
## 10. 待确认问题
|
||||
|
||||
以下问题本轮故意保留,不冒充已确认结论:
|
||||
|
||||
1. `50001/TCP+JSON` 请求 JSON 的精确 envelope 结构、字段必填规则、响应包统一格式。
|
||||
2. `GetServerVersion` 在高层 JSON 中的完整请求 / 响应字段。
|
||||
3. `GetClientVersion` 的实际版本字符串来源,以及 Python 包装失败路径。
|
||||
4. `ListFlyShotTraj` 是高层一次性返回数组,还是客户端内部循环 `GetNextListFlyShotTraj` 后再拼装列表。
|
||||
5. `UploadFlyShotTraj` 是否采用开始 / 数据 / 结束的多阶段上传协议。
|
||||
6. `IsFlyShotTrajValid` 对 `self-adapt-icsp` 的真实支持边界。
|
||||
7. `SetTCP` / `GetTCP` 在高层 JSON 中是否暴露 `tcp_id` 概念。
|
||||
8. `SetActiveController` 切换控制器时是否会隐式触发 `_DisconnectAll`。
|
||||
|
||||
## 11. 后续实现使用方式
|
||||
|
||||
等继续扩展 `Flyshot.ControllerClientCompat` 时,建议按以下顺序使用本文档:
|
||||
|
||||
1. 先把 32 个 API 按本文档拆成命令表。
|
||||
2. 先实现高置信、状态简单的接口:
|
||||
- `GetServerVersion`
|
||||
- `SetUpRobot`
|
||||
- `IsSetUp`
|
||||
- `GetName`
|
||||
- `GetDoF`
|
||||
- `GetSpeedRatio`
|
||||
- `SetSpeedRatio`
|
||||
- `GetIO`
|
||||
- `SetIO`
|
||||
3. 再实现返回复杂结构的接口:
|
||||
- `GetTCP`
|
||||
- `GetJointPosition`
|
||||
- `GetPose`
|
||||
- `GetNearestIK`
|
||||
4. 最后实现飞拍轨迹相关接口,并把本文档中的“待确认问题”逐项收敛成兼容测试。
|
||||
165
docs/fanuc-10010-state-frame.md
Normal file
165
docs/fanuc-10010-state-frame.md
Normal file
@@ -0,0 +1,165 @@
|
||||
# FANUC TCP 10010 状态帧字段说明
|
||||
|
||||
本文档整理当前现场真实抓包中 `TCP 10010` 状态通道的已确认字段布局,并明确哪些结论已经由抓包和代码验证,哪些仍然只是工作假设。
|
||||
|
||||
读取时间:2026-05-03
|
||||
|
||||
## 1. 结论范围
|
||||
|
||||
本文基于以下证据整理:
|
||||
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `Rvbust/uttc-20260428/20260428.pcap`
|
||||
- `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs`
|
||||
- `tests/Flyshot.Core.Tests/FanucProtocolTests.cs`
|
||||
|
||||
当前结论仅覆盖现场已确认的 `R30iB + RVBUSTSM` 这一路状态通道行为,不提前推广为所有 FANUC 机型或所有旧版本协议的通用结论。
|
||||
|
||||
## 2. 通道性质
|
||||
|
||||
真实抓包显示,`TCP 10010` 是控制柜到上位机的单向状态流:
|
||||
|
||||
- 上位机先主动建立 TCP 连接。
|
||||
- 建连后,带应用层 payload 的业务包全部来自 `192.168.10.11:10010 -> 192.168.10.10:41726`。
|
||||
- 上位机在该通道上只回 TCP `ACK`,没有观察到应用层请求体。
|
||||
|
||||
因此当前实现应把 `10010` 当作“持续推送的固定长度状态帧”处理,而不是像 `TCP 10012` 那样按请求/响应语义建模。
|
||||
|
||||
## 3. 整体布局
|
||||
|
||||
当前现场抓包确认,状态帧固定为 `90B`:
|
||||
|
||||
```text
|
||||
doz 3 bytes
|
||||
length u32 = 90
|
||||
msg_id u32
|
||||
pose[6] f32
|
||||
joint_or_ext[9] f32
|
||||
tail[4] u32
|
||||
zod 3 bytes
|
||||
```
|
||||
|
||||
其中:
|
||||
|
||||
- 帧头 magic 固定为 `doz`
|
||||
- 帧尾 magic 固定为 `zod`
|
||||
- 长度字段固定为 `0x5a`,即 `90`
|
||||
- 当前全量抓包中 `msg_id` 恒为 `0`
|
||||
- `tail[4]` 当前全量抓包中恒为 `(2, 0, 0, 1)`
|
||||
|
||||
## 4. 样例帧
|
||||
|
||||
以下样例帧来自 `20260428.pcap` 中首条 `tcp.port == 10010 && tcp.len > 0` 的 payload:
|
||||
|
||||
```text
|
||||
646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64
|
||||
```
|
||||
|
||||
对应解析值:
|
||||
|
||||
- `pose[6]`
|
||||
- `273.26715`
|
||||
- `483.85544`
|
||||
- `467.73764`
|
||||
- `-57.253044`
|
||||
- `-89.618675`
|
||||
- `-62.21883`
|
||||
- `joint_or_ext[9]`
|
||||
- `1.0567309`
|
||||
- `0.011662138`
|
||||
- `-0.01789339`
|
||||
- `-0.015160045`
|
||||
- `0.02149596`
|
||||
- `0.009560025`
|
||||
- `0`
|
||||
- `0`
|
||||
- `0`
|
||||
- `tail[4]`
|
||||
- `2`
|
||||
- `0`
|
||||
- `0`
|
||||
- `1`
|
||||
|
||||
## 5. 正式字段表
|
||||
|
||||
| 偏移 | 长度 | 类型 | 样例值(hex) | 样例值(解析后) | 当前推断含义 |
|
||||
| --- | --- | --- | --- | --- | --- |
|
||||
| `0x00` | `3` | `char[3]` | `64 6f 7a` | `"doz"` | 固定帧头 magic |
|
||||
| `0x03` | `4` | `u32 be` | `00 00 00 5a` | `90` | 帧总长度 |
|
||||
| `0x07` | `4` | `u32 be` | `00 00 00 00` | `0` | `msg_id`,当前抓包全为 `0` |
|
||||
| `0x0B` | `4` | `f32 be` | `43 88 a2 32` | `273.26715` | `pose[0]`,推断为 TCP `X(mm)` |
|
||||
| `0x0F` | `4` | `f32 be` | `43 f1 ed 7f` | `483.85544` | `pose[1]`,推断为 TCP `Y(mm)` |
|
||||
| `0x13` | `4` | `f32 be` | `43 e9 de 6b` | `467.73764` | `pose[2]`,推断为 TCP `Z(mm)` |
|
||||
| `0x17` | `4` | `f32 be` | `c2 65 03 1e` | `-57.253044` | `pose[3]`,推断为姿态角 `W(deg)` |
|
||||
| `0x1B` | `4` | `f32 be` | `c2 b3 3c c3` | `-89.618675` | `pose[4]`,推断为姿态角 `P(deg)` |
|
||||
| `0x1F` | `4` | `f32 be` | `c2 78 e0 15` | `-62.21883` | `pose[5]`,推断为姿态角 `R(deg)` |
|
||||
| `0x23` | `4` | `f32 be` | `3f 87 42 f5` | `1.0567309` | `joint_or_ext[0]`,推断为 `J1(rad)` |
|
||||
| `0x27` | `4` | `f32 be` | `3c 3f 12 8d` | `0.011662138` | `joint_or_ext[1]`,推断为 `J2(rad)` |
|
||||
| `0x2B` | `4` | `f32 be` | `bc 92 95 29` | `-0.01789339` | `joint_or_ext[2]`,推断为 `J3(rad)` |
|
||||
| `0x2F` | `4` | `f32 be` | `bc 78 61 d6` | `-0.015160045` | `joint_or_ext[3]`,推断为 `J4(rad)` |
|
||||
| `0x33` | `4` | `f32 be` | `3c b0 18 4c` | `0.02149596` | `joint_or_ext[4]`,推断为 `J5(rad)` |
|
||||
| `0x37` | `4` | `f32 be` | `3c 1c a1 a7` | `0.009560025` | `joint_or_ext[5]`,推断为 `J6(rad)` |
|
||||
| `0x3B` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[6]`,扩展轴槽位,当前样本恒 `0` |
|
||||
| `0x3F` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[7]`,扩展轴槽位,当前样本恒 `0` |
|
||||
| `0x43` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[8]`,扩展轴槽位,当前样本恒 `0` |
|
||||
| `0x47` | `4` | `u32 be` | `00 00 00 02` | `2` | `tail[0]`,诊断状态字,物理语义未坐实 |
|
||||
| `0x4B` | `4` | `u32 be` | `00 00 00 00` | `0` | `tail[1]`,诊断状态字,物理语义未坐实 |
|
||||
| `0x4F` | `4` | `u32 be` | `00 00 00 00` | `0` | `tail[2]`,诊断状态字,物理语义未坐实 |
|
||||
| `0x53` | `4` | `u32 be` | `00 00 00 01` | `1` | `tail[3]`,诊断状态字,物理语义未坐实 |
|
||||
| `0x57` | `3` | `char[3]` | `7a 6f 64` | `"zod"` | 固定帧尾 magic |
|
||||
|
||||
## 6. 已确认结论
|
||||
|
||||
### 6.1 已由真实抓包确认
|
||||
|
||||
1. `TCP 10010` 是独立状态流,不是 `TCP 10012` 的请求/响应复用。
|
||||
2. 当前现场状态帧固定为 `90B`,不是早期静态分析里出现过的 `134B`。
|
||||
3. `msg_id` 在 `20260428.pcap` 当前全量样本中恒为 `0`。
|
||||
4. `tail[4]` 在 `20260428.pcap` 当前全量样本中恒为 `(2, 0, 0, 1)`。
|
||||
5. `pose[6]` 的量纲表现符合 `X/Y/Z(mm) + W/P/R(deg)`。
|
||||
6. `joint_or_ext[6..8]` 在当前现场样本中恒为 `0`。
|
||||
|
||||
### 6.2 已由数值范围和交叉对照强支持
|
||||
|
||||
1. `joint_or_ext[0..5]` 更像关节角 `rad`,而不是 `deg`。
|
||||
2. 该判断与 `../analysis/UTTC_20260428_packet_validation.md` 的结论一致。
|
||||
3. 该判断也与 `UDP 60015` 响应包中的关节 `deg` 形成互补关系:二者不能简单视作同单位直接复用。
|
||||
|
||||
## 7. 待确认项
|
||||
|
||||
以下内容当前不要写死为最终协议真义:
|
||||
|
||||
1. `tail[4]` 四个 `u32` 分别代表什么控制器语义。
|
||||
2. `msg_id` 是否在其他控制柜版本、程序状态或异常态下会出现非零值。
|
||||
3. `pose[3..5]` 是否可以严格命名为 FANUC 标准 `W/P/R`,还是只是与其数值表现一致。
|
||||
4. `joint_or_ext[6..8]` 在带外部轴的现场是否仍复用同一布局。
|
||||
|
||||
## 8. 与当前代码实现的对齐情况
|
||||
|
||||
当前仓库里 `Flyshot.Runtime.Fanuc` 已按 `90B` 固定帧解析:
|
||||
|
||||
- `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs`
|
||||
- `tests/Flyshot.Core.Tests/FanucProtocolTests.cs`
|
||||
|
||||
当前实现已经与抓包对齐的部分:
|
||||
|
||||
1. 固定长度 `90B`
|
||||
2. `doz ... zod` 帧头帧尾校验
|
||||
3. `pose[6] + joint_or_ext[9] + tail[4]` 的字节布局
|
||||
4. `tail[4]` 原样保留到 `ControllerStateSnapshot.StateTailWords`
|
||||
|
||||
当前仍建议后续关注的点:
|
||||
|
||||
1. `FanucStateFrame` 已把该字段从 `JointDegrees` 更正为 `JointRadians`,后续新增代码应继续沿用弧度制命名。
|
||||
2. 如果后续状态页或运行时逻辑需要直接展示该通道关节值,仍需明确标注这是 `10010` 的弧度值,避免和 `UDP 60015` 的 degree 语义混淆。
|
||||
|
||||
## 9. 建议用法
|
||||
|
||||
在当前 replacement 实现里,`TCP 10010` 更适合作为以下用途:
|
||||
|
||||
1. 提供机器人当前笛卡尔位姿和关节反馈快照。
|
||||
2. 提供状态通道是否健康、是否陈旧的连接诊断依据。
|
||||
3. 保留 `tail[4]` 原始状态字,供现场排错或后续继续逆向。
|
||||
|
||||
当前不建议直接用 `tail[4]` 去驱动明确业务判断,除非后续拿到新的现场对照证据。
|
||||
182
docs/fanuc-field-runtime-workflow.md
Normal file
182
docs/fanuc-field-runtime-workflow.md
Normal file
@@ -0,0 +1,182 @@
|
||||
# FANUC Field Runtime Workflow
|
||||
|
||||
本文档记录当前现场主链路的 HTTP 调用顺序,以及每一步在 FANUC 三条真机通道上的动作。它替代旧 `ControllerClient` 工作流说明;旧 `50001/TCP+JSON` 入口不再作为运行目标。
|
||||
|
||||
## 1. 初始化
|
||||
|
||||
推荐使用聚合端点完成当前现场的一次性初始化:
|
||||
|
||||
```bash
|
||||
POST /init_mpc_robt
|
||||
{
|
||||
"server_ip": "127.0.0.1",
|
||||
"port": 50001,
|
||||
"robot_name": "FANUC_LR_Mate_200iD",
|
||||
"robot_ip": "192.168.10.11",
|
||||
"sim": false
|
||||
}
|
||||
```
|
||||
|
||||
该端点内部顺序:
|
||||
|
||||
1. `ConnectServer(server_ip, port)`:兼容旧参数形状,仅记录服务连接语义。
|
||||
2. `SetUpRobot(robot_name)`:加载机器人配置、关节限制和伺服周期。
|
||||
3. `SetActiveController(sim)`:选择仿真或 FANUC 真机运行时。
|
||||
4. `Connect(robot_ip)`:真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
|
||||
5. `EnableRobot(2)`:真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`,随后允许 J519 在收到机器人 UDP status 包后回发下一帧命令。
|
||||
|
||||
也可以使用拆分端点按同样顺序调用:
|
||||
|
||||
```text
|
||||
POST /connect_server/?server_ip=127.0.0.1&port=50001
|
||||
POST /setup_robot/?robot_name=FANUC_LR_Mate_200iD
|
||||
POST /set_active_controller/?sim=false
|
||||
POST /connect_robot/?ip=192.168.10.11
|
||||
GET /enable_robot/?buffer_size=2
|
||||
```
|
||||
|
||||
## 2. 参数设置
|
||||
|
||||
规划约束参数:
|
||||
|
||||
当前现场抓包已经确认,`50001/TCP+JSON` 的 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求不会显式携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此新系统把规划约束分成两层处理:
|
||||
|
||||
1. 旧 `RobotConfig.json` 中已有的 `acc_limit / jerk_limit` 继续作为模型加载时的基础倍率。
|
||||
2. 若旧系统导出的 `JointTraj.txt` 明显比当前 C# 规划更慢,使用 replacement-only 的内部校准参数限制规划阶段加速度,设计字段为 `planning_acceleration_scale`。
|
||||
|
||||
`planning_acceleration_scale` 只影响 `JointTraj.txt` 这类规划结果时间轴,不下发到 FANUC 控制柜,也不改变 J519 发送周期。若需要临时整体验证,也可以使用当前已有的 `planning_speed_scale`,但它是新系统兼容开关,不是旧抓包中出现的字段。
|
||||
|
||||
速度倍率:
|
||||
|
||||
```bash
|
||||
POST /set_speedRatio/
|
||||
{ "speed": 0.7 }
|
||||
```
|
||||
|
||||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`,同时运行时保存当前倍率。`speed_ratio` 是执行期倍率,不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍按机器人 8ms 节拍更新目标,`speed_ratio` 只缩放原轨迹采样时间:
|
||||
|
||||
```text
|
||||
t_send = k * 0.008
|
||||
t_traj = t_send * speed_ratio
|
||||
send_count = floor(duration / (0.008 * speed_ratio)) + 1
|
||||
```
|
||||
|
||||
TCP 和普通 IO:
|
||||
|
||||
```text
|
||||
POST /set_tcp/ body: { "x": 0, "y": 0, "z": 0 }
|
||||
GET /get_tcp/
|
||||
POST /set_io/?port=7&value=true&io_type=DO
|
||||
GET /get_io/?port=7&io_type=DO
|
||||
```
|
||||
|
||||
飞拍触发 IO 不走独立 `TCP 10012 SetIO`,而是嵌入 `UDP 60015` J519 命令包的 `write_io_type/index/mask/value` 字段。
|
||||
|
||||
## 3. 点到点 MoveJoint
|
||||
|
||||
```bash
|
||||
POST /move_joint/
|
||||
{ "joints": [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145] }
|
||||
```
|
||||
|
||||
`MoveJoint` 不再直接把最终关节写成单个 J519 目标,而是按现场抓包确认的 PTP 临时轨迹执行:
|
||||
|
||||
1. 从当前运行时状态读取当前关节坐标,单位为 `rad`。
|
||||
2. 以当前关节和目标关节构造关节空间直线。
|
||||
3. 用五次 smoothstep `10u^3 - 15u^4 + 6u^5` 生成起停平滑的进度。
|
||||
4. 真机执行时仍由 J519 层把 `rad` 转成 `deg`,并按当前 `speed_ratio` 重采样。
|
||||
|
||||
已确认抓包按响应 `status=15` 运动窗口统计:
|
||||
|
||||
| 抓包 | speed_ratio | 运动窗口点数 | 运动窗口时长 |
|
||||
|------|-------------|----------------------|----------|
|
||||
| `2026042802-mvpoint.pcap` | 1.0 | 40 | 约 0.312s |
|
||||
| `2026042802-mvpoint0.7.pcap` | 0.7 | 55 | 约 0.432s |
|
||||
| `2026042802-mvpoint0.5.pcap` | 0.5 | 77 | 约 0.608s |
|
||||
|
||||
抓包命令流在运动窗口前后还会持续发送保持不变的起点/终点目标;功能复刻以 `status=15` 运动窗口为点数口径,并把最后一个采样点压到目标关节。实际目标几乎严格位于“起点 -> 终点”的同一条关节空间直线上,`speed_ratio` 体现为 J519 发送时间轴上的减速重采样,而不是改变路径形状。
|
||||
|
||||
## 4. 飞拍轨迹
|
||||
|
||||
上传:
|
||||
|
||||
```bash
|
||||
POST /upload_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"waypoints": [[...]],
|
||||
"shot_flags": [false, true],
|
||||
"offset_values": [0, 0],
|
||||
"addrs": [[1, 3]]
|
||||
}
|
||||
```
|
||||
|
||||
校验:
|
||||
|
||||
```bash
|
||||
POST /is_flyShotTrajValid/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false
|
||||
}
|
||||
```
|
||||
|
||||
执行:
|
||||
|
||||
```bash
|
||||
POST /execute_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"move_to_start": true,
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false,
|
||||
"use_cache": true,
|
||||
"wait": true
|
||||
}
|
||||
```
|
||||
|
||||
执行链路:
|
||||
|
||||
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
||||
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹;规划阶段先应用 `acc_limit / jerk_limit`,再应用 replacement-only 的规划加速度校准参数。
|
||||
3. 生成 `TrajectoryDoEvent`,把拍照触发绑定到轨迹时间。
|
||||
4. 若 `move_to_start=true`,先从运行时当前关节位置生成临时 PTP 稠密轨迹移动到规划轨迹起点,并等待运行时 `IsInMotion=false` 后再启动飞拍轨迹,避免第一帧 J519 目标从当前位置跳到起点。
|
||||
5. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 轨迹时间步长重采样并转成 `deg`,命令实际发包由机器人 UDP status 包驱动。
|
||||
6. 若 `wait=true`,正式飞拍轨迹启动后继续等待运行时 `IsInMotion=false`,机器人执行完整条飞拍轨迹后 HTTP 才返回;`wait=false` 时启动后立即返回。
|
||||
7. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪,则拒绝执行。
|
||||
8. 周期命令中嵌入 IO 脉冲;当前 UTTC 抓包确认 mask 集合为 `10/12/14`,共 17 个 set 脉冲和 17 个 clear 帧。
|
||||
|
||||
`method="doubles"` 当前明确返回未实现;现场主链路使用 `icsp` / `self-adapt-icsp`。
|
||||
|
||||
## 5. 停止与断开
|
||||
|
||||
```text
|
||||
GET /stop_move/
|
||||
GET /disable_robot/
|
||||
POST /disconnect_robot/
|
||||
```
|
||||
|
||||
真机模式下:
|
||||
|
||||
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 状态包驱动发送。
|
||||
- `DisableRobot()` 发送 J519 packet type 2 状态输出停止包,然后 `StopProg("RVBUSTSM")`。
|
||||
- `Disconnect()` 关闭状态、命令和 J519 三条通道,并清理本地运行状态。
|
||||
|
||||
## 6. 现场抓包覆盖
|
||||
|
||||
`tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs` 直接解析以下抓包并与 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 对比:
|
||||
|
||||
| 抓包 | 速度 | 运行 J519 点数 | 发送时长 |
|
||||
|------|------|----------------|----------|
|
||||
| `2026042802-0.5.pcap` | 0.5 | 1851 | 14.800309s |
|
||||
| `2026042802-0.7.pcap` | 0.7 | 1322 | 10.568313s |
|
||||
| `2026042802-1.pcap` | 1.0 | 926 | 7.400125s |
|
||||
|
||||
测试同时检查:
|
||||
|
||||
- 主运行窗口命令序号连续,无重复 seq;J519 客户端单元测试覆盖按最新 status sequence 回发命令。
|
||||
- 响应 `status=15` 段覆盖主运行窗口,响应相对命令滞后 2 到 8 帧。
|
||||
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`,最大绝对误差小于 `0.07deg`。
|
||||
- `lastData=0`,结束运动当前依赖 J519 packet type 2 状态输出停止包;`../j519 协议.pcap` 中另有 1 个 `LastData=1` 后紧跟 type 2 的样本,停止语义后续单独验证。
|
||||
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
|
||||
187
docs/fanuc-socket-implementation-plan.md
Normal file
187
docs/fanuc-socket-implementation-plan.md
Normal file
@@ -0,0 +1,187 @@
|
||||
# FANUC 真机协议 Socket 通信层实现计划
|
||||
|
||||
## 上下文
|
||||
|
||||
状态更新:本计划中的 Socket 客户端和 `FanucControllerRuntime` 改造已经落地;当前事实以 `README.md` 和 `docs/fanuc-field-runtime-workflow.md` 为准。本文保留为实现过程记录。
|
||||
|
||||
计划制定时 `flyshot-replacement` 项目已完成:
|
||||
- 三条 FANUC 通信链路的二进制协议编解码(`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`)
|
||||
- 抓包样本验证的协议测试(5 个 FanucProtocolTests 全部通过)
|
||||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||||
- HTTP 兼容层控制器和状态监控页
|
||||
- 轨迹规划与飞拍触发编排层
|
||||
|
||||
2026-04-28 `Rvbust/uttc-20260428/20260428.pcap` 新增约束:
|
||||
|
||||
- `TCP 10010` 状态帧继续确认为固定 `90B`。
|
||||
- `UDP 60015` 命令 `target[0..5]` 为关节角 `deg`,而 `JointDetialTraj.txt` 为 `rad`。
|
||||
- `speed_ratio=0.7` 在本抓包中表现为 UDP 下发时间轴约 `1.427730x` 拉伸;机器人侧 `TCP 10012` 未抓到 `0x2207 SetSpeedRatio`。
|
||||
- `UTTC_MS11` 的 17 个飞拍触发点与 17 个 UDP IO 脉冲一一对齐,`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
**历史缺失项(已完成)**:计划制定时 `FanucControllerRuntime` 仍是纯内存状态桩。当前实现已经改为持有 `FanucCommandClient`、`FanucStateClient` 和 `FanucJ519Client`,真机模式会建立三条通道并从状态/J519 响应读取运行状态。
|
||||
|
||||
## 目标
|
||||
|
||||
将 `FanucControllerRuntime` 从内存桩改造为具备真实 FANUC R30iB 通信能力的运行时,使 HTTP 层的每个指令真正下发到控制柜。
|
||||
|
||||
## 架构设计
|
||||
|
||||
### 分层结构
|
||||
|
||||
```
|
||||
LegacyHttpApiController / StatusController (HTTP 适配层,保持不动)
|
||||
↓ 调用同步接口
|
||||
IControllerRuntime / ControllerClientCompatService (兼容层,保持不动)
|
||||
↓ 调用同步接口
|
||||
FanucControllerRuntime (改造:从内存桩 → 委托给三个 Socket 客户端)
|
||||
↓ 内部持有并调度
|
||||
FanucCommandClient (TCP 10012,Req/Res 同步命令通道)
|
||||
FanucStateClient (TCP 10010,持续接收状态帧后台循环)
|
||||
FanucJ519Client (UDP 60015,8ms 周期发送 + 接收响应)
|
||||
↓ 使用现有编解码
|
||||
FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有,不改动)
|
||||
```
|
||||
|
||||
### 关键设计决策
|
||||
|
||||
1. **接口保持同步**:`IControllerRuntime` 现有 18 个方法全为同步签名。内部 Socket I/O 采用 `Task` + `.GetAwaiter().GetResult()` 短时间阻塞,或后台线程 + 锁同步状态快照。避免一次性推翻整个兼容层。
|
||||
|
||||
2. **三个独立客户端**:每条物理通道一个类,各自管理连接生命周期,便于单独测试和故障定位。
|
||||
|
||||
3. **状态通道后台循环**:`FanucStateClient` 内部启动 `Task` 持续 `ReadAsync(90)`,解析状态帧后写入线程安全的 `ControllerStateSnapshot` 缓存。
|
||||
|
||||
4. **J519 周期发送器**:`FanucJ519Client` 内部用 `PeriodicTimer` 或 `Task.Delay` 实现约 8ms 周期的发送循环。命令通过线程安全的队列/缓冲区注入。
|
||||
|
||||
5. **RVBUSTSM 程序生命周期隐式管理**:`EnableRobot()` 时自动走 `StopProg→Reset→GetProgStatus→StartProg("RVBUSTSM")` 序列(与抓包一致)。`DisableRobot()` 时发送 `StopProg`。
|
||||
|
||||
6. **连接顺序**:`Connect()` 按顺序建立三条通道 — 先 TCP 10010(状态),再 TCP 10012(命令),最后 UDP 60015(运动)。
|
||||
|
||||
## 实现步骤
|
||||
|
||||
### Phase 1: TCP 10012 命令客户端
|
||||
|
||||
**新建文件**:`src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs`
|
||||
|
||||
职责:
|
||||
- `Connect(string ip, int port = 10012)` — 建立 TcpClient 连接
|
||||
- `SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body)` — 发送并等待响应
|
||||
- `SendProgramCommandAsync(uint messageId, string programName)` — 封装程序名命令
|
||||
- `Disconnect()` — 关闭连接
|
||||
- 线程安全(单个命令通道同一时间只处理一个请求)
|
||||
|
||||
需要封装的具体命令方法:
|
||||
- `StopProgramAsync(string name)` → `PackProgramCommand(0x2103, name)`
|
||||
- `ResetRobotAsync()` → `PackEmptyCommand(0x2100)`
|
||||
- `GetProgramStatusAsync(string name)` → `PackProgramCommand(0x2003, name)`
|
||||
- `StartProgramAsync(string name)` → `PackProgramCommand(0x2102, name)`
|
||||
- `GetTcpAsync()` / `SetTcpAsync()` — 已按 `tcp_id + f32[7] pose` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现;注意 2026-04-28 真实运行抓包未出现机器人侧 `0x2207`,执行链路仍必须在 UDP 发送时间尺度上应用当前速度倍率
|
||||
- `GetIoAsync()` / `SetIoAsync()` — 已按 `io_type / io_index / f32 io_value` 字段布局实现
|
||||
|
||||
**测试**:`tests/Flyshot.Core.Tests/FanucCommandClientTests.cs`
|
||||
- 用 `TcpListener` 本地模拟控制器,验证帧收发与解析
|
||||
|
||||
### Phase 2: TCP 10010 状态客户端
|
||||
|
||||
**新建文件**:`src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs`
|
||||
|
||||
职责:
|
||||
- `Connect(string ip, int port = 10010)` — 建立 TcpClient 连接
|
||||
- 内部启动后台 `Task` 循环 `ReadAsync(FanucStateProtocol.StateFrameLength)`
|
||||
- 每收到一帧调用 `FanucStateProtocol.ParseFrame()`
|
||||
- 将解析结果写入线程安全的最新状态缓存
|
||||
- 单帧接收超时后标记状态陈旧,不再把旧帧当作当前位姿/关节状态使用
|
||||
- EOF、坏帧、Socket 异常或超时后关闭当前连接,并按退避策略自动重连 TCP 10010
|
||||
- `GetLatestFrame()` — 返回最近一次解析的状态帧
|
||||
- `GetStatus()` — 返回连接阶段、陈旧状态、最近异常和重连次数
|
||||
- `Disconnect()` — 取消后台循环并关闭连接
|
||||
|
||||
**测试**:`tests/Flyshot.Core.Tests/FanucStateClientTests.cs`
|
||||
- 用 `TcpListener` 本地发送抓包样本 hex,验证后台循环能正确解析。
|
||||
- 用本地模拟控制器验证无状态帧超时、EOF 后退避重连和重连后的继续收帧。
|
||||
- `FanucStateProtocol` 已用 `j519 协议.pcap` 中多条 90B 样本锁定 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]`。
|
||||
- `Rvbust/uttc-20260428/20260428.pcap` 再次确认 `10010` 状态帧固定 90B,平均间隔约 25.6ms。
|
||||
- 尾部状态字当前只作为 `ControllerStateSnapshot.stateTailWords` 诊断字段保留,不从 `[2,0,0,1]` 推断使能或运动状态。
|
||||
|
||||
### Phase 3: UDP 60015 J519 运动客户端
|
||||
|
||||
**新建文件**:`src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs`
|
||||
|
||||
职责:
|
||||
- `Connect(string ip, int port = 60015)` — 创建 UdpClient
|
||||
- 发送 init packet (`PackInitPacket()`)
|
||||
- 内部启动发送循环(约 8ms 周期)
|
||||
- `UpdateCommand(FanucJ519Command command)` — 原子更新下一周期要发送的命令
|
||||
- `StartMotion()` — 启动发送循环
|
||||
- `StopMotion()` — 发送 end packet,停止循环
|
||||
- 接收线程:持续 `ReceiveAsync()` 解析 132B 响应,更新反馈状态
|
||||
- `Disconnect()` — 清理
|
||||
|
||||
执行注意事项:
|
||||
|
||||
- 规划层输出关节角为 `rad`,J519 命令 `target[0..5]` 必须转为 `deg`。
|
||||
- 发送循环不能只按 `JointDetialTraj` 行号逐行发;需要保持约 8ms 的 J519 实发周期,同时按当前 `speed_ratio` 对原轨迹时间轴做缩放。
|
||||
- 实发规则:第 `k` 个 J519 周期的发送时间为 `t_send = k * 0.008`,轨迹采样时间为 `t_traj = t_send * speed_ratio`,命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`,因此主运行实发 `1322` 个运行包,而不是 `JointDetialTraj.txt` 的 `464` 行。
|
||||
- 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`,不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
|
||||
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后,闭环判断要容忍该延迟。
|
||||
|
||||
**测试**:`tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs`
|
||||
- 用本地 UDP socket 模拟控制器收发
|
||||
|
||||
### Phase 4: 重写 FanucControllerRuntime
|
||||
|
||||
**改造文件**:`src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||||
|
||||
将当前内存桩替换为真实运行时:
|
||||
|
||||
- 持有三个客户端实例:`FanucCommandClient`、`FanucStateClient`、`FanucJ519Client`
|
||||
- `Connect(robotIp)` — 顺序连接 10010 → 10012 → 60015
|
||||
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列(Stop→Reset→Status→Start RVBUSTSM),然后启动 J519
|
||||
- `DisableRobot()` — 停止 J519,发送 StopProg
|
||||
- `Disconnect()` — 断开三条通道
|
||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换,并按 `t_send = k * 0.008`、`t_traj = t_send * speed_ratio` 重采样后,通过 J519 逐周期发送
|
||||
- `StopMove()` — 立即停止 J519 发送循环
|
||||
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态;若状态通道未连接,回退到内存值
|
||||
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
||||
- `SetTcp()` / `SetSpeedRatio()` / `SetIo()` — 通过命令通道发送
|
||||
|
||||
### Phase 5: 端到端集成测试
|
||||
|
||||
**改造/新建测试**:
|
||||
- `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs` — 补充真实连接流程(可用本地模拟器)
|
||||
- `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` — 用本地 TCP/UDP 模拟器验证完整链路
|
||||
|
||||
**验证命令**:
|
||||
```bash
|
||||
cd flyshot-replacement
|
||||
dotnet build FlyshotReplacement.sln -v minimal
|
||||
dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal
|
||||
dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal
|
||||
```
|
||||
|
||||
## 风险与回退策略
|
||||
|
||||
1. **真机连接风险**:第一版 Socket 实现可能有超时/重连问题。`FanucControllerRuntime` 保留 `_simulationMode` 路径,仿真模式下仍走内存桩。
|
||||
2. **性能风险**:同步接口内部阻塞 Socket 可能影响 HTTP 并发。若实测有问题,后续将 `IControllerRuntime` 改为 async。
|
||||
3. **现场验证风险**:TCP 10012 参数命令已按逆向结论实现,但仍需在真实 R30iB 控制柜上确认默认 `tcp_id=1`、IO 类型/地址和错误码语义。
|
||||
|
||||
## 关键文件清单
|
||||
|
||||
| 文件 | 动作 |
|
||||
|------|------|
|
||||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs` | 新建 |
|
||||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs` | 新建 |
|
||||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs` | 新建 |
|
||||
| `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs` | 重写 |
|
||||
| `tests/Flyshot.Core.Tests/FanucCommandClientTests.cs` | 新建 |
|
||||
| `tests/Flyshot.Core.Tests/FanucStateClientTests.cs` | 新建 |
|
||||
| `tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs` | 新建 |
|
||||
| `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` | 新建 |
|
||||
|
||||
## 下一步验证标准
|
||||
|
||||
- `FanucControllerRuntime` 的 `Connect()` 能成功建立三条 TCP/UDP 连接
|
||||
- `EnableRobot()` 能走完 `RVBUSTSM` 启动序列
|
||||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点,并按当前 `speed_ratio` 推进原始轨迹时间
|
||||
- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存
|
||||
- 现有 10 个集成测试和 25 个核心测试仍然通过
|
||||
232
docs/fanuc-stream-motion-manual-notes.md
Normal file
232
docs/fanuc-stream-motion-manual-notes.md
Normal file
@@ -0,0 +1,232 @@
|
||||
# FANUC Stream Motion 文档要点与实现差异
|
||||
|
||||
本文记录 `../FANUC_stream_motion.pdf` 中与本仓库 `Flyshot.Runtime.Fanuc` 直接相关的重点,并对照当前实现状态。
|
||||
|
||||
读取时间:2026-05-03
|
||||
|
||||
## 1. 文档定位
|
||||
|
||||
`FANUC_stream_motion.pdf` 对应 FANUC `Stream motion` 功能,选项号为 `A05B-2600-J519`。它描述的是外部设备通过以太网实时发送期望位置,让机器人按外部生成路径运动的控制方式。
|
||||
|
||||
文档明确要求外部设备自行生成满足机器人约束的路径,包括速度、加速度、jerk、可达性、姿态连续性等。FANUC 不提供完整运动学、逆解和碰撞检测公式。
|
||||
|
||||
## 2. 使用前提与示教程序
|
||||
|
||||
1. 机器人侧需要安装 J519 stream motion 选项。
|
||||
2. 物理网口通过 `$STMO.$PHYS_PORT` 选择,`1` 表示 `CD38A`,`2` 表示 `CD38B`。
|
||||
3. 机器人程序必须包含成对的 `IBGN start[*]` 和 `IBGN end[*]` 指令,二者编号必须一致,`start` 必须在 `end` 前一行。
|
||||
4. `IBGN start[*]` 执行期间,机器人根据外部设备发来的期望位置运动;`IBGN end[*]` 之后程序继续执行。
|
||||
5. 执行时要求 `AUTO` 模式和 `100% OVERRIDE`。
|
||||
|
||||
当前实现中的 `FanucControllerRuntime.EnableRobot()` 会按现场抓包流程启动 `RVBUSTSM` 程序,并随后允许 J519 在收到机器人 UDP status 包后回发命令。是否满足 `AUTO / 100% OVERRIDE / IBGN start` 已到位,当前只通过 J519 状态位和现场程序行为间接判断,没有在代码里读取或设置这些控制器状态。
|
||||
|
||||
## 3. UDP 60015 协议结构
|
||||
|
||||
协议使用 UDP,大端字节序,机器人侧端口为 `60015`。通信周期通常为 `8ms`,部分机型支持 `4ms`。状态包输出可以在任意时间通过 start/stop 控制包启停,不要求已经进入 `IBGN start[*]`。
|
||||
|
||||
### 3.1 状态输出 start 包
|
||||
|
||||
外部设备发给机器人:
|
||||
|
||||
| 字段 | 长度 | 值 |
|
||||
| --- | --- | --- |
|
||||
| Packet type | 4B | `0` |
|
||||
| Version | 4B | `1` |
|
||||
|
||||
当前实现:`FanucJ519Protocol.PackInitPacket()` 已按 8B 大端控制包实现,`FanucJ519Client.ConnectAsync()` 连接后立即发送。
|
||||
|
||||
### 3.2 状态包
|
||||
|
||||
机器人发给外部设备,长度为 `132B`:
|
||||
|
||||
| 偏移 | 字段 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `0x00` | Packet type | `0` |
|
||||
| `0x04` | Version | `1` |
|
||||
| `0x08` | Sequence No. | 状态包序号,发送 start 包后从 `1` 重新开始 |
|
||||
| `0x0c` | Status | bit0 接受命令、bit1 已收到命令、bit2 SYSRDY、bit3 运动中 |
|
||||
| `0x0d..0x12` | Read I/O 回显和值 | 回显命令包中的读取 IO 类型、索引、掩码,并返回 16 点 IO 值 |
|
||||
| `0x14` | Timestamp | ms 单位,2ms 分辨率 |
|
||||
| `0x18..0x38` | Cartesian / external axis | `X/Y/Z/W/P/R` 加 3 个扩展轴 |
|
||||
| `0x3c..0x5c` | Joint | `J1..J9`,单位 degree |
|
||||
| `0x60..0x80` | Motor current | `J1..J9` 电流,单位 A |
|
||||
|
||||
当前实现:`FanucJ519Protocol.ParseResponse()` 已解析 `132B` 状态包,并暴露 `AcceptsCommand`、`ReceivedCommand`、`SystemReady`、`RobotInMotion` 四个状态位。`FanucControllerRuntime.GetSnapshot()` 也会把最新 J519 状态写进快照。
|
||||
|
||||
### 3.3 命令包
|
||||
|
||||
外部设备发给机器人,长度为 `64B`:
|
||||
|
||||
| 偏移 | 字段 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `0x00` | Packet type | `1` |
|
||||
| `0x04` | Version | `1` |
|
||||
| `0x08` | Sequence No. | 第一包应等于刚收到的状态包序号,后续逐包递增 |
|
||||
| `0x0c` | Last data | 正常为 `0`;结束外部控制时最后一包设为 `1` |
|
||||
| `0x0d..0x11` | Reading I/O | 可读取最多 16 个连续 IO 点 |
|
||||
| `0x12` | Data format | `0` 笛卡尔,`1` 关节 |
|
||||
| `0x13..0x19` | Writing I/O | 可写入最多 16 个连续 IO 点 |
|
||||
| `0x1a` | unused | 2B |
|
||||
| `0x1c..0x3c` | target[9] | 9 个 f32 目标值;关节格式时单位 degree |
|
||||
|
||||
当前实现:`FanucJ519Protocol.PackCommandPacket()` 已按上述布局打包,默认 `dataStyle=1`,也就是关节格式。运行时会把规划输出的弧度制关节轨迹转换为 degree 后下发。
|
||||
|
||||
### 3.4 状态输出 stop 包
|
||||
|
||||
外部设备发给机器人:
|
||||
|
||||
| 字段 | 长度 | 值 |
|
||||
| --- | --- | --- |
|
||||
| Packet type | 4B | `2` |
|
||||
| Version | 4B | `1` |
|
||||
|
||||
文档把它定义为“停止状态包输出”的控制包,不是命令流正常终止的首选动作。命令流结束应通过 command packet 的 `Last data=1` 表达。
|
||||
|
||||
当前实现:`FanucJ519Client.StopMotionAsync()` 当前会停止状态包驱动发送并发送 packet type `2`,而稠密轨迹执行期间保持 `LastData=0`。这是与 FANUC 文档最明显的语义差异之一;已有多数 UTTC 抓包显示主运行窗口 `LastData=0`,但 `../j519 协议.pcap` 中存在 1 个 `LastData=1` 后紧跟 packet type `2` 的样本,后续应单独校准停止语义。
|
||||
|
||||
## 4. 通信时序重点
|
||||
|
||||
文档推荐的时序是:
|
||||
|
||||
1. 外部设备发送状态输出 start 包。
|
||||
2. 机器人每个通信周期输出状态包。
|
||||
3. 机器人程序执行到 `IBGN start[*]` 后,状态包 bit0 变为 `1`,表示等待命令包。
|
||||
4. 外部设备收到 bit0 为 `1` 的状态包后,立即发送第一帧命令包,第一帧命令序号应等于刚收到的状态包序号。
|
||||
5. 后续每收到一个状态包,外部设备应立即发送下一帧命令包。
|
||||
6. 结束命令通信时,发送 `Last data=1` 的最后一帧命令包。
|
||||
|
||||
当前实现对照:
|
||||
|
||||
1. `FanucJ519Client` 已改为收到机器人 132B status 包后立即回发当前命令,不再由上位机本地固定 8ms 发送循环主动发包。
|
||||
2. 命令包 sequence 已按刚收到的 status packet sequence 写入,避免第一帧从本地 `0` 起步。
|
||||
3. `FanucControllerRuntime.ExecuteTrajectory()` 启动前会检查已有 J519 响应中的 `AcceptsCommand` 和 `SystemReady`;但如果还没收到状态包,则会放行,后续命令仍要等第一帧 status 到达才会发出。
|
||||
4. 当前稠密轨迹结束不发送 `LastData=1`,而是依赖停止 J519 状态包驱动发送和 packet type `2` stop 控制包。
|
||||
|
||||
序号和节拍已经按手册方向校准;停止语义仍需在真实 R30iB 联调中继续确认。
|
||||
|
||||
## 5. 命令缓冲
|
||||
|
||||
文档说明机器人可以缓冲提前到达的 command packet。默认启用,缓冲上限为 `$STMO.$PKT_STACK - 1`,`$PKT_STACK` 默认 `10`,可配置范围 `2..10`。`$STMO.$START_MOVE` 决定积累多少未处理命令包后开始运动,默认 `1`。
|
||||
|
||||
注意事项:
|
||||
|
||||
1. 只有 command packet 会进入缓冲。
|
||||
2. status output stop packet 会立即处理。
|
||||
3. 如果 command buffer 中还有未处理包,不应发送 status output stop packet。
|
||||
4. 使用 `Last data=1` 时,机器人会先处理完缓冲里的命令包,再结束外部控制。
|
||||
|
||||
当前实现没有显式预填 `$PKT_STACK` 缓冲,也没有读取 `$START_MOVE`。`FanucJ519Client` 只保存一个“当前命令”,由后台循环持续发送;`FanucControllerRuntime.SendDenseTrajectory()` 另一个 8ms 循环负责按轨迹时间更新这条当前命令。这与文档的“按状态包响应并可提前发多包缓冲”模型不同。
|
||||
|
||||
## 6. 可执行运动条件
|
||||
|
||||
文档列出的主要运动约束:
|
||||
|
||||
1. 目标点必须可达。
|
||||
2. 笛卡尔格式下目标点对应的关节解必须唯一,且 configuration 要与 `IBGN start` 开始时一致。
|
||||
3. 各轴必须满足上下限。
|
||||
4. 不能发生自碰撞。
|
||||
5. 必须考虑 FANUC J3 轴定义:J3 不是相对 J2 臂的夹角,而是机器人视角下相对水平面的 J3 臂角度。
|
||||
6. 外部设备必须控制每轴速度、加速度、jerk 不超过 `$STMO_GRP` 下的限制。
|
||||
7. 状态包中的当前位置是 servo feedback position,不是 command position。轨迹起点应平滑连接到机器人 command position,而不能简单用当前 servo position 直接起步。
|
||||
8. reducer load 超限也会导致停机,负载相关计算不公开。
|
||||
|
||||
当前实现对这些条件的覆盖:
|
||||
|
||||
| 条件 | 当前状态 |
|
||||
| --- | --- |
|
||||
| 关节格式下发 | 已实现,当前现场链路默认只使用关节格式 |
|
||||
| `rad -> deg` | 已实现,并由 UTTC J519 golden tests 覆盖 |
|
||||
| `speed_ratio` 下发时间轴缩放 | 已实现,J519 实发时间为 `t_send = k * 0.008`,原轨迹采样时间为 `t_traj = t_send * speed_ratio` |
|
||||
| IO 触发嵌入 J519 命令包 | 已实现,使用 `write_io_type/index/mask/value` |
|
||||
| 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数,但未从 FANUC `$STMO_GRP` 在线读取,也未实现手册附录中的 20 档速度/负载插值 |
|
||||
| J3 轴定义 | 当前文档未见专门处理;需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 |
|
||||
| command position 起步 | `MoveJoint` 会用当前运行时记录的关节作为起点生成 PTP 稠密轨迹;但没有通过 FANUC HMI 通信读取 command position |
|
||||
| reducer load | 未建模,依赖保守规划和现场报警反馈 |
|
||||
| 笛卡尔格式限制 | 运行时不走笛卡尔 J519 目标格式,暂不覆盖 configuration 变化报警 |
|
||||
|
||||
## 7. 系统变量
|
||||
|
||||
与本仓库后续最相关的变量:
|
||||
|
||||
| 变量 | 默认/含义 |
|
||||
| --- | --- |
|
||||
| `$STMO.$PHYS_PORT` | 物理口,`1=CD38A`,`2=CD38B` |
|
||||
| `$STMO.$COM_INT` | 通信周期,单位 ms,通常 `8`,只读 |
|
||||
| `$STMO.$PKT_STACK` | command buffer 最大保留量,默认 `10` |
|
||||
| `$STMO.$START_MOVE` | 缓冲中积累多少未处理命令后开始运动,默认 `1` |
|
||||
| `$STMO_GRP.$JNT_VEL_LIM[*]` | 各轴速度上限,degree/s,只读 |
|
||||
| `$STMO_GRP.$JNT_ACC_LIM[*]` | 各轴加速度上限,degree/s^2,只读 |
|
||||
| `$STMO_GRP.$JNT_JRK_LIM[*]` | 各轴 jerk 上限,degree/s^3,只读 |
|
||||
| `$STMO_GRP.$LMT_MODE` | 加速度/jerk 限制计算模式 |
|
||||
| `$STMO_GRP.$WARN_LIM` | 接近限制时报警阈值,默认 `80%` |
|
||||
| `$STMO_GRP.$FLTR_LN` | 命令目标移动平均滤波窗口 |
|
||||
| `$STMO_GRP.$MAX_SPD` | 用于限制计算的 flange center 最大速度 |
|
||||
|
||||
当前实现没有读取或设置上述系统变量。`RobotProfile.ServoPeriod` 当前决定运行时发送周期;对当前现场而言应继续确认它与 `$COM_INT` 一致。
|
||||
|
||||
## 8. 附录 B:加速度和 jerk 限制
|
||||
|
||||
文档说明,在 `$STMO_GRP[].$LMT_MODE=0` 时,加速度和 jerk 的允许上限会根据 flange center speed 与 payload 计算:
|
||||
|
||||
1. 以 `$MAX_SPD` 分成 20 档速度区间。
|
||||
2. 每个轴、每种限制类型都有无负载和最大负载两张 20 档表。
|
||||
3. 实际 payload 通过线性插值得到限制表。
|
||||
4. 实际 flange center speed 在相邻速度档之间线性插值。
|
||||
5. 限制值不是每个通信周期都更新,而是在超过 `Vmax/20` 到再次跌回阈值的整段时间内,以该段观测到的 `Vpeak` 决定。
|
||||
6. 如果长时间不跌回阈值,会按中间检查时间做临时判断。
|
||||
|
||||
文档还提供了 packet type `3` 的限制表查询协议:
|
||||
|
||||
| 包 | 方向 | 重点字段 |
|
||||
| --- | --- | --- |
|
||||
| 请求 | 外部设备 -> 机器人 | packet type `3`、version `1`、axis `1..9`、limit type `0=velocity/1=acceleration/2=jerk` |
|
||||
| 响应 | 机器人 -> 外部设备 | packet type `3`、version `1`、axis、limit type、`Vmax`、中间检查时间、无负载 20 档、最大负载 20 档 |
|
||||
|
||||
当前实现没有 packet type `3` 查询,也没有实现手册描述的动态限制表算法。现阶段规划时长和保守程度主要依赖 replacement 自身参数与现场抓包对齐。
|
||||
|
||||
## 9. 报警与诊断
|
||||
|
||||
文档中与实现最相关的报警:
|
||||
|
||||
| 报警 | 含义 |
|
||||
| --- | --- |
|
||||
| `MOTN-600` | 命令序号与机器人期望不一致 |
|
||||
| `MOTN-602` | data format 非法 |
|
||||
| `MOTN-603` | 后续命令包未在通信周期内到达 |
|
||||
| `MOTN-604` | 命令包过多,超出缓冲 |
|
||||
| `MOTN-605` | 目标位置包含 NaN 或 infinity |
|
||||
| `MOTN-606` | 非 AUTO 或 override 不是 100% |
|
||||
| `MOTN-607` | 协议版本不匹配 |
|
||||
| `MOTN-609/610/611` | 速度、加速度、jerk 超限 |
|
||||
| `MOTN-612/613/614` | 接近速度、加速度、jerk 限制 |
|
||||
| `MOTN-617` | 目标点与当前位置不连续 |
|
||||
| `MOTN-619` | 当前机型不支持笛卡尔目标格式 |
|
||||
| `PRIO-023` | 读写的 IO 类型或索引未分配 |
|
||||
|
||||
当前 `ControllerStateSnapshot.ActiveAlarms` 仍为空,Web 状态页也尚未接入 FANUC 报警列表。后续现场联调如果出现报警,应优先按上述表格关联 J519 包序号、目标数据、IO 字段、发送间隔和状态包 bit。
|
||||
|
||||
## 10. 与当前代码的结论
|
||||
|
||||
已基本对齐:
|
||||
|
||||
1. UDP 60015、大端、start/stop 控制包、64B command packet、132B status packet 的基础二进制布局。
|
||||
2. `Data format=1` 的关节目标下发。
|
||||
3. 状态位 bit0..bit3 的解析和快照暴露。
|
||||
4. 规划输出 `rad` 转 J519 `deg`。
|
||||
5. 根据 `speed_ratio` 做运行期时间轴缩放,而不是改变规划文件时间。
|
||||
6. 飞拍 IO 触发通过 command packet 的写 IO 字段下发。
|
||||
7. 命令发送按机器人 UDP status 包驱动,并使用最新 status sequence 回发。
|
||||
|
||||
主要差异/风险:
|
||||
|
||||
1. 当前未实现命令缓冲预填,也未读取 `$PKT_STACK / $START_MOVE`。
|
||||
2. 当前停止运动依赖 packet type `2` stop 控制包,没有稳定发送 `LastData=1` 的最后 command packet;这与手册标准结束语义不同。
|
||||
3. 当前未实现 packet type `3` 的速度/加速度/jerk 限制表查询,也未实现 payload/speed 20 档动态限制算法。
|
||||
4. 当前没有自动校验 `AUTO / 100% OVERRIDE / brake control / resume offset / payload` 等控制器前置状态。
|
||||
5. 当前没有报警码读取和 `MOTN-* / PRIO-*` 映射。
|
||||
|
||||
建议后续联调优先级:
|
||||
|
||||
1. 验证运动结束是否必须补 `LastData=1`;如果当前 stop 控制包能稳定工作,也应在文档中标为现场兼容路径,而不是手册标准路径。
|
||||
2. 抓一次报警现场包,确认 `MOTN-600/603/617` 等是否能从包序号与状态位直接定位。
|
||||
3. 如果后续追求更稳的真实机运行,补 packet type `3` 限制表查询,并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。
|
||||
457
docs/flyshot-speedratio-execution-design-20260509.md
Normal file
457
docs/flyshot-speedratio-execution-design-20260509.md
Normal file
@@ -0,0 +1,457 @@
|
||||
# 飞拍任务 `speedRatio` 执行侧调速设计
|
||||
|
||||
## 1. 背景
|
||||
|
||||
当前系统同时存在两个速度相关参数:
|
||||
|
||||
- `planning_speed_scale`
|
||||
- 规划阶段参数。
|
||||
- 作用是让规划结果在时长与整体动力学上更接近现场旧系统或当前配置要求。
|
||||
- 启动后固定,不希望在运行中再修改语义。
|
||||
- `speedRatio`
|
||||
- 运行阶段参数。
|
||||
- 期望成为统一在线调速入口。
|
||||
- 本次范围内只要求对“下一次开始执行的飞拍任务”生效,不要求对正在执行中的任务中途热切换。
|
||||
|
||||
本次任务已经明确收敛:
|
||||
|
||||
- 只修改飞拍任务链路。
|
||||
- 不修改 `move_joint`。
|
||||
- 不通过放宽阈值、关闭校验或只看平均速度来规避问题。
|
||||
|
||||
## 2. 现状问题
|
||||
|
||||
当前飞拍执行链路的核心语义是:
|
||||
|
||||
1. 规划层先生成 `DenseJointTrajectory`。
|
||||
2. 运行时保持 `8ms` 物理发送周期不变。
|
||||
3. 对第 `k` 个发送点,按 `trajectoryTime = sendTime * speedRatio` 回映射到规划轨迹时间。
|
||||
4. 再在原始稠密轨迹上做线性插值,得到实际要发的关节目标。
|
||||
5. 最后再用离散差分检查 `vel/acc/jerk`。
|
||||
|
||||
该结构在 `speedRatio < 1` 时有天然风险:
|
||||
|
||||
- 物理发送周期固定为 `8ms`,但轨迹时间推进被压缩。
|
||||
- 线性插值会把规划轨迹中原本“看起来连续”的局部变化映射成新的离散点列。
|
||||
- 这些新点列在三阶后向差分上容易形成尖峰。
|
||||
- 该风险在 `speedRatio = 0.5` 时已经暴露为明显的 Jerk 超限告警。
|
||||
|
||||
问题本质不是“平均速度是否合理”,而是:
|
||||
|
||||
- 最终 `8ms` 发送点列本身没有先被当作一等公民建模。
|
||||
- `speedRatio` 的执行语义落在“发送前临场回采样”阶段。
|
||||
- 离散约束校验发生得太晚,只能报错或事后分析。
|
||||
|
||||
## 3. 目标
|
||||
|
||||
本设计目标如下:
|
||||
|
||||
- 保持 `planning_speed_scale` 只属于规划层。
|
||||
- `speedRatio` 只作用于飞拍执行层,且仅对下一次启动的飞拍任务生效。
|
||||
- 任意有效 `speedRatio` 下,最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
|
||||
- 不依赖“插值后自然会平滑”的经验假设。
|
||||
- 当 `speedRatio = 0.5` 等较慢倍率导致候选点列不满足约束时,系统自动拉长执行时长,直到点列通过校验。
|
||||
- 不破坏现有 `shot timeline / sample offset` 语义。
|
||||
- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。
|
||||
|
||||
## 4. 非目标
|
||||
|
||||
本次设计明确不包含以下内容:
|
||||
|
||||
- 不修改 `move_joint` 的调速和生成机制。
|
||||
- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。
|
||||
- 不修改 `planning_speed_scale` 的含义。
|
||||
- 不在任务执行中途支持 `speedRatio` 热切换。
|
||||
- 不通过调大容差、降低限值检查强度或直接跳过 Jerk 检查来“解决问题”。
|
||||
|
||||
## 5. 设计总览
|
||||
|
||||
飞拍执行链路调整为“两段式”:
|
||||
|
||||
1. 规划段
|
||||
- 维持当前实现。
|
||||
- 仍由 `planning_speed_scale` 参与规划时长和规划层轨迹生成。
|
||||
- 输出 `TrajectoryResult.DenseJointTrajectory`、`TriggerTimeline`、`ShotEvents` 等规划结果。
|
||||
2. 执行准备段
|
||||
- 新增“飞拍执行侧最终发送序列生成器”。
|
||||
- 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。
|
||||
- 生成最终 `8ms` 发送队列。
|
||||
- 对最终队列做逐周期 `vel/acc/jerk` 校验。
|
||||
- 如果不通过,则自动拉长执行时长后重建队列,直到通过。
|
||||
3. 发送段
|
||||
- 运行时只消费已经准备好的最终 `8ms` 队列。
|
||||
- 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。
|
||||
|
||||
核心变化是:
|
||||
|
||||
- `speedRatio` 不再直接驱动“发送前临场插值”。
|
||||
- `speedRatio` 改为驱动“执行侧最终队列构建”。
|
||||
- 最终队列一旦生成并校验通过,运行时只负责按 J519 状态包节奏出队发送。
|
||||
|
||||
## 6. 统一语义
|
||||
|
||||
本次只改飞拍任务,但仍需把飞拍内部语义说清楚,避免再次混淆规划倍率和执行倍率。
|
||||
|
||||
### 6.1 `planning_speed_scale`
|
||||
|
||||
- 只影响规划结果。
|
||||
- 影响 `DenseJointTrajectory` 的时间轴与规划层速度/加速度/Jerk 使用方式。
|
||||
- 飞拍开始执行前就已经固定。
|
||||
- 在线设置 `speedRatio` 时不应反向修改它。
|
||||
|
||||
### 6.2 `speedRatio`
|
||||
|
||||
- 只影响飞拍执行准备阶段。
|
||||
- 只对下一次启动的飞拍任务生效。
|
||||
- 表示“用户期望的执行层时间推进速度”。
|
||||
- 该值先用于构建候选发送队列。
|
||||
- 如果候选发送队列在离散 `8ms` 点列上不满足约束,则系统自动进一步拉长执行时长,直到满足约束。
|
||||
|
||||
也就是说:
|
||||
|
||||
- `speedRatio` 是执行目标倍率。
|
||||
- 但最终真实执行时长允许比该目标更保守。
|
||||
- 保守的原因不是放宽标准,而是严格保证离散动力学约束。
|
||||
|
||||
## 7. 最终发送序列生成
|
||||
|
||||
### 7.1 新增执行侧生成器
|
||||
|
||||
建议新增一个飞拍专用生成器,命名可类似:
|
||||
|
||||
- `FlyshotExecutionSendSequenceBuilder`
|
||||
- 或 `FlyshotExecutionQueueBuilder`
|
||||
|
||||
职责:
|
||||
|
||||
- 输入规划层 `DenseJointTrajectory`。
|
||||
- 输入 `durationSeconds`、`servoPeriodSeconds`、`speedRatio`。
|
||||
- 输出最终真实发送的 `8ms` 点列。
|
||||
- 输出与该点列一致的触发绑定结果。
|
||||
- 输出校验与自动拉长过程中的诊断信息。
|
||||
|
||||
### 7.2 第一版候选队列
|
||||
|
||||
候选队列仍可以沿用当前时间轴语义作为起点:
|
||||
|
||||
- `sendTime = sampleIndex * 0.008`
|
||||
- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)`
|
||||
|
||||
但该候选队列只作为“第一轮尝试”,不能直接视为最终执行结果。
|
||||
|
||||
### 7.3 自动拉长策略
|
||||
|
||||
当候选队列的离散 `vel/acc/jerk` 校验失败时:
|
||||
|
||||
- 不修改规划层轨迹。
|
||||
- 不修改 `planning_speed_scale`。
|
||||
- 不放宽校验阈值。
|
||||
- 只在执行侧拉长最终发送时长,然后重新构建候选队列。
|
||||
|
||||
可实现为等价的两种方式之一:
|
||||
|
||||
1. 延长物理发送总点数,使发送总时长变长。
|
||||
2. 在保持 `8ms` 周期不变的前提下,降低执行侧有效轨迹时间推进速度。
|
||||
|
||||
本质上二者等价,建议统一落成第二种表达:
|
||||
|
||||
- 对外仍说“自动拉长执行时长”。
|
||||
- 对内通过更保守的 `trajectoryTime(sendTime)` 映射来实现。
|
||||
|
||||
### 7.4 自动拉长的迭代规则
|
||||
|
||||
建议采用单调保守策略:
|
||||
|
||||
1. 先按请求 `speedRatio` 构建第 1 版候选队列。
|
||||
2. 对候选队列做离散校验。
|
||||
3. 若失败,则按固定步长或倍率逐轮拉长,再次构建候选队列。
|
||||
4. 一旦通过,立即停止迭代,产出最终队列。
|
||||
5. 若超过安全迭代上限仍未通过,则拒绝执行并输出首个超限诊断。
|
||||
|
||||
这样可以保证:
|
||||
|
||||
- 自动拉长过程是可解释、可记录的。
|
||||
- 不会因为局部修补而引入新的不可预测尖峰。
|
||||
|
||||
## 8. 逐周期约束校验
|
||||
|
||||
最终发送队列必须在执行前通过离散检查。
|
||||
|
||||
### 8.1 校验对象
|
||||
|
||||
校验对象必须是最终真实发送的 `8ms` 点列,而不是:
|
||||
|
||||
- 规划层原始稠密轨迹,
|
||||
- 中间候选插值轨迹,
|
||||
- 或平均时长推导。
|
||||
|
||||
### 8.2 校验指标
|
||||
|
||||
保留现有逐周期离散差分口径:
|
||||
|
||||
- 速度
|
||||
- 加速度
|
||||
- Jerk
|
||||
|
||||
并继续使用当前机器人模型和运行时限值:
|
||||
|
||||
- `velocity_eff = velocity_base`
|
||||
- `acceleration_eff = acceleration_base * acc_limit`
|
||||
- `jerk_eff = jerk_base * jerk_limit`
|
||||
|
||||
### 8.3 校验失败的处理
|
||||
|
||||
飞拍链路按本次确认采用 A:
|
||||
|
||||
- 若失败,优先自动拉长执行时长重试。
|
||||
- 只有在超过拉长上限后仍失败时才拒绝执行。
|
||||
|
||||
拒绝执行时必须输出:
|
||||
|
||||
- 首次失败的轴
|
||||
- 时间窗
|
||||
- 指标类型
|
||||
- `actual / limit / ratio`
|
||||
- 失败发生在哪一轮自动拉长尝试中
|
||||
|
||||
## 9. 触发时序与绑定
|
||||
|
||||
### 9.1 保持现有语义
|
||||
|
||||
以下语义不变:
|
||||
|
||||
- `TriggerTimeline`
|
||||
- `ShotEvents`
|
||||
- `TriggerSampleIndexOffsetCycles`
|
||||
- `io_keep_cycles`
|
||||
|
||||
### 9.2 绑定对象改为最终发送队列
|
||||
|
||||
当前实现中,触发通常绑定到由回采样生成的 `samples`。
|
||||
|
||||
本次改为:
|
||||
|
||||
- 先生成最终发送队列。
|
||||
- 再用该最终队列做 `TriggerSampleBinder.Bind(...)`。
|
||||
|
||||
这样可以保证:
|
||||
|
||||
- 触发绑定和真实发送完全一致。
|
||||
- 导出工件中的 `ShotEvents.json` 与真实执行时序一致。
|
||||
- 自动拉长后触发点仍可被精确追溯到最终发送点索引。
|
||||
|
||||
### 9.3 关于触发数量与顺序
|
||||
|
||||
自动拉长执行时长时:
|
||||
|
||||
- 触发数量不能变。
|
||||
- 触发顺序不能变。
|
||||
- 只允许触发在最终发送队列中的绑定索引后移或保持可解释等价。
|
||||
|
||||
## 10. 运行时职责调整
|
||||
|
||||
本次需要显式收紧 `FanucControllerRuntime` 的职责。
|
||||
|
||||
### 10.1 改造前
|
||||
|
||||
`FanucControllerRuntime.SendDenseTrajectory(...)` 当前同时负责:
|
||||
|
||||
- 读取 `DenseJointTrajectory`
|
||||
- 根据 `_speedRatio` 回采样
|
||||
- 绑定触发
|
||||
- 生成命令队列
|
||||
- 写出 `ActualSend*` 工件
|
||||
|
||||
### 10.2 改造后
|
||||
|
||||
`FanucControllerRuntime` 只负责:
|
||||
|
||||
- 接收已经准备好的最终发送队列
|
||||
- 接收与该队列一致的触发绑定结果
|
||||
- 预构建命令队列并交给 J519 客户端
|
||||
- 记录真实发送过程与导出工件
|
||||
|
||||
不再负责:
|
||||
|
||||
- 按 `_speedRatio` 从 `DenseJointTrajectory` 临场生成发送点
|
||||
|
||||
这样做的目的是让:
|
||||
|
||||
- 运行时负责发送。
|
||||
- 执行准备层负责离散稳定性。
|
||||
|
||||
两层边界更清晰,也更利于测试。
|
||||
|
||||
## 11. 导出工件与日志
|
||||
|
||||
### 11.1 工件统一基于最终发送队列
|
||||
|
||||
以下文件必须全部从最终发送队列生成:
|
||||
|
||||
- `ActualSendJointTraj.txt`
|
||||
- `ActualSendTiming.txt`
|
||||
- `ActualSendJerkStats.txt`
|
||||
- `ShotEvents.json`
|
||||
|
||||
不允许再出现:
|
||||
|
||||
- 导出文件基于一套样本,
|
||||
- 真实发送队列又基于另一套样本。
|
||||
|
||||
### 11.2 推荐新增日志字段
|
||||
|
||||
每次飞拍执行建议至少记录:
|
||||
|
||||
- 请求 `speedRatio`
|
||||
- 规划层轨迹时长
|
||||
- 第一轮候选队列点数
|
||||
- 第一轮候选队列是否通过校验
|
||||
- 若失败,首个失败窗口的:
|
||||
- 关节轴
|
||||
- 时间区间
|
||||
- 指标类型
|
||||
- `actual`
|
||||
- `limit`
|
||||
- `ratio`
|
||||
- 自动拉长轮数
|
||||
- 最终采用的执行时长
|
||||
- 最终发送队列点数
|
||||
- 最终触发绑定数量
|
||||
- 最终校验通过结论
|
||||
|
||||
### 11.3 日志级别建议
|
||||
|
||||
- `Information`
|
||||
- 记录执行请求、最终采用结果、最终通过结论
|
||||
- `Warning`
|
||||
- 记录第一轮候选失败与自动拉长启动
|
||||
- `Debug`
|
||||
- 记录每一轮拉长的中间参数与详细差分统计
|
||||
|
||||
## 12. 验收口径
|
||||
|
||||
### 12.1 功能验收
|
||||
|
||||
- 飞拍 `speedRatio` 可在线设置。
|
||||
- 该值对下一次启动的飞拍任务生效。
|
||||
- 不需要修改 `planning_speed_scale`。
|
||||
|
||||
### 12.2 约束验收
|
||||
|
||||
在 `speedRatio = {1.0, 0.8, 0.5}` 下:
|
||||
|
||||
- 最终发送队列逐周期 `vel/acc/jerk` 不超限。
|
||||
- 不出现固定区间正负交替的大幅 Jerk 尖峰。
|
||||
|
||||
### 12.3 工件与日志验收
|
||||
|
||||
- `ActualSend*` 文件能反映最终真实发送点位与时间映射。
|
||||
- 日志能定位自动拉长前后的关键参数和校验结果。
|
||||
|
||||
### 12.4 回归验收
|
||||
|
||||
- `speedRatio = 1.0` 时不退化。
|
||||
- 触发事件数量不异常漂移。
|
||||
- 触发绑定顺序不异常漂移。
|
||||
|
||||
## 13. 测试方案
|
||||
|
||||
### 13.1 单元测试
|
||||
|
||||
建议新增:
|
||||
|
||||
- 飞拍执行侧最终发送序列生成器测试
|
||||
- `speedRatio` 非法边界值测试
|
||||
- 第一轮候选失败后自动拉长成功测试
|
||||
- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试
|
||||
- 触发绑定始终与最终发送队列一致的测试
|
||||
|
||||
### 13.2 编排测试
|
||||
|
||||
建议补充:
|
||||
|
||||
- `ExecuteFlyShotTraj` 进入运行时前,已经拿到最终发送队列
|
||||
- `FanucControllerRuntime` 不再自行按 `_speedRatio` 对飞拍轨迹回采样
|
||||
|
||||
### 13.3 集成与黄金样本
|
||||
|
||||
建议至少覆盖:
|
||||
|
||||
- `UTTC_MS11`
|
||||
- `speedRatio = 1.0`
|
||||
- `speedRatio = 0.8`
|
||||
- `speedRatio = 0.5`
|
||||
|
||||
检查项:
|
||||
|
||||
- 最终发送点数
|
||||
- 最终发送时长
|
||||
- `ActualSendTiming`
|
||||
- `ActualSendJointTraj`
|
||||
- `ActualSendJerkStats`
|
||||
- `ShotEvents` 绑定结果
|
||||
- 是否消除当前已知 Jerk 尖峰
|
||||
|
||||
## 14. 代码落点建议
|
||||
|
||||
建议实现边界如下:
|
||||
|
||||
### 14.1 `src/Flyshot.Core.Planning/Sampling/`
|
||||
|
||||
新增飞拍执行侧最终发送序列生成器及相关结果类型,负责:
|
||||
|
||||
- 候选队列构建
|
||||
- 自动拉长
|
||||
- 离散校验
|
||||
- 触发绑定输入准备
|
||||
|
||||
### 14.2 `src/Flyshot.ControllerClientCompat/`
|
||||
|
||||
在飞拍执行准备阶段调用该生成器,把最终发送队列放入执行结果或新的执行上下文对象。
|
||||
|
||||
### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||||
|
||||
调整 `SendDenseTrajectory(...)`:
|
||||
|
||||
- 不再自行根据 `_speedRatio` 从 `DenseJointTrajectory` 临场重采样
|
||||
- 直接消费已经构建好的最终发送队列
|
||||
|
||||
### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs`
|
||||
|
||||
`ActualSend*` 导出改为复用最终发送队列,确保导出工件与运行时一致。
|
||||
|
||||
### 14.5 `tests/Flyshot.Core.Tests/`
|
||||
|
||||
补飞拍专项单测、UTTC 回归和工件一致性测试。
|
||||
|
||||
## 15. 风险与注意事项
|
||||
|
||||
### 15.1 不能把自动拉长误解为“偷偷改 speedRatio”
|
||||
|
||||
对外语义仍应保留:
|
||||
|
||||
- 用户请求的是某个 `speedRatio`
|
||||
- 系统为了满足离散动力学约束,自动采用了更保守的最终执行时长
|
||||
|
||||
日志里必须把这件事说清楚,不能让现场误以为参数未生效。
|
||||
|
||||
### 15.2 不能让触发绑定脱离最终队列
|
||||
|
||||
只要最终发送点数变化,触发绑定就必须重新基于最终队列计算,否则工件与执行会再次分叉。
|
||||
|
||||
### 15.3 `speedRatio = 1.0` 必须优先做回归保护
|
||||
|
||||
这是最容易被“执行侧重构”误伤的路径,必须作为首要回归项。
|
||||
|
||||
## 16. 结论
|
||||
|
||||
本次飞拍调速设计的核心结论是:
|
||||
|
||||
- `planning_speed_scale` 继续只属于规划层。
|
||||
- `speedRatio` 只属于飞拍执行层,并且只对下一次启动的飞拍任务生效。
|
||||
- 飞拍执行必须先生成最终 `8ms` 发送队列,再对该队列做逐周期 `vel/acc/jerk` 校验。
|
||||
- 若 `speedRatio < 1` 导致第一轮候选队列不满足约束,则自动拉长执行时长后重建,直到通过。
|
||||
- 运行时不再对飞拍轨迹做发送前临场回采样。
|
||||
- 触发绑定、导出工件与真实发送必须全部统一到同一份最终发送队列上。
|
||||
|
||||
该方案可以在不修改 `move_joint`、不放宽校验阈值、不中断现有触发语义的前提下,把飞拍 `speedRatio` 调速从“经验插值”收敛为“可校验、可追踪、可自动保守化”的执行机制。
|
||||
70
docs/legacy-fit-uttc-ms11-plan.md
Normal file
70
docs/legacy-fit-uttc-ms11-plan.md
Normal file
@@ -0,0 +1,70 @@
|
||||
# UTTC_MS11 Legacy Fit 计划
|
||||
|
||||
## 目标
|
||||
|
||||
把 `Rvbust/前两个点正常 飞拍失败的运行` 中的旧 1x 轨迹拟合逻辑收敛到当前 replacement 实现里,让 `UTTC_MS11` 在新系统中尽量复现旧系统的轨迹时间轴和中间点形状。
|
||||
|
||||
当前已确认的事实:
|
||||
|
||||
- `Config/RobotConfig.json` 里的 `UTTC_MS11` 示教点与旧样本一致。
|
||||
- 旧 `1倍速度 角度坐标点/waypoint.txt` 已给出 20 个示教点的 legacy 时间节点。
|
||||
- 旧 1x 的节点时间与当前规划时间存在稳定比例,约为 `0.742277`。
|
||||
- 当前运行时 `ApplySmoothStartStopTiming` 会再次改写时间轴,这会破坏旧 waypoint time 的拟合结果。
|
||||
|
||||
## 拟合策略
|
||||
|
||||
优先分两层处理,不把不同问题混成一个旋钮:
|
||||
|
||||
1. 时间轴拟合
|
||||
- 先把 `UTTC_MS11` 的规划时间拉回旧 1x 的节点时间。
|
||||
- 通过 `planning_speed_scale` 复现旧 `waypoint.txt` 的时间比例。
|
||||
- 对 legacy-fit 轨迹,禁止运行时二次平滑起停时间重映射。
|
||||
|
||||
2. 空间曲线拟合
|
||||
- 保持原始示教点不变。
|
||||
- 先用当前插补器 + legacy 时间轴做第一版对齐。
|
||||
- 如果中间点仍和旧轨迹差异明显,再用旧 `JointDetialTraj.txt` 在 knot 附近反推速度/加速度,升级为 Hermite 拟合。
|
||||
|
||||
## 计划分解
|
||||
|
||||
### 1. 配置层
|
||||
|
||||
- 为 `RobotConfig.json` 增加明确的 legacy-fit 运行开关。
|
||||
- 当前现场的 `UTTC_MS11` 显式启用:
|
||||
- `planning_speed_scale = 0.742277`
|
||||
- `smooth_start_stop_timing = false`
|
||||
- 保留默认行为为兼容旧实现,以免影响其他轨迹。
|
||||
|
||||
### 2. 编排层
|
||||
|
||||
- `ControllerClientTrajectoryOrchestrator` 读取运行配置后,按开关决定是否调用 `ApplySmoothStartStopTiming`。
|
||||
- 缓存键必须包含该开关,避免 legacy-fit 和普通飞拍共用一份结果。
|
||||
- `saveTrajectory` / `IsFlyshotTrajectoryValid` 仍然输出规划结果,只是 legacy-fit 轨迹不再被二次改写时间轴。
|
||||
|
||||
### 3. 运行层
|
||||
|
||||
- `FanucControllerRuntime` 继续使用 8ms 物理发送周期。
|
||||
- DenseSend 实发点数仍按 `duration / (8ms * speedRatio)` 计算。
|
||||
- 终点要保留完整落点,不因为非整周期而丢掉最后一个点。
|
||||
|
||||
### 4. 测试层
|
||||
|
||||
- 增加配置测试,确认新开关可解析,默认值不破坏旧行为。
|
||||
- 增加编排测试,确认 UTTC_MS11 的规划时刻与旧 `waypoint.txt` 一致。
|
||||
- 增加运行测试,确认 legacy-fit 目录能写出稳定的 DenseSend 诊断文件。
|
||||
- 继续保留原有平滑起停测试,作为“显式开启平滑时”的回归保护。
|
||||
|
||||
## 验收标准
|
||||
|
||||
- `UTTC_MS11` 的 waypoint time 与旧 `waypoint.txt` 对齐。
|
||||
- `UTTC_MS11` 运行时不再额外套一层平滑重映射。
|
||||
- DenseSend 输出稳定,且不再受旧 bin 目录残留影响。
|
||||
- 现有默认轨迹和非 UTTC 场景不被破坏。
|
||||
|
||||
## 后续可能的第二阶段
|
||||
|
||||
如果时间轴对齐后,中间点仍和旧轨迹有明显偏差,再做第二阶段:
|
||||
|
||||
- 从旧 `JointDetialTraj.txt` 提取 knot 附近速度/加速度。
|
||||
- 用 Hermite / quintic Hermite 继续逼近旧曲线形状。
|
||||
- 将空间曲线拟合与时间轴拟合分开验收。
|
||||
160
docs/move-joint-jerk-comparison-20260505.md
Normal file
160
docs/move-joint-jerk-comparison-20260505.md
Normal file
@@ -0,0 +1,160 @@
|
||||
# MoveJoint 失败样本六轴限值与 ActualSendJerkStats 对比
|
||||
|
||||
记录时间:2026-05-05
|
||||
|
||||
## 1. 目的
|
||||
|
||||
本文档固定记录以下三类证据,避免后续继续混用测试基线、旧文档结论和当前运行目录中的真实模型数据:
|
||||
|
||||
- 当前运行目录 `.robot` 模型中的六轴基础 `velocity / acceleration / jerk`
|
||||
- 现场示教器读取到的六轴 `velocity / acceleration / jerk`
|
||||
- 当前运行目录 `RobotConfig.json` 中的 `acc_limit / jerk_limit`
|
||||
- 当前失败样本 `ActualSendJerkStats.txt` 中的逐轴实发跃度峰值
|
||||
|
||||
本次样本对应目录:
|
||||
|
||||
- `.robot`:`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Models/LR_Mate_200iD_7L.robot`
|
||||
- 配置:`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/RobotConfig.json`
|
||||
- 实发跃度:`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/move-joint/DenseSend/20260505_203416_563/ActualSendJerkStats.txt`
|
||||
- 抓包:`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/move-joint/DenseSend/20260505_203416_563/移动点 跃度过大.pcap`
|
||||
|
||||
## 2. 当前运行模型的真实六轴限值
|
||||
|
||||
当前仓库运行时通过 `RobotModelLoader.LoadProfile(...)` 从 `.robot` 中读取每轴 `limit.velocity / limit.acceleration / limit.jerk`,然后只对加速度和 jerk 叠加 `RobotConfig.json` 的全局倍率:
|
||||
|
||||
- `velocity_eff = velocity_base`
|
||||
- `acceleration_eff = acceleration_base * acc_limit`
|
||||
- `jerk_eff = jerk_base * jerk_limit`
|
||||
|
||||
当前运行目录 `RobotConfig.json` 中:
|
||||
|
||||
- `acc_limit = 0.74`
|
||||
- `jerk_limit = 0.74`
|
||||
|
||||
按当前运行目录真实模型解出的六轴基础值与生效值如下:
|
||||
|
||||
| Joint | vel_base | acc_base | jerk_base | vel_eff | acc_eff | jerk_eff(rad/s^3) | jerk_eff(deg/s^3) |
|
||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | 6.45 | 26.90 | 224.22 | 6.45 | 19.9060 | 165.9228 | 9506.6762 |
|
||||
| Joint2 | 5.41 | 22.54 | 187.86 | 5.41 | 16.6796 | 139.0164 | 7965.0530 |
|
||||
| Joint3 | 7.15 | 29.81 | 248.46 | 7.15 | 22.0594 | 183.8604 | 10534.4249 |
|
||||
| Joint4 | 9.59 | 39.99 | 333.30 | 9.59 | 29.5926 | 246.6420 | 14131.5457 |
|
||||
| Joint5 | 9.51 | 39.63 | 330.27 | 9.51 | 29.3262 | 244.3998 | 14003.0771 |
|
||||
| Joint6 | 17.45 | 72.72 | 606.01 | 17.45 | 53.8128 | 448.4474 | 25694.1434 |
|
||||
|
||||
重要结论:
|
||||
|
||||
- 当前运行目录中,`Joint1.jerk_base` 不是测试基线里常见的 `272.7`,而是 `224.22`。
|
||||
- 因此当前样本的 `Joint1` 生效 jerk 上限应按 `224.22 * 0.74 = 165.9228 rad/s^3` 计算。
|
||||
|
||||
## 3. 示教器读取到的实际机器人限值
|
||||
|
||||
2026-05-06 现场从机器人示教器读取到的速度、加速度与加加速度限制如下。示教器显示口径为角度制;下表同时记录换算后的弧度制,便于与 `.robot` / `RobotProfile` 中的基础限值直接对照。
|
||||
|
||||
换算公式:
|
||||
|
||||
- `rad = deg * π / 180`
|
||||
|
||||
| Joint | velocity(deg/s) | velocity(rad/s) | acceleration(deg/s^2) | acceleration(rad/s^2) | jerk(deg/s^3) | jerk(rad/s^3) |
|
||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | 370 | 6.457718 | 1541.667 | 26.907165 | 12847.223 | 224.226341 |
|
||||
| Joint2 | 310 | 5.410521 | 1291.667 | 22.543842 | 10763.889 | 187.865303 |
|
||||
| Joint3 | 410 | 7.155850 | 1708.333 | 29.816036 | 14236.111 | 248.467010 |
|
||||
| Joint4 | 550 | 9.599311 | 2291.667 | 39.997135 | 19097.223 | 333.309419 |
|
||||
| Joint5 | 545 | 9.512044 | 2270.833 | 39.633513 | 18923.611 | 330.279318 |
|
||||
| Joint6 | 1000 | 17.453293 | 4166.667 | 72.722058 | 34722.223 | 606.017115 |
|
||||
|
||||
这组数据的含义:
|
||||
|
||||
- 示教器读取值与当前运行 `.robot` 中的 `velocity_base / acceleration_base / jerk_base` 基本一致,可作为实际机器人基础限值证据。
|
||||
- 它还没有叠加当前 `RobotConfig.json` 的 `acc_limit = 0.74` 与 `jerk_limit = 0.74`;若用于本次失败样本比较,仍应使用第 2 节中的 `acc_eff / jerk_eff` 作为生效上限。
|
||||
|
||||
## 4. ActualSendJerkStats 的单位边界
|
||||
|
||||
`ActualSendJerkStats.txt` 的真实输入不是弧度,而是 J519 下发用的角度制关节目标:
|
||||
|
||||
1. `SampleDenseJointTrajectoryDegrees(...)` 先把轨迹点从 `rad` 转成 `deg`
|
||||
2. `BuildDenseSendJointRow(...)` 把这组角度制关节写入 `ActualSendJointTraj.txt`
|
||||
3. `BuildDenseSendJerkRow(...)` 再直接基于这组角度制关节做三阶差分
|
||||
|
||||
2026-05-06 之后,`ActualSendJointTraj.txt` 第一列和 `ActualSendJerkStats.txt` 的 `dt` 都使用 J519 实发时间;若需要查看被 `speed_ratio` 缩放后的原轨迹采样时间,应读取同目录的 `ActualSendTiming.txt`。因此当前这份 `ActualSendJerkStats.txt` 的逐轴跃度应按以下方式理解:
|
||||
|
||||
- 文本中的数值口径:`deg/s^3`
|
||||
- 若要与 `.robot` / `RobotProfile` 中的 jerk limit 比较,需要先换算为 `rad/s^3`
|
||||
- 换算公式:`jerk_rad = jerk_deg * π / 180`
|
||||
|
||||
## 5. 全文件逐轴最大跃度对比
|
||||
|
||||
扫描整份 `ActualSendJerkStats.txt` 后,各轴绝对值最大跃度如下:
|
||||
|
||||
| Joint | peak window(s) | peak line | peak actual(deg/s^3) | peak actual(rad/s^3) | jerk_eff(rad/s^3) | peak/limit |
|
||||
| --- | --- | ---: | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | `1.056 -> 1.064` | 133 | 21868.115990 | 381.670625 | 165.922800 | 2.3003 |
|
||||
| Joint2 | `1.056 -> 1.064` | 133 | 40.271793 | 0.702875 | 139.016400 | 0.0051 |
|
||||
| Joint3 | `1.056 -> 1.064` | 133 | 98.314401 | 1.715910 | 183.860400 | 0.0093 |
|
||||
| Joint4 | `1.056 -> 1.064` | 133 | 0.207266 | 0.003617 | 246.642000 | 0.0000 |
|
||||
| Joint5 | `1.056 -> 1.064` | 133 | 26.759688 | 0.467045 | 244.399800 | 0.0019 |
|
||||
| Joint6 | `1.056 -> 1.064` | 133 | 2.328736 | 0.040644 | 448.447400 | 0.0001 |
|
||||
|
||||
结论非常明确:
|
||||
|
||||
- 全文件范围内,只有 `Joint1` 的实发跃度显著超过当前生效 jerk 上限。
|
||||
- 其余 5 个轴即使取全文件峰值,也远低于各自当前生效 jerk limit。
|
||||
- 当前样本本质上是一个“J1 主导”的跃度问题,而不是六轴普遍同时逼近上限。
|
||||
|
||||
## 6. 报警窗口逐轴对比
|
||||
|
||||
结合抓包与 J519 序号,报警前最后一个关键窗口是:
|
||||
|
||||
- `seq=41552`
|
||||
- 轨迹时间窗口:`0.296 -> 0.304s`
|
||||
- `ActualSendJerkStats.txt` 行号:38
|
||||
|
||||
该窗口逐轴跃度如下:
|
||||
|
||||
| Joint | alarm window jerk(deg/s^3) | alarm window jerk(rad/s^3) | jerk_eff(rad/s^3) | alarm/limit |
|
||||
| --- | ---: | ---: | ---: | ---: |
|
||||
| Joint1 | -20395.713579 | 355.972355 | 165.922800 | 2.1454 |
|
||||
| Joint2 | -37.560252 | 0.655550 | 139.016400 | 0.0047 |
|
||||
| Joint3 | 91.694793 | 1.600376 | 183.860400 | 0.0087 |
|
||||
| Joint4 | -0.193310 | 0.003374 | 246.642000 | 0.0000 |
|
||||
| Joint5 | 24.957931 | 0.435598 | 244.399800 | 0.0018 |
|
||||
| Joint6 | 2.171939 | 0.037907 | 448.447400 | 0.0001 |
|
||||
|
||||
该窗口的直接结论与全局扫描一致:
|
||||
|
||||
- 机器人开始报警的 `0.296 -> 0.304s` 窗口里,真正越限的仍然只有 `Joint1`
|
||||
- `Joint1` 在报警窗口内已经达到当前生效 jerk limit 的 `2.1454x`
|
||||
- 其余 5 轴在同一窗口仍远低于生效 jerk 上限
|
||||
|
||||
## 7. 报警窗口与全局峰值窗口的关系
|
||||
|
||||
本次样本不能简单理解为“最大峰值出现的位置就是首次报警位置”。
|
||||
|
||||
当前证据表明:
|
||||
|
||||
- 首次报警相关窗口在 `0.296 -> 0.304s`
|
||||
- 全文件最大的 J1 跃度峰值出现在更后面的 `1.056 -> 1.064s`
|
||||
|
||||
这说明至少有两件事需要分开:
|
||||
|
||||
1. 机器人第一次进入异常态时,`Joint1` 已经在 `0.296 -> 0.304s` 超限约 `2.15x`
|
||||
2. 即便忽略第一次报警,后续轨迹中仍存在更高的 J1 跃度峰值,说明当前 `MoveJoint` 临时轨迹整体都偏激,不只是单个孤立点异常
|
||||
|
||||
## 8. 当前可落地的结论
|
||||
|
||||
基于当前运行目录的真实模型、配置和实发跃度文件,本次失败样本可以先固定为下面这组结论:
|
||||
|
||||
- 当前运行模型 `Joint1.jerk_base = 224.22`,不是 `272.7`
|
||||
- 现场示教器读取到的 `Joint1.jerk = 12847.223 deg/s^3 = 224.226341 rad/s^3`,与当前运行模型基础值一致
|
||||
- 当前样本 `jerk_limit = 0.74`,所以 `Joint1.jerk_eff = 165.9228 rad/s^3`
|
||||
- `ActualSendJerkStats.txt` 需要按 `deg/s^3` 理解,再换算成 `rad/s^3` 后与模型 jerk limit 对比
|
||||
- 无论看报警窗口还是看全文件峰值,越限主体都只有 `Joint1`
|
||||
- 报警窗口 `0.296 -> 0.304s` 中,`Joint1` 已经约为当前生效 jerk 上限的 `2.1454x`
|
||||
- 全文件最大峰值窗口 `1.056 -> 1.064s` 中,`Joint1` 约为当前生效 jerk 上限的 `2.3003x`
|
||||
|
||||
因此当前最合理的根因指向仍然是:
|
||||
|
||||
- `MoveJoint` 临时轨迹生成得过于激进
|
||||
- 当前问题首先应按 `Joint1` 的 jerk 约束失配来处理
|
||||
- 暂时没有证据支持“六轴普遍一起逼近限制”或“网络链路导致跃度统计失真”这类解释
|
||||
592
docs/planning-duration-mismatch-investigation.md
Normal file
592
docs/planning-duration-mismatch-investigation.md
Normal file
@@ -0,0 +1,592 @@
|
||||
# 轨迹规划时长差异调查记录
|
||||
|
||||
## 背景
|
||||
|
||||
当前新 C# 规划链路在不额外缩放规划约束时,部分真实现场轨迹会比旧 RVBUST/FlyingShot 导出的 `JointTraj.txt` 更短。
|
||||
|
||||
最典型现象:
|
||||
|
||||
- 真实 `Rvbust/uttc-20260428/Data/JointTraj.txt`:`UTTC_MS11` 总时长约 `7.403046s`。
|
||||
- 新 C# 当前默认规划输出:`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/UTTC_MS11/JointTraj.txt` 总时长约 `5.495112s`。
|
||||
- 实体机复核确认:修改运行时 `speed_ratio` 不影响 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 生成的 `JointTraj.txt` 规划时长。
|
||||
|
||||
因此,本问题不应继续归因到运行时 `speed_ratio`,而应归到规划阶段的有效关节约束来源。
|
||||
|
||||
## 已确认事实
|
||||
|
||||
1. `speed_ratio` 是运行执行倍率。
|
||||
|
||||
UTTC 抓包和实体机测试都显示,`speed_ratio=0.7` 会拉伸 J519 实际下发时间和包数,但不会改变已生成的 `JointTraj.txt` 规划时间轴。
|
||||
|
||||
2. `JointTraj.txt` 是规划结果点位。
|
||||
|
||||
`saveTrajectory` / `SaveTrajInfo` / `IsFlyshotTrajectoryValid(saveTrajectory=true)` 生成的 `JointTraj.txt` 表示规划后的 sparse waypoint 时间轴,不是上传的原始飞拍路径,也不是 J519 逐周期下发点。
|
||||
|
||||
3. UTTC_MS11 的差异是整条时间轴等比例缩放。
|
||||
|
||||
对 `UTTC_MS11`,真实时间和当前 C# 默认规划时间之间的比例在所有 waypoint 上都一致:
|
||||
|
||||
```text
|
||||
C#默认规划时间 / 真实规划时间 = 5.495112 / 7.403046 = 0.742277
|
||||
```
|
||||
|
||||
这说明路点顺序、相对分段时间和 ICSP 主要逻辑基本一致,差异更像是规划时传入的有效 `vel/acc/jerk` joint limits 存在整体倍率差异。
|
||||
|
||||
4. 现场配置中没有找到显式倍率字段。
|
||||
|
||||
已检查现场现有配置,未发现类似 `planning_speed_scale` 或等价字段保存了 `0.742277`、`0.7`、`0.9` 等规划倍率值。
|
||||
|
||||
## 样本对比
|
||||
|
||||
| 样本 | 真实 `JointTraj.txt` 时长 | 当前/已有新规划时长 | 等效倍率 `新规划/真实` | 说明 |
|
||||
| --- | ---: | ---: | ---: | --- |
|
||||
| `UTTC_MS11` | `7.403046s` | `5.495112s` | `0.742277` | 所有 waypoint 时间均为同一比例 |
|
||||
| `UTTC_MS11_TEST01` | `7.805885s` | `5.814370s` | `0.744870` | `20260428 多点` 新增 1 个路径点后仍几乎是整条时间轴等比例缩放 |
|
||||
| `EOL10_EAU_0` | `14.849788s` | `10.489800s` | `0.706394` | 同样表现为新规划偏快 |
|
||||
| `EOL9_EAU_0` / `EOL9_EAU_90` | `6.400851s` | `5.651140s` | `0.882873` | `EOL9 EAU 0` 与 `EOL9 EAU 90` 的真实 `JointTraj.txt` 文件一致 |
|
||||
| `EOL9_EAU_90` | `6.400851s` | `6.471610s` | `1.011055` | 使用 `speedRatio=0.9 + self-adapt-icsp` 的旧离线结果已接近真实 |
|
||||
|
||||
这些样本说明差异不是 `UTTC_MS11` 的个案,也不是一个可以全局写死的常数。不同真实样本对应的等效规划倍率不同。
|
||||
|
||||
## `20260428 多点` 新样本对比
|
||||
|
||||
2026-04-30 追加现场新样本:
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/RobotConfig.json
|
||||
../Rvbust/20260428 多点/JointTraj.txt
|
||||
../Rvbust/20260428 多点/JointDetialTraj.txt
|
||||
```
|
||||
|
||||
该样本中的飞拍程序名为:
|
||||
|
||||
```text
|
||||
UTTC_MS11_TEST01
|
||||
```
|
||||
|
||||
配置摘要:
|
||||
|
||||
```text
|
||||
waypoints=21
|
||||
shot_flags=21
|
||||
acc_limit=1
|
||||
jerk_limit=1
|
||||
```
|
||||
|
||||
实机导出的 `JointTraj.txt`:
|
||||
|
||||
```text
|
||||
rows=21
|
||||
duration=7.805885s
|
||||
```
|
||||
|
||||
用当前 C# `ICspPlanner`、同一个 `LR_Mate_200iD_7L.robot`、同一份 `RobotConfig.json` 规划:
|
||||
|
||||
```text
|
||||
rows=21
|
||||
duration=5.814370s
|
||||
C# / 实机 = 0.744870
|
||||
```
|
||||
|
||||
逐点/逐段统计:
|
||||
|
||||
```text
|
||||
point_ratio_std = 2.18e-7
|
||||
segment_ratio_std = 9.0e-7
|
||||
max_joint_diff = 5.0e-7 rad
|
||||
```
|
||||
|
||||
这说明:
|
||||
|
||||
1. 新样本的路点关节值与 C# 输入基本完全一致,不是解析错点或单位错位。
|
||||
2. 新增 1 个路径点后,C# 与旧系统仍然保持几乎严格的整条时间轴等比例差异。
|
||||
3. `UTTC_MS11_TEST01` 的倍率 `0.744870` 与原 `UTTC_MS11` 的 `0.742277` 非常接近,进一步支持“同一类 UTTC 现场导出使用了一组更保守的 effective JointLimits”这一判断。
|
||||
|
||||
和原 `UTTC_MS11` 对比:
|
||||
|
||||
```text
|
||||
原 UTTC_MS11 实机 rows=20 duration=7.403046s
|
||||
新 UTTC_MS11_TEST01 实机 rows=21 duration=7.805885s
|
||||
新增路径点后实机时长增加 0.402839s
|
||||
```
|
||||
|
||||
当前观察不到新增点导致规划形状或局部段比例失真;它更像是在同一套旧系统规划约束下正常增加了一段路径时间。
|
||||
|
||||
## `20260430.pcap` 初始化抓包复核
|
||||
|
||||
2026-04-30 继续复核现场提供的完整初始化抓包:
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/20260430.pcap
|
||||
```
|
||||
|
||||
抓包总览:
|
||||
|
||||
```text
|
||||
packet_count=4821
|
||||
tcp_payload_bytes=35302
|
||||
udp_payload_bytes=451946
|
||||
```
|
||||
|
||||
主要有效负载会话为:
|
||||
|
||||
```text
|
||||
UDP 192.168.10.11:60015 -> 192.168.10.10:56118 260700B
|
||||
UDP 192.168.10.11:60015 -> 192.168.10.10:48455 127116B
|
||||
UDP 192.168.10.10:48455 -> 192.168.10.11:60015 62088B
|
||||
TCP 192.168.10.11:10010 -> 192.168.10.10:42106 35102B
|
||||
TCP 192.168.10.11:10012 -> 192.168.10.10:33528 106B
|
||||
TCP 192.168.10.10:33528 -> 192.168.10.11:10012 94B
|
||||
```
|
||||
|
||||
全包搜索以下明文关键字没有命中:
|
||||
|
||||
```text
|
||||
Joint / joint / Limit / limit / vel / acc / jerk / Speed / speed /
|
||||
Robot / JSON / Traj / ratio / Ratio / GetJoint / SetJoint
|
||||
```
|
||||
|
||||
`TCP 10012` 命令通道按 `doz + length + message_id + body + zod` 解码后,只看到以下初始化/程序命令:
|
||||
|
||||
| 方向 | 消息号 | 含义 | 请求体/结果 |
|
||||
| --- | ---: | --- | --- |
|
||||
| C->R | `0x0001` | 未知握手 | 空请求 |
|
||||
| R->C | `0x0001` | 未知握手响应 | `result=0` |
|
||||
| C->R | `0x2000` | 未知版本/状态查询 | 空请求 |
|
||||
| R->C | `0x2000` | 未知版本/状态响应 | 包含 `0.6.0` 字符串 |
|
||||
| C->R | `0x2100` | `ResetRobot` | 空请求 |
|
||||
| R->C | `0x2100` | `ResetRobot` 响应 | `result=0` |
|
||||
| C->R | `0x2003` | `GetProgramStatus("RVBUSTSM")` | 程序名 `RVBUSTSM` |
|
||||
| R->C | `0x2003` | 程序状态响应 | `result=0, status=1` |
|
||||
| C->R | `0x2102` | `StartProgram("RVBUSTSM")` | 程序名 `RVBUSTSM` |
|
||||
| R->C | `0x2102` | 启动响应 | `result=0` |
|
||||
|
||||
没有看到:
|
||||
|
||||
- `0x2207 SetSpeedRatio`
|
||||
- `0x2206 GetSpeedRatio`
|
||||
- `0x2200/0x2201 GetTcp/SetTcp`
|
||||
- `0x2208/0x2209 GetIo/SetIo`
|
||||
- 任何疑似 `JointLimits / velocity / acceleration / jerk` 的参数帧
|
||||
|
||||
`TCP 10010` 状态通道只有机器人侧到上位机的状态帧:
|
||||
|
||||
```text
|
||||
390 个 90B 状态帧
|
||||
1 个 2B 连接前导
|
||||
```
|
||||
|
||||
这些帧与已逆向的 `pose[6] + joint[6] + external_axes[3] + raw_tail_words[4]` 状态布局一致,不携带规划约束。
|
||||
|
||||
`UDP 60015` J519 通道只出现既有三类长度:
|
||||
|
||||
```text
|
||||
C->R 8B 初始化包 1 个
|
||||
C->R 64B 目标关节命令包 970 个
|
||||
R->C 132B 反馈包 2938 个
|
||||
```
|
||||
|
||||
没有出现其他长度的 UDP 参数帧。64B 命令包是 J519 逐周期目标关节/IO 命令,132B 是机器人反馈;这条链路承载的是执行期 streaming motion,而不是旧 RVBUST 规划器的 joint limit 配置。
|
||||
|
||||
阶段结论:
|
||||
|
||||
```text
|
||||
20260430.pcap 只覆盖机器人侧 10010 / 10012 / 60015 通信。
|
||||
它没有 50001/TCP+JSON,也没有 ControllerServer/Python 客户端到旧服务端的配置调用。
|
||||
因此,这份抓包看不到旧规划阶段的 effective JointLimits。
|
||||
```
|
||||
|
||||
这并不否定“旧系统规划瞬间存在更保守的 effective JointLimits”这一方向;它只说明这份初始化抓包不是抓取该信息的位置。若要抓到这类限制,需要抓旧服务端内部 `_GetJointLimits/_SetJointLimits`,或者抓上层 Python/GUI 与 ControllerServer 之间的配置/规划调用,而不是只抓机器人控制柜侧的执行链路。
|
||||
|
||||
## `all-50001.pcap` 本机 50001 抓包复核
|
||||
|
||||
2026-04-30 追加复核本机所有网卡抓包:
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/all-50001.pcap
|
||||
SHA256=C3543F314AE446CABA8E2097EFAFB36F39DD73FFE166F051A1F9387CFD15990F
|
||||
```
|
||||
|
||||
该文件由 `tcpdump -i any` 生成,pcap linktype 为 `113`,即 Linux cooked capture。按 SLL 头解析后,确认抓到了本机到本机的 `50001` TCP JSON 通信:
|
||||
|
||||
```text
|
||||
192.168.1.100:35814 -> 192.168.1.100:50001 217B payload
|
||||
192.168.1.100:50001 -> 192.168.1.100:35814 91B payload
|
||||
```
|
||||
|
||||
客户端到服务端的完整 JSON 命令序列为:
|
||||
|
||||
```json
|
||||
{"reply_from_client":true}
|
||||
{"cmd":"SetUpRobot","robot_name":"FANUC_LR_Mate_200iD_7L"}
|
||||
{"cmd":"IsSetUp"}
|
||||
{"cmd":"SetActiveController","sim":false}
|
||||
{"cmd":"Connect","ip":"192.168.10.11"}
|
||||
{"buffer_size":8,"cmd":"EnableRobot"}
|
||||
```
|
||||
|
||||
服务端返回为:
|
||||
|
||||
```json
|
||||
{"test_from_server": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
```
|
||||
|
||||
这说明本次抓包确实覆盖到了旧 `50001` 控制链路,但当前只包含机器人初始化、连接和使能流程。里面没有看到:
|
||||
|
||||
- `ExecuteFlyShotTraj`
|
||||
- `SaveTrajInfo`
|
||||
- `IsFlyShotTrajValid`
|
||||
- `GetJointLimits / SetJointLimits`
|
||||
- `SetVelocityLimit / SetAccelerationLimit / SetJerkLimit`
|
||||
- 任何包含 `acc_limit / jerk_limit / JointLimits / velocity / acceleration / jerk` 的配置 JSON
|
||||
|
||||
阶段结论:
|
||||
|
||||
```text
|
||||
all-50001.pcap 已经证明抓包接口选对了;
|
||||
但这次只抓到了初始化链路,没有抓到规划/保存轨迹那一刻的 50001 请求。
|
||||
```
|
||||
|
||||
该待确认点已由下一节 `all-50001-plan.pcap` 覆盖:后续抓包确实抓到了 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)`,仍未出现规划限制字段。
|
||||
|
||||
## `all-50001-plan.pcap` 规划执行抓包复核
|
||||
|
||||
2026-04-30 追加复核规划/执行动作期间的本机 50001 抓包:
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/all-50001-plan.pcap
|
||||
SHA256=311DC45B4789ED11EBEAB7A396E2EE7A16EC8534E20F10127FB43BBAD823C21D
|
||||
```
|
||||
|
||||
该抓包同样是 `tcpdump -i any` 生成的 Linux cooked capture,已按 SLL 头解析。有效 TCP JSON 流为:
|
||||
|
||||
```text
|
||||
192.168.1.100:35814 -> 192.168.1.100:50001 2612B payload
|
||||
192.168.1.100:50001 -> 192.168.1.100:35814 516B payload
|
||||
```
|
||||
|
||||
客户端到服务端的关键命令序列为:
|
||||
|
||||
```json
|
||||
{"cmd":"ListFlyShotTraj"}
|
||||
{"cmd":"GetNextListFlyShotTraj","count":0}
|
||||
{"cmd":"SetSpeedRatio","ratio":0.5}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
{"cmd":"SetSpeedRatio","ratio":1.0}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
{"cmd":"StartUploadFlyShotTraj","name":"UTTC_MS11"}
|
||||
{"cmd":"UploadFlyShotTraj", "...":"4 批共 20 个 waypoint,每批包含 waypoints / shot_flags / offset_values / addrs"}
|
||||
{"cmd":"EndUploadFlyShotTraj","name":"UTTC_MS11"}
|
||||
{"cmd":"ListFlyShotTraj"}
|
||||
{"cmd":"GetNextListFlyShotTraj","count":0}
|
||||
```
|
||||
|
||||
两次执行请求均为:
|
||||
|
||||
```json
|
||||
{
|
||||
"cmd": "ExecuteFlyShotTraj",
|
||||
"method": "icsp",
|
||||
"move_to_start": true,
|
||||
"name": "UTTC_MS11_TEST01",
|
||||
"save_traj": true,
|
||||
"use_cache": false,
|
||||
"wait": true
|
||||
}
|
||||
```
|
||||
|
||||
它们前面的速度倍率分别为:
|
||||
|
||||
```text
|
||||
第一次:SetSpeedRatio ratio=0.5
|
||||
第二次:SetSpeedRatio ratio=1.0
|
||||
```
|
||||
|
||||
服务端对所有命令均返回:
|
||||
|
||||
```json
|
||||
{"res": true}
|
||||
```
|
||||
|
||||
这份抓包确认了两点:
|
||||
|
||||
1. 公开 50001 JSON 链路确实会把 `SetSpeedRatio` 和 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 发给旧服务端。
|
||||
2. 即便覆盖到了实际执行/保存轨迹动作,请求中仍没有出现 `GetJointLimits / SetJointLimits`、`SetVelocityLimit / SetAccelerationLimit / SetJerkLimit`,也没有 `acc_limit / jerk_limit / velocity / acceleration / jerk / JointLimits` 等规划限制字段。
|
||||
|
||||
因此,当前能从 50001 抓包确认的是:
|
||||
|
||||
```text
|
||||
规划方法、是否保存轨迹、是否使用缓存、是否等待执行,都会显式发到旧服务端;
|
||||
速度倍率通过 SetSpeedRatio 单独发到旧服务端;
|
||||
但 effective JointLimits 没有通过这次公开 50001 JSON 请求显式传入。
|
||||
```
|
||||
|
||||
这进一步收敛了差异来源:如果旧系统规划时确实使用了更保守的 joint limits,它更可能来自旧服务端在 `SetUpRobot("FANUC_LR_Mate_200iD_7L")` 后加载/初始化的内部状态,或来自 GUI/服务端内部私有路径,而不是这次 50001 公开 JSON 在 `ExecuteFlyShotTraj` 请求中传入的字段。
|
||||
|
||||
## Joint3/Joint2 couple A/B 测试
|
||||
|
||||
2026-04-30 追加测试:为了验证 `.robot` 中 `Joint3` 对 `Joint2` 的 couple 是否是规划时长差异主因,使用 Python ICSP demo 做了多组只读 A/B。
|
||||
|
||||
测试模型来自:
|
||||
|
||||
```text
|
||||
flyshot-replacement/Config/Models/LR_Mate_200iD_7L.robot
|
||||
```
|
||||
|
||||
其中 `Joint3` 的 couple 信息为:
|
||||
|
||||
```text
|
||||
q3_kin = q3_raw + q2_kin * 1.0 + 0.0
|
||||
```
|
||||
|
||||
测试变体:
|
||||
|
||||
- `raw`:原始 6 轴路点直接规划。
|
||||
- `replace_q3=q3+q2`:规划输入中把第 3 轴替换为耦合后的运动学角。
|
||||
- `replace_q3=q3-q2`:反向符号试探,排除符号理解错误。
|
||||
- `raw+constraint(q3+q2)`:保留原始 6 轴,同时追加虚拟约束轴 `q3+q2`,用 Joint3 的 `vel/acc/jerk` 限值检查。
|
||||
- `raw+constraint(q3-q2)`:反向符号的虚拟约束轴试探。
|
||||
|
||||
结果:
|
||||
|
||||
| 样本 | 真实时长 | 最接近变体 | 变体时长 | 变体/真实 | 与真实差值 | 结论 |
|
||||
| --- | ---: | --- | ---: | ---: | ---: | --- |
|
||||
| `UTTC_MS11` | `7.403046s` | `raw` | `5.495112s` | `0.742277` | `1.907934s` | couple 变体全部更短,且破坏原本严格等比例关系 |
|
||||
| `EOL10_EAU_0` | `14.849788s` | `replace_q3=q3+q2` | `10.600711s` | `0.713863` | `4.249077s` | couple 只改善约 `0.11s`,距离真实仍差 `4.25s` |
|
||||
| `EOL9_EAU_90` | `6.400851s` | `raw+constraint(q3+q2)` | `5.748560s` | `0.898093` | `0.652291s` | couple 约束有小幅影响,但仍不足以解释真实时长 |
|
||||
|
||||
关键观察:
|
||||
|
||||
1. `UTTC_MS11` 的 `raw` 规划时间和真实时间保持严格等比例,`point_ratio_std=0`、`segment_ratio_std≈0`;加入 couple 后反而出现分段比例波动。
|
||||
2. `EOL10_EAU_0` 与 `EOL9_EAU_90` 的 couple 变体只带来小幅时长变化,不能解释 10% 到 30% 级别的差异。
|
||||
3. 因此,当前证据不支持“只要把 Joint3/Joint2 couple 带入 ICSP,就能对齐旧 RVBUST 规划时长”。
|
||||
|
||||
阶段结论:
|
||||
|
||||
`Joint3` couple 确实是 C# 与 Python demo 当前都没有进入规划约束的缺口,但它不像本轮时长 mismatch 的主因。它更可能影响 FK/运动学边界或少数局部段约束;当前主要时长差异仍更像有效 joint limits、旧系统运行期规划倍率、或 RPS 内部 ICSP 参数来源不同。
|
||||
|
||||
## 同模型复核与更可能的差异层
|
||||
|
||||
2026-04-30 继续复核:
|
||||
|
||||
1. 当前仓库固化的模型与旧 `FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot` 字节哈希一致。
|
||||
2. `ControllerClientCompatRobotCatalog` 当前会把 `FANUC_LR_Mate_200iD` 和 `FANUC_LR_Mate_200iD_7L` 都映射到 `LR_Mate_200iD_7L.robot`。
|
||||
3. `LR_Mate_200iD.robot` 短臂模型的前三轴 `vel/acc/jerk` 比 `7L` 更高。用短臂模型试算会让轨迹更短,不会解释“旧系统真实导出更慢”。
|
||||
|
||||
模型 A/B:
|
||||
|
||||
| 样本 | 真实时长 | `LR_Mate_200iD_7L.robot` | `LR_Mate_200iD.robot` | 结论 |
|
||||
| --- | ---: | ---: | ---: | --- |
|
||||
| `UTTC_MS11` | `7.403046s` | `5.495112s` | `5.345600s` | 短臂模型方向更错 |
|
||||
| `EOL10_EAU_0` | `14.849788s` | `10.489800s` | `10.342456s` | 短臂模型方向更错 |
|
||||
|
||||
因此,如果现场确认机器人模型确实一致,差异层就不应继续放在 `.robot` 静态文件本身,而应放在旧服务端规划时的运行态:
|
||||
|
||||
- 服务端内部存在 `_GetJointLimits / _SetJointLimits`,说明规划消费的是一份可能被运行期覆写的 `current JointLimits`。
|
||||
- `ControllerClient.h` 的 `ExecuteFlyShotTraj(..., use_cache=false)` 明确说明旧服务端可以把计算好的轨迹保存在内存中并复用。
|
||||
- `SaveTrajInfo(name, method)` 没有 `use_cache` 参数,不能仅凭公开头文件判断它一定每次从当前配置重新规划。
|
||||
|
||||
当前更合理的解释是:
|
||||
|
||||
```text
|
||||
同一个 .robot
|
||||
-> SetUpRobot 初始化基础 JointLimits
|
||||
-> 旧服务端运行期间可能被 _SetJointLimits / 速度倍率联动 / 缓存轨迹 覆盖
|
||||
-> SaveTrajInfo 或 IsFlyShotTrajValid(save_traj=true) 导出的是真正规划时那份状态
|
||||
-> 当前 C# 每次用静态 .robot + RobotConfig 重新规划,所以时长更短
|
||||
```
|
||||
|
||||
尤其需要注意:`EOL10_EAU_0` 的 `新规划/真实` 比例为 `0.706394`,接近 `0.7`;`EOL9_EAU_90` 的比例为 `0.882873`,接近 `0.9`。这不像模型误差,更像历史导出时混入了某个运行态速度/限制倍率。`UTTC_MS11` 的 `0.742277` 不等于抓包确认的执行层 `0.7`,所以不能简单把所有样本都归因到 `SetSpeedRatio`,但“运行态规划约束不是静态模型值”仍是目前最强方向。
|
||||
|
||||
## 旧服务端与 GUI 二进制复核
|
||||
|
||||
2026-04-30 继续从旧系统二进制字符串中复核,重点看公开 Python/HTTP 层没有暴露出来的运行态对象。
|
||||
|
||||
### 服务端确实持有 runtime JointLimits
|
||||
|
||||
`../FlyingShot/FlyingShot/Python/ControllerServer/ControllerServer.cpython-37m-x86_64-linux-gnu.so` 中能稳定看到以下方法和关键字:
|
||||
|
||||
```text
|
||||
ControllerServer.ControllerServer._GetJointLimits
|
||||
ControllerServer.ControllerServer._SetJointLimits
|
||||
ControllerServer.ControllerServer._IsWaypointInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJerkLimits
|
||||
ControllerServer.ControllerServer._ExecuteFlyShotTraj
|
||||
ControllerServer.ControllerServer._SaveTrajInfo
|
||||
ControllerServer.ControllerServer._IsFlyShotTrajValid
|
||||
SetVelocityLimit
|
||||
SetAccelerationLimit
|
||||
SetJerkLimit
|
||||
GetMaxVelocity
|
||||
GetMaxAcceleration
|
||||
GetMaxJerk
|
||||
m_acc_limit
|
||||
m_jerk_limit
|
||||
save_traj_only
|
||||
use_cache
|
||||
```
|
||||
|
||||
这比公开 `ControllerClient.h` 暴露的信息更多。它说明旧服务端内部不是只把 `.robot` 静态值直接传给 `TrajectoryRnICSP`,而是存在一份可以查询、设置、校验、再用于规划的运行期 `JointLimits`。
|
||||
|
||||
### GUI 也直接接触规划约束与保存逻辑
|
||||
|
||||
旧 GUI 二进制里也能看到同一条链:
|
||||
|
||||
- `../FlyingShot/FlyingShot/Python/GUI/Robot/RobotManager.cpython-37m-x86_64-linux-gnu.so`
|
||||
- `GetJointLimits`
|
||||
- `TrajectoryRnICSP`
|
||||
- `IsTrajInJointLimits`
|
||||
- `IsTrajInJerkLimits`
|
||||
- `acc_limit`
|
||||
- `jerk_limit`
|
||||
- `../FlyingShot/FlyingShot/Python/GUI/Robot/RobotConfig.cpython-37m-x86_64-linux-gnu.so`
|
||||
- `SaveTraj`
|
||||
- `m_acc_limit`
|
||||
- `m_jerk_limit`
|
||||
- `../FlyingShot/FlyingShot/Python/GUI/Panels/FlyshotDockPanel.cpython-37m-x86_64-linux-gnu.so`
|
||||
- `__SaveTraj`
|
||||
- `IsTrajInJointLimits`
|
||||
- `IsTrajInJerkLimits`
|
||||
- `m_acc_limit`
|
||||
- `m_jerk_limit`
|
||||
|
||||
这说明旧 GUI 的“保存轨迹/检查轨迹”路径很可能不是简单调用公开 `ControllerClient.SaveTrajInfo` 后结束,而是直接拿当前 `JointLimits + acc_limit + jerk_limit` 做规划、合法性检查或保存。
|
||||
|
||||
### UAES 接口没有显式对齐 JointLimits
|
||||
|
||||
`../flyshot-uaes-interface/main.py` 中 `/execute_flyshot/` 的执行路径是:
|
||||
|
||||
```text
|
||||
c.ExecuteFlyShotTraj(name=name, move_to_start=True, method="icsp", save_traj=True)
|
||||
```
|
||||
|
||||
`/set_speedRatio/` 是单独接口:
|
||||
|
||||
```text
|
||||
c.SetSpeedRatio(speed)
|
||||
```
|
||||
|
||||
同时,`../flyshot-uaes-interface/lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so` 和 `../flyshot-uaes-interface/lib/libControllerClient.so` 中只看到公开客户端侧的:
|
||||
|
||||
```text
|
||||
GetSpeedRatio
|
||||
SetSpeedRatio
|
||||
ExecuteFlyShotTraj
|
||||
SaveTrajInfo
|
||||
IsFlyShotTrajValid
|
||||
JointLimits
|
||||
```
|
||||
|
||||
没有看到客户端侧 `GetJointLimits / SetJointLimits` 符号。也就是说,UAES Python 服务本身大概率没有主动把旧服务端的 runtime JointLimits 设置成某个值;如果现场旧导出时的 limits 被改过,更可能来自:
|
||||
|
||||
- 旧 GUI 初始化/保存路径;
|
||||
- 旧服务端内部默认初始化;
|
||||
- 服务端隐藏 TCP JSON 方法;
|
||||
- 历史上某次执行/保存后留下的缓存结果。
|
||||
|
||||
### 样本文件与配置文件可能不是同一次运行态
|
||||
|
||||
新增一个需要警惕的现象:
|
||||
|
||||
- `../Rvbust/EOL9 EAU 0/eol9_eau_0.json` 中 `acc_limit=1`、`jerk_limit=1`。
|
||||
- `../Rvbust/EOL9 EAU 90/eol9_eau_90.json` 中 `acc_limit=0.8`、`jerk_limit=0.8`。
|
||||
- 但两个目录下保存的真实 `JointTraj.txt` 内容和时长一致。
|
||||
|
||||
哈希复核:
|
||||
|
||||
```text
|
||||
EOL9 EAU 0 JointTraj.txt SHA256=DFD8E1130742CFB4ED72F70D0E8CA4E3A16F421E0D0D9D921B9F5177717536EC
|
||||
EOL9 EAU 90 JointTraj.txt SHA256=DFD8E1130742CFB4ED72F70D0E8CA4E3A16F421E0D0D9D921B9F5177717536EC
|
||||
eol9_eau_0.json SHA256=354D0D3F71499951976504802C4B2860132D1E4FF753738715A500529CD0BB68
|
||||
eol9_eau_90.json SHA256=7F854AA227D842CAE734AFA378FEEFA742D797F99FBE536E1B98DF981CD32B27
|
||||
```
|
||||
|
||||
这说明不能默认认为“某个 JSON 文件当前内容”就一定是旁边 `Data/JointTraj.txt` 的生成状态。旧系统的保存文件可能来自缓存、拷贝、历史运行态,或 GUI/服务端中未落盘到该 JSON 的当前 `JointLimits`。
|
||||
|
||||
本轮新增证据把方向进一步收敛为:
|
||||
|
||||
```text
|
||||
同一个 .robot 文件本身不是问题核心;
|
||||
真正影响时长的是旧系统规划瞬间的 effective JointLimits;
|
||||
但这份状态没有出现在现有配置、机器人侧抓包或 50001 公开 JSON 中。
|
||||
```
|
||||
|
||||
如果未来能直接进入旧服务端进程,仍可在 `SaveTrajInfo` / `IsFlyShotTrajValid(save_traj=true)` 前后抓取 `_GetJointLimits` 返回值,并把它与 `.robot` 原始 `vel/acc/jerk` 和当前 JSON 的 `acc_limit/jerk_limit` 做数值对比。但这不再阻塞 replacement 的现场对齐:当前设计默认用显式内部规划加速度参数补齐这份不可见状态。
|
||||
|
||||
## 当前判断
|
||||
|
||||
当前最可信的解释是:
|
||||
|
||||
1. 旧 RVBUST/FlyingShot 生成真实 `JointTraj.txt` 时,规划阶段使用的有效 joint limits 并不总是 `.robot` 文件中的原始 `velocity / acceleration / jerk`。
|
||||
2. 这些有效 joint limits 可能来自服务运行期状态,例如旧服务端内部的 `_SetJointLimits`、上层 GUI/脚本初始化流程、机器人环境配置,或其他未落入当前 JSON 文件的运行时参数。
|
||||
3. 现有现场 JSON 中只明确保存了:
|
||||
- `acc_limit`
|
||||
- `jerk_limit`
|
||||
- `adapt_icsp_try_num`
|
||||
- IO 相关配置
|
||||
4. 已重新抓取机器人侧 `10010/10012/60015` 和本机 `50001/TCP+JSON`,仍没有看到 `JointLimits / velocity / acceleration / jerk / acc_limit / jerk_limit` 通过公开链路在规划时下发。
|
||||
5. 目前没有证据表明现场配置文件或公开 TCP JSON 显式保存了一个“规划速度倍率”或“规划加速度限制”。
|
||||
|
||||
因此,`0.742277` 不应被理解为固定业务常量。它只是 `UTTC_MS11` 在当前 C# 默认约束和真实导出结果之间反推出来的等效规划倍率。
|
||||
|
||||
## 兼容设计决策
|
||||
|
||||
由于重新抓包后仍抓不到旧系统的 effective limits,新系统后续不再继续假设公开链路会传入这份数据,而是采用 replacement-only 的显式规划约束参数补齐不可见状态。
|
||||
|
||||
参数分层如下:
|
||||
|
||||
1. `acc_limit / jerk_limit`
|
||||
- 来源:旧 `RobotConfig.json` 中已经存在的字段。
|
||||
- 语义:继续作为旧配置的基础倍率,参与 `.robot` 模型加载。
|
||||
- 限制:现场样本中 `acc_limit=1`、`jerk_limit=1` 时,不能解释旧导出轨迹更慢的问题。
|
||||
|
||||
2. `planning_acceleration_scale`
|
||||
- 来源:新系统内部兼容参数,不声称来自旧 RVBUST 配置或抓包。
|
||||
- 语义:只用于规划阶段,额外缩放 `JointLimit.AccelerationLimit`,用于复现旧服务端不可见的保守加速度约束。
|
||||
- 默认值:`1.0`,表示不额外限制。
|
||||
- 现场校准:若按纯加速度限制解释 `UTTC_MS11_TEST01`,可先用 `(5.814370 / 7.805885)^2 ≈ 0.5548` 作为候选起点,再用真实 `JointTraj.txt` 对拍确认。
|
||||
|
||||
3. `planning_speed_scale`
|
||||
- 来源:当前 C# 已支持的显式兼容字段。
|
||||
- 语义:把整条规划时间轴按速度倍率解释,联动缩放速度、加速度、jerk。
|
||||
- 定位:保留为临时整体验证开关;当后续落地 `planning_acceleration_scale` 后,现场默认优先使用加速度限制参数,而不是把 `planning_speed_scale` 当成旧系统事实。
|
||||
|
||||
当前 C# 已支持的 `planning_speed_scale` 形式为:
|
||||
|
||||
```json
|
||||
{
|
||||
"robot": {
|
||||
"planning_speed_scale": 0.742277
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
该字段只用于规划阶段:
|
||||
|
||||
- `vel *= planning_speed_scale`
|
||||
- `acc *= planning_speed_scale^2`
|
||||
- `jerk *= planning_speed_scale^3`
|
||||
|
||||
它不等同于运行时 `/set_speedRatio/`,也不改变 J519 的 8ms 发送周期。运行阶段仍按:
|
||||
|
||||
```text
|
||||
t_send = k * 0.008
|
||||
t_traj = t_send * speed_ratio
|
||||
```
|
||||
|
||||
从已生成轨迹中重采样。
|
||||
|
||||
由于现场真实配置和本轮抓包中都没有找到这类倍率,所有 `planning_*` 字段都必须标注为 replacement-only 兼容校准参数,不能声称它们来自旧配置文件或公开 TCP JSON。
|
||||
|
||||
## 后续设计方向
|
||||
|
||||
1. 默认不再把运行时 `speed_ratio` 混入 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 的规划时间计算。
|
||||
2. 后续实现优先新增 `planning_acceleration_scale`,只限制规划加速度,并将其写入 `RobotConfig.json` 的 `robot` 节点或当前现场默认配置。
|
||||
3. 若只需快速对齐整条时间轴,可临时使用现有 `planning_speed_scale`;但文档、日志和配置说明必须标注它是新系统校准值。
|
||||
4. 如果未来能直接调用旧服务端 `_GetJointLimits`,再用返回值替换当前反推参数;在此之前,显式内部参数是当前可控且可审计的兼容策略。
|
||||
202
docs/python-client-interface-reverse-engineering.md
Normal file
202
docs/python-client-interface-reverse-engineering.md
Normal file
@@ -0,0 +1,202 @@
|
||||
# Python ControllerClient 接口逆向记录
|
||||
|
||||
## 背景
|
||||
|
||||
本记录用于确认旧 `PyControllerClient` 对 Python 暴露了哪些接口,尤其确认是否能通过 Python client 直接查询或设置旧服务端运行态 `JointLimits`。
|
||||
|
||||
复核对象:
|
||||
|
||||
```text
|
||||
../flyshot-uaes-interface/lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so
|
||||
../FlyingShot/FlyingShot/Lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so
|
||||
../flyshot-uaes-interface/lib/libControllerClient.so
|
||||
../FlyingShot/FlyingShot/Lib/libControllerClient.so
|
||||
../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h
|
||||
../FlyingShot/FlyingShot/Include/ControllerClient/Types.h
|
||||
../flyshot-uaes-interface/UseControllerClient.py
|
||||
../flyshot-uaes-interface/main.py
|
||||
```
|
||||
|
||||
两份 Python 扩展与两份底层 client 库哈希一致:
|
||||
|
||||
```text
|
||||
PyControllerClient.cpython-37m-x86_64-linux-gnu.so
|
||||
SHA256=648CC23CBC6DF83822B58AC4A10211EE1DF8029AD8933D31032187748DF7F4BC
|
||||
|
||||
libControllerClient.so
|
||||
SHA256=6D6FD3F20F0791F1CF11EEE5B1D479E2DCB6A1A2C8AB00A1165575BAB4B62813
|
||||
```
|
||||
|
||||
因此 `flyshot-uaes-interface/lib` 与 `FlyingShot/FlyingShot/Lib` 中的 Python client 可视为同一份接口。
|
||||
|
||||
## 暴露的 Python 类型
|
||||
|
||||
`PyControllerClient` 暴露以下类型:
|
||||
|
||||
| 类型 | 来源 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `ControllerClient` | `ControllerClient.h` | TCP JSON client,高层控制入口 |
|
||||
| `JointPositions` | `Types.h` | 关节位置容器,可用 6 维列表构造,也支持下标读写 |
|
||||
| `Pose` | `Types.h` | TCP/末端位姿容器,C++ 侧为 7 元数组 |
|
||||
| `JointLimits` | `Types.h` | 关节上下限、速度、加速度、jerk 容器 |
|
||||
| `IOType` | `Types.h` | IO 枚举 |
|
||||
|
||||
`IOType` 的枚举值:
|
||||
|
||||
```text
|
||||
IOType.kIOTypeDI = 1
|
||||
IOType.kIOTypeDO = 2
|
||||
IOType.kIOTypeRI = 8
|
||||
IOType.kIOTypeRO = 9
|
||||
```
|
||||
|
||||
## ControllerClient 暴露方法
|
||||
|
||||
二进制字符串和 C++ 公开头文件交叉确认,Python client 暴露的方法为:
|
||||
|
||||
| Python 方法 | 典型调用 | 返回形态 | 说明 |
|
||||
| --- | --- | --- | --- |
|
||||
| `ConnectServer` | `c.ConnectServer(server_ip="127.0.0.1", port=50001)` | `bool` | 连接旧 `50001/TCP+JSON` 服务端 |
|
||||
| `GetServerVersion` | `c.GetServerVersion()` | `str` | Python 包装层把 C++ out 参数折叠成返回值 |
|
||||
| `GetClientVersion` | `c.GetClientVersion()` | `str` | 获取 client 版本 |
|
||||
| `SetUpRobot` | `c.SetUpRobot("FANUC_LR_Mate_200iD")` | `bool` | 按机器人名称初始化服务端机器人模型 |
|
||||
| `SetUpRobotFromEnv` | `c.SetUpRobotFromEnv(env_file)` | `bool` | 从环境文件初始化 |
|
||||
| `IsSetUp` | `c.IsSetUp()` | `bool` | 判断服务端是否已经初始化机器人 |
|
||||
| `SetShowTCP` | `c.SetShowTCP(is_show=True, axis_length=0.1, axis_size=2)` | `bool` | 仿真显示 TCP 坐标系 |
|
||||
| `GetName` | `c.GetName()` | `str` | 获取机器人名称 |
|
||||
| `GetDoF` | `c.GetDoF()` | `int` | 获取自由度 |
|
||||
| `SetActiveController` | `c.SetActiveController(sim=True)` | `bool` | 切换仿真/真实控制器 |
|
||||
| `Connect` | `c.Connect("192.168.10.101")` | `bool` | 连接机器人控制器 |
|
||||
| `Disconnect` | `c.Disconnect()` | `bool` | 断开机器人控制器 |
|
||||
| `EnableRobot` | `c.EnableRobot()` / `c.EnableRobot(8)` | `bool` | 使能机器人,参数为 buffer size |
|
||||
| `DisableRobot` | `c.DisableRobot()` | `bool` | 下使能 |
|
||||
| `GetSpeedRatio` | `c.GetSpeedRatio()` | `float` | 获取执行速度倍率 |
|
||||
| `SetSpeedRatio` | `c.SetSpeedRatio(0.8)` | `bool` | 设置执行速度倍率 |
|
||||
| `GetTCP` | `res, tcp = c.GetTCP()` | `(bool, Pose)` | 获取 TCP |
|
||||
| `SetTCP` | `c.SetTCP(tcp)` | `bool` | 设置 TCP |
|
||||
| `GetIO` | `res, value = c.GetIO(port=1, io_type=IOType.kIOTypeDI)` | `(bool, bool)` | 读取 IO |
|
||||
| `SetIO` | `c.SetIO(port=1, value=True, io_type=IOType.kIOTypeDO)` | `bool` | 写 IO |
|
||||
| `StopMove` | `c.StopMove()` | `bool` | 停止运动 |
|
||||
| `GetJointPosition` | `res, joints = c.GetJointPosition()` | `(bool, JointPositions)` | 获取当前关节角 |
|
||||
| `GetPose` | `res, pose = c.GetPose()` | `(bool, Pose)` | 获取当前末端位姿 |
|
||||
| `GetNearestIK` | `res, ik = c.GetNearestIK(pose, joint_seed=joints)` | `(bool, JointPositions)` | 按 seed 求最近 IK |
|
||||
| `MoveJoint` | `c.MoveJoint(joint_positions)` | `bool` | 关节运动 |
|
||||
| `ExecuteTrajectory` | `c.ExecuteTrajectory(waypoints=[...], method="icsp", save_traj=True)` | `bool` | 执行普通轨迹 |
|
||||
| `UploadFlyShotTraj` | `c.UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)` | `bool` | 上传飞拍轨迹 |
|
||||
| `DeleteFlyShotTraj` | `c.DeleteFlyShotTraj(name)` | `bool` | 删除飞拍轨迹 |
|
||||
| `ListFlyShotTraj` | `c.ListFlyShotTraj()` | `list[str]` | 列出已上传飞拍轨迹 |
|
||||
| `ExecuteFlyShotTraj` | `c.ExecuteFlyShotTraj(name, move_to_start=True, method="icsp", save_traj=True)` | `bool` | 执行飞拍轨迹 |
|
||||
| `SaveTrajInfo` | `c.SaveTrajInfo(name, method="icsp")` | `bool` | 保存规划结果到 `~/Rvbust/Data` |
|
||||
| `IsFlyShotTrajValid` | `valid, time = c.IsFlyShotTrajValid(name, method="icsp", save_traj=True)` | `(bool, float)` | 检查飞拍轨迹是否合法并返回规划时长 |
|
||||
|
||||
## 没有暴露的关键接口
|
||||
|
||||
本轮重点确认:Python client 暴露方法中没有看到:
|
||||
|
||||
```text
|
||||
GetJointLimits
|
||||
SetJointLimits
|
||||
_GetJointLimits
|
||||
_SetJointLimits
|
||||
```
|
||||
|
||||
虽然 `PyControllerClient` 绑定了 `JointLimits` 类型,并且 `libControllerClient.so` 中存在 `JointLimits` 的输出运算符符号,但公开 `ControllerClient` 方法表中没有任何接收或返回 `JointLimits` 的 client 入口。
|
||||
|
||||
这和旧服务端二进制不同。旧服务端 `ControllerServer.cpython-37m-x86_64-linux-gnu.so` 中能看到:
|
||||
|
||||
```text
|
||||
ControllerServer.ControllerServer._GetJointLimits
|
||||
ControllerServer.ControllerServer._SetJointLimits
|
||||
ControllerServer.ControllerServer._IsWaypointInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJerkLimits
|
||||
```
|
||||
|
||||
因此当前判断是:
|
||||
|
||||
```text
|
||||
Python client 公开 API 不能直接抓 runtime JointLimits;
|
||||
runtime JointLimits 查询能力存在于旧服务端内部,而不是 PyControllerClient 公开接口中。
|
||||
```
|
||||
|
||||
## UAES Python 服务实际使用的接口
|
||||
|
||||
`../flyshot-uaes-interface/main.py` 只使用了公开 client 方法:
|
||||
|
||||
- `ConnectServer`
|
||||
- `SetUpRobot`
|
||||
- `IsSetUp`
|
||||
- `EnableRobot`
|
||||
- `DisableRobot`
|
||||
- `SetActiveController`
|
||||
- `Connect`
|
||||
- `GetName`
|
||||
- `GetServerVersion`
|
||||
- `GetDoF`
|
||||
- `GetSpeedRatio`
|
||||
- `SetTCP`
|
||||
- `GetTCP`
|
||||
- `SetIO`
|
||||
- `GetJointPosition`
|
||||
- `MoveJoint`
|
||||
- `ListFlyShotTraj`
|
||||
- `UploadFlyShotTraj`
|
||||
- `ExecuteFlyShotTraj`
|
||||
- `SetSpeedRatio`
|
||||
- `DeleteFlyShotTraj`
|
||||
- `GetPose`
|
||||
|
||||
其中 `/execute_flyshot/` 调用:
|
||||
|
||||
```text
|
||||
c.ExecuteFlyShotTraj(name=name, move_to_start=True, method="icsp", save_traj=True)
|
||||
```
|
||||
|
||||
`/set_speedRatio/` 调用:
|
||||
|
||||
```text
|
||||
c.SetSpeedRatio(speed)
|
||||
```
|
||||
|
||||
没有看到 UAES 服务通过 Python client 设置或查询 `JointLimits`。
|
||||
|
||||
2026-04-30 追加 `50001/TCP+JSON` 抓包复核后,这个判断进一步收敛。`all-50001-plan.pcap` 中已经抓到两次真实规划/执行请求:
|
||||
|
||||
```json
|
||||
{"cmd":"SetSpeedRatio","ratio":0.5}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
{"cmd":"SetSpeedRatio","ratio":1.0}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
```
|
||||
|
||||
请求中仍没有 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此公开 Python client 与公开 50001 JSON 都没有把规划限制作为参数传给 `ExecuteFlyShotTraj`。
|
||||
|
||||
另外,`main.py` 的 `/execute_trajectory/` 中出现:
|
||||
|
||||
```text
|
||||
c.yrxm(waypoints=joint_positions, method='icsp', save_traj=True)
|
||||
```
|
||||
|
||||
`yrxm` 不在 `PyControllerClient` 暴露方法表中,按上下文应是 `ExecuteTrajectory` 的笔误;这条不影响飞拍主路径 `/execute_flyshot/`。
|
||||
|
||||
## 对当前时长差异调查的含义
|
||||
|
||||
如果要抓旧系统规划时使用的 effective `vel/acc/jerk`,优先级应调整为:
|
||||
|
||||
1. 在旧服务端进程内直接调用或插桩 `_GetJointLimits`。
|
||||
2. 或者逆向 `50001/TCP+JSON` 的 hidden command envelope,再尝试发送 `GetJointLimits` / `_GetJointLimits`。
|
||||
3. 不应指望现有 `PyControllerClient.ControllerClient` 直接提供 `GetJointLimits`。
|
||||
|
||||
如果短期内无法进入旧服务端内部,新系统不再继续等待这份不可见状态;设计上使用 replacement-only 的内部规划约束参数补齐,优先限制规划加速度,例如 `planning_acceleration_scale`。该参数必须标注为新系统校准值,不能写成旧 Python client 或旧 50001 JSON 的公开字段。
|
||||
|
||||
最小现场验证脚本可以先确认 Python client 暴露面:
|
||||
|
||||
```python
|
||||
from PyControllerClient import ControllerClient
|
||||
|
||||
c = ControllerClient()
|
||||
names = [x for x in dir(c) if "Limit" in x or "limit" in x]
|
||||
print(names)
|
||||
```
|
||||
|
||||
按当前二进制逆向,预期不会出现 `GetJointLimits` / `SetJointLimits`。
|
||||
62
docs/robot-joint-limit-table-20260505.md
Normal file
62
docs/robot-joint-limit-table-20260505.md
Normal file
@@ -0,0 +1,62 @@
|
||||
# 机器人六轴限值提取表
|
||||
|
||||
记录时间:2026-05-05
|
||||
|
||||
## 1. 目的
|
||||
|
||||
本文档把当前机器人模型中的六轴基础限值整理成一份固定表格,明确区分以下几类信息:
|
||||
|
||||
- 模型原始值:来自 `LR_Mate_200iD_7L_clean.json` 中每个关节的 `limit.velocity / limit.acceleration / limit.jerk / limit.effort`
|
||||
- 配置倍率:来自当前运行配置 `Config/RobotConfig.json` 中的 `acc_limit / jerk_limit`
|
||||
- 运行时有效值:模型原始值叠加当前配置倍率后的结果
|
||||
|
||||
本表只覆盖六个旋转关节 `Joint1` 到 `Joint6`。
|
||||
`JointEffector` 属于末端固定关节,不计入“每个轴”的速度、加速度、跃度统计。
|
||||
|
||||
## 2. 当前取值规则
|
||||
|
||||
当前仓库运行时对六轴限值的读取规则是:
|
||||
|
||||
- `velocity_eff = velocity_base`
|
||||
- `acceleration_eff = acceleration_base * acc_limit`
|
||||
- `jerk_eff = jerk_base * jerk_limit`
|
||||
|
||||
当前 `Config/RobotConfig.json` 中的倍率为:
|
||||
|
||||
- `acc_limit = 1`
|
||||
- `jerk_limit = 1`
|
||||
|
||||
因此本次表格中的“基础值”和“有效值”数值相同。
|
||||
|
||||
## 3. 六轴限值表
|
||||
|
||||
| Joint | velocity_base | acceleration_base | jerk_base | effort_raw | acc_limit | jerk_limit | velocity_eff | acceleration_eff | jerk_eff | 备注 |
|
||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: | --- |
|
||||
| Joint1 | 6.45 | 26.90 | 224.22 | 0.0 | 1 | 1 | 6.45 | 26.90 | 224.22 | 模型字段存在,`effort` 当前为 0 |
|
||||
| Joint2 | 5.41 | 22.54 | 187.86 | 0.0 | 1 | 1 | 5.41 | 22.54 | 187.86 | 模型字段存在,`effort` 当前为 0 |
|
||||
| Joint3 | 7.15 | 29.81 | 248.46 | 0.0 | 1 | 1 | 7.15 | 29.81 | 248.46 | 模型字段存在,`effort` 当前为 0 |
|
||||
| Joint4 | 9.59 | 39.99 | 333.30 | 0.0 | 1 | 1 | 9.59 | 39.99 | 333.30 | 模型字段存在,`effort` 当前为 0 |
|
||||
| Joint5 | 9.51 | 39.63 | 330.27 | 0.0 | 1 | 1 | 9.51 | 39.63 | 330.27 | 模型字段存在,`effort` 当前为 0 |
|
||||
| Joint6 | 17.45 | 72.72 | 606.01 | 0.0 | 1 | 1 | 17.45 | 72.72 | 606.01 | 模型字段存在,`effort` 当前为 0 |
|
||||
|
||||
## 4. 关于“电流信息”的说明
|
||||
|
||||
这份模型文件里确实有 `limit.effort` 字段,但当前证据只能说明:
|
||||
|
||||
- 它是模型文件中的一个静态字段
|
||||
- 当前六轴以及 `JointEffector` 的 `effort` 都是 `0.0`
|
||||
- 当前 `flyshot-replacement` 代码链路没有把它当作实时电流来源来使用
|
||||
|
||||
因此当前结论应固定为:
|
||||
|
||||
- `velocity / acceleration / jerk` 可以从这份模型文档中提取
|
||||
- `effort` 只能当作模型原始字段记录
|
||||
- `effort` 不能直接解释为现场真实电流,也不能替代 J519 反馈中的电机电流数据
|
||||
|
||||
## 5. 后续使用约定
|
||||
|
||||
后续如果有人问“每个轴的速度、加速度、跃度是不是从这个文档来的”,默认回答应为:
|
||||
|
||||
- 是,六轴基础限值来自模型文件中的 `joint.limit`
|
||||
- 运行时有效加速度和有效跃度还要再乘 `RobotConfig.json` 的全局倍率
|
||||
- 不是,真实电流不要从这个文件里的 `effort` 去推断
|
||||
@@ -0,0 +1,272 @@
|
||||
# Minimal Runtime Orchestration Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Build the first real execution slice after the HTTP-only refactor by routing `ExecuteTrajectory` and `ExecuteFlyShotTraj` through planning, triggering, and a new minimal FANUC runtime project instead of in-memory last-point assignment.
|
||||
|
||||
**Architecture:** Keep `Flyshot.Server.Host` as a pure HTTP adapter and keep uploaded program state in `Flyshot.ControllerClientCompat`, but move controller runtime state into a new `Flyshot.Runtime.Fanuc` project and add a focused planning/orchestration helper in `Flyshot.ControllerClientCompat`. Ordinary trajectory execution will use `ICspPlanner`; uploaded flyshot execution will use `SelfAdaptIcspPlanner` plus `ShotTimelineBuilder`, then hand the resulting `TrajectoryResult` to the runtime.
|
||||
|
||||
**Tech Stack:** C#, .NET 8, xUnit, existing `Flyshot.Core.Domain`, `Flyshot.Core.Planning`, `Flyshot.Core.Triggering`, ASP.NET Core DI.
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Add Runtime Contracts And Minimal FANUC Runtime
|
||||
|
||||
**Files:**
|
||||
- Create: `src/Flyshot.Runtime.Common/IControllerRuntime.cs`
|
||||
- Create: `src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj`
|
||||
- Create: `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||||
- Modify: `FlyshotReplacement.sln`
|
||||
- Modify: `src/Flyshot.Server.Host/Flyshot.Server.Host.csproj`
|
||||
- Modify: `src/Flyshot.Server.Host/Program.cs`
|
||||
- Test: `tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs`
|
||||
|
||||
- [x] **Step 1: Write the failing runtime test**
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
|
||||
{
|
||||
var runtime = new FanucControllerRuntime();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
runtime.SetActiveController(sim: false);
|
||||
runtime.Connect("192.168.10.101");
|
||||
runtime.EnableRobot(bufferSize: 2);
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(1.2),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
Assert.Equal("Connected", snapshot.ConnectionState);
|
||||
Assert.False(snapshot.IsInMotion);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because `FanucControllerRuntime` and `IControllerRuntime` do not exist.
|
||||
|
||||
- [x] **Step 3: Write the minimal runtime contracts and implementation**
|
||||
|
||||
```csharp
|
||||
public interface IControllerRuntime
|
||||
{
|
||||
void ResetRobot(RobotProfile robot, string robotName);
|
||||
void SetActiveController(bool sim);
|
||||
void Connect(string robotIp);
|
||||
void Disconnect();
|
||||
void EnableRobot(int bufferSize);
|
||||
void DisableRobot();
|
||||
void StopMove();
|
||||
double GetSpeedRatio();
|
||||
void SetSpeedRatio(double ratio);
|
||||
IReadOnlyList<double> GetTcp();
|
||||
void SetTcp(double x, double y, double z);
|
||||
bool GetIo(int port, string ioType);
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
IReadOnlyList<double> GetPose();
|
||||
ControllerStateSnapshot GetSnapshot();
|
||||
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
|
||||
}
|
||||
```
|
||||
|
||||
```csharp
|
||||
public sealed class FanucControllerRuntime : IControllerRuntime
|
||||
{
|
||||
// Stage-1 runtime: owns controller state in one place so later sockets can replace internals without rewriting compat service.
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 4: Run test to verify it passes**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
### Task 2: Add Planning And Triggering Orchestration For Execution
|
||||
|
||||
**Files:**
|
||||
- Create: `src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs`
|
||||
- Create: `src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs`
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj`
|
||||
- Modify: `tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj`
|
||||
- Test: `tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs`
|
||||
|
||||
- [x] **Step 1: Write the failing orchestration tests**
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
orchestrator.PlanOrdinaryTrajectory(robot,
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]));
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||||
|
||||
Assert.True(bundle.Result.IsValid);
|
||||
Assert.Single(bundle.Result.ShotEvents);
|
||||
Assert.Single(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run tests to verify they fail**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientTrajectoryOrchestrator -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because the orchestrator types do not exist.
|
||||
|
||||
- [x] **Step 3: Write the minimal orchestration layer**
|
||||
|
||||
```csharp
|
||||
public sealed class PlannedExecutionBundle
|
||||
{
|
||||
public PlannedExecutionBundle(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, TrajectoryResult result)
|
||||
{
|
||||
PlannedTrajectory = plannedTrajectory;
|
||||
ShotTimeline = shotTimeline;
|
||||
Result = result;
|
||||
}
|
||||
|
||||
public PlannedTrajectory PlannedTrajectory { get; }
|
||||
public ShotTimeline ShotTimeline { get; }
|
||||
public TrajectoryResult Result { get; }
|
||||
}
|
||||
```
|
||||
|
||||
```csharp
|
||||
public sealed class ControllerClientTrajectoryOrchestrator
|
||||
{
|
||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> waypoints) { ... }
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded) { ... }
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 4: Run tests to verify they pass**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientTrajectoryOrchestrator -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
### Task 3: Rewire ControllerClientCompatService To Runtime + Orchestrator
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs`
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs`
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs`
|
||||
- Modify: `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs`
|
||||
- Modify: `tests/Flyshot.Server.IntegrationTests/ControllerClientCompatRegistrationTests.cs`
|
||||
- Test: `tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs`
|
||||
- Test: `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs`
|
||||
|
||||
- [x] **Step 1: Write the failing compat-service test**
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
|
||||
{
|
||||
var service = TestRobotFactory.CreateCompatService();
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
service.ExecuteTrajectory(
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]));
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because current service still treats ordinary execution as "move to last waypoint".
|
||||
|
||||
- [x] **Step 3: Rewire service to the runtime and orchestrator**
|
||||
|
||||
```csharp
|
||||
public sealed class ControllerClientCompatService : IControllerClientCompatService
|
||||
{
|
||||
private readonly IControllerRuntime _runtime;
|
||||
private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
|
||||
|
||||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints);
|
||||
_runtime.ExecuteTrajectory(bundle.Result, bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions);
|
||||
}
|
||||
|
||||
public void ExecuteTrajectoryByName(string name)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
var uploaded = RequireUploadedTrajectory(name);
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||||
_runtime.ExecuteTrajectory(bundle.Result, bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 4: Run focused tests to verify green**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter "ControllerClientCompatService|ControllerClientTrajectoryOrchestrator|FanucControllerRuntime" -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
- [x] **Step 5: Run integration verification**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS, with existing HTTP compatibility tests still green.
|
||||
|
||||
### Task 4: Verify Solution Build And Update Progress Docs
|
||||
|
||||
**Files:**
|
||||
- Modify: `README.md`
|
||||
- Modify: `AGENTS.md`
|
||||
- Modify: `docs/controller-client-api-compatibility-requirements.md`
|
||||
|
||||
- [x] **Step 1: Update docs to reflect the new stage**
|
||||
|
||||
```markdown
|
||||
- [x] 落地最小 FANUC 运行时骨架
|
||||
- [x] 将 ExecuteTrajectory / ExecuteFlyShotTraj 接入 Planning + Triggering + Runtime
|
||||
- [x] 落地 Web 状态页
|
||||
- [x] 固化 10010 / 10012 / 60015 FANUC 基础协议帧编解码
|
||||
- [ ] 落地真实 10010 / 10012 / 60015 Socket 通讯与现场联调
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run final build**
|
||||
|
||||
Run: `dotnet build FlyshotReplacement.sln --no-restore -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS with 0 errors.
|
||||
6
global.json
Normal file
6
global.json
Normal file
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"sdk": {
|
||||
"version": "8.0.420",
|
||||
"rollForward": "disable"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 HTTP-only ControllerClient 兼容层的基础配置。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置对外暴露的服务端版本号。
|
||||
/// </summary>
|
||||
public string ServerVersion { get; set; } = "flyshot-replacement-controller-client-compat/0.1.0";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置运行配置根目录;为空时默认使用程序基目录下的 Config。
|
||||
/// </summary>
|
||||
public string? ConfigRoot { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置旧父工作区根目录;仅用于测试或旧样本显式兼容。
|
||||
/// </summary>
|
||||
public string? WorkspaceRoot { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 解析运行配置根目录,确保运行时默认不再依赖源码仓库位置。
|
||||
/// </summary>
|
||||
/// <returns>运行配置根目录的绝对路径。</returns>
|
||||
public string ResolveConfigRoot()
|
||||
{
|
||||
var root = string.IsNullOrWhiteSpace(ConfigRoot)
|
||||
? Path.Combine(AppContext.BaseDirectory, "Config")
|
||||
: ConfigRoot;
|
||||
|
||||
return Path.GetFullPath(root);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析显式配置的旧父工作区根目录;未配置时返回 null。
|
||||
/// </summary>
|
||||
/// <returns>旧父工作区根目录的绝对路径,或 null。</returns>
|
||||
public string? ResolveLegacyWorkspaceRoot()
|
||||
{
|
||||
return string.IsNullOrWhiteSpace(WorkspaceRoot)
|
||||
? null
|
||||
: Path.GetFullPath(WorkspaceRoot);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧版 ControllerClient 的机器人名称,解析当前 replacement 仓库支持的真实模型文件。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatRobotCatalog
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存当前现场支持的机器人名称到运行目录 JSON 模型文件名映射。
|
||||
/// </summary>
|
||||
private static readonly IReadOnlyDictionary<string, string> SupportedRobotModelFileMap = new Dictionary<string, string>(StringComparer.Ordinal)
|
||||
{
|
||||
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.json",
|
||||
["FANUC_LR_Mate_200iD_7L"] = "LR_Mate_200iD_7L.json"
|
||||
};
|
||||
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotModelLoader _robotModelLoader;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化机器人兼容目录解析器。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置。</param>
|
||||
/// <param name="robotModelLoader">机器人 JSON 模型加载器。</param>
|
||||
public ControllerClientCompatRobotCatalog(
|
||||
ControllerClientCompatOptions options,
|
||||
RobotModelLoader robotModelLoader)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端的机器人名称加载对应模型。
|
||||
/// </summary>
|
||||
/// <param name="robotName">旧客户端传入的机器人名称。</param>
|
||||
/// <param name="accLimitScale">RobotConfig.json 中的加速度倍率。</param>
|
||||
/// <param name="jerkLimitScale">RobotConfig.json 中的 jerk 倍率。</param>
|
||||
/// <returns>兼容层加载出的机器人模型。</returns>
|
||||
public RobotProfile LoadProfile(string robotName, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(robotName))
|
||||
{
|
||||
throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
|
||||
}
|
||||
|
||||
if (!SupportedRobotModelFileMap.TryGetValue(robotName, out var modelFileName))
|
||||
{
|
||||
throw new InvalidOperationException($"Unsupported robot name: {robotName}");
|
||||
}
|
||||
|
||||
var modelPath = ResolveModelPath(modelFileName);
|
||||
return _robotModelLoader.LoadProfile(modelPath, accLimitScale, jerkLimitScale);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析机器人模型路径,只从运行目录 Config/Models 读取当前现场固化的 JSON 模型。
|
||||
/// </summary>
|
||||
/// <param name="modelFileName">运行目录 Config/Models 下的机器人 JSON 模型文件名。</param>
|
||||
/// <returns>可传给 JSON 模型加载器的模型文件绝对路径。</returns>
|
||||
private string ResolveModelPath(string modelFileName)
|
||||
{
|
||||
var configModelPath = Path.Combine(_options.ResolveConfigRoot(), "Models", modelFileName);
|
||||
if (File.Exists(configModelPath))
|
||||
{
|
||||
return configModelPath;
|
||||
}
|
||||
|
||||
return "Not found";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,887 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 在宿主进程内实现 HTTP-only ControllerClient 兼容语义,并把控制器状态委托给运行时。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatService : IControllerClientCompatService
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly ControllerClientCompatRobotCatalog _robotCatalog;
|
||||
private readonly IControllerRuntime _runtime;
|
||||
private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
private readonly FlyshotTrajectoryArtifactWriter _artifactWriter;
|
||||
private readonly JsonFlyshotTrajectoryStore _trajectoryStore;
|
||||
private readonly ILogger<ControllerClientCompatService>? _logger;
|
||||
private RobotProfile? _activeRobotProfile;
|
||||
private string? _configuredRobotName;
|
||||
private CompatibilityRobotSettings? _robotSettings;
|
||||
private string? _connectedServerIp;
|
||||
private int _connectedServerPort;
|
||||
private bool _showTcp = true;
|
||||
private double _showTcpAxisLength = 0.1;
|
||||
private int _showTcpAxisSize = 2;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置。</param>
|
||||
/// <param name="robotCatalog">机器人模型目录。</param>
|
||||
/// <param name="runtime">控制器运行时。</param>
|
||||
/// <param name="trajectoryOrchestrator">轨迹规划与触发编排器。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器。</param>
|
||||
/// <param name="artifactWriter">saveTrajectory 规划结果点位导出器。</param>
|
||||
/// <param name="trajectoryStore">统一 RobotConfig.json 持久化存储;为空时按配置根目录创建默认实例。</param>
|
||||
/// <param name="logger">日志记录器;允许测试直接构造时传入 null。</param>
|
||||
public ControllerClientCompatService(
|
||||
ControllerClientCompatOptions options,
|
||||
ControllerClientCompatRobotCatalog robotCatalog,
|
||||
IControllerRuntime runtime,
|
||||
ControllerClientTrajectoryOrchestrator trajectoryOrchestrator,
|
||||
RobotConfigLoader configLoader,
|
||||
FlyshotTrajectoryArtifactWriter? artifactWriter = null,
|
||||
JsonFlyshotTrajectoryStore? trajectoryStore = null,
|
||||
ILogger<ControllerClientCompatService>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotCatalog = robotCatalog ?? throw new ArgumentNullException(nameof(robotCatalog));
|
||||
_runtime = runtime ?? throw new ArgumentNullException(nameof(runtime));
|
||||
_trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
_artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader());
|
||||
_trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader);
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string ServerVersion => _options.ServerVersion;
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool IsSetUp
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _activeRobotProfile is not null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前运行时是否处于运动态。
|
||||
/// </summary>
|
||||
public bool IsInMotion => _runtime.GetSnapshot().IsInMotion;
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ConnectServer(string serverIp, int port)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(serverIp))
|
||||
{
|
||||
throw new ArgumentException("服务端 IP 不能为空。", nameof(serverIp));
|
||||
}
|
||||
|
||||
if (port <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(port), "端口必须大于 0。");
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
// HTTP-only 阶段仍记录旧客户端期望的 50001 地址,便于后续 TCP 入口恢复时复用状态。
|
||||
_connectedServerIp = serverIp;
|
||||
_connectedServerPort = port;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ConnectServer 完成: {ServerIp}:{Port}", serverIp, port);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string GetServerVersion()
|
||||
{
|
||||
return ServerVersion;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string GetClientVersion()
|
||||
{
|
||||
return "flyshot-replacement-controller-client-compat/0.1.0";
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetUpRobot(string robotName)
|
||||
{
|
||||
_logger?.LogInformation("SetUpRobot 开始: robotName={RobotName}", robotName);
|
||||
|
||||
var robotSettings = TryLoadRobotSettings() ?? CreateDefaultRobotSettings();
|
||||
var robotProfile = _robotCatalog.LoadProfile(
|
||||
robotName,
|
||||
robotSettings.AccLimitScale,
|
||||
robotSettings.JerkLimitScale);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
// 机器人重新初始化时同步重置运行时和上传轨迹目录,保持旧服务初始化语义。
|
||||
_configuredRobotName = robotName;
|
||||
_activeRobotProfile = robotProfile;
|
||||
_uploadedTrajectories.Clear();
|
||||
_runtime.ResetRobot(robotProfile, robotName);
|
||||
_robotSettings = robotSettings;
|
||||
|
||||
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
|
||||
var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _);
|
||||
foreach (var saved in savedTrajectories)
|
||||
{
|
||||
_uploadedTrajectories[saved.Key] = saved.Value;
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, 恢复轨迹数={TrajCount}",
|
||||
robotName,
|
||||
robotProfile.DegreesOfFreedom,
|
||||
robotSettings.AccLimitScale,
|
||||
robotSettings.JerkLimitScale,
|
||||
_uploadedTrajectories.Count);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetUpRobotFromEnv(string envFile)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(envFile))
|
||||
{
|
||||
throw new ArgumentException("环境文件路径不能为空。", nameof(envFile));
|
||||
}
|
||||
|
||||
throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetShowTcp(bool isShow, double axisLength, int axisSize)
|
||||
{
|
||||
if (axisLength <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(axisLength), "TCP 坐标轴长度必须大于 0。");
|
||||
}
|
||||
|
||||
if (axisSize <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(axisSize), "TCP 坐标轴线宽必须大于 0。");
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
|
||||
// 当前无 GUI 渲染层,先保存显示参数,保证旧 SDK 参数不会在 HTTP 边界丢失。
|
||||
_showTcp = isShow;
|
||||
_showTcpAxisLength = axisLength;
|
||||
_showTcpAxisSize = axisSize;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetActiveController(bool sim)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetActiveController(sim);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Connect(string robotIp)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(robotIp))
|
||||
{
|
||||
throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("Connect 开始: robotIp={RobotIp}", robotIp);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Connect(robotIp);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Disconnect();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
_logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}", bufferSize);
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.EnableRobot(bufferSize);
|
||||
}
|
||||
_logger?.LogInformation("EnableRobot 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
_logger?.LogInformation("DisableRobot 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.DisableRobot();
|
||||
}
|
||||
_logger?.LogInformation("DisableRobot 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
_logger?.LogInformation("StopMove 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.StopMove();
|
||||
}
|
||||
_logger?.LogInformation("StopMove 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public ControllerStateSnapshot GetControllerSnapshot()
|
||||
{
|
||||
return _runtime.GetSnapshot();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public double GetSpeedRatio()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetSpeedRatio();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetSpeedRatio(double ratio)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetSpeedRatio(ratio);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetIo(int port, bool value, string ioType)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetIo(port, value, ioType);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool GetIo(int port, string ioType)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetIo(port, ioType);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
ArgumentNullException.ThrowIfNull(seed);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("位姿必须是 [x,y,z,qx,qy,qz,qw] 七元数组。", nameof(pose));
|
||||
}
|
||||
|
||||
if (seed.Count != GetDegreesOfFreedom())
|
||||
{
|
||||
throw new ArgumentException("seed 关节数量必须与机器人自由度一致。", nameof(seed));
|
||||
}
|
||||
|
||||
throw new NotSupportedException("GetNearestIK 尚未接入逆解求解器。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetTcp(double x, double y, double z)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetTcp(x, y, z);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetTcp()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetTcp();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetJointPositions()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetJointPositions();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void MoveJoint(IReadOnlyList<double> jointPositions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(jointPositions);
|
||||
|
||||
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
|
||||
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("MoveJoint 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
if (waypoints.Count == 0)
|
||||
{
|
||||
throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ExecuteTrajectory 开始: 路点数={WaypointCount}, method={Method}, saveTraj={SaveTraj}",
|
||||
waypoints.Count, options.Method, options.SaveTrajectory);
|
||||
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
|
||||
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
|
||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
|
||||
bundle.Result.Method,
|
||||
bundle.Result.Duration.TotalSeconds,
|
||||
bundle.Result.IsValid,
|
||||
bundle.Result.DenseJointTrajectory?.Count ?? 0,
|
||||
planningSpeedScale);
|
||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ExecuteTrajectory 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetPose()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetPose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"UploadTrajectory 开始: name={Name}, waypoints={WaypointCount}, shotFlags={ShotCount}",
|
||||
trajectory.Name,
|
||||
trajectory.Waypoints.Count,
|
||||
trajectory.ShotFlags.Count(static f => f));
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRuntimeEnabled();
|
||||
_uploadedTrajectories[trajectory.Name] = trajectory;
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<string> ListTrajectoryNames()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _uploadedTrajectories.Keys.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null)
|
||||
{
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
|
||||
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
if (trajectory.Waypoints.Count == 0)
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||||
}
|
||||
|
||||
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||||
var settings = RequireRobotSettings();
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
|
||||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
|
||||
ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
|
||||
name,
|
||||
bundle.Result.Method,
|
||||
bundle.Result.Duration.TotalSeconds,
|
||||
bundle.Result.TriggerTimeline.Count,
|
||||
bundle.Result.UsedCache,
|
||||
settings.PlanningSpeedScale);
|
||||
|
||||
if (options.MoveToStart)
|
||||
{
|
||||
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
|
||||
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
|
||||
}
|
||||
else
|
||||
{
|
||||
//检验当前机械臂的关节坐标与计划轨迹的第一个点之前的差异,如果差异过大.就不报警,不执行下去
|
||||
var currentJointPositions = _runtime.GetJointPositions();
|
||||
var targetJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[0].Positions;
|
||||
var diff = currentJointPositions.Zip(targetJointPositions, (c, t) => Math.Abs(c - t)).Sum();
|
||||
if (diff > 0.01)
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之前的差异过大 name={Name}", name);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||
if (options.Wait)
|
||||
{
|
||||
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成,避免后续 J519 目标发生突变。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人模型。</param>
|
||||
/// <param name="targetJointPositions">目标关节位置,单位为弧度。</param>
|
||||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||||
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
|
||||
{
|
||||
var currentJointPositions = _runtime.GetJointPositions();
|
||||
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
|
||||
EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions));
|
||||
|
||||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger);
|
||||
_logger?.LogInformation(
|
||||
"{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
|
||||
operationName,
|
||||
speedRatio,
|
||||
moveResult.Duration.TotalSeconds,
|
||||
moveResult.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
_runtime.ExecuteTrajectory(moveResult, targetJointPositions);
|
||||
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待运行时报告当前运动结束,用于把 move_to_start 与正式飞拍轨迹串行化。
|
||||
/// </summary>
|
||||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||||
/// <param name="plannedDuration">规划运动时长。</param>
|
||||
private void WaitForRuntimeMotionComplete(string operationName, TimeSpan plannedDuration)
|
||||
{
|
||||
var timeout = ResolveMotionCompletionTimeout(plannedDuration);
|
||||
var deadline = DateTimeOffset.UtcNow.Add(timeout);
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (!_runtime.GetSnapshot().IsInMotion)
|
||||
{
|
||||
_logger?.LogInformation("{OperationName} 运动完成", operationName);
|
||||
return;
|
||||
}
|
||||
|
||||
if (DateTimeOffset.UtcNow >= deadline)
|
||||
{
|
||||
throw new TimeoutException($"{operationName} 等待运动完成超时,planned={plannedDuration.TotalSeconds:F3}s, timeout={timeout.TotalSeconds:F3}s。");
|
||||
}
|
||||
|
||||
Thread.Sleep(TimeSpan.FromMilliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据规划时长推导等待超时,给真机通信和状态更新留出余量。
|
||||
/// </summary>
|
||||
/// <param name="plannedDuration">规划运动时长。</param>
|
||||
/// <returns>等待运行时完成的最大时长。</returns>
|
||||
private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration)
|
||||
{
|
||||
var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0);
|
||||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
// 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。
|
||||
var planningSettings = RequireRobotSettings();
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
trajectory,
|
||||
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
|
||||
planningSettings,
|
||||
planningSettings.PlanningSpeedScale);
|
||||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
|
||||
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
|
||||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
|
||||
|
||||
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
// _trajectoryStore.Save(robotName, settings, trajectory);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
var planningSettings = RequireRobotSettings();
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
trajectory,
|
||||
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
|
||||
planningSettings,
|
||||
planningSettings.PlanningSpeedScale);
|
||||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
|
||||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
|
||||
|
||||
duration = bundle.Result.Duration;
|
||||
_logger?.LogInformation(
|
||||
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
|
||||
name, bundle.Result.IsValid, duration.TotalSeconds);
|
||||
return bundle.Result.IsValid;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DeleteTrajectory(string name)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (!_uploadedTrajectories.Remove(name))
|
||||
{
|
||||
_logger?.LogWarning("DeleteTrajectory 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("DeleteFlyShotTraj failed");
|
||||
}
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
_trajectoryStore.Delete(robotName, name);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string GetRobotName()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public int GetDegreesOfFreedom()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _activeRobotProfile?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前机器人配置,未初始化时抛出兼容错误。
|
||||
/// </summary>
|
||||
/// <returns>当前机器人配置。</returns>
|
||||
private RobotProfile RequireActiveRobot()
|
||||
{
|
||||
return _activeRobotProfile ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前机器人兼容配置;未加载旧配置时回退到现场默认值。
|
||||
/// </summary>
|
||||
/// <returns>当前机器人配置。</returns>
|
||||
private CompatibilityRobotSettings RequireRobotSettings()
|
||||
{
|
||||
return _robotSettings ?? CreateDefaultRobotSettings();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验机器人已经完成初始化。
|
||||
/// </summary>
|
||||
private void EnsureRobotSetup()
|
||||
{
|
||||
_ = RequireActiveRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验运行时已经处于可执行状态。
|
||||
/// </summary>
|
||||
private void EnsureRuntimeEnabled()
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (!_runtime.GetSnapshot().IsEnabled)
|
||||
{
|
||||
throw new InvalidOperationException("Robot has not been enabled.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验关节向量与当前机器人自由度一致,且所有值都是有限数值。
|
||||
/// </summary>
|
||||
/// <param name="joints">待校验关节向量,单位为弧度。</param>
|
||||
/// <param name="expectedCount">期望自由度。</param>
|
||||
/// <param name="paramName">调用方参数名。</param>
|
||||
private static void EnsureJointVector(IReadOnlyList<double> joints, int expectedCount, string paramName)
|
||||
{
|
||||
if (joints.Count != expectedCount)
|
||||
{
|
||||
throw new ArgumentException($"关节数量必须为 {expectedCount}。", paramName);
|
||||
}
|
||||
|
||||
for (var index = 0; index < joints.Count; index++)
|
||||
{
|
||||
var value = joints[index];
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个关节值必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
||||
/// </summary>
|
||||
/// <param name="name">飞拍轨迹名称。</param>
|
||||
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
|
||||
/// <param name="robot">当前机器人模型。</param>
|
||||
/// <param name="bundle">规划结果包。</param>
|
||||
private void ExportFlyshotArtifactsIfRequested(
|
||||
string name,
|
||||
bool saveTrajectory,
|
||||
RobotProfile robot,
|
||||
PlannedExecutionBundle bundle)
|
||||
{
|
||||
if (!saveTrajectory)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为飞拍链路预先构建最终发送队列,确保运行时只消费已经离散校验通过的 8ms 点列。
|
||||
/// </summary>
|
||||
private static PlannedExecutionBundle PrepareFlyshotExecutionBundle(
|
||||
RobotProfile robot,
|
||||
PlannedExecutionBundle bundle,
|
||||
double speedRatio)
|
||||
{
|
||||
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
|
||||
robot,
|
||||
bundle.Result,
|
||||
robot.ServoPeriod.TotalSeconds,
|
||||
speedRatio);
|
||||
var preparedResult = new TrajectoryResult(
|
||||
programName: bundle.Result.ProgramName,
|
||||
method: bundle.Result.Method,
|
||||
isValid: bundle.Result.IsValid,
|
||||
duration: bundle.Result.Duration,
|
||||
shotEvents: bundle.Result.ShotEvents,
|
||||
triggerTimeline: bundle.Result.TriggerTimeline,
|
||||
artifacts: bundle.Result.Artifacts,
|
||||
failureReason: bundle.Result.FailureReason,
|
||||
usedCache: bundle.Result.UsedCache,
|
||||
originalWaypointCount: bundle.Result.OriginalWaypointCount,
|
||||
plannedWaypointCount: bundle.Result.PlannedWaypointCount,
|
||||
triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
|
||||
denseJointTrajectory: bundle.Result.DenseJointTrajectory,
|
||||
preparedFlyshotExecution: preparedExecution);
|
||||
return new PlannedExecutionBundle(bundle.PlannedTrajectory, bundle.ShotTimeline, preparedResult, preparedExecution);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置;失败时返回 null。
|
||||
/// </summary>
|
||||
/// <returns>加载到的机器人配置,或 null。</returns>
|
||||
private CompatibilityRobotSettings? TryLoadRobotSettings()
|
||||
{
|
||||
foreach (var root in EnumerateRobotConfigRoots())
|
||||
{
|
||||
try
|
||||
{
|
||||
// 运行配置根本身已经是 Config 目录,这里用绝对路径避免再次追加 Config。
|
||||
var configPath = Path.Combine(root, "RobotConfig.json");
|
||||
var loaded = _configLoader.Load(configPath, root);
|
||||
return loaded.Robot;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// 单个候选根目录加载失败时继续尝试下一个兼容入口。
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 枚举 RobotConfig.json 的配置根目录,运行目录 Config 优先,旧父工作区仅在显式配置时参与。
|
||||
/// </summary>
|
||||
/// <returns>待尝试的配置根目录列表。</returns>
|
||||
private IEnumerable<string> EnumerateRobotConfigRoots()
|
||||
{
|
||||
yield return _options.ResolveConfigRoot();
|
||||
|
||||
var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
|
||||
if (legacyWorkspaceRoot is not null)
|
||||
{
|
||||
yield return legacyWorkspaceRoot;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造与旧现场默认行为一致的机器人兼容配置。
|
||||
/// </summary>
|
||||
/// <returns>默认机器人配置。</returns>
|
||||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||||
{
|
||||
return new CompatibilityRobotSettings(
|
||||
useDo: false,
|
||||
ioAddresses: Array.Empty<int>(),
|
||||
ioKeepCycles: 2,
|
||||
triggerSampleIndexOffsetCycles: 7,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.DependencyInjection;
|
||||
using Microsoft.Extensions.Options;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 提供 ControllerClient HTTP 兼容层的依赖注入注册入口。
|
||||
/// </summary>
|
||||
public static class ControllerClientCompatServiceCollectionExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// 将 HTTP-only 的 ControllerClient 兼容服务注册到当前宿主。
|
||||
/// </summary>
|
||||
/// <param name="services">当前宿主服务集合。</param>
|
||||
/// <param name="configuration">宿主配置根。</param>
|
||||
/// <returns>同一服务集合,便于链式调用。</returns>
|
||||
public static IServiceCollection AddControllerClientCompat(this IServiceCollection services, IConfiguration configuration)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(services);
|
||||
ArgumentNullException.ThrowIfNull(configuration);
|
||||
|
||||
services
|
||||
.AddOptions<ControllerClientCompatOptions>()
|
||||
.Bind(configuration.GetSection("ControllerClientCompat"));
|
||||
|
||||
services.AddSingleton(static serviceProvider => serviceProvider.GetRequiredService<IOptions<ControllerClientCompatOptions>>().Value);
|
||||
services.AddSingleton<RobotModelLoader>();
|
||||
services.AddSingleton<RobotConfigLoader>();
|
||||
services.AddSingleton<ControllerClientCompatRobotCatalog>();
|
||||
services.AddSingleton<ControllerClientTrajectoryOrchestrator>();
|
||||
services.AddSingleton<FlyshotTrajectoryArtifactWriter>();
|
||||
services.AddSingleton<JsonFlyshotTrajectoryStore>();
|
||||
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
|
||||
services.AddSingleton<IControllerClientCompatService, ControllerClientCompatService>();
|
||||
|
||||
return services;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 保存一条已上传到兼容层内存目录中的飞拍轨迹,供 HTTP API 层列出、执行和删除。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatUploadedTrajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条飞拍轨迹快照,并立即复制所有数组,避免调用方后续原地修改。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="waypoints">关节路点集合。</param>
|
||||
/// <param name="shotFlags">拍摄标志集合。</param>
|
||||
/// <param name="offsetValues">偏移周期集合。</param>
|
||||
/// <param name="addressGroups">地址组集合。</param>
|
||||
public ControllerClientCompatUploadedTrajectory(
|
||||
string name,
|
||||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||||
IEnumerable<bool> shotFlags,
|
||||
IEnumerable<int> offsetValues,
|
||||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
ArgumentNullException.ThrowIfNull(shotFlags);
|
||||
ArgumentNullException.ThrowIfNull(offsetValues);
|
||||
ArgumentNullException.ThrowIfNull(addressGroups);
|
||||
|
||||
Name = name;
|
||||
Waypoints = waypoints.Select(static waypoint => waypoint.ToArray()).ToArray();
|
||||
ShotFlags = shotFlags.ToArray();
|
||||
OffsetValues = offsetValues.ToArray();
|
||||
AddressGroups = addressGroups.Select(static group => group.ToArray()).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹名称。
|
||||
/// </summary>
|
||||
public string Name { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取已复制的关节路点集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double[]> Waypoints { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取拍摄标志集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<bool> ShotFlags { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取偏移周期集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<int> OffsetValues { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取地址组集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<int[]> AddressGroups { get; }
|
||||
}
|
||||
@@ -0,0 +1,607 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Triggering;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientTrajectoryOrchestrator
|
||||
{
|
||||
/// <summary>
|
||||
/// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。
|
||||
/// </summary>
|
||||
private const int MaxDenseLimitStretchIterations = 100;
|
||||
|
||||
/// <summary>
|
||||
/// 每次离散限幅失败后统一放大的时间倍率。
|
||||
/// </summary>
|
||||
private const double DenseLimitStretchFactor = 1.01;
|
||||
|
||||
/// <summary>
|
||||
/// 平滑起停重定时混合系数:0 表示不平滑,1 表示完全使用平滑时间律。
|
||||
/// 当前取值用于“弱平滑”,仅轻微拉伸首尾时间段,避免起停过慢。
|
||||
/// </summary>
|
||||
private const double SmoothStartStopBlend = 0.60;
|
||||
|
||||
private readonly ICspPlanner _icspPlanner;
|
||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
|
||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||||
private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化轨迹编排器。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
/// <param name="loggerFactory">日志工厂;允许 null。</param>
|
||||
public ControllerClientTrajectoryOrchestrator(
|
||||
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
|
||||
ILoggerFactory? loggerFactory = null)
|
||||
{
|
||||
_logger = logger;
|
||||
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>());
|
||||
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对普通轨迹执行 ICSP 规划。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="waypoints">普通轨迹关节路点。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||||
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> waypoints,
|
||||
TrajectoryExecutionOptions? options = null,
|
||||
double planningSpeedScale = 1.0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
|
||||
waypoints.Count, options.Method, planningSpeedScale);
|
||||
|
||||
var program = CreateProgram(
|
||||
name: "ordinary-trajectory",
|
||||
waypoints: waypoints,
|
||||
shotFlags: Enumerable.Repeat(false, waypoints.Count),
|
||||
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
||||
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
||||
|
||||
var method = ParseOrdinaryMethod(options.Method);
|
||||
var request = new TrajectoryRequest(
|
||||
robot: planningRobot,
|
||||
program: program,
|
||||
method: method,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method);
|
||||
var executionTrajectory = plannedTrajectory;
|
||||
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
|
||||
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
||||
var result = CreateResult(
|
||||
executionTrajectory,
|
||||
shotTimeline,
|
||||
denseJointTrajectory,
|
||||
usedCache: false,
|
||||
triggerSampleIndexOffsetCycles: 0);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
|
||||
result.Duration.TotalSeconds,
|
||||
result.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对已经上传的飞拍轨迹执行自适应 ICSP 规划并生成触发时间轴。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <param name="settings">兼容层机器人设置。</param>
|
||||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||||
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions? options = null,
|
||||
CompatibilityRobotSettings? settings = null,
|
||||
double? planningSpeedScale = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(uploaded);
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
settings ??= CreateDefaultRobotSettings();
|
||||
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
|
||||
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
|
||||
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale, settings.SmoothStartStopTiming);
|
||||
|
||||
var program = CreateProgram(
|
||||
name: uploaded.Name,
|
||||
waypoints: uploaded.Waypoints,
|
||||
shotFlags: uploaded.ShotFlags,
|
||||
offsetValues: uploaded.OffsetValues,
|
||||
addressGroups: uploaded.AddressGroups);
|
||||
|
||||
var method = ParseFlyshotMethod(options.Method);
|
||||
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
|
||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||
{
|
||||
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
|
||||
|
||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志,规划轨迹和触发时间轴保持不可变复用。
|
||||
return cachedBundle;
|
||||
}
|
||||
|
||||
var request = new TrajectoryRequest(
|
||||
robot: planningRobot,
|
||||
program: program,
|
||||
method: method,
|
||||
moveToStart: options.MoveToStart,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory,
|
||||
useCache: options.UseCache);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method, settings);
|
||||
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
|
||||
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
|
||||
var shotTimeline = _shotTimelineBuilder.Build(
|
||||
smoothedExecutionTrajectory,
|
||||
holdCycles: settings.IoKeepCycles,
|
||||
samplePeriod: planningRobot.ServoPeriod,
|
||||
useDo: settings.UseDo);
|
||||
var result = CreateResult(
|
||||
smoothedExecutionTrajectory,
|
||||
shotTimeline,
|
||||
denseJointTrajectory,
|
||||
usedCache: false,
|
||||
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
|
||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
|
||||
uploaded.Name, result.Duration.TotalSeconds, result.TriggerTimeline.Count, result.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
if (options.UseCache)
|
||||
{
|
||||
_flyshotCache[cacheKey] = bundle;
|
||||
}
|
||||
|
||||
return bundle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按普通轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseOrdinaryMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按飞拍轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseFlyshotMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按领域枚举分派到当前已经落地的规划器。
|
||||
/// </summary>
|
||||
/// <param name="request">规划请求。</param>
|
||||
/// <param name="method">规划方法。</param>
|
||||
/// <returns>规划轨迹。</returns>
|
||||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
PlanningMethod.Icsp => _icspPlanner.Plan(request),
|
||||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
|
||||
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 归一化旧 SDK 的 method 字符串。
|
||||
/// </summary>
|
||||
/// <param name="method">原始方法名。</param>
|
||||
/// <returns>小写短横线方法名。</returns>
|
||||
private static string NormalizeMethod(string method)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(method))
|
||||
{
|
||||
return "icsp";
|
||||
}
|
||||
|
||||
return method.Trim().ToLowerInvariant();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键,避免同名覆盖后误用旧规划结果。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置。</param>
|
||||
/// <param name="uploaded">上传轨迹。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <returns>缓存键。</returns>
|
||||
private static string CreateFlyshotCacheKey(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions options,
|
||||
CompatibilityRobotSettings settings,
|
||||
double planningSpeedScale)
|
||||
{
|
||||
var hash = new HashCode();
|
||||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||||
hash.Add(planningSpeedScale);
|
||||
hash.Add(uploaded.Name, StringComparer.Ordinal);
|
||||
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
|
||||
hash.Add(options.MoveToStart);
|
||||
hash.Add(options.SaveTrajectory);
|
||||
hash.Add(settings.UseDo);
|
||||
hash.Add(settings.IoKeepCycles);
|
||||
hash.Add(settings.TriggerSampleIndexOffsetCycles);
|
||||
hash.Add(settings.AdaptIcspTryNum);
|
||||
hash.Add(settings.SmoothStartStopTiming);
|
||||
|
||||
foreach (var limit in robot.JointLimits)
|
||||
{
|
||||
hash.Add(limit.JointName, StringComparer.Ordinal);
|
||||
hash.Add(limit.VelocityLimit);
|
||||
hash.Add(limit.AccelerationLimit);
|
||||
hash.Add(limit.JerkLimit);
|
||||
}
|
||||
|
||||
foreach (var waypoint in uploaded.Waypoints)
|
||||
{
|
||||
foreach (var value in waypoint)
|
||||
{
|
||||
hash.Add(value);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var flag in uploaded.ShotFlags)
|
||||
{
|
||||
hash.Add(flag);
|
||||
}
|
||||
|
||||
foreach (var offset in uploaded.OffsetValues)
|
||||
{
|
||||
hash.Add(offset);
|
||||
}
|
||||
|
||||
foreach (var group in uploaded.AddressGroups)
|
||||
{
|
||||
foreach (var address in group)
|
||||
{
|
||||
hash.Add(address);
|
||||
}
|
||||
}
|
||||
|
||||
return hash.ToHashCode().ToString("X8");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造编排器直接调用时的默认兼容配置,保持既有单元测试中的 DO 生成行为。
|
||||
/// </summary>
|
||||
/// <returns>默认机器人兼容配置。</returns>
|
||||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||||
{
|
||||
return new CompatibilityRobotSettings(
|
||||
useDo: true,
|
||||
ioAddresses: Array.Empty<int>(),
|
||||
ioKeepCycles: 0,
|
||||
triggerSampleIndexOffsetCycles: 7,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划阶段得到的轨迹。</param>
|
||||
/// <param name="settings">当前 RobotConfig.json 解析出的兼容设置。</param>
|
||||
/// <returns>运行时真正用于采样和触发的轨迹。</returns>
|
||||
private static PlannedTrajectory ApplyExecutionTiming(PlannedTrajectory plannedTrajectory, CompatibilityRobotSettings settings)
|
||||
{
|
||||
// legacy-fit 模式需要严格保留 waypoint.txt 反推出的节点时间,不能再二次改写时间轴。
|
||||
return settings.SmoothStartStopTiming
|
||||
? ApplySmoothStartStopTiming(plannedTrajectory)
|
||||
: plannedTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按规划全局速度倍率生成规划专用机器人约束。
|
||||
/// </summary>
|
||||
/// <param name="robot">原始机器人约束。</param>
|
||||
/// <param name="planningSpeedScale">规划阶段的全局速度倍率,1.0 表示不额外缩放。</param>
|
||||
/// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
|
||||
private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale)
|
||||
{
|
||||
if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (Math.Abs(planningSpeedScale - 1.0) < 1e-12)
|
||||
{
|
||||
return robot;
|
||||
}
|
||||
|
||||
// RVBUST 规划阶段会用独立限速倍率缩放有效限制;运行时 speedRatio 仍只负责 J519 下发重采样。
|
||||
var scaledLimits = robot.JointLimits
|
||||
.Select(limit => new JointLimit(
|
||||
limit.JointName,
|
||||
limit.VelocityLimit * planningSpeedScale,
|
||||
limit.AccelerationLimit * planningSpeedScale * planningSpeedScale,
|
||||
limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale))
|
||||
.ToArray();
|
||||
|
||||
return new RobotProfile(
|
||||
name: robot.Name,
|
||||
modelPath: robot.ModelPath,
|
||||
degreesOfFreedom: robot.DegreesOfFreedom,
|
||||
jointLimits: scaledLimits,
|
||||
jointCouplings: robot.JointCouplings,
|
||||
servoPeriod: robot.ServoPeriod,
|
||||
triggerPeriod: robot.TriggerPeriod);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把兼容层输入数组转换成领域层 FlyshotProgram。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="waypoints">关节路点。</param>
|
||||
/// <param name="shotFlags">拍照标志。</param>
|
||||
/// <param name="offsetValues">偏移周期。</param>
|
||||
/// <param name="addressGroups">IO 地址组。</param>
|
||||
/// <returns>领域层飞拍程序。</returns>
|
||||
private static FlyshotProgram CreateProgram(
|
||||
string name,
|
||||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||||
IEnumerable<bool> shotFlags,
|
||||
IEnumerable<int> offsetValues,
|
||||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||||
{
|
||||
return new FlyshotProgram(
|
||||
name: name,
|
||||
waypoints: waypoints.Select(static waypoint => new JointWaypoint(waypoint)).ToArray(),
|
||||
shotFlags: shotFlags.ToArray(),
|
||||
offsetValues: offsetValues.ToArray(),
|
||||
addressGroups: addressGroups.Select(static group => new IoAddressGroup(group)).ToArray());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从规划轨迹和触发时间轴构造运行时可消费的稳定结果对象。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||
/// <param name="shotTimeline">触发时间轴。</param>
|
||||
/// <returns>运行时执行结果描述。</returns>
|
||||
private static TrajectoryResult CreateResult(
|
||||
PlannedTrajectory plannedTrajectory,
|
||||
ShotTimeline shotTimeline,
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
bool usedCache,
|
||||
int triggerSampleIndexOffsetCycles)
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: plannedTrajectory.OriginalProgram.Name,
|
||||
method: plannedTrajectory.Method,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(plannedTrajectory.WaypointTimes[^1]),
|
||||
shotEvents: shotTimeline.ShotEvents,
|
||||
triggerTimeline: shotTimeline.TriggerTimeline,
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: usedCache,
|
||||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
||||
triggerSampleIndexOffsetCycles: triggerSampleIndexOffsetCycles,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成满足离散速度、加速度和 Jerk 限制的稠密执行轨迹。
|
||||
/// </summary>
|
||||
private IReadOnlyList<IReadOnlyList<double>> CreateLimitCompliantDenseTrajectory(
|
||||
ref PlannedTrajectory executionTrajectory,
|
||||
bool shapeTrajectoryEdges)
|
||||
{
|
||||
for (var iteration = 0; iteration <= MaxDenseLimitStretchIterations; iteration++)
|
||||
{
|
||||
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
|
||||
executionTrajectory,
|
||||
samplePeriod: executionTrajectory.Robot.ServoPeriod.TotalSeconds,
|
||||
smoothStartStop: shapeTrajectoryEdges);
|
||||
|
||||
try
|
||||
{
|
||||
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
||||
executionTrajectory.Robot,
|
||||
denseJointTrajectory,
|
||||
trajectoryName: executionTrajectory.OriginalProgram.Name);
|
||||
return denseJointTrajectory;
|
||||
}
|
||||
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
|
||||
{
|
||||
_logger?.LogWarning(ex, "稠密轨迹离散限幅校验失败,准备拉长时间轴重试");
|
||||
// 离散差分超限时统一拉长时间轴,保持路点几何不变并降低速度、加速度和 Jerk。
|
||||
executionTrajectory = StretchTrajectoryTiming(executionTrajectory, DenseLimitStretchFactor);
|
||||
_logger?.LogInformation(
|
||||
"离散差分超限,拉长时间轴,iteration={Iteration}, factor={StretchFactor}",
|
||||
iteration,
|
||||
DenseLimitStretchFactor);
|
||||
_logger?.LogInformation("拉长之后的总时间={TotalTime}", executionTrajectory.WaypointTimes[^1]);
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("稠密轨迹离散限幅校验未能产生有效结果。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按统一倍率拉长轨迹时间轴,保留原始路点和触发元数据。
|
||||
/// </summary>
|
||||
private PlannedTrajectory StretchTrajectoryTiming(PlannedTrajectory trajectory, double stretchFactor)
|
||||
{
|
||||
var waypointTimes = trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray();
|
||||
var segmentDurations = trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray();
|
||||
var segmentScales = trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray();
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
originalProgram: trajectory.OriginalProgram,
|
||||
plannedWaypoints: trajectory.PlannedWaypoints,
|
||||
waypointTimes: waypointTimes,
|
||||
segmentDurations: segmentDurations,
|
||||
segmentScales: segmentScales,
|
||||
method: trajectory.Method,
|
||||
iterations: trajectory.Iterations,
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为飞拍执行生成平滑起停时间轴(仅重定时,不改几何路点)。
|
||||
/// 该方法保持 <see cref="PlannedTrajectory.PlannedWaypoints"/> 不变,只重映射 <see cref="PlannedTrajectory.WaypointTimes"/>,
|
||||
/// 让轨迹在起点和终点附近具有更柔和的速度变化,从而降低“突然起步/突然收尾”带来的离散差分尖峰。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划器输出的原始轨迹(通常是线性时间轴)。</param>
|
||||
/// <returns>
|
||||
/// 若满足平滑条件,则返回新的重定时轨迹;若路点数量不足或总时长无效,则直接返回输入轨迹。
|
||||
/// </returns>
|
||||
private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
|
||||
{
|
||||
var originalTimes = plannedTrajectory.WaypointTimes;
|
||||
// 至少需要“起点-中间点-终点”三类点,才有可平滑的中间区间。
|
||||
if (originalTimes.Count < 3)
|
||||
{
|
||||
return plannedTrajectory;
|
||||
}
|
||||
|
||||
var totalDuration = originalTimes[^1];
|
||||
// 轨迹总时长必须为正,避免后续归一化进度出现除零或无意义映射。
|
||||
if (totalDuration <= 0.0)
|
||||
{
|
||||
return plannedTrajectory;
|
||||
}
|
||||
|
||||
var smoothedTimes = new double[originalTimes.Count];
|
||||
// 强制固定边界:起点仍在 0,终点仍在总时长,保证任务总耗时不变。
|
||||
smoothedTimes[0] = 0.0;
|
||||
smoothedTimes[^1] = totalDuration;
|
||||
|
||||
for (var index = 1; index < originalTimes.Count - 1; index++)
|
||||
{
|
||||
// 把原始时刻归一化到 [0, 1],用于统一时间律变换。
|
||||
var normalizedProgress = originalTimes[index] / totalDuration;
|
||||
// 先求完整平滑时间律对应的时刻,再与原线性时刻按比例混合。
|
||||
// 这里采用弱平滑(blend=0.35):保留大部分原节奏,仅轻微降低首尾突变。
|
||||
var smoothedProgress = InvertSmoothStartStopProgress(normalizedProgress);
|
||||
var blendedProgress = ((1.0 - SmoothStartStopBlend) * normalizedProgress)
|
||||
+ (SmoothStartStopBlend * smoothedProgress);
|
||||
smoothedTimes[index] = totalDuration * blendedProgress;
|
||||
}
|
||||
|
||||
var segmentDurations = new double[smoothedTimes.Length - 1];
|
||||
for (var index = 0; index < segmentDurations.Length; index++)
|
||||
{
|
||||
// 重建每段持续时间,供后续稠密采样和限幅检查使用。
|
||||
segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
|
||||
}
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: plannedTrajectory.Robot,
|
||||
originalProgram: plannedTrajectory.OriginalProgram,
|
||||
plannedWaypoints: plannedTrajectory.PlannedWaypoints,
|
||||
waypointTimes: smoothedTimes,
|
||||
segmentDurations: segmentDurations,
|
||||
segmentScales: plannedTrajectory.SegmentScales,
|
||||
method: plannedTrajectory.Method,
|
||||
iterations: plannedTrajectory.Iterations,
|
||||
threshold: plannedTrajectory.Threshold);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 反解平滑时间律进度:给定目标进度 p,求满足 f(t)=p 的 t。
|
||||
/// 其中 f(t) 由 <see cref="EvaluateSmoothStartStopProgress"/> 给出,是单调递增的 7 次 smootherstep 曲线,
|
||||
/// 通过二分法可稳定得到对应的归一化时间,避免显式求高次方程根带来的复杂性和数值不稳定。
|
||||
/// </summary>
|
||||
/// <param name="normalizedProgress">目标归一化进度,理论范围 [0, 1]。</param>
|
||||
/// <returns>与目标进度对应的归一化时间,范围 [0, 1]。</returns>
|
||||
private static double InvertSmoothStartStopProgress(double normalizedProgress)
|
||||
{
|
||||
// 防御式裁剪,避免上游浮点误差将进度推到区间外。
|
||||
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
|
||||
var low = 0.0;
|
||||
var high = 1.0;
|
||||
// 固定迭代次数确保耗时可预测;40 次足以把区间收敛到工程可用精度。
|
||||
for (var iteration = 0; iteration < 40; iteration++)
|
||||
{
|
||||
var middle = (low + high) / 2.0;
|
||||
var progress = EvaluateSmoothStartStopProgress(middle);
|
||||
// f(middle) 小于目标,说明根在右半区间;否则在左半区间。
|
||||
if (progress < target)
|
||||
{
|
||||
low = middle;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = middle;
|
||||
}
|
||||
}
|
||||
|
||||
return (low + high) / 2.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 7 次 smootherstep 的进度函数值 f(u)。
|
||||
/// 该函数在 u=0 和 u=1 处具有更高阶导数连续性,可让起停段速度变化更平滑,
|
||||
/// 适合用于飞拍轨迹的时间重参数化,减少离散导数在边界处的突变。
|
||||
/// </summary>
|
||||
/// <param name="normalizedTime">归一化时间 u,理论范围 [0, 1]。</param>
|
||||
/// <returns>归一化进度 f(u),范围 [0, 1]。</returns>
|
||||
private static double EvaluateSmoothStartStopProgress(double normalizedTime)
|
||||
{
|
||||
// 先裁剪再计算多项式,确保数值稳定且不受外部越界输入影响。
|
||||
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = u * u;
|
||||
var u3 = u2 * u;
|
||||
var u4 = u3 * u;
|
||||
var u5 = u4 * u;
|
||||
var u6 = u5 * u;
|
||||
var u7 = u6 * u;
|
||||
return (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<FrameworkReference Include="Microsoft.AspNetCore.App" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,54 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍轨迹执行接口的可选参数,字段名对齐旧 `ControllerClient::ExecuteFlyShotTraj`。
|
||||
/// </summary>
|
||||
public sealed class FlyshotExecutionOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化飞拍轨迹执行参数。
|
||||
/// </summary>
|
||||
/// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param>
|
||||
/// <param name="method">轨迹生成方法,支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
|
||||
/// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param>
|
||||
public FlyshotExecutionOptions(
|
||||
bool moveToStart = true,
|
||||
string method = "icsp",
|
||||
bool saveTrajectory = true,
|
||||
bool useCache = true,
|
||||
bool wait = true)
|
||||
{
|
||||
MoveToStart = moveToStart;
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
UseCache = useCache;
|
||||
Wait = wait;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取执行前是否自动移动到轨迹起点。
|
||||
/// </summary>
|
||||
public bool MoveToStart { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否优先复用已规划轨迹缓存。
|
||||
/// </summary>
|
||||
public bool UseCache { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否等待机器人执行完整条飞拍轨迹后再返回。
|
||||
/// </summary>
|
||||
public bool Wait { get; }
|
||||
}
|
||||
@@ -0,0 +1,325 @@
|
||||
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 Microsoft.Extensions.Logging;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
|
||||
/// </summary>
|
||||
public sealed class FlyshotTrajectoryArtifactWriter
|
||||
{
|
||||
/// <summary>
|
||||
/// 旧 Data 明细点位文件使用的默认采样周期,单位为秒。
|
||||
/// </summary>
|
||||
private const double LegacyDetailSamplePeriodSeconds = 0.016;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC J519 实际下发的固定伺服周期,单位为秒。
|
||||
/// </summary>
|
||||
private const double ActualSendServoPeriodSeconds = 0.008;
|
||||
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotModelLoader _robotModelLoader;
|
||||
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化规划结果点位导出器。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置,用于定位运行配置根目录。</param>
|
||||
/// <param name="robotModelLoader">机器人模型加载器,用于生成笛卡尔点位。</param>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public FlyshotTrajectoryArtifactWriter(
|
||||
ControllerClientCompatOptions options,
|
||||
RobotModelLoader robotModelLoader,
|
||||
ILogger<FlyshotTrajectoryArtifactWriter>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将飞拍规划结果导出到 Config/Data/name。
|
||||
/// </summary>
|
||||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="bundle">规划结果包。</param>
|
||||
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
|
||||
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(bundle);
|
||||
|
||||
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
||||
Directory.CreateDirectory(outputDir);
|
||||
if (bundle.Result.DenseJointTrajectory is null)
|
||||
{
|
||||
throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。");
|
||||
}
|
||||
|
||||
// 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”,避免再次从 PlannedTrajectory 生成另一条轨迹。
|
||||
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
||||
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
|
||||
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
|
||||
var executionDenseTrajectory = bundle.Result.DenseJointTrajectory;
|
||||
var jointDetailTrajectory = DownsampleDenseRows(
|
||||
executionDenseTrajectory,
|
||||
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
|
||||
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
|
||||
var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);
|
||||
|
||||
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
|
||||
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
|
||||
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
|
||||
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
|
||||
WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
|
||||
trajectoryName,
|
||||
outputDir,
|
||||
jointTrajectory.Count,
|
||||
jointDetailTrajectory.Count,
|
||||
speedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点,供 saveTrajectory 离线对比真实下发序列。
|
||||
/// </summary>
|
||||
private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
|
||||
var preparedExecution = result.PreparedFlyshotExecution;
|
||||
if (result.DenseJointTrajectory is null && preparedExecution is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (preparedExecution is null && (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio)))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||||
}
|
||||
var samples = preparedExecution is null
|
||||
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||
result.DenseJointTrajectory!,
|
||||
result.Duration.TotalSeconds,
|
||||
ActualSendServoPeriodSeconds,
|
||||
speedRatio)
|
||||
: preparedExecution.Samples.Select(static sample => new J519SendSample(
|
||||
sample.SampleIndex,
|
||||
sample.SendTime,
|
||||
sample.TrajectoryTime,
|
||||
sample.SpeedRatio,
|
||||
sample.JointsDegrees)).ToArray();
|
||||
var triggerBindings = preparedExecution is null
|
||||
? TriggerSampleBinder.Bind(
|
||||
result.TriggerTimeline,
|
||||
samples,
|
||||
result.TriggerSampleIndexOffsetCycles)
|
||||
: preparedExecution.TriggerBindings.Select(static binding =>
|
||||
new TriggerSampleBinding(
|
||||
binding.Trigger,
|
||||
new J519SendSample(
|
||||
binding.Sample.SampleIndex,
|
||||
binding.Sample.SendTime,
|
||||
binding.Sample.TrajectoryTime,
|
||||
binding.Sample.SpeedRatio,
|
||||
binding.Sample.JointsDegrees),
|
||||
binding.SampleIndex,
|
||||
binding.FoundInWindow)).ToArray();
|
||||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||
robot,
|
||||
samples,
|
||||
trajectoryName: result.ProgramName);
|
||||
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
|
||||
List<IReadOnlyList<double>> timingRows = preparedExecution is null
|
||||
? new List<IReadOnlyList<double>>(samples.Count)
|
||||
: preparedExecution.TimingRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
|
||||
List<IReadOnlyList<double>> jerkRows = preparedExecution is null
|
||||
? []
|
||||
: preparedExecution.JerkRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
|
||||
double? previousSendTime = null;
|
||||
double[]? previousJoints = null;
|
||||
double[]? previousVelocity = null;
|
||||
double[]? previousAcceleration = null;
|
||||
|
||||
foreach (var sample in samples)
|
||||
{
|
||||
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
|
||||
if (preparedExecution is null)
|
||||
{
|
||||
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
|
||||
|
||||
if (previousSendTime is not null && previousJoints is not null)
|
||||
{
|
||||
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
|
||||
previousSendTime.Value,
|
||||
sample.SendTime,
|
||||
previousJoints,
|
||||
sample.JointsDegrees,
|
||||
ref previousVelocity,
|
||||
ref previousAcceleration));
|
||||
}
|
||||
}
|
||||
|
||||
previousSendTime = sample.SendTime;
|
||||
previousJoints = sample.JointsDegrees.ToArray();
|
||||
}
|
||||
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), jointRows);
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), timingRows);
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), jerkRows);
|
||||
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造实际发送点位文本行,格式为 send_time + 关节角度 + io_mask + io_value。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> BuildActualSendJointRow(double sendTime, IReadOnlyList<double> joints)
|
||||
{
|
||||
var row = new double[joints.Count + 3];
|
||||
row[0] = Math.Round(sendTime, 6);
|
||||
for (var index = 0; index < joints.Count; index++)
|
||||
{
|
||||
row[index + 1] = Math.Round(joints[index], 6);
|
||||
}
|
||||
|
||||
row[^2] = 0.0;
|
||||
row[^1] = 0.0;
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 以空格分隔的旧轨迹文本格式写出数值行。
|
||||
/// </summary>
|
||||
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var row in rows)
|
||||
{
|
||||
sb.AppendLine(string.Join(" ", row.Select(static value => $"{value:F6}")));
|
||||
}
|
||||
|
||||
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 JointTraj.txt 行数据,格式为 time + 关节弧度。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildJointRows(PlannedTrajectory trajectory)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||||
{
|
||||
var row = new List<double>(trajectory.PlannedWaypoints[index].Positions.Count + 1)
|
||||
{
|
||||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||||
};
|
||||
row.AddRange(trajectory.PlannedWaypoints[index].Positions.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 CartTraj.txt 行数据,格式为 time + x/y/z/qx/qy/qz/qw。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRows(
|
||||
PlannedTrajectory trajectory,
|
||||
RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||||
{
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, trajectory.PlannedWaypoints[index].Positions.ToArray());
|
||||
var row = new List<double>(pose.Length + 1)
|
||||
{
|
||||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||||
};
|
||||
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 基于执行侧稠密关节轨迹生成笛卡尔导出行,保持与 JointDetialTraj.txt 同一来源。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRowsFromJointDense(
|
||||
IReadOnlyList<IReadOnlyList<double>> jointDenseRows,
|
||||
RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(jointDenseRows.Count);
|
||||
foreach (var jointRow in jointDenseRows)
|
||||
{
|
||||
var jointPositions = jointRow.Skip(1).ToArray();
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPositions);
|
||||
var row = new List<double>(pose.Length + 1)
|
||||
{
|
||||
Math.Round(jointRow[0], 6)
|
||||
};
|
||||
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 8ms 执行稠密轨迹按指定周期抽稀为低频兼容视图,并始终保留终点。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> DownsampleDenseRows(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseRows,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var result = new List<IReadOnlyList<double>>();
|
||||
var epsilon = 1e-6;
|
||||
var nextSampleTime = 0.0;
|
||||
|
||||
foreach (var row in denseRows)
|
||||
{
|
||||
var sampleTime = row[0];
|
||||
if (sampleTime + epsilon < nextSampleTime)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0))
|
||||
{
|
||||
result.Add(row);
|
||||
nextSampleTime += samplePeriodSeconds;
|
||||
}
|
||||
}
|
||||
|
||||
if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1]))
|
||||
{
|
||||
result.Add(denseRows[^1]);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将轨迹名转换为可用目录名,避免 HTTP 输入中的路径字符污染输出目录。
|
||||
/// </summary>
|
||||
private static string SanitizeDirectoryName(string name)
|
||||
{
|
||||
var invalidChars = Path.GetInvalidFileNameChars();
|
||||
var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
|
||||
return new string(chars);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,217 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 对飞拍稠密关节轨迹的首尾采样点做速度整形,降低启动和结束时的单步角度变化。
|
||||
/// </summary>
|
||||
internal static class FlyshotTrajectoryEdgeShaper
|
||||
{
|
||||
/// <summary>
|
||||
/// 首尾整形默认覆盖的采样点数(含锚点)。
|
||||
/// </summary>
|
||||
internal const int DefaultEdgePointCount = 10;
|
||||
|
||||
/// <summary>
|
||||
/// 对稠密关节轨迹做首尾整形,时间列保持不变,首段采用 ease-in,尾段采用 ease-out。
|
||||
/// </summary>
|
||||
/// <param name="denseJointTrajectory">输入稠密关节轨迹,每行格式为 [time, j1..jN],关节单位为弧度。</param>
|
||||
/// <param name="maxEdgeStepDegrees">保留旧签名兼容调用方;当前实现不再按角度阈值扩窗。</param>
|
||||
/// <param name="maxWindowPoints">单侧整形覆盖的采样点数(含锚点),默认首尾各 10 点。</param>
|
||||
/// <returns>经过首尾整形后的新轨迹;若不满足整形条件则返回原轨迹副本。</returns>
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> ShapeDenseJointTrajectory(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double maxEdgeStepDegrees = 0.0,
|
||||
int maxWindowPoints = DefaultEdgePointCount)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
|
||||
if (denseJointTrajectory.Count == 0)
|
||||
{
|
||||
return Array.Empty<IReadOnlyList<double>>();
|
||||
}
|
||||
|
||||
var copiedRows = denseJointTrajectory
|
||||
.Select(static row => row.ToArray())
|
||||
.ToArray();
|
||||
if (copiedRows.Length < 5 || maxWindowPoints < 2)
|
||||
{
|
||||
return copiedRows;
|
||||
}
|
||||
|
||||
var lastIndex = copiedRows.Length - 1;
|
||||
var window = Math.Min(maxWindowPoints, lastIndex / 2);
|
||||
if (window < 2)
|
||||
{
|
||||
return copiedRows;
|
||||
}
|
||||
|
||||
// 以原始轨迹为参考估计窗口边界的速度,并在位移累计量上做单段单调整形,
|
||||
// 目标是让首尾 10 点表现为更平滑的加减速,而不是硬匹配高阶导数导致振荡。
|
||||
var originalRows = copiedRows
|
||||
.Select(static row => row.ToArray())
|
||||
.ToArray();
|
||||
|
||||
ApplyLeadingHermiteBlend(copiedRows, originalRows, window);
|
||||
ApplyTrailingHermiteBlend(copiedRows, originalRows, window);
|
||||
|
||||
return copiedRows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对首段做单段 Hermite 累计位移整形:起点速度为 0,窗口末端按原轨迹边界速度接回中段。
|
||||
/// </summary>
|
||||
private static void ApplyLeadingHermiteBlend(double[][] rows, double[][] originalRows, int window)
|
||||
{
|
||||
var startRow = originalRows[0];
|
||||
var endRow = originalRows[window];
|
||||
var totalDuration = endRow[0] - startRow[0];
|
||||
if (totalDuration <= 0.0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
|
||||
{
|
||||
var delta = endRow[jointIndex] - startRow[jointIndex];
|
||||
if (Math.Abs(delta) <= 1e-12)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var endVelocity = EstimateVelocity(originalRows, window, jointIndex);
|
||||
var normalizedEndSlope = ClampNormalizedSlope((endVelocity * totalDuration) / delta);
|
||||
|
||||
for (var index = 1; index < window; index++)
|
||||
{
|
||||
var normalizedTime = (rows[index][0] - startRow[0]) / totalDuration;
|
||||
var shapedValue = startRow[jointIndex]
|
||||
+ (delta * EvaluateHermiteProgress(normalizedTime, startSlope: 0.0, endSlope: normalizedEndSlope));
|
||||
var blendWeight = Math.Pow(1.0 - normalizedTime, 2.0);
|
||||
rows[index][jointIndex] = Lerp(originalRows[index][jointIndex], shapedValue, blendWeight);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对尾段做单段 Hermite 累计位移整形:窗口起点按原轨迹边界速度接入,终点速度减到 0。
|
||||
/// </summary>
|
||||
private static void ApplyTrailingHermiteBlend(double[][] rows, double[][] originalRows, int window)
|
||||
{
|
||||
var startIndex = rows.Length - 1 - window;
|
||||
var startRow = originalRows[startIndex];
|
||||
var endRow = originalRows[^1];
|
||||
var totalDuration = endRow[0] - startRow[0];
|
||||
if (totalDuration <= 0.0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
|
||||
{
|
||||
var delta = endRow[jointIndex] - startRow[jointIndex];
|
||||
if (Math.Abs(delta) <= 1e-12)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var startVelocity = EstimateVelocity(originalRows, startIndex, jointIndex);
|
||||
var normalizedStartSlope = ClampNormalizedSlope((startVelocity * totalDuration) / delta);
|
||||
|
||||
for (var index = 1; index < window; index++)
|
||||
{
|
||||
var normalizedTime = (rows[startIndex + index][0] - startRow[0]) / totalDuration;
|
||||
var shapedValue = startRow[jointIndex]
|
||||
+ (delta * EvaluateHermiteProgress(normalizedTime, startSlope: normalizedStartSlope, endSlope: 0.0));
|
||||
var blendWeight = Math.Pow(normalizedTime, 2.0);
|
||||
rows[startIndex + index][jointIndex] = Lerp(originalRows[startIndex + index][jointIndex], shapedValue, blendWeight);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 估算给定行在原始轨迹上的一阶导,首尾退化为单边差分。
|
||||
/// </summary>
|
||||
private static double EstimateVelocity(double[][] rows, int index, int jointIndex)
|
||||
{
|
||||
if (index <= 0)
|
||||
{
|
||||
var dt = rows[1][0] - rows[0][0];
|
||||
return dt <= 0.0 ? 0.0 : (rows[1][jointIndex] - rows[0][jointIndex]) / dt;
|
||||
}
|
||||
|
||||
if (index >= rows.Length - 1)
|
||||
{
|
||||
var dt = rows[^1][0] - rows[^2][0];
|
||||
return dt <= 0.0 ? 0.0 : (rows[^1][jointIndex] - rows[^2][jointIndex]) / dt;
|
||||
}
|
||||
|
||||
var previousDt = rows[index][0] - rows[index - 1][0];
|
||||
var nextDt = rows[index + 1][0] - rows[index][0];
|
||||
if (previousDt <= 0.0 || nextDt <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
|
||||
var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
|
||||
return (backward + forward) / 2.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 估算给定行在原始轨迹上的二阶导,端点退化为 0 以避免放大边界噪声。
|
||||
/// </summary>
|
||||
private static double EstimateAcceleration(double[][] rows, int index, int jointIndex)
|
||||
{
|
||||
if (index <= 0 || index >= rows.Length - 1)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var previousDt = rows[index][0] - rows[index - 1][0];
|
||||
var nextDt = rows[index + 1][0] - rows[index][0];
|
||||
if (previousDt <= 0.0 || nextDt <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
|
||||
var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
|
||||
var averageDt = (previousDt + nextDt) / 2.0;
|
||||
return averageDt <= 0.0 ? 0.0 : (forward - backward) / averageDt;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 Hermite 累计位移曲线在 0..1 归一化时间上的进度值。
|
||||
/// </summary>
|
||||
private static double EvaluateHermiteProgress(double normalizedTime, double startSlope, double endSlope)
|
||||
{
|
||||
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = u * u;
|
||||
var u3 = u2 * u;
|
||||
var h00 = (2.0 * u3) - (3.0 * u2) + 1.0;
|
||||
var h10 = u3 - (2.0 * u2) + u;
|
||||
var h01 = (-2.0 * u3) + (3.0 * u2);
|
||||
var h11 = u3 - u2;
|
||||
return (h00 * 0.0) + (h10 * startSlope) + (h01 * 1.0) + (h11 * endSlope);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把归一化边界斜率限制在单调 Hermite 常见的稳定区间内,避免过冲和窗口内振荡。
|
||||
/// </summary>
|
||||
private static double ClampNormalizedSlope(double normalizedSlope)
|
||||
{
|
||||
if (double.IsNaN(normalizedSlope) || double.IsInfinity(normalizedSlope))
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
return Math.Clamp(normalizedSlope, 0.0, 3.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在线性插值基础上做温和混合,避免首尾窗口为了追赶锚点而产生过大的局部跃度。
|
||||
/// </summary>
|
||||
private static double Lerp(double originalValue, double shapedValue, double weight)
|
||||
{
|
||||
var clampedWeight = Math.Clamp(weight, 0.0, 1.0);
|
||||
return originalValue + ((shapedValue - originalValue) * clampedWeight);
|
||||
}
|
||||
}
|
||||
226
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
226
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
@@ -0,0 +1,226 @@
|
||||
using System.Text.Json;
|
||||
using System.Text.Json.Nodes;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 使用运行目录 Config/RobotConfig.json 持久化单机器人飞拍轨迹和机器人配置。
|
||||
/// </summary>
|
||||
public sealed class JsonFlyshotTrajectoryStore
|
||||
{
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
private readonly ILogger<JsonFlyshotTrajectoryStore>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化基于 JSON 文件的轨迹存储。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置,用于定位运行配置根目录。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器,用于反序列化已保存的轨迹。</param>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader, ILogger<JsonFlyshotTrajectoryStore>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将单条轨迹持久化到统一 RobotConfig.json,同时更新机器人配置段。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
|
||||
/// <param name="settings">当前机器人级兼容配置。</param>
|
||||
/// <param name="trajectory">要保存的已上传轨迹。</param>
|
||||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(settings);
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
|
||||
robotName,
|
||||
trajectory.Name,
|
||||
trajectory.Waypoints.Count);
|
||||
|
||||
var path = ResolveStorePath();
|
||||
var directory = Path.GetDirectoryName(path)!;
|
||||
Directory.CreateDirectory(directory);
|
||||
|
||||
JsonObject root;
|
||||
if (File.Exists(path))
|
||||
{
|
||||
var existingJson = File.ReadAllText(path);
|
||||
root = JsonNode.Parse(existingJson)?.AsObject() ?? new JsonObject();
|
||||
}
|
||||
else
|
||||
{
|
||||
root = new JsonObject();
|
||||
}
|
||||
|
||||
// 更新 robot 配置段,保持与旧版 RobotConfig.json 字段名一致。
|
||||
root["robot"] = SerializeRobotSettings(settings);
|
||||
|
||||
// 确保 flying_shots 节点存在。
|
||||
if (!root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) || flyingShotsNode is not JsonObject flyingShotsObj)
|
||||
{
|
||||
flyingShotsObj = new JsonObject();
|
||||
root["flying_shots"] = flyingShotsObj;
|
||||
}
|
||||
|
||||
flyingShotsObj[trajectory.Name] = SerializeTrajectory(trajectory);
|
||||
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
|
||||
_logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
|
||||
/// <param name="trajectoryName">要删除的轨迹名称。</param>
|
||||
public void Delete(string robotName, string trajectoryName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName);
|
||||
|
||||
var path = ResolveStorePath();
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 文件不存在 {Path}", path);
|
||||
return;
|
||||
}
|
||||
|
||||
var existingJson = File.ReadAllText(path);
|
||||
var root = JsonNode.Parse(existingJson)?.AsObject();
|
||||
if (root is null)
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 无法解析 JSON {Path}", path);
|
||||
return;
|
||||
}
|
||||
|
||||
if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj)
|
||||
{
|
||||
var removed = flyingShotsObj.Remove(trajectoryName);
|
||||
if (removed)
|
||||
{
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
_logger?.LogInformation("RobotConfig 轨迹已删除: {TrajectoryName}", trajectoryName);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载统一 RobotConfig.json 中的所有轨迹,并回传机器人配置。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
|
||||
/// <param name="settings">输出 RobotConfig.json 中的机器人配置;若文件不存在或解析失败则为 null。</param>
|
||||
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
|
||||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||||
{
|
||||
var path = ResolveStorePath();
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 无持久化数据: {Path}", path);
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 正在加载: {Path}", path);
|
||||
|
||||
var loaded = _configLoader.Load(path, _options.ResolveConfigRoot());
|
||||
settings = loaded.Robot;
|
||||
|
||||
var dict = new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
foreach (var program in loaded.Programs)
|
||||
{
|
||||
var traj = new ControllerClientCompatUploadedTrajectory(
|
||||
name: program.Value.Name,
|
||||
waypoints: program.Value.Waypoints.Select(static wp => wp.Positions),
|
||||
shotFlags: program.Value.ShotFlags,
|
||||
offsetValues: program.Value.OffsetValues,
|
||||
addressGroups: program.Value.AddressGroups.Select(static g => g.Addresses));
|
||||
dict[program.Key] = traj;
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
|
||||
robotName,
|
||||
dict.Count,
|
||||
settings?.UseDo,
|
||||
settings?.IoKeepCycles);
|
||||
|
||||
return dict;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError(ex, "RobotConfig 加载失败: {Path}", path);
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把机器人兼容配置序列化为 JSON 对象,字段名与旧版 RobotConfig.json 一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeRobotSettings(CompatibilityRobotSettings settings)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["use_do"] = JsonValue.Create(settings.UseDo),
|
||||
["io_addr"] = JsonSerializer.SerializeToNode(settings.IoAddresses),
|
||||
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
|
||||
["trigger_sample_index_offset_cycles"] = JsonValue.Create(settings.TriggerSampleIndexOffsetCycles),
|
||||
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
||||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
||||
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
|
||||
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
|
||||
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把已上传轨迹序列化为 JSON 对象,字段名与旧版 RobotConfig.json 的 flying_shots 节点一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["traj_waypoints"] = JsonSerializer.SerializeToNode(trajectory.Waypoints),
|
||||
["shot_flags"] = JsonSerializer.SerializeToNode(trajectory.ShotFlags),
|
||||
["offset_values"] = JsonSerializer.SerializeToNode(trajectory.OffsetValues),
|
||||
["addr"] = JsonSerializer.SerializeToNode(trajectory.AddressGroups)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析单程序单机器人的统一配置文件路径。
|
||||
/// </summary>
|
||||
private string ResolveStorePath()
|
||||
{
|
||||
return Path.Combine(_options.ResolveConfigRoot(), "RobotConfig.json");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,226 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 定义 HTTP-only 兼容层对外暴露的 ControllerClient 语义服务接口。
|
||||
/// </summary>
|
||||
public interface IControllerClientCompatService
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取当前兼容层对外报告的服务端版本号。
|
||||
/// </summary>
|
||||
string ServerVersion { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已经完成机器人初始化。
|
||||
/// </summary>
|
||||
bool IsSetUp { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 保存当前调用方期望连接的 replacement 服务端地址。
|
||||
/// </summary>
|
||||
/// <param name="serverIp">客户端传入的服务端 IP。</param>
|
||||
/// <param name="port">客户端传入的服务端端口。</param>
|
||||
void ConnectServer(string serverIp, int port);
|
||||
|
||||
/// <summary>
|
||||
/// 获取兼容服务端版本号。
|
||||
/// </summary>
|
||||
/// <returns>服务端版本号。</returns>
|
||||
string GetServerVersion();
|
||||
|
||||
/// <summary>
|
||||
/// 获取兼容客户端版本号。
|
||||
/// </summary>
|
||||
/// <returns>客户端版本号。</returns>
|
||||
string GetClientVersion();
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端使用的机器人名称完成机器人初始化。
|
||||
/// </summary>
|
||||
/// <param name="robotName">机器人名称。</param>
|
||||
void SetUpRobot(string robotName);
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端传入的环境文件完成机器人初始化。
|
||||
/// </summary>
|
||||
/// <param name="envFile">环境文件路径。</param>
|
||||
void SetUpRobotFromEnv(string envFile);
|
||||
|
||||
/// <summary>
|
||||
/// 设置是否显示 TCP 坐标轴。
|
||||
/// </summary>
|
||||
/// <param name="isShow">是否显示 TCP。</param>
|
||||
/// <param name="axisLength">坐标轴长度。</param>
|
||||
/// <param name="axisSize">坐标轴线宽。</param>
|
||||
void SetShowTcp(bool isShow, double axisLength, int axisSize);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前激活的控制器类型。
|
||||
/// </summary>
|
||||
/// <param name="sim">是否为仿真控制器。</param>
|
||||
void SetActiveController(bool sim);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前控制器已经建立连接。
|
||||
/// </summary>
|
||||
/// <param name="robotIp">控制器 IP。</param>
|
||||
void Connect(string robotIp);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前控制器已经断开。
|
||||
/// </summary>
|
||||
void Disconnect();
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前机器人进入使能态。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">缓冲区大小。</param>
|
||||
void EnableRobot(int bufferSize);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前机器人退出使能态。
|
||||
/// </summary>
|
||||
void DisableRobot();
|
||||
|
||||
/// <summary>
|
||||
/// 停止当前运动状态。
|
||||
/// </summary>
|
||||
void StopMove();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前控制器运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>控制器运行时状态快照。</returns>
|
||||
ControllerStateSnapshot GetControllerSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
/// <returns>当前速度倍率。</returns>
|
||||
double GetSpeedRatio();
|
||||
|
||||
/// <summary>
|
||||
/// 更新当前速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
void SetSpeedRatio(double ratio);
|
||||
|
||||
/// <summary>
|
||||
/// 写入兼容层缓存的 IO 数值。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="value">IO 值。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 读取兼容层缓存的 IO 数值。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
/// <returns>缓存中的 IO 值。</returns>
|
||||
bool GetIo(int port, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 按给定位姿和 seed 计算最近 IK。
|
||||
/// </summary>
|
||||
/// <param name="pose">目标位姿数组。</param>
|
||||
/// <param name="seed">IK seed 关节数组。</param>
|
||||
/// <returns>IK 结果关节数组。</returns>
|
||||
IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed);
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前 TCP 三维坐标。
|
||||
/// </summary>
|
||||
/// <param name="x">TCP X。</param>
|
||||
/// <param name="y">TCP Y。</param>
|
||||
/// <param name="z">TCP Z。</param>
|
||||
void SetTcp(double x, double y, double z);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前 TCP 三维坐标。
|
||||
/// </summary>
|
||||
/// <returns>TCP 数组。</returns>
|
||||
IReadOnlyList<double> GetTcp();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前关节位置。
|
||||
/// </summary>
|
||||
/// <returns>关节位置数组。</returns>
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
|
||||
/// <summary>
|
||||
/// 更新当前关节位置。
|
||||
/// </summary>
|
||||
/// <param name="jointPositions">目标关节位置。</param>
|
||||
void MoveJoint(IReadOnlyList<double> jointPositions);
|
||||
|
||||
/// <summary>
|
||||
/// 执行普通轨迹。
|
||||
/// </summary>
|
||||
/// <param name="waypoints">轨迹路点集合。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前末端位姿快照。
|
||||
/// </summary>
|
||||
/// <returns>位姿数组。</returns>
|
||||
IReadOnlyList<double> GetPose();
|
||||
|
||||
/// <summary>
|
||||
/// 上传一条飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="trajectory">飞拍轨迹。</param>
|
||||
void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory);
|
||||
|
||||
/// <summary>
|
||||
/// 列出当前已上传的飞拍轨迹名称。
|
||||
/// </summary>
|
||||
/// <returns>轨迹名称列表。</returns>
|
||||
IReadOnlyList<string> ListTrajectoryNames();
|
||||
|
||||
/// <summary>
|
||||
/// 执行指定名称的飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="options">飞拍执行参数。</param>
|
||||
void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null);
|
||||
|
||||
/// <summary>
|
||||
/// 保存指定飞拍轨迹的轨迹信息。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="method">轨迹生成方法。</param>
|
||||
void SaveTrajectoryInfo(string name, string method = "icsp");
|
||||
|
||||
/// <summary>
|
||||
/// 检查指定飞拍轨迹是否可执行。
|
||||
/// </summary>
|
||||
/// <param name="duration">输出规划轨迹总时长。</param>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="method">轨迹生成方法。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
/// <returns>轨迹是否有效。</returns>
|
||||
bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false);
|
||||
|
||||
/// <summary>
|
||||
/// 删除指定名称的飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
void DeleteTrajectory(string name);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前配置过的机器人名称。
|
||||
/// </summary>
|
||||
/// <returns>机器人名称。</returns>
|
||||
string GetRobotName();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前机器人自由度。
|
||||
/// </summary>
|
||||
/// <returns>机器人自由度。</returns>
|
||||
int GetDegreesOfFreedom();
|
||||
}
|
||||
@@ -0,0 +1,327 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// MoveJoint 轨迹生成器。
|
||||
/// 将起始关节角到目标关节角的单段运动,按速度、加速度、jerk 约束生成稠密点到点轨迹,
|
||||
/// 供 FANUC J519 伺服流逐周期下发。
|
||||
///
|
||||
/// 核心思路:路径只取关节空间直线 q=q0+(q1-q0)*s(t),时间律使用 7 阶平滑函数;
|
||||
/// 生成后再按离散采样点反算速度、加速度和 jerk,确保真实下发点也满足限制。
|
||||
/// </summary>
|
||||
internal static class MoveJointTrajectoryGenerator
|
||||
{
|
||||
/// <summary>
|
||||
/// 最小段基础时长(秒)。零位移时仍由采样对齐逻辑生成起点和终点两帧。
|
||||
/// </summary>
|
||||
private const double MinimumMoveJointDurationSeconds = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的一阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的二阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的三阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||||
|
||||
/// <summary>
|
||||
/// 单次 MoveJoint 最大采样点数上限,防止极端配置下生成过大的轨迹数组。
|
||||
/// </summary>
|
||||
private const int MaxMoveJointSampleCount = 1_000_000;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验允许的浮点容差。
|
||||
/// </summary>
|
||||
private const double DiscreteLimitTolerance = 1.000001;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验失败时最多拉长的采样周期次数。
|
||||
/// </summary>
|
||||
private const int MaxDiscreteLimitStretchIterations = 10_000;
|
||||
|
||||
/// <summary>
|
||||
/// 计算 MoveJoint 轨迹的完整结果。
|
||||
///
|
||||
/// 处理流程:
|
||||
/// 1. 根据关节限位计算连续时间律理论最短时长
|
||||
/// 2. 按 speedRatio 换算轨迹采样周期,并将时长对齐到整数个采样间隔
|
||||
/// 3. 用 7 阶平滑点到点时间律生成稠密轨迹点
|
||||
/// 4. 按离散点反查速度、加速度和 jerk,必要时拉长时长重算
|
||||
/// 5. 封装为 TrajectoryResult 返回
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置,含自由度数和关节限位。</param>
|
||||
/// <param name="startJoints">起始关节角(弧度)。</param>
|
||||
/// <param name="targetJoints">目标关节角(弧度)。</param>
|
||||
/// <param name="speedRatio">速度倍率,必须大于 0;当前链路中用于换算轨迹采样周期。</param>
|
||||
/// <param name="logger">可选的诊断日志。</param>
|
||||
public static TrajectoryResult CreateResult(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double speedRatio,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(startJoints);
|
||||
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
if (startJoints.Count != robot.DegreesOfFreedom || targetJoints.Count != robot.DegreesOfFreedom)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
|
||||
}
|
||||
|
||||
var requestedDurationSeconds = ResolveDurationSeconds(robot, startJoints, targetJoints);
|
||||
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds * speedRatio;
|
||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
|
||||
var stretchCount = 0;
|
||||
while (!SatisfiesDiscreteJointLimits(robot, denseJointTrajectory))
|
||||
{
|
||||
stretchCount++;
|
||||
if (stretchCount > MaxDiscreteLimitStretchIterations)
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint duration cannot be stretched enough to satisfy joint limits.");
|
||||
}
|
||||
|
||||
// 连续时间律满足限位后,仍以实际离散点为准;不满足时逐周期拉长后重采样。
|
||||
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
|
||||
denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
}
|
||||
|
||||
logger?.LogDebug(
|
||||
"MoveJointTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
|
||||
requestedDurationSeconds,
|
||||
durationSeconds,
|
||||
speedRatio,
|
||||
samplePeriodSeconds,
|
||||
denseJointTrajectory.Count,
|
||||
stretchCount);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: "move-joint",
|
||||
method: PlanningMethod.Doubles,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 2,
|
||||
plannedWaypointCount: denseJointTrajectory.Count,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 7 阶平滑点到点时间律和每轴限位,计算 MoveJoint 理论最短时长。
|
||||
///
|
||||
/// 时间律为 s(u)=35u^4-84u^5+70u^6-20u^7,其中 u=t/T。
|
||||
/// 各轴位移 d_i 共用同一个 s(t),所以每轴分别按 d_i 放大速度、加速度和 jerk,再取全局最大时长。
|
||||
/// </summary>
|
||||
internal static double ResolveDurationSeconds(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var duration = MinimumMoveJointDurationSeconds;
|
||||
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
|
||||
if (distance <= 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var limit = robot.JointLimits[index];
|
||||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
|
||||
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
|
||||
return duration;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将请求时长向上对齐到整数个采样周期,确保轨迹末帧正好落在 duration 处。
|
||||
/// </summary>
|
||||
/// <param name="durationSeconds">请求的理论最短时长(秒)。</param>
|
||||
/// <param name="samplePeriodSeconds">采样周期(秒)。</param>
|
||||
/// <returns>对齐后的时长,为 samplePeriodSeconds 的整数倍。</returns>
|
||||
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
||||
|
||||
return intervals * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成从起始关节角到目标关节角的稠密等时间隔轨迹点序列。
|
||||
///
|
||||
/// 每行格式:[time_seconds, joint_0, joint_1, ..., joint_n-1]。
|
||||
/// </summary>
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double durationSeconds,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
|
||||
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
|
||||
|
||||
for (var index = 0L; index < sampleCount; index++)
|
||||
{
|
||||
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
|
||||
rows.Add(CreateRow(time, durationSeconds, startJoints, targetJoints));
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 7 阶平滑点到点时间律的位置归一化值。
|
||||
/// </summary>
|
||||
/// <param name="normalizedTime">归一化时间 u,取值会被限制在 [0, 1]。</param>
|
||||
/// <returns>归一化位置 s(u),范围 [0, 1]。</returns>
|
||||
internal static double EvaluateSmoothPtpPositionScale(double normalizedTime)
|
||||
{
|
||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = clamped * clamped;
|
||||
var u4 = u2 * u2;
|
||||
|
||||
return u4 * (35.0 + (clamped * (-84.0 + (clamped * (70.0 - (20.0 * clamped))))));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算时长对应的采样间隔数(向上取整)。
|
||||
/// 采样间隔数 + 1 = 采样点数,因为轨迹包含起点和终点。
|
||||
/// </summary>
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint sample count is not representable.");
|
||||
}
|
||||
|
||||
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
|
||||
intervals = Math.Max(1, intervals);
|
||||
if (intervals + 1 > MaxMoveJointSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造单个轨迹行:[time_seconds, joint_0, ..., joint_N-1]。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> CreateRow(
|
||||
double timeSeconds,
|
||||
double durationSeconds,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||
var scale = EvaluateSmoothPtpPositionScale(u);
|
||||
var row = new double[startJoints.Count + 1];
|
||||
row[0] = Math.Round(timeSeconds, 9);
|
||||
|
||||
for (var index = 0; index < startJoints.Count; index++)
|
||||
{
|
||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * scale);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 用生成后的离散采样点复核每轴速度、加速度和 jerk,避免连续时间律在采样后仍出现差分越限。
|
||||
/// </summary>
|
||||
private static bool SatisfiesDiscreteJointLimits(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
double? previousTime = null;
|
||||
double[]? previousPositions = null;
|
||||
double[]? previousVelocities = null;
|
||||
double[]? previousAccelerations = null;
|
||||
|
||||
foreach (var row in rows)
|
||||
{
|
||||
var currentTime = row[0];
|
||||
var currentPositions = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
currentPositions[index] = row[index + 1];
|
||||
}
|
||||
|
||||
if (previousTime is not null && previousPositions is not null)
|
||||
{
|
||||
var dt = currentTime - previousTime.Value;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint dense trajectory timestamps must be strictly increasing.");
|
||||
}
|
||||
|
||||
var velocities = new double[robot.DegreesOfFreedom];
|
||||
var accelerations = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var limit = robot.JointLimits[index];
|
||||
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||||
if (Math.Abs(velocities[index]) > limit.VelocityLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
accelerations[index] = previousVelocities is null
|
||||
? 0.0
|
||||
: (velocities[index] - previousVelocities[index]) / dt;
|
||||
if (Math.Abs(accelerations[index]) > limit.AccelerationLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (previousAccelerations is not null)
|
||||
{
|
||||
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
|
||||
if (Math.Abs(jerk) > limit.JerkLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocities = velocities;
|
||||
previousAccelerations = accelerations;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPositions = currentPositions;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
49
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
Normal file
49
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
Normal file
@@ -0,0 +1,49 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Triggering;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示兼容层执行轨迹前生成的完整规划结果包。
|
||||
/// </summary>
|
||||
public sealed class PlannedExecutionBundle
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一份执行规划结果包。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||
/// <param name="shotTimeline">飞拍触发时间轴。</param>
|
||||
/// <param name="result">对运行时和监控层暴露的规划结果。</param>
|
||||
public PlannedExecutionBundle(
|
||||
PlannedTrajectory plannedTrajectory,
|
||||
ShotTimeline shotTimeline,
|
||||
TrajectoryResult result,
|
||||
FlyshotPreparedExecution? preparedExecution = null)
|
||||
{
|
||||
PlannedTrajectory = plannedTrajectory ?? throw new ArgumentNullException(nameof(plannedTrajectory));
|
||||
ShotTimeline = shotTimeline ?? throw new ArgumentNullException(nameof(shotTimeline));
|
||||
Result = result ?? throw new ArgumentNullException(nameof(result));
|
||||
PreparedExecution = preparedExecution;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划后的轨迹。
|
||||
/// </summary>
|
||||
public PlannedTrajectory PlannedTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞拍触发时间轴。
|
||||
/// </summary>
|
||||
public ShotTimeline ShotTimeline { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取运行时可消费的规划结果。
|
||||
/// </summary>
|
||||
public TrajectoryResult Result { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞拍链路预先准备好的最终发送队列;普通轨迹与 move_joint 为 null。
|
||||
/// </summary>
|
||||
public FlyshotPreparedExecution? PreparedExecution { get; }
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示普通轨迹执行接口的可选参数,字段名对齐旧 `ControllerClient::ExecuteTrajectory`。
|
||||
/// </summary>
|
||||
public sealed class TrajectoryExecutionOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化普通轨迹执行参数。
|
||||
/// </summary>
|
||||
/// <param name="method">轨迹生成方法,支持 `icsp` 或 `doubles`。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false)
|
||||
{
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
30
src/Flyshot.Core.Config/LoadedRobotModel.cs
Normal file
30
src/Flyshot.Core.Config/LoadedRobotModel.cs
Normal file
@@ -0,0 +1,30 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
/// <summary>
|
||||
/// 表示一次 JSON 模型解析后生成的完整机器人模型视图集合。
|
||||
/// </summary>
|
||||
public sealed class LoadedRobotModel
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化完整机器人模型视图集合。
|
||||
/// </summary>
|
||||
/// <param name="profile">规划和运行时使用的关节约束视图。</param>
|
||||
/// <param name="kinematicsModel">正运动学导出使用的几何链视图。</param>
|
||||
public LoadedRobotModel(RobotProfile profile, RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
Profile = profile ?? throw new ArgumentNullException(nameof(profile));
|
||||
KinematicsModel = kinematicsModel ?? throw new ArgumentNullException(nameof(kinematicsModel));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划和运行时使用的关节约束视图。
|
||||
/// </summary>
|
||||
public RobotProfile Profile { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取正运动学导出使用的几何链视图。
|
||||
/// </summary>
|
||||
public RobotKinematicsModel KinematicsModel { get; }
|
||||
}
|
||||
@@ -22,7 +22,7 @@ public enum CompatibilityPathStyle
|
||||
public static class PathCompatibility
|
||||
{
|
||||
/// <summary>
|
||||
/// 按旧系统常见目录约定解析配置文件路径。
|
||||
/// 按当前服务配置目录约定解析配置文件路径。
|
||||
/// </summary>
|
||||
/// <param name="configPath">调用方传入的原始配置路径。</param>
|
||||
/// <param name="repoRoot">当前兼容搜索的仓库根目录。</param>
|
||||
@@ -48,11 +48,10 @@ public static class PathCompatibility
|
||||
}
|
||||
|
||||
var normalizedRepoRoot = Path.GetFullPath(repoRoot);
|
||||
var fileName = Path.GetFileName(rawPath);
|
||||
var checkedPaths = new List<string>();
|
||||
|
||||
// 先按最常见的候选路径顺序尝试,保持与旧工具链相近的定位逻辑。
|
||||
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath, fileName))
|
||||
// 相对路径只允许落在当前服务根目录的 Config 下,避免隐式回退到父工作区旧文件。
|
||||
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath))
|
||||
{
|
||||
var fullCandidate = Path.GetFullPath(candidate);
|
||||
if (checkedPaths.Contains(fullCandidate, StringComparer.OrdinalIgnoreCase))
|
||||
@@ -67,18 +66,6 @@ public static class PathCompatibility
|
||||
}
|
||||
}
|
||||
|
||||
// 最后一层兜底按文件名全仓库搜索,但只接受唯一命中,避免同名配置误判。
|
||||
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);
|
||||
@@ -106,15 +93,11 @@ public static class PathCompatibility
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 枚举旧系统中最常见的配置候选路径。
|
||||
/// 枚举当前服务配置目录下允许的配置候选路径。
|
||||
/// </summary>
|
||||
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath, string fileName)
|
||||
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath)
|
||||
{
|
||||
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);
|
||||
yield return Path.Combine(repoRoot, "Config", rawPath);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
@@ -15,9 +16,12 @@ public sealed class CompatibilityRobotSettings
|
||||
bool useDo,
|
||||
IEnumerable<int> ioAddresses,
|
||||
int ioKeepCycles,
|
||||
int triggerSampleIndexOffsetCycles,
|
||||
double accLimitScale,
|
||||
double jerkLimitScale,
|
||||
int adaptIcspTryNum)
|
||||
int adaptIcspTryNum,
|
||||
double planningSpeedScale = 1.0,
|
||||
bool smoothStartStopTiming = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(ioAddresses);
|
||||
|
||||
@@ -26,6 +30,11 @@ public sealed class CompatibilityRobotSettings
|
||||
throw new ArgumentOutOfRangeException(nameof(ioKeepCycles), "IO 保持周期不能为负数。");
|
||||
}
|
||||
|
||||
if (triggerSampleIndexOffsetCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(triggerSampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
|
||||
}
|
||||
|
||||
if (accLimitScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
|
||||
@@ -36,6 +45,11 @@ public sealed class CompatibilityRobotSettings
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
if (planningSpeedScale <= 0.0 || double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (adaptIcspTryNum < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
|
||||
@@ -51,9 +65,12 @@ public sealed class CompatibilityRobotSettings
|
||||
UseDo = useDo;
|
||||
IoAddresses = copiedIoAddresses;
|
||||
IoKeepCycles = ioKeepCycles;
|
||||
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
|
||||
AccLimitScale = accLimitScale;
|
||||
JerkLimitScale = jerkLimitScale;
|
||||
AdaptIcspTryNum = adaptIcspTryNum;
|
||||
PlanningSpeedScale = planningSpeedScale;
|
||||
SmoothStartStopTiming = smoothStartStopTiming;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -71,6 +88,12 @@ public sealed class CompatibilityRobotSettings
|
||||
/// </summary>
|
||||
public int IoKeepCycles { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取触发绑定到最佳 sample 后,还要再向后偏移的命令周期数。
|
||||
/// 该值作用在 J519 命令 sample 时间轴,不是 60015 状态反馈周期。
|
||||
/// </summary>
|
||||
public int TriggerSampleIndexOffsetCycles { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取加速度全局倍率。
|
||||
/// </summary>
|
||||
@@ -81,6 +104,16 @@ public sealed class CompatibilityRobotSettings
|
||||
/// </summary>
|
||||
public double JerkLimitScale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划阶段的全局速度倍率,只影响 JointTraj 基准时间,不等同于运行时 J519 下发速度倍率。
|
||||
/// </summary>
|
||||
public double PlanningSpeedScale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
|
||||
/// </summary>
|
||||
public bool SmoothStartStopTiming { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取自适应补点最大尝试次数。
|
||||
/// </summary>
|
||||
@@ -131,6 +164,17 @@ public sealed class LoadedRobotConfig
|
||||
/// </summary>
|
||||
public sealed class RobotConfigLoader
|
||||
{
|
||||
private readonly ILogger<RobotConfigLoader>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 RobotConfigLoader。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public RobotConfigLoader(ILogger<RobotConfigLoader>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载一份旧版 RobotConfig.json。
|
||||
/// </summary>
|
||||
@@ -139,6 +183,8 @@ public sealed class RobotConfigLoader
|
||||
/// <returns>规范化后的配置文档。</returns>
|
||||
public LoadedRobotConfig Load(string configPath, string? repoRoot = null)
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 开始加载: configPath={ConfigPath}, repoRoot={RepoRoot}", configPath, repoRoot);
|
||||
|
||||
var resolvedRepoRoot = ResolveRepoRoot(repoRoot);
|
||||
var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot);
|
||||
|
||||
@@ -151,9 +197,12 @@ public sealed class RobotConfigLoader
|
||||
useDo: ReadBoolean(robotElement, "use_do", defaultValue: false),
|
||||
ioAddresses: ReadIntArray(robotElement, "io_addr"),
|
||||
ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0),
|
||||
triggerSampleIndexOffsetCycles: ReadInt(robotElement, "trigger_sample_index_offset_cycles", defaultValue: 7),
|
||||
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
|
||||
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
|
||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0));
|
||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0),
|
||||
planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0),
|
||||
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
|
||||
|
||||
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
|
||||
foreach (var programElement in flyingShotsElement.EnumerateObject())
|
||||
@@ -163,6 +212,10 @@ public sealed class RobotConfigLoader
|
||||
programs.Add(programName, program);
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
|
||||
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
|
||||
|
||||
return new LoadedRobotConfig(
|
||||
sourcePath: resolvedConfigPath,
|
||||
robot: robot,
|
||||
@@ -253,7 +306,7 @@ public sealed class RobotConfigLoader
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 推断仓库根目录,优先使用调用方显式传入的值。
|
||||
/// 推断当前 replacement 仓库根目录,优先使用调用方显式传入的值。
|
||||
/// </summary>
|
||||
private static string ResolveRepoRoot(string? repoRoot)
|
||||
{
|
||||
@@ -267,7 +320,7 @@ public sealed class RobotConfigLoader
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||||
return current.FullName;
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
|
||||
@@ -1,28 +1,49 @@
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
/// <summary>
|
||||
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
|
||||
/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
|
||||
/// </summary>
|
||||
public sealed class RobotModelLoader
|
||||
{
|
||||
private const uint JsonChunkType = 0x4E4F534A;
|
||||
private readonly ILogger<RobotModelLoader>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
|
||||
/// 初始化 RobotModelLoader。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">.robot 文件路径。</param>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||
/// <param name="accLimitScale">加速度全局倍率。</param>
|
||||
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
||||
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
|
||||
public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
return LoadProfileAndKinematics(modelPath, accLimitScale, jerkLimitScale).Profile;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载机器人 JSON 文件并一次性生成规划约束视图与运动学几何视图。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||
/// <param name="accLimitScale">加速度全局倍率。</param>
|
||||
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
||||
/// <returns>包含规划约束视图和运动学几何视图的加载结果。</returns>
|
||||
public LoadedRobotModel LoadProfileAndKinematics(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(modelPath))
|
||||
{
|
||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
||||
throw new ArgumentException("机器人 JSON 路径不能为空。", nameof(modelPath));
|
||||
}
|
||||
|
||||
if (accLimitScale <= 0.0)
|
||||
@@ -35,15 +56,72 @@ public sealed class RobotModelLoader
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotModel JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
using var document = JsonDocument.Parse(File.ReadAllText(resolvedModelPath));
|
||||
|
||||
var robotBody = FindRobotBody(document.RootElement);
|
||||
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
||||
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
||||
: Path.GetFileNameWithoutExtension(resolvedModelPath);
|
||||
|
||||
var profile = BuildProfile(robotBody, profileName, resolvedModelPath, accLimitScale, jerkLimitScale);
|
||||
var kinematicsModel = BuildKinematicsModel(robotBody, profileName);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotModel JSON 加载完成: profileName={ProfileName}, dof={Dof}, 几何关节数={JointCount}, resolvedPath={ResolvedPath}",
|
||||
profile.Name, profile.DegreesOfFreedom, kinematicsModel.Joints.Count, resolvedModelPath);
|
||||
|
||||
return new LoadedRobotModel(profile, kinematicsModel);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载机器人 JSON 文件并生成运动学侧需要的完整几何模型。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||
/// <returns>包含完整关节几何链的运动学模型。</returns>
|
||||
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
|
||||
{
|
||||
return LoadProfileAndKinematics(modelPath).KinematicsModel;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在 robotics.bodies 中找到当前现场机器人主体。
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var body in bodies.EnumerateArray())
|
||||
{
|
||||
if (body.TryGetProperty("joints", out _) && body.TryGetProperty("name", out _))
|
||||
{
|
||||
return body;
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidDataException("未在机器人 JSON 中找到包含 joints 的机器人主体。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从机器人主体构造规划约束视图。
|
||||
/// </summary>
|
||||
private RobotProfile BuildProfile(JsonElement robotBody, string profileName, string resolvedModelPath, double accLimitScale, double jerkLimitScale)
|
||||
{
|
||||
var jointLimits = new List<JointLimit>();
|
||||
var jointCouplings = new List<JointCoupling>();
|
||||
|
||||
@@ -67,15 +145,22 @@ public sealed class RobotModelLoader
|
||||
{
|
||||
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
|
||||
?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
|
||||
|
||||
var multiplier = coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0;
|
||||
var offset = coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0;
|
||||
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));
|
||||
multiplier: multiplier,
|
||||
offset: offset));
|
||||
_logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var jointLimit in jointLimits)
|
||||
{
|
||||
_logger?.LogInformation("关节 {JointName} 的限制值: 速度={VelocityLimit}, 加速度={AccelerationLimit}, Jerk={JerkLimit}", jointLimit.JointName, jointLimit.VelocityLimit, jointLimit.AccelerationLimit, jointLimit.JerkLimit);
|
||||
}
|
||||
|
||||
return new RobotProfile(
|
||||
name: profileName,
|
||||
modelPath: resolvedModelPath,
|
||||
@@ -87,99 +172,32 @@ public sealed class RobotModelLoader
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 GLB 文件中提取 JSON chunk 文本。
|
||||
/// 从机器人主体构造正运动学几何视图。
|
||||
/// </summary>
|
||||
private static string ReadJsonChunk(string modelPath)
|
||||
private RobotKinematicsModel BuildKinematicsModel(JsonElement robotBody, string profileName)
|
||||
{
|
||||
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。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在 robotics.bodies 中找到 type=2 的机器人主体。
|
||||
/// </summary>
|
||||
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 的机器人主体。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成运动学侧需要的完整几何模型。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">.robot 文件路径。</param>
|
||||
/// <returns>包含完整关节几何链的运动学模型。</returns>
|
||||
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<RobotJointGeometry>();
|
||||
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
|
||||
{
|
||||
var jointName = jointElement.GetProperty("name").GetString()
|
||||
?? throw new InvalidDataException("关节缺少 name。");
|
||||
// jointType: 关节类型编码;用于区分旋转关节/其他结构关节,后续几何链路可据此决定求解策略。
|
||||
var jointType = jointElement.TryGetProperty("type", out var typeElement)
|
||||
? typeElement.GetInt32()
|
||||
: 0;
|
||||
// origin: 关节局部原点配置,格式通常为 [x, y, z, qx, qy, qz, qw],定义父坐标到关节坐标的位姿。
|
||||
var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
||||
// axis: 关节运动轴;部分模型为 4 元组 [x, y, z, scale],其中方向向量用于正运动学雅可比计算。
|
||||
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;
|
||||
// originXyz: 平移分量 (x,y,z),用于构建关节在父链路下的位置偏移。
|
||||
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 };
|
||||
// originQuat: 旋转分量 (qx,qy,qz,qw),用于构建关节在父链路下的姿态;缺省时回退单位四元数。
|
||||
var originQuat = origin.Length >= 7 ? origin[3..7] : [0.0, 0.0, 0.0, 1.0];
|
||||
|
||||
// coupleMaster/coupleMultiplier/coupleOffset: 关节耦合参数,描述 slave 关节如何由 master 关节线性映射得到。
|
||||
// 典型关系: slave = master * multiplier + offset。
|
||||
string? coupleMaster = null;
|
||||
double coupleMultiplier = 0.0;
|
||||
double coupleOffset = 0.0;
|
||||
@@ -190,19 +208,45 @@ public sealed class RobotModelLoader
|
||||
coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
|
||||
}
|
||||
|
||||
var parentLink = jointElement.GetProperty("parent").GetString() ?? string.Empty;
|
||||
var childLink = jointElement.GetProperty("child").GetString() ?? string.Empty;
|
||||
_logger?.LogInformation(
|
||||
"几何关节解析: name={JointName}, parent={Parent}, child={Child}, type={JointType}, axis={Axis}, originXyz={OriginXyz}, originQuat={OriginQuat}, coupleMaster={CoupleMaster}, coupleMultiplier={CoupleMultiplier}, coupleOffset={CoupleOffset}",
|
||||
jointName,
|
||||
parentLink,
|
||||
childLink,
|
||||
jointType,
|
||||
string.Join(", ", axisVector.Select(static v => v.ToString("G17"))),
|
||||
string.Join(", ", originXyz.Select(static v => v.ToString("G17"))),
|
||||
string.Join(", ", originQuat.Select(static v => v.ToString("G17"))),
|
||||
coupleMaster ?? "<none>",
|
||||
coupleMultiplier,
|
||||
coupleOffset);
|
||||
|
||||
joints.Add(new RobotJointGeometry(
|
||||
// name: 当前关节名,作为几何链和耦合关系的主键。
|
||||
name: jointName,
|
||||
parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
|
||||
child: jointElement.GetProperty("child").GetString() ?? string.Empty,
|
||||
// parent: 父 link 名称,用于串起机器人树结构。
|
||||
parent: parentLink,
|
||||
// child: 子 link 名称,标识该关节输出到哪个连杆。
|
||||
child: childLink,
|
||||
// jointType: 关节类型编码,供运动学模型区分计算路径。
|
||||
jointType: jointType,
|
||||
// axis: 关节轴方向向量,决定旋转/平移沿哪个局部方向发生。
|
||||
axis: axisVector,
|
||||
// originXyz: 关节原点平移分量。
|
||||
originXyz: originXyz,
|
||||
// originQuatXyzw: 关节原点旋转四元数分量。
|
||||
originQuatXyzw: originQuat,
|
||||
// coupleMaster: 耦合主关节名(无耦合时为 null)。
|
||||
coupleMaster: coupleMaster,
|
||||
// coupleMultiplier: 耦合线性比例系数。
|
||||
coupleMultiplier: coupleMultiplier,
|
||||
// coupleOffset: 耦合常量偏移量。
|
||||
coupleOffset: coupleOffset));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("几何模型构建完成: profileName={ProfileName}, jointCount={JointCount}", profileName, joints.Count);
|
||||
return new RobotKinematicsModel(name: profileName, joints: joints);
|
||||
}
|
||||
|
||||
|
||||
@@ -18,7 +18,14 @@ public sealed class ControllerStateSnapshot
|
||||
double speedRatio,
|
||||
IEnumerable<double>? jointPositions = null,
|
||||
IEnumerable<double>? cartesianPose = null,
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null)
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null,
|
||||
IEnumerable<uint>? stateTailWords = null,
|
||||
byte? j519Status = null,
|
||||
uint? j519Sequence = null,
|
||||
bool? j519AcceptsCommand = null,
|
||||
bool? j519ReceivedCommand = null,
|
||||
bool? j519SystemReady = null,
|
||||
bool? j519RobotInMotion = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(connectionState))
|
||||
{
|
||||
@@ -34,6 +41,7 @@ public sealed class ControllerStateSnapshot
|
||||
var copiedJointPositions = jointPositions?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedCartesianPose = cartesianPose?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedActiveAlarms = activeAlarms?.ToArray() ?? Array.Empty<RuntimeAlarm>();
|
||||
var copiedStateTailWords = stateTailWords?.ToArray() ?? Array.Empty<uint>();
|
||||
|
||||
CapturedAt = capturedAt;
|
||||
ConnectionState = connectionState;
|
||||
@@ -43,6 +51,13 @@ public sealed class ControllerStateSnapshot
|
||||
JointPositions = copiedJointPositions;
|
||||
CartesianPose = copiedCartesianPose;
|
||||
ActiveAlarms = copiedActiveAlarms;
|
||||
StateTailWords = copiedStateTailWords;
|
||||
J519Status = j519Status;
|
||||
J519Sequence = j519Sequence;
|
||||
J519AcceptsCommand = j519AcceptsCommand;
|
||||
J519ReceivedCommand = j519ReceivedCommand;
|
||||
J519SystemReady = j519SystemReady;
|
||||
J519RobotInMotion = j519RobotInMotion;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -92,4 +107,46 @@ public sealed class ControllerStateSnapshot
|
||||
/// </summary>
|
||||
[JsonPropertyName("activeAlarms")]
|
||||
public IReadOnlyList<RuntimeAlarm> ActiveAlarms { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 10010 状态帧尾部原始状态字,仅用于诊断,不直接推断运行语义。
|
||||
/// </summary>
|
||||
[JsonPropertyName("stateTailWords")]
|
||||
public IReadOnlyList<uint> StateTailWords { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应的原始状态字节;没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Status")]
|
||||
public byte? J519Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应序号;没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Sequence")]
|
||||
public uint? J519Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 accept_cmd 状态位;没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519AcceptsCommand")]
|
||||
public bool? J519AcceptsCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 received_cmd 状态位;没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519ReceivedCommand")]
|
||||
public bool? J519ReceivedCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 sysrdy 状态位;没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519SystemReady")]
|
||||
public bool? J519SystemReady { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 rbt_inmotion 状态位;没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519RobotInMotion")]
|
||||
public bool? J519RobotInMotion { get; }
|
||||
}
|
||||
|
||||
202
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
Normal file
202
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
Normal file
@@ -0,0 +1,202 @@
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍链路在进入运行时之前就已经准备完成的最终发送结果。
|
||||
/// </summary>
|
||||
public sealed class FlyshotPreparedExecution
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一份飞拍最终发送结果。
|
||||
/// </summary>
|
||||
/// <param name="samples">最终 8ms 发送点列。</param>
|
||||
/// <param name="triggerBindings">与最终发送点列对齐的触发绑定结果。</param>
|
||||
/// <param name="timingRows">与最终发送点列一致的时间映射诊断行。</param>
|
||||
/// <param name="jerkRows">与最终发送点列一致的跃度诊断行。</param>
|
||||
/// <param name="requestSpeedRatio">请求的执行倍率。</param>
|
||||
/// <param name="finalSpeedRatio">通过离散校验后实际采用的保守倍率。</param>
|
||||
/// <param name="finalDurationSeconds">最终发送总时长,单位为秒。</param>
|
||||
/// <param name="stretchIterationCount">自动拉长执行时长的迭代次数。</param>
|
||||
public FlyshotPreparedExecution(
|
||||
IEnumerable<FlyshotPreparedSample> samples,
|
||||
IEnumerable<FlyshotPreparedTriggerBinding> triggerBindings,
|
||||
IEnumerable<IReadOnlyList<double>> timingRows,
|
||||
IEnumerable<IReadOnlyList<double>> jerkRows,
|
||||
double requestSpeedRatio,
|
||||
double finalSpeedRatio,
|
||||
double finalDurationSeconds,
|
||||
int stretchIterationCount)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
ArgumentNullException.ThrowIfNull(triggerBindings);
|
||||
ArgumentNullException.ThrowIfNull(timingRows);
|
||||
ArgumentNullException.ThrowIfNull(jerkRows);
|
||||
|
||||
if (requestSpeedRatio <= 0.0 || double.IsNaN(requestSpeedRatio) || double.IsInfinity(requestSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(requestSpeedRatio), "请求速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalSpeedRatio), "最终速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (finalDurationSeconds < 0.0 || double.IsNaN(finalDurationSeconds) || double.IsInfinity(finalDurationSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalDurationSeconds), "最终发送总时长必须是有限非负数。");
|
||||
}
|
||||
|
||||
if (stretchIterationCount < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "拉长迭代次数必须是非负整数。");
|
||||
}
|
||||
|
||||
Samples = samples.Select(static sample => sample).ToArray();
|
||||
TriggerBindings = triggerBindings.Select(static binding => binding).ToArray();
|
||||
TimingRows = timingRows.Select(static row => row.ToArray()).ToArray();
|
||||
JerkRows = jerkRows.Select(static row => row.ToArray()).ToArray();
|
||||
RequestSpeedRatio = requestSpeedRatio;
|
||||
FinalSpeedRatio = finalSpeedRatio;
|
||||
FinalDurationSeconds = finalDurationSeconds;
|
||||
StretchIterationCount = stretchIterationCount;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终 8ms 发送点列。
|
||||
/// </summary>
|
||||
public IReadOnlyList<FlyshotPreparedSample> Samples { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与最终发送点列对齐的触发绑定结果。
|
||||
/// </summary>
|
||||
public IReadOnlyList<FlyshotPreparedTriggerBinding> TriggerBindings { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与最终发送点列一致的时间映射诊断行。
|
||||
/// </summary>
|
||||
public IReadOnlyList<IReadOnlyList<double>> TimingRows { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与最终发送点列一致的跃度诊断行。
|
||||
/// </summary>
|
||||
public IReadOnlyList<IReadOnlyList<double>> JerkRows { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取请求的执行倍率。
|
||||
/// </summary>
|
||||
public double RequestSpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取通过离散校验后实际采用的保守倍率。
|
||||
/// </summary>
|
||||
public double FinalSpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终发送总时长,单位为秒。
|
||||
/// </summary>
|
||||
public double FinalDurationSeconds { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取自动拉长执行时长的迭代次数。
|
||||
/// </summary>
|
||||
public int StretchIterationCount { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍最终发送队列中的一个 8ms 发送点。
|
||||
/// </summary>
|
||||
public sealed class FlyshotPreparedSample
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条最终发送点。
|
||||
/// </summary>
|
||||
/// <param name="sampleIndex">发送周期序号。</param>
|
||||
/// <param name="sendTime">物理发送时间,单位为秒。</param>
|
||||
/// <param name="trajectoryTime">回映射到规划轨迹的采样时间,单位为秒。</param>
|
||||
/// <param name="speedRatio">生成该发送点时采用的执行倍率。</param>
|
||||
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
|
||||
public FlyshotPreparedSample(
|
||||
long sampleIndex,
|
||||
double sendTime,
|
||||
double trajectoryTime,
|
||||
double speedRatio,
|
||||
IReadOnlyList<double> jointsDegrees)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(jointsDegrees);
|
||||
SampleIndex = sampleIndex;
|
||||
SendTime = sendTime;
|
||||
TrajectoryTime = trajectoryTime;
|
||||
SpeedRatio = speedRatio;
|
||||
JointsDegrees = jointsDegrees.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取发送周期序号。
|
||||
/// </summary>
|
||||
public long SampleIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取物理发送时间,单位为秒。
|
||||
/// </summary>
|
||||
public double SendTime { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取回映射到规划轨迹的采样时间,单位为秒。
|
||||
/// </summary>
|
||||
public double TrajectoryTime { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取生成该发送点时采用的执行倍率。
|
||||
/// </summary>
|
||||
public double SpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 下发使用的角度制关节目标。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointsDegrees { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示理论触发事件最终绑定到哪个发送点的结果。
|
||||
/// </summary>
|
||||
public sealed class FlyshotPreparedTriggerBinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条最终触发绑定结果。
|
||||
/// </summary>
|
||||
/// <param name="trigger">理论触发事件。</param>
|
||||
/// <param name="sample">最终绑定的发送点。</param>
|
||||
/// <param name="sampleIndex">最终绑定到的发送点索引。</param>
|
||||
/// <param name="foundInWindow">是否在理论搜索窗口内找到该发送点。</param>
|
||||
public FlyshotPreparedTriggerBinding(
|
||||
TrajectoryDoEvent trigger,
|
||||
FlyshotPreparedSample sample,
|
||||
int sampleIndex,
|
||||
bool foundInWindow)
|
||||
{
|
||||
Trigger = trigger ?? throw new ArgumentNullException(nameof(trigger));
|
||||
Sample = sample ?? throw new ArgumentNullException(nameof(sample));
|
||||
SampleIndex = sampleIndex;
|
||||
FoundInWindow = foundInWindow;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取理论触发事件。
|
||||
/// </summary>
|
||||
public TrajectoryDoEvent Trigger { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终绑定的发送点。
|
||||
/// </summary>
|
||||
public FlyshotPreparedSample Sample { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终绑定到的发送点索引。
|
||||
/// </summary>
|
||||
public int SampleIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否在理论搜索窗口内完成绑定。
|
||||
/// </summary>
|
||||
public bool FoundInWindow { get; }
|
||||
}
|
||||
@@ -3,7 +3,7 @@ using System.Text.Json.Serialization;
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// 描述机器人运动学链所需的完整关节几何信息,从 .robot GLB 中提取。
|
||||
/// 描述机器人运动学链所需的完整关节几何信息,从现场固化的机器人 JSON 中提取。
|
||||
///
|
||||
/// 为什么与 RobotProfile 分开?
|
||||
/// ---
|
||||
|
||||
@@ -3,12 +3,12 @@ using System.Text.Json.Serialization;
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// Describes the robot model contract consumed by planning and runtime orchestration.
|
||||
/// 描述规划与运行时编排共同使用的机器人模型契约。
|
||||
/// </summary>
|
||||
public sealed class RobotProfile
|
||||
{
|
||||
/// <summary>
|
||||
/// Initializes a new robot profile with validated joint limits and coupling metadata.
|
||||
/// 使用已校验的关节约束与耦合元数据初始化机器人画像。
|
||||
/// </summary>
|
||||
public RobotProfile(
|
||||
string name,
|
||||
@@ -47,7 +47,7 @@ public sealed class RobotProfile
|
||||
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();
|
||||
|
||||
@@ -66,55 +66,55 @@ public sealed class RobotProfile
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the robot profile name exposed to the rest of the runtime.
|
||||
/// 获取对运行时其余模块暴露的机器人画像名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("name")]
|
||||
public string Name { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the source path of the robot model file.
|
||||
/// 获取机器人模型文件的来源路径。
|
||||
/// </summary>
|
||||
[JsonPropertyName("modelPath")]
|
||||
public string ModelPath { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the active revolute degree-of-freedom count.
|
||||
/// 获取当前生效的旋转关节自由度数量。
|
||||
/// </summary>
|
||||
[JsonPropertyName("degreesOfFreedom")]
|
||||
public int DegreesOfFreedom { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the validated per-joint kinematic limits.
|
||||
/// 获取按关节校验后的运动学约束。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jointLimits")]
|
||||
public IReadOnlyList<JointLimit> JointLimits { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets optional joint coupling metadata parsed from the robot model.
|
||||
/// 获取从机器人模型解析出的可选关节耦合元数据。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jointCouplings")]
|
||||
public IReadOnlyList<JointCoupling> JointCouplings { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the servo scheduling period used by the runtime.
|
||||
/// 获取运行时使用的伺服调度周期。
|
||||
/// </summary>
|
||||
[JsonPropertyName("servoPeriod")]
|
||||
public TimeSpan ServoPeriod { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the trigger scheduling period used by shot-event alignment.
|
||||
/// 获取飞拍事件对齐使用的触发调度周期。
|
||||
/// </summary>
|
||||
[JsonPropertyName("triggerPeriod")]
|
||||
public TimeSpan TriggerPeriod { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Describes a single revolute joint limit set required by the planners.
|
||||
/// 描述规划器所需的单个旋转关节约束集合。
|
||||
/// </summary>
|
||||
public sealed class JointLimit
|
||||
{
|
||||
/// <summary>
|
||||
/// Initializes a validated joint limit record.
|
||||
/// 初始化一个已校验的关节约束记录。
|
||||
/// </summary>
|
||||
public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit)
|
||||
{
|
||||
@@ -145,37 +145,37 @@ public sealed class JointLimit
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the joint name associated with the limits.
|
||||
/// 获取该约束对应的关节名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jointName")]
|
||||
public string JointName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the velocity limit in joint space units.
|
||||
/// 获取关节空间单位下的速度上限。
|
||||
/// </summary>
|
||||
[JsonPropertyName("velocityLimit")]
|
||||
public double VelocityLimit { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the acceleration limit in joint space units.
|
||||
/// 获取关节空间单位下的加速度上限。
|
||||
/// </summary>
|
||||
[JsonPropertyName("accelerationLimit")]
|
||||
public double AccelerationLimit { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the jerk limit in joint space units.
|
||||
/// 获取关节空间单位下的跃度上限。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jerkLimit")]
|
||||
public double JerkLimit { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Describes a joint-coupling rule that must be applied before kinematics or planning.
|
||||
/// 描述在运动学计算或轨迹规划前必须应用的关节耦合规则。
|
||||
/// </summary>
|
||||
public sealed class JointCoupling
|
||||
{
|
||||
/// <summary>
|
||||
/// Initializes a validated joint-coupling description.
|
||||
/// 初始化一个已校验的关节耦合描述。
|
||||
/// </summary>
|
||||
public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset)
|
||||
{
|
||||
@@ -201,25 +201,25 @@ public sealed class JointCoupling
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the dependent joint name.
|
||||
/// 获取从属(被驱动)关节名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("slaveJointName")]
|
||||
public string SlaveJointName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the source joint name.
|
||||
/// 获取主导(驱动)关节名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("masterJointName")]
|
||||
public string MasterJointName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the coupling multiplier applied to the master joint angle.
|
||||
/// 获取作用在主导关节角度上的耦合倍率。
|
||||
/// </summary>
|
||||
[JsonPropertyName("multiplier")]
|
||||
public double Multiplier { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the additive offset applied after the multiplier.
|
||||
/// 获取在耦合倍率之后叠加的偏移量。
|
||||
/// </summary>
|
||||
[JsonPropertyName("offset")]
|
||||
public double Offset { get; }
|
||||
|
||||
@@ -21,7 +21,10 @@ public sealed class TrajectoryResult
|
||||
string? failureReason,
|
||||
bool usedCache,
|
||||
int originalWaypointCount,
|
||||
int plannedWaypointCount)
|
||||
int plannedWaypointCount,
|
||||
int triggerSampleIndexOffsetCycles = 0,
|
||||
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null,
|
||||
FlyshotPreparedExecution? preparedFlyshotExecution = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
@@ -43,6 +46,11 @@ public sealed class TrajectoryResult
|
||||
throw new ArgumentOutOfRangeException(nameof(plannedWaypointCount), "Planned waypoint count must be greater than or equal to the original waypoint count.");
|
||||
}
|
||||
|
||||
if (triggerSampleIndexOffsetCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(triggerSampleIndexOffsetCycles), "Trigger sample index offset cycles must be zero or positive.");
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(shotEvents);
|
||||
ArgumentNullException.ThrowIfNull(triggerTimeline);
|
||||
ArgumentNullException.ThrowIfNull(artifacts);
|
||||
@@ -51,6 +59,7 @@ public sealed class TrajectoryResult
|
||||
var copiedShotEvents = shotEvents.ToArray();
|
||||
var copiedTriggerTimeline = triggerTimeline.ToArray();
|
||||
var copiedArtifacts = artifacts.ToArray();
|
||||
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
|
||||
|
||||
ProgramName = programName;
|
||||
Method = method;
|
||||
@@ -63,6 +72,9 @@ public sealed class TrajectoryResult
|
||||
UsedCache = usedCache;
|
||||
OriginalWaypointCount = originalWaypointCount;
|
||||
PlannedWaypointCount = plannedWaypointCount;
|
||||
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
|
||||
DenseJointTrajectory = copiedDenseJointTrajectory;
|
||||
PreparedFlyshotExecution = preparedFlyshotExecution;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -130,6 +142,25 @@ public sealed class TrajectoryResult
|
||||
/// </summary>
|
||||
[JsonPropertyName("plannedWaypointCount")]
|
||||
public int PlannedWaypointCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the configured command-sample offset applied after trigger binding picks the best sample.
|
||||
/// </summary>
|
||||
[JsonPropertyName("triggerSampleIndexOffsetCycles")]
|
||||
public int TriggerSampleIndexOffsetCycles { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the dense joint trajectory samples where each row is [time, j1, j2, ...].
|
||||
/// Null when dense sampling was not performed (e.g. simulation fallback).
|
||||
/// </summary>
|
||||
[JsonPropertyName("denseJointTrajectory")]
|
||||
public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence.
|
||||
/// </summary>
|
||||
[JsonIgnore]
|
||||
public FlyshotPreparedExecution? PreparedFlyshotExecution { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -140,7 +171,13 @@ public sealed class TrajectoryDoEvent
|
||||
/// <summary>
|
||||
/// Initializes a validated runtime trigger event.
|
||||
/// </summary>
|
||||
public TrajectoryDoEvent(int waypointIndex, double triggerTime, int offsetCycles, int holdCycles, IoAddressGroup addressGroup)
|
||||
public TrajectoryDoEvent(
|
||||
int waypointIndex,
|
||||
double triggerTime,
|
||||
int offsetCycles,
|
||||
int holdCycles,
|
||||
IoAddressGroup addressGroup,
|
||||
IEnumerable<double>? referenceJointsDegrees = null)
|
||||
{
|
||||
if (waypointIndex < 0)
|
||||
{
|
||||
@@ -162,6 +199,7 @@ public sealed class TrajectoryDoEvent
|
||||
OffsetCycles = offsetCycles;
|
||||
HoldCycles = holdCycles;
|
||||
AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup));
|
||||
ReferenceJointsDegrees = referenceJointsDegrees?.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -193,6 +231,13 @@ public sealed class TrajectoryDoEvent
|
||||
/// </summary>
|
||||
[JsonPropertyName("addressGroup")]
|
||||
public IoAddressGroup AddressGroup { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the teach waypoint joints converted to degrees.
|
||||
/// This is used to select the closest real send sample inside the trigger time window.
|
||||
/// </summary>
|
||||
[JsonPropertyName("referenceJointsDegrees")]
|
||||
public IReadOnlyList<double>? ReferenceJointsDegrees { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
|
||||
namespace Flyshot.Core.Planning.Export;
|
||||
|
||||
@@ -15,12 +16,28 @@ namespace Flyshot.Core.Planning.Export;
|
||||
/// </summary>
|
||||
public static class TrajectoryExporter
|
||||
{
|
||||
/// <summary>
|
||||
/// 导出规划关节轨迹关键点到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteJointTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出稠密关节轨迹到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteDenseRows(path, rows);
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出规划笛卡尔轨迹关键点到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteCartesianTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -28,7 +45,7 @@ public static class TrajectoryExporter
|
||||
/// </summary>
|
||||
public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteDenseRows(path, rows);
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -36,13 +53,51 @@ public static class TrajectoryExporter
|
||||
/// </summary>
|
||||
public static void WriteShotEvents(string path, IReadOnlyList<ShotEvent> events)
|
||||
{
|
||||
WriteShotEvents(path, events, bindings: null);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出触发事件到 JSON 文件,并可选附带最终绑定到的实际 sample 信息。
|
||||
/// </summary>
|
||||
public static void WriteShotEvents(
|
||||
string path,
|
||||
IReadOnlyList<ShotEvent> events,
|
||||
IReadOnlyList<TriggerSampleBinding>? bindings)
|
||||
{
|
||||
var bindingByWaypointIndex = bindings?
|
||||
.GroupBy(static binding => binding.Trigger.WaypointIndex)
|
||||
.ToDictionary(static group => group.Key, static group => group.First());
|
||||
|
||||
var payload = events.Select(e => new
|
||||
{
|
||||
waypoint_index = e.WaypointIndex,
|
||||
trigger_time = Math.Round(e.TriggerTime, 6),
|
||||
sample_index = e.SampleIndex,
|
||||
sample_time = Math.Round(e.SampleTime, 6),
|
||||
addrs = e.AddressGroup.Addresses.ToList()
|
||||
addrs = e.AddressGroup.Addresses.ToList(),
|
||||
trigger_window_seconds = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out var binding)
|
||||
? Math.Round(TriggerSampleBinder.TriggerBindingToleranceSeconds, 6)
|
||||
: (double?)null,
|
||||
selected_sample_index = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? binding.SampleIndex
|
||||
: (int?)null,
|
||||
selected_send_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? Math.Round(binding.Sample.SendTime, 6)
|
||||
: (double?)null,
|
||||
selected_trajectory_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? Math.Round(binding.Sample.TrajectoryTime, 6)
|
||||
: (double?)null,
|
||||
io_mask = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? ComputeIoValue(binding.Trigger.AddressGroup)
|
||||
: (ushort?)null,
|
||||
reference_joints_deg = bindingByWaypointIndex is not null
|
||||
&& bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
&& binding.Trigger.ReferenceJointsDegrees is { Count: > 0 }
|
||||
? binding.Trigger.ReferenceJointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
|
||||
: null,
|
||||
trigger_joints_deg = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? binding.Sample.JointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
|
||||
: null
|
||||
}).ToList();
|
||||
|
||||
var json = JsonSerializer.Serialize(payload, new JsonSerializerOptions
|
||||
@@ -53,7 +108,7 @@ public static class TrajectoryExporter
|
||||
File.WriteAllText(path, json, new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
private static void WriteRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var row in rows)
|
||||
@@ -63,4 +118,21 @@ public static class TrajectoryExporter
|
||||
|
||||
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码,和运行时 / saveTrajectory 导出保持一致。
|
||||
/// </summary>
|
||||
private static ushort ComputeIoValue(IoAddressGroup group)
|
||||
{
|
||||
ushort value = 0;
|
||||
foreach (var addr in group.Addresses)
|
||||
{
|
||||
if (addr is >= 1 and <= 16)
|
||||
{
|
||||
value |= (ushort)(1 << (addr - 1));
|
||||
}
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,6 +6,10 @@
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
194
src/Flyshot.Core.Planning/ICSP-算法说明.md
Normal file
194
src/Flyshot.Core.Planning/ICSP-算法说明.md
Normal file
@@ -0,0 +1,194 @@
|
||||
# ICSP 算法说明(`ICspPlanner`)
|
||||
|
||||
本文档用于解释 `Flyshot.Core.Planning.ICspPlanner` 当前实现的 **ICSP 规划算法**在本仓库中的真实含义与计算逻辑,便于与逆向结论对照、以及指导后续改造(例如“按约束生成中间点位”)。
|
||||
|
||||
> 适用范围:本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代(retiming)”** 版本。
|
||||
> 重要澄清:`ICspPlanner` 的主要输出是 **时间轴**(每个示教点的时间戳),而不是直接输出固定周期的稠密点序列;稠密点在后续采样层生成。
|
||||
|
||||
---
|
||||
|
||||
## 1. 名词与数据形态
|
||||
|
||||
### 1.1 输入
|
||||
|
||||
- **示教点(路点)**:`request.Program.Waypoints`
|
||||
每个路点是关节空间向量 \(q_i \in \mathbb{R}^{dof}\)。
|
||||
- **关节约束**:`request.Robot.JointLimits[d]` 提供每轴上限:
|
||||
- 速度上限 \(v_{lim}[d]\)
|
||||
- 加速度上限 \(a_{lim}[d]\)
|
||||
- 跃度(jerk)上限 \(j_{lim}[d]\)
|
||||
|
||||
### 1.2 输出(`PlannedTrajectory`)
|
||||
|
||||
`ICspPlanner` 的输出 **不是稠密轨迹点序列**,而是:
|
||||
|
||||
- `PlannedWaypoints`:规划后路点(对于普通 `icsp`,与输入示教点相同;补点发生在 `SelfAdaptIcspPlanner`)
|
||||
- `WaypointTimes`:每个路点的绝对时间 \(t_i\)(秒)
|
||||
- `SegmentDurations`:每段时长 \(T_i = t_{i+1}-t_i\)(秒)
|
||||
- `SegmentScales`:每段缩放因子 `scale_i`
|
||||
- `Iterations` / `Threshold`:收敛信息
|
||||
|
||||
后续模块会基于 `PlannedWaypoints + WaypointTimes` 重建样条并采样,生成稠密点:
|
||||
|
||||
- 规划层稠密采样:`TrajectorySampler.SampleJointTrajectory(...)`
|
||||
- 运行时 J519 重采样(速度倍率映射 + rad->deg):`J519SendTrajectorySampler.SampleDenseJointTrajectory(...)`
|
||||
|
||||
---
|
||||
|
||||
## 2. 算法总体目标(retiming)
|
||||
|
||||
给定一组关节示教点 \(\{q_i\}_{i=0}^{N-1}\),在不改变路点位置的前提下,为每段分配时长 \(\{T_i\}_{i=0}^{N-2}\),使得用 **clamped-zero 三次样条**连接后的轨迹在每段上满足:
|
||||
|
||||
- \(\max|\dot q_d(t)| \le v_{lim}[d]\)
|
||||
- \(\max|\ddot q_d(t)| \le a_{lim}[d]\)
|
||||
- \(\max|\dddot q_d(t)| \le j_{lim}[d]\)
|
||||
|
||||
实现策略是“逐段缩放时长”的迭代法:每轮用当前 \(\{T_i\}\) 构造样条并解析求导峰值,再根据超限程度把相应段时长乘以缩放因子,使峰值回落。
|
||||
|
||||
---
|
||||
|
||||
## 3. 计算步骤(与代码一致)
|
||||
|
||||
### 3.1 前置条件
|
||||
|
||||
- 路点数 \(N \ge 4\)(否则抛异常)
|
||||
|
||||
### 3.2 初始段时长
|
||||
|
||||
段数 \(nseg = N-1\)。
|
||||
|
||||
初始段时长取相邻路点关节空间欧氏距离:
|
||||
|
||||
\[
|
||||
T_i^{(0)} = \|q_{i+1}-q_i\|_2
|
||||
\]
|
||||
|
||||
### 3.3 由段时长构造绝对时间轴
|
||||
|
||||
\[
|
||||
t_0 = 0,\quad t_{i+1} = t_i + T_i
|
||||
\]
|
||||
|
||||
### 3.4 用 clamped-zero 边界构造三次样条
|
||||
|
||||
以 \((t_i, q_i)\) 为节点构造分段三次多项式:
|
||||
|
||||
\[
|
||||
S_i(t) = a_i t^3 + b_i t^2 + c_i t + d_i,\quad t \in [t_i, t_{i+1}]
|
||||
\]
|
||||
|
||||
边界条件为 **clamped-zero**(起点/终点一阶导为 0),用于与逆向锁定的参考行为对齐。
|
||||
|
||||
### 3.5 解析计算每段导数峰值
|
||||
|
||||
对每段、每轴,解析求最大绝对值:
|
||||
|
||||
- 一阶导(速度)是二次函数:端点与顶点候选取最大
|
||||
- 二阶导(加速度)是一次函数:端点取最大
|
||||
- 三阶导(跃度)是常数:直接取绝对值
|
||||
|
||||
得到三张矩阵:
|
||||
|
||||
- `maxDq[seg,d] = max_t |dq/dt|`
|
||||
- `maxDdq[seg,d] = max_t |d²q/dt²|`
|
||||
- `maxDddq[seg,d] = max_t |d³q/dt³|`
|
||||
|
||||
### 3.6 计算每段缩放因子(核心公式)
|
||||
|
||||
对段 `seg`,对每个关节 `d` 计算三类“超限比”:
|
||||
|
||||
\[
|
||||
s_v = \left|\frac{maxDq[seg,d]}{v_{lim}[d]}\right|
|
||||
\]
|
||||
|
||||
\[
|
||||
s_a = \sqrt{\left|\frac{maxDdq[seg,d]}{a_{lim}[d]}\right|}
|
||||
\]
|
||||
|
||||
\[
|
||||
s_j = \sqrt[3]{\left|\frac{maxDddq[seg,d]}{j_{lim}[d]}\right|}
|
||||
\]
|
||||
|
||||
段缩放因子取所有轴、三类约束的最大值:
|
||||
|
||||
\[
|
||||
scale_{seg}=\max_d \max(s_v, s_a, s_j)
|
||||
\]
|
||||
|
||||
> 指数来源:时间拉长 \(k\) 倍时,速度按 \(1/k\) 缩小、加速度按 \(1/k^2\) 缩小、跃度按 \(1/k^3\) 缩小,因此超限比需要分别取一次方/平方根/立方根来求“应当拉长多少倍”。
|
||||
|
||||
### 3.7 收敛指标与最优解保存
|
||||
|
||||
每轮用如下指标衡量“离约束满足还差多少”:
|
||||
|
||||
\[
|
||||
threshold = \sum_{seg} |scale_{seg} - 1|
|
||||
\]
|
||||
|
||||
若本轮 `threshold` 小于历史最佳,则保存当前解作为 `best`(包含 `bestDurations / bestScales / bestWaypointTimes` 等)。
|
||||
|
||||
### 3.8 收敛判定与段时长更新
|
||||
|
||||
- 若 `threshold < _threshold`(默认 `1e-3`),认为收敛并提前结束迭代
|
||||
- 否则更新每段时长:
|
||||
|
||||
\[
|
||||
T_{seg} \leftarrow T_{seg} \cdot scale_{seg}
|
||||
\]
|
||||
|
||||
并进入下一轮。
|
||||
|
||||
---
|
||||
|
||||
## 4. 最终判定(global_scale)
|
||||
|
||||
迭代结束后取历史最优缩放因子的最大值:
|
||||
|
||||
\[
|
||||
globalScale=\max_{seg}(scale_{seg})
|
||||
\]
|
||||
|
||||
若启用强制判定(`enforceFinalScale=true`)且:
|
||||
|
||||
\[
|
||||
globalScale > 1 + \text{finalScaleTolerance}
|
||||
\]
|
||||
|
||||
则判定“未收敛/不可执行”并抛异常。默认容差 `finalScaleTolerance=1e-2`,用于容忍 C# spline 与参考实现间的小量数值差异。
|
||||
|
||||
---
|
||||
|
||||
## 5. 与“补点/中间点位”的关系(常见误解澄清)
|
||||
|
||||
### 5.1 `ICspPlanner` 不负责生成固定周期的中间点位
|
||||
|
||||
`ICspPlanner` 的核心工作是 **时间轴规划(retiming)**:在不改变示教点位置的情况下,通过缩放每段时长让样条导数峰值满足约束。
|
||||
|
||||
固定周期(例如 8ms/16ms)的“中间点位序列”属于 **采样层**:
|
||||
|
||||
- `TrajectorySampler`:按 `samplePeriod` 在样条上取样,得到 `[time, j1..jN]`(关节单位仍为 rad)
|
||||
- `J519SendTrajectorySampler`:按 `servoPeriod` 生成真实发送序列,用 `speedRatio` 把 `sendTime` 映射到 `trajectoryTime` 并线性插值,再做 `rad -> deg`
|
||||
|
||||
### 5.2 `SelfAdaptIcspPlanner` 才包含“补点”逻辑,但它很粗
|
||||
|
||||
`self-adapt-icsp` 的补点策略在 `SelfAdaptIcspPlanner` 中:当某些段 `scale > 1 + tolerance` 时,对这些段插入关节空间**中点**再重规划。该策略的目的主要是“救收敛”,不是生成最终稠密序列。
|
||||
|
||||
---
|
||||
|
||||
## 6. 后续改造建议(定位落点)
|
||||
|
||||
如果需求是“根据示教点 + v/a/j 限制,直接生成可下发的稠密点位序列”,通常有两条路径:
|
||||
|
||||
1. **保留 ICSP retiming**:继续用 `ICspPlanner` 求时间轴,再在采样层按固定周期生成中间点位(当前架构就是这条路)。此时需要讨论的是采样周期、速度倍率映射、以及是否要对采样序列再做约束校验或二次整形。
|
||||
2. **做真正的自适应插点/细分**:把“插点策略”升级为基于约束的细分(而不只是插中点),这更自然的落点是 `SelfAdaptIcspPlanner` 或新增一个“约束驱动细分器”,而不是把稠密点生成塞进 `ICspPlanner`。
|
||||
|
||||
---
|
||||
|
||||
## 7. 关联实现位置(便于跳转)
|
||||
|
||||
- 算法入口:`src/Flyshot.Core.Planning/ICspPlanner.cs`
|
||||
- 自适应补点:`src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs`
|
||||
- 三次样条实现(clamped-zero + 解析导峰值):`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs`
|
||||
- 规划层稠密采样:`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs`
|
||||
- J519 实发重采样:`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs`
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
@@ -24,6 +25,54 @@ public sealed class ICspPlanner
|
||||
/// </summary>
|
||||
public const int DefaultMaxIterations = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 默认最终 scale 容差。当前 C# spline 与旧系统对齐样本存在约 1% 内的数值余量。
|
||||
/// </summary>
|
||||
public const double DefaultFinalScaleTolerance = 1e-2;
|
||||
|
||||
private readonly double _threshold;
|
||||
private readonly int _maxIterations;
|
||||
private readonly bool _enforceFinalScale;
|
||||
private readonly double _finalScaleTolerance;
|
||||
private readonly ILogger<ICspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ICSP 规划器。
|
||||
/// </summary>
|
||||
/// <param name="threshold">收敛阈值。</param>
|
||||
/// <param name="maxIterations">最大迭代轮数。</param>
|
||||
/// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
|
||||
/// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
|
||||
/// <param name="logger">日志记录器;允许 null,供无日志场景使用。</param>
|
||||
public ICspPlanner(
|
||||
double threshold = DefaultThreshold,
|
||||
int maxIterations = DefaultMaxIterations,
|
||||
bool enforceFinalScale = true,
|
||||
double finalScaleTolerance = DefaultFinalScaleTolerance,
|
||||
ILogger<ICspPlanner>? logger = null)
|
||||
{
|
||||
if (threshold <= 0.0 || double.IsNaN(threshold) || double.IsInfinity(threshold))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值必须为有限正数。");
|
||||
}
|
||||
|
||||
if (maxIterations < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(maxIterations), "最大迭代轮数不能为负数。");
|
||||
}
|
||||
|
||||
if (finalScaleTolerance < 0.0 || double.IsNaN(finalScaleTolerance) || double.IsInfinity(finalScaleTolerance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalScaleTolerance), "最终 scale 容差必须为有限非负数。");
|
||||
}
|
||||
|
||||
_threshold = threshold;
|
||||
_maxIterations = maxIterations;
|
||||
_enforceFinalScale = enforceFinalScale;
|
||||
_finalScaleTolerance = finalScaleTolerance;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行 ICSP 规划,返回包含完整时间轴和收敛信息的轨迹。
|
||||
/// </summary>
|
||||
@@ -37,9 +86,22 @@ public sealed class ICspPlanner
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划开始: 名称={Name}, 路点数={WaypointCount}, 自由度={Dof}, threshold={Threshold}, maxIterations={MaxIterations}",
|
||||
request.Program.Name, waypoints.Count, request.Robot.DegreesOfFreedom, _threshold, _maxIterations);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 输入路点: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Positions.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
var qs = WaypointsToArray(waypoints);
|
||||
var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"ICSP 约束限值: vel=[{Vel}], acc=[{Acc}], jerk=[{Jerk}]",
|
||||
string.Join(", ", velLimits.Select(v => v.ToString("F2"))),
|
||||
string.Join(", ", accLimits.Select(a => a.ToString("F2"))),
|
||||
string.Join(", ", jerkLimits.Select(j => j.ToString("F2"))));
|
||||
|
||||
// 初始段时长直接取相邻示教点的关节空间欧氏距离。
|
||||
var segmentDurations = ComputeInitialDurations(qs);
|
||||
int nseg = segmentDurations.Length;
|
||||
@@ -52,7 +114,7 @@ public sealed class ICspPlanner
|
||||
int bestIterations = 0;
|
||||
double[]? bestWaypointTimes = null;
|
||||
|
||||
for (int iteration = 0; iteration <= DefaultMaxIterations; iteration++)
|
||||
for (int iteration = 0; iteration <= _maxIterations; iteration++)
|
||||
{
|
||||
var waypointTimes = CumulativeTimes(segmentDurations);
|
||||
var spline = new CubicSplineInterpolator(waypointTimes, qs);
|
||||
@@ -89,8 +151,11 @@ public sealed class ICspPlanner
|
||||
bestWaypointTimes = (double[])waypointTimes.Clone();
|
||||
}
|
||||
|
||||
if (currentThreshold < DefaultThreshold)
|
||||
if (currentThreshold < _threshold)
|
||||
{
|
||||
_logger?.LogDebug(
|
||||
"ICSP 第 {Iteration} 轮收敛: threshold={CurrentThreshold:E6}",
|
||||
iteration + 1, currentThreshold);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -105,6 +170,25 @@ public sealed class ICspPlanner
|
||||
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
|
||||
}
|
||||
|
||||
var globalScale = bestScales.Max();
|
||||
if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
|
||||
{
|
||||
_logger?.LogError(
|
||||
"ICSP 规划未收敛: global_scale={GlobalScale:F6} > {Tolerance:F6}, 段缩放=[{Scales}]",
|
||||
globalScale, 1.0 + _finalScaleTolerance,
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
throw new InvalidOperationException(
|
||||
$"ICSP 规划未收敛,global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6},轨迹不可执行。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划完成: 名称={Name}, 迭代轮数={Iterations}, 收敛阈值={Threshold:E6}, 总时长={Duration:F4}s, global_scale={GlobalScale:F6}",
|
||||
request.Program.Name, bestIterations, bestThreshold, bestWaypointTimes[^1], globalScale);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 段时长: [{Durations}], 段缩放: [{Scales}]",
|
||||
string.Join(", ", bestDurations.Select(d => d.ToString("F4"))),
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: request.Robot,
|
||||
originalProgram: request.Program,
|
||||
|
||||
@@ -0,0 +1,161 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列,并在必要时自动拉长执行时长直到通过离散限幅校验。
|
||||
/// </summary>
|
||||
public static class FlyshotExecutionSendSequenceBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 自动拉长时每轮采用的保守倍率缩减系数。
|
||||
/// </summary>
|
||||
private const double StretchFactor = 0.95;
|
||||
|
||||
/// <summary>
|
||||
/// 自动拉长尝试的最大迭代次数。
|
||||
/// </summary>
|
||||
private const int MaxStretchIterations = 16;
|
||||
|
||||
/// <summary>
|
||||
/// 根据规划层稠密轨迹和执行层 speedRatio 构建最终发送队列。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人关节限值配置。</param>
|
||||
/// <param name="result">规划结果。</param>
|
||||
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
|
||||
/// <param name="requestedSpeedRatio">请求的执行倍率。</param>
|
||||
/// <returns>通过离散校验后的飞拍最终发送结果。</returns>
|
||||
public static FlyshotPreparedExecution Build(
|
||||
RobotProfile robot,
|
||||
TrajectoryResult result,
|
||||
double servoPeriodSeconds,
|
||||
double requestedSpeedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(result);
|
||||
|
||||
if (result.DenseJointTrajectory is null)
|
||||
{
|
||||
throw new InvalidOperationException("飞拍执行准备前必须先生成规划层稠密关节轨迹。");
|
||||
}
|
||||
|
||||
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。");
|
||||
}
|
||||
|
||||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。");
|
||||
}
|
||||
|
||||
var effectiveSpeedRatio = requestedSpeedRatio;
|
||||
InvalidOperationException? firstFailure = null;
|
||||
|
||||
for (var iteration = 0; iteration <= MaxStretchIterations; iteration++)
|
||||
{
|
||||
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||
result.DenseJointTrajectory,
|
||||
result.Duration.TotalSeconds,
|
||||
servoPeriodSeconds,
|
||||
effectiveSpeedRatio);
|
||||
|
||||
try
|
||||
{
|
||||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||
robot,
|
||||
samples,
|
||||
trajectoryName: result.ProgramName);
|
||||
|
||||
return BuildPreparedExecution(
|
||||
result,
|
||||
samples,
|
||||
requestedSpeedRatio,
|
||||
effectiveSpeedRatio,
|
||||
iteration);
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
firstFailure ??= exception;
|
||||
if (iteration >= MaxStretchIterations)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"飞拍最终发送队列离散限幅校验失败,已达到最大自动拉长次数 {MaxStretchIterations}。",
|
||||
firstFailure);
|
||||
}
|
||||
|
||||
// 只在执行侧进一步保守化 trajectoryTime(sendTime) 映射,不回写规划层轨迹。
|
||||
effectiveSpeedRatio *= StretchFactor;
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("飞拍最终发送队列构建失败。", firstFailure);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将通过校验的 J519 采样点和触发绑定结果封装为稳定的飞拍执行结果。
|
||||
/// </summary>
|
||||
private static FlyshotPreparedExecution BuildPreparedExecution(
|
||||
TrajectoryResult result,
|
||||
IReadOnlyList<J519SendSample> samples,
|
||||
double requestedSpeedRatio,
|
||||
double finalSpeedRatio,
|
||||
int stretchIterationCount)
|
||||
{
|
||||
var preparedSamples = samples.Select(static sample =>
|
||||
new FlyshotPreparedSample(
|
||||
sample.SampleIndex,
|
||||
sample.SendTime,
|
||||
sample.TrajectoryTime,
|
||||
sample.SpeedRatio,
|
||||
sample.JointsDegrees)).ToArray();
|
||||
var sampleLookup = preparedSamples.ToDictionary(static sample => sample.SampleIndex);
|
||||
var triggerBindings = TriggerSampleBinder.Bind(
|
||||
result.TriggerTimeline,
|
||||
samples,
|
||||
result.TriggerSampleIndexOffsetCycles)
|
||||
.Select(binding =>
|
||||
new FlyshotPreparedTriggerBinding(
|
||||
binding.Trigger,
|
||||
sampleLookup[binding.Sample.SampleIndex],
|
||||
binding.SampleIndex,
|
||||
binding.FoundInWindow))
|
||||
.ToArray();
|
||||
var timingRows = samples.Select(J519SendTrajectorySampler.BuildTimingRow).ToArray();
|
||||
var jerkRows = BuildJerkRows(samples);
|
||||
var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;
|
||||
|
||||
return new FlyshotPreparedExecution(
|
||||
preparedSamples,
|
||||
triggerBindings,
|
||||
timingRows,
|
||||
jerkRows,
|
||||
requestedSpeedRatio,
|
||||
finalSpeedRatio,
|
||||
finalDurationSeconds,
|
||||
stretchIterationCount);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为最终发送点列构建跃度诊断行,保证导出工件和运行时发送复用同一份数据。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildJerkRows(IReadOnlyList<J519SendSample> samples)
|
||||
{
|
||||
var jerkRows = new List<IReadOnlyList<double>>(Math.Max(0, samples.Count - 1));
|
||||
double[]? previousVelocity = null;
|
||||
double[]? previousAcceleration = null;
|
||||
|
||||
for (var index = 1; index < samples.Count; index++)
|
||||
{
|
||||
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
|
||||
samples[index - 1].SendTime,
|
||||
samples[index].SendTime,
|
||||
samples[index - 1].JointsDegrees,
|
||||
samples[index].JointsDegrees,
|
||||
ref previousVelocity,
|
||||
ref previousAcceleration));
|
||||
}
|
||||
|
||||
return jerkRows;
|
||||
}
|
||||
}
|
||||
42
src/Flyshot.Core.Planning/Sampling/J519SendSample.cs
Normal file
42
src/Flyshot.Core.Planning/Sampling/J519SendSample.cs
Normal file
@@ -0,0 +1,42 @@
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 J519 伺服链路在某一个物理发送周期上的轨迹采样结果。
|
||||
/// </summary>
|
||||
/// <param name="sampleIndex">从 0 开始的发送周期序号。</param>
|
||||
/// <param name="sendTime">J519 物理发送时间,单位为秒。</param>
|
||||
/// <param name="trajectoryTime">映射回规划轨迹的采样时间,单位为秒。</param>
|
||||
/// <param name="speedRatio">生成该采样点时使用的速度倍率。</param>
|
||||
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
|
||||
public sealed class J519SendSample(
|
||||
long sampleIndex,
|
||||
double sendTime,
|
||||
double trajectoryTime,
|
||||
double speedRatio,
|
||||
IReadOnlyList<double> jointsDegrees)
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取从 0 开始的发送周期序号。
|
||||
/// </summary>
|
||||
public long SampleIndex { get; } = sampleIndex;
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 物理发送时间,单位为秒。
|
||||
/// </summary>
|
||||
public double SendTime { get; } = sendTime;
|
||||
|
||||
/// <summary>
|
||||
/// 获取映射回规划轨迹的采样时间,单位为秒。
|
||||
/// </summary>
|
||||
public double TrajectoryTime { get; } = trajectoryTime;
|
||||
|
||||
/// <summary>
|
||||
/// 获取生成该采样点时使用的速度倍率。
|
||||
/// </summary>
|
||||
public double SpeedRatio { get; } = speedRatio;
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 下发使用的角度制关节目标。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointsDegrees { get; } = jointsDegrees.ToArray();
|
||||
}
|
||||
242
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs
Normal file
242
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs
Normal file
@@ -0,0 +1,242 @@
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把规划层稠密关节轨迹重采样为 J519 物理发送周期上的角度制目标。
|
||||
/// <para>
|
||||
/// 算法约定:
|
||||
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN],time 为规划轨迹时间,关节单位为弧度;
|
||||
/// 输出的 J519 采样点按物理伺服周期排列,关节单位转换为角度制,供 UDP 60015 实时下发和离线 ActualSend 文件共用。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算:
|
||||
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
|
||||
/// 末尾额外保留一个终点钳制周期,确保轨迹时长不是周期整数倍时仍会输出最终点。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod;
|
||||
/// speedRatio 不改变物理发送周期,只用于把发送时间映射回规划轨迹时间:
|
||||
/// trajectoryTime = min(sendTime * speedRatio, duration)。
|
||||
/// 之后在原始稠密关节轨迹上按 trajectoryTime 做线性插值,并把每个关节从 rad 转为 deg。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 诊断行也在这里统一生成:Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio;
|
||||
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分,依次近似速度、加速度和跃度,格式为
|
||||
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
|
||||
/// </para>
|
||||
/// </summary>
|
||||
public static class J519SendTrajectorySampler
|
||||
{
|
||||
/// <summary>
|
||||
/// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
|
||||
/// </summary>
|
||||
/// <param name="denseJointTrajectory">规划层稠密关节轨迹,每行格式为 [time, j1..jN],关节单位为弧度。</param>
|
||||
/// <param name="durationSeconds">规划轨迹总时长,单位为秒。</param>
|
||||
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
|
||||
/// <param name="speedRatio">速度倍率;只缩放轨迹采样时间,不改变物理发送周期。</param>
|
||||
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
|
||||
public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double durationSeconds,
|
||||
double servoPeriodSeconds,
|
||||
double speedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
|
||||
ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);
|
||||
|
||||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||||
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
|
||||
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
|
||||
var segmentIndex = 0;
|
||||
|
||||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||||
{
|
||||
// J519 物理周期固定,speed_ratio 只用于把发送时间映射回原始轨迹时间。
|
||||
var sendTime = sampleIndex * servoPeriodSeconds;
|
||||
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
|
||||
var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
|
||||
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
|
||||
}
|
||||
|
||||
return samples;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
|
||||
/// </summary>
|
||||
/// <param name="durationSeconds">规划轨迹总时长,单位为秒。</param>
|
||||
/// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长,单位为秒。</param>
|
||||
/// <returns>包含终点钳制周期的采样点数量。</returns>
|
||||
public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
|
||||
{
|
||||
if (durationSeconds < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
|
||||
}
|
||||
|
||||
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
|
||||
}
|
||||
|
||||
// 非周期整数倍时多保留一个终点钳制周期,和真实 J519 下发序列保持一致。
|
||||
return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造实发时间映射文本行,格式为 sample_index + send_time + trajectory_time + speed_ratio。
|
||||
/// </summary>
|
||||
/// <param name="sample">待写出的 J519 实发采样点。</param>
|
||||
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
|
||||
public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(sample);
|
||||
return
|
||||
[
|
||||
sample.SampleIndex,
|
||||
Math.Round(sample.SendTime, 6),
|
||||
Math.Round(sample.TrajectoryTime, 6),
|
||||
Math.Round(sample.SpeedRatio, 6)
|
||||
];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造相邻发送点之间的角度制跃度统计行。
|
||||
/// </summary>
|
||||
/// <param name="previousTime">上一帧发送时间,单位为秒。</param>
|
||||
/// <param name="currentTime">当前帧发送时间,单位为秒。</param>
|
||||
/// <param name="previousJoints">上一帧角度制关节目标。</param>
|
||||
/// <param name="currentJoints">当前帧角度制关节目标。</param>
|
||||
/// <param name="previousVelocity">上一帧关节速度,调用后更新为当前帧速度。</param>
|
||||
/// <param name="previousAcceleration">上一帧关节加速度,调用后更新为当前帧加速度。</param>
|
||||
/// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
|
||||
public static IReadOnlyList<double> BuildJerkRow(
|
||||
double previousTime,
|
||||
double currentTime,
|
||||
IReadOnlyList<double> previousJoints,
|
||||
IReadOnlyList<double> currentJoints,
|
||||
ref double[]? previousVelocity,
|
||||
ref double[]? previousAcceleration)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(previousJoints);
|
||||
ArgumentNullException.ThrowIfNull(currentJoints);
|
||||
|
||||
var dt = currentTime - previousTime;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
dt = 1e-9;
|
||||
}
|
||||
|
||||
var jointCount = currentJoints.Count;
|
||||
var currentVelocity = new double[jointCount];
|
||||
var currentAcceleration = new double[jointCount];
|
||||
var currentJerk = new double[jointCount];
|
||||
var maxAbsJerk = 0.0;
|
||||
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
|
||||
if (previousVelocity is not null)
|
||||
{
|
||||
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
|
||||
}
|
||||
|
||||
if (previousAcceleration is not null)
|
||||
{
|
||||
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
|
||||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocity = currentVelocity;
|
||||
previousAcceleration = currentAcceleration;
|
||||
|
||||
var row = new double[jointCount + 4];
|
||||
row[0] = Math.Round(previousTime, 6);
|
||||
row[1] = Math.Round(currentTime, 6);
|
||||
row[2] = Math.Round(dt, 6);
|
||||
row[3] = Math.Round(maxAbsJerk, 6);
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
row[index + 4] = Math.Round(currentJerk[index], 6);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在稠密关节轨迹上按时间线性插值,并转换成 J519 下发使用的角度制目标。
|
||||
/// </summary>
|
||||
private static double[] SampleDenseJointTrajectoryDegrees(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double trajectoryTime,
|
||||
ref int segmentIndex)
|
||||
{
|
||||
if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
|
||||
{
|
||||
return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = denseJointTrajectory.Count - 1;
|
||||
if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
|
||||
{
|
||||
return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = denseJointTrajectory[segmentIndex];
|
||||
var end = denseJointTrajectory[segmentIndex + 1];
|
||||
var startTime = start[0];
|
||||
var endTime = end[0];
|
||||
var segmentDuration = endTime - startTime;
|
||||
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
|
||||
var joints = new double[start.Count - 1];
|
||||
for (var index = 0; index < joints.Length; index++)
|
||||
{
|
||||
joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 J519 实发采样的基础输入,避免错误时间轴进入运行时链路。
|
||||
/// </summary>
|
||||
private static void ValidateInputs(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double durationSeconds,
|
||||
double servoPeriodSeconds,
|
||||
double speedRatio)
|
||||
{
|
||||
if (denseJointTrajectory.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException("稠密关节轨迹为空。");
|
||||
}
|
||||
|
||||
if (durationSeconds < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
|
||||
}
|
||||
|
||||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
|
||||
}
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 角度单位转换:rad -> deg。
|
||||
/// </summary>
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
}
|
||||
206
src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs
Normal file
206
src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs
Normal file
@@ -0,0 +1,206 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 对最终生成的关节轨迹点做速度、加速度和 Jerk 离散复核。
|
||||
/// </summary>
|
||||
public static class TrajectoryLimitValidator
|
||||
{
|
||||
/// <summary>
|
||||
/// 离散差分校验允许的默认浮点容差倍率。
|
||||
/// </summary>
|
||||
public const double DefaultLimitTolerance = 1.000001;
|
||||
|
||||
/// <summary>
|
||||
/// 校验弧度制稠密关节轨迹是否满足机器人关节限制。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人约束配置。</param>
|
||||
/// <param name="rows">稠密轨迹行,格式为 time + 关节弧度。</param>
|
||||
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
|
||||
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
||||
public static void ValidateDenseJointTrajectory(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> rows,
|
||||
double toleranceMultiplier = DefaultLimitTolerance,
|
||||
string? trajectoryName = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(rows);
|
||||
ValidateTolerance(toleranceMultiplier);
|
||||
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 J519 实际发送采样点是否满足机器人关节限制。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人约束配置。</param>
|
||||
/// <param name="samples">J519 发送采样点,关节单位为角度。</param>
|
||||
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
|
||||
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
||||
public static void ValidateJ519SendSamples(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<J519SendSample> samples,
|
||||
double toleranceMultiplier = DefaultLimitTolerance,
|
||||
string? trajectoryName = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
ValidateTolerance(toleranceMultiplier);
|
||||
|
||||
var rows = new List<IReadOnlyList<double>>(samples.Count);
|
||||
foreach (var sample in samples)
|
||||
{
|
||||
var row = new double[robot.DegreesOfFreedom + 1];
|
||||
row[0] = sample.SendTime;
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
row[index + 1] = DegreesToRadians(sample.JointsDegrees[index]);
|
||||
}
|
||||
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验容差倍率必须为有限正数。
|
||||
/// </summary>
|
||||
private static void ValidateTolerance(double toleranceMultiplier)
|
||||
{
|
||||
if (toleranceMultiplier <= 0.0 || double.IsNaN(toleranceMultiplier) || double.IsInfinity(toleranceMultiplier))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(toleranceMultiplier), "限值容差倍率必须是有限正数。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对弧度制轨迹行执行统一的离散差分限幅校验。
|
||||
/// </summary>
|
||||
private static void ValidateRows(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> rows,
|
||||
double toleranceMultiplier,
|
||||
string trajectoryName)
|
||||
{
|
||||
double? previousTime = null;
|
||||
double[]? previousPositions = null;
|
||||
double[]? previousVelocities = null;
|
||||
double[]? previousAccelerations = null;
|
||||
|
||||
for (var rowIndex = 0; rowIndex < rows.Count; rowIndex++)
|
||||
{
|
||||
var row = rows[rowIndex];
|
||||
if (row.Count < robot.DegreesOfFreedom + 1)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"轨迹 {trajectoryName} 第 {rowIndex + 1} 行关节列数量不足,期望至少 {robot.DegreesOfFreedom + 1} 列,实际 {row.Count} 列。");
|
||||
}
|
||||
|
||||
var currentTime = row[0];
|
||||
var currentPositions = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
currentPositions[index] = row[index + 1];
|
||||
}
|
||||
|
||||
if (previousTime is not null && previousPositions is not null)
|
||||
{
|
||||
var dt = currentTime - previousTime.Value;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"轨迹 {trajectoryName} 时间戳必须严格递增,第 {rowIndex + 1} 行 dt={dt:F9}s。");
|
||||
}
|
||||
|
||||
var currentVelocities = new double[robot.DegreesOfFreedom];
|
||||
var currentAccelerations = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var jointLimit = robot.JointLimits[index];
|
||||
currentVelocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||||
ThrowIfExceeded(
|
||||
trajectoryName,
|
||||
rowIndex,
|
||||
previousTime.Value,
|
||||
currentTime,
|
||||
jointLimit.JointName,
|
||||
"速度",
|
||||
currentVelocities[index],
|
||||
jointLimit.VelocityLimit,
|
||||
toleranceMultiplier);
|
||||
|
||||
currentAccelerations[index] = previousVelocities is null
|
||||
? 0.0
|
||||
: (currentVelocities[index] - previousVelocities[index]) / dt;
|
||||
ThrowIfExceeded(
|
||||
trajectoryName,
|
||||
rowIndex,
|
||||
previousTime.Value,
|
||||
currentTime,
|
||||
jointLimit.JointName,
|
||||
"加速度",
|
||||
currentAccelerations[index],
|
||||
jointLimit.AccelerationLimit,
|
||||
toleranceMultiplier);
|
||||
|
||||
if (previousAccelerations is not null)
|
||||
{
|
||||
var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
|
||||
ThrowIfExceeded(
|
||||
trajectoryName,
|
||||
rowIndex,
|
||||
previousTime.Value,
|
||||
currentTime,
|
||||
jointLimit.JointName,
|
||||
"Jerk",
|
||||
jerk,
|
||||
jointLimit.JerkLimit,
|
||||
toleranceMultiplier*4);
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocities = currentVelocities;
|
||||
previousAccelerations = currentAccelerations;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPositions = currentPositions;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当某个差分指标超过限制时抛出包含关节和时间窗的诊断异常。
|
||||
/// </summary>
|
||||
private static void ThrowIfExceeded(
|
||||
string trajectoryName,
|
||||
int rowIndex,
|
||||
double previousTime,
|
||||
double currentTime,
|
||||
string jointName,
|
||||
string metricName,
|
||||
double actual,
|
||||
double limit,
|
||||
double toleranceMultiplier)
|
||||
{
|
||||
var absActual = Math.Abs(actual);
|
||||
var effectiveLimit = limit * toleranceMultiplier;
|
||||
if (absActual <= effectiveLimit)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"轨迹 {trajectoryName} 第 {rowIndex + 1} 行 {jointName} {metricName}超限: " +
|
||||
$"time={previousTime:F6}->{currentTime:F6}s, actual={actual:F6}, limit={limit:F6}, ratio={absActual / limit:F4}。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 角度单位转换:deg -> rad。
|
||||
/// </summary>
|
||||
private static double DegreesToRadians(double degrees)
|
||||
{
|
||||
return degrees * Math.PI / 180.0;
|
||||
}
|
||||
}
|
||||
@@ -19,7 +19,8 @@ public static class TrajectorySampler
|
||||
public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
|
||||
PlannedTrajectory trajectory,
|
||||
double samplePeriod = 0.016,
|
||||
int decimals = 6)
|
||||
int decimals = 6,
|
||||
bool smoothStartStop = false)
|
||||
{
|
||||
var spline = RebuildSpline(trajectory);
|
||||
double duration = trajectory.WaypointTimes[^1];
|
||||
@@ -28,7 +29,10 @@ public static class TrajectorySampler
|
||||
|
||||
foreach (var t in times)
|
||||
{
|
||||
var pos = spline.Evaluate(t);
|
||||
var evaluationTime = smoothStartStop
|
||||
? MapSmoothStartStopEvaluationTime(t, duration)
|
||||
: t;
|
||||
var pos = spline.Evaluate(evaluationTime);
|
||||
var row = new List<double>(pos.Length + 1);
|
||||
row.Add(Math.Round(t, decimals));
|
||||
foreach (var value in pos)
|
||||
@@ -49,7 +53,8 @@ public static class TrajectorySampler
|
||||
PlannedTrajectory trajectory,
|
||||
RobotKinematicsModel kinematicsModel,
|
||||
double samplePeriod = 0.016,
|
||||
int decimals = 6)
|
||||
int decimals = 6,
|
||||
bool smoothStartStop = false)
|
||||
{
|
||||
var spline = RebuildSpline(trajectory);
|
||||
double duration = trajectory.WaypointTimes[^1];
|
||||
@@ -58,7 +63,10 @@ public static class TrajectorySampler
|
||||
|
||||
foreach (var t in times)
|
||||
{
|
||||
var jointPos = spline.Evaluate(t);
|
||||
var evaluationTime = smoothStartStop
|
||||
? MapSmoothStartStopEvaluationTime(t, duration)
|
||||
: t;
|
||||
var jointPos = spline.Evaluate(evaluationTime);
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
|
||||
var row = new List<double>(pose.Length + 1);
|
||||
row.Add(Math.Round(t, decimals));
|
||||
@@ -103,4 +111,26 @@ public static class TrajectorySampler
|
||||
|
||||
return times;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把线性采样时间映射为整段平滑起停的评估时间。
|
||||
/// 使用 7 次 smootherstep 时间律,让起点和终点的一到三阶导都自然收敛到 0。
|
||||
/// </summary>
|
||||
private static double MapSmoothStartStopEvaluationTime(double sampleTime, double duration)
|
||||
{
|
||||
if (duration <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var normalizedTime = Math.Clamp(sampleTime / duration, 0.0, 1.0);
|
||||
var u2 = normalizedTime * normalizedTime;
|
||||
var u3 = u2 * normalizedTime;
|
||||
var u4 = u3 * normalizedTime;
|
||||
var u5 = u4 * normalizedTime;
|
||||
var u6 = u5 * normalizedTime;
|
||||
var u7 = u6 * normalizedTime;
|
||||
var progress = (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
|
||||
return duration * progress;
|
||||
}
|
||||
}
|
||||
|
||||
134
src/Flyshot.Core.Planning/Sampling/TriggerSampleBinder.cs
Normal file
134
src/Flyshot.Core.Planning/Sampling/TriggerSampleBinder.cs
Normal file
@@ -0,0 +1,134 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把理论触发事件绑定到实际 J519 发送 sample。
|
||||
/// 绑定规则为:先在理论触发时间前后固定时间窗内筛候选点,再优先选择关节差最小的 sample。
|
||||
/// </summary>
|
||||
public static class TriggerSampleBinder
|
||||
{
|
||||
/// <summary>
|
||||
/// 当前触发绑定使用的固定近时窗半宽,单位为秒。
|
||||
/// 即:以理论触发时间为中心,在前后各 100ms 的范围内寻找最优 sample。
|
||||
/// </summary>
|
||||
public const double TriggerBindingToleranceSeconds = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// 把一组触发事件绑定到采样后的 J519 sample。
|
||||
/// </summary>
|
||||
/// <param name="triggers">待绑定的触发事件。</param>
|
||||
/// <param name="samples">J519 实发 sample。</param>
|
||||
/// <param name="sampleIndexOffsetCycles">在最佳绑定 sample 基础上继续向后偏移的命令周期数。</param>
|
||||
/// <returns>按最终 sampleIndex 排序的绑定结果。</returns>
|
||||
public static IReadOnlyList<TriggerSampleBinding> Bind(
|
||||
IReadOnlyList<TrajectoryDoEvent> triggers,
|
||||
IReadOnlyList<J519SendSample> samples,
|
||||
int sampleIndexOffsetCycles = 0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(triggers);
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
if (sampleIndexOffsetCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(sampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
|
||||
}
|
||||
|
||||
var bindings = new List<TriggerSampleBinding>(triggers.Count);
|
||||
if (triggers.Count == 0 || samples.Count == 0)
|
||||
{
|
||||
return bindings;
|
||||
}
|
||||
|
||||
var lastSampleIndex = samples.Count - 1;
|
||||
foreach (var trigger in triggers)
|
||||
{
|
||||
var fallbackIndex = FindClosestTrajectoryTimeSampleIndex(samples, trigger.TriggerTime);
|
||||
var candidateStart = Math.Max(0, fallbackIndex - 1);
|
||||
var candidateEnd = Math.Min(lastSampleIndex, fallbackIndex + 1);
|
||||
var bestIndex = fallbackIndex;
|
||||
var bestScore = double.PositiveInfinity;
|
||||
var foundInWindow = false;
|
||||
|
||||
for (var sampleIndex = candidateStart; sampleIndex <= candidateEnd; sampleIndex++)
|
||||
{
|
||||
var sample = samples[sampleIndex];
|
||||
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) > TriggerBindingToleranceSeconds)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
foundInWindow = true;
|
||||
var score = trigger.ReferenceJointsDegrees is { Count: > 0 }
|
||||
? ComputeJointDifferenceScore(trigger.ReferenceJointsDegrees, sample.JointsDegrees)
|
||||
: Math.Abs(sample.TrajectoryTime - trigger.TriggerTime);
|
||||
if (score < bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
bestIndex = sampleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
var finalIndex = Math.Min(lastSampleIndex, bestIndex + sampleIndexOffsetCycles);
|
||||
bindings.Add(new TriggerSampleBinding(trigger, samples[finalIndex], finalIndex, foundInWindow));
|
||||
}
|
||||
|
||||
return bindings
|
||||
.OrderBy(static binding => binding.SampleIndex)
|
||||
.ThenBy(static binding => binding.Trigger.WaypointIndex)
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按轨迹时间找到最接近理论触发时刻的 sample。
|
||||
/// </summary>
|
||||
public static int FindClosestTrajectoryTimeSampleIndex(IReadOnlyList<J519SendSample> samples, double triggerTime)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
|
||||
var bestIndex = 0;
|
||||
var bestDelta = double.PositiveInfinity;
|
||||
for (var sampleIndex = 0; sampleIndex < samples.Count; sampleIndex++)
|
||||
{
|
||||
var delta = Math.Abs(samples[sampleIndex].TrajectoryTime - triggerTime);
|
||||
if (delta < bestDelta)
|
||||
{
|
||||
bestDelta = delta;
|
||||
bestIndex = sampleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
return bestIndex;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算参考示教点与候选 sample 的关节差评分,使用平方和即可稳定排序。
|
||||
/// </summary>
|
||||
public static double ComputeJointDifferenceScore(
|
||||
IReadOnlyList<double>? referenceJointsDegrees,
|
||||
IReadOnlyList<double> sampleJointsDegrees)
|
||||
{
|
||||
if (referenceJointsDegrees is null || referenceJointsDegrees.Count == 0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var jointCount = Math.Min(referenceJointsDegrees.Count, sampleJointsDegrees.Count);
|
||||
var score = 0.0;
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
var delta = sampleJointsDegrees[index] - referenceJointsDegrees[index];
|
||||
score += delta * delta;
|
||||
}
|
||||
|
||||
return score;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示一个理论触发事件最终绑定到哪个 J519 sample 的结果。
|
||||
/// </summary>
|
||||
public sealed record TriggerSampleBinding(
|
||||
TrajectoryDoEvent Trigger,
|
||||
J519SendSample Sample,
|
||||
int SampleIndex,
|
||||
bool FoundInWindow);
|
||||
@@ -1,4 +1,5 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
@@ -8,7 +9,7 @@ namespace Flyshot.Core.Planning;
|
||||
/// 为什么需要这层?
|
||||
/// ---
|
||||
/// 逆向分析已经指出:原系统里普通 icsp 若仍有段 scale > 1,不会直接返回未收敛结果;
|
||||
/// 配置中还明确存在 adapt_icsp_try_num。本层把“超限段统一插入中点后再重规划”的逻辑显式落地,
|
||||
/// 配置中还明确存在 adapt_icsp_try_num。本层把"超限段统一插入中点后再重规划"的逻辑显式落地,
|
||||
/// 补上 demo 缺失的失败恢复路径。
|
||||
///
|
||||
/// 补点策略:
|
||||
@@ -24,7 +25,18 @@ public sealed class SelfAdaptIcspPlanner
|
||||
/// </summary>
|
||||
public const double ScaleTolerance = 5e-4;
|
||||
|
||||
private readonly ICspPlanner _innerPlanner = new();
|
||||
private readonly ICspPlanner _innerPlanner;
|
||||
private readonly ILogger<SelfAdaptIcspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 SelfAdaptIcspPlanner。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public SelfAdaptIcspPlanner(ILogger<SelfAdaptIcspPlanner>? logger = null)
|
||||
{
|
||||
_innerPlanner = new ICspPlanner(enforceFinalScale: false, logger: null);
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行自适应 ICSP 规划,允许在超限段插入中点后重试。
|
||||
@@ -48,6 +60,10 @@ public sealed class SelfAdaptIcspPlanner
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SelfAdaptICSP 规划开始: 名称={Name}, 原始路点数={WaypointCount}, 最大补点次数={MaxAttempts}",
|
||||
request.Program.Name, originalWaypointCount, adaptIcspTryNum);
|
||||
|
||||
var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
var currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
@@ -76,6 +92,9 @@ public sealed class SelfAdaptIcspPlanner
|
||||
|
||||
if (badSegments.Count == 0)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"SelfAdaptICSP 规划完成: 名称={Name}, 补点轮数={Attempts}, 最终路点数={WaypointCount}, 迭代次数={Iterations}, 总时长={Duration:F4}s",
|
||||
request.Program.Name, attempt, currentWaypoints.Length, trajectory.Iterations, trajectory.WaypointTimes[^1]);
|
||||
// 所有段都满足约束,收敛成功。返回包含补中点后路点的轨迹。
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
@@ -89,15 +108,24 @@ public sealed class SelfAdaptIcspPlanner
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
|
||||
_logger?.LogWarning(
|
||||
"SelfAdaptICSP 第 {Attempt} 轮存在超限段: 超限段数={BadCount}, 段索引=[{Segments}], 最大缩放={MaxScale:F4}",
|
||||
attempt, badSegments.Count, string.Join(", ", badSegments), trajectory.SegmentScales.Max());
|
||||
|
||||
if (attempt >= maxAttempts)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// 对超限段插入中点,并同步扩展 shot 元数据。
|
||||
int waypointCountBefore = currentWaypoints.Length;
|
||||
(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) =
|
||||
InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"SelfAdaptICSP 补中点: 路点数 {Before} -> {After}, 插入段=[{Segments}]",
|
||||
waypointCountBefore, currentWaypoints.Length, string.Join(", ", badSegments));
|
||||
|
||||
currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
@@ -109,6 +137,9 @@ public sealed class SelfAdaptIcspPlanner
|
||||
}
|
||||
|
||||
double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN;
|
||||
_logger?.LogError(
|
||||
"SelfAdaptICSP 规划失败: 名称={Name}, 在 {Attempts} 轮补点后仍未收敛, 最大段缩放因子={MaxScale:F6}",
|
||||
request.Program.Name, adaptIcspTryNum, maxScale);
|
||||
throw new InvalidOperationException(
|
||||
$"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛,最大段缩放因子={maxScale:F6}。");
|
||||
}
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Triggering;
|
||||
|
||||
@@ -7,16 +8,33 @@ namespace Flyshot.Core.Triggering;
|
||||
/// 根据规划轨迹和飞拍配置生成触发时间轴,把示教点上的 shot_flags / offset_values / addr
|
||||
/// 映射成带理论时间和离散化时间的 ShotEvent,以及可直接注入伺服流的 TrajectoryDoEvent。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 本构建器只负责“时间轴映射”和“事件打包”,不负责轨迹规划与运行时发送。
|
||||
/// 其中:
|
||||
/// - ShotEvent 用于诊断、导出和日志观察;
|
||||
/// - TrajectoryDoEvent 用于运行时伺服链路注入 DO 脉冲。
|
||||
/// </remarks>
|
||||
public sealed class ShotTimelineBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 将原始示教点索引映射到规划后时间轴上的时间戳解析器。
|
||||
/// </summary>
|
||||
private readonly WaypointTimestampResolver _resolver;
|
||||
|
||||
/// <summary>
|
||||
/// 可选日志记录器,用于输出时间轴构建统计信息。
|
||||
/// </summary>
|
||||
private readonly ILogger<ShotTimelineBuilder>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ShotTimelineBuilder,依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
|
||||
/// </summary>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver)
|
||||
/// <param name="resolver">时间戳解析器。</param>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver, ILogger<ShotTimelineBuilder>? logger = null)
|
||||
{
|
||||
_resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -25,8 +43,20 @@ public sealed class ShotTimelineBuilder
|
||||
/// <param name="trajectory">规划后的轨迹(含补中点信息和机器人配置)。</param>
|
||||
/// <param name="holdCycles">IO 保持周期数(对应原系统的 io_keep_cycles)。</param>
|
||||
/// <param name="samplePeriod">稠密采样周期,用于离散化 sample_index 和 sample_time。</param>
|
||||
/// <param name="useDo">是否生成可注入伺服流的 DO 事件。</param>
|
||||
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod)
|
||||
/// <exception cref="ArgumentNullException">当 <paramref name="trajectory"/> 为 null 时抛出。</exception>
|
||||
/// <exception cref="ArgumentOutOfRangeException">
|
||||
/// 当 <paramref name="holdCycles"/> 小于 0,或 <paramref name="samplePeriod"/> 小于等于 0 时抛出。
|
||||
/// </exception>
|
||||
/// <remarks>
|
||||
/// 时间计算规则:
|
||||
/// - 理论触发时间:timestamp[i] + offset_values[i] * trigger_period;
|
||||
/// - 离散采样索引:Round(trigger_time / sample_period);
|
||||
/// - 离散采样时间:sample_index * sample_period。
|
||||
/// 该离散化与运行时 8ms 周期下发模型一致,可用于对齐导出与诊断。
|
||||
/// </remarks>
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod, bool useDo = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
@@ -49,6 +79,7 @@ public sealed class ShotTimelineBuilder
|
||||
var shotEvents = new List<ShotEvent>();
|
||||
var triggerTimeline = new List<TrajectoryDoEvent>();
|
||||
|
||||
// 遍历原始示教点,仅对 shot_flag=true 的点生成触发事件。
|
||||
for (int i = 0; i < program.Waypoints.Count; i++)
|
||||
{
|
||||
if (!program.ShotFlags[i])
|
||||
@@ -56,6 +87,7 @@ public sealed class ShotTimelineBuilder
|
||||
continue;
|
||||
}
|
||||
|
||||
// 先得到连续时间上的理论触发时刻,再映射到离散采样周期。
|
||||
double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds;
|
||||
int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds);
|
||||
double sampleTime = sampleIndex * samplePeriodSeconds;
|
||||
@@ -69,13 +101,25 @@ public sealed class ShotTimelineBuilder
|
||||
sampleTime: sampleTime,
|
||||
addressGroup: addressGroup));
|
||||
|
||||
if (useDo)
|
||||
{
|
||||
// use_do=false 时保留 ShotEvent 诊断信息,但不向运行时下发 IO 脉冲。
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
offsetCycles: program.OffsetValues[i],
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup));
|
||||
addressGroup: addressGroup,
|
||||
referenceJointsDegrees: program.Waypoints[i].Positions.Select(static radians => radians * 180.0 / Math.PI)));
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ShotTimeline 构建完成: shotFlags总数={ShotFlagCount}, 触发事件数={TriggerCount}, useDo={UseDo}, holdCycles={HoldCycles}",
|
||||
program.ShotFlags.Count(static f => f),
|
||||
triggerTimeline.Count,
|
||||
useDo,
|
||||
holdCycles);
|
||||
|
||||
return new ShotTimeline(shotEvents, triggerTimeline);
|
||||
}
|
||||
|
||||
@@ -6,4 +6,8 @@
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
116
src/Flyshot.Runtime.Common/IControllerRuntime.cs
Normal file
116
src/Flyshot.Runtime.Common/IControllerRuntime.cs
Normal file
@@ -0,0 +1,116 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Runtime.Common;
|
||||
|
||||
/// <summary>
|
||||
/// 定义控制器运行时的最小状态与执行契约,供兼容层在不关心底层 Socket 细节的情况下调度轨迹。
|
||||
/// </summary>
|
||||
public interface IControllerRuntime
|
||||
{
|
||||
/// <summary>
|
||||
/// 重置当前机器人模型并清空控制器运行时状态。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="robotName">兼容层传入的机器人名称。</param>
|
||||
void ResetRobot(RobotProfile robot, string robotName);
|
||||
|
||||
/// <summary>
|
||||
/// 选择当前活动控制器类型。
|
||||
/// </summary>
|
||||
/// <param name="sim">是否使用仿真控制器。</param>
|
||||
void SetActiveController(bool sim);
|
||||
|
||||
/// <summary>
|
||||
/// 建立到控制器 IP 的连接。
|
||||
/// </summary>
|
||||
/// <param name="robotIp">控制器 IP。</param>
|
||||
void Connect(string robotIp);
|
||||
|
||||
/// <summary>
|
||||
/// 断开当前控制器连接。
|
||||
/// </summary>
|
||||
void Disconnect();
|
||||
|
||||
/// <summary>
|
||||
/// 使能机器人并记录底层缓冲区大小。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">运行时缓冲区大小。</param>
|
||||
void EnableRobot(int bufferSize);
|
||||
|
||||
/// <summary>
|
||||
/// 关闭机器人使能。
|
||||
/// </summary>
|
||||
void DisableRobot();
|
||||
|
||||
/// <summary>
|
||||
/// 停止当前运动。
|
||||
/// </summary>
|
||||
void StopMove();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
/// <returns>速度倍率。</returns>
|
||||
double GetSpeedRatio();
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
void SetSpeedRatio(double ratio);
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前 TCP 坐标。
|
||||
/// </summary>
|
||||
/// <returns>TCP 三维坐标。</returns>
|
||||
IReadOnlyList<double> GetTcp();
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前 TCP 坐标。
|
||||
/// </summary>
|
||||
/// <param name="x">TCP X。</param>
|
||||
/// <param name="y">TCP Y。</param>
|
||||
/// <param name="z">TCP Z。</param>
|
||||
void SetTcp(double x, double y, double z);
|
||||
|
||||
/// <summary>
|
||||
/// 读取指定 IO 端口。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
/// <returns>IO 当前值。</returns>
|
||||
bool GetIo(int port, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 写入指定 IO 端口。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前关节位置。
|
||||
/// </summary>
|
||||
/// <returns>当前关节位置。</returns>
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前末端位姿。
|
||||
/// </summary>
|
||||
/// <returns>当前末端位姿。</returns>
|
||||
IReadOnlyList<double> GetPose();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>控制器状态快照。</returns>
|
||||
ControllerStateSnapshot GetSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 执行一条已经完成规划的轨迹,并更新最终关节位置。
|
||||
/// </summary>
|
||||
/// <param name="result">规划结果。</param>
|
||||
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
|
||||
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
|
||||
}
|
||||
1135
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
1135
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
File diff suppressed because it is too large
Load Diff
25
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
25
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
@@ -0,0 +1,25 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
454
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
454
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
@@ -0,0 +1,454 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10012 命令通道客户端,提供 Req/Res 同步命令下发能力。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandClient : IDisposable
|
||||
{
|
||||
private readonly SemaphoreSlim _sendLock = new(1, 1);
|
||||
private readonly ILogger<FanucCommandClient>? _logger;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC 命令通道客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public FanucCommandClient(ILogger<FanucCommandClient>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10012 命令通道的连接。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">命令通道端口,默认 10012。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 10012, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_tcpClient is not null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道已经连接,请先 Disconnect。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("CommandClient ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_tcpClient = new TcpClient { NoDelay = true };
|
||||
await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_stream = _tcpClient.GetStream();
|
||||
|
||||
_logger?.LogInformation("CommandClient 已连接: {Ip}:{Port}", ip, port);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开命令通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_logger?.LogInformation("CommandClient Disconnect");
|
||||
|
||||
_stream?.Dispose();
|
||||
_stream = null;
|
||||
_tcpClient?.Dispose();
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送通用命令并等待响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="body">命令业务体。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>原始响应帧。</returns>
|
||||
public async Task<byte[]> SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackFrame(messageId, body.Span);
|
||||
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送携带程序名的命令并等待响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SendProgramCommandAsync(uint messageId, string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient StopProgram: {ProgramName}", programName);
|
||||
var result = await SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient StopProgram 成功: {ProgramName}", programName);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient ResetRobot");
|
||||
var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient ResetRobot 成功");
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定程序状态。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>程序状态响应。</returns>
|
||||
public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient GetProgramStatus: {ProgramName}", programName);
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient GetProgramStatus 成功: {ProgramName}", programName);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient StartProgram: {ProgramName}", programName);
|
||||
var result = await SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient StartProgram 成功: {ProgramName}", programName);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetSpeedRatio");
|
||||
var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetSpeedRatio 成功: ratio={Ratio}", result.Ratio);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetSpeedRatio: ratio={Ratio}", ratio);
|
||||
var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetSpeedRatio 成功: ratio={Ratio}", ratio);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetTcp: tcpId={TcpId}", tcpId);
|
||||
var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetTcp 成功: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", result.Pose.Select(v => v.ToString("F2"))));
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> pose, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetTcp: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", pose.Take(3).Select(v => v.ToString("F2"))));
|
||||
var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetTcp 成功: tcpId={TcpId}", tcpId);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public async Task<FanucIoResponse> GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetIo: port={Port}, ioType={IoType}", port, ioType);
|
||||
var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetIo 成功: port={Port}, value={Value}", port, result.Value);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetIo: port={Port}, value={Value}, ioType={IoType}", port, value, ioType);
|
||||
var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetIo 成功: port={Port}, value={Value}", port, value);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_stream?.Dispose();
|
||||
_stream = null;
|
||||
_tcpClient?.Dispose();
|
||||
_tcpClient = null;
|
||||
_sendLock.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 直接发送已封装的帧并读取响应。
|
||||
/// </summary>
|
||||
private async Task<byte[]> SendRawFrameAsync(byte[] frame, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var response = await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient 发送帧成功: 帧长度={FrameLength}, 响应长度={ResponseLength}", frame.Length, response.Length);
|
||||
return response;
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验普通命令响应结果码,失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucCommandResultResponse EnsureSuccess(FanucCommandResultResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验程序状态响应结果码,失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucProgramStatusResponse EnsureSuccess(FanucProgramStatusResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验速度倍率响应结果码,失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucSpeedRatioResponse EnsureSuccess(FanucSpeedRatioResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 TCP 位姿响应结果码,失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucTcpResponse EnsureSuccess(FanucTcpResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 IO 读取响应结果码,失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucIoResponse EnsureSuccess(FanucIoResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造包含 FANUC 命令上下文的失败异常。
|
||||
/// </summary>
|
||||
private static InvalidOperationException CreateCommandFailureException(uint messageId, uint resultCode)
|
||||
{
|
||||
return new InvalidOperationException(
|
||||
$"FANUC command 0x{messageId:X4} failed with result_code {resultCode}.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中读取一条完整的 doz/zod 响应帧。
|
||||
/// </summary>
|
||||
private async Task<byte[]> ReadResponseFrameAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
// 先读取 11 字节头:doz(3) + length(4) + msg_id(4)
|
||||
var header = new byte[11];
|
||||
await ReadExactAsync(header, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
if (header[0] != (byte)'d' || header[1] != (byte)'o' || header[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("响应帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(header.AsSpan(3, 4));
|
||||
if (declaredLength < 14)
|
||||
{
|
||||
throw new InvalidDataException("响应帧声明长度过短。");
|
||||
}
|
||||
|
||||
var remaining = (int)declaredLength - 11;
|
||||
var frame = new byte[declaredLength];
|
||||
header.CopyTo(frame, 0);
|
||||
await ReadExactAsync(frame.AsMemory(11, remaining), cancellationToken).ConfigureAwait(false);
|
||||
|
||||
// 校验帧尾
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("响应帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取指定长度的字节。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(Memory<byte> buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
var totalRead = 0;
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await _stream.ReadAsync(buffer.Slice(totalRead), cancellationToken).ConfigureAwait(false);
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("命令通道已断开,读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
}
|
||||
621
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
621
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
@@ -0,0 +1,621 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
|
||||
/// </summary>
|
||||
public static class FanucCommandMessageIds
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取控制器程序状态的消息号。
|
||||
/// </summary>
|
||||
public const uint GetProgramStatus = 0x2003;
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器的消息号。
|
||||
/// </summary>
|
||||
public const uint ResetRobot = 0x2100;
|
||||
|
||||
/// <summary>
|
||||
/// 启动控制器程序的消息号。
|
||||
/// </summary>
|
||||
public const uint StartProgram = 0x2102;
|
||||
|
||||
/// <summary>
|
||||
/// 停止控制器程序的消息号。
|
||||
/// </summary>
|
||||
public const uint StopProgram = 0x2103;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 的消息号。
|
||||
/// </summary>
|
||||
public const uint GetTcp = 0x2200;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 的消息号。
|
||||
/// </summary>
|
||||
public const uint SetTcp = 0x2201;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率的消息号。
|
||||
/// </summary>
|
||||
public const uint GetSpeedRatio = 0x2206;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率的消息号。
|
||||
/// </summary>
|
||||
public const uint SetSpeedRatio = 0x2207;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO 的消息号。
|
||||
/// </summary>
|
||||
public const uint GetIo = 0x2208;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO 的消息号。
|
||||
/// </summary>
|
||||
public const uint SetIo = 0x2209;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
|
||||
/// </summary>
|
||||
public static class FanucIoTypes
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 数字输入 DI。
|
||||
/// </summary>
|
||||
public const uint DigitalInput = 1;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 数字输出 DO。
|
||||
/// </summary>
|
||||
public const uint DigitalOutput = 2;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输入 RI。
|
||||
/// </summary>
|
||||
public const uint RobotInput = 8;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输出 RO。
|
||||
/// </summary>
|
||||
public const uint RobotOutput = 9;
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型字符串,例如 DI、DO、RI、RO。</param>
|
||||
/// <returns>命令通道使用的 IO 类型数值。</returns>
|
||||
public static uint FromName(string ioType)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(ioType))
|
||||
{
|
||||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||||
}
|
||||
|
||||
return ioType.Trim().ToUpperInvariant() switch
|
||||
{
|
||||
"DI" or "KIOTYPEDI" => DigitalInput,
|
||||
"DO" or "KIOTYPEDO" => DigitalOutput,
|
||||
"RI" or "KIOTYPERI" => RobotInput,
|
||||
"RO" or "KIOTYPERO" => RobotOutput,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandResultResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化命令结果响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucCommandResultResponse(uint messageId, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 速度倍率响应。
|
||||
/// </summary>
|
||||
public sealed class FanucSpeedRatioResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化速度倍率响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="ratioInt">控制器返回的整数百分比。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
RatioInt = ratioInt;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的整数百分比。
|
||||
/// </summary>
|
||||
public uint RatioInt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取转换后的 0.0 到 1.0 速度倍率。
|
||||
/// </summary>
|
||||
public double Ratio => RatioInt / 100.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 TCP 位姿响应。
|
||||
/// </summary>
|
||||
public sealed class FanucTcpResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 TCP 位姿响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="tcpId">控制器返回的 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
TcpId = tcpId;
|
||||
Pose = pose.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的 TCP ID。
|
||||
/// </summary>
|
||||
public uint TcpId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 7 维 TCP 位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 IO 读取响应。
|
||||
/// </summary>
|
||||
public sealed class FanucIoResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 IO 读取响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="numericValue">控制器返回的 float IO 数值。</param>
|
||||
public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
NumericValue = numericValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的原始数值。
|
||||
/// </summary>
|
||||
public double NumericValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取按布尔 IO 解释后的值。
|
||||
/// </summary>
|
||||
public bool Value => Math.Abs(NumericValue) > double.Epsilon;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 程序状态响应。
|
||||
/// </summary>
|
||||
public sealed class FanucProgramStatusResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化程序状态响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="programStatus">控制器程序状态。</param>
|
||||
public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
ProgramStatus = programStatus;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器程序状态值。
|
||||
/// </summary>
|
||||
public uint ProgramStatus { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
|
||||
/// </summary>
|
||||
public static class FanucCommandProtocol
|
||||
{
|
||||
/// <summary>
|
||||
/// 将无业务体命令封装为 TCP 10012 二进制帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackEmptyCommand(uint messageId)
|
||||
{
|
||||
return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将程序名命令封装为 TCP 10012 二进制帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="programName">控制器程序名。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackProgramCommand(uint messageId, string programName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
throw new ArgumentException("程序名不能为空。", nameof(programName));
|
||||
}
|
||||
|
||||
var programNameBytes = Encoding.ASCII.GetBytes(programName);
|
||||
var body = new byte[sizeof(uint) + programNameBytes.Length];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
|
||||
programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));
|
||||
|
||||
return PackFrame(messageId, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取速度倍率命令。
|
||||
/// </summary>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetSpeedRatioCommand()
|
||||
{
|
||||
return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置速度倍率命令,按旧系统逻辑转换为 0..100 的整数百分比。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetSpeedRatioCommand(double ratio)
|
||||
{
|
||||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||||
}
|
||||
|
||||
var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
|
||||
return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetTcpCommand(uint tcpId)
|
||||
{
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
|
||||
return PackFrame(FanucCommandMessageIds.GetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) + sizeof(float) * 7];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
|
||||
for (int i = 0; i < 7; i++)
|
||||
{
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
|
||||
}
|
||||
|
||||
return PackFrame(FanucCommandMessageIds.SetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 IO 命令,字段顺序为 io_type 后接 io_index。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
return PackFrame(FanucCommandMessageIds.GetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 IO 命令,字段顺序为 io_type、io_index、float io_value。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 布尔值。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2 + sizeof(float)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
|
||||
return PackFrame(FanucCommandMessageIds.SetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>命令结果响应。</returns>
|
||||
public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint))
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令响应体长度不足。");
|
||||
}
|
||||
|
||||
return new FanucCommandResultResponse(
|
||||
messageId,
|
||||
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
|
||||
}
|
||||
|
||||
// GetSpeedRatio 的字段顺序特殊:ratio_int 在前,result_code 在后。
|
||||
var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetTCP 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
|
||||
{
|
||||
throw new InvalidDataException("FANUC TCP 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
var pose = new double[7];
|
||||
for (int i = 0; i < pose.Length; i++)
|
||||
{
|
||||
pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
|
||||
}
|
||||
|
||||
return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetIO 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) + sizeof(float))
|
||||
{
|
||||
throw new InvalidDataException("FANUC IO 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
|
||||
return new FanucIoResponse(messageId, resultCode, ioValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>程序状态响应。</returns>
|
||||
public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
||||
}
|
||||
|
||||
// all-reconnect.pcap 中字段顺序为 prog_status 后接 result_code。
|
||||
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按 FANUC 命令通道 framing 规则封装完整帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="body">业务体。</param>
|
||||
/// <returns>完整命令帧。</returns>
|
||||
internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
|
||||
{
|
||||
var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
|
||||
var frame = new byte[frameLength];
|
||||
|
||||
frame[0] = (byte)'d';
|
||||
frame[1] = (byte)'o';
|
||||
frame[2] = (byte)'z';
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
|
||||
body.CopyTo(frame.AsSpan(11));
|
||||
frame[^3] = (byte)'z';
|
||||
frame[^2] = (byte)'o';
|
||||
frame[^1] = (byte)'d';
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验完整帧并读取消息号。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>响应消息号。</returns>
|
||||
private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
if (frame.Length < 14)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧长度不足。");
|
||||
}
|
||||
|
||||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||
if (declaredLength != frame.Length)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
|
||||
}
|
||||
|
||||
return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取完整帧中的业务体切片。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>业务体切片。</returns>
|
||||
private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
return frame.Slice(11, frame.Length - 14);
|
||||
}
|
||||
}
|
||||
654
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
654
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
@@ -0,0 +1,654 @@
|
||||
using System.Diagnostics;
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端,提供状态包驱动的命令发送与响应接收能力。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Client : IDisposable
|
||||
{
|
||||
private readonly object _commandLock = new();
|
||||
private readonly object _responseLock = new();
|
||||
private readonly ILogger<FanucJ519Client>? _logger;
|
||||
private UdpClient? _udpClient;
|
||||
private CancellationTokenSource? _cts;
|
||||
private Thread? _receiveThread;
|
||||
private FanucJ519Command? _currentCommand;
|
||||
private FanucJ519Command? _lastSentCommand;
|
||||
// 稠密轨迹执行时预装的命令队列,由机器人状态包节拍逐帧出队。
|
||||
private Queue<FanucJ519Command>? _commandQueue;
|
||||
private TaskCompletionSource? _commandQueueDrainedCompletion;
|
||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||
private FanucJ519Response? _latestResponse;
|
||||
private long _slowSendCount;
|
||||
private long _maxReceiveToSendTicks;
|
||||
private uint _sequenceBufferSize;
|
||||
// 标记 StartMotion 前是否刚装载过新目标,用于区分新命令和上次运动残留目标。
|
||||
private bool _hasPendingCommandForStart;
|
||||
private bool _motionStarted;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已创建 UDP 套接字。
|
||||
/// </summary>
|
||||
public bool IsConnected => _udpClient is not null;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC J519 客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器;允许 null,供无日志场景使用。</param>
|
||||
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">运动通道端口,默认 60015。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_udpClient is not null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道已经连接,请先 Disconnect。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_udpClient = new UdpClient();
|
||||
_udpClient.Connect(ip, port);
|
||||
ConfigureRealtimeSocket(_udpClient.Client);
|
||||
ConfigureProcessPriority();
|
||||
|
||||
// 发送初始化包。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation("J519 初始化包已发送");
|
||||
|
||||
_cts = new CancellationTokenSource();
|
||||
_receiveThread = new Thread(ReceiveLoop)
|
||||
{
|
||||
IsBackground = true,
|
||||
Name = "J519 UDP realtime loop",
|
||||
Priority = ThreadPriority.Highest
|
||||
};
|
||||
_receiveThread.Start();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动 J519 命令发送许可;实际发包由机器人状态包节拍驱动。
|
||||
/// </summary>
|
||||
public void StartMotion()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道未连接。");
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_lastSentCommand = null;
|
||||
if (_motionStarted)
|
||||
{
|
||||
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_hasPendingCommandForStart)
|
||||
{
|
||||
_currentCommand = null;
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
|
||||
_hasPendingCommandForStart = false;
|
||||
_motionStarted = true;
|
||||
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 配置状态包驱动回发时附加到机器人 sequence 的前视缓冲深度。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">要附加到状态序号上的缓冲深度。</param>
|
||||
public void SetSequenceBufferSize(int bufferSize)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (bufferSize < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(bufferSize), "J519 sequence buffer size 不能为负数。");
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_sequenceBufferSize = (uint)bufferSize;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送状态输出停止包并停止 J519 命令发送。
|
||||
/// </summary>
|
||||
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送");
|
||||
lock (_commandLock)
|
||||
{
|
||||
_motionStarted = false;
|
||||
_hasPendingCommandForStart = false;
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
|
||||
// FANUC 手册中 packet type=2 表示停止状态包输出;当前保留现场抓包兼容行为。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 原子更新下一周期要发送的 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="command">新的 J519 命令。</param>
|
||||
public void UpdateCommand(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
_currentCommand = command;
|
||||
_hasPendingCommandForStart = true;
|
||||
_commandHistoryForTests?.Add(command);
|
||||
}
|
||||
|
||||
if (_logger?.IsEnabled(LogLevel.Debug) == true)
|
||||
{
|
||||
//_logger.LogDebug(
|
||||
// "J519 UpdateCommand: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
|
||||
// command.TargetJoints,
|
||||
// command.WriteIoMask,
|
||||
// command.WriteIoValue);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 装载一整段 J519 命令队列;后续每个可接收命令的机器人状态包会自动取出下一帧。
|
||||
/// </summary>
|
||||
/// <param name="commands">按执行顺序排列的 J519 命令列表,至少包含一帧。</param>
|
||||
public void LoadCommandQueue(IReadOnlyList<FanucJ519Command> commands)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(commands);
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
if (commands.Count == 0)
|
||||
{
|
||||
throw new ArgumentException("J519 命令队列至少需要包含一帧。", nameof(commands));
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
_commandQueue = new Queue<FanucJ519Command>(commands);
|
||||
// 队列耗尽后继续保持最后一帧目标,避免运动结束后回落到旧目标或空目标。
|
||||
_currentCommand = commands[^1];
|
||||
_hasPendingCommandForStart = true;
|
||||
_commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
|
||||
_commandHistoryForTests?.AddRange(commands);
|
||||
}
|
||||
_logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count);
|
||||
_logger?.LogInformation("开始运动前向机器人发送的sequence={Sequence}",_lastSentCommand?.Sequence ?? 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待当前预装命令队列被状态包全部取出;无队列时立即完成。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>表示等待过程的任务。</returns>
|
||||
internal Task WaitForCommandQueueDrainedAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
Task waitTask;
|
||||
lock (_commandLock)
|
||||
{
|
||||
waitTask = _commandQueueDrainedCompletion?.Task ?? Task.CompletedTask;
|
||||
}
|
||||
|
||||
return waitTask.WaitAsync(cancellationToken);
|
||||
}
|
||||
|
||||
/// 判断当前是否没有等待出队的命令;仅供单元测试断言。
|
||||
/// </summary>
|
||||
/// <returns>如果队列为空或尚未装载队列,则返回 true。</returns>
|
||||
internal bool IsCommandQueueDrainedForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _commandQueue is null || _commandQueue.Count == 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开命令历史记录,仅供单元测试验证运行时生成的命令序列。
|
||||
/// </summary>
|
||||
internal void EnableCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_commandHistoryForTests = [];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取测试记录的命令历史。
|
||||
/// </summary>
|
||||
/// <returns>命令历史快照。</returns>
|
||||
internal IReadOnlyList<FanucJ519Command> GetCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _commandHistoryForTests?.ToArray() ?? Array.Empty<FanucJ519Command>();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次通过 UpdateCommand 设置的 J519 命令;供测试断言使用。
|
||||
/// </summary>
|
||||
/// <returns>当前 J519 命令或 null。</returns>
|
||||
internal FanucJ519Command? GetCurrentCommand()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _currentCommand;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次解析的 J519 响应;若尚未收到任何响应则返回 null。
|
||||
/// </summary>
|
||||
/// <returns>最新 J519 响应或 null。</returns>
|
||||
public FanucJ519Response? GetLatestResponse()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
return _latestResponse;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开 J519 通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_udpClient?.Dispose();
|
||||
_receiveThread?.Join(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// 忽略释放期间的套接字关闭异常。
|
||||
}
|
||||
|
||||
_receiveThread = null;
|
||||
|
||||
_cts?.Dispose();
|
||||
_cts = null;
|
||||
|
||||
_udpClient = null;
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
_lastSentCommand = null;
|
||||
CompleteCommandQueueLocked();
|
||||
_commandHistoryForTests = null;
|
||||
_hasPendingCommandForStart = false;
|
||||
_motionStarted = false;
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_udpClient?.Dispose();
|
||||
_receiveThread?.Join(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// 忽略释放期间的套接字关闭异常。
|
||||
}
|
||||
|
||||
_cts?.Dispose();
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
_hasPendingCommandForStart = false;
|
||||
}
|
||||
}
|
||||
|
||||
private static void ConfigureProcessPriority()
|
||||
{
|
||||
try
|
||||
{
|
||||
var process = Process.GetCurrentProcess();
|
||||
if (process.PriorityClass < ProcessPriorityClass.High)
|
||||
{
|
||||
process.PriorityClass = ProcessPriorityClass.High;
|
||||
}
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// 某些部署环境不允许提升进程优先级;实时链路仍按普通优先级运行。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 配置 J519 UDP 套接字的低延迟参数。
|
||||
/// </summary>
|
||||
/// <param name="socket">已连接 FANUC 60015 的 UDP 套接字。</param>
|
||||
private void ConfigureRealtimeSocket(Socket socket)
|
||||
{
|
||||
socket.ReceiveBufferSize = 1024 * 1024;
|
||||
socket.SendBufferSize = 1024 * 1024;
|
||||
|
||||
try
|
||||
{
|
||||
// DSCP EF(46) 标记低延迟流量,是否生效取决于现场网卡、交换机和控制柜网络策略。
|
||||
socket.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.TypeOfService, 0xB8);
|
||||
}
|
||||
catch (SocketException ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "J519 UDP 套接字无法设置 DSCP EF 优先级标记");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台接收循环:在专用高优先级线程中同步接收 132B 响应并立即回发命令。
|
||||
/// </summary>
|
||||
private void ReceiveLoop()
|
||||
{
|
||||
var udpClient = _udpClient;
|
||||
var cancellationToken = _cts?.Token ?? CancellationToken.None;
|
||||
if (udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 ReceiveLoop 启动");
|
||||
long receiveCount = 0;
|
||||
FanucJ519Response? lastLoggedResponse = null;
|
||||
var receiveBuffer = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||
var commandBuffer = new byte[FanucJ519Protocol.CommandPacketLength];
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
var received = udpClient.Client.Receive(receiveBuffer);
|
||||
if (received == FanucJ519Protocol.ResponsePacketLength)
|
||||
{
|
||||
var receiveTicks = Stopwatch.GetTimestamp();
|
||||
var response = FanucJ519Protocol.ParseResponse(receiveBuffer);
|
||||
|
||||
// 先按状态包节拍回发命令,再做低频日志处理,减少受信周期内的非必要工作。
|
||||
if (response.AcceptsCommand)
|
||||
{
|
||||
SendCommandForStatus(udpClient.Client, response, commandBuffer, receiveTicks);
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = response;
|
||||
}
|
||||
|
||||
receiveCount++;
|
||||
|
||||
// 仅在状态变化时记录 Info,避免高频日志。
|
||||
if (lastLoggedResponse is null
|
||||
|| lastLoggedResponse.Status != response.Status
|
||||
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|
||||
|| lastLoggedResponse.SystemReady != response.SystemReady
|
||||
|| lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, received={received}, SystemReady={SystemReady}, RobotInMotion={RobotInMotion}, pose=[{Pose}], joints=[{Joints}]",
|
||||
response.Status,
|
||||
response.Sequence,
|
||||
response.AcceptsCommand,
|
||||
response.ReceivedCommand,
|
||||
response.SystemReady,
|
||||
response.RobotInMotion,
|
||||
string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
|
||||
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F3"))));
|
||||
|
||||
var lastSentTargetJoints = GetLastSentTargetJointsLogText();
|
||||
_logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
|
||||
lastLoggedResponse = response;
|
||||
|
||||
// 如果状态从AcceptsCommand true 变为false,说明机器人报错,清空队列
|
||||
if (!response.AcceptsCommand)
|
||||
{
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
_logger?.LogWarning("J519 接收状态包显示机器人不可接受命令,已清空命令队列");
|
||||
}
|
||||
|
||||
}
|
||||
else if (receiveCount % 1000 == 0)
|
||||
{
|
||||
var maxReceiveToSendMs = Stopwatch.GetElapsedTime(0, Interlocked.Read(ref _maxReceiveToSendTicks)).TotalMilliseconds;
|
||||
_logger?.LogDebug(
|
||||
"J519 已接收 {Count} 个响应包,receive-to-send 最大耗时约 {MaxMs:F3}ms,超过 0.5ms 次数 {SlowCount}",
|
||||
receiveCount,
|
||||
maxReceiveToSendMs,
|
||||
Interlocked.Read(ref _slowSendCount));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
_logger?.LogInformation("J519 ReceiveLoop 正常取消,共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出,共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
catch (SocketException ex) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogInformation(ex, "J519 ReceiveLoop 因取消关闭套接字退出,共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="socket">已连接的 UDP 套接字。</param>
|
||||
/// <param name="response">刚收到的机器人状态包。</param>
|
||||
/// <param name="commandBuffer">可复用的 64B 命令包缓冲区。</param>
|
||||
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
|
||||
private void SendCommandForStatus(Socket socket, FanucJ519Response response, byte[] commandBuffer, long receiveTicks)
|
||||
{
|
||||
FanucJ519Command? command;
|
||||
var willDrainQueue = false;
|
||||
lock (_commandLock)
|
||||
{
|
||||
if (!_motionStarted)
|
||||
{
|
||||
command = null;
|
||||
}
|
||||
else if (_commandQueue is { Count: > 0 } queue)
|
||||
{
|
||||
// 状态包是唯一节拍源:每收到一帧可接收状态,才取出下一条目标。
|
||||
var queuedCommand = queue.Dequeue();
|
||||
_currentCommand = queuedCommand;
|
||||
willDrainQueue = queue.Count == 0;
|
||||
command = queuedCommand;
|
||||
}
|
||||
else
|
||||
{
|
||||
command = _currentCommand;
|
||||
}
|
||||
}
|
||||
|
||||
if (command is null)
|
||||
{
|
||||
command = TryBuildHoldCommandFromLatestResponse(response);
|
||||
}
|
||||
|
||||
if (command is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
uint sequenceToSend;
|
||||
lock (_commandLock)
|
||||
{
|
||||
sequenceToSend = response.Sequence + _sequenceBufferSize;
|
||||
}
|
||||
|
||||
FanucJ519Protocol.PackCommandPacket(command, sequenceToSend, commandBuffer);
|
||||
socket.Send(commandBuffer);
|
||||
TrackReceiveToSendLatency(receiveTicks);
|
||||
// _logger?.LogDebug("J519 已回发命令包,seq={Seq}", sequence);
|
||||
// _logger?.LogDebug(
|
||||
// "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
|
||||
// command.TargetJoints,
|
||||
// command.WriteIoMask,
|
||||
// command.WriteIoValue);
|
||||
lock (_commandLock)
|
||||
{
|
||||
_lastSentCommand = command;
|
||||
if (willDrainQueue && _commandQueue is { Count: 0 })
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当当前没有显式目标时,使用最近一帧状态反馈关节角构造保姿命令,维持机器人当前位置。
|
||||
/// </summary>
|
||||
/// <param name="response">当前收到的机器人状态包。</param>
|
||||
/// <returns>可用于保姿的临时 J519 命令;若反馈关节不足则返回 null。</returns>
|
||||
private static FanucJ519Command? TryBuildHoldCommandFromLatestResponse(FanucJ519Response response)
|
||||
{
|
||||
if (response.JointDegrees.Count < 6)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// 无运动目标时,持续回发机器人当前反馈关节,保持伺服流与机器人当前位置一致。
|
||||
return new FanucJ519Command(
|
||||
sequence: response.Sequence,
|
||||
targetJoints:
|
||||
[
|
||||
response.JointDegrees[0],
|
||||
response.JointDegrees[1],
|
||||
response.JointDegrees[2],
|
||||
response.JointDegrees[3],
|
||||
response.JointDegrees[4],
|
||||
response.JointDegrees[5]
|
||||
]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录状态包到命令包发出的最大耗时和慢发送次数,供低频诊断日志观察调度抖动。
|
||||
/// </summary>
|
||||
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
|
||||
private void TrackReceiveToSendLatency(long receiveTicks)
|
||||
{
|
||||
var elapsedTicks = Stopwatch.GetTimestamp() - receiveTicks;
|
||||
var currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
|
||||
while (elapsedTicks > currentMax
|
||||
&& Interlocked.CompareExchange(ref _maxReceiveToSendTicks, elapsedTicks, currentMax) != currentMax)
|
||||
{
|
||||
currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
|
||||
}
|
||||
|
||||
if (Stopwatch.GetElapsedTime(0, elapsedTicks) > TimeSpan.FromMilliseconds(0.5))
|
||||
{
|
||||
Interlocked.Increment(ref _slowSendCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清空当前命令队列,并唤醒等待队列结束的运行时任务。
|
||||
/// </summary>
|
||||
private void CompleteCommandQueueLocked()
|
||||
{
|
||||
_commandQueue?.Clear();
|
||||
_commandQueue = null;
|
||||
_commandQueueDrainedCompletion?.TrySetResult();
|
||||
_commandQueueDrainedCompletion = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取最近一次已成功发送命令的目标关节轴文本,便于状态日志直接对照控制目标。
|
||||
/// </summary>
|
||||
/// <returns>格式化后的目标关节轴文本;如果尚未发送命令则返回占位符。</returns>
|
||||
private string GetLastSentTargetJointsLogText()
|
||||
{
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _lastSentCommand is null
|
||||
? "n/a"
|
||||
: string.Join(", ", _lastSentCommand.TargetJoints.Take(6).Select(v => v.ToString("F5")));
|
||||
}
|
||||
}
|
||||
}
|
||||
403
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
403
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
@@ -0,0 +1,403 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Command
|
||||
{
|
||||
private readonly double[] _targetJoints;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 命令数据。
|
||||
/// </summary>
|
||||
/// <param name="sequence">命令序号。</param>
|
||||
/// <param name="targetJoints">目标关节或扩展轴数据,最多 9 个槽位。</param>
|
||||
/// <param name="lastData">是否为最后一帧数据。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="dataStyle">目标数据类型。</param>
|
||||
/// <param name="writeIoType">写入 IO 类型。</param>
|
||||
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
|
||||
/// <param name="writeIoMask">写入 IO 掩码。</param>
|
||||
/// <param name="writeIoValue">写入 IO 数值。</param>
|
||||
public FanucJ519Command(
|
||||
uint sequence,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
byte lastData = 0,
|
||||
byte readIoType = 2,
|
||||
ushort readIoIndex = 1,
|
||||
ushort readIoMask = 255,
|
||||
byte dataStyle = 1,
|
||||
byte writeIoType = 2,
|
||||
ushort writeIoIndex = 1,
|
||||
ushort writeIoMask = 0,
|
||||
ushort writeIoValue = 0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||
if (targetJoints.Count is <= 0 or > 9)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
|
||||
}
|
||||
|
||||
Sequence = sequence;
|
||||
LastData = lastData;
|
||||
ReadIoType = readIoType;
|
||||
ReadIoIndex = readIoIndex;
|
||||
ReadIoMask = readIoMask;
|
||||
DataStyle = dataStyle;
|
||||
WriteIoType = writeIoType;
|
||||
WriteIoIndex = writeIoIndex;
|
||||
WriteIoMask = writeIoMask;
|
||||
WriteIoValue = writeIoValue;
|
||||
_targetJoints = targetJoints.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取命令序号。
|
||||
/// </summary>
|
||||
public uint Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否为最后一帧数据。
|
||||
/// </summary>
|
||||
public byte LastData { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 类型。
|
||||
/// </summary>
|
||||
public byte ReadIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort ReadIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort ReadIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取目标数据类型。
|
||||
/// </summary>
|
||||
public byte DataStyle { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 类型。
|
||||
/// </summary>
|
||||
public byte WriteIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort WriteIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort WriteIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 数值。
|
||||
/// </summary>
|
||||
public ushort WriteIoValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取目标关节或扩展轴数据。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> TargetJoints => _targetJoints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Response
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly double[] _jointDegrees;
|
||||
private readonly double[] _motorCurrents;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 响应数据。
|
||||
/// </summary>
|
||||
/// <param name="messageType">响应类型。</param>
|
||||
/// <param name="version">协议版本。</param>
|
||||
/// <param name="sequence">响应序号。</param>
|
||||
/// <param name="status">状态位集合。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="readIoValue">读取 IO 数值。</param>
|
||||
/// <param name="timestamp">控制器时间戳。</param>
|
||||
/// <param name="pose">TCP 笛卡尔位姿。</param>
|
||||
/// <param name="externalAxes">扩展轴反馈。</param>
|
||||
/// <param name="jointDegrees">关节角度反馈。</param>
|
||||
/// <param name="motorCurrents">电机电流反馈。</param>
|
||||
public FanucJ519Response(
|
||||
uint messageType,
|
||||
uint version,
|
||||
uint sequence,
|
||||
byte status,
|
||||
byte readIoType,
|
||||
ushort readIoIndex,
|
||||
ushort readIoMask,
|
||||
ushort readIoValue,
|
||||
uint timestamp,
|
||||
IEnumerable<double> pose,
|
||||
IEnumerable<double> externalAxes,
|
||||
IEnumerable<double> jointDegrees,
|
||||
IEnumerable<double> motorCurrents)
|
||||
{
|
||||
MessageType = messageType;
|
||||
Version = version;
|
||||
Sequence = sequence;
|
||||
Status = status;
|
||||
ReadIoType = readIoType;
|
||||
ReadIoIndex = readIoIndex;
|
||||
ReadIoMask = readIoMask;
|
||||
ReadIoValue = readIoValue;
|
||||
Timestamp = timestamp;
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_externalAxes = externalAxes?.ToArray() ?? throw new ArgumentNullException(nameof(externalAxes));
|
||||
_jointDegrees = jointDegrees?.ToArray() ?? throw new ArgumentNullException(nameof(jointDegrees));
|
||||
_motorCurrents = motorCurrents?.ToArray() ?? throw new ArgumentNullException(nameof(motorCurrents));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应类型。
|
||||
/// </summary>
|
||||
public uint MessageType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取协议版本。
|
||||
/// </summary>
|
||||
public uint Version { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应序号。
|
||||
/// </summary>
|
||||
public uint Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态位集合。
|
||||
/// </summary>
|
||||
public byte Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 类型。
|
||||
/// </summary>
|
||||
public byte ReadIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort ReadIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort ReadIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 数值。
|
||||
/// </summary>
|
||||
public ushort ReadIoValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器时间戳。
|
||||
/// </summary>
|
||||
public uint Timestamp { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 笛卡尔位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取扩展轴反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取关节角度反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
||||
|
||||
/// <summary>
|
||||
/// 获取电机电流反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> MotorCurrents => _motorCurrents;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器是否接受命令。
|
||||
/// </summary>
|
||||
public bool AcceptsCommand => (Status & 0b0001) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器是否已收到命令。
|
||||
/// </summary>
|
||||
public bool ReceivedCommand => (Status & 0b0010) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器系统是否就绪。
|
||||
/// </summary>
|
||||
public bool SystemReady => (Status & 0b0100) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取机器人是否处于运动中。
|
||||
/// </summary>
|
||||
public bool RobotInMotion => (Status & 0b1000) != 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。
|
||||
/// </summary>
|
||||
public static class FanucJ519Protocol
|
||||
{
|
||||
/// <summary>
|
||||
/// J519 初始化和结束控制包长度。
|
||||
/// </summary>
|
||||
public const int ControlPacketLength = 8;
|
||||
|
||||
/// <summary>
|
||||
/// J519 命令包长度。
|
||||
/// </summary>
|
||||
public const int CommandPacketLength = 64;
|
||||
|
||||
/// <summary>
|
||||
/// J519 响应包长度。
|
||||
/// </summary>
|
||||
public const int ResponsePacketLength = 132;
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 初始化包。
|
||||
/// </summary>
|
||||
/// <returns>初始化包。</returns>
|
||||
public static byte[] PackInitPacket()
|
||||
{
|
||||
return PackControlPacket(0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 结束包。
|
||||
/// </summary>
|
||||
/// <returns>结束包。</returns>
|
||||
public static byte[] PackEndPacket()
|
||||
{
|
||||
return PackControlPacket(2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 64 字节命令包。
|
||||
/// </summary>
|
||||
/// <param name="command">命令数据。</param>
|
||||
/// <returns>命令包。</returns>
|
||||
public static byte[] PackCommandPacket(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
|
||||
var packet = new byte[CommandPacketLength];
|
||||
PackCommandPacket(command, command.Sequence, packet);
|
||||
return packet;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 J519 64 字节命令包写入调用方提供的缓冲区。
|
||||
/// </summary>
|
||||
/// <param name="command">命令数据。</param>
|
||||
/// <param name="sequence">本次出包使用的机器人状态包序号。</param>
|
||||
/// <param name="packet">长度至少为 64 字节的命令包缓冲区。</param>
|
||||
public static void PackCommandPacket(FanucJ519Command command, uint sequence, Span<byte> packet)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
if (packet.Length < CommandPacketLength)
|
||||
{
|
||||
throw new ArgumentException("J519 命令包缓冲区长度不足。", nameof(packet));
|
||||
}
|
||||
|
||||
packet.Slice(0, CommandPacketLength).Clear();
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x00, sizeof(uint)), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x04, sizeof(uint)), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x08, sizeof(uint)), sequence);
|
||||
packet[0x0c] = command.LastData;
|
||||
packet[0x0d] = command.ReadIoType;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort)), command.ReadIoIndex);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x10, sizeof(ushort)), command.ReadIoMask);
|
||||
packet[0x12] = command.DataStyle;
|
||||
packet[0x13] = command.WriteIoType;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x14, sizeof(ushort)), command.WriteIoIndex);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x16, sizeof(ushort)), command.WriteIoMask);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x18, sizeof(ushort)), command.WriteIoValue);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x1a, sizeof(ushort)), 0);
|
||||
|
||||
// J519 命令包固定保留 9 个 f32 目标槽位,少于 9 个时剩余槽位补零。
|
||||
for (var index = 0; index < 9; index++)
|
||||
{
|
||||
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
|
||||
BinaryPrimitives.WriteSingleBigEndian(packet.Slice(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 J519 132 字节响应包。
|
||||
/// </summary>
|
||||
/// <param name="packet">响应包。</param>
|
||||
/// <returns>响应解析结果。</returns>
|
||||
public static FanucJ519Response ParseResponse(ReadOnlySpan<byte> packet)
|
||||
{
|
||||
if (packet.Length != ResponsePacketLength)
|
||||
{
|
||||
throw new InvalidDataException("FANUC J519 响应包长度不正确。");
|
||||
}
|
||||
|
||||
return new FanucJ519Response(
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x00, sizeof(uint))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x04, sizeof(uint))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x08, sizeof(uint))),
|
||||
packet[0x0c],
|
||||
packet[0x0d],
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x10, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x12, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x14, sizeof(uint))),
|
||||
ReadFloatArray(packet, 0x18, 6),
|
||||
ReadFloatArray(packet, 0x30, 3),
|
||||
ReadFloatArray(packet, 0x3c, 9),
|
||||
ReadFloatArray(packet, 0x60, 9));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 控制包。
|
||||
/// </summary>
|
||||
/// <param name="packetType">控制包类型。</param>
|
||||
/// <returns>控制包。</returns>
|
||||
private static byte[] PackControlPacket(uint packetType)
|
||||
{
|
||||
var packet = new byte[ControlPacketLength];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0, sizeof(uint)), packetType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(sizeof(uint), sizeof(uint)), 1);
|
||||
return packet;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从响应包中读取固定长度 f32 数组。
|
||||
/// </summary>
|
||||
/// <param name="packet">响应包。</param>
|
||||
/// <param name="offset">数组起始偏移。</param>
|
||||
/// <param name="count">数组元素数量。</param>
|
||||
/// <returns>转换成 double 的数值数组。</returns>
|
||||
private static double[] ReadFloatArray(ReadOnlySpan<byte> packet, int offset, int count)
|
||||
{
|
||||
var values = new double[count];
|
||||
for (var index = 0; index < count; index++)
|
||||
{
|
||||
values[index] = BinaryPrimitives.ReadSingleBigEndian(packet.Slice(offset + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
return values;
|
||||
}
|
||||
}
|
||||
632
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
632
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
@@ -0,0 +1,632 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的连接阶段。
|
||||
/// </summary>
|
||||
public enum FanucStateConnectionState
|
||||
{
|
||||
/// <summary>
|
||||
/// 状态通道未连接。
|
||||
/// </summary>
|
||||
Disconnected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在建立连接。
|
||||
/// </summary>
|
||||
Connecting,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道已连接并由后台循环接收状态帧。
|
||||
/// </summary>
|
||||
Connected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道在限定时间内没有收到完整状态帧。
|
||||
/// </summary>
|
||||
TimedOut,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在按退避策略重新连接。
|
||||
/// </summary>
|
||||
Reconnecting,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10010 状态通道的超时和重连参数。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置接收一帧完整 90B 状态帧允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan FrameTimeout { get; init; } = TimeSpan.FromMilliseconds(250);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置初始重连等待时间。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectInitialDelay { get; init; } = TimeSpan.FromMilliseconds(100);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置重连等待时间的上限。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(5);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置单次 TCP 建连允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的当前诊断状态。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientStatus
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化状态通道诊断状态。
|
||||
/// </summary>
|
||||
public FanucStateClientStatus(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
DateTimeOffset? lastFrameAt,
|
||||
long reconnectAttemptCount,
|
||||
string? lastErrorMessage)
|
||||
{
|
||||
State = state;
|
||||
IsFrameStale = isFrameStale;
|
||||
LastFrameAt = lastFrameAt;
|
||||
ReconnectAttemptCount = reconnectAttemptCount;
|
||||
LastErrorMessage = lastErrorMessage;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前连接阶段。
|
||||
/// </summary>
|
||||
public FanucStateConnectionState State { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近缓存状态帧是否已经超过状态帧超时窗口。
|
||||
/// </summary>
|
||||
public bool IsFrameStale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次成功解析状态帧的 UTC 时间。
|
||||
/// </summary>
|
||||
public DateTimeOffset? LastFrameAt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取后台循环发起重连的累计次数。
|
||||
/// </summary>
|
||||
public long ReconnectAttemptCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次状态通道异常的诊断文本。
|
||||
/// </summary>
|
||||
public string? LastErrorMessage { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10010 状态通道客户端,持续接收状态帧并缓存最新快照。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClient : IDisposable
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly FanucStateClientOptions _options;
|
||||
private readonly ILogger<FanucStateClient>? _logger;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private CancellationTokenSource? _receiveCts;
|
||||
private Task? _receiveTask;
|
||||
private FanucStateFrame? _latestFrame;
|
||||
private FanucStateConnectionState _connectionState = FanucStateConnectionState.Disconnected;
|
||||
private DateTimeOffset? _lastConnectedAt;
|
||||
private DateTimeOffset? _lastFrameAt;
|
||||
private long _reconnectAttemptCount;
|
||||
private string? _lastErrorMessage;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 使用默认状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
public FanucStateClient()
|
||||
: this(new FanucStateClientOptions(), null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options)
|
||||
: this(options, null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数和日志记录器初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
/// <param name="logger">日志记录器;允许 null。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options, ILogger<FanucStateClient>? logger)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(options);
|
||||
ValidateOptions(options);
|
||||
_options = options;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => GetStatus().State == FanucStateConnectionState.Connected;
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">状态通道端口,默认 10010。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 10010, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_receiveTask is not null)
|
||||
{
|
||||
throw new InvalidOperationException("状态通道已经连接,请先 Disconnect。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("StateClient ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_receiveCts = new CancellationTokenSource();
|
||||
using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _receiveCts.Token);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Connecting;
|
||||
_latestFrame = null;
|
||||
_lastConnectedAt = null;
|
||||
_lastFrameAt = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, linkedCts.Token).ConfigureAwait(false);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_logger?.LogError(exception, "StateClient 连接失败: {Ip}:{Port}", ip, port);
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
}
|
||||
|
||||
_receiveCts.Dispose();
|
||||
_receiveCts = null;
|
||||
throw;
|
||||
}
|
||||
|
||||
_receiveTask = Task.Run(
|
||||
() => ReceiveAndReconnectLoopAsync(ip, port, _receiveCts.Token),
|
||||
_receiveCts.Token);
|
||||
|
||||
_logger?.LogInformation("StateClient 已连接并启动接收循环: {Ip}:{Port}", ip, port);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开状态通道并停止后台接收循环。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_logger?.LogInformation("StateClient Disconnect");
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次解析的状态帧;若尚未收到任何帧则返回 null。
|
||||
/// </summary>
|
||||
/// <returns>最新状态帧或 null。</returns>
|
||||
public FanucStateFrame? GetLatestFrame()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _latestFrame;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前诊断状态。
|
||||
/// </summary>
|
||||
/// <returns>状态通道诊断快照。</returns>
|
||||
public FanucStateClientStatus GetStatus()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return new FanucStateClientStatus(
|
||||
_connectionState,
|
||||
IsFrameStaleLocked(DateTimeOffset.UtcNow),
|
||||
_lastFrameAt,
|
||||
_reconnectAttemptCount,
|
||||
_lastErrorMessage);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台循环:持续接收状态帧;断线、超时或坏帧后进入退避重连。
|
||||
/// </summary>
|
||||
private async Task ReceiveAndReconnectLoopAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var reconnectDelay = _options.ReconnectInitialDelay;
|
||||
_logger?.LogInformation("StateClient 接收循环启动: {Ip}:{Port}", ip, port);
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
await ReceiveCurrentConnectionAsync(cancellationToken).ConfigureAwait(false);
|
||||
reconnectDelay = _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogInformation("StateClient 接收循环正常取消");
|
||||
return;
|
||||
}
|
||||
catch (TimeoutException ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 接收超时");
|
||||
MarkReceiveFailure(FanucStateConnectionState.TimedOut, ex.Message);
|
||||
}
|
||||
catch (Exception ex) when (ex is IOException or InvalidDataException or SocketException or ObjectDisposedException)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 连接异常,准备重连");
|
||||
MarkReceiveFailure(FanucStateConnectionState.Reconnecting, ex.Message);
|
||||
}
|
||||
|
||||
CloseCurrentConnection();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
reconnectDelay = await ReconnectWithBackoffAsync(ip, port, reconnectDelay, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前连接中持续读取状态帧,直到连接异常或被取消。
|
||||
/// </summary>
|
||||
private async Task ReceiveCurrentConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
NetworkStream stream;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream ?? throw new IOException("状态通道未连接。");
|
||||
}
|
||||
|
||||
var buffer = new byte[FanucStateProtocol.StateFrameLength];
|
||||
long frameCount = 0;
|
||||
FanucStateFrame? lastLoggedFrame = null;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await ReadExactAsync(stream, buffer, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(buffer);
|
||||
lock (_stateLock)
|
||||
{
|
||||
_latestFrame = frame;
|
||||
_lastFrameAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
|
||||
frameCount++;
|
||||
|
||||
// 仅在状态变化或首次接收时记录 Info,避免高频日志。
|
||||
if (lastLoggedFrame is null
|
||||
|| lastLoggedFrame.CartesianPose[0] != frame.CartesianPose[0]
|
||||
|| !lastLoggedFrame.RawTailWords.SequenceEqual(frame.RawTailWords))
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"StateClient 收到状态帧: pose=[{X:F1}, {Y:F1}, {Z:F1}], tail=[{Tail}]",
|
||||
frame.CartesianPose[0],
|
||||
frame.CartesianPose[1],
|
||||
frame.CartesianPose[2],
|
||||
string.Join(", ", frame.RawTailWords));
|
||||
lastLoggedFrame = frame;
|
||||
}
|
||||
else if (frameCount % 1000 == 0)
|
||||
{
|
||||
_logger?.LogDebug("StateClient 已接收 {Count} 个状态帧", frameCount);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取固定长度字节,超过帧超时窗口则抛出超时异常。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.FrameTimeout);
|
||||
|
||||
var totalRead = 0;
|
||||
try
|
||||
{
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await stream.ReadAsync(
|
||||
buffer.AsMemory(totalRead, buffer.Length - totalRead),
|
||||
timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("状态通道已断开,读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw new TimeoutException("状态通道接收超时,未在限定时间内收到完整 90B 状态帧。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开 TCP 状态通道并更新连接状态。
|
||||
/// </summary>
|
||||
private async Task OpenConnectionAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var tcpClient = new TcpClient { NoDelay = true };
|
||||
try
|
||||
{
|
||||
_logger?.LogInformation("StateClient 正在连接 {Ip}:{Port}...", ip, port);
|
||||
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.ConnectTimeout);
|
||||
await tcpClient.ConnectAsync(ip, port, timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_tcpClient = tcpClient;
|
||||
_stream = tcpClient.GetStream();
|
||||
_lastConnectedAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("StateClient 已连接到 {Ip}:{Port}", ip, port);
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogWarning("StateClient 连接 {Ip}:{Port} 超时", ip, port);
|
||||
tcpClient.Dispose();
|
||||
throw new TimeoutException("状态通道建连超时。");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 连接 {Ip}:{Port} 失败", ip, port);
|
||||
tcpClient.Dispose();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按退避策略循环尝试重新连接,并返回下一次异常后的退避时间。
|
||||
/// </summary>
|
||||
private async Task<TimeSpan> ReconnectWithBackoffAsync(
|
||||
string ip,
|
||||
int port,
|
||||
TimeSpan reconnectDelay,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var nextDelay = reconnectDelay;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"StateClient 将在 {Delay}ms 后尝试重连 {Ip}:{Port}...",
|
||||
nextDelay.TotalMilliseconds,
|
||||
ip,
|
||||
port);
|
||||
|
||||
await Task.Delay(nextDelay, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_reconnectAttemptCount++;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation(
|
||||
"StateClient 重连成功: {Ip}:{Port}, 累计重连次数={Count}",
|
||||
ip,
|
||||
port,
|
||||
_reconnectAttemptCount);
|
||||
return _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex) when (ex is SocketException or IOException or TimeoutException)
|
||||
{
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
_lastErrorMessage = ex.Message;
|
||||
}
|
||||
|
||||
_logger?.LogWarning(
|
||||
ex,
|
||||
"StateClient 重连失败: {Ip}:{Port}, 下次等待={NextDelay}ms",
|
||||
ip,
|
||||
port,
|
||||
nextDelay.TotalMilliseconds * 2);
|
||||
|
||||
nextDelay = IncreaseReconnectDelay(nextDelay);
|
||||
}
|
||||
}
|
||||
|
||||
return nextDelay;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭当前 TCP 连接,不清除最新状态帧,供重连路径保留诊断数据。
|
||||
/// </summary>
|
||||
private void CloseCurrentConnection()
|
||||
{
|
||||
NetworkStream? stream;
|
||||
TcpClient? tcpClient;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream;
|
||||
tcpClient = _tcpClient;
|
||||
_stream = null;
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
stream?.Dispose();
|
||||
tcpClient?.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录接收异常并更新状态通道连接阶段。
|
||||
/// </summary>
|
||||
private void MarkReceiveFailure(FanucStateConnectionState state, string message)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = state;
|
||||
_lastErrorMessage = message;
|
||||
}
|
||||
|
||||
_logger?.LogWarning("StateClient 接收失败: state={State}, message={Message}", state, message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭后台循环和 socket 资源。
|
||||
/// </summary>
|
||||
private void Shutdown(bool clearLatestFrame)
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
CloseCurrentConnection();
|
||||
|
||||
try
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(2));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 后台循环可能因取消而抛出 OperationCanceledException,忽略即可。
|
||||
}
|
||||
|
||||
_receiveTask = null;
|
||||
_receiveCts?.Dispose();
|
||||
_receiveCts = null;
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
_lastConnectedAt = null;
|
||||
_lastErrorMessage = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
if (clearLatestFrame)
|
||||
{
|
||||
_latestFrame = null;
|
||||
_lastFrameAt = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断缓存帧是否已经不能代表当前控制柜状态。
|
||||
/// </summary>
|
||||
private bool IsFrameStaleLocked(DateTimeOffset now)
|
||||
{
|
||||
if (_latestFrame is null)
|
||||
{
|
||||
return _connectionState is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting
|
||||
|| _reconnectAttemptCount > 0
|
||||
|| (_lastConnectedAt.HasValue && now - _lastConnectedAt.Value > _options.FrameTimeout);
|
||||
}
|
||||
|
||||
return _lastFrameAt.HasValue && now - _lastFrameAt.Value > _options.FrameTimeout;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算下一轮重连等待时间。
|
||||
/// </summary>
|
||||
private TimeSpan IncreaseReconnectDelay(TimeSpan currentDelay)
|
||||
{
|
||||
var doubledMilliseconds = Math.Max(currentDelay.TotalMilliseconds * 2.0, _options.ReconnectInitialDelay.TotalMilliseconds);
|
||||
var cappedMilliseconds = Math.Min(doubledMilliseconds, _options.ReconnectMaxDelay.TotalMilliseconds);
|
||||
return TimeSpan.FromMilliseconds(cappedMilliseconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态通道参数,避免后台循环使用无效时间窗口。
|
||||
/// </summary>
|
||||
private static void ValidateOptions(FanucStateClientOptions options)
|
||||
{
|
||||
ValidatePositive(options.FrameTimeout, nameof(options.FrameTimeout));
|
||||
ValidatePositive(options.ReconnectInitialDelay, nameof(options.ReconnectInitialDelay));
|
||||
ValidatePositive(options.ReconnectMaxDelay, nameof(options.ReconnectMaxDelay));
|
||||
ValidatePositive(options.ConnectTimeout, nameof(options.ConnectTimeout));
|
||||
|
||||
if (options.ReconnectMaxDelay < options.ReconnectInitialDelay)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(options), "最大重连等待时间不能小于初始重连等待时间。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验时间参数必须为正值。
|
||||
/// </summary>
|
||||
private static void ValidatePositive(TimeSpan value, string parameterName)
|
||||
{
|
||||
if (value <= TimeSpan.Zero)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(parameterName, "时间参数必须大于 0。");
|
||||
}
|
||||
}
|
||||
}
|
||||
187
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
187
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
@@ -0,0 +1,187 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。
|
||||
/// </summary>
|
||||
public sealed class FanucStateFrame
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _jointOrExtensionValues;
|
||||
private readonly double[] _jointRadians;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly uint[] _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化状态帧解析结果。
|
||||
/// </summary>
|
||||
/// <param name="messageId">状态帧消息号或序号。</param>
|
||||
/// <param name="pose">控制器回传的笛卡尔位姿。</param>
|
||||
/// <param name="jointOrExtensionValues">控制器回传的关节或扩展轴状态。</param>
|
||||
/// <param name="tailWords">状态帧尾部状态槽位。</param>
|
||||
public FanucStateFrame(
|
||||
uint messageId,
|
||||
IEnumerable<double> pose,
|
||||
IEnumerable<double> jointOrExtensionValues,
|
||||
IEnumerable<uint> tailWords)
|
||||
{
|
||||
MessageId = messageId;
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
|
||||
_tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));
|
||||
|
||||
if (_pose.Length != 6)
|
||||
{
|
||||
throw new ArgumentException("状态帧位姿必须包含 6 个 float。", nameof(pose));
|
||||
}
|
||||
|
||||
if (_jointOrExtensionValues.Length != 9)
|
||||
{
|
||||
throw new ArgumentException("状态帧关节/扩展轴必须包含 9 个 float。", nameof(jointOrExtensionValues));
|
||||
}
|
||||
|
||||
if (_tailWords.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
|
||||
}
|
||||
|
||||
_jointRadians = _jointOrExtensionValues.Take(6).ToArray();
|
||||
_externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧消息号或序号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿 X/Y/Z/W/P/R,单位来自 FANUC 状态服务器。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> CartesianPose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的关节或扩展轴状态。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
||||
|
||||
/// <summary>
|
||||
/// 获取前 6 个机器人关节角度,当前现场抓包更支持按弧度制理解。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointRadians => _jointRadians;
|
||||
|
||||
/// <summary>
|
||||
/// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧尾部状态槽位。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> TailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取原始尾部状态字。当前抓包中恒为 [2,0,0,1],语义暂不强行推断。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> RawTailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 0 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord0 => _tailWords[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 1 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord1 => _tailWords[1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 2 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord2 => _tailWords[2];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 3 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord3 => _tailWords[3];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。
|
||||
/// </summary>
|
||||
public static class FanucStateProtocol
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 状态通道抓包确认的完整帧长度。
|
||||
/// </summary>
|
||||
public const int StateFrameLength = 90;
|
||||
|
||||
/// <summary>
|
||||
/// 解析 TCP 10010 状态通道中的单个完整状态帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整状态帧。</param>
|
||||
/// <returns>状态帧解析结果。</returns>
|
||||
public static FanucStateFrame ParseFrame(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
ValidateFrame(frame);
|
||||
|
||||
var pose = new double[6];
|
||||
var jointOrExtensionValues = new double[9];
|
||||
var tailWords = new uint[4];
|
||||
|
||||
// 状态帧采用固定布局,偏移来自抓包与 StateServer 逆向结论。
|
||||
for (var index = 0; index < pose.Length; index++)
|
||||
{
|
||||
pose[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(11 + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
for (var index = 0; index < jointOrExtensionValues.Length; index++)
|
||||
{
|
||||
jointOrExtensionValues[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(35 + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
for (var index = 0; index < tailWords.Length; index++)
|
||||
{
|
||||
tailWords[index] = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(71 + (index * sizeof(uint)), sizeof(uint)));
|
||||
}
|
||||
|
||||
return new FanucStateFrame(
|
||||
BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint))),
|
||||
pose,
|
||||
jointOrExtensionValues,
|
||||
tailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态帧的长度、magic 和长度字段。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整状态帧。</param>
|
||||
private static void ValidateFrame(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
if (frame.Length != StateFrameLength)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧长度不符合 TCP 10010 固定帧布局。");
|
||||
}
|
||||
|
||||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||
if (declaredLength != frame.Length)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧长度字段与实际长度不一致。");
|
||||
}
|
||||
}
|
||||
}
|
||||
13
src/Flyshot.Server.Host/.config/dotnet-tools.json
Normal file
13
src/Flyshot.Server.Host/.config/dotnet-tools.json
Normal file
@@ -0,0 +1,13 @@
|
||||
{
|
||||
"version": 1,
|
||||
"isRoot": true,
|
||||
"tools": {
|
||||
"dotnet-ef": {
|
||||
"version": "10.0.7",
|
||||
"commands": [
|
||||
"dotnet-ef"
|
||||
],
|
||||
"rollForward": false
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
using Flyshot.Server.Host;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
using Microsoft.Extensions.Options;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供浏览器调试页所需的运行时配置 API。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 本控制器自身不进入 Swagger 文档(<see cref="ApiExplorerSettingsAttribute.IgnoreApi"/>)。
|
||||
/// 调试页静态资源位于 wwwroot,Swagger 地址由配置 API 下发。
|
||||
/// </remarks>
|
||||
[ApiController]
|
||||
[ApiExplorerSettings(IgnoreApi = true)]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class DebugConsoleController : ControllerBase
|
||||
{
|
||||
/// <summary>
|
||||
/// Swagger 配置项,用于决定调试页是否对外暴露以及拼接 OpenAPI JSON 地址。
|
||||
/// </summary>
|
||||
private readonly HostSwaggerOptions _swaggerOptions;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化在线调试页控制器。
|
||||
/// </summary>
|
||||
/// <param name="swaggerOptions">来自 <c>Swagger</c> 配置节的标准选项。</param>
|
||||
public DebugConsoleController(IOptions<HostSwaggerOptions> swaggerOptions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(swaggerOptions);
|
||||
_swaggerOptions = swaggerOptions.Value ?? new HostSwaggerOptions();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回静态调试页启动时所需的 Swagger 文档地址。
|
||||
/// </summary>
|
||||
/// <returns>当 Swagger 启用时返回配置;否则返回 404,与 Swagger UI 保持一致的可见性策略。</returns>
|
||||
[HttpGet("/api/debug/config")]
|
||||
public IActionResult GetDebugConfig()
|
||||
{
|
||||
if (!_swaggerOptions.Enabled)
|
||||
{
|
||||
return NotFound();
|
||||
}
|
||||
|
||||
return Ok(new
|
||||
{
|
||||
SwaggerJsonUrl = ResolveSwaggerJsonUrl(_swaggerOptions)
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 <see cref="HostSwaggerOptions.JsonRouteTemplate"/> 与 <see cref="HostSwaggerOptions.DocumentName"/> 解析出 Swagger JSON 实际地址。
|
||||
/// </summary>
|
||||
/// <param name="options">Swagger 配置选项。</param>
|
||||
/// <returns>形如 <c>/swagger/v1/swagger.json</c> 的绝对路径。</returns>
|
||||
private static string ResolveSwaggerJsonUrl(HostSwaggerOptions options)
|
||||
{
|
||||
// Swashbuckle 的 RouteTemplate 不带前导斜杠,这里统一加上保证前端 fetch 走绝对路径。
|
||||
var template = options.JsonRouteTemplate ?? "swagger/{documentName}/swagger.json";
|
||||
var path = template.Replace("{documentName}", options.DocumentName ?? "v1", StringComparison.Ordinal);
|
||||
return path.StartsWith('/') ? path : "/" + path;
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user