Compare commits
5 Commits
af65ca03a0
...
milestone/
| Author | SHA1 | Date | |
|---|---|---|---|
| 1779067b5c | |||
| c6829d214a | |||
| 70b0ccd414 | |||
| 783716ff44 | |||
| b1710e5d01 |
@@ -11,7 +11,32 @@
|
|||||||
"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(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 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.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 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')"
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
3
.gitignore
vendored
3
.gitignore
vendored
@@ -397,3 +397,6 @@ FodyWeavers.xsd
|
|||||||
# JetBrains Rider
|
# JetBrains Rider
|
||||||
*.sln.iml
|
*.sln.iml
|
||||||
Config/Data/*
|
Config/Data/*
|
||||||
|
.dotnet-home/*
|
||||||
|
codex-dotnet-home/*
|
||||||
|
.dotnet-sdk8/*
|
||||||
|
|||||||
11
AGENTS.md
11
AGENTS.md
@@ -112,6 +112,15 @@ flyshot-replacement/
|
|||||||
- 所有静态变量都必须提供 XML 注释。
|
- 所有静态变量都必须提供 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. 构建与验证命令
|
## 5. 构建与验证命令
|
||||||
|
|
||||||
在当前环境中，推荐使用下面两条命令：
|
在当前环境中，推荐使用下面两条命令：
|
||||||
@@ -178,7 +187,7 @@ flyshot-replacement/
|
|||||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||||
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
|
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
|
||||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
||||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
- `Rvbust/uttc-20260428` 抓包确认 `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` 对应约两周期清零。
|
- `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 闭环状态判断与现场联调仍需补齐。
|
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，J519 闭环状态判断与现场联调仍需补齐。
|
||||||
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
|
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
|
||||||
|
|||||||
@@ -153,5 +153,5 @@ flyshot-replacement/
|
|||||||
- 解决方案构建已通过。
|
- 解决方案构建已通过。
|
||||||
- `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
- `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
||||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
- `Rvbust/uttc-20260428` 抓包确认 `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` 对应约两周期清零。
|
- `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.
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"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -1 +1,303 @@
|
|||||||
{"robot": {"use_do": true, "io_addr": [7, 8], "io_keep_cycles": 2, "acc_limit": 1, "jerk_limit": 1, "adapt_icsp_try_num": 5}, "flying_shots": {"20251015": {"traj_waypoints": [[1.047438621520996, -0.0002488955215085298, -0.0014060207176953554, 0.009022523649036884, 0.010111905634403229, 0.009573347866535187], [0.7661270499229431, -0.04437164217233658, -0.13630111515522003, -0.41718506813049316, 0.010093353688716888, 0.009594489820301533], [0.7661266326904297, 0.2170650213956833, -0.13630135357379913, -0.4171852171421051, 0.010093353688716888, 0.009594779461622238], [1.0311520099639893, -0.062108494341373444, -0.1363297700881958, 0.30276036262512207, 0.15847623348236084, 0.00956842489540577], [1.4012629985809326, -0.05120057240128517, -0.13633012771606445, 0.3027600347995758, 0.15847666561603546, 0.00956842489540577], [1.0567246675491333, 0.01165649201720953, -0.01786380261182785, -0.015170873142778873, 0.02149667963385582, 0.009576244279742241]], "shot_flags": [false, true, true, true, true, true], "offset_values": [0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4], [2, 4], [3, 4], [2, 4]]}, "TEST20251214": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8067416, 0.011661344, -0.11788314, -0.01516874, 0.021492079, 0.009567846], [0.60675246, -0.03833516, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.14043556], [1.1667324, 0.05164983, -0.11789217, 0.23482007, 0.021492079, -0.14043556], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0], "addr": [[], [], [3, 4, 2], [3, 4, 2], [3, 4, 2], [3, 4, 2], []]}, "UTTC_MS11": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145], [0.96600056, 0.20607172, -0.12233179, -1.2394339, 0.10493033, 1.2958988], [0.9618476, 0.15288207, -0.14867093, -0.7176314, 0.1764264, 0.73228663], [0.76189893, -0.028442925, -0.30919823, 0.10463613, 0.5615024, -0.39399016], [1.1271763, 0.074403025, -0.27347943, -0.5227772, 0.52098846, 0.79633313], [1.0555661, 0.4026262, -0.08746306, 0.6301835, 0.09644133, -0.5463328], [1.2300354, 0.28612664, -0.23486805, -0.4868128, 0.25369516, 0.55347764], [1.2144431, -0.29855102, -0.15202847, -1.0205934, 0.13317892, 1.1246506], [1.2840607, -0.11222197, -0.16805042, -2.248135, 0.2560587, 2.4434967], [1.3189346, -0.25620222, -0.12730704, -2.285038, 0.30872014, 2.4765089], [1.502615, -0.25304365, -0.23878741, -1.2194318, 0.46674785, 1.5533328], [1.07723, -0.07387611, -0.1707704, -1.8916591, 0.38677844, 2.061968], [1.3920237, 0.08098731, -0.2672306, -0.9780007, 0.4561093, 0.9102286], [1.9016331, 0.023924276, -0.58633333, -0.8441697, 0.76730615, 1.4842151], [1.9300697, -0.06738541, -0.56542397, -0.892083, 0.77194446, 1.5293273], [2.0611632, -0.30327517, -0.54225636, -1.0395275, 0.8505439, 1.6429617], [1.0921186, -0.40034482, -0.1803499, 1.3524796, 0.6210477, -1.2159473], [1.0521278, -0.40034503, -0.1803492, 1.3524843, 0.6210471, -1.2159531], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, false, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4, 2], [3, 4, 2], [4, 2], [4, 2], [3, 4], [3, 4], [4, 2], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], []]}, "5U": {"traj_waypoints": [[-0.95982397, 0.6331447, -1.0055008, 0.79858834, 1.1564041, -0.4260437], [-0.98353565, 0.66203266, -0.9758351, 0.8320198, 1.1455917, -0.45941326]], "shot_flags": [false, false], "offset_values": [0, 0], "addr": [[], []]}}}
|
{
|
||||||
|
"robot": {
|
||||||
|
"use_do": true,
|
||||||
|
"io_addr": [
|
||||||
|
7,
|
||||||
|
8
|
||||||
|
],
|
||||||
|
"io_keep_cycles": 2,
|
||||||
|
"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
|
||||||
|
],
|
||||||
|
[]
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -103,7 +103,7 @@
|
|||||||
|
|
||||||
5. FANUC UDP 60015 J519 运动链路
|
5. FANUC UDP 60015 J519 运动链路
|
||||||
- [x] 重新确认 J519 发送节拍与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 收到机器人 UDP status 后按该 status sequence 回发命令，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令内容。
|
- [x] 重新确认 J519 发送节拍与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 收到机器人 UDP status 后按该 status sequence 回发命令，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令内容。
|
||||||
- [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整 8ms 发送索引/时间尺度：第 `k` 个 J519 目标采样 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
- [x] 执行时将规划输出 `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] 补齐 `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] 校验序号递增、状态包 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] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐；UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧，mask 集合为 `10/12/14`。
|
||||||
|
|||||||
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
|
||||||
|
@@ -137,7 +137,7 @@
|
|||||||
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
||||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
|
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
|
||||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。
|
- `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 行导出点，而是按 `floor(duration / (0.008 * speed_ratio)) + 1` 形成 J519 运行包。
|
- 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`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
||||||
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||||
|
|||||||
@@ -515,7 +515,7 @@ UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)
|
|||||||
- `SetSpeedRatio`：`MsgID = 0x2207`
|
- `SetSpeedRatio`：`MsgID = 0x2207`
|
||||||
- `GetIO`：`MsgID = 0x2208`
|
- `GetIO`：`MsgID = 0x2208`
|
||||||
- `SetIO`：`MsgID = 0x2209`
|
- `SetIO`：`MsgID = 0x2209`
|
||||||
- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。实发规则为 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
- 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`
|
- `StartUploadFlyShotTraj`
|
||||||
- `EndUploadFlyShotTraj`
|
- `EndUploadFlyShotTraj`
|
||||||
|
|||||||
@@ -53,10 +53,11 @@ POST /set_speedRatio/
|
|||||||
{ "speed": 0.7 }
|
{ "speed": 0.7 }
|
||||||
```
|
```
|
||||||
|
|
||||||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍必须按该倍率重采样轨迹时间轴：
|
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍按机器人 8ms 节拍更新目标，`speed_ratio` 只缩放原轨迹采样时间：
|
||||||
|
|
||||||
```text
|
```text
|
||||||
t_traj = k * 0.008 * speed_ratio
|
t_send = k * 0.008
|
||||||
|
t_traj = t_send * speed_ratio
|
||||||
send_count = floor(duration / (0.008 * speed_ratio)) + 1
|
send_count = floor(duration / (0.008 * speed_ratio)) + 1
|
||||||
```
|
```
|
||||||
|
|
||||||
|
|||||||
@@ -120,8 +120,8 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
|||||||
执行注意事项：
|
执行注意事项：
|
||||||
|
|
||||||
- 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。
|
- 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。
|
||||||
- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要按当前 `speed_ratio` 对轨迹时间轴做缩放，再采样到约 8ms 的 J519 周期。
|
- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要保持约 8ms 的 J519 实发周期，同时按当前 `speed_ratio` 对原轨迹时间轴做缩放。
|
||||||
- 实发规则：第 `k` 个 J519 周期采样 `t_traj = k * 0.008 * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。
|
- 实发规则：第 `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` 模拟拍照触发。
|
- 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
|
||||||
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。
|
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。
|
||||||
|
|
||||||
@@ -139,7 +139,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
|||||||
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
|
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
|
||||||
- `DisableRobot()` — 停止 J519，发送 StopProg
|
- `DisableRobot()` — 停止 J519，发送 StopProg
|
||||||
- `Disconnect()` — 断开三条通道
|
- `Disconnect()` — 断开三条通道
|
||||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 J519 逐周期发送
|
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_send = k * 0.008`、`t_traj = t_send * speed_ratio` 重采样后，通过 J519 逐周期发送
|
||||||
- `StopMove()` — 立即停止 J519 发送循环
|
- `StopMove()` — 立即停止 J519 发送循环
|
||||||
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
|
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
|
||||||
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
||||||
|
|||||||
@@ -136,7 +136,7 @@
|
|||||||
| --- | --- |
|
| --- | --- |
|
||||||
| 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 |
|
| 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 |
|
||||||
| `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 |
|
| `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 |
|
||||||
| `speed_ratio` 下发时间轴缩放 | 已实现，规则为 `t_traj = k * 0.008 * speed_ratio` |
|
| `speed_ratio` 下发时间轴缩放 | 已实现，J519 实发时间为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio` |
|
||||||
| IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` |
|
| IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` |
|
||||||
| 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 |
|
| 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 |
|
||||||
| J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 |
|
| J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 |
|
||||||
|
|||||||
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 约束失配来处理
|
||||||
|
- 暂时没有证据支持“六轴普遍一起逼近限制”或“网络链路导致跃度统计失真”这类解释
|
||||||
@@ -576,7 +576,8 @@ eol9_eau_90.json SHA256=7F854AA227D842CAE734AFA378FEEFA742D797F99FBE53
|
|||||||
它不等同于运行时 `/set_speedRatio/`，也不改变 J519 的 8ms 发送周期。运行阶段仍按：
|
它不等同于运行时 `/set_speedRatio/`，也不改变 J519 的 8ms 发送周期。运行阶段仍按：
|
||||||
|
|
||||||
```text
|
```text
|
||||||
t_traj = k * 0.008 * speed_ratio
|
t_send = k * 0.008
|
||||||
|
t_traj = t_send * speed_ratio
|
||||||
```
|
```
|
||||||
|
|
||||||
从已生成轨迹中重采样。
|
从已生成轨迹中重采样。
|
||||||
|
|||||||
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` 去推断
|
||||||
6
global.json
Normal file
6
global.json
Normal file
@@ -0,0 +1,6 @@
|
|||||||
|
{
|
||||||
|
"sdk": {
|
||||||
|
"version": "8.0.420",
|
||||||
|
"rollForward": "disable"
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -9,12 +9,12 @@ namespace Flyshot.ControllerClientCompat;
|
|||||||
public sealed class ControllerClientCompatRobotCatalog
|
public sealed class ControllerClientCompatRobotCatalog
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 保存当前现场支持的机器人名称到运行目录模型文件名映射。
|
/// 保存当前现场支持的机器人名称到运行目录 JSON 模型文件名映射。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private static readonly IReadOnlyDictionary<string, string> SupportedRobotModelFileMap = new Dictionary<string, string>(StringComparer.Ordinal)
|
private static readonly IReadOnlyDictionary<string, string> SupportedRobotModelFileMap = new Dictionary<string, string>(StringComparer.Ordinal)
|
||||||
{
|
{
|
||||||
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.robot",
|
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.json",
|
||||||
["FANUC_LR_Mate_200iD_7L"] = "LR_Mate_200iD_7L.robot"
|
["FANUC_LR_Mate_200iD_7L"] = "LR_Mate_200iD_7L.json"
|
||||||
};
|
};
|
||||||
|
|
||||||
private readonly ControllerClientCompatOptions _options;
|
private readonly ControllerClientCompatOptions _options;
|
||||||
@@ -24,7 +24,7 @@ public sealed class ControllerClientCompatRobotCatalog
|
|||||||
/// 初始化机器人兼容目录解析器。
|
/// 初始化机器人兼容目录解析器。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="options">兼容层基础配置。</param>
|
/// <param name="options">兼容层基础配置。</param>
|
||||||
/// <param name="robotModelLoader">.robot 文件加载器。</param>
|
/// <param name="robotModelLoader">机器人 JSON 模型加载器。</param>
|
||||||
public ControllerClientCompatRobotCatalog(
|
public ControllerClientCompatRobotCatalog(
|
||||||
ControllerClientCompatOptions options,
|
ControllerClientCompatOptions options,
|
||||||
RobotModelLoader robotModelLoader)
|
RobotModelLoader robotModelLoader)
|
||||||
@@ -57,10 +57,10 @@ public sealed class ControllerClientCompatRobotCatalog
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 解析机器人模型路径，运行目录 Config/Models 优先，旧父工作区只作为显式兼容入口。
|
/// 解析机器人模型路径，只从运行目录 Config/Models 读取当前现场固化的 JSON 模型。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="modelFileName">运行目录 Models 下的机器人模型文件名。</param>
|
/// <param name="modelFileName">运行目录 Config/Models 下的机器人 JSON 模型文件名。</param>
|
||||||
/// <returns>可传给 .robot 加载器的模型文件绝对路径。</returns>
|
/// <returns>可传给 JSON 模型加载器的模型文件绝对路径。</returns>
|
||||||
private string ResolveModelPath(string modelFileName)
|
private string ResolveModelPath(string modelFileName)
|
||||||
{
|
{
|
||||||
var configModelPath = Path.Combine(_options.ResolveConfigRoot(), "Models", modelFileName);
|
var configModelPath = Path.Combine(_options.ResolveConfigRoot(), "Models", modelFileName);
|
||||||
@@ -69,12 +69,6 @@ public sealed class ControllerClientCompatRobotCatalog
|
|||||||
return configModelPath;
|
return configModelPath;
|
||||||
}
|
}
|
||||||
|
|
||||||
var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
|
return "Not found";
|
||||||
if (legacyWorkspaceRoot is not null)
|
|
||||||
{
|
|
||||||
return Path.Combine(legacyWorkspaceRoot, "FlyingShot", "FlyingShot", "Models", modelFileName);
|
|
||||||
}
|
|
||||||
|
|
||||||
return configModelPath;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -606,14 +606,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||||
robot,
|
robot,
|
||||||
trajectory,
|
trajectory,
|
||||||
new FlyshotExecutionOptions(saveTrajectory: true, method: method),
|
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
|
||||||
planningSettings,
|
planningSettings,
|
||||||
planningSettings.PlanningSpeedScale);
|
planningSettings.PlanningSpeedScale);
|
||||||
|
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
|
||||||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
|
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
|
||||||
|
|
||||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
// _trajectoryStore.Save(robotName, settings, trajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
|
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
|
||||||
@@ -777,7 +778,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle);
|
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||||
|
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|||||||
@@ -12,6 +12,16 @@ namespace Flyshot.ControllerClientCompat;
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class ControllerClientTrajectoryOrchestrator
|
public sealed class ControllerClientTrajectoryOrchestrator
|
||||||
{
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。
|
||||||
|
/// </summary>
|
||||||
|
private const int MaxDenseLimitStretchIterations = 100;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 每次离散限幅失败后统一放大的时间倍率。
|
||||||
|
/// </summary>
|
||||||
|
private const double DenseLimitStretchFactor = 1.01;
|
||||||
|
|
||||||
private readonly ICspPlanner _icspPlanner;
|
private readonly ICspPlanner _icspPlanner;
|
||||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
|
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
|
||||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||||
@@ -22,11 +32,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
/// 初始化轨迹编排器。
|
/// 初始化轨迹编排器。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="logger">日志记录器；允许 null。</param>
|
/// <param name="logger">日志记录器；允许 null。</param>
|
||||||
public ControllerClientTrajectoryOrchestrator(ILogger<ControllerClientTrajectoryOrchestrator>? logger = null)
|
/// <param name="loggerFactory">日志工厂；允许 null。</param>
|
||||||
|
public ControllerClientTrajectoryOrchestrator(
|
||||||
|
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
|
||||||
|
ILoggerFactory? loggerFactory = null)
|
||||||
{
|
{
|
||||||
_logger = logger;
|
_logger = logger;
|
||||||
_icspPlanner = new(logger: null);
|
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>());
|
||||||
_selfAdaptIcspPlanner = new(logger: null);
|
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -65,8 +78,10 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
saveTrajectoryArtifacts: options.SaveTrajectory);
|
saveTrajectoryArtifacts: options.SaveTrajectory);
|
||||||
|
|
||||||
var plannedTrajectory = PlanByMethod(request, method);
|
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 shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
||||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
var result = CreateResult(executionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
|
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
|
||||||
@@ -97,8 +112,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}",
|
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
|
||||||
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale);
|
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale, settings.SmoothStartStopTiming);
|
||||||
|
|
||||||
var program = CreateProgram(
|
var program = CreateProgram(
|
||||||
name: uploaded.Name,
|
name: uploaded.Name,
|
||||||
@@ -109,15 +124,21 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
|
|
||||||
var method = ParseFlyshotMethod(options.Method);
|
var method = ParseFlyshotMethod(options.Method);
|
||||||
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
|
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
|
||||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
//if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||||
{
|
//{
|
||||||
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
|
// _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
|
||||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
// var executionTrajectory = ApplyExecutionTiming(cachedBundle.PlannedTrajectory, settings);
|
||||||
return new PlannedExecutionBundle(
|
// var executionTimeline = _shotTimelineBuilder.Build(
|
||||||
cachedBundle.PlannedTrajectory,
|
// executionTrajectory,
|
||||||
cachedBundle.ShotTimeline,
|
// holdCycles: settings.IoKeepCycles,
|
||||||
CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
|
// samplePeriod: planningRobot.ServoPeriod,
|
||||||
}
|
// useDo: settings.UseDo);
|
||||||
|
// // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
||||||
|
// return new PlannedExecutionBundle(
|
||||||
|
// cachedBundle.PlannedTrajectory,
|
||||||
|
// executionTimeline,
|
||||||
|
// CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false));
|
||||||
|
//}
|
||||||
|
|
||||||
var request = new TrajectoryRequest(
|
var request = new TrajectoryRequest(
|
||||||
robot: planningRobot,
|
robot: planningRobot,
|
||||||
@@ -128,12 +149,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
useCache: options.UseCache);
|
useCache: options.UseCache);
|
||||||
|
|
||||||
var plannedTrajectory = PlanByMethod(request, method, settings);
|
var plannedTrajectory = PlanByMethod(request, method, settings);
|
||||||
|
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
|
||||||
|
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
|
||||||
var shotTimeline = _shotTimelineBuilder.Build(
|
var shotTimeline = _shotTimelineBuilder.Build(
|
||||||
plannedTrajectory,
|
smoothedExecutionTrajectory,
|
||||||
holdCycles: settings.IoKeepCycles,
|
holdCycles: settings.IoKeepCycles,
|
||||||
samplePeriod: planningRobot.ServoPeriod,
|
samplePeriod: planningRobot.ServoPeriod,
|
||||||
useDo: settings.UseDo);
|
useDo: settings.UseDo);
|
||||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
|
||||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
@@ -237,6 +260,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
hash.Add(settings.UseDo);
|
hash.Add(settings.UseDo);
|
||||||
hash.Add(settings.IoKeepCycles);
|
hash.Add(settings.IoKeepCycles);
|
||||||
hash.Add(settings.AdaptIcspTryNum);
|
hash.Add(settings.AdaptIcspTryNum);
|
||||||
|
hash.Add(settings.SmoothStartStopTiming);
|
||||||
|
|
||||||
foreach (var limit in robot.JointLimits)
|
foreach (var limit in robot.JointLimits)
|
||||||
{
|
{
|
||||||
@@ -290,6 +314,20 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
adaptIcspTryNum: 5);
|
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>
|
||||||
/// 按规划全局速度倍率生成规划专用机器人约束。
|
/// 按规划全局速度倍率生成规划专用机器人约束。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -357,12 +395,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||||
/// <param name="shotTimeline">触发时间轴。</param>
|
/// <param name="shotTimeline">触发时间轴。</param>
|
||||||
/// <returns>运行时执行结果描述。</returns>
|
/// <returns>运行时执行结果描述。</returns>
|
||||||
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
|
private static TrajectoryResult CreateResult(
|
||||||
|
PlannedTrajectory plannedTrajectory,
|
||||||
|
ShotTimeline shotTimeline,
|
||||||
|
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||||
|
bool usedCache)
|
||||||
{
|
{
|
||||||
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
|
|
||||||
plannedTrajectory,
|
|
||||||
samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds);
|
|
||||||
|
|
||||||
return new TrajectoryResult(
|
return new TrajectoryResult(
|
||||||
programName: plannedTrajectory.OriginalProgram.Name,
|
programName: plannedTrajectory.OriginalProgram.Name,
|
||||||
method: plannedTrajectory.Method,
|
method: plannedTrajectory.Method,
|
||||||
@@ -377,4 +415,149 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
||||||
denseJointTrajectory: denseJointTrajectory);
|
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>
|
||||||
|
/// 为飞拍执行生成一条平滑起停的时间轴。
|
||||||
|
/// 保持路点位置不变，只重映射路点时刻，让起点和终点附近的速度自然收敛。
|
||||||
|
/// </summary>
|
||||||
|
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];
|
||||||
|
smoothedTimes[0] = 0.0;
|
||||||
|
smoothedTimes[^1] = totalDuration;
|
||||||
|
|
||||||
|
for (var index = 1; index < originalTimes.Count - 1; index++)
|
||||||
|
{
|
||||||
|
var normalizedProgress = originalTimes[index] / totalDuration;
|
||||||
|
smoothedTimes[index] = totalDuration * InvertSmoothStartStopProgress(normalizedProgress);
|
||||||
|
}
|
||||||
|
|
||||||
|
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>
|
||||||
|
/// 反解 7 次 smootherstep 的时间进度，用二分法把原始线性进度映射成平滑时间轴。
|
||||||
|
/// </summary>
|
||||||
|
private static double InvertSmoothStartStopProgress(double normalizedProgress)
|
||||||
|
{
|
||||||
|
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
|
||||||
|
var low = 0.0;
|
||||||
|
var high = 1.0;
|
||||||
|
for (var iteration = 0; iteration < 40; iteration++)
|
||||||
|
{
|
||||||
|
var middle = (low + high) / 2.0;
|
||||||
|
var progress = EvaluateSmoothStartStopProgress(middle);
|
||||||
|
if (progress < target)
|
||||||
|
{
|
||||||
|
low = middle;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
high = middle;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return (low + high) / 2.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 计算 7 次 smootherstep 进度值，用于整段平滑起停时间律。
|
||||||
|
/// </summary>
|
||||||
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -5,6 +5,7 @@ using Flyshot.Core.Planning.Export;
|
|||||||
using Flyshot.Core.Planning.Kinematics;
|
using Flyshot.Core.Planning.Kinematics;
|
||||||
using Flyshot.Core.Planning.Sampling;
|
using Flyshot.Core.Planning.Sampling;
|
||||||
using Microsoft.Extensions.Logging;
|
using Microsoft.Extensions.Logging;
|
||||||
|
using System.Text;
|
||||||
|
|
||||||
namespace Flyshot.ControllerClientCompat;
|
namespace Flyshot.ControllerClientCompat;
|
||||||
|
|
||||||
@@ -18,6 +19,11 @@ public sealed class FlyshotTrajectoryArtifactWriter
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
private const double LegacyDetailSamplePeriodSeconds = 0.016;
|
private const double LegacyDetailSamplePeriodSeconds = 0.016;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// FANUC J519 实际下发的固定伺服周期，单位为秒。
|
||||||
|
/// </summary>
|
||||||
|
private const double ActualSendServoPeriodSeconds = 0.008;
|
||||||
|
|
||||||
private readonly ControllerClientCompatOptions _options;
|
private readonly ControllerClientCompatOptions _options;
|
||||||
private readonly RobotModelLoader _robotModelLoader;
|
private readonly RobotModelLoader _robotModelLoader;
|
||||||
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
|
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
|
||||||
@@ -44,7 +50,8 @@ public sealed class FlyshotTrajectoryArtifactWriter
|
|||||||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||||||
/// <param name="robot">当前机器人配置。</param>
|
/// <param name="robot">当前机器人配置。</param>
|
||||||
/// <param name="bundle">规划结果包。</param>
|
/// <param name="bundle">规划结果包。</param>
|
||||||
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle)
|
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
|
||||||
|
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
|
||||||
{
|
{
|
||||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||||
{
|
{
|
||||||
@@ -56,31 +63,135 @@ public sealed class FlyshotTrajectoryArtifactWriter
|
|||||||
|
|
||||||
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
||||||
Directory.CreateDirectory(outputDir);
|
Directory.CreateDirectory(outputDir);
|
||||||
|
if (bundle.Result.DenseJointTrajectory is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。");
|
||||||
|
}
|
||||||
|
|
||||||
// 明细文件对齐旧 Data 目录的 16ms 采样；运行时 J519 仍可使用自己的 8ms 伺服采样。
|
// 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”，避免再次从 PlannedTrajectory 生成另一条轨迹。
|
||||||
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
||||||
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
|
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
|
||||||
var jointDetailTrajectory = TrajectorySampler.SampleJointTrajectory(
|
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
|
||||||
bundle.PlannedTrajectory,
|
var executionDenseTrajectory = bundle.Result.DenseJointTrajectory;
|
||||||
samplePeriod: LegacyDetailSamplePeriodSeconds);
|
var jointDetailTrajectory = DownsampleDenseRows(
|
||||||
|
executionDenseTrajectory,
|
||||||
|
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
|
||||||
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
|
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
|
||||||
var cartDetailTrajectory = TrajectorySampler.SampleCartesianTrajectory(
|
var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);
|
||||||
bundle.PlannedTrajectory,
|
|
||||||
kinematicsModel,
|
|
||||||
samplePeriod: LegacyDetailSamplePeriodSeconds);
|
|
||||||
|
|
||||||
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
|
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
|
||||||
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
|
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
|
||||||
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
|
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
|
||||||
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
|
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
|
||||||
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents);
|
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents);
|
||||||
|
WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio);
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}",
|
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
|
||||||
trajectoryName,
|
trajectoryName,
|
||||||
outputDir,
|
outputDir,
|
||||||
jointTrajectory.Count,
|
jointTrajectory.Count,
|
||||||
jointDetailTrajectory.Count);
|
jointDetailTrajectory.Count,
|
||||||
|
speedRatio);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。
|
||||||
|
/// </summary>
|
||||||
|
private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(robot);
|
||||||
|
|
||||||
|
if (result.DenseJointTrajectory is null)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||||||
|
}
|
||||||
|
|
||||||
|
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||||
|
result.DenseJointTrajectory,
|
||||||
|
result.Duration.TotalSeconds,
|
||||||
|
ActualSendServoPeriodSeconds,
|
||||||
|
speedRatio);
|
||||||
|
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||||
|
robot,
|
||||||
|
samples,
|
||||||
|
trajectoryName: result.ProgramName);
|
||||||
|
}
|
||||||
|
catch (Exception e)
|
||||||
|
{
|
||||||
|
_logger?.LogError(e, "ValidateJ519SendSamples 失败，program={ProgramName}", result.ProgramName);
|
||||||
|
}
|
||||||
|
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
|
||||||
|
var timingRows = new List<IReadOnlyList<double>>(samples.Count);
|
||||||
|
var jerkRows = new List<IReadOnlyList<double>>();
|
||||||
|
double? previousSendTime = null;
|
||||||
|
double[]? previousJoints = null;
|
||||||
|
double[]? previousVelocity = null;
|
||||||
|
double[]? previousAcceleration = null;
|
||||||
|
|
||||||
|
foreach (var sample in samples)
|
||||||
|
{
|
||||||
|
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
|
||||||
|
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
|
||||||
|
|
||||||
|
if (previousSendTime is not null && previousJoints is not null)
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <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>
|
/// <summary>
|
||||||
@@ -124,6 +235,63 @@ public sealed class FlyshotTrajectoryArtifactWriter
|
|||||||
return rows;
|
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>
|
/// <summary>
|
||||||
/// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。
|
/// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -195,7 +195,9 @@ public sealed class JsonFlyshotTrajectoryStore
|
|||||||
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
|
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
|
||||||
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
||||||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
||||||
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum)
|
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
|
||||||
|
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
|
||||||
|
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -3,59 +3,66 @@ using Microsoft.Extensions.Logging;
|
|||||||
|
|
||||||
namespace Flyshot.ControllerClientCompat;
|
namespace Flyshot.ControllerClientCompat;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// MoveJoint 轨迹生成器。
|
||||||
|
/// 将起始关节角到目标关节角的单段运动，按速度、加速度、jerk 约束生成稠密点到点轨迹，
|
||||||
|
/// 供 FANUC J519 伺服流逐周期下发。
|
||||||
|
///
|
||||||
|
/// 核心思路：路径只取关节空间直线 q=q0+(q1-q0)*s(t)，时间律使用 7 阶平滑函数；
|
||||||
|
/// 生成后再按离散采样点反算速度、加速度和 jerk，确保真实下发点也满足限制。
|
||||||
|
/// </summary>
|
||||||
internal static class MoveJointTrajectoryGenerator
|
internal static class MoveJointTrajectoryGenerator
|
||||||
{
|
{
|
||||||
private const double BaseMoveJointDurationSeconds = 0.320;
|
/// <summary>
|
||||||
private const double VelocityShapeCoefficient = 2.0759961613199973;
|
/// 最小段基础时长（秒）。零位移时仍由采样对齐逻辑生成起点和终点两帧。
|
||||||
private const double AccelerationShapeCoefficient = 7.986313199999984;
|
/// </summary>
|
||||||
private const double JerkShapeCoefficient = 36.12609273600853;
|
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;
|
private const int MaxMoveJointSampleCount = 1_000_000;
|
||||||
|
|
||||||
private static readonly double[] CapturedMvpointAlpha =
|
/// <summary>
|
||||||
[
|
/// 离散限位校验允许的浮点容差。
|
||||||
0.000000000000,
|
/// </summary>
|
||||||
0.000012196163,
|
private const double DiscreteLimitTolerance = 1.000001;
|
||||||
0.000106156906,
|
|
||||||
0.000764380061,
|
|
||||||
0.002550804028,
|
|
||||||
0.006029689194,
|
|
||||||
0.011765134027,
|
|
||||||
0.020321400844,
|
|
||||||
0.032262426551,
|
|
||||||
0.048152469303,
|
|
||||||
0.068555498563,
|
|
||||||
0.093895155669,
|
|
||||||
0.124210027377,
|
|
||||||
0.159174512929,
|
|
||||||
0.198230386318,
|
|
||||||
0.240813559900,
|
|
||||||
0.286359937276,
|
|
||||||
0.334305411725,
|
|
||||||
0.384085546646,
|
|
||||||
0.435136609163,
|
|
||||||
0.486894129077,
|
|
||||||
0.538794033110,
|
|
||||||
0.590272360135,
|
|
||||||
0.640764719629,
|
|
||||||
0.689707151220,
|
|
||||||
0.736535405849,
|
|
||||||
0.780685354316,
|
|
||||||
0.821592775628,
|
|
||||||
0.858693734065,
|
|
||||||
0.891423926949,
|
|
||||||
0.919286047395,
|
|
||||||
0.942156722091,
|
|
||||||
0.960255163676,
|
|
||||||
0.974119666692,
|
|
||||||
0.984314536393,
|
|
||||||
0.991403790959,
|
|
||||||
0.995951593494,
|
|
||||||
0.998522142663,
|
|
||||||
0.999679443354,
|
|
||||||
0.999987892657,
|
|
||||||
1.000000000000
|
|
||||||
];
|
|
||||||
|
|
||||||
|
/// <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(
|
public static TrajectoryResult CreateResult(
|
||||||
RobotProfile robot,
|
RobotProfile robot,
|
||||||
IReadOnlyList<double> startJoints,
|
IReadOnlyList<double> startJoints,
|
||||||
@@ -82,9 +89,28 @@ internal static class MoveJointTrajectoryGenerator
|
|||||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||||
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, 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(
|
logger?.LogDebug(
|
||||||
"MoveJointTrajectoryGenerator: 请求时长={RequestedDuration:F4}s, 对齐后时长={Duration:F4}s, speedRatio={SpeedRatio}, 采样周期={SamplePeriod:F6}s, 采样数={SampleCount}",
|
"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);
|
requestedDurationSeconds,
|
||||||
|
durationSeconds,
|
||||||
|
speedRatio,
|
||||||
|
samplePeriodSeconds,
|
||||||
|
denseJointTrajectory.Count,
|
||||||
|
stretchCount);
|
||||||
|
|
||||||
return new TrajectoryResult(
|
return new TrajectoryResult(
|
||||||
programName: "move-joint",
|
programName: "move-joint",
|
||||||
@@ -101,9 +127,18 @@ internal static class MoveJointTrajectoryGenerator
|
|||||||
denseJointTrajectory: denseJointTrajectory);
|
denseJointTrajectory: denseJointTrajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
internal static double ResolveDurationSeconds(RobotProfile robot, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
/// <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 = BaseMoveJointDurationSeconds;
|
var duration = MinimumMoveJointDurationSeconds;
|
||||||
|
|
||||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||||
{
|
{
|
||||||
@@ -114,9 +149,9 @@ internal static class MoveJointTrajectoryGenerator
|
|||||||
}
|
}
|
||||||
|
|
||||||
var limit = robot.JointLimits[index];
|
var limit = robot.JointLimits[index];
|
||||||
var velocityDuration = distance * VelocityShapeCoefficient / limit.VelocityLimit;
|
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
|
||||||
var accelerationDuration = Math.Sqrt(distance * AccelerationShapeCoefficient / limit.AccelerationLimit);
|
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||||
var jerkDuration = Math.Cbrt(distance * JerkShapeCoefficient / limit.JerkLimit);
|
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
|
||||||
|
|
||||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||||
}
|
}
|
||||||
@@ -124,6 +159,12 @@ internal static class MoveJointTrajectoryGenerator
|
|||||||
return duration;
|
return duration;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将请求时长向上对齐到整数个采样周期，确保轨迹末帧正好落在 duration 处。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="durationSeconds">请求的理论最短时长（秒）。</param>
|
||||||
|
/// <param name="samplePeriodSeconds">采样周期（秒）。</param>
|
||||||
|
/// <returns>对齐后的时长，为 samplePeriodSeconds 的整数倍。</returns>
|
||||||
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||||
{
|
{
|
||||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||||
@@ -133,26 +174,14 @@ internal static class MoveJointTrajectoryGenerator
|
|||||||
|
|
||||||
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
||||||
|
|
||||||
return Math.Max(1, intervals) * samplePeriodSeconds;
|
return intervals * samplePeriodSeconds;
|
||||||
}
|
|
||||||
|
|
||||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
|
||||||
{
|
|
||||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
|
||||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
|
||||||
{
|
|
||||||
throw new InvalidOperationException("MoveJoint sample count is not representable.");
|
|
||||||
}
|
|
||||||
|
|
||||||
var intervals = (long)Math.Ceiling(rawIntervals - 1e-9);
|
|
||||||
if (intervals < 1 || intervals + 1 > MaxMoveJointSampleCount)
|
|
||||||
{
|
|
||||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
|
||||||
}
|
|
||||||
|
|
||||||
return intervals;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 生成从起始关节角到目标关节角的稠密等时间隔轨迹点序列。
|
||||||
|
///
|
||||||
|
/// 每行格式：[time_seconds, joint_0, joint_1, ..., joint_n-1]。
|
||||||
|
/// </summary>
|
||||||
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||||
IReadOnlyList<double> startJoints,
|
IReadOnlyList<double> startJoints,
|
||||||
IReadOnlyList<double> targetJoints,
|
IReadOnlyList<double> targetJoints,
|
||||||
@@ -171,30 +200,128 @@ internal static class MoveJointTrajectoryGenerator
|
|||||||
return rows;
|
return rows;
|
||||||
}
|
}
|
||||||
|
|
||||||
private static IReadOnlyList<double> CreateRow(double timeSeconds, double durationSeconds, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
/// <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 u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||||
var alpha = InterpolateCapturedAlpha(u);
|
var scale = EvaluateSmoothPtpPositionScale(u);
|
||||||
var row = new double[startJoints.Count + 1];
|
var row = new double[startJoints.Count + 1];
|
||||||
row[0] = Math.Round(timeSeconds, 9);
|
row[0] = Math.Round(timeSeconds, 9);
|
||||||
|
|
||||||
for (var index = 0; index < startJoints.Count; index++)
|
for (var index = 0; index < startJoints.Count; index++)
|
||||||
{
|
{
|
||||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * alpha);
|
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * scale);
|
||||||
}
|
}
|
||||||
|
|
||||||
return row;
|
return row;
|
||||||
}
|
}
|
||||||
|
|
||||||
internal static double InterpolateCapturedAlpha(double normalizedTime)
|
/// <summary>
|
||||||
|
/// 用生成后的离散采样点复核每轴速度、加速度和 jerk，避免连续时间律在采样后仍出现差分越限。
|
||||||
|
/// </summary>
|
||||||
|
private static bool SatisfiesDiscreteJointLimits(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||||
{
|
{
|
||||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
double? previousTime = null;
|
||||||
var scaledIndex = clamped * (CapturedMvpointAlpha.Length - 1);
|
double[]? previousPositions = null;
|
||||||
var lower = (int)Math.Floor(scaledIndex);
|
double[]? previousVelocities = null;
|
||||||
var upper = Math.Min(lower + 1, CapturedMvpointAlpha.Length - 1);
|
double[]? previousAccelerations = null;
|
||||||
var fraction = scaledIndex - lower;
|
|
||||||
|
|
||||||
return CapturedMvpointAlpha[lower]
|
foreach (var row in rows)
|
||||||
+ ((CapturedMvpointAlpha[upper] - CapturedMvpointAlpha[lower]) * fraction);
|
{
|
||||||
|
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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
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; }
|
||||||
|
}
|
||||||
@@ -19,7 +19,8 @@ public sealed class CompatibilityRobotSettings
|
|||||||
double accLimitScale,
|
double accLimitScale,
|
||||||
double jerkLimitScale,
|
double jerkLimitScale,
|
||||||
int adaptIcspTryNum,
|
int adaptIcspTryNum,
|
||||||
double planningSpeedScale = 1.0)
|
double planningSpeedScale = 1.0,
|
||||||
|
bool smoothStartStopTiming = true)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(ioAddresses);
|
ArgumentNullException.ThrowIfNull(ioAddresses);
|
||||||
|
|
||||||
@@ -62,6 +63,7 @@ public sealed class CompatibilityRobotSettings
|
|||||||
JerkLimitScale = jerkLimitScale;
|
JerkLimitScale = jerkLimitScale;
|
||||||
AdaptIcspTryNum = adaptIcspTryNum;
|
AdaptIcspTryNum = adaptIcspTryNum;
|
||||||
PlanningSpeedScale = planningSpeedScale;
|
PlanningSpeedScale = planningSpeedScale;
|
||||||
|
SmoothStartStopTiming = smoothStartStopTiming;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -94,6 +96,11 @@ public sealed class CompatibilityRobotSettings
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
public double PlanningSpeedScale { get; }
|
public double PlanningSpeedScale { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
|
||||||
|
/// </summary>
|
||||||
|
public bool SmoothStartStopTiming { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取自适应补点最大尝试次数。
|
/// 获取自适应补点最大尝试次数。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -180,7 +187,8 @@ public sealed class RobotConfigLoader
|
|||||||
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
|
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
|
||||||
jerkLimitScale: ReadDouble(robotElement, "jerk_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));
|
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);
|
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
|
||||||
foreach (var programElement in flyingShotsElement.EnumerateObject())
|
foreach (var programElement in flyingShotsElement.EnumerateObject())
|
||||||
@@ -191,8 +199,8 @@ public sealed class RobotConfigLoader
|
|||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
|
"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.AdaptIcspTryNum, programs.Count);
|
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
|
||||||
|
|
||||||
return new LoadedRobotConfig(
|
return new LoadedRobotConfig(
|
||||||
sourcePath: resolvedConfigPath,
|
sourcePath: resolvedConfigPath,
|
||||||
|
|||||||
@@ -1,4 +1,3 @@
|
|||||||
using System.Text;
|
|
||||||
using System.Text.Json;
|
using System.Text.Json;
|
||||||
using Flyshot.Core.Domain;
|
using Flyshot.Core.Domain;
|
||||||
using Microsoft.Extensions.Logging;
|
using Microsoft.Extensions.Logging;
|
||||||
@@ -6,11 +5,10 @@ using Microsoft.Extensions.Logging;
|
|||||||
namespace Flyshot.Core.Config;
|
namespace Flyshot.Core.Config;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
|
/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class RobotModelLoader
|
public sealed class RobotModelLoader
|
||||||
{
|
{
|
||||||
private const uint JsonChunkType = 0x4E4F534A;
|
|
||||||
private readonly ILogger<RobotModelLoader>? _logger;
|
private readonly ILogger<RobotModelLoader>? _logger;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -23,17 +21,29 @@ public sealed class RobotModelLoader
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
|
/// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="modelPath">.robot 文件路径。</param>
|
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||||
/// <param name="accLimitScale">加速度全局倍率。</param>
|
/// <param name="accLimitScale">加速度全局倍率。</param>
|
||||||
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
||||||
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
|
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
|
||||||
public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
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))
|
if (string.IsNullOrWhiteSpace(modelPath))
|
||||||
{
|
{
|
||||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
throw new ArgumentException("机器人 JSON 路径不能为空。", nameof(modelPath));
|
||||||
}
|
}
|
||||||
|
|
||||||
if (accLimitScale <= 0.0)
|
if (accLimitScale <= 0.0)
|
||||||
@@ -46,17 +56,72 @@ public sealed class RobotModelLoader
|
|||||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
_logger?.LogInformation("RobotModel JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
||||||
|
|
||||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
using var document = JsonDocument.Parse(File.ReadAllText(resolvedModelPath));
|
||||||
using var document = JsonDocument.Parse(jsonText);
|
|
||||||
|
|
||||||
var robotBody = FindRobotBody(document.RootElement);
|
var robotBody = FindRobotBody(document.RootElement);
|
||||||
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
||||||
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
||||||
: 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 jointLimits = new List<JointLimit>();
|
||||||
var jointCouplings = new List<JointCoupling>();
|
var jointCouplings = new List<JointCoupling>();
|
||||||
|
|
||||||
@@ -80,18 +145,21 @@ public sealed class RobotModelLoader
|
|||||||
{
|
{
|
||||||
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
|
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
|
||||||
?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
|
?? 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(
|
jointCouplings.Add(new JointCoupling(
|
||||||
slaveJointName: jointName,
|
slaveJointName: jointName,
|
||||||
masterJointName: masterJointName,
|
masterJointName: masterJointName,
|
||||||
multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0,
|
multiplier: multiplier,
|
||||||
offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0));
|
offset: offset));
|
||||||
|
_logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation(
|
foreach (var jointLimit in jointLimits)
|
||||||
"RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}",
|
{
|
||||||
profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath);
|
_logger?.LogInformation("关节 {JointName} 的限制值: 速度={VelocityLimit}, 加速度={AccelerationLimit}, Jerk={JerkLimit}", jointLimit.JointName, jointLimit.VelocityLimit, jointLimit.AccelerationLimit, jointLimit.JerkLimit);
|
||||||
|
}
|
||||||
|
|
||||||
return new RobotProfile(
|
return new RobotProfile(
|
||||||
name: profileName,
|
name: profileName,
|
||||||
@@ -104,101 +172,32 @@ public sealed class RobotModelLoader
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 从 GLB 文件中提取 JSON chunk 文本。
|
/// 从机器人主体构造正运动学几何视图。
|
||||||
/// </summary>
|
/// </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));
|
|
||||||
}
|
|
||||||
|
|
||||||
_logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", 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>();
|
var joints = new List<RobotJointGeometry>();
|
||||||
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
|
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
|
||||||
{
|
{
|
||||||
var jointName = jointElement.GetProperty("name").GetString()
|
var jointName = jointElement.GetProperty("name").GetString()
|
||||||
?? throw new InvalidDataException("关节缺少 name。");
|
?? throw new InvalidDataException("关节缺少 name。");
|
||||||
|
// jointType: 关节类型编码；用于区分旋转关节/其他结构关节，后续几何链路可据此决定求解策略。
|
||||||
var jointType = jointElement.TryGetProperty("type", out var typeElement)
|
var jointType = jointElement.TryGetProperty("type", out var typeElement)
|
||||||
? typeElement.GetInt32()
|
? typeElement.GetInt32()
|
||||||
: 0;
|
: 0;
|
||||||
|
// origin: 关节局部原点配置，格式通常为 [x, y, z, qx, qy, qz, qw]，定义父坐标到关节坐标的位姿。
|
||||||
var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
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();
|
var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
||||||
// axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。
|
// axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。
|
||||||
var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
|
var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
|
||||||
|
// originXyz: 平移分量 (x,y,z)，用于构建关节在父链路下的位置偏移。
|
||||||
var originXyz = origin.Length >= 3 ? origin[..3] : origin;
|
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;
|
string? coupleMaster = null;
|
||||||
double coupleMultiplier = 0.0;
|
double coupleMultiplier = 0.0;
|
||||||
double coupleOffset = 0.0;
|
double coupleOffset = 0.0;
|
||||||
@@ -209,21 +208,45 @@ public sealed class RobotModelLoader
|
|||||||
coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
|
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(
|
joints.Add(new RobotJointGeometry(
|
||||||
|
// name: 当前关节名，作为几何链和耦合关系的主键。
|
||||||
name: jointName,
|
name: jointName,
|
||||||
parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
|
// parent: 父 link 名称，用于串起机器人树结构。
|
||||||
child: jointElement.GetProperty("child").GetString() ?? string.Empty,
|
parent: parentLink,
|
||||||
|
// child: 子 link 名称，标识该关节输出到哪个连杆。
|
||||||
|
child: childLink,
|
||||||
|
// jointType: 关节类型编码，供运动学模型区分计算路径。
|
||||||
jointType: jointType,
|
jointType: jointType,
|
||||||
|
// axis: 关节轴方向向量，决定旋转/平移沿哪个局部方向发生。
|
||||||
axis: axisVector,
|
axis: axisVector,
|
||||||
|
// originXyz: 关节原点平移分量。
|
||||||
originXyz: originXyz,
|
originXyz: originXyz,
|
||||||
|
// originQuatXyzw: 关节原点旋转四元数分量。
|
||||||
originQuatXyzw: originQuat,
|
originQuatXyzw: originQuat,
|
||||||
|
// coupleMaster: 耦合主关节名（无耦合时为 null）。
|
||||||
coupleMaster: coupleMaster,
|
coupleMaster: coupleMaster,
|
||||||
|
// coupleMultiplier: 耦合线性比例系数。
|
||||||
coupleMultiplier: coupleMultiplier,
|
coupleMultiplier: coupleMultiplier,
|
||||||
|
// coupleOffset: 耦合常量偏移量。
|
||||||
coupleOffset: coupleOffset));
|
coupleOffset: coupleOffset));
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count);
|
_logger?.LogInformation("几何模型构建完成: profileName={ProfileName}, jointCount={JointCount}", profileName, joints.Count);
|
||||||
|
|
||||||
return new RobotKinematicsModel(name: profileName, joints: joints);
|
return new RobotKinematicsModel(name: profileName, joints: joints);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -3,7 +3,7 @@ using System.Text.Json.Serialization;
|
|||||||
namespace Flyshot.Core.Domain;
|
namespace Flyshot.Core.Domain;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 描述机器人运动学链所需的完整关节几何信息，从 .robot GLB 中提取。
|
/// 描述机器人运动学链所需的完整关节几何信息，从现场固化的机器人 JSON 中提取。
|
||||||
///
|
///
|
||||||
/// 为什么与 RobotProfile 分开？
|
/// 为什么与 RobotProfile 分开？
|
||||||
/// ---
|
/// ---
|
||||||
|
|||||||
@@ -3,12 +3,12 @@ using System.Text.Json.Serialization;
|
|||||||
namespace Flyshot.Core.Domain;
|
namespace Flyshot.Core.Domain;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Describes the robot model contract consumed by planning and runtime orchestration.
|
/// 描述规划与运行时编排共同使用的机器人模型契约。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class RobotProfile
|
public sealed class RobotProfile
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Initializes a new robot profile with validated joint limits and coupling metadata.
|
/// 使用已校验的关节约束与耦合元数据初始化机器人画像。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public RobotProfile(
|
public RobotProfile(
|
||||||
string name,
|
string name,
|
||||||
@@ -47,7 +47,7 @@ public sealed class RobotProfile
|
|||||||
ArgumentNullException.ThrowIfNull(jointLimits);
|
ArgumentNullException.ThrowIfNull(jointLimits);
|
||||||
ArgumentNullException.ThrowIfNull(jointCouplings);
|
ArgumentNullException.ThrowIfNull(jointCouplings);
|
||||||
|
|
||||||
// Snapshot the collections once so downstream layers cannot mutate domain state in place.
|
// 先对集合做一次快照，避免下游直接原地修改领域状态。
|
||||||
var copiedJointLimits = jointLimits.ToArray();
|
var copiedJointLimits = jointLimits.ToArray();
|
||||||
var copiedJointCouplings = jointCouplings.ToArray();
|
var copiedJointCouplings = jointCouplings.ToArray();
|
||||||
|
|
||||||
@@ -66,55 +66,55 @@ public sealed class RobotProfile
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the robot profile name exposed to the rest of the runtime.
|
/// 获取对运行时其余模块暴露的机器人画像名称。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("name")]
|
[JsonPropertyName("name")]
|
||||||
public string Name { get; }
|
public string Name { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the source path of the robot model file.
|
/// 获取机器人模型文件的来源路径。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("modelPath")]
|
[JsonPropertyName("modelPath")]
|
||||||
public string ModelPath { get; }
|
public string ModelPath { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the active revolute degree-of-freedom count.
|
/// 获取当前生效的旋转关节自由度数量。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("degreesOfFreedom")]
|
[JsonPropertyName("degreesOfFreedom")]
|
||||||
public int DegreesOfFreedom { get; }
|
public int DegreesOfFreedom { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the validated per-joint kinematic limits.
|
/// 获取按关节校验后的运动学约束。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("jointLimits")]
|
[JsonPropertyName("jointLimits")]
|
||||||
public IReadOnlyList<JointLimit> JointLimits { get; }
|
public IReadOnlyList<JointLimit> JointLimits { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets optional joint coupling metadata parsed from the robot model.
|
/// 获取从机器人模型解析出的可选关节耦合元数据。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("jointCouplings")]
|
[JsonPropertyName("jointCouplings")]
|
||||||
public IReadOnlyList<JointCoupling> JointCouplings { get; }
|
public IReadOnlyList<JointCoupling> JointCouplings { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the servo scheduling period used by the runtime.
|
/// 获取运行时使用的伺服调度周期。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("servoPeriod")]
|
[JsonPropertyName("servoPeriod")]
|
||||||
public TimeSpan ServoPeriod { get; }
|
public TimeSpan ServoPeriod { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the trigger scheduling period used by shot-event alignment.
|
/// 获取飞拍事件对齐使用的触发调度周期。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("triggerPeriod")]
|
[JsonPropertyName("triggerPeriod")]
|
||||||
public TimeSpan TriggerPeriod { get; }
|
public TimeSpan TriggerPeriod { get; }
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Describes a single revolute joint limit set required by the planners.
|
/// 描述规划器所需的单个旋转关节约束集合。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class JointLimit
|
public sealed class JointLimit
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Initializes a validated joint limit record.
|
/// 初始化一个已校验的关节约束记录。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit)
|
public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit)
|
||||||
{
|
{
|
||||||
@@ -145,37 +145,37 @@ public sealed class JointLimit
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the joint name associated with the limits.
|
/// 获取该约束对应的关节名称。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("jointName")]
|
[JsonPropertyName("jointName")]
|
||||||
public string JointName { get; }
|
public string JointName { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the velocity limit in joint space units.
|
/// 获取关节空间单位下的速度上限。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("velocityLimit")]
|
[JsonPropertyName("velocityLimit")]
|
||||||
public double VelocityLimit { get; }
|
public double VelocityLimit { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the acceleration limit in joint space units.
|
/// 获取关节空间单位下的加速度上限。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("accelerationLimit")]
|
[JsonPropertyName("accelerationLimit")]
|
||||||
public double AccelerationLimit { get; }
|
public double AccelerationLimit { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the jerk limit in joint space units.
|
/// 获取关节空间单位下的跃度上限。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("jerkLimit")]
|
[JsonPropertyName("jerkLimit")]
|
||||||
public double JerkLimit { get; }
|
public double JerkLimit { get; }
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Describes a joint-coupling rule that must be applied before kinematics or planning.
|
/// 描述在运动学计算或轨迹规划前必须应用的关节耦合规则。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class JointCoupling
|
public sealed class JointCoupling
|
||||||
{
|
{
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Initializes a validated joint-coupling description.
|
/// 初始化一个已校验的关节耦合描述。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset)
|
public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset)
|
||||||
{
|
{
|
||||||
@@ -201,25 +201,25 @@ public sealed class JointCoupling
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the dependent joint name.
|
/// 获取从属（被驱动）关节名称。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("slaveJointName")]
|
[JsonPropertyName("slaveJointName")]
|
||||||
public string SlaveJointName { get; }
|
public string SlaveJointName { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the source joint name.
|
/// 获取主导（驱动）关节名称。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("masterJointName")]
|
[JsonPropertyName("masterJointName")]
|
||||||
public string MasterJointName { get; }
|
public string MasterJointName { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the coupling multiplier applied to the master joint angle.
|
/// 获取作用在主导关节角度上的耦合倍率。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("multiplier")]
|
[JsonPropertyName("multiplier")]
|
||||||
public double Multiplier { get; }
|
public double Multiplier { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Gets the additive offset applied after the multiplier.
|
/// 获取在耦合倍率之后叠加的偏移量。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[JsonPropertyName("offset")]
|
[JsonPropertyName("offset")]
|
||||||
public double Offset { get; }
|
public double Offset { get; }
|
||||||
|
|||||||
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`
|
||||||
|
|
||||||
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(
|
public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
|
||||||
PlannedTrajectory trajectory,
|
PlannedTrajectory trajectory,
|
||||||
double samplePeriod = 0.016,
|
double samplePeriod = 0.016,
|
||||||
int decimals = 6)
|
int decimals = 6,
|
||||||
|
bool smoothStartStop = false)
|
||||||
{
|
{
|
||||||
var spline = RebuildSpline(trajectory);
|
var spline = RebuildSpline(trajectory);
|
||||||
double duration = trajectory.WaypointTimes[^1];
|
double duration = trajectory.WaypointTimes[^1];
|
||||||
@@ -28,7 +29,10 @@ public static class TrajectorySampler
|
|||||||
|
|
||||||
foreach (var t in times)
|
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);
|
var row = new List<double>(pos.Length + 1);
|
||||||
row.Add(Math.Round(t, decimals));
|
row.Add(Math.Round(t, decimals));
|
||||||
foreach (var value in pos)
|
foreach (var value in pos)
|
||||||
@@ -49,7 +53,8 @@ public static class TrajectorySampler
|
|||||||
PlannedTrajectory trajectory,
|
PlannedTrajectory trajectory,
|
||||||
RobotKinematicsModel kinematicsModel,
|
RobotKinematicsModel kinematicsModel,
|
||||||
double samplePeriod = 0.016,
|
double samplePeriod = 0.016,
|
||||||
int decimals = 6)
|
int decimals = 6,
|
||||||
|
bool smoothStartStop = false)
|
||||||
{
|
{
|
||||||
var spline = RebuildSpline(trajectory);
|
var spline = RebuildSpline(trajectory);
|
||||||
double duration = trajectory.WaypointTimes[^1];
|
double duration = trajectory.WaypointTimes[^1];
|
||||||
@@ -58,7 +63,10 @@ public static class TrajectorySampler
|
|||||||
|
|
||||||
foreach (var t in times)
|
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 pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
|
||||||
var row = new List<double>(pose.Length + 1);
|
var row = new List<double>(pose.Length + 1);
|
||||||
row.Add(Math.Round(t, decimals));
|
row.Add(Math.Round(t, decimals));
|
||||||
@@ -103,4 +111,26 @@ public static class TrajectorySampler
|
|||||||
|
|
||||||
return times;
|
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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,8 +1,11 @@
|
|||||||
using System.Diagnostics;
|
using System.Diagnostics;
|
||||||
|
using System.Text;
|
||||||
using Flyshot.Core.Domain;
|
using Flyshot.Core.Domain;
|
||||||
|
using Flyshot.Core.Planning.Sampling;
|
||||||
using Flyshot.Runtime.Common;
|
using Flyshot.Runtime.Common;
|
||||||
using Flyshot.Runtime.Fanuc.Protocol;
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
using Microsoft.Extensions.Logging;
|
using Microsoft.Extensions.Logging;
|
||||||
|
using Microsoft.Extensions.Logging.Abstractions;
|
||||||
|
|
||||||
namespace Flyshot.Runtime.Fanuc;
|
namespace Flyshot.Runtime.Fanuc;
|
||||||
|
|
||||||
@@ -32,20 +35,31 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
|
private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
|
||||||
private bool _disposed;
|
private bool _disposed;
|
||||||
private CancellationTokenSource? _sendCts;
|
private CancellationTokenSource? _sendCts;
|
||||||
private Task? _sendTask;
|
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 初始化 FANUC 控制器运行时。
|
/// 初始化 FANUC 控制器运行时。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
/// <param name="logger">运行时日志记录器。</param>
|
||||||
public FanucControllerRuntime(ILogger<FanucControllerRuntime>? logger = null)
|
/// <param name="j519Logger">J519 客户端日志记录器。</param>
|
||||||
|
public FanucControllerRuntime(ILogger<FanucControllerRuntime> logger, ILogger<FanucJ519Client> j519Logger)
|
||||||
{
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(logger);
|
||||||
|
ArgumentNullException.ThrowIfNull(j519Logger);
|
||||||
|
|
||||||
_commandClient = new FanucCommandClient();
|
_commandClient = new FanucCommandClient();
|
||||||
_stateClient = new FanucStateClient();
|
_stateClient = new FanucStateClient();
|
||||||
_j519Client = new FanucJ519Client();
|
_j519Client = new FanucJ519Client(j519Logger);
|
||||||
_logger = logger;
|
_logger = logger;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化一份无日志的 FANUC 控制器运行时，供离线测试或手工构造场景使用。
|
||||||
|
/// </summary>
|
||||||
|
public FanucControllerRuntime()
|
||||||
|
: this(NullLogger<FanucControllerRuntime>.Instance, NullLogger<FanucJ519Client>.Instance)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 供测试注入 mock 客户端的内部构造函数。
|
/// 供测试注入 mock 客户端的内部构造函数。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -166,6 +180,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
{
|
{
|
||||||
EnsureConnected();
|
EnsureConnected();
|
||||||
_bufferSize = bufferSize;
|
_bufferSize = bufferSize;
|
||||||
|
_j519Client.SetSequenceBufferSize(bufferSize);
|
||||||
|
|
||||||
if (IsSimulationMode)
|
if (IsSimulationMode)
|
||||||
{
|
{
|
||||||
@@ -174,10 +189,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 真机模式：走完整 RVBUSTSM 启动序列（与抓包一致）。
|
// 真机模式：按 all-reconnect.pcap 的重连序列启动 RVBUSTSM，暂不发送 StopProg。
|
||||||
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||||
_commandClient.ResetRobotAsync().GetAwaiter().GetResult();
|
_commandClient.ResetRobotAsync().GetAwaiter().GetResult();
|
||||||
_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
|
//_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||||
_commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
_commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||||
|
|
||||||
_j519Client.StartMotion();
|
_j519Client.StartMotion();
|
||||||
@@ -253,11 +268,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
|
|
||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureConnected();
|
//EnsureConnected();
|
||||||
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
|
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
|
||||||
if (!IsSimulationMode)
|
if (!IsSimulationMode)
|
||||||
{
|
{
|
||||||
_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
|
//_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
|
||||||
}
|
}
|
||||||
|
|
||||||
_speedRatio = clampedRatio;
|
_speedRatio = clampedRatio;
|
||||||
@@ -453,6 +468,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(result);
|
ArgumentNullException.ThrowIfNull(result);
|
||||||
ArgumentNullException.ThrowIfNull(finalJointPositions);
|
ArgumentNullException.ThrowIfNull(finalJointPositions);
|
||||||
|
CancellationToken denseSendCancellationToken = default;
|
||||||
|
CancellationTokenSource? denseSendCancellationSource = null;
|
||||||
|
var shouldRunDenseTrajectory = false;
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}",
|
"ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}",
|
||||||
@@ -475,15 +493,17 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
|
|
||||||
EnsureJ519ReadyForDenseExecution();
|
EnsureJ519ReadyForDenseExecution();
|
||||||
|
|
||||||
// 真机模式且存在稠密路点：启动后台高精度发送任务。
|
// 真机模式且存在稠密路点：准备可被 StopMove 取消的同步发送任务。
|
||||||
_isInMotion = true;
|
_isInMotion = true;
|
||||||
_sendCts = new CancellationTokenSource();
|
_sendCts = new CancellationTokenSource();
|
||||||
var ct = _sendCts.Token;
|
denseSendCancellationSource = _sendCts;
|
||||||
_sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct);
|
denseSendCancellationToken = _sendCts.Token;
|
||||||
_logger?.LogInformation("ExecuteTrajectory 已启动后台稠密发送任务");
|
shouldRunDenseTrajectory = true;
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
_logger?.LogInformation("ExecuteTrajectory 开始同步稠密发送任务");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
if (!IsSimulationMode)
|
if (!IsSimulationMode)
|
||||||
{
|
{
|
||||||
// 真机模式无稠密路点：回退到单点收敛。
|
// 真机模式无稠密路点：回退到单点收敛。
|
||||||
@@ -500,6 +520,28 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (shouldRunDenseTrajectory)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
// 稠密轨迹必须等 J519 队列被机器人状态包实际取完后再向上层返回。
|
||||||
|
SendDenseTrajectory(result, finalJointPositions, denseSendCancellationToken);
|
||||||
|
_logger?.LogInformation("ExecuteTrajectory 完成(稠密模式)");
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
lock (_stateLock)
|
||||||
|
{
|
||||||
|
if (ReferenceEquals(_sendCts, denseSendCancellationSource))
|
||||||
|
{
|
||||||
|
denseSendCancellationSource?.Dispose();
|
||||||
|
_sendCts = null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 释放运行时持有的所有 Socket 客户端。
|
/// 释放运行时持有的所有 Socket 客户端。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -519,42 +561,52 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 后台高精度发送任务：按 J519 周期发送命令，并按 speed_ratio 推进原始轨迹时间。
|
/// 稠密轨迹发送任务：预生成完整 J519 命令队列，并等待机器人状态包按 speed_ratio 推进到执行完成。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
|
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
|
||||||
{
|
{
|
||||||
var denseTrajectory = result.DenseJointTrajectory!;
|
|
||||||
var triggers = result.TriggerTimeline;
|
var triggers = result.TriggerTimeline;
|
||||||
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
|
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
|
||||||
var speedRatio = _speedRatio;
|
var speedRatio = _speedRatio;
|
||||||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||||||
var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
|
var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
|
||||||
var durationSeconds = result.Duration.TotalSeconds;
|
var durationSeconds = result.Duration.TotalSeconds;
|
||||||
var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds);
|
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||||
var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency);
|
result.DenseJointTrajectory!,
|
||||||
|
durationSeconds,
|
||||||
|
servoPeriodSeconds,
|
||||||
|
speedRatio);
|
||||||
|
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||||
|
_robot!,
|
||||||
|
samples,
|
||||||
|
trajectoryName: result.ProgramName);
|
||||||
|
var sampleCount = samples.Count;
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
|
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
|
||||||
result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);
|
result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);
|
||||||
|
|
||||||
var stopwatch = Stopwatch.StartNew();
|
var stopwatch = Stopwatch.StartNew();
|
||||||
long nextTick = stopwatch.ElapsedTicks;
|
|
||||||
ushort ioValue = 0;
|
ushort ioValue = 0;
|
||||||
ushort ioMask = 0;
|
ushort ioMask = 0;
|
||||||
int holdRemaining = -1;
|
int holdRemaining = -1;
|
||||||
int segmentIndex = 0;
|
|
||||||
long logInterval = Math.Max(1, sampleCount / 10);
|
long logInterval = Math.Max(1, sampleCount / 10);
|
||||||
int triggerFiredCount = 0;
|
int triggerFiredCount = 0;
|
||||||
|
var commands = new List<FanucJ519Command>(sampleCount);
|
||||||
|
var sentJointRows = new List<IReadOnlyList<double>>(sampleCount);
|
||||||
|
var sentTimingRows = new List<IReadOnlyList<double>>(sampleCount);
|
||||||
|
var sentJerkRows = new List<IReadOnlyList<double>>();
|
||||||
|
var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
|
||||||
|
double? previousSendTime = null;
|
||||||
|
double[]? previousJoints = null;
|
||||||
|
double[]? previousVelocity = null;
|
||||||
|
double[]? previousAcceleration = null;
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
foreach (var sample in samples)
|
||||||
{
|
{
|
||||||
cancellationToken.ThrowIfCancellationRequested();
|
cancellationToken.ThrowIfCancellationRequested();
|
||||||
nextTick += periodTicks;
|
|
||||||
|
|
||||||
var trajectoryTime = Math.Min(sampleIndex * trajectoryStepSeconds, durationSeconds);
|
|
||||||
var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex);
|
|
||||||
|
|
||||||
// 递减 IO 保持计数器；若已到期则清零。
|
// 递减 IO 保持计数器；若已到期则清零。
|
||||||
var clearMaskAfterSend = false;
|
var clearMaskAfterSend = false;
|
||||||
@@ -574,7 +626,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
{
|
{
|
||||||
foreach (var trigger in triggers)
|
foreach (var trigger in triggers)
|
||||||
{
|
{
|
||||||
if (Math.Abs(trajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
|
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
|
||||||
{
|
{
|
||||||
ioMask = ComputeIoValue(trigger.AddressGroup);
|
ioMask = ComputeIoValue(trigger.AddressGroup);
|
||||||
ioValue = ioMask;
|
ioValue = ioMask;
|
||||||
@@ -582,7 +634,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
triggerFiredCount++;
|
triggerFiredCount++;
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"J519 IO触发: time={Time:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
|
"J519 IO触发: time={Time:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
|
||||||
trajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
|
sample.TrajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -590,43 +642,65 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
|
|
||||||
var command = new FanucJ519Command(
|
var command = new FanucJ519Command(
|
||||||
sequence: 0,
|
sequence: 0,
|
||||||
targetJoints: joints,
|
targetJoints: sample.JointsDegrees,
|
||||||
writeIoType: 2,
|
writeIoType: 2,
|
||||||
writeIoIndex: 1,
|
writeIoIndex: 1,
|
||||||
writeIoMask: ioMask,
|
writeIoMask: ioMask,
|
||||||
writeIoValue: ioValue);
|
writeIoValue: ioValue);
|
||||||
|
|
||||||
_j519Client.UpdateCommand(command);
|
commands.Add(command);
|
||||||
|
sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue));
|
||||||
|
sentTimingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
|
||||||
|
|
||||||
|
if (previousSendTime is not null && previousJoints is not null)
|
||||||
|
{
|
||||||
|
var jerkRow = J519SendTrajectorySampler.BuildJerkRow(
|
||||||
|
previousSendTime.Value,
|
||||||
|
sample.SendTime,
|
||||||
|
previousJoints,
|
||||||
|
sample.JointsDegrees,
|
||||||
|
ref previousVelocity,
|
||||||
|
ref previousAcceleration);
|
||||||
|
sentJerkRows.Add(jerkRow);
|
||||||
|
}
|
||||||
|
|
||||||
if (clearMaskAfterSend)
|
if (clearMaskAfterSend)
|
||||||
{
|
{
|
||||||
ioMask = 0;
|
ioMask = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
previousSendTime = sample.SendTime;
|
||||||
|
previousJoints = sample.JointsDegrees.ToArray();
|
||||||
|
|
||||||
// 周期性记录进度（Debug 级别，避免高频 Info 日志）。
|
// 周期性记录进度（Debug 级别，避免高频 Info 日志）。
|
||||||
if (sampleIndex > 0 && sampleIndex % logInterval == 0)
|
if (sample.SampleIndex > 0 && sample.SampleIndex % logInterval == 0)
|
||||||
{
|
{
|
||||||
_logger?.LogDebug(
|
_logger?.LogDebug(
|
||||||
"SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), time={Time:F4}s",
|
"SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), sendTime={SendTime:F4}s, trajectoryTime={TrajectoryTime:F4}s",
|
||||||
(int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, trajectoryTime);
|
(int)((double)sample.SampleIndex / sampleCount * 100),
|
||||||
|
sample.SampleIndex,
|
||||||
|
sampleCount,
|
||||||
|
sample.SendTime,
|
||||||
|
sample.TrajectoryTime);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentTimingRows, sentJerkRows);
|
||||||
|
|
||||||
// 高精度忙等待直到下一伺服周期。
|
// 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。
|
||||||
while (stopwatch.ElapsedTicks < nextTick)
|
_j519Client.LoadCommandQueue(commands);
|
||||||
|
if (_j519Client.IsConnected)
|
||||||
{
|
{
|
||||||
Thread.SpinWait(1);
|
_j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 实际耗时={ElapsedMs}ms",
|
"SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 队列装载耗时={ElapsedMs}ms",
|
||||||
sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds);
|
sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds);
|
||||||
}
|
}
|
||||||
catch (OperationCanceledException)
|
catch (OperationCanceledException)
|
||||||
{
|
{
|
||||||
_logger?.LogWarning(
|
_logger?.LogWarning(
|
||||||
"SendDenseTrajectory 被取消: 已完成 {Percent}% ({Current}/{Total}), 触发次数={TriggerFiredCount}",
|
"SendDenseTrajectory 被取消: 已生成 {Current}/{Total} 条命令, 触发次数={TriggerFiredCount}",
|
||||||
(int)((double)(sampleCount > 0 ? 0 : 0) / sampleCount * 100), 0, sampleCount, triggerFiredCount);
|
commands.Count, sampleCount, triggerFiredCount);
|
||||||
// 正常取消，轨迹被中断。
|
// 正常取消，轨迹被中断。
|
||||||
}
|
}
|
||||||
finally
|
finally
|
||||||
@@ -639,123 +713,175 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发包数。
|
|
||||||
/// </summary>
|
|
||||||
private static long CalculateDenseSendSampleCount(double durationSeconds, double trajectoryStepSeconds)
|
|
||||||
{
|
|
||||||
if (durationSeconds < 0.0)
|
|
||||||
{
|
|
||||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "Trajectory duration must be non-negative.");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
|
|
||||||
{
|
|
||||||
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
|
|
||||||
}
|
|
||||||
|
|
||||||
return (long)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// <summary>
|
|
||||||
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 要求的角度制关节目标。
|
|
||||||
/// </summary>
|
|
||||||
private static double[] SampleDenseJointTrajectoryDegrees(
|
|
||||||
IReadOnlyList<IReadOnlyList<double>> denseTrajectory,
|
|
||||||
double trajectoryTime,
|
|
||||||
ref int segmentIndex)
|
|
||||||
{
|
|
||||||
if (denseTrajectory.Count == 0)
|
|
||||||
{
|
|
||||||
throw new InvalidOperationException("Dense joint trajectory is empty.");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (denseTrajectory.Count == 1 || trajectoryTime <= denseTrajectory[0][0])
|
|
||||||
{
|
|
||||||
return denseTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
|
|
||||||
}
|
|
||||||
|
|
||||||
var lastIndex = denseTrajectory.Count - 1;
|
|
||||||
if (trajectoryTime >= denseTrajectory[lastIndex][0])
|
|
||||||
{
|
|
||||||
return denseTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
|
|
||||||
}
|
|
||||||
|
|
||||||
while (segmentIndex < lastIndex - 1 && denseTrajectory[segmentIndex + 1][0] < trajectoryTime)
|
|
||||||
{
|
|
||||||
segmentIndex++;
|
|
||||||
}
|
|
||||||
|
|
||||||
var start = denseTrajectory[segmentIndex];
|
|
||||||
var end = denseTrajectory[segmentIndex + 1];
|
|
||||||
var startTime = start[0];
|
|
||||||
var endTime = end[0];
|
|
||||||
var segmentDuration = endTime - startTime;
|
|
||||||
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
|
|
||||||
|
|
||||||
var jointCount = start.Count - 1;
|
|
||||||
var joints = new double[jointCount];
|
|
||||||
for (var index = 0; index < jointCount; index++)
|
|
||||||
{
|
|
||||||
var radians = start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha);
|
|
||||||
joints[index] = RadiansToDegrees(radians);
|
|
||||||
}
|
|
||||||
|
|
||||||
return joints;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
|
/// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private void EnsureJ519ReadyForDenseExecution()
|
private void EnsureJ519ReadyForDenseExecution()
|
||||||
{
|
{
|
||||||
var response = _j519Client.GetLatestResponse();
|
EnsureJ519ReadyForDenseExecutionCore(
|
||||||
if (response is null)
|
() => _j519Client.GetLatestResponse(),
|
||||||
|
() =>
|
||||||
|
{
|
||||||
|
// 若当前状态未就绪，则先尝试一次 EnableRobot 走现场既有恢复链路。
|
||||||
|
EnableRobot(_bufferSize);
|
||||||
|
},
|
||||||
|
static () => Thread.Sleep(TimeSpan.FromMilliseconds(500)));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 执行稠密发送前的 J519 就绪校验；未就绪时允许先做一次启机重试。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="getLatestResponse">读取最新 J519 状态的委托。</param>
|
||||||
|
/// <param name="retryEnableRobot">状态未就绪时触发一次 EnableRobot 重试的委托。</param>
|
||||||
|
/// <param name="waitAfterRetry">重试后等待状态刷新的委托。</param>
|
||||||
|
internal static void EnsureJ519ReadyForDenseExecutionCore(
|
||||||
|
Func<FanucJ519Response?> getLatestResponse,
|
||||||
|
Action retryEnableRobot,
|
||||||
|
Action waitAfterRetry)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(getLatestResponse);
|
||||||
|
ArgumentNullException.ThrowIfNull(retryEnableRobot);
|
||||||
|
ArgumentNullException.ThrowIfNull(waitAfterRetry);
|
||||||
|
|
||||||
|
var response = getLatestResponse();
|
||||||
|
if (response is null || IsJ519ReadyForDenseExecution(response))
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (response.AcceptsCommand && response.SystemReady)
|
retryEnableRobot();
|
||||||
|
waitAfterRetry();
|
||||||
|
|
||||||
|
response = getLatestResponse();
|
||||||
|
if (response is null || IsJ519ReadyForDenseExecution(response))
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
throw new InvalidOperationException(
|
throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response));
|
||||||
"J519 status is not ready for dense execution: "
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 判断当前 J519 响应是否已经满足稠密执行前置条件。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="response">待检查的 J519 状态响应。</param>
|
||||||
|
/// <returns>接受命令且系统就绪时返回 true，否则返回 false。</returns>
|
||||||
|
internal static bool IsJ519ReadyForDenseExecution(FanucJ519Response response)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(response);
|
||||||
|
return response is { AcceptsCommand: true, SystemReady: true };
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 组装 J519 未就绪时的异常消息，便于保留现场关键状态位。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="response">最近一次收到的 J519 状态响应。</param>
|
||||||
|
/// <returns>包含状态位和序号的诊断消息。</returns>
|
||||||
|
internal static string BuildJ519DenseExecutionNotReadyMessage(FanucJ519Response response)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(response);
|
||||||
|
return "J519 status is not ready for dense execution: "
|
||||||
+ $"accept_cmd={response.AcceptsCommand}, "
|
+ $"accept_cmd={response.AcceptsCommand}, "
|
||||||
+ $"received_cmd={response.ReceivedCommand}, "
|
+ $"received_cmd={response.ReceivedCommand}, "
|
||||||
+ $"sysrdy={response.SystemReady}, "
|
+ $"sysrdy={response.SystemReady}, "
|
||||||
+ $"rbt_inmotion={response.RobotInMotion}, "
|
+ $"rbt_inmotion={response.RobotInMotion}, "
|
||||||
+ $"seq={response.Sequence}, "
|
+ $"seq={response.Sequence}, "
|
||||||
+ $"status=0x{response.Status:X2}.");
|
+ $"status=0x{response.Status:X2}.";
|
||||||
}
|
|
||||||
|
|
||||||
private static double RadiansToDegrees(double radians)
|
|
||||||
{
|
|
||||||
return radians * 180.0 / Math.PI;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。
|
/// 为单次稠密发送创建按日期时间归档的输出目录。
|
||||||
|
/// </summary>
|
||||||
|
private static string CreateDenseSendOutputDirectory(string programName)
|
||||||
|
{
|
||||||
|
var safeProgramName = SanitizeDirectoryName(string.IsNullOrWhiteSpace(programName) ? "unnamed-program" : programName);
|
||||||
|
var timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss_fff");
|
||||||
|
var outputDir = Path.Combine(
|
||||||
|
AppContext.BaseDirectory,
|
||||||
|
"Config",
|
||||||
|
"Data",
|
||||||
|
safeProgramName,
|
||||||
|
"DenseSend",
|
||||||
|
timestamp);
|
||||||
|
Directory.CreateDirectory(outputDir);
|
||||||
|
return outputDir;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。
|
||||||
|
/// </summary>
|
||||||
|
private static IReadOnlyList<double> BuildDenseSendJointRow(double sendTime, IReadOnlyList<double> joints, ushort ioMask, ushort ioValue)
|
||||||
|
{
|
||||||
|
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] = ioMask;
|
||||||
|
row[^1] = ioValue;
|
||||||
|
return row;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。
|
||||||
|
/// </summary>
|
||||||
|
private void TryWriteDenseSendArtifacts(
|
||||||
|
string outputDir,
|
||||||
|
IReadOnlyList<IReadOnlyList<double>> sentJointRows,
|
||||||
|
IReadOnlyList<IReadOnlyList<double>> sentTimingRows,
|
||||||
|
IReadOnlyList<IReadOnlyList<double>> sentJerkRows)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), sentJointRows);
|
||||||
|
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), sentTimingRows);
|
||||||
|
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), sentJerkRows);
|
||||||
|
_logger?.LogInformation(
|
||||||
|
"SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, timingRows={TimingRows}, jerkRows={JerkRows}",
|
||||||
|
outputDir,
|
||||||
|
sentJointRows.Count,
|
||||||
|
sentTimingRows.Count,
|
||||||
|
sentJerkRows.Count);
|
||||||
|
}
|
||||||
|
catch (Exception exception)
|
||||||
|
{
|
||||||
|
_logger?.LogWarning(exception, "SendDenseTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <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>
|
||||||
|
/// 清理程序名中的非法路径字符，避免输出目录受外部输入污染。
|
||||||
|
/// </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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 取消当前稠密发送任务，避免旧轨迹继续向机器人下发。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private void CancelSendTaskLocked()
|
private void CancelSendTaskLocked()
|
||||||
{
|
{
|
||||||
_sendCts?.Cancel();
|
_sendCts?.Cancel();
|
||||||
|
|
||||||
if (_sendTask is not null)
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
_sendTask.Wait(TimeSpan.FromSeconds(2));
|
|
||||||
}
|
|
||||||
catch (AggregateException)
|
|
||||||
{
|
|
||||||
// 忽略取消异常。
|
|
||||||
}
|
|
||||||
|
|
||||||
_sendTask = null;
|
|
||||||
}
|
|
||||||
|
|
||||||
_sendCts?.Dispose();
|
_sendCts?.Dispose();
|
||||||
_sendCts = null;
|
_sendCts = null;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -18,6 +18,7 @@
|
|||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
<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" />
|
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
|||||||
@@ -548,9 +548,9 @@ public static class FanucCommandProtocol
|
|||||||
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
||||||
}
|
}
|
||||||
|
|
||||||
// 抓包样本中的字段顺序为 result_code 后接 prog_status。
|
// all-reconnect.pcap 中字段顺序为 prog_status 后接 result_code。
|
||||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||||
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||||
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,3 +1,4 @@
|
|||||||
|
using System.Diagnostics;
|
||||||
using System.Net.Sockets;
|
using System.Net.Sockets;
|
||||||
using Microsoft.Extensions.Logging;
|
using Microsoft.Extensions.Logging;
|
||||||
|
|
||||||
@@ -13,10 +14,19 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
private readonly ILogger<FanucJ519Client>? _logger;
|
private readonly ILogger<FanucJ519Client>? _logger;
|
||||||
private UdpClient? _udpClient;
|
private UdpClient? _udpClient;
|
||||||
private CancellationTokenSource? _cts;
|
private CancellationTokenSource? _cts;
|
||||||
private Task? _receiveTask;
|
private Thread? _receiveThread;
|
||||||
private FanucJ519Command? _currentCommand;
|
private FanucJ519Command? _currentCommand;
|
||||||
|
private FanucJ519Command? _lastSentCommand;
|
||||||
|
// 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。
|
||||||
|
private Queue<FanucJ519Command>? _commandQueue;
|
||||||
|
private TaskCompletionSource? _commandQueueDrainedCompletion;
|
||||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||||
private FanucJ519Response? _latestResponse;
|
private FanucJ519Response? _latestResponse;
|
||||||
|
private long _slowSendCount;
|
||||||
|
private long _maxReceiveToSendTicks;
|
||||||
|
private uint _sequenceBufferSize;
|
||||||
|
// 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。
|
||||||
|
private bool _hasPendingCommandForStart;
|
||||||
private bool _motionStarted;
|
private bool _motionStarted;
|
||||||
private bool _disposed;
|
private bool _disposed;
|
||||||
|
|
||||||
@@ -28,7 +38,7 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 初始化 FANUC J519 客户端。
|
/// 初始化 FANUC J519 客户端。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="logger">日志记录器；允许 null。</param>
|
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||||
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
|
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
|
||||||
{
|
{
|
||||||
_logger = logger;
|
_logger = logger;
|
||||||
@@ -58,13 +68,21 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
|
|
||||||
_udpClient = new UdpClient();
|
_udpClient = new UdpClient();
|
||||||
_udpClient.Connect(ip, port);
|
_udpClient.Connect(ip, port);
|
||||||
|
ConfigureRealtimeSocket(_udpClient.Client);
|
||||||
|
ConfigureProcessPriority();
|
||||||
|
|
||||||
// 发送初始化包。
|
// 发送初始化包。
|
||||||
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
||||||
_logger?.LogInformation("J519 初始化包已发送");
|
_logger?.LogInformation("J519 初始化包已发送");
|
||||||
|
|
||||||
_cts = new CancellationTokenSource();
|
_cts = new CancellationTokenSource();
|
||||||
_receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token);
|
_receiveThread = new Thread(ReceiveLoop)
|
||||||
|
{
|
||||||
|
IsBackground = true,
|
||||||
|
Name = "J519 UDP realtime loop",
|
||||||
|
Priority = ThreadPriority.Highest
|
||||||
|
};
|
||||||
|
_receiveThread.Start();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -81,18 +99,46 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
|
|
||||||
lock (_commandLock)
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
|
_lastSentCommand = null;
|
||||||
if (_motionStarted)
|
if (_motionStarted)
|
||||||
{
|
{
|
||||||
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!_hasPendingCommandForStart)
|
||||||
|
{
|
||||||
|
_currentCommand = null;
|
||||||
|
CompleteCommandQueueLocked();
|
||||||
|
}
|
||||||
|
|
||||||
|
_hasPendingCommandForStart = false;
|
||||||
_motionStarted = true;
|
_motionStarted = true;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
|
_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>
|
/// <summary>
|
||||||
/// 发送状态输出停止包并停止 J519 命令发送。
|
/// 发送状态输出停止包并停止 J519 命令发送。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -109,6 +155,8 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
lock (_commandLock)
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
_motionStarted = false;
|
_motionStarted = false;
|
||||||
|
_hasPendingCommandForStart = false;
|
||||||
|
CompleteCommandQueueLocked();
|
||||||
}
|
}
|
||||||
|
|
||||||
// FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
|
// FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
|
||||||
@@ -127,15 +175,78 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
|
|
||||||
lock (_commandLock)
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
|
CompleteCommandQueueLocked();
|
||||||
_currentCommand = command;
|
_currentCommand = command;
|
||||||
|
_hasPendingCommandForStart = true;
|
||||||
_commandHistoryForTests?.Add(command);
|
_commandHistoryForTests?.Add(command);
|
||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogDebug(
|
if (_logger?.IsEnabled(LogLevel.Debug) == true)
|
||||||
"J519 UpdateCommand: joints=[{Joints}], ioMask=0x{IoMask:X4}, ioValue=0x{IoValue:X4}",
|
{
|
||||||
string.Join(", ", command.TargetJoints.Select(j => j.ToString("F2"))),
|
//_logger.LogDebug(
|
||||||
command.WriteIoMask,
|
// "J519 UpdateCommand: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
|
||||||
command.WriteIoValue);
|
// 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>
|
||||||
@@ -204,26 +315,28 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
_udpClient?.Dispose();
|
||||||
|
_receiveThread?.Join(TimeSpan.FromSeconds(1));
|
||||||
}
|
}
|
||||||
catch (AggregateException)
|
catch (ObjectDisposedException)
|
||||||
{
|
{
|
||||||
// 忽略取消异常。
|
// 忽略释放期间的套接字关闭异常。
|
||||||
}
|
}
|
||||||
|
|
||||||
_receiveTask?.Dispose();
|
_receiveThread = null;
|
||||||
_receiveTask = null;
|
|
||||||
|
|
||||||
_cts?.Dispose();
|
_cts?.Dispose();
|
||||||
_cts = null;
|
_cts = null;
|
||||||
|
|
||||||
_udpClient?.Dispose();
|
|
||||||
_udpClient = null;
|
_udpClient = null;
|
||||||
|
|
||||||
lock (_commandLock)
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
_currentCommand = null;
|
_currentCommand = null;
|
||||||
|
_lastSentCommand = null;
|
||||||
|
CompleteCommandQueueLocked();
|
||||||
_commandHistoryForTests = null;
|
_commandHistoryForTests = null;
|
||||||
|
_hasPendingCommandForStart = false;
|
||||||
_motionStarted = false;
|
_motionStarted = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -248,40 +361,67 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
|
||||||
}
|
|
||||||
catch (AggregateException)
|
|
||||||
{
|
|
||||||
// 忽略取消异常。
|
|
||||||
}
|
|
||||||
|
|
||||||
_receiveTask?.Dispose();
|
|
||||||
_cts?.Dispose();
|
|
||||||
_udpClient?.Dispose();
|
_udpClient?.Dispose();
|
||||||
|
_receiveThread?.Join(TimeSpan.FromSeconds(1));
|
||||||
|
}
|
||||||
|
catch (ObjectDisposedException)
|
||||||
|
{
|
||||||
|
// 忽略释放期间的套接字关闭异常。
|
||||||
}
|
}
|
||||||
|
|
||||||
private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
|
_cts?.Dispose();
|
||||||
|
|
||||||
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
return new FanucJ519Command(
|
CompleteCommandQueueLocked();
|
||||||
sequence,
|
_hasPendingCommandForStart = false;
|
||||||
source.TargetJoints,
|
}
|
||||||
source.LastData,
|
}
|
||||||
source.ReadIoType,
|
|
||||||
source.ReadIoIndex,
|
private static void ConfigureProcessPriority()
|
||||||
source.ReadIoMask,
|
{
|
||||||
source.DataStyle,
|
try
|
||||||
source.WriteIoType,
|
{
|
||||||
source.WriteIoIndex,
|
var process = Process.GetCurrentProcess();
|
||||||
source.WriteIoMask,
|
if (process.PriorityClass < ProcessPriorityClass.High)
|
||||||
source.WriteIoValue);
|
{
|
||||||
|
process.PriorityClass = ProcessPriorityClass.High;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (Exception)
|
||||||
|
{
|
||||||
|
// 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 后台接收循环：持续接收 132B 响应并解析。
|
/// 配置 J519 UDP 套接字的低延迟参数。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
/// <param name="socket">已连接 FANUC 60015 的 UDP 套接字。</param>
|
||||||
|
private void ConfigureRealtimeSocket(Socket socket)
|
||||||
{
|
{
|
||||||
if (_udpClient is null)
|
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;
|
return;
|
||||||
}
|
}
|
||||||
@@ -289,43 +429,74 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
_logger?.LogInformation("J519 ReceiveLoop 启动");
|
_logger?.LogInformation("J519 ReceiveLoop 启动");
|
||||||
long receiveCount = 0;
|
long receiveCount = 0;
|
||||||
FanucJ519Response? lastLoggedResponse = null;
|
FanucJ519Response? lastLoggedResponse = null;
|
||||||
|
var receiveBuffer = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||||
|
var commandBuffer = new byte[FanucJ519Protocol.CommandPacketLength];
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
while (!cancellationToken.IsCancellationRequested)
|
while (!cancellationToken.IsCancellationRequested)
|
||||||
{
|
{
|
||||||
var result = await _udpClient.ReceiveAsync(cancellationToken).ConfigureAwait(false);
|
var received = udpClient.Client.Receive(receiveBuffer);
|
||||||
if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength)
|
if (received == FanucJ519Protocol.ResponsePacketLength)
|
||||||
{
|
{
|
||||||
var response = FanucJ519Protocol.ParseResponse(result.Buffer);
|
var receiveTicks = Stopwatch.GetTimestamp();
|
||||||
|
var response = FanucJ519Protocol.ParseResponse(receiveBuffer);
|
||||||
|
|
||||||
|
// 先按状态包节拍回发命令，再做低频日志处理，减少受信周期内的非必要工作。
|
||||||
|
if (response.AcceptsCommand)
|
||||||
|
{
|
||||||
|
SendCommandForStatus(udpClient.Client, response, commandBuffer, receiveTicks);
|
||||||
|
}
|
||||||
|
|
||||||
lock (_responseLock)
|
lock (_responseLock)
|
||||||
{
|
{
|
||||||
_latestResponse = response;
|
_latestResponse = response;
|
||||||
}
|
}
|
||||||
|
|
||||||
receiveCount++;
|
receiveCount++;
|
||||||
await SendCommandForStatusAsync(response, cancellationToken).ConfigureAwait(false);
|
|
||||||
|
|
||||||
// 仅在状态变化时记录 Info，避免高频日志。
|
// 仅在状态变化时记录 Info，避免高频日志。
|
||||||
if (lastLoggedResponse is null
|
if (lastLoggedResponse is null
|
||||||
|| lastLoggedResponse.Status != response.Status
|
|| lastLoggedResponse.Status != response.Status
|
||||||
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|
||||||
|| lastLoggedResponse.SystemReady != response.SystemReady)
|
|| lastLoggedResponse.SystemReady != response.SystemReady
|
||||||
|
|| lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
|
||||||
{
|
{
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, sysrdy={SysRdy}, motion={Motion}, pose=[{Pose}], joints=[{Joints}]",
|
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, received={received}, SystemReady={SystemReady}, RobotInMotion={RobotInMotion}, pose=[{Pose}], joints=[{Joints}]",
|
||||||
response.Status,
|
response.Status,
|
||||||
response.Sequence,
|
response.Sequence,
|
||||||
response.AcceptsCommand,
|
response.AcceptsCommand,
|
||||||
|
response.ReceivedCommand,
|
||||||
response.SystemReady,
|
response.SystemReady,
|
||||||
response.RobotInMotion,
|
response.RobotInMotion,
|
||||||
string.Join(", ", response.Pose.Select(v => v.ToString("F1"))),
|
string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
|
||||||
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F2"))));
|
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F3"))));
|
||||||
|
|
||||||
|
var lastSentTargetJoints = GetLastSentTargetJointsLogText();
|
||||||
|
_logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
|
||||||
lastLoggedResponse = response;
|
lastLoggedResponse = response;
|
||||||
|
|
||||||
|
// 如果状态从AcceptsCommand true 变为false,说明机器人报错,清空队列
|
||||||
|
if (!response.AcceptsCommand)
|
||||||
|
{
|
||||||
|
lock (_commandLock)
|
||||||
|
{
|
||||||
|
_currentCommand = null;
|
||||||
|
CompleteCommandQueueLocked();
|
||||||
|
}
|
||||||
|
_logger?.LogWarning("J519 接收状态包显示机器人不可接受命令，已清空命令队列");
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
else if (receiveCount % 1000 == 0)
|
else if (receiveCount % 1000 == 0)
|
||||||
{
|
{
|
||||||
_logger?.LogDebug("J519 已接收 {Count} 个响应包", receiveCount);
|
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));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -338,27 +509,46 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
{
|
{
|
||||||
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
|
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
|
||||||
}
|
}
|
||||||
|
catch (SocketException ex) when (cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
_logger?.LogInformation(ex, "J519 ReceiveLoop 因取消关闭套接字退出，共接收 {Count} 个包", receiveCount);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="response">刚收到的状态包。</param>
|
/// <param name="socket">已连接的 UDP 套接字。</param>
|
||||||
/// <param name="cancellationToken">取消令牌。</param>
|
/// <param name="response">刚收到的机器人状态包。</param>
|
||||||
private async Task SendCommandForStatusAsync(FanucJ519Response response, CancellationToken cancellationToken)
|
/// <param name="commandBuffer">可复用的 64B 命令包缓冲区。</param>
|
||||||
|
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
|
||||||
|
private void SendCommandForStatus(Socket socket, FanucJ519Response response, byte[] commandBuffer, long receiveTicks)
|
||||||
{
|
{
|
||||||
var udpClient = _udpClient;
|
|
||||||
if (udpClient is null)
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
FanucJ519Command? command;
|
FanucJ519Command? command;
|
||||||
|
var willDrainQueue = false;
|
||||||
lock (_commandLock)
|
lock (_commandLock)
|
||||||
{
|
{
|
||||||
command = !_motionStarted || _currentCommand is null
|
if (!_motionStarted)
|
||||||
? null
|
{
|
||||||
: WithSequence(_currentCommand, response.Sequence);
|
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)
|
if (command is null)
|
||||||
@@ -366,7 +556,99 @@ public sealed class FanucJ519Client : IDisposable
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
uint sequenceToSend;
|
||||||
await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
|
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")));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -301,28 +301,45 @@ public static class FanucJ519Protocol
|
|||||||
ArgumentNullException.ThrowIfNull(command);
|
ArgumentNullException.ThrowIfNull(command);
|
||||||
|
|
||||||
var packet = new byte[CommandPacketLength];
|
var packet = new byte[CommandPacketLength];
|
||||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, sizeof(uint)), 1);
|
PackCommandPacket(command, command.Sequence, packet);
|
||||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1);
|
return packet;
|
||||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence);
|
}
|
||||||
|
|
||||||
|
/// <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[0x0c] = command.LastData;
|
||||||
packet[0x0d] = command.ReadIoType;
|
packet[0x0d] = command.ReadIoType;
|
||||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, sizeof(ushort)), command.ReadIoIndex);
|
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort)), command.ReadIoIndex);
|
||||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask);
|
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x10, sizeof(ushort)), command.ReadIoMask);
|
||||||
packet[0x12] = command.DataStyle;
|
packet[0x12] = command.DataStyle;
|
||||||
packet[0x13] = command.WriteIoType;
|
packet[0x13] = command.WriteIoType;
|
||||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x14, sizeof(ushort)), command.WriteIoIndex);
|
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x14, sizeof(ushort)), command.WriteIoIndex);
|
||||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask);
|
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x16, sizeof(ushort)), command.WriteIoMask);
|
||||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue);
|
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x18, sizeof(ushort)), command.WriteIoValue);
|
||||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0);
|
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x1a, sizeof(ushort)), 0);
|
||||||
|
|
||||||
// J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
|
// J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
|
||||||
for (var index = 0; index < 9; index++)
|
for (var index = 0; index < 9; index++)
|
||||||
{
|
{
|
||||||
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
|
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
|
||||||
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
|
BinaryPrimitives.WriteSingleBigEndian(packet.Slice(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
|
||||||
}
|
}
|
||||||
|
|
||||||
return packet;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|||||||
@@ -52,12 +52,12 @@ public sealed class FanucStateClientOptions
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取或设置重连等待时间的上限。
|
/// 获取或设置重连等待时间的上限。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(2);
|
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(5);
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取或设置单次 TCP 建连允许的最长时间。
|
/// 获取或设置单次 TCP 建连允许的最长时间。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(2);
|
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(5);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|||||||
@@ -154,12 +154,12 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状。
|
/// 兼容旧 `EnableRobot(buffer_size=4)` 参数形状。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="buffer_size">控制器执行缓冲区大小。</param>
|
/// <param name="buffer_size">控制器执行缓冲区大小。</param>
|
||||||
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
|
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
|
||||||
[HttpGet("/enable_robot/")]
|
[HttpGet("/enable_robot/")]
|
||||||
public IActionResult EnableRobot([FromQuery] int buffer_size = 2)
|
public IActionResult EnableRobot([FromQuery] int buffer_size = 4)
|
||||||
{
|
{
|
||||||
_logger.LogInformation("EnableRobot 调用: buffer_size={BufferSize}", buffer_size);
|
_logger.LogInformation("EnableRobot 调用: buffer_size={BufferSize}", buffer_size);
|
||||||
try
|
try
|
||||||
@@ -709,7 +709,7 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
data.robot_name, data.robot_ip, data.sim, data.server_ip, data.port);
|
data.robot_name, data.robot_ip, data.sim, data.server_ip, data.port);
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_compatService.ConnectServer(data.server_ip, data.port);
|
|
||||||
_compatService.SetUpRobot(data.robot_name);
|
_compatService.SetUpRobot(data.robot_name);
|
||||||
if (!_compatService.IsSetUp)
|
if (!_compatService.IsSetUp)
|
||||||
{
|
{
|
||||||
@@ -719,7 +719,7 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
|
|
||||||
_compatService.SetActiveController(data.sim);
|
_compatService.SetActiveController(data.sim);
|
||||||
_compatService.Connect(data.robot_ip);
|
_compatService.Connect(data.robot_ip);
|
||||||
_compatService.EnableRobot(2);
|
_compatService.EnableRobot(8);
|
||||||
_logger.LogInformation("InitMpcRobot 成功: robot_name={RobotName}", data.robot_name);
|
_logger.LogInformation("InitMpcRobot 成功: robot_name={RobotName}", data.robot_name);
|
||||||
return Ok(new { message = "init_Success", returnCode = 0 });
|
return Ok(new { message = "init_Success", returnCode = 0 });
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,10 +1,9 @@
|
|||||||
using Flyshot.Core.Config;
|
using Flyshot.Core.Config;
|
||||||
using Flyshot.Core.Domain;
|
|
||||||
|
|
||||||
namespace Flyshot.Core.Tests;
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 锁定 Task 3 的兼容输入行为，确保旧配置、.robot 元数据和路径策略都能被稳定加载。
|
/// 锁定 Task 3 的兼容输入行为，确保旧配置、JSON 模型元数据和路径策略都能被稳定加载。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class ConfigCompatibilityTests
|
public sealed class ConfigCompatibilityTests
|
||||||
{
|
{
|
||||||
@@ -25,6 +24,7 @@ public sealed class ConfigCompatibilityTests
|
|||||||
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
|
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
|
||||||
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
|
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
|
||||||
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
||||||
|
Assert.True(loaded.Robot.SmoothStartStopTiming);
|
||||||
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
|
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
|
||||||
|
|
||||||
var program = Assert.Contains("EOL10_EAU_0", loaded.Programs);
|
var program = Assert.Contains("EOL10_EAU_0", loaded.Programs);
|
||||||
@@ -73,6 +73,7 @@ public sealed class ConfigCompatibilityTests
|
|||||||
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
|
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
|
||||||
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
|
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
|
||||||
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
||||||
|
Assert.True(loaded.Robot.SmoothStartStopTiming);
|
||||||
Assert.Equal([0, 0, 0], program.OffsetValues);
|
Assert.Equal([0, 0, 0], program.OffsetValues);
|
||||||
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
|
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
|
||||||
}
|
}
|
||||||
@@ -123,13 +124,53 @@ public sealed class ConfigCompatibilityTests
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
|
/// 验证 RobotConfig.json 可以关闭飞拍执行前的二次平滑起停时间重映射。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void RobotConfigLoader_LoadsSmoothStartStopTimingSwitch()
|
||||||
|
{
|
||||||
|
var tempRoot = CreateTempDirectory();
|
||||||
|
try
|
||||||
|
{
|
||||||
|
var configPath = Path.Combine(tempRoot, "legacy.json");
|
||||||
|
File.WriteAllText(
|
||||||
|
configPath,
|
||||||
|
"""
|
||||||
|
{
|
||||||
|
"robot": {
|
||||||
|
"use_do": true,
|
||||||
|
"io_keep_cycles": 2,
|
||||||
|
"acc_limit": 1.0,
|
||||||
|
"jerk_limit": 1.0,
|
||||||
|
"smooth_start_stop_timing": false
|
||||||
|
},
|
||||||
|
"flying_shots": {
|
||||||
|
"demo": {
|
||||||
|
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
|
||||||
|
"shot_flags": [false, false, false, false]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
""");
|
||||||
|
|
||||||
|
var loaded = new RobotConfigLoader().Load(configPath);
|
||||||
|
|
||||||
|
Assert.False(loaded.Robot.SmoothStartStopTiming);
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
Directory.Delete(tempRoot, recursive: true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 JSON 模型解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
|
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
|
||||||
{
|
{
|
||||||
var workspaceRoot = GetWorkspaceRoot();
|
var replacementRoot = GetReplacementRoot();
|
||||||
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
|
||||||
|
|
||||||
var profile = new RobotModelLoader().LoadProfile(modelPath);
|
var profile = new RobotModelLoader().LoadProfile(modelPath);
|
||||||
|
|
||||||
@@ -147,13 +188,13 @@ public sealed class ConfigCompatibilityTests
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。
|
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到 JSON 模型关节限制上。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
|
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
|
||||||
{
|
{
|
||||||
var workspaceRoot = GetWorkspaceRoot();
|
var replacementRoot = GetReplacementRoot();
|
||||||
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
|
||||||
|
|
||||||
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
|
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
|
||||||
|
|
||||||
@@ -161,6 +202,28 @@ public sealed class ConfigCompatibilityTests
|
|||||||
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 JSON 模型可一次解析后同时生成规划约束视图和运动学几何视图。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void RobotModelLoader_LoadsProfileAndKinematics_FromSingleParse()
|
||||||
|
{
|
||||||
|
var replacementRoot = GetReplacementRoot();
|
||||||
|
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
|
||||||
|
|
||||||
|
var loaded = new RobotModelLoader().LoadProfileAndKinematics(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
|
||||||
|
|
||||||
|
Assert.Equal("FANUC_LR_Mate_200iD_7L", loaded.Profile.Name);
|
||||||
|
Assert.Equal(modelPath, loaded.Profile.ModelPath);
|
||||||
|
Assert.Equal(6, loaded.Profile.DegreesOfFreedom);
|
||||||
|
Assert.Equal(14.905, loaded.Profile.JointLimits[2].AccelerationLimit, precision: 3);
|
||||||
|
Assert.Equal(62.115, loaded.Profile.JointLimits[2].JerkLimit, precision: 3);
|
||||||
|
|
||||||
|
Assert.Equal("FANUC_LR_Mate_200iD_7L", loaded.KinematicsModel.Name);
|
||||||
|
Assert.True(loaded.KinematicsModel.Joints.Count >= loaded.Profile.DegreesOfFreedom);
|
||||||
|
Assert.Contains(loaded.KinematicsModel.Joints, static joint => joint.Name == "Joint3" && joint.CoupleMaster == "Joint2");
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证路径兼容层只从当前服务配置目录解析相对配置，并按平台策略生成默认用户数据目录。
|
/// 验证路径兼容层只从当前服务配置目录解析相对配置，并按平台策略生成默认用户数据目录。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -34,7 +34,7 @@ public sealed class ControllerClientCompatConfigRootTests
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证机器人目录优先从显式 ConfigRoot/Models 加载 .robot 文件。
|
/// 验证机器人目录优先从显式 ConfigRoot/Models 加载现场 JSON 模型文件。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public void ControllerClientCompatRobotCatalog_LoadsModelFromConfigRootModels()
|
public void ControllerClientCompatRobotCatalog_LoadsModelFromConfigRootModels()
|
||||||
@@ -48,7 +48,7 @@ public sealed class ControllerClientCompatConfigRootTests
|
|||||||
|
|
||||||
var profile = catalog.LoadProfile("FANUC_LR_Mate_200iD");
|
var profile = catalog.LoadProfile("FANUC_LR_Mate_200iD");
|
||||||
|
|
||||||
Assert.Equal(Path.Combine(configRoot, "Models", "LR_Mate_200iD_7L.robot"), profile.ModelPath);
|
Assert.Equal(Path.Combine(configRoot, "Models", "LR_Mate_200iD_7L.json"), profile.ModelPath);
|
||||||
}
|
}
|
||||||
finally
|
finally
|
||||||
{
|
{
|
||||||
@@ -117,15 +117,15 @@ public sealed class ControllerClientCompatConfigRootTests
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 复制仓库内已固化的现场机器人模型到临时 Config/Models 目录。
|
/// 复制仓库内已固化的现场机器人 JSON 模型到临时 Config/Models 目录。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private static void CopySampleRobotModel(string configRoot)
|
private static void CopySampleRobotModel(string configRoot)
|
||||||
{
|
{
|
||||||
var modelDir = Path.Combine(configRoot, "Models");
|
var modelDir = Path.Combine(configRoot, "Models");
|
||||||
Directory.CreateDirectory(modelDir);
|
Directory.CreateDirectory(modelDir);
|
||||||
File.Copy(
|
File.Copy(
|
||||||
Path.Combine(GetReplacementRoot(), "Config", "Models", "LR_Mate_200iD_7L.robot"),
|
Path.Combine(GetReplacementRoot(), "Config", "Models", "LR_Mate_200iD_7L.json"),
|
||||||
Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
|
Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
|
|||||||
@@ -99,7 +99,7 @@ public sealed class FanucCommandClientTests : IDisposable
|
|||||||
{
|
{
|
||||||
using var client = new FanucCommandClient();
|
using var client = new FanucCommandClient();
|
||||||
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
|
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
|
||||||
var responseFrame = Convert.FromHexString("646f7a000000160000200300000000000000017a6f64");
|
var responseFrame = Convert.FromHexString("646f7a000000160000200300000001000000007a6f64");
|
||||||
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||||
|
|
||||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|||||||
@@ -3,6 +3,9 @@ using Flyshot.ControllerClientCompat;
|
|||||||
using Flyshot.Core.Config;
|
using Flyshot.Core.Config;
|
||||||
using Flyshot.Runtime.Fanuc;
|
using Flyshot.Runtime.Fanuc;
|
||||||
using Flyshot.Runtime.Fanuc.Protocol;
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
using System.Buffers.Binary;
|
||||||
|
using System.Net;
|
||||||
|
using System.Net.Sockets;
|
||||||
using System.Reflection;
|
using System.Reflection;
|
||||||
|
|
||||||
namespace Flyshot.Core.Tests;
|
namespace Flyshot.Core.Tests;
|
||||||
@@ -12,12 +15,12 @@ namespace Flyshot.Core.Tests;
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class FanucControllerRuntimeDenseTests
|
public sealed class FanucControllerRuntimeDenseTests
|
||||||
{
|
{
|
||||||
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
|
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||||||
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
|
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||||||
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
|
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
/// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列，并输出角度制目标。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Fact]
|
[Fact]
|
||||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
||||||
@@ -59,6 +62,260 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
Assert.Equal(5, commands.Count);
|
Assert.Equal(5, commands.Count);
|
||||||
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
|
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
|
||||||
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
||||||
|
Assert.False(j519Client.IsCommandQueueDrainedForTests());
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证真机稠密轨迹接口会等待 J519 队列被状态包实际取完后再返回。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task ExecuteTrajectory_WithDenseWaypoints_RealMode_WaitsForJ519QueueToDrainBeforeReturning()
|
||||||
|
{
|
||||||
|
using var server = new UdpClient(0);
|
||||||
|
using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(5));
|
||||||
|
using var commandClient = new FanucCommandClient();
|
||||||
|
using var stateClient = new FanucStateClient();
|
||||||
|
using var j519Client = new FanucJ519Client();
|
||||||
|
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||||
|
var robot = TestRobotFactory.CreateRobotProfile();
|
||||||
|
var port = ((IPEndPoint)server.Client.LocalEndPoint!).Port;
|
||||||
|
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||||
|
j519Client.EnableCommandHistoryForTests();
|
||||||
|
await j519Client.ConnectAsync("127.0.0.1", port, cts.Token);
|
||||||
|
var initResult = await server.ReceiveAsync(cts.Token);
|
||||||
|
j519Client.StartMotion();
|
||||||
|
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||||
|
|
||||||
|
var denseTrajectory = new[]
|
||||||
|
{
|
||||||
|
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||||
|
};
|
||||||
|
var result = new TrajectoryResult(
|
||||||
|
programName: "wait-drain",
|
||||||
|
method: PlanningMethod.Icsp,
|
||||||
|
isValid: true,
|
||||||
|
duration: TimeSpan.FromSeconds(0.016),
|
||||||
|
shotEvents: Array.Empty<ShotEvent>(),
|
||||||
|
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||||
|
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||||
|
failureReason: null,
|
||||||
|
usedCache: false,
|
||||||
|
originalWaypointCount: 4,
|
||||||
|
plannedWaypointCount: 4,
|
||||||
|
denseJointTrajectory: denseTrajectory);
|
||||||
|
|
||||||
|
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
|
||||||
|
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
|
||||||
|
|
||||||
|
// 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
|
||||||
|
Assert.False(executeTask.IsCompleted);
|
||||||
|
|
||||||
|
for (uint sequence = 100; sequence < 103; sequence++)
|
||||||
|
{
|
||||||
|
await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token);
|
||||||
|
_ = await server.ReceiveAsync(cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
await executeTask.WaitAsync(TimeSpan.FromSeconds(2), cts.Token);
|
||||||
|
Assert.True(j519Client.IsCommandQueueDrainedForTests());
|
||||||
|
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证真机稠密发送会把实际下发给机器人的整条点位与点间跃度按时间目录落盘。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_WritesActualSentPointsAndJerkStats()
|
||||||
|
{
|
||||||
|
using var commandClient = new FanucCommandClient();
|
||||||
|
using var stateClient = new FanucStateClient();
|
||||||
|
using var j519Client = new FanucJ519Client();
|
||||||
|
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||||
|
var robot = TestRobotFactory.CreateRobotProfile();
|
||||||
|
var programName = $"dense-send-{Guid.NewGuid():N}";
|
||||||
|
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", programName);
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||||
|
j519Client.EnableCommandHistoryForTests();
|
||||||
|
ForceRealModeEnabled(runtime, speedRatio: 0.5);
|
||||||
|
|
||||||
|
var denseTrajectory = new[]
|
||||||
|
{
|
||||||
|
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||||
|
};
|
||||||
|
|
||||||
|
var result = new TrajectoryResult(
|
||||||
|
programName: programName,
|
||||||
|
method: PlanningMethod.Icsp,
|
||||||
|
isValid: true,
|
||||||
|
duration: TimeSpan.FromSeconds(0.016),
|
||||||
|
shotEvents: Array.Empty<ShotEvent>(),
|
||||||
|
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||||
|
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||||
|
failureReason: null,
|
||||||
|
usedCache: false,
|
||||||
|
originalWaypointCount: 4,
|
||||||
|
plannedWaypointCount: 4,
|
||||||
|
denseJointTrajectory: denseTrajectory);
|
||||||
|
|
||||||
|
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||||
|
WaitUntilIdle(runtime);
|
||||||
|
|
||||||
|
var commands = j519Client.GetCommandHistoryForTests();
|
||||||
|
var runDir = GetSingleDenseSendRunDirectory(outputRoot);
|
||||||
|
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
|
||||||
|
var timingPath = Path.Combine(runDir, "ActualSendTiming.txt");
|
||||||
|
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
|
||||||
|
|
||||||
|
Assert.True(File.Exists(pointsPath));
|
||||||
|
Assert.True(File.Exists(timingPath));
|
||||||
|
Assert.True(File.Exists(jerkPath));
|
||||||
|
|
||||||
|
var pointLines = File.ReadAllLines(pointsPath);
|
||||||
|
var timingLines = File.ReadAllLines(timingPath);
|
||||||
|
var jerkLines = File.ReadAllLines(jerkPath);
|
||||||
|
Assert.Equal(commands.Count, pointLines.Length);
|
||||||
|
Assert.Equal(commands.Count, timingLines.Length);
|
||||||
|
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
|
||||||
|
|
||||||
|
var firstColumns = ParseColumns(pointLines[0]);
|
||||||
|
var secondColumns = ParseColumns(pointLines[1]);
|
||||||
|
var lastColumns = ParseColumns(pointLines[^1]);
|
||||||
|
Assert.Equal(9, firstColumns.Length);
|
||||||
|
Assert.Equal(9, lastColumns.Length);
|
||||||
|
Assert.Equal(0.0, firstColumns[0], precision: 6);
|
||||||
|
Assert.Equal(0.008, secondColumns[0], precision: 6);
|
||||||
|
Assert.Equal(180.0, lastColumns[1], precision: 6);
|
||||||
|
|
||||||
|
var firstTimingColumns = ParseColumns(timingLines[0]);
|
||||||
|
var secondTimingColumns = ParseColumns(timingLines[1]);
|
||||||
|
var lastTimingColumns = ParseColumns(timingLines[^1]);
|
||||||
|
Assert.Equal(4, firstTimingColumns.Length);
|
||||||
|
Assert.Equal(0.0, firstTimingColumns[0], precision: 6);
|
||||||
|
Assert.Equal(0.0, firstTimingColumns[1], precision: 6);
|
||||||
|
Assert.Equal(0.0, firstTimingColumns[2], precision: 6);
|
||||||
|
Assert.Equal(0.5, firstTimingColumns[3], precision: 6);
|
||||||
|
Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
|
||||||
|
Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
|
||||||
|
Assert.Equal(0.004, secondTimingColumns[2], precision: 6);
|
||||||
|
Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
|
||||||
|
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
|
||||||
|
|
||||||
|
var firstJerkColumns = ParseColumns(jerkLines[0]);
|
||||||
|
Assert.Equal(10, firstJerkColumns.Length);
|
||||||
|
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
|
||||||
|
Assert.Equal(0.008, firstJerkColumns[2], precision: 6);
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
if (Directory.Exists(outputRoot))
|
||||||
|
{
|
||||||
|
Directory.Delete(outputRoot, recursive: true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 轨迹执行一次完整 1x 稠密下发，
|
||||||
|
/// 并把 0.088s 报警窗口附近的实发时间、关节与跃度摘要落盘，便于继续对照现场报警帧。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_WritesDenseSendDebugWindowAtOneX()
|
||||||
|
{
|
||||||
|
using var commandClient = new FanucCommandClient();
|
||||||
|
using var stateClient = new FanucStateClient();
|
||||||
|
using var j519Client = new FanucJ519Client();
|
||||||
|
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||||
|
var fixture = LoadUttcMs11RuntimeFixture();
|
||||||
|
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||||
|
var bundle = orchestrator.PlanUploadedFlyshot(
|
||||||
|
fixture.Robot,
|
||||||
|
fixture.Uploaded,
|
||||||
|
settings: fixture.Settings);
|
||||||
|
var result = WithUniqueProgramName(bundle.Result, $"UTTC_MS11_legacyfit_{Guid.NewGuid():N}");
|
||||||
|
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", result.ProgramName);
|
||||||
|
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
|
||||||
|
var beforeRunDirectories = Directory.Exists(denseSendRoot)
|
||||||
|
? Directory.GetDirectories(denseSendRoot).ToHashSet(StringComparer.OrdinalIgnoreCase)
|
||||||
|
: new HashSet<string>(StringComparer.OrdinalIgnoreCase);
|
||||||
|
|
||||||
|
runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
|
||||||
|
j519Client.EnableCommandHistoryForTests();
|
||||||
|
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||||
|
|
||||||
|
runtime.ExecuteTrajectory(result, result.DenseJointTrajectory![0].Skip(1).ToArray());
|
||||||
|
WaitUntilIdle(runtime);
|
||||||
|
|
||||||
|
var runDirectory = GetNewDenseSendRunDirectory(outputRoot, beforeRunDirectories);
|
||||||
|
var pointsPath = Path.Combine(runDirectory, "ActualSendJointTraj.txt");
|
||||||
|
var timingPath = Path.Combine(runDirectory, "ActualSendTiming.txt");
|
||||||
|
var jerkPath = Path.Combine(runDirectory, "ActualSendJerkStats.txt");
|
||||||
|
var summaryPath = Path.Combine(runDirectory, "AlarmWindow_0p088s_Summary.txt");
|
||||||
|
|
||||||
|
Assert.True(File.Exists(pointsPath));
|
||||||
|
Assert.True(File.Exists(timingPath));
|
||||||
|
Assert.True(File.Exists(jerkPath));
|
||||||
|
|
||||||
|
var pointsLines = File.ReadAllLines(pointsPath);
|
||||||
|
var timingLines = File.ReadAllLines(timingPath);
|
||||||
|
var jerkLines = File.ReadAllLines(jerkPath);
|
||||||
|
|
||||||
|
Assert.NotEmpty(pointsLines);
|
||||||
|
Assert.NotEmpty(timingLines);
|
||||||
|
Assert.NotEmpty(jerkLines);
|
||||||
|
Assert.Equal(927, pointsLines.Length);
|
||||||
|
Assert.Equal(927, timingLines.Length);
|
||||||
|
|
||||||
|
var firstPoint = ParseColumns(pointsLines[0]);
|
||||||
|
var secondPoint = ParseColumns(pointsLines[1]);
|
||||||
|
Assert.Equal(0.0, firstPoint[0], precision: 6);
|
||||||
|
Assert.Equal(0.008, secondPoint[0], precision: 6);
|
||||||
|
|
||||||
|
var firstStepJ1 = Math.Abs(secondPoint[1] - firstPoint[1]);
|
||||||
|
Assert.True(firstStepJ1 > 1e-6, $"UTTC_MS11 实发首步不应被压成 0，actual={firstStepJ1:F9}deg");
|
||||||
|
|
||||||
|
const double targetSendTime = 0.088;
|
||||||
|
const double windowHalfWidth = 0.024;
|
||||||
|
var summaryLines = new List<string>
|
||||||
|
{
|
||||||
|
$"program={result.ProgramName}",
|
||||||
|
$"send_time_target_seconds={targetSendTime:F6}",
|
||||||
|
$"window_half_width_seconds={windowHalfWidth:F6}",
|
||||||
|
$"points_path={pointsPath}",
|
||||||
|
$"timing_path={timingPath}",
|
||||||
|
$"jerk_path={jerkPath}",
|
||||||
|
"timing_window:"
|
||||||
|
};
|
||||||
|
|
||||||
|
foreach (var line in timingLines.Select(ParseColumns))
|
||||||
|
{
|
||||||
|
var sendTime = line[1];
|
||||||
|
if (Math.Abs(sendTime - targetSendTime) <= windowHalfWidth + 1e-9)
|
||||||
|
{
|
||||||
|
summaryLines.Add(
|
||||||
|
$"timing send_index={line[0]:F0} send_time={line[1]:F6} trajectory_time={line[2]:F6} speed_ratio={line[3]:F6}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
summaryLines.Add("jerk_window:");
|
||||||
|
foreach (var line in jerkLines.Select(ParseColumns))
|
||||||
|
{
|
||||||
|
var endTime = line[1];
|
||||||
|
if (Math.Abs(endTime - targetSendTime) <= windowHalfWidth + 1e-9)
|
||||||
|
{
|
||||||
|
summaryLines.Add(
|
||||||
|
$"jerk end_time={line[1]:F6} dt={line[2]:F6} j1={line[3]:F6} j2={line[4]:F6} j3={line[5]:F6} j4={line[6]:F6} j5={line[7]:F6} j6={line[8]:F6} max_abs={line.Skip(3).Max(static value => Math.Abs(value)):F6}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
File.WriteAllLines(summaryPath, summaryLines);
|
||||||
|
Assert.True(File.Exists(summaryPath));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -135,9 +392,9 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
|
|
||||||
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
||||||
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
||||||
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
|
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||||
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
|
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||||
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
|
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||||
}
|
}
|
||||||
|
|
||||||
[Fact]
|
[Fact]
|
||||||
@@ -349,6 +606,58 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
|
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证稠密执行前若 J519 首次未就绪，会先尝试一次 EnableRobot，再在 500ms 后复查状态。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void EnsureJ519ReadyForDenseExecutionCore_RetriesEnableRobotOnceBeforePassing()
|
||||||
|
{
|
||||||
|
var responses = new Queue<FanucJ519Response?>(
|
||||||
|
[
|
||||||
|
CreateJ519Response(status: 0b0001, sequence: 11),
|
||||||
|
CreateJ519Response(status: 0b0101, sequence: 12)
|
||||||
|
]);
|
||||||
|
var enableRobotRetryCount = 0;
|
||||||
|
var waitCount = 0;
|
||||||
|
|
||||||
|
var exception = Record.Exception(
|
||||||
|
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
|
||||||
|
() => responses.Dequeue(),
|
||||||
|
() => enableRobotRetryCount++,
|
||||||
|
() => waitCount++));
|
||||||
|
|
||||||
|
Assert.Null(exception);
|
||||||
|
Assert.Equal(1, enableRobotRetryCount);
|
||||||
|
Assert.Equal(1, waitCount);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证重试 EnableRobot 之后若 J519 仍未就绪，会继续抛出带状态位的异常。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void EnsureJ519ReadyForDenseExecutionCore_ThrowsWhenStillNotReadyAfterRetry()
|
||||||
|
{
|
||||||
|
var responses = new Queue<FanucJ519Response?>(
|
||||||
|
[
|
||||||
|
CreateJ519Response(status: 0b0000, sequence: 21),
|
||||||
|
CreateJ519Response(status: 0b0010, sequence: 22)
|
||||||
|
]);
|
||||||
|
var enableRobotRetryCount = 0;
|
||||||
|
var waitCount = 0;
|
||||||
|
|
||||||
|
var exception = Assert.Throws<InvalidOperationException>(
|
||||||
|
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
|
||||||
|
() => responses.Dequeue(),
|
||||||
|
() => enableRobotRetryCount++,
|
||||||
|
() => waitCount++));
|
||||||
|
|
||||||
|
Assert.Equal(1, enableRobotRetryCount);
|
||||||
|
Assert.Equal(1, waitCount);
|
||||||
|
Assert.Contains("accept_cmd=False", exception.Message, StringComparison.Ordinal);
|
||||||
|
Assert.Contains("sysrdy=False", exception.Message, StringComparison.Ordinal);
|
||||||
|
Assert.Contains("seq=22", exception.Message, StringComparison.Ordinal);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
|
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -395,7 +704,7 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
}
|
}
|
||||||
|
|
||||||
[Fact]
|
[Fact]
|
||||||
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
|
public void MoveJointTrajectoryGenerator_GeneratesSmoothJerkLimitedPtpTrajectoryAtSpeedOne()
|
||||||
{
|
{
|
||||||
var robot = CreateMoveJointReferenceRobotProfile();
|
var robot = CreateMoveJointReferenceRobotProfile();
|
||||||
var startJoints = new[]
|
var startJoints = new[]
|
||||||
@@ -420,64 +729,46 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||||
var rows = result.DenseJointTrajectory!;
|
var rows = result.DenseJointTrajectory!;
|
||||||
|
|
||||||
Assert.Equal(41, rows.Count);
|
Assert.True(rows.Count > 41, $"平滑 MoveJoint 不应再固定输出 41 点，actual={rows.Count}。");
|
||||||
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
|
Assert.True(result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
|
||||||
|
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||||
|
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||||
|
AssertAllJointJerksWithinLimits(rows, robot);
|
||||||
|
}
|
||||||
|
|
||||||
var expectedAlpha = new[]
|
/// <summary>
|
||||||
|
/// 验证报警样本中的 MoveJoint 预移动作不会因为点到点时间律重采样而产生任一轴跃度尖峰。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void MoveJointTrajectoryGenerator_ActualAlarmSampleKeepsJointOneJerkWithinLimit()
|
||||||
{
|
{
|
||||||
0.000000000000,
|
var robot = CreateMoveJointRuntimeLimitRobotProfile();
|
||||||
0.000012196163,
|
var startJoints = new[]
|
||||||
0.000106156906,
|
{
|
||||||
0.000764380061,
|
DegreesToRadians(71.454618),
|
||||||
0.002550804028,
|
DegreesToRadians(0.688433),
|
||||||
0.006029689194,
|
DegreesToRadians(-1.074197),
|
||||||
0.011765134027,
|
DegreesToRadians(-0.869001),
|
||||||
0.020321400844,
|
DegreesToRadians(1.218057),
|
||||||
0.032262426551,
|
DegreesToRadians(0.547036)
|
||||||
0.048152469303,
|
};
|
||||||
0.068555498563,
|
var targetJoints = new[]
|
||||||
0.093895155669,
|
{
|
||||||
0.124210027377,
|
DegreesToRadians(60.546227),
|
||||||
0.159174512929,
|
DegreesToRadians(0.668344),
|
||||||
0.198230386318,
|
DegreesToRadians(-1.025155),
|
||||||
0.240813559900,
|
DegreesToRadians(-0.869105),
|
||||||
0.286359937276,
|
DegreesToRadians(1.231405),
|
||||||
0.334305411725,
|
DegreesToRadians(0.548197)
|
||||||
0.384085546646,
|
|
||||||
0.435136609163,
|
|
||||||
0.486894129077,
|
|
||||||
0.538794033110,
|
|
||||||
0.590272360135,
|
|
||||||
0.640764719629,
|
|
||||||
0.689707151220,
|
|
||||||
0.736535405849,
|
|
||||||
0.780685354316,
|
|
||||||
0.821592775628,
|
|
||||||
0.858693734065,
|
|
||||||
0.891423926949,
|
|
||||||
0.919286047395,
|
|
||||||
0.942156722091,
|
|
||||||
0.960255163676,
|
|
||||||
0.974119666692,
|
|
||||||
0.984314536393,
|
|
||||||
0.991403790959,
|
|
||||||
0.995951593494,
|
|
||||||
0.998522142663,
|
|
||||||
0.999679443354,
|
|
||||||
0.999987892657,
|
|
||||||
1.000000000000
|
|
||||||
};
|
};
|
||||||
|
|
||||||
for (var index = 0; index < rows.Count; index++)
|
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||||
{
|
var rows = result.DenseJointTrajectory!;
|
||||||
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
|
AssertAllJointJerksWithinLimits(rows, robot);
|
||||||
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
|
|
||||||
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
[Fact]
|
[Fact]
|
||||||
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
|
public void MoveJointTrajectoryGenerator_DoesNotShortenLimitDurationWhenSpeedRatioDoesNotDivideWindow()
|
||||||
{
|
{
|
||||||
var robot = CreateMoveJointReferenceRobotProfile();
|
var robot = CreateMoveJointReferenceRobotProfile();
|
||||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||||
@@ -486,7 +777,9 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||||
var rows = result.DenseJointTrajectory!;
|
var rows = result.DenseJointTrajectory!;
|
||||||
|
|
||||||
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
Assert.True(
|
||||||
|
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
|
||||||
|
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||||
}
|
}
|
||||||
@@ -513,12 +806,11 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
|
|
||||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||||
var rows = result.DenseJointTrajectory!;
|
var rows = result.DenseJointTrajectory!;
|
||||||
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
var expectedVelocityDuration = Math.PI * SmoothPtpVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||||||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
var expectedAccelerationDuration = Math.Sqrt(Math.PI * SmoothPtpAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||||||
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
var expectedJerkDuration = Math.Cbrt(Math.PI * SmoothPtpJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||||||
var expectedMinimumDuration = new[]
|
var expectedMinimumDuration = new[]
|
||||||
{
|
{
|
||||||
0.320,
|
|
||||||
expectedVelocityDuration,
|
expectedVelocityDuration,
|
||||||
expectedAccelerationDuration,
|
expectedAccelerationDuration,
|
||||||
expectedJerkDuration
|
expectedJerkDuration
|
||||||
@@ -585,6 +877,30 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 创建用于就绪状态测试的最小 J519 响应。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="status">待注入的 J519 状态位。</param>
|
||||||
|
/// <param name="sequence">待注入的响应序号。</param>
|
||||||
|
/// <returns>包含最小字段集合的测试响应。</returns>
|
||||||
|
private static FanucJ519Response CreateJ519Response(byte status, uint sequence)
|
||||||
|
{
|
||||||
|
return new FanucJ519Response(
|
||||||
|
messageType: 0,
|
||||||
|
version: 1,
|
||||||
|
sequence: sequence,
|
||||||
|
status: status,
|
||||||
|
readIoType: 0,
|
||||||
|
readIoIndex: 0,
|
||||||
|
readIoMask: 0,
|
||||||
|
readIoValue: 0,
|
||||||
|
timestamp: 0,
|
||||||
|
pose: new double[6],
|
||||||
|
externalAxes: new double[3],
|
||||||
|
jointDegrees: new double[6],
|
||||||
|
motorCurrents: new double[6]);
|
||||||
|
}
|
||||||
|
|
||||||
private static RobotProfile CreateMoveJointReferenceRobotProfile()
|
private static RobotProfile CreateMoveJointReferenceRobotProfile()
|
||||||
{
|
{
|
||||||
return new RobotProfile(
|
return new RobotProfile(
|
||||||
@@ -605,6 +921,111 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 创建当前运行配置下的 MoveJoint 限值模型，覆盖报警样本使用的 acc_limit/jerk_limit=0.74。
|
||||||
|
/// </summary>
|
||||||
|
private static RobotProfile CreateMoveJointRuntimeLimitRobotProfile()
|
||||||
|
{
|
||||||
|
const double limitScale = 0.74;
|
||||||
|
return new RobotProfile(
|
||||||
|
name: "FANUC_LR_Mate_200iD",
|
||||||
|
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||||||
|
degreesOfFreedom: 6,
|
||||||
|
jointLimits:
|
||||||
|
[
|
||||||
|
new JointLimit("J1", 6.45, 26.90 * limitScale, 224.22 * limitScale),
|
||||||
|
new JointLimit("J2", 5.41, 22.54 * limitScale, 187.86 * limitScale),
|
||||||
|
new JointLimit("J3", 7.15, 29.81 * limitScale, 248.46 * limitScale),
|
||||||
|
new JointLimit("J4", 9.59, 39.99 * limitScale, 333.30 * limitScale),
|
||||||
|
new JointLimit("J5", 9.51, 39.63 * limitScale, 330.27 * limitScale),
|
||||||
|
new JointLimit("J6", 17.45, 72.72 * limitScale, 606.01 * limitScale)
|
||||||
|
],
|
||||||
|
jointCouplings: Array.Empty<JointCoupling>(),
|
||||||
|
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||||
|
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 以发送周期上的二阶后向差分估算单轴最大绝对跃度，用来复现 ActualSendJerkStats 的判定口径。
|
||||||
|
/// </summary>
|
||||||
|
private static double MaxAbsJointJerk(IReadOnlyList<IReadOnlyList<double>> rows, int jointIndex)
|
||||||
|
{
|
||||||
|
double? previousTime = null;
|
||||||
|
double? previousPosition = null;
|
||||||
|
double? previousVelocity = null;
|
||||||
|
double? previousAcceleration = null;
|
||||||
|
var maxAbsJerk = 0.0;
|
||||||
|
|
||||||
|
foreach (var row in rows)
|
||||||
|
{
|
||||||
|
var currentTime = row[0];
|
||||||
|
var currentPosition = row[jointIndex + 1];
|
||||||
|
if (previousTime is not null && previousPosition is not null)
|
||||||
|
{
|
||||||
|
var dt = currentTime - previousTime.Value;
|
||||||
|
Assert.True(dt > 0.0, "MoveJoint 稠密轨迹时间戳必须严格递增。");
|
||||||
|
|
||||||
|
var velocity = (currentPosition - previousPosition.Value) / dt;
|
||||||
|
var acceleration = previousVelocity is null ? 0.0 : (velocity - previousVelocity.Value) / dt;
|
||||||
|
if (previousAcceleration is not null)
|
||||||
|
{
|
||||||
|
var jerk = (acceleration - previousAcceleration.Value) / dt;
|
||||||
|
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(jerk));
|
||||||
|
}
|
||||||
|
|
||||||
|
previousVelocity = velocity;
|
||||||
|
previousAcceleration = acceleration;
|
||||||
|
}
|
||||||
|
|
||||||
|
previousTime = currentTime;
|
||||||
|
previousPosition = currentPosition;
|
||||||
|
}
|
||||||
|
|
||||||
|
return maxAbsJerk;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证稠密 MoveJoint 轨迹按 8ms 差分后不会超过机器人每轴 jerk 限值。
|
||||||
|
/// </summary>
|
||||||
|
private static void AssertAllJointJerksWithinLimits(IReadOnlyList<IReadOnlyList<double>> rows, RobotProfile robot)
|
||||||
|
{
|
||||||
|
for (var jointIndex = 0; jointIndex < robot.DegreesOfFreedom; jointIndex++)
|
||||||
|
{
|
||||||
|
var peakJerk = MaxAbsJointJerk(rows, jointIndex);
|
||||||
|
var limit = robot.JointLimits[jointIndex].JerkLimit;
|
||||||
|
Assert.True(
|
||||||
|
peakJerk <= limit * 1.000001,
|
||||||
|
$"J{jointIndex + 1} jerk {peakJerk:F6} rad/s^3 exceeds limit {limit:F6} rad/s^3.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按 7 阶平滑点到点时间律的速度、加速度、jerk 形状系数计算理论最小时长。
|
||||||
|
/// </summary>
|
||||||
|
private static double ExpectedSmoothPtpDuration(
|
||||||
|
RobotProfile robot,
|
||||||
|
IReadOnlyList<double> startJoints,
|
||||||
|
IReadOnlyList<double> targetJoints)
|
||||||
|
{
|
||||||
|
var duration = 0.0;
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
private static double DegreesToRadians(double degrees)
|
private static double DegreesToRadians(double degrees)
|
||||||
{
|
{
|
||||||
return degrees * Math.PI / 180.0;
|
return degrees * Math.PI / 180.0;
|
||||||
@@ -631,6 +1052,128 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 等待测试条件成立，用于观察后台执行路径是否已经到达指定状态。
|
||||||
|
/// </summary>
|
||||||
|
private static async Task WaitUntilAsync(Func<bool> condition, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
|
||||||
|
while (DateTimeOffset.UtcNow < deadline)
|
||||||
|
{
|
||||||
|
cancellationToken.ThrowIfCancellationRequested();
|
||||||
|
if (condition())
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
await Task.Delay(1, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new TimeoutException("Timed out waiting for test condition.");
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
||||||
|
/// </summary>
|
||||||
|
private static async Task SendStatusPacketAsync(
|
||||||
|
UdpClient server,
|
||||||
|
IPEndPoint clientEndpoint,
|
||||||
|
uint sequence,
|
||||||
|
CancellationToken cancellationToken,
|
||||||
|
byte status = 0b0111)
|
||||||
|
{
|
||||||
|
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
|
||||||
|
responsePacket[0x0c] = status;
|
||||||
|
await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取一次测试执行生成的唯一稠密发送记录目录。
|
||||||
|
/// </summary>
|
||||||
|
private static string GetSingleDenseSendRunDirectory(string outputRoot)
|
||||||
|
{
|
||||||
|
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
|
||||||
|
Assert.True(Directory.Exists(denseSendRoot));
|
||||||
|
|
||||||
|
var runDirectories = Directory.GetDirectories(denseSendRoot);
|
||||||
|
Assert.Single(runDirectories);
|
||||||
|
return runDirectories[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取本次测试刚生成的新稠密发送记录目录，避免误读历史运行产物。
|
||||||
|
/// </summary>
|
||||||
|
private static string GetNewDenseSendRunDirectory(string outputRoot, IReadOnlySet<string> beforeRunDirectories)
|
||||||
|
{
|
||||||
|
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
|
||||||
|
Assert.True(Directory.Exists(denseSendRoot));
|
||||||
|
|
||||||
|
var newDirectories = Directory.GetDirectories(denseSendRoot)
|
||||||
|
.Where(path => !beforeRunDirectories.Contains(path))
|
||||||
|
.ToArray();
|
||||||
|
Assert.Single(newDirectories);
|
||||||
|
return newDirectories[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。
|
||||||
|
/// </summary>
|
||||||
|
private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
|
||||||
|
{
|
||||||
|
var configRoot = TestRobotFactory.GetConfigRoot();
|
||||||
|
var configPath = Path.Combine(configRoot, "RobotConfig.json");
|
||||||
|
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
|
||||||
|
var program = loaded.Programs["UTTC_MS11"];
|
||||||
|
var uploaded = new ControllerClientCompatUploadedTrajectory(
|
||||||
|
name: program.Name,
|
||||||
|
waypoints: program.Waypoints.Select(static waypoint => waypoint.Positions),
|
||||||
|
shotFlags: program.ShotFlags,
|
||||||
|
offsetValues: program.OffsetValues,
|
||||||
|
addressGroups: program.AddressGroups.Select(static group => group.Addresses));
|
||||||
|
var options = new ControllerClientCompatOptions
|
||||||
|
{
|
||||||
|
ConfigRoot = configRoot
|
||||||
|
};
|
||||||
|
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
|
||||||
|
.LoadProfile("FANUC_LR_Mate_200iD", loaded.Robot.AccLimitScale, loaded.Robot.JerkLimitScale);
|
||||||
|
|
||||||
|
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 复制一份结果并替换程序名，让 DenseSend 调试文件写入唯一目录，避免测试之间复用旧目录权限。
|
||||||
|
/// </summary>
|
||||||
|
private static TrajectoryResult WithUniqueProgramName(TrajectoryResult result, string programName)
|
||||||
|
{
|
||||||
|
return new TrajectoryResult(
|
||||||
|
programName: programName,
|
||||||
|
method: result.Method,
|
||||||
|
isValid: result.IsValid,
|
||||||
|
duration: result.Duration,
|
||||||
|
shotEvents: result.ShotEvents,
|
||||||
|
triggerTimeline: result.TriggerTimeline,
|
||||||
|
artifacts: result.Artifacts,
|
||||||
|
failureReason: result.FailureReason,
|
||||||
|
usedCache: result.UsedCache,
|
||||||
|
originalWaypointCount: result.OriginalWaypointCount,
|
||||||
|
plannedWaypointCount: result.PlannedWaypointCount,
|
||||||
|
denseJointTrajectory: result.DenseJointTrajectory);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 解析空格分隔的纯文本数值列。
|
||||||
|
/// </summary>
|
||||||
|
private static double[] ParseColumns(string line)
|
||||||
|
{
|
||||||
|
return line
|
||||||
|
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
|
||||||
|
.Select(static value => double.Parse(value))
|
||||||
|
.ToArray();
|
||||||
|
}
|
||||||
|
|
||||||
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
||||||
{
|
{
|
||||||
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
||||||
@@ -675,4 +1218,13 @@ public sealed class FanucControllerRuntimeDenseTests
|
|||||||
Assert.NotNull(field);
|
Assert.NotNull(field);
|
||||||
field.SetValue(client, response);
|
field.SetValue(client, response);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
|
||||||
|
/// </summary>
|
||||||
|
private sealed record UttcMs11RuntimeFixture(
|
||||||
|
string ConfigRoot,
|
||||||
|
CompatibilityRobotSettings Settings,
|
||||||
|
ControllerClientCompatUploadedTrajectory Uploaded,
|
||||||
|
RobotProfile Robot);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -0,0 +1,32 @@
|
|||||||
|
using Flyshot.Runtime.Common;
|
||||||
|
using Flyshot.Runtime.Fanuc;
|
||||||
|
using Microsoft.Extensions.DependencyInjection;
|
||||||
|
using System.Reflection;
|
||||||
|
|
||||||
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 FANUC 运行时通过依赖注入解析时，能够稳定拿到日志依赖。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucControllerRuntimeLoggingRegistrationTests
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 验证宿主按 IControllerRuntime 解析 FANUC 运行时时，不会因为可选日志参数而退回到无日志实例。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void ServiceProvider_Resolves_FanucControllerRuntime_WithLogger()
|
||||||
|
{
|
||||||
|
var services = new ServiceCollection();
|
||||||
|
services.AddLogging();
|
||||||
|
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
|
||||||
|
|
||||||
|
using var serviceProvider = services.BuildServiceProvider();
|
||||||
|
|
||||||
|
var runtime = serviceProvider.GetRequiredService<IControllerRuntime>();
|
||||||
|
var concreteRuntime = Assert.IsType<FanucControllerRuntime>(runtime);
|
||||||
|
var loggerField = typeof(FanucControllerRuntime).GetField("_logger", BindingFlags.Instance | BindingFlags.NonPublic);
|
||||||
|
|
||||||
|
Assert.NotNull(loggerField);
|
||||||
|
Assert.NotNull(loggerField!.GetValue(concreteRuntime));
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -2,6 +2,7 @@ using System.Buffers.Binary;
|
|||||||
using System.Net;
|
using System.Net;
|
||||||
using System.Net.Sockets;
|
using System.Net.Sockets;
|
||||||
using Flyshot.Runtime.Fanuc.Protocol;
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
using Microsoft.Extensions.Logging;
|
||||||
|
|
||||||
namespace Flyshot.Core.Tests;
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
@@ -162,6 +163,29 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
await client.StopMotionAsync(_cts.Token);
|
await client.StopMotionAsync(_cts.Token);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证配置 J519 buffer size 后，实际回发命令序号会在状态包序号基础上增加该缓冲深度。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StartMotion_AddsConfiguredBufferSizeToStatusSequence()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
client.SetSequenceBufferSize(8);
|
||||||
|
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||||
|
client.StartMotion();
|
||||||
|
|
||||||
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
|
||||||
|
|
||||||
|
var result = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||||
|
Assert.Equal(108u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
|
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -193,6 +217,46 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
|
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证预装命令队列会被机器人状态包逐帧出队，队列耗尽后继续保持最后目标。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StartMotion_DequeuesPreparedCommandsForStatusPacketsAndHoldsLastCommand()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
client.StartMotion();
|
||||||
|
client.LoadCommandQueue(
|
||||||
|
[
|
||||||
|
new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
|
||||||
|
new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
|
||||||
|
new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0])
|
||||||
|
]);
|
||||||
|
|
||||||
|
var packets = new List<byte[]>();
|
||||||
|
for (uint sequence = 700; sequence < 704; sequence++)
|
||||||
|
{
|
||||||
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
|
||||||
|
var result = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
packets.Add(result.Buffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
|
||||||
|
var sequences = packets
|
||||||
|
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
|
||||||
|
.ToArray();
|
||||||
|
var firstJointTargets = packets
|
||||||
|
.Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)))
|
||||||
|
.ToArray();
|
||||||
|
|
||||||
|
Assert.Equal([700u, 701u, 702u, 703u], sequences);
|
||||||
|
Assert.Equal([1.0f, 2.0f, 3.0f, 3.0f], firstJointTargets);
|
||||||
|
Assert.True(client.IsCommandQueueDrainedForTests());
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
|
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -233,6 +297,61 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
await client.StopMotionAsync(_cts.Token);
|
await client.StopMotionAsync(_cts.Token);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证没有显式目标时，会使用最近一帧状态反馈里的关节角持续构造 hold 命令。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StartMotion_WithoutExplicitCommand_HoldsLatestResponseJointDegrees()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
client.StartMotion();
|
||||||
|
|
||||||
|
await SendStatusPacketAsync(
|
||||||
|
initResult.RemoteEndPoint,
|
||||||
|
sequence: 401,
|
||||||
|
jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
|
||||||
|
var holdPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
||||||
|
|
||||||
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, holdPacket.Buffer.Length);
|
||||||
|
Assert.Equal(401u, BinaryPrimitives.ReadUInt32BigEndian(holdPacket.Buffer.AsSpan(0x08, 4)));
|
||||||
|
Assert.Equal(10.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||||
|
Assert.Equal(20.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
|
||||||
|
Assert.Equal(60.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x30, 4)), precision: 6);
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证存在显式目标时，状态反馈生成的 hold 命令不会覆盖当前待发送目标。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StartMotion_WithExplicitCommand_PrefersExplicitTargetOverHoldCommand()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
|
||||||
|
client.StartMotion();
|
||||||
|
|
||||||
|
await SendStatusPacketAsync(
|
||||||
|
initResult.RemoteEndPoint,
|
||||||
|
sequence: 402,
|
||||||
|
jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
|
||||||
|
var commandPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
||||||
|
|
||||||
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandPacket.Buffer.Length);
|
||||||
|
Assert.Equal(402u, BinaryPrimitives.ReadUInt32BigEndian(commandPacket.Buffer.AsSpan(0x08, 4)));
|
||||||
|
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||||
|
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
|
||||||
|
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x30, 4)), precision: 6);
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
|
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -287,16 +406,119 @@ public sealed class FanucJ519ClientTests : IDisposable
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证状态变化日志会附带最近一次实际发送的目标关节轴，便于联调时对照控制目标。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task ReceiveLoop_LogsLastSentTargetJointsWhenStatusChanges()
|
||||||
|
{
|
||||||
|
var logger = new CapturingLogger<FanucJ519Client>();
|
||||||
|
using var client = new FanucJ519Client(logger);
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
|
||||||
|
client.StartMotion();
|
||||||
|
|
||||||
|
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 42);
|
||||||
|
_ = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
await Task.Delay(200, _cts.Token);
|
||||||
|
|
||||||
|
Assert.Contains(
|
||||||
|
logger.Entries,
|
||||||
|
entry => entry.Level == LogLevel.Information
|
||||||
|
&& entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal)
|
||||||
|
&& entry.Message.Contains("1.00000, 2.00000, 3.00000, 4.00000, 5.00000, 6.00000", StringComparison.Ordinal));
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence)
|
private async Task SendStatusPacketAsync(
|
||||||
|
IPEndPoint clientEndpoint,
|
||||||
|
uint sequence,
|
||||||
|
IReadOnlyList<double>? jointDegrees = null)
|
||||||
{
|
{
|
||||||
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
|
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
|
||||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
|
||||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
|
||||||
responsePacket[0x0c] = 15;
|
responsePacket[0x0c] = 15;
|
||||||
|
|
||||||
|
if (jointDegrees is not null)
|
||||||
|
{
|
||||||
|
for (var index = 0; index < Math.Min(6, jointDegrees.Count); index++)
|
||||||
|
{
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(
|
||||||
|
responsePacket.AsSpan(0x3c + index * sizeof(float), sizeof(float)),
|
||||||
|
(float)jointDegrees[index]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
|
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 收集测试过程中的结构化日志，便于断言运行期输出内容。
|
||||||
|
/// </summary>
|
||||||
|
private sealed class CapturingLogger<T> : ILogger<T>
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取已记录的日志条目。
|
||||||
|
/// </summary>
|
||||||
|
public List<LogEntry> Entries { get; } = [];
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 开始日志作用域；当前测试无需作用域，直接返回空对象。
|
||||||
|
/// </summary>
|
||||||
|
public IDisposable BeginScope<TState>(TState state)
|
||||||
|
where TState : notnull
|
||||||
|
{
|
||||||
|
return NullScope.Instance;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 指示所有日志级别均启用，便于测试完整捕获输出。
|
||||||
|
/// </summary>
|
||||||
|
public bool IsEnabled(LogLevel logLevel)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 记录一条格式化后的日志消息。
|
||||||
|
/// </summary>
|
||||||
|
public void Log<TState>(
|
||||||
|
LogLevel logLevel,
|
||||||
|
EventId eventId,
|
||||||
|
TState state,
|
||||||
|
Exception? exception,
|
||||||
|
Func<TState, Exception?, string> formatter)
|
||||||
|
{
|
||||||
|
Entries.Add(new LogEntry(logLevel, formatter(state, exception)));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示一次日志记录。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record LogEntry(LogLevel Level, string Message);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供空日志作用域，避免测试中额外分配。
|
||||||
|
/// </summary>
|
||||||
|
private sealed class NullScope : IDisposable
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取单例空作用域。
|
||||||
|
/// </summary>
|
||||||
|
public static NullScope Instance { get; } = new();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 释放空作用域；无需实际动作。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ public sealed class FanucProtocolTests
|
|||||||
var stopResponse = FanucCommandProtocol.ParseResultResponse(
|
var stopResponse = FanucCommandProtocol.ParseResultResponse(
|
||||||
Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
|
Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
|
||||||
var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
|
var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
|
||||||
Convert.FromHexString("646f7a000000160000200300000000000000017a6f64"));
|
Convert.FromHexString("646f7a000000160000200300000001000000007a6f64"));
|
||||||
|
|
||||||
Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
|
Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
|
||||||
Assert.True(stopResponse.IsSuccess);
|
Assert.True(stopResponse.IsSuccess);
|
||||||
@@ -183,6 +183,24 @@ public sealed class FanucProtocolTests
|
|||||||
Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
|
Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证实时发送路径可以复用命令对象，并用机器人状态包序号覆盖出包序号。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void J519Protocol_PacksCommandWithOverrideSequenceIntoReusableBuffer()
|
||||||
|
{
|
||||||
|
var command = new FanucJ519Command(
|
||||||
|
sequence: 2,
|
||||||
|
targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||||
|
var packet = new byte[FanucJ519Protocol.CommandPacketLength];
|
||||||
|
|
||||||
|
FanucJ519Protocol.PackCommandPacket(command, sequence: 456, packet);
|
||||||
|
|
||||||
|
Assert.Equal(456u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
|
||||||
|
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
|
||||||
|
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
|
/// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
99
tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs
Normal file
99
tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs
Normal file
@@ -0,0 +1,99 @@
|
|||||||
|
using Flyshot.Core.Planning.Sampling;
|
||||||
|
|
||||||
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 J519 实发重采样器在离线导出和运行时下发之间保持一致的时间轴语义。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class J519SendTrajectorySamplerTests
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 speed_ratio 只缩放轨迹时间，物理发送时间仍按固定伺服周期推进。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void SampleDenseJointTrajectory_MapsSendTimeToScaledTrajectoryTimeAndDegrees()
|
||||||
|
{
|
||||||
|
var denseTrajectory = new[]
|
||||||
|
{
|
||||||
|
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||||
|
};
|
||||||
|
|
||||||
|
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||||
|
denseTrajectory,
|
||||||
|
durationSeconds: 0.016,
|
||||||
|
servoPeriodSeconds: 0.008,
|
||||||
|
speedRatio: 0.5);
|
||||||
|
|
||||||
|
Assert.Equal(5, samples.Count);
|
||||||
|
Assert.Equal(0, samples[0].SampleIndex);
|
||||||
|
Assert.Equal(0.0, samples[0].SendTime, precision: 6);
|
||||||
|
Assert.Equal(0.0, samples[0].TrajectoryTime, precision: 6);
|
||||||
|
Assert.Equal(0.0, samples[0].JointsDegrees[0], precision: 6);
|
||||||
|
|
||||||
|
Assert.Equal(1, samples[1].SampleIndex);
|
||||||
|
Assert.Equal(0.008, samples[1].SendTime, precision: 6);
|
||||||
|
Assert.Equal(0.004, samples[1].TrajectoryTime, precision: 6);
|
||||||
|
Assert.Equal(45.0, samples[1].JointsDegrees[0], precision: 6);
|
||||||
|
|
||||||
|
Assert.Equal(4, samples[^1].SampleIndex);
|
||||||
|
Assert.Equal(0.032, samples[^1].SendTime, precision: 6);
|
||||||
|
Assert.Equal(0.016, samples[^1].TrajectoryTime, precision: 6);
|
||||||
|
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void SampleDenseJointTrajectory_RejectsEmptyDenseTrajectory()
|
||||||
|
{
|
||||||
|
var exception = Assert.Throws<InvalidOperationException>(() =>
|
||||||
|
J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||||
|
Array.Empty<IReadOnlyList<double>>(),
|
||||||
|
durationSeconds: 0.016,
|
||||||
|
servoPeriodSeconds: 0.008,
|
||||||
|
speedRatio: 1.0));
|
||||||
|
|
||||||
|
Assert.Contains("稠密关节轨迹为空", exception.Message);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证非法 speed_ratio 会在公共入口统一拦截。
|
||||||
|
/// </summary>
|
||||||
|
[Theory]
|
||||||
|
[InlineData(0.0)]
|
||||||
|
[InlineData(double.NaN)]
|
||||||
|
[InlineData(double.PositiveInfinity)]
|
||||||
|
public void SampleDenseJointTrajectory_RejectsInvalidSpeedRatio(double speedRatio)
|
||||||
|
{
|
||||||
|
var denseTrajectory = new[]
|
||||||
|
{
|
||||||
|
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||||
|
};
|
||||||
|
|
||||||
|
Assert.Throws<ArgumentOutOfRangeException>(() =>
|
||||||
|
J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||||
|
denseTrajectory,
|
||||||
|
durationSeconds: 0.0,
|
||||||
|
servoPeriodSeconds: 0.008,
|
||||||
|
speedRatio: speedRatio));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证公共诊断行格式与既有 ActualSendTiming 文件保持一致。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void BuildTimingRow_UsesLegacyActualSendColumnOrder()
|
||||||
|
{
|
||||||
|
var row = J519SendTrajectorySampler.BuildTimingRow(new J519SendSample(
|
||||||
|
sampleIndex: 2,
|
||||||
|
sendTime: 0.016,
|
||||||
|
trajectoryTime: 0.008,
|
||||||
|
speedRatio: 0.5,
|
||||||
|
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||||
|
|
||||||
|
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -2,7 +2,6 @@ using Flyshot.Core.Config;
|
|||||||
using Flyshot.Core.Domain;
|
using Flyshot.Core.Domain;
|
||||||
using Flyshot.Core.Planning;
|
using Flyshot.Core.Planning;
|
||||||
using Flyshot.Core.Planning.Export;
|
using Flyshot.Core.Planning.Export;
|
||||||
using Flyshot.Core.Planning.Kinematics;
|
|
||||||
using Flyshot.Core.Planning.Sampling;
|
using Flyshot.Core.Planning.Sampling;
|
||||||
using Flyshot.Core.Triggering;
|
using Flyshot.Core.Triggering;
|
||||||
using Xunit.Abstractions;
|
using Xunit.Abstractions;
|
||||||
@@ -24,12 +23,11 @@ public sealed class OfflinePlanTests
|
|||||||
/// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。
|
/// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
[Theory]
|
[Theory]
|
||||||
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", false, 1.0)]
|
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", false, 1.0)]
|
||||||
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", true, 0.9)]
|
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", true, 0.9)]
|
||||||
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL10_EAU_0", false, 1.0)]
|
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "EOL10_EAU_0", false, 1.0)]
|
||||||
public void GenerateTrajectory_MatchesPythonDemo(
|
public void GenerateTrajectory_MatchesPythonDemo(
|
||||||
string configPath,
|
string configPath,
|
||||||
string robotModelPath,
|
|
||||||
string trajName,
|
string trajName,
|
||||||
bool useSelfAdapt,
|
bool useSelfAdapt,
|
||||||
double speedRatio)
|
double speedRatio)
|
||||||
@@ -42,9 +40,13 @@ public sealed class OfflinePlanTests
|
|||||||
// 1. 加载配置和模型。
|
// 1. 加载配置和模型。
|
||||||
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
|
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
|
||||||
var program = loadedConfig.Programs[trajName];
|
var program = loadedConfig.Programs[trajName];
|
||||||
var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath);
|
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
|
||||||
var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale);
|
var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
|
||||||
var kinematicsModel = new RobotModelLoader().LoadKinematicsModel(resolvedRobotModelPath);
|
resolvedRobotModelPath,
|
||||||
|
loadedConfig.Robot.AccLimitScale,
|
||||||
|
loadedConfig.Robot.JerkLimitScale);
|
||||||
|
var baseProfile = loadedModel.Profile;
|
||||||
|
var kinematicsModel = loadedModel.KinematicsModel;
|
||||||
|
|
||||||
// 2. 应用 speed_ratio 缩放。
|
// 2. 应用 speed_ratio 缩放。
|
||||||
var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);
|
var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
using Flyshot.Core.Config;
|
using Flyshot.Core.Config;
|
||||||
using Flyshot.Core.Domain;
|
using Flyshot.Core.Domain;
|
||||||
using Flyshot.Core.Planning;
|
using Flyshot.Core.Planning;
|
||||||
|
using Flyshot.Core.Planning.Sampling;
|
||||||
using Flyshot.Core.Triggering;
|
using Flyshot.Core.Triggering;
|
||||||
|
|
||||||
namespace Flyshot.Core.Tests;
|
namespace Flyshot.Core.Tests;
|
||||||
@@ -66,7 +67,7 @@ public sealed class PlanningCompatibilityTests
|
|||||||
{
|
{
|
||||||
var workspaceRoot = GetWorkspaceRoot();
|
var workspaceRoot = GetWorkspaceRoot();
|
||||||
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
|
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
|
||||||
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
|
||||||
|
|
||||||
var config = new RobotConfigLoader().Load(configPath);
|
var config = new RobotConfigLoader().Load(configPath);
|
||||||
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
|
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
|
||||||
@@ -171,6 +172,28 @@ public sealed class PlanningCompatibilityTests
|
|||||||
Assert.Equal(2, doEvent.HoldCycles);
|
Assert.Equal(2, doEvent.HoldCycles);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证稠密轨迹生成后会复核离散加速度，避免采样结果绕过 ICSP 段 scale 校验。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void TrajectoryLimitValidator_Throws_WhenDenseTrajectoryAccelerationExceedsLimit()
|
||||||
|
{
|
||||||
|
var robot = CreateRobotProfile([100.0], [10.0], [1000.0]);
|
||||||
|
|
||||||
|
var exception = Assert.Throws<InvalidOperationException>(() =>
|
||||||
|
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
||||||
|
robot,
|
||||||
|
[
|
||||||
|
[0.0, 0.0],
|
||||||
|
[0.008, 0.0],
|
||||||
|
[0.016, 0.01]
|
||||||
|
],
|
||||||
|
trajectoryName: "dense-acceleration-check"));
|
||||||
|
|
||||||
|
Assert.Contains("加速度", exception.Message);
|
||||||
|
Assert.Contains("dense-acceleration-check", exception.Message);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
|
/// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user