xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(fanuc): 统一 speedRatio 执行倍率语义

Browse Source 
* 将 speedRatio 前移到规划/准备阶段，运行时只消费已生成的 8ms 队列
* 区分旧格式规划导出与 ActualSend 实发诊断工件
* 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
yunxiao.zhu
2026-05-11 17:21:18 +08:00
parent d82128da4a
commit d120ef2a39
17 changed files with 1395 additions and 534 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
docs/flyshot-speedratio-execution-design-20260509.md
View File

@@ -47,10 +47,10 @@
本设计目标如下：
- 保持 `planning_speed_scale` 只属于规划层。
- `speedRatio` 只作用于飞拍执行层，且仅对下一次启动的飞拍任务生效。
- 任意有效 `speedRatio` 下，最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
- `speedRatio` 是运行时统一执行倍率，可影响下一次启动的飞拍、普通轨迹和 `MoveJoint`。
- 飞拍链路在任意有效 `speedRatio` 下，最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
- 不依赖“插值后自然会平滑”的经验假设。
- 当 `speedRatio = 0.5` 等较慢倍率导致候选点列不满足约束时，系统自动拉长执行时长，直到点列通过校验。
- 当请求 `speedRatio` 下的最终发送队列不满足约束时，拒绝执行并输出超限诊断；不在执行侧偷偷改写本次请求倍率。
- 不破坏现有 `shot timeline / sample offset` 语义。
- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。
@@ -58,7 +58,7 @@
本次设计明确不包含以下内容：
- 不修改 `move_joint` 的调速和生成机制。
- 不恢复旧的发送阶段临场回采样机制；普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段折算。
- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。
- 不修改 `planning_speed_scale` 的含义。
- 不在任务执行中途支持 `speedRatio` 热切换。
@@ -77,7 +77,7 @@
- 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。
- 生成最终 `8ms` 发送队列。
- 对最终队列做逐周期 `vel/acc/jerk` 校验。
- 如果不通过，则自动拉长执行时长后重建队列，直到通过。
- 如果不通过，则拒绝执行，提示降低 `planning_speed_scale` 重新规划或显式设置更低的 `speedRatio`。
3. 发送段
- 运行时只消费已经准备好的最终 `8ms` 队列。
- 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。
@@ -90,7 +90,7 @@
## 6. 统一语义
本次只改飞拍任务，但仍需把飞拍内部语义说清楚，避免再次混淆规划倍率和执行倍率。
本次以飞拍任务为主线，但 `speedRatio` 允许作为运行时统一执行倍率影响普通轨迹和 `MoveJoint`，需要把三类入口的边界说清楚，避免再次混淆规划倍率和执行倍率。
### 6.1 `planning_speed_scale`
@@ -101,17 +101,17 @@
### 6.2 `speedRatio`
- 只影响飞拍执行准备阶段。
- 只对下一次启动的飞拍任务生效。
- 表示“用户期望的执行层时间推进速度”。
- 该值先用于构建候选发送队列。
- 如果候选发送队列在离散 `8ms` 点列上不满足约束，则系统自动进一步拉长执行时长，直到满足约束。
- 影响下一次启动的运动任务，不在任务执行中途热切换。
- 对飞拍任务，表示用户期望的执行层时间推进速度，可前移到规划/执行准备阶段生成最终发送队列。
- 对普通轨迹和 `MoveJoint`，允许在规划或临时 PTP 轨迹生成阶段折算进有效关节限位，使运行时只消费已经生成好的 `8ms` 稠密点列。
- `speedRatio = 0.8` 表示执行节奏接近把同几何轨迹时间轴整体拉长 `1 / 0.8`。
- 例如 `planning_speed_scale = 0.9` 且 `speedRatio = 0.8` 时，最终执行节奏应接近同几何轨迹在 `planning_speed_scale ≈ 0.72` 下的结果。
也就是说：
- `speedRatio` 是执行目标倍率。
- 但最终真实执行时长允许比该目标更保守。
- 保守的原因不是放宽标准，而是严格保证离散动力学约束。
- `speedRatio` 是确定的执行倍率，不是校验失败后的自动搜索变量。
- `finalSpeedRatio` 正常必须等于请求值。
- 若该倍率下最终队列超限，问题暴露给调用方，而不是由执行侧自动改成另一个倍率。
## 7. 最终发送序列生成
@@ -124,54 +124,33 @@
职责：
- 输入规划层 `DenseJointTrajectory`。
- 输入 `durationSeconds`、`servoPeriodSeconds`、`speedRatio`。
- 输入规划层连续 `PlannedTrajectory`，优先使用经过执行侧定时处理后的 `ExecutionTrajectory`。
- 输入 `servoPeriodSeconds`、`speedRatio`。
- 输出最终真实发送的 `8ms` 点列。
- 输出与该点列一致的触发绑定结果。
- 输出校验与自动拉长过程中的诊断信息。
- 输出请求倍率、最终倍率和历史倍率改写次数等诊断信息；当前语义下最终倍率等于请求倍率、改写次数为 0。
### 7.2 第一版候选队列
### 7.2 最终请求倍率队列
候选队列仍可以沿用当前时间轴语义作为起点：
最终队列按固定时间轴语义生成：
- `sendTime = sampleIndex * 0.008`
- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)`
- 关节目标从连续样条直接取点，并从 `rad` 转为 J519 需要的 `deg`
但该候选队列只作为“第一轮尝试”，不能直接视为最终执行结果。
该队列就是本次请求倍率下的最终发送队列，后续只做校验和触发绑定，不再进行二次滤波或倍率搜索。
### 7.3 自动拉长策略
### 7.3 校验失败策略
当候选队列的离散 `vel/acc/jerk` 校验失败时：
当最终队列的离散 `vel/acc/jerk` 校验失败时：
- 不修改规划层轨迹。
- 不修改 `planning_speed_scale`。
- 不放宽校验阈值。
- 只在执行侧拉长最终发送时长，然后重新构建候选队列。
- 不降低本次 `speedRatio`。
- 直接拒绝执行并保留首个超限诊断。
可实现为等价的两种方式之一：
1. 延长物理发送总点数，使发送总时长变长。
2. 在保持 `8ms` 周期不变的前提下，降低执行侧有效轨迹时间推进速度。
本质上二者等价，建议统一落成第二种表达：
- 对外仍说“自动拉长执行时长”。
- 对内通过更保守的 `trajectoryTime(sendTime)` 映射来实现。
### 7.4 自动拉长的迭代规则
建议采用单调保守策略：
1. 先按请求 `speedRatio` 构建第 1 版候选队列。
2. 对候选队列做离散校验。
3. 若失败，则按固定步长或倍率逐轮拉长，再次构建候选队列。
4. 一旦通过，立即停止迭代，产出最终队列。
5. 若超过安全迭代上限仍未通过，则拒绝执行并输出首个超限诊断。
这样可以保证：
- 自动拉长过程是可解释、可记录的。
- 不会因为局部修补而引入新的不可预测尖峰。
这样可以避免把 `speedRatio` 误实现成安全优化器。现场若希望从 `planning_speed_scale = 0.9` 的 10s 轨迹降到接近 `planning_speed_scale = 0.7` 的执行效果，应显式设置类似 `speedRatio = 0.8`，而不是让系统在失败后自动猜一个更低倍率。
## 8. 逐周期约束校验
@@ -201,10 +180,11 @@
### 8.3 校验失败的处理
飞拍链路按本次确认采用 A：
飞拍链路按当前修正语义采用“确定倍率 + 失败拒绝”：
- 若失败，优先自动拉长执行时长重试。
- 只有在超过拉长上限后仍失败时才拒绝执行。
- 若请求倍率下失败，立即拒绝执行。
- 不进行执行侧自动倍率搜索。
- 调用方可以选择降低 `planning_speed_scale` 重新规划，或显式设置更低的 `speedRatio` 后再次执行。
拒绝执行时必须输出：
@@ -212,7 +192,8 @@
- 时间窗
- 指标类型
- `actual / limit / ratio`
- 失败发生在哪一轮自动拉长尝试中
- 请求 `speedRatio`
- 当前 `finalSpeedRatio`，正常等于请求值
## 9. 触发时序与绑定
@@ -238,15 +219,15 @@
- 触发绑定和真实发送完全一致。
- 导出工件中的 `ShotEvents.json` 与真实执行时序一致。
- 自动拉长后触发点仍可被精确追溯到最终发送点索引。
- 触发点可被精确追溯到本次请求倍率下的最终发送点索引。
### 9.3 关于触发数量与顺序
自动拉长执行时长时：
请求倍率改变最终发送时长时：
- 触发数量不能变。
- 触发顺序不能变。
- 只允许触发在最终发送队列中的绑定索引后移或保持可解释等价。
- 触发在最终发送队列中的绑定索引应随时间轴拉长后移或保持可解释等价。
## 10. 运行时职责调整
@@ -284,9 +265,19 @@
## 11. 导出工件与日志
### 11.1 工件统一基于最终发送队列
### 11.1 旧规划导出和实发诊断必须分开
以下文件必须全部从最终发送队列生成：
`saveTrajectory` 目录中存在两类不同用途的文件，不能再混成同一条时间轴：
第一类是旧格式规划导出，用于和旧 RVBUST 导出的规划轨迹、离线分析脚本做对比。它们只受 `planning_speed_scale` 和规划/整形参数影响，不受运行时 `speedRatio` 影响：
- `JointTraj.txt`
- `JointDetialTraj.txt`
- `CartTraj.txt`
- `CartDetialTraj.txt`
- `JointDetialTraj.analysis.txt`
第二类是实发诊断，用于观察当前 `speedRatio` 下真实准备发送的 8ms 队列和触发绑定。它们必须全部从最终发送队列生成：
- `ActualSendJointTraj.txt`
- `ActualSendTiming.txt`
@@ -295,7 +286,8 @@
不允许再出现：
- 导出文件基于一套样本，
- 旧规划导出被运行时 `speedRatio` 改写，
- `ActualSend*` 基于一套样本，
- 真实发送队列又基于另一套样本。
### 11.2 推荐新增日志字段
@@ -304,8 +296,8 @@
- 请求 `speedRatio`
- 规划层轨迹时长
- 第一轮候选队列点数
- 第一轮候选队列是否通过校验
- 最终发送队列点数
- 最终发送队列是否通过校验
- 若失败，首个失败窗口的：
- 关节轴
- 时间区间
@@ -313,7 +305,7 @@
- `actual`
- `limit`
- `ratio`
- 自动拉长轮数
- 倍率改写次数，当前正常为 0
- 最终采用的执行时长
- 最终发送队列点数
- 最终触发绑定数量
@@ -324,16 +316,16 @@
- `Information`
- 记录执行请求、最终采用结果、最终通过结论
- `Warning`
- 记录第一轮候选失败与自动拉长启动
- 记录请求倍率下最终发送队列校验失败
- `Debug`
- 记录每一轮拉长的中间参数与详细差分统计
- 记录最终发送队列的时间映射与详细差分统计
## 12. 验收口径
### 12.1 功能验收
- 飞拍 `speedRatio` 可在线设置。
- 该值对下一次启动的飞拍任务生效。
- `speedRatio` 可在线设置。
- 该值对下一次启动的飞拍、普通轨迹或 `MoveJoint` 生效。
- 不需要修改 `planning_speed_scale`。
### 12.2 约束验收
@@ -345,8 +337,9 @@
### 12.3 工件与日志验收
- `JointTraj.txt / JointDetialTraj.txt / CartTraj.txt / CartDetialTraj.txt` 不随运行时 `speedRatio` 改变。
- `ActualSend*` 文件能反映最终真实发送点位与时间映射。
- 日志能定位自动拉长前后的关键参数和校验结果。
- 日志能定位请求倍率、最终发送队列和校验失败窗口。
### 12.4 回归验收
@@ -362,8 +355,10 @@
- 飞拍执行侧最终发送序列生成器测试
- `speedRatio` 非法边界值测试
- 第一轮候选失败后自动拉长成功测试
- 请求倍率下队列超限时拒绝执行测试
- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试
- `planning_speed_scale = 0.9` 加 `speedRatio = 0.8` 的执行时间轴等价于约 `0.72` 规划倍率的回归测试
- 普通轨迹和 `MoveJoint` 在倍率乘积相同时生成等价或可解释等价的稠密执行轨迹
- 触发绑定始终与最终发送队列一致的测试
### 13.2 编排测试
@@ -372,6 +367,8 @@
- `ExecuteFlyShotTraj` 进入运行时前，已经拿到最终发送队列
- `FanucControllerRuntime` 不再自行按 `_speedRatio` 对飞拍轨迹回采样
- `ExecuteTrajectory` 和 `MoveJoint` 的调速在规划/生成阶段完成，运行时不再做临场倍率重采样
- `SaveTrajectoryInfo` 的旧格式规划导出不随运行时 `speedRatio` 改变，而 `ActualSend*` 仍随运行时 `speedRatio` 改变
### 13.3 集成与黄金样本
@@ -401,13 +398,13 @@
新增飞拍执行侧最终发送序列生成器及相关结果类型，负责：
- 候选队列构建
- 自动拉长
- 离散校验
- 触发绑定输入准备
### 14.2 `src/Flyshot.ControllerClientCompat/`
在飞拍执行准备阶段调用该生成器，把最终发送队列放入执行结果或新的执行上下文对象。
普通轨迹规划入口和 `MoveJoint` 临时轨迹生成入口可以读取当前 `speedRatio`，并在进入运行时前折算成已调速的稠密轨迹。
### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
@@ -418,7 +415,10 @@
### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs`
`ActualSend*` 导出改为复用最终发送队列，确保导出工件与运行时一致。
`FlyshotTrajectoryArtifactWriter` 接收两份视图：
- 旧格式规划导出视图：写 `JointTraj.txt`、`JointDetialTraj.txt`、`CartTraj.txt`、`CartDetialTraj.txt`
- 实发诊断视图：写 `ActualSendJointTraj.txt`、`ActualSendTiming.txt`、`ActualSendJerkStats.txt`、`ShotEvents.json`
### 14.5 `tests/Flyshot.Core.Tests/`
@@ -426,14 +426,15 @@
## 15. 风险与注意事项
### 15.1 不能把自动拉长误解为“偷偷改 speedRatio”
### 15.1 不能把 speedRatio 做成安全优化器
对外语义仍应保留：
对外语义必须保持：
- 用户请求的是某个 `speedRatio`
- 系统为了满足离散动力学约束，自动采用了更保守的最终执行时长
- 系统按这个 `speedRatio` 构建最终发送队列
- 若队列超限，则拒绝执行并要求调用方显式选择新的规划倍率或执行倍率
日志里必须把这件事说清楚，不能让现场误以为参数未生效。
不能因为校验失败就在执行侧自动把 `0.8` 改成 `0.2`；这会让 `speedRatio` 和 `planning_speed_scale` 的关系失去可解释性。
### 15.2 不能让触发绑定脱离最终队列
@@ -448,10 +449,12 @@
本次飞拍调速设计的核心结论是：
- `planning_speed_scale` 继续只属于规划层。
- `speedRatio` 只属于飞拍执行层，并且只对下一次启动的飞拍任务生效。
- `speedRatio` 是运行时统一执行倍率，并且只对下一次启动的运动任务生效。
- 普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段，运行时只发送已生成好的 `8ms` 点列。
- 飞拍执行必须先生成最终 `8ms` 发送队列，再对该队列做逐周期 `vel/acc/jerk` 校验。
- 若 `speedRatio < 1` 导致第一轮候选队列不满足约束，则自动拉长执行时长后重建，直到通过。
- `speedRatio` 按 `trajectoryTime = sendTime * speedRatio` 确定性拉长执行时间轴，`finalSpeedRatio` 不应被自动改写。
- 若请求倍率下最终队列不满足约束，则拒绝执行并提示降低 `planning_speed_scale` 重新规划或显式设置更低 `speedRatio`。
- 运行时不再对飞拍轨迹做发送前临场回采样。
- 触发绑定、导出工件与真实发送必须全部统一到同一份最终发送队列上。
- 旧格式规划导出必须保持不受运行时 `speedRatio` 影响；`ActualSend*` 和 `ShotEvents.json` 必须统一到真实发送队列上。
该方案可以在不修改 `move_joint`、不放宽校验阈值、不中断现有触发语义的前提下，把飞拍 `speedRatio` 调速从“经验插值”收敛为“可校验、可追踪、可自动保守化”的执行机制。
该方案可以在不放宽校验阈值、不中断现有触发语义的前提下，把 `speedRatio` 调速从“发送阶段经验插值”收敛为“规划/准备阶段完成、运行时确定发送”的执行机制。

145
src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs
View File

@@ -405,14 +405,16 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
_logger?.LogInformation(
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
bundle.Result.Method,
bundle.Result.Duration.TotalSeconds,
bundle.Result.IsValid,
bundle.Result.DenseJointTrajectory?.Count ?? 0,
planningSpeedScale);
planningSpeedScale,
speedRatio);
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
}
@@ -495,32 +497,40 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
var settings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
options.SaveTrajectory,
robot,
trajectory,
options,
settings,
bundle,
settings.PlanningSpeedScale,
speedRatio);
_logger?.LogInformation(
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
name,
bundle.Result.Method,
bundle.Result.Duration.TotalSeconds,
bundle.Result.TriggerTimeline.Count,
bundle.Result.UsedCache,
settings.PlanningSpeedScale);
settings.PlanningSpeedScale,
speedRatio);
if (options.MoveToStart)
{
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
}
else
{
//检验当前机械臂的关节坐标与计划轨迹的第一个点之前的差异,如果差异过大.就不报警,不执行下去
var currentJointPositions = _runtime.GetJointPositions();
var targetJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[0].Positions;
var diff = currentJointPositions.Zip(targetJointPositions, (c, t) => Math.Abs(c - t)).Sum();
if (diff > 0.01)
// 正式飞拍前必须确认机器人反馈已经在轨迹起点附近，避免 J519 目标突变。
if (!IsFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name))
{
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之前的差异过大 name={Name}", name);
return;
}
}
@@ -529,7 +539,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
if (options.Wait)
{
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
var executionDuration = bundle.PreparedExecution is null
? bundle.Result.Duration
: TimeSpan.FromSeconds(bundle.PreparedExecution.FinalDurationSeconds);
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", executionDuration);
}
}
@@ -561,6 +574,33 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 校验当前反馈是否接近飞拍起点；不接近时直接抛出兼容错误。
/// </summary>
private void EnsureFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
{
if (!IsFeedbackNearFlyshotStart(targetJointPositions, name))
{
throw new InvalidOperationException("Robot feedback is not near flyshot start.");
}
}
/// <summary>
/// 检查当前机械臂关节反馈与计划轨迹第一个点之间的差异。
/// </summary>
private bool IsFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
{
var currentJointPositions = _runtime.GetJointPositions();
var diff = currentJointPositions.Zip(targetJointPositions, (current, target) => Math.Abs(current - target)).Sum();
if (diff <= 0.01)
{
return true;
}
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之间的差异过大 name={Name}, diff={Diff}", name, diff);
return false;
}
/// <summary>
/// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。
/// </summary>
@@ -620,15 +660,26 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
var planningSettings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
@@ -658,14 +709,25 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
var planningSettings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
@@ -785,20 +847,42 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <param name="name">飞拍轨迹名称。</param>
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
/// <param name="robot">当前机器人模型。</param>
/// <param name="bundle">规划结果包。</param>
/// <param name="uploaded">已上传的飞拍轨迹。</param>
/// <param name="options">本次规划选项。</param>
/// <param name="settings">当前机器人兼容设置。</param>
/// <param name="executionBundle">运行时和 ActualSend 诊断使用的规划结果包。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次运行时速度倍率。</param>
private void ExportFlyshotArtifactsIfRequested(
string name,
bool saveTrajectory,
RobotProfile robot,
PlannedExecutionBundle bundle)
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions options,
CompatibilityRobotSettings settings,
PlannedExecutionBundle executionBundle,
double planningSpeedScale,
double speedRatio)
{
if (!saveTrajectory)
{
return;
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
// 旧格式 Joint/Cart 导出必须只反映 planning_speed_scale；ActualSend* 才反映当前运行时 speedRatio。
var exportPlanningBundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
uploaded,
new FlyshotExecutionOptions(
moveToStart: options.MoveToStart,
useCache: false,
saveTrajectory: options.SaveTrajectory,
method: options.Method,
wait: options.Wait),
settings,
planningSpeedScale,
speedRatio: 1.0);
_artifactWriter.WriteUploadedFlyshot(name, robot, exportPlanningBundle, executionBundle, speedRatio);
}
/// <summary>
@@ -811,9 +895,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
{
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
robot,
bundle.ExecutionTrajectory,
bundle.Result,
robot.ServoPeriod.TotalSeconds,
speedRatio);
requestedSpeedRatio: speedRatio,
samplingSpeedRatio: 1.0);
var preparedResult = new TrajectoryResult(
programName: bundle.Result.ProgramName,
method: bundle.Result.Method,
@@ -829,7 +915,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
denseJointTrajectory: bundle.Result.DenseJointTrajectory,
preparedFlyshotExecution: preparedExecution);
return new PlannedExecutionBundle(bundle.PlannedTrajectory, bundle.ShotTimeline, preparedResult, preparedExecution);
return new PlannedExecutionBundle(
bundle.PlannedTrajectory,
bundle.ShotTimeline,
preparedResult,
preparedExecution,
bundle.ExecutionTrajectory);
}
/// <summary>

53
src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs
View File

@@ -55,21 +55,24 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="waypoints">普通轨迹关节路点。</param>
/// <param name="options">执行参数。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次执行速度倍率；普通轨迹在规划阶段折算进有效限速倍率。</param>
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanOrdinaryTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> waypoints,
TrajectoryExecutionOptions? options = null,
double planningSpeedScale = 1.0)
double planningSpeedScale = 1.0,
double speedRatio = 1.0)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions();
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(planningSpeedScale, speedRatio, "普通轨迹");
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale);
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale, speedRatio, effectivePlanningSpeedScale);
var program = CreateProgram(
name: "ordinary-trajectory",
@@ -101,7 +104,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
result.Duration.TotalSeconds,
result.DenseJointTrajectory?.Count ?? 0);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: executionTrajectory);
}
/// <summary>
@@ -112,24 +115,35 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="options">执行参数。</param>
/// <param name="settings">兼容层机器人设置。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次飞拍使用的速度倍率；为空时使用配置默认值。</param>
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanUploadedFlyshot(
RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions? options = null,
CompatibilityRobotSettings? settings = null,
double? planningSpeedScale = null)
double? planningSpeedScale = null,
double? speedRatio = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(uploaded);
options ??= new FlyshotExecutionOptions();
settings ??= CreateDefaultRobotSettings();
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var requestedSpeedRatio = speedRatio ?? settings.SpeedRatio;
var configuredPlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(configuredPlanningSpeedScale, requestedSpeedRatio, "飞拍");
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale, settings.SmoothStartStopTiming);
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
uploaded.Name,
uploaded.Waypoints.Count,
options.Method,
options.UseCache,
configuredPlanningSpeedScale,
requestedSpeedRatio,
effectivePlanningSpeedScale,
settings.SmoothStartStopTiming);
var program = CreateProgram(
name: uploaded.Name,
@@ -170,7 +184,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
denseJointTrajectory,
usedCache: false,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: smoothedExecutionTrajectory);
_logger?.LogInformation(
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
@@ -329,6 +343,23 @@ public sealed class ControllerClientTrajectoryOrchestrator
adaptIcspTryNum: 5);
}
/// <summary>
/// 解析飞拍生成轨迹时真正使用的规划速度倍率。
/// </summary>
/// <param name="planningSpeedScale">配置或调用方传入的规划倍率。</param>
/// <param name="speedRatio">本次执行速度倍率。</param>
/// <returns>用于缩放规划关节限制的有效倍率。</returns>
private static double ResolveEffectivePlanningSpeedScale(double planningSpeedScale, double speedRatio, string trajectoryKind)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), $"{trajectoryKind}速度倍率必须是有限正数。");
}
// 生成轨迹时把执行倍率折算进规划限制，保证 planning_speed_scale * speed_ratio 相同的配置产出同一条轨迹。
return planningSpeedScale * speedRatio;
}
/// <summary>
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
/// </summary>
@@ -361,7 +392,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
return robot;
}
// RVBUST 规划阶段会用独立限速倍率缩放有效限制；运行时 speedRatio 仍只负责 J519 下发重采样。
// speed_ratio 已在调用方折算到规划倍率中；运行时只消费已经生成好的 8ms 稠密轨迹。
var scaledLimits = robot.JointLimits
.Select(limit => new JointLimit(
limit.JointName,

47
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs
View File

@@ -49,9 +49,27 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="bundle">规划结果包。</param>
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
/// <param name="bundle">规划结果包；旧规划导出和实发诊断使用同一份视图。</param>
/// <param name="speedRatio">请求速度倍率，仅作为诊断列保留；最终采样倍率已在规划阶段处理。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
{
WriteUploadedFlyshot(trajectoryName, robot, bundle, bundle, speedRatio);
}
/// <summary>
/// 将飞拍规划结果导出到 Config/Data/name，并显式区分旧规划导出视图和实发诊断视图。
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="exportPlanningBundle">旧格式规划导出使用的结果包，不应包含运行时 speedRatio 折算。</param>
/// <param name="executionBundle">实发诊断使用的结果包，应与运行时最终发送队列保持一致。</param>
/// <param name="speedRatio">请求速度倍率，仅作为 ActualSend 诊断列保留。</param>
public void WriteUploadedFlyshot(
string trajectoryName,
RobotProfile robot,
PlannedExecutionBundle exportPlanningBundle,
PlannedExecutionBundle executionBundle,
double speedRatio = 1.0)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
@@ -59,31 +77,32 @@ public sealed class FlyshotTrajectoryArtifactWriter
}
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(bundle);
ArgumentNullException.ThrowIfNull(exportPlanningBundle);
ArgumentNullException.ThrowIfNull(executionBundle);
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
Directory.CreateDirectory(outputDir);
if (bundle.Result.DenseJointTrajectory is null)
if (exportPlanningBundle.Result.DenseJointTrajectory is null)
{
throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。");
throw new InvalidOperationException("导出飞拍旧格式规划工件前必须先生成规划明细轨迹。");
}
// 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”，避免再次从 PlannedTrajectory 生成另一条轨迹。
// 旧格式规划文件固定使用不含运行时 speedRatio 的规划视图，避免和 ActualSend 实发诊断混在一起。
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
var jointTrajectory = BuildJointRows(exportPlanningBundle.PlannedTrajectory);
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
var executionDenseTrajectory = bundle.Result.DenseJointTrajectory;
var executionDenseTrajectory = exportPlanningBundle.Result.DenseJointTrajectory;
var jointDetailTrajectory = DownsampleDenseRows(
executionDenseTrajectory,
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
var cartTrajectory = BuildCartesianRows(exportPlanningBundle.PlannedTrajectory, kinematicsModel);
var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio);
WriteActualSendArtifacts(outputDir, robot, executionBundle.Result, speedRatio);
_logger?.LogInformation(
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
@@ -107,16 +126,12 @@ public sealed class FlyshotTrajectoryArtifactWriter
return;
}
if (preparedExecution is null && (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio)))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var samples = preparedExecution is null
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio)
speedRatio: 1.0)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
@@ -161,7 +176,7 @@ public sealed class FlyshotTrajectoryArtifactWriter
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
if (preparedExecution is null)
{
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample, speedRatio, stretchIterationCount: 0));
if (previousSendTime is not null && previousJoints is not null)
{

39
src/Flyshot.ControllerClientCompat/MoveJointTrajectoryGenerator.cs
View File

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

15
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
View File

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

14
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
View File

@@ -13,9 +13,9 @@ public sealed class FlyshotPreparedExecution
/// <param name="timingRows">与最终发送点列一致的时间映射诊断行。</param>
/// <param name="jerkRows">与最终发送点列一致的跃度诊断行。</param>
/// <param name="requestSpeedRatio">请求的执行倍率。</param>
/// <param name="finalSpeedRatio">通过离散校验后实际采用的保守倍率。</param>
/// <param name="finalSpeedRatio">最终发送队列采用的采样倍率；请求倍率已折算进规划时为 1。</param>
/// <param name="finalDurationSeconds">最终发送总时长，单位为秒。</param>
/// <param name="stretchIterationCount">自动拉长执行时长的迭代次数。</param>
/// <param name="stretchIterationCount">历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。</param>
public FlyshotPreparedExecution(
IEnumerable<FlyshotPreparedSample> samples,
IEnumerable<FlyshotPreparedTriggerBinding> triggerBindings,
@@ -48,7 +48,7 @@ public sealed class FlyshotPreparedExecution
if (stretchIterationCount < 0)
{
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "拉长迭代次数必须是非负整数。");
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写迭代次数必须是非负整数。");
}
Samples = samples.Select(static sample => sample).ToArray();
@@ -87,7 +87,7 @@ public sealed class FlyshotPreparedExecution
public double RequestSpeedRatio { get; }
/// <summary>
/// 获取通过离散校验后实际采用的保守倍率。
/// 获取最终发送队列采用的采样倍率；请求倍率保留在 <see cref="RequestSpeedRatio"/> 中用于诊断。
/// </summary>
public double FinalSpeedRatio { get; }
@@ -97,7 +97,7 @@ public sealed class FlyshotPreparedExecution
public double FinalDurationSeconds { get; }
/// <summary>
/// 获取自动拉长执行时长的迭代次数。
/// 获取历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。
/// </summary>
public int StretchIterationCount { get; }
}
@@ -113,7 +113,7 @@ public sealed class FlyshotPreparedSample
/// <param name="sampleIndex">发送周期序号。</param>
/// <param name="sendTime">物理发送时间，单位为秒。</param>
/// <param name="trajectoryTime">回映射到规划轨迹的采样时间，单位为秒。</param>
/// <param name="speedRatio">生成该发送点时采用的执行倍率。</param>
/// <param name="speedRatio">生成该发送点时采用的队列采样倍率，已规划调速路径通常为 1。</param>
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
public FlyshotPreparedSample(
long sampleIndex,
@@ -146,7 +146,7 @@ public sealed class FlyshotPreparedSample
public double TrajectoryTime { get; }
/// <summary>
/// 获取生成该发送点时采用的执行倍率。
/// 获取生成该发送点时采用的队列采样倍率。
/// </summary>
public double SpeedRatio { get; }

102
src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs
View File

@@ -3,42 +3,32 @@ using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并在必要时自动拉长执行时长直到通过离散限幅校验。
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并执行发送口径的离散限幅校验。
/// </summary>
public static class FlyshotExecutionSendSequenceBuilder
{
/// <summary>
/// 自动拉长时每轮采用的保守倍率缩减系数。
/// </summary>
private const double StretchFactor = 0.95;
/// <summary>
/// 自动拉长尝试的最大迭代次数。
/// </summary>
private const int MaxStretchIterations = 16;
/// <summary>
/// 根据规划层稠密轨迹和执行层 speedRatio 构建最终发送队列。
/// 根据连续规划轨迹构建最终发送队列；请求 speedRatio 只作为诊断信息保留。
/// </summary>
/// <param name="robot">机器人关节限值配置。</param>
/// <param name="plannedTrajectory">规划阶段产出的连续轨迹。</param>
/// <param name="result">规划结果。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
/// <param name="requestedSpeedRatio">请求的执行倍率。</param>
/// <param name="requestedSpeedRatio">请求的执行倍率，必须已经在规划阶段折算进轨迹。</param>
/// <param name="samplingSpeedRatio">最终队列采样倍率；为空时使用 1.0，表示直接采样已调速后的规划轨迹。</param>
/// <returns>通过离散校验后的飞拍最终发送结果。</returns>
public static FlyshotPreparedExecution Build(
RobotProfile robot,
PlannedTrajectory plannedTrajectory,
TrajectoryResult result,
double servoPeriodSeconds,
double requestedSpeedRatio)
double requestedSpeedRatio,
double? samplingSpeedRatio = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(plannedTrajectory);
ArgumentNullException.ThrowIfNull(result);
if (result.DenseJointTrajectory is null)
{
throw new InvalidOperationException("飞拍执行准备前必须先生成规划层稠密关节轨迹。");
}
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。");
@@ -49,48 +39,40 @@ public static class FlyshotExecutionSendSequenceBuilder
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。");
}
var effectiveSpeedRatio = requestedSpeedRatio;
InvalidOperationException? firstFailure = null;
for (var iteration = 0; iteration <= MaxStretchIterations; iteration++)
var finalSpeedRatio = samplingSpeedRatio ?? 1.0;
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
{
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory,
result.Duration.TotalSeconds,
servoPeriodSeconds,
effectiveSpeedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName,
validateJerk: false);
return BuildPreparedExecution(
result,
samples,
requestedSpeedRatio,
effectiveSpeedRatio,
iteration);
}
catch (InvalidOperationException exception)
{
firstFailure ??= exception;
if (iteration >= MaxStretchIterations)
{
throw new InvalidOperationException(
$"飞拍最终发送队列离散限幅校验失败，已达到最大自动拉长次数 {MaxStretchIterations}。",
firstFailure);
}
// 只在执行侧进一步保守化 trajectoryTime(sendTime) 映射，不回写规划层轨迹。
effectiveSpeedRatio *= StretchFactor;
}
throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。");
}
throw new InvalidOperationException("飞拍最终发送队列构建失败。", firstFailure);
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
plannedTrajectory,
servoPeriodSeconds,
finalSpeedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName,
validateJerk: false,
strictJerkTolerance: true);
}
catch (InvalidOperationException exception)
{
throw new InvalidOperationException(
$"飞拍最终发送队列在请求 speed_ratio={requestedSpeedRatio:F6} 下离散限幅校验失败。" +
"请降低 planning_speed_scale 或 speed_ratio 后重新规划；执行侧不会在发送阶段临场重采样。",
exception);
}
return BuildPreparedExecution(
result,
samples,
requestedSpeedRatio,
finalSpeedRatio,
stretchIterationCount: 0);
}
/// <summary>
@@ -122,7 +104,9 @@ public static class FlyshotExecutionSendSequenceBuilder
binding.SampleIndex,
binding.FoundInWindow))
.ToArray();
var timingRows = samples.Select(J519SendTrajectorySampler.BuildTimingRow).ToArray();
var timingRows = samples
.Select(sample => J519SendTrajectorySampler.BuildTimingRow(sample, requestedSpeedRatio, stretchIterationCount))
.ToArray();
var jerkRows = BuildJerkRows(samples);
var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;

594
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs
View File

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

17
src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs
View File

@@ -41,12 +41,14 @@ public static class TrajectoryLimitValidator
/// <param name="toleranceMultiplier">限值容差倍率，用于过滤浮点舍入误差。</param>
/// <param name="trajectoryName">诊断用轨迹名称。</param>
/// <param name="validateJerk">是否校验离散 jerk；飞拍链路可临时关闭，仅保留速度/加速度约束。</param>
/// <param name="strictJerkTolerance">是否按原始 jerk 限值做硬约束；false 时保留历史宽容口径。</param>
public static void ValidateJ519SendSamples(
RobotProfile robot,
IReadOnlyList<J519SendSample> samples,
double toleranceMultiplier = DefaultLimitTolerance,
string? trajectoryName = null,
bool validateJerk = true)
bool validateJerk = false,
bool strictJerkTolerance = true)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(samples);
@@ -65,7 +67,13 @@ public static class TrajectoryLimitValidator
rows.Add(row);
}
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory", validateJerk);
ValidateRows(
robot,
rows,
toleranceMultiplier,
trajectoryName ?? "j519-send-trajectory",
validateJerk,
strictJerkTolerance);
}
/// <summary>
@@ -87,7 +95,8 @@ public static class TrajectoryLimitValidator
IReadOnlyList<IReadOnlyList<double>> rows,
double toleranceMultiplier,
string trajectoryName,
bool validateJerk)
bool validateJerk,
bool strictJerkTolerance = false)
{
double? previousTime = null;
double[]? previousPositions = null;
@@ -162,7 +171,7 @@ public static class TrajectoryLimitValidator
"Jerk",
jerk,
jointLimit.JerkLimit,
toleranceMultiplier*4);
strictJerkTolerance ? toleranceMultiplier : toleranceMultiplier * 4.0);
}
}

29
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
View File

@@ -487,11 +487,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
if (!IsSimulationMode && result.DenseJointTrajectory is not null)
{
if (_speedRatio <= 0.0)
{
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
}
EnsureJ519ReadyForDenseExecution();
// 真机模式且存在稠密路点：准备可被 StopMove 取消的同步发送任务。
@@ -562,13 +557,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
/// <summary>
/// 稠密轨迹发送任务：预生成完整 J519 命令队列，并等待机器人状态包按 speed_ratio 推进到执行完成。
/// 稠密轨迹发送任务：预生成完整 J519 命令队列，并等待机器人状态包驱动队列执行完成。
/// </summary>
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
{
var triggers = result.TriggerTimeline;
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
var speedRatio = _speedRatio;
var preparedExecution = result.PreparedFlyshotExecution;
var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds;
var samples = preparedExecution is null
@@ -576,7 +570,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
result.DenseJointTrajectory!,
durationSeconds,
servoPeriodSeconds,
speedRatio)
speedRatio: 1.0)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
@@ -590,8 +584,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
var sampleCount = samples.Count;
_logger?.LogInformation(
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, runtimeSpeedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
result.ProgramName, sampleCount, durationSeconds, _speedRatio, servoPeriodSeconds * 1000, triggers.Count);
var triggerBindings = preparedExecution is null
? TriggerSampleBinder.Bind(
@@ -627,6 +621,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
var preparedJerkRows = preparedExecution?.JerkRows;
var diagnosticRequestedSpeedRatio = _speedRatio > 0.0 && !double.IsNaN(_speedRatio) && !double.IsInfinity(_speedRatio)
? _speedRatio
: 1.0;
try
{
@@ -687,7 +684,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
commands.Add(command);
sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue));
sentTimingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
sentTimingRows.Add(preparedExecution is null
? J519SendTrajectorySampler.BuildTimingRow(sample, diagnosticRequestedSpeedRatio, stretchIterationCount: 0)
: J519SendTrajectorySampler.BuildTimingRow(
sample,
preparedExecution.RequestSpeedRatio,
preparedExecution.StretchIterationCount));
if (preparedJerkRows is not null)
{
@@ -778,7 +780,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
/// <param name="getLatestResponse">读取最新 J519 状态的委托。</param>
/// <param name="retryEnableRobot">状态未就绪时触发一次 EnableRobot 重试的委托。</param>
/// <param name="waitAfterRetry">重试后等待状态刷新的委托。</param>
internal static void EnsureJ519ReadyForDenseExecutionCore(
internal void EnsureJ519ReadyForDenseExecutionCore(
Func<FanucJ519Response?> getLatestResponse,
Action retryEnableRobot,
Action waitAfterRetry)
@@ -788,11 +790,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
ArgumentNullException.ThrowIfNull(waitAfterRetry);
var response = getLatestResponse();
if (response is null || IsJ519ReadyForDenseExecution(response))
{
return;
}
_logger?.LogInformation("当前机器人不满足执行条件,重新启用机器人");
try
{
retryEnableRobot();

10
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
View File

@@ -8,13 +8,13 @@ namespace Flyshot.Core.Tests;
public sealed class ConfigCompatibilityTests
{
/// <summary>
/// 验证现有 RobotConfig.json 能被加载，并保持关键机器人参数与飞拍程序内容不变。
/// 验证现有旧样本配置能被加载，并保持关键机器人参数与飞拍程序内容不变。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms()
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json");
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "EOL10_EAU_0.json");
var loaded = new RobotConfigLoader().Load(configPath);
@@ -253,7 +253,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath);
@@ -277,7 +277,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
@@ -292,7 +292,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_LoadsProfileAndKinematics_FromSingleParse()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var loaded = new RobotModelLoader().LoadProfileAndKinematics(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);

108
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -21,10 +21,10 @@ public sealed class FanucControllerRuntimeDenseTests
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列，并输出角度制目标。
/// 验证真机 J519 会直接消费已经按 8ms 生成好的稠密轨迹，不再按运行时 speedRatio 临场重采样。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_UsesPlannedDenseRowsAndConvertsRadiansToDegrees()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -38,8 +38,8 @@ public sealed class FanucControllerRuntimeDenseTests
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 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -56,13 +56,19 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(5, commands.Count);
Assert.Equal(3, commands.Count);
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]));
var expectedJ1Targets = new[] { 0.0, 0.08, 0.16 };
var actualJ1Targets = commands.Select(static command => command.TargetJoints[0]).ToArray();
Assert.Equal(expectedJ1Targets.Length, actualJ1Targets.Length);
for (var index = 0; index < expectedJ1Targets.Length; index++)
{
Assert.Equal(expectedJ1Targets[index], actualJ1Targets[index], precision: 6);
}
Assert.False(j519Client.IsCommandQueueDrainedForTests());
}
@@ -90,8 +96,8 @@ public sealed class FanucControllerRuntimeDenseTests
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 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
programName: "wait-drain",
@@ -107,7 +113,7 @@ public sealed class FanucControllerRuntimeDenseTests
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);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
// 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
@@ -200,14 +206,16 @@ public sealed class FanucControllerRuntimeDenseTests
var firstTimingColumns = ParseColumns(timingLines[0]);
var secondTimingColumns = ParseColumns(timingLines[1]);
var lastTimingColumns = ParseColumns(timingLines[^1]);
Assert.Equal(4, firstTimingColumns.Length);
Assert.Equal(6, 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, firstTimingColumns[3], precision: 6);
Assert.Equal(0.5, firstTimingColumns[4], precision: 6);
Assert.Equal(0.0, firstTimingColumns[5], 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(0.008, secondTimingColumns[2], precision: 6);
Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
@@ -355,6 +363,7 @@ public sealed class FanucControllerRuntimeDenseTests
planningSpeedScale: fullSpeedSettings.PlanningSpeedScale);
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
fixture.Robot,
bundle.ExecutionTrajectory,
bundle.Result,
fixture.Robot.ServoPeriod.TotalSeconds,
speedRatio);
@@ -387,7 +396,9 @@ public sealed class FanucControllerRuntimeDenseTests
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.Equal(preparedExecution.Samples.Count, commands.Count);
AssertJointDegreesEqual(result.DenseJointTrajectory[0].Skip(1).ToArray(), commands[0].TargetJoints);
Assert.Equal(speedRatio, preparedExecution.FinalSpeedRatio, precision: 6);
Assert.Equal(0, preparedExecution.StretchIterationCount);
Assert.Equal(preparedExecution.Samples[0].JointsDegrees, commands[0].TargetJoints);
}
finally
{
@@ -463,13 +474,13 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
/// 验证 MoveJoint 会在生成阶段折算 speed_ratio，运行时只发送已经规划好的 8ms 稠密点列。
/// </summary>
[Theory]
[InlineData(1.0)]
[InlineData(0.7)]
[InlineData(0.5)]
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryWithSpeedRatioAlreadyPlanned(double speedRatio)
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -524,7 +535,7 @@ public sealed class FanucControllerRuntimeDenseTests
}
[Fact]
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
public void MoveJointTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
@@ -536,9 +547,12 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
AssertDenseRowsUseServoPeriod(fullSpeed.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed07.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed05.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 1.0));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5));
}
[Fact]
@@ -565,10 +579,10 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
/// 验证运行时稠密发送不再依赖当前 speed_ratio；倍率合法性应在上游规划/生成阶段处理。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_IgnoresRuntimeSpeedRatioForDenseFallback()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -576,12 +590,14 @@ public sealed class FanucControllerRuntimeDenseTests
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 0.0);
var denseTrajectory = new[]
{
new[] { 0.0, 0.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 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -598,9 +614,12 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
var exception = Assert.Throws<InvalidOperationException>(
() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(3, commands.Count);
Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
}
/// <summary>
@@ -819,8 +838,10 @@ public sealed class FanucControllerRuntimeDenseTests
var enableRobotRetryCount = 0;
var waitCount = 0;
using var runtime = new FanucControllerRuntime();
var exception = Record.Exception(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => runtime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
@@ -844,8 +865,10 @@ public sealed class FanucControllerRuntimeDenseTests
var enableRobotRetryCount = 0;
var waitCount = 0;
using var runtime = new FanucControllerRuntime();
var exception = Assert.Throws<InvalidOperationException>(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => runtime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
@@ -980,8 +1003,9 @@ public sealed class FanucControllerRuntimeDenseTests
var rows = result.DenseJointTrajectory!;
Assert.True(
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7),
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
AssertDenseRowsUseServoPeriod(rows, robot.ServoPeriod.TotalSeconds);
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
}
@@ -1207,8 +1231,14 @@ public sealed class FanucControllerRuntimeDenseTests
private static double ExpectedSmoothPtpDuration(
RobotProfile robot,
IReadOnlyList<double> startJoints,
IReadOnlyList<double> targetJoints)
IReadOnlyList<double> targetJoints,
double speedRatio = 1.0)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var duration = 0.0;
for (var index = 0; index < robot.DegreesOfFreedom; index++)
{
@@ -1219,15 +1249,27 @@ public sealed class FanucControllerRuntimeDenseTests
}
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);
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / (limit.VelocityLimit * speedRatio);
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / (limit.AccelerationLimit * speedRatio * speedRatio));
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / (limit.JerkLimit * speedRatio * speedRatio * speedRatio));
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
/// <summary>
/// 验证 MoveJoint 稠密轨迹已经是物理伺服周期，不把 speed_ratio 编码到采样间隔里。
/// </summary>
private static void AssertDenseRowsUseServoPeriod(IReadOnlyList<IReadOnlyList<double>> rows, double servoPeriodSeconds)
{
Assert.True(rows.Count > 1);
for (var index = 1; index < rows.Count; index++)
{
Assert.Equal(servoPeriodSeconds, rows[index][0] - rows[index - 1][0], precision: 6);
}
}
private static double DegreesToRadians(double degrees)
{
return degrees * Math.PI / 180.0;

134
tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs
View File

@@ -1,4 +1,6 @@
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Core.Tests;
@@ -43,6 +45,58 @@ public sealed class J519SendTrajectorySamplerTests
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证飞拍 speed_ratio 安全队列从连续规划样条直接采样，而不是先把规划轨迹离散成稠密点后再线性插值。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_DirectlyEvaluatesContinuousSplineForSpeedRatio()
{
var trajectory = CreateSingleJointTrajectory();
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.5);
var sample = Assert.Single(samples.Where(static item => Math.Abs(item.TrajectoryTime - 0.004) < 1e-9));
Assert.Equal(0.008, sample.SendTime, precision: 6);
Assert.Equal(0.004, sample.TrajectoryTime, precision: 6);
Assert.NotEqual(45.0, sample.JointsDegrees[0], precision: 3);
var expected = new CubicSplineInterpolator(
trajectory.WaypointTimes.ToArray(),
trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray())
.Evaluate(0.004)[0] * 180.0 / Math.PI;
Assert.Equal(expected, sample.JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证 speed_ratio 等价于执行侧时间轴拉长：原轨迹 0.8 倍执行应与时间整体拉长 1/0.8 的轨迹 1 倍执行一致。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_SpeedRatioMatchesUniformPlanningTimeStretch()
{
var trajectory = CreateSingleJointTrajectory();
var stretchedTrajectory = StretchTrajectoryTime(trajectory, stretchFactor: 1.0 / 0.8);
var ratioSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.8);
var stretchedSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
stretchedTrajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Equal(stretchedSamples.Count, ratioSamples.Count);
for (var index = 0; index < ratioSamples.Count; index++)
{
Assert.Equal(stretchedSamples[index].SendTime, ratioSamples[index].SendTime, precision: 6);
Assert.Equal(stretchedSamples[index].TrajectoryTime * 0.8, ratioSamples[index].TrajectoryTime, precision: 6);
Assert.Equal(stretchedSamples[index].JointsDegrees[0], ratioSamples[index].JointsDegrees[0], precision: 6);
}
}
/// <summary>
/// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。
/// </summary>
@@ -96,4 +150,84 @@ public sealed class J519SendTrajectorySamplerTests
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
}
/// <summary>
/// 验证倍率诊断行在保留旧 4 列的同时追加请求倍率和历史改写次数。
/// </summary>
[Fact]
public void BuildTimingRow_WithRatioDiagnostics_AppendsRequestedRatioAndIterationCount()
{
var sample = new J519SendSample(
sampleIndex: 2,
sendTime: 0.016,
trajectoryTime: 0.0076,
speedRatio: 0.475,
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var row = J519SendTrajectorySampler.BuildTimingRow(
sample,
requestedSpeedRatio: 0.5,
stretchIterationCount: 1);
Assert.Equal([2.0, 0.016, 0.0076, 0.475, 0.5, 1.0], row);
}
/// <summary>
/// 构造一个最小单关节规划轨迹，便于验证 speed_ratio 直接样条采样语义。
/// </summary>
private static PlannedTrajectory CreateSingleJointTrajectory()
{
var robot = new RobotProfile(
name: "TestRobot",
modelPath: "Models/Test.robot",
degreesOfFreedom: 1,
jointLimits: [new JointLimit("J1", 100.0, 1000.0, 10000.0)],
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
var program = new FlyshotProgram(
name: "spline-sample",
waypoints:
[
new JointWaypoint([0.0]),
new JointWaypoint([Math.PI / 2.0]),
new JointWaypoint([Math.PI])
],
shotFlags: [false, false, false],
offsetValues: [0, 0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>())
]);
return new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.008, 0.016],
segmentDurations: [0.008, 0.008],
segmentScales: [1.0, 1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
}
/// <summary>
/// 构造统一拉长时间轴后的轨迹，模拟更低 planning_speed_scale 生成的等几何时间结果。
/// </summary>
private static PlannedTrajectory StretchTrajectoryTime(PlannedTrajectory trajectory, double stretchFactor)
{
return new PlannedTrajectory(
robot: trajectory.Robot,
originalProgram: trajectory.OriginalProgram,
plannedWaypoints: trajectory.PlannedWaypoints,
waypointTimes: trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(),
segmentDurations: trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(),
segmentScales: trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(),
method: trajectory.Method,
iterations: trajectory.Iterations,
threshold: trajectory.Threshold);
}
}

2
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
View File

@@ -40,7 +40,7 @@ public sealed class OfflinePlanTests
// 1. 加载配置和模型。
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
var program = loadedConfig.Programs[trajName];
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
resolvedRobotModelPath,
loadedConfig.Robot.AccLimitScale,

140
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -67,7 +67,7 @@ public sealed class PlanningCompatibilityTests
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var config = new RobotConfigLoader().Load(configPath);
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
@@ -269,6 +269,144 @@ public sealed class PlanningCompatibilityTests
validateJerk: false);
}
/// <summary>
/// 验证飞拍最终 J519 队列的 jerk 硬约束使用严格限值，不再沿用历史上放大 4 倍的宽容口径。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_ThrowsForJ519Jerk_WhenStrictFinalQueueValidationIsEnabled()
{
var robot = CreateRobotProfile([100.0], [1000.0], [100.0]);
var samples = new[]
{
new J519SendSample(0, 0.000, 0.000, 1.0, [0.0]),
new J519SendSample(1, 0.008, 0.008, 1.0, [0.0]),
new J519SendSample(2, 0.016, 0.016, 1.0, [0.003754]),
new J519SendSample(3, 0.024, 0.024, 1.0, [0.007508])
};
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: "strict-final-j519",
validateJerk: true));
Assert.Contains("Jerk", exception.Message);
Assert.Contains("strict-final-j519", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列在请求倍率下严格 jerk 超限时直接拒绝执行，而不是自动改写 speed_ratio。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_ThrowsInsteadOfRewritingRequestedSpeedRatio()
{
var robot = CreateRobotProfile([100.0], [10000.0], [800.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "strict-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var initialSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
initialSamples,
trajectoryName: "strict-builder-initial",
validateJerk: true));
var exception = Assert.Throws<InvalidOperationException>(() =>
FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 1.0));
Assert.Contains("speed_ratio=1.000000", exception.Message);
Assert.Contains("planning_speed_scale", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列通过严格校验时，请求 speed_ratio 会原样成为最终倍率。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_KeepsRequestedSpeedRatioWhenQueuePasses()
{
var robot = CreateRobotProfile([100.0], [10000.0], [10000.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "rewrite-speedratio-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var prepared = FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 0.8);
Assert.Equal(0.8, prepared.RequestSpeedRatio, precision: 6);
Assert.Equal(0.8, prepared.FinalSpeedRatio, precision: 6);
Assert.Equal(0, prepared.StretchIterationCount);
Assert.All(prepared.Samples, sample => Assert.Equal(0.8, sample.SpeedRatio, precision: 6));
Assert.All(prepared.TimingRows, row =>
{
Assert.Equal(0.8, row[3], precision: 6);
Assert.Equal(0.8, row[4], precision: 6);
Assert.Equal(0.0, row[5], precision: 6);
});
}
/// <summary>
/// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
/// </summary>

325
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -158,6 +158,83 @@ public sealed class RuntimeOrchestrationTests
$"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
}
/// <summary>
/// 验证飞拍生成轨迹时会把 speed_ratio 折算进有效规划倍率，使倍率乘积相同的配置生成等价轨迹。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesSpeedRatioAsPlanningScaleFactor()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var planningSideSlowdown = CreateFlyshotSettings(planningSpeedScale: 0.45, speedRatio: 1.0);
var runtimeSideSlowdown = CreateFlyshotSettings(planningSpeedScale: 0.9, speedRatio: 0.5);
var planningSideBundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: planningSideSlowdown);
var runtimeSideBundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: runtimeSideSlowdown);
Assert.Equal(planningSideBundle.Result.Duration.TotalSeconds, runtimeSideBundle.Result.Duration.TotalSeconds, precision: 6);
Assert.Equal(planningSideBundle.ExecutionTrajectory.WaypointTimes.Count, runtimeSideBundle.ExecutionTrajectory.WaypointTimes.Count);
for (var index = 0; index < planningSideBundle.ExecutionTrajectory.WaypointTimes.Count; index++)
{
Assert.Equal(
planningSideBundle.ExecutionTrajectory.WaypointTimes[index],
runtimeSideBundle.ExecutionTrajectory.WaypointTimes[index],
precision: 6);
}
Assert.Equal(planningSideBundle.Result.DenseJointTrajectory!.Count, runtimeSideBundle.Result.DenseJointTrajectory!.Count);
for (var rowIndex = 0; rowIndex < planningSideBundle.Result.DenseJointTrajectory.Count; rowIndex++)
{
var planningSideRow = planningSideBundle.Result.DenseJointTrajectory[rowIndex];
var runtimeSideRow = runtimeSideBundle.Result.DenseJointTrajectory[rowIndex];
Assert.Equal(planningSideRow.Count, runtimeSideRow.Count);
for (var columnIndex = 0; columnIndex < planningSideRow.Count; columnIndex++)
{
Assert.Equal(planningSideRow[columnIndex], runtimeSideRow[columnIndex], precision: 6);
}
}
}
/// <summary>
/// 验证普通轨迹生成也会把 speed_ratio 折算进规划倍率，避免把限速留到发送阶段处理。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_UsesSpeedRatioAsPlanningScaleFactor()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.001, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.002, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.003, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var planningSideBundle = orchestrator.PlanOrdinaryTrajectory(
robot,
waypoints,
planningSpeedScale: 0.45);
var runtimeSideBundle = orchestrator.PlanOrdinaryTrajectory(
robot,
waypoints,
planningSpeedScale: 0.9,
speedRatio: 0.5);
Assert.Equal(planningSideBundle.Result.Duration.TotalSeconds, runtimeSideBundle.Result.Duration.TotalSeconds, precision: 6);
Assert.Equal(planningSideBundle.Result.DenseJointTrajectory!.Count, runtimeSideBundle.Result.DenseJointTrajectory!.Count);
for (var rowIndex = 0; rowIndex < planningSideBundle.Result.DenseJointTrajectory.Count; rowIndex++)
{
var planningSideRow = planningSideBundle.Result.DenseJointTrajectory[rowIndex];
var runtimeSideRow = runtimeSideBundle.Result.DenseJointTrajectory[rowIndex];
for (var columnIndex = 0; columnIndex < planningSideRow.Count; columnIndex++)
{
Assert.Equal(planningSideRow[columnIndex], runtimeSideRow[columnIndex], precision: 6);
}
}
}
/// <summary>
/// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。
/// </summary>
@@ -314,35 +391,37 @@ public sealed class RuntimeOrchestrationTests
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var legacyFitSettings = DisableSmoothStartStopTiming(fixture.Settings);
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings);
settings: legacyFitSettings);
var baselineBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
var executionDense = TrajectorySampler.SampleJointTrajectory(
bundle.ExecutionTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
Assert.Equal(0.7422771653721995, fixture.Settings.PlanningSpeedScale, precision: 12);
Assert.False(fixture.Settings.SmoothStartStopTiming);
Assert.Equal(0.74227, legacyFitSettings.PlanningSpeedScale, precision: 6);
Assert.False(legacyFitSettings.SmoothStartStopTiming);
Assert.Equal(fixture.Uploaded.Waypoints.Count, bundle.PlannedTrajectory.WaypointTimes.Count);
Assert.Equal(
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / fixture.Settings.PlanningSpeedScale,
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / legacyFitSettings.PlanningSpeedScale,
bundle.PlannedTrajectory.WaypointTimes[^1],
precision: 6);
Assert.Equal(7.403046, bundle.PlannedTrajectory.WaypointTimes[^1], precision: 3);
// 关闭二次时间重映射时，运行时稠密点应直接使用规划样条采样，避免再次改变通用规划时间轴。
Assert.Equal(rawDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(rawDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
// 关闭二次时间重映射时，稠密点应和最终执行时间轴自洽；若离散限幅额外拉长，也要反映在 ExecutionTrajectory 上。
Assert.Equal(executionDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(executionDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
Assert.True(bundle.ExecutionTrajectory.WaypointTimes[^1] >= bundle.PlannedTrajectory.WaypointTimes[^1]);
Assert.True(
Math.Abs(bundle.PlannedTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留规划终点时间，planned={bundle.PlannedTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
Math.Abs(bundle.ExecutionTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留最终执行时间轴终点，execution={bundle.ExecutionTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
}
/// <summary>
@@ -697,6 +776,54 @@ public sealed class RuntimeOrchestrationTests
Assert.Throws<ArgumentException>(Act);
}
/// <summary>
/// 验证兼容服务执行普通轨迹时会把运行时 speed_ratio 前移到规划阶段。
/// </summary>
[Fact]
public void ControllerClientCompatService_ExecuteTrajectory_PassesSpeedRatioIntoOrdinaryPlanning()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot, planningSpeedScale: 0.9, speedRatio: 1.0);
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var runtime = new RecordingControllerRuntime();
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
var waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.001, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.002, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.003, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
runtime.SetSpeedRatio(0.5);
service.ExecuteTrajectory(waypoints);
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
var expected = new ControllerClientTrajectoryOrchestrator()
.PlanOrdinaryTrajectory(robot, waypoints, planningSpeedScale: 0.45);
var actual = Assert.IsType<TrajectoryResult>(runtime.LastExecutedResult);
Assert.Equal(expected.Result.Duration.TotalSeconds, actual.Duration.TotalSeconds, precision: 6);
Assert.Equal(expected.Result.DenseJointTrajectory!.Count, actual.DenseJointTrajectory!.Count);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
/// </summary>
@@ -706,6 +833,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -750,6 +878,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -793,6 +922,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -960,7 +1090,7 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并按执行侧稠密轨迹时长应用当前 speed_ratio。
/// 验证 SaveTrajectoryInfo 会导出按 J519 8ms 实发周期重采样的点位，并保留当前 speed_ratio 诊断信息。
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
@@ -994,17 +1124,21 @@ public sealed class RuntimeOrchestrationTests
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var shotEventsJson = File.ReadAllText(shotEventsPath);
var executionDuration = double.Parse(
var planningExportDuration = double.Parse(
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
CultureInfo.InvariantCulture);
var minimumExpectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1;
var planningExportRowsAtServoPeriod = (int)Math.Ceiling(Math.Max(0.0, (planningExportDuration / 0.008) - 1e-9)) + 1;
Assert.Equal(pointRows.Length, timingRows.Length);
Assert.True(pointRows.Length >= minimumExpectedRows, $"最终发送点数应不少于请求倍率的首轮候选值，actual={pointRows.Length}, min={minimumExpectedRows}");
Assert.True(
pointRows.Length > planningExportRowsAtServoPeriod,
$"ActualSend 应按当前 speedRatio 的执行视图导出，不能再用旧 JointDetialTraj 规划时长约束点数。actual={pointRows.Length}, planningRows={planningExportRowsAtServoPeriod}");
Assert.Equal(0.0, pointRows[0][0], precision: 6);
Assert.Equal(0.008, pointRows[1][0], precision: 6);
Assert.True(timingRows[1][2] <= 0.004 + 1e-6, $"自动拉长后 trajectory_time 推进不应快于请求倍率，actual={timingRows[1][2]:F6}");
Assert.True(timingRows[1][3] <= 0.5 + 1e-6, $"最终采用倍率不应快于请求倍率，actual={timingRows[1][3]:F6}");
Assert.Equal(0.008, timingRows[1][2], precision: 6);
Assert.Equal(1.0, timingRows[1][3], precision: 6);
Assert.Equal(0.5, timingRows[1][4], precision: 6);
Assert.Equal(0.0, timingRows[1][5], precision: 6);
Assert.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson);
Assert.Contains("\"selected_sample_index\"", shotEventsJson);
}
@@ -1015,11 +1149,56 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图，
/// 而不是再次从 PlannedTrajectory 独立重采样。
/// 验证 SaveTrajectoryInfo 的旧格式规划导出只受 planning_speed_scale 影响，不跟随运行时 speed_ratio 改变。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample()
public void ControllerClientCompatService_SaveTrajectoryInfo_KeepsLegacyPlanningExportsIndependentFromSpeedRatio()
{
var fullSpeedRoot = CreateTempConfigRoot();
var halfSpeedRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(fullSpeedRoot, planningSpeedScale: 0.45, speedRatio: 1.0);
WriteRobotConfigWithDemoTrajectory(halfSpeedRoot, planningSpeedScale: 0.45, speedRatio: 0.5);
SaveDemoTrajectoryInfo(fullSpeedRoot);
SaveDemoTrajectoryInfo(halfSpeedRoot);
var fullSpeedOutput = Path.Combine(fullSpeedRoot, "Data", "demo-flyshot");
var halfSpeedOutput = Path.Combine(halfSpeedRoot, "Data", "demo-flyshot");
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "JointTraj.txt"),
Path.Combine(halfSpeedOutput, "JointTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "JointDetialTraj.txt"),
Path.Combine(halfSpeedOutput, "JointDetialTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "CartTraj.txt"),
Path.Combine(halfSpeedOutput, "CartTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "CartDetialTraj.txt"),
Path.Combine(halfSpeedOutput, "CartDetialTraj.txt"));
var fullSpeedActualSend = File.ReadAllLines(Path.Combine(fullSpeedOutput, "ActualSendJointTraj.txt"));
var halfSpeedActualSend = File.ReadAllLines(Path.Combine(halfSpeedOutput, "ActualSendJointTraj.txt"));
var fullSpeedTiming = File.ReadAllLines(Path.Combine(fullSpeedOutput, "ActualSendTiming.txt"));
var halfSpeedTiming = File.ReadAllLines(Path.Combine(halfSpeedOutput, "ActualSendTiming.txt"));
Assert.NotEqual(fullSpeedActualSend.Length, halfSpeedActualSend.Length);
Assert.NotEqual(fullSpeedTiming.Length, halfSpeedTiming.Length);
}
finally
{
Directory.Delete(fullSpeedRoot, recursive: true);
Directory.Delete(halfSpeedRoot, recursive: true);
}
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自旧规划导出视图的 16ms 明细轨迹，
/// 而不是来自 speed_ratio 后的实发执行视图。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExportPlanningDenseDownsample()
{
var fixture = LoadUttcMs11RuntimeFixture();
var configRoot = CreateTempConfigRoot();
@@ -1028,20 +1207,26 @@ public sealed class RuntimeOrchestrationTests
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader());
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
var exportBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var executionBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0,
speedRatio: 0.5);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, exportBundle, executionBundle, speedRatio: 0.5);
var outputDir = Path.Combine(configRoot, "Data", "UTTC_MS11");
var exportedRows = File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt"))
.Select(ParseSpaceSeparatedDoubles)
.ToArray();
var expectedRows = DownsampleDenseRows(
bundle.Result.DenseJointTrajectory!,
exportBundle.Result.DenseJointTrajectory!,
samplePeriodSeconds: 0.016)
.Select(static row => row.ToArray())
.ToArray();
@@ -1085,16 +1270,21 @@ public sealed class RuntimeOrchestrationTests
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。
/// </summary>
/// <param name="configRoot">测试运行配置根。</param>
private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
private static void WriteRobotConfigWithDemoTrajectory(
string configRoot,
double planningSpeedScale = 0.45,
double speedRatio = 1.0)
{
File.WriteAllText(
Path.Combine(configRoot, "RobotConfig.json"),
"""
$$"""
{
"robot": {
"use_do": true,
"io_addr": [7, 8],
"io_keep_cycles": 2,
"speed_ratio": {{speedRatio.ToString(CultureInfo.InvariantCulture)}},
"planning_speed_scale": {{planningSpeedScale.ToString(CultureInfo.InvariantCulture)}},
"acc_limit": 1.0,
"jerk_limit": 1.0,
"adapt_icsp_try_num": 5
@@ -1103,9 +1293,9 @@ public sealed class RuntimeOrchestrationTests
"demo-flyshot": {
"traj_waypoints": [
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
],
"shot_flags": [false, true, false, false],
"offset_values": [0, 1, 0, 0],
@@ -1116,6 +1306,42 @@ public sealed class RuntimeOrchestrationTests
""");
}
/// <summary>
/// 使用指定运行配置根执行 demo-flyshot 的 SaveTrajectoryInfo。
/// </summary>
private static void SaveDemoTrajectoryInfo(string configRoot)
{
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
new RecordingControllerRuntime(),
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SaveTrajectoryInfo("demo-flyshot");
}
/// <summary>
/// 按数值逐行比较两个轨迹文本文件，避免纯文本格式差异影响判断。
/// </summary>
private static void AssertNumericFilesEqual(string expectedPath, string actualPath)
{
var expectedRows = File.ReadAllLines(expectedPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var actualRows = File.ReadAllLines(actualPath).Select(ParseSpaceSeparatedDoubles).ToArray();
Assert.Equal(expectedRows.Length, actualRows.Length);
for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++)
{
Assert.Equal(expectedRows[rowIndex].Length, actualRows[rowIndex].Length);
for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++)
{
Assert.Equal(expectedRows[rowIndex][columnIndex], actualRows[rowIndex][columnIndex], precision: 6);
}
}
}
/// <summary>
/// 角度转弧度，供轨迹整形测试构造输入。
/// </summary>
@@ -1183,6 +1409,43 @@ public sealed class RuntimeOrchestrationTests
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
speedRatio: settings.SpeedRatio,
smoothStartStopTiming: true);
}
/// <summary>
/// 为 legacy-fit 对比测试显式关闭二次时间重映射。
/// </summary>
private static CompatibilityRobotSettings DisableSmoothStartStopTiming(CompatibilityRobotSettings settings)
{
return new CompatibilityRobotSettings(
useDo: settings.UseDo,
ioAddresses: settings.IoAddresses,
ioKeepCycles: settings.IoKeepCycles,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles,
accLimitScale: settings.AccLimitScale,
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
speedRatio: settings.SpeedRatio,
smoothStartStopTiming: false);
}
/// <summary>
/// 构造飞拍倍率测试使用的最小兼容设置。
/// </summary>
private static CompatibilityRobotSettings CreateFlyshotSettings(double planningSpeedScale, double speedRatio)
{
return new CompatibilityRobotSettings(
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 2,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5,
planningSpeedScale: planningSpeedScale,
speedRatio: speedRatio,
smoothStartStopTiming: true);
}
@@ -1555,9 +1818,9 @@ internal static class TestRobotFactory
waypoints:
[
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
],
shotFlags: [false, true, false, false],
offsetValues: [0, 1, 0, 0],