diff --git a/docs/flyshot-speedratio-execution-design-20260509.md b/docs/flyshot-speedratio-execution-design-20260509.md new file mode 100644 index 0000000..f7131a7 --- /dev/null +++ b/docs/flyshot-speedratio-execution-design-20260509.md @@ -0,0 +1,457 @@ +# 飞拍任务 `speedRatio` 执行侧调速设计 + +## 1. 背景 + +当前系统同时存在两个速度相关参数： + +- `planning_speed_scale` + - 规划阶段参数。 + - 作用是让规划结果在时长与整体动力学上更接近现场旧系统或当前配置要求。 + - 启动后固定，不希望在运行中再修改语义。 +- `speedRatio` + - 运行阶段参数。 + - 期望成为统一在线调速入口。 + - 本次范围内只要求对“下一次开始执行的飞拍任务”生效，不要求对正在执行中的任务中途热切换。 + +本次任务已经明确收敛： + +- 只修改飞拍任务链路。 +- 不修改 `move_joint`。 +- 不通过放宽阈值、关闭校验或只看平均速度来规避问题。 + +## 2. 现状问题 + +当前飞拍执行链路的核心语义是： + +1. 规划层先生成 `DenseJointTrajectory`。 +2. 运行时保持 `8ms` 物理发送周期不变。 +3. 对第 `k` 个发送点，按 `trajectoryTime = sendTime * speedRatio` 回映射到规划轨迹时间。 +4. 再在原始稠密轨迹上做线性插值，得到实际要发的关节目标。 +5. 最后再用离散差分检查 `vel/acc/jerk`。 + +该结构在 `speedRatio < 1` 时有天然风险： + +- 物理发送周期固定为 `8ms`，但轨迹时间推进被压缩。 +- 线性插值会把规划轨迹中原本“看起来连续”的局部变化映射成新的离散点列。 +- 这些新点列在三阶后向差分上容易形成尖峰。 +- 该风险在 `speedRatio = 0.5` 时已经暴露为明显的 Jerk 超限告警。 + +问题本质不是“平均速度是否合理”，而是： + +- 最终 `8ms` 发送点列本身没有先被当作一等公民建模。 +- `speedRatio` 的执行语义落在“发送前临场回采样”阶段。 +- 离散约束校验发生得太晚，只能报错或事后分析。 + +## 3. 目标 + +本设计目标如下： + +- 保持 `planning_speed_scale` 只属于规划层。 +- `speedRatio` 只作用于飞拍执行层，且仅对下一次启动的飞拍任务生效。 +- 任意有效 `speedRatio` 下，最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。 +- 不依赖“插值后自然会平滑”的经验假设。 +- 当 `speedRatio = 0.5` 等较慢倍率导致候选点列不满足约束时，系统自动拉长执行时长，直到点列通过校验。 +- 不破坏现有 `shot timeline / sample offset` 语义。 +- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。 + +## 4. 非目标 + +本次设计明确不包含以下内容： + +- 不修改 `move_joint` 的调速和生成机制。 +- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。 +- 不修改 `planning_speed_scale` 的含义。 +- 不在任务执行中途支持 `speedRatio` 热切换。 +- 不通过调大容差、降低限值检查强度或直接跳过 Jerk 检查来“解决问题”。 + +## 5. 设计总览 + +飞拍执行链路调整为“两段式”： + +1. 规划段 + - 维持当前实现。 + - 仍由 `planning_speed_scale` 参与规划时长和规划层轨迹生成。 + - 输出 `TrajectoryResult.DenseJointTrajectory`、`TriggerTimeline`、`ShotEvents` 等规划结果。 +2. 执行准备段 + - 新增“飞拍执行侧最终发送序列生成器”。 + - 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。 + - 生成最终 `8ms` 发送队列。 + - 对最终队列做逐周期 `vel/acc/jerk` 校验。 + - 如果不通过，则自动拉长执行时长后重建队列，直到通过。 +3. 发送段 + - 运行时只消费已经准备好的最终 `8ms` 队列。 + - 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。 + +核心变化是： + +- `speedRatio` 不再直接驱动“发送前临场插值”。 +- `speedRatio` 改为驱动“执行侧最终队列构建”。 +- 最终队列一旦生成并校验通过，运行时只负责按 J519 状态包节奏出队发送。 + +## 6. 统一语义 + +本次只改飞拍任务，但仍需把飞拍内部语义说清楚，避免再次混淆规划倍率和执行倍率。 + +### 6.1 `planning_speed_scale` + +- 只影响规划结果。 +- 影响 `DenseJointTrajectory` 的时间轴与规划层速度/加速度/Jerk 使用方式。 +- 飞拍开始执行前就已经固定。 +- 在线设置 `speedRatio` 时不应反向修改它。 + +### 6.2 `speedRatio` + +- 只影响飞拍执行准备阶段。 +- 只对下一次启动的飞拍任务生效。 +- 表示“用户期望的执行层时间推进速度”。 +- 该值先用于构建候选发送队列。 +- 如果候选发送队列在离散 `8ms` 点列上不满足约束，则系统自动进一步拉长执行时长，直到满足约束。 + +也就是说： + +- `speedRatio` 是执行目标倍率。 +- 但最终真实执行时长允许比该目标更保守。 +- 保守的原因不是放宽标准，而是严格保证离散动力学约束。 + +## 7. 最终发送序列生成 + +### 7.1 新增执行侧生成器 + +建议新增一个飞拍专用生成器，命名可类似： + +- `FlyshotExecutionSendSequenceBuilder` +- 或 `FlyshotExecutionQueueBuilder` + +职责： + +- 输入规划层 `DenseJointTrajectory`。 +- 输入 `durationSeconds`、`servoPeriodSeconds`、`speedRatio`。 +- 输出最终真实发送的 `8ms` 点列。 +- 输出与该点列一致的触发绑定结果。 +- 输出校验与自动拉长过程中的诊断信息。 + +### 7.2 第一版候选队列 + +候选队列仍可以沿用当前时间轴语义作为起点： + +- `sendTime = sampleIndex * 0.008` +- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)` + +但该候选队列只作为“第一轮尝试”，不能直接视为最终执行结果。 + +### 7.3 自动拉长策略 + +当候选队列的离散 `vel/acc/jerk` 校验失败时： + +- 不修改规划层轨迹。 +- 不修改 `planning_speed_scale`。 +- 不放宽校验阈值。 +- 只在执行侧拉长最终发送时长，然后重新构建候选队列。 + +可实现为等价的两种方式之一： + +1. 延长物理发送总点数，使发送总时长变长。 +2. 在保持 `8ms` 周期不变的前提下，降低执行侧有效轨迹时间推进速度。 + +本质上二者等价，建议统一落成第二种表达： + +- 对外仍说“自动拉长执行时长”。 +- 对内通过更保守的 `trajectoryTime(sendTime)` 映射来实现。 + +### 7.4 自动拉长的迭代规则 + +建议采用单调保守策略： + +1. 先按请求 `speedRatio` 构建第 1 版候选队列。 +2. 对候选队列做离散校验。 +3. 若失败，则按固定步长或倍率逐轮拉长，再次构建候选队列。 +4. 一旦通过，立即停止迭代，产出最终队列。 +5. 若超过安全迭代上限仍未通过，则拒绝执行并输出首个超限诊断。 + +这样可以保证： + +- 自动拉长过程是可解释、可记录的。 +- 不会因为局部修补而引入新的不可预测尖峰。 + +## 8. 逐周期约束校验 + +最终发送队列必须在执行前通过离散检查。 + +### 8.1 校验对象 + +校验对象必须是最终真实发送的 `8ms` 点列，而不是： + +- 规划层原始稠密轨迹， +- 中间候选插值轨迹， +- 或平均时长推导。 + +### 8.2 校验指标 + +保留现有逐周期离散差分口径： + +- 速度 +- 加速度 +- Jerk + +并继续使用当前机器人模型和运行时限值： + +- `velocity_eff = velocity_base` +- `acceleration_eff = acceleration_base * acc_limit` +- `jerk_eff = jerk_base * jerk_limit` + +### 8.3 校验失败的处理 + +飞拍链路按本次确认采用 A： + +- 若失败，优先自动拉长执行时长重试。 +- 只有在超过拉长上限后仍失败时才拒绝执行。 + +拒绝执行时必须输出： + +- 首次失败的轴 +- 时间窗 +- 指标类型 +- `actual / limit / ratio` +- 失败发生在哪一轮自动拉长尝试中 + +## 9. 触发时序与绑定 + +### 9.1 保持现有语义 + +以下语义不变： + +- `TriggerTimeline` +- `ShotEvents` +- `TriggerSampleIndexOffsetCycles` +- `io_keep_cycles` + +### 9.2 绑定对象改为最终发送队列 + +当前实现中，触发通常绑定到由回采样生成的 `samples`。 + +本次改为： + +- 先生成最终发送队列。 +- 再用该最终队列做 `TriggerSampleBinder.Bind(...)`。 + +这样可以保证： + +- 触发绑定和真实发送完全一致。 +- 导出工件中的 `ShotEvents.json` 与真实执行时序一致。 +- 自动拉长后触发点仍可被精确追溯到最终发送点索引。 + +### 9.3 关于触发数量与顺序 + +自动拉长执行时长时： + +- 触发数量不能变。 +- 触发顺序不能变。 +- 只允许触发在最终发送队列中的绑定索引后移或保持可解释等价。 + +## 10. 运行时职责调整 + +本次需要显式收紧 `FanucControllerRuntime` 的职责。 + +### 10.1 改造前 + +`FanucControllerRuntime.SendDenseTrajectory(...)` 当前同时负责： + +- 读取 `DenseJointTrajectory` +- 根据 `_speedRatio` 回采样 +- 绑定触发 +- 生成命令队列 +- 写出 `ActualSend*` 工件 + +### 10.2 改造后 + +`FanucControllerRuntime` 只负责： + +- 接收已经准备好的最终发送队列 +- 接收与该队列一致的触发绑定结果 +- 预构建命令队列并交给 J519 客户端 +- 记录真实发送过程与导出工件 + +不再负责： + +- 按 `_speedRatio` 从 `DenseJointTrajectory` 临场生成发送点 + +这样做的目的是让： + +- 运行时负责发送。 +- 执行准备层负责离散稳定性。 + +两层边界更清晰，也更利于测试。 + +## 11. 导出工件与日志 + +### 11.1 工件统一基于最终发送队列 + +以下文件必须全部从最终发送队列生成： + +- `ActualSendJointTraj.txt` +- `ActualSendTiming.txt` +- `ActualSendJerkStats.txt` +- `ShotEvents.json` + +不允许再出现： + +- 导出文件基于一套样本， +- 真实发送队列又基于另一套样本。 + +### 11.2 推荐新增日志字段 + +每次飞拍执行建议至少记录： + +- 请求 `speedRatio` +- 规划层轨迹时长 +- 第一轮候选队列点数 +- 第一轮候选队列是否通过校验 +- 若失败，首个失败窗口的： + - 关节轴 + - 时间区间 + - 指标类型 + - `actual` + - `limit` + - `ratio` +- 自动拉长轮数 +- 最终采用的执行时长 +- 最终发送队列点数 +- 最终触发绑定数量 +- 最终校验通过结论 + +### 11.3 日志级别建议 + +- `Information` + - 记录执行请求、最终采用结果、最终通过结论 +- `Warning` + - 记录第一轮候选失败与自动拉长启动 +- `Debug` + - 记录每一轮拉长的中间参数与详细差分统计 + +## 12. 验收口径 + +### 12.1 功能验收 + +- 飞拍 `speedRatio` 可在线设置。 +- 该值对下一次启动的飞拍任务生效。 +- 不需要修改 `planning_speed_scale`。 + +### 12.2 约束验收 + +在 `speedRatio = {1.0, 0.8, 0.5}` 下： + +- 最终发送队列逐周期 `vel/acc/jerk` 不超限。 +- 不出现固定区间正负交替的大幅 Jerk 尖峰。 + +### 12.3 工件与日志验收 + +- `ActualSend*` 文件能反映最终真实发送点位与时间映射。 +- 日志能定位自动拉长前后的关键参数和校验结果。 + +### 12.4 回归验收 + +- `speedRatio = 1.0` 时不退化。 +- 触发事件数量不异常漂移。 +- 触发绑定顺序不异常漂移。 + +## 13. 测试方案 + +### 13.1 单元测试 + +建议新增： + +- 飞拍执行侧最终发送序列生成器测试 +- `speedRatio` 非法边界值测试 +- 第一轮候选失败后自动拉长成功测试 +- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试 +- 触发绑定始终与最终发送队列一致的测试 + +### 13.2 编排测试 + +建议补充： + +- `ExecuteFlyShotTraj` 进入运行时前，已经拿到最终发送队列 +- `FanucControllerRuntime` 不再自行按 `_speedRatio` 对飞拍轨迹回采样 + +### 13.3 集成与黄金样本 + +建议至少覆盖： + +- `UTTC_MS11` +- `speedRatio = 1.0` +- `speedRatio = 0.8` +- `speedRatio = 0.5` + +检查项： + +- 最终发送点数 +- 最终发送时长 +- `ActualSendTiming` +- `ActualSendJointTraj` +- `ActualSendJerkStats` +- `ShotEvents` 绑定结果 +- 是否消除当前已知 Jerk 尖峰 + +## 14. 代码落点建议 + +建议实现边界如下： + +### 14.1 `src/Flyshot.Core.Planning/Sampling/` + +新增飞拍执行侧最终发送序列生成器及相关结果类型，负责： + +- 候选队列构建 +- 自动拉长 +- 离散校验 +- 触发绑定输入准备 + +### 14.2 `src/Flyshot.ControllerClientCompat/` + +在飞拍执行准备阶段调用该生成器，把最终发送队列放入执行结果或新的执行上下文对象。 + +### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs` + +调整 `SendDenseTrajectory(...)`： + +- 不再自行根据 `_speedRatio` 从 `DenseJointTrajectory` 临场重采样 +- 直接消费已经构建好的最终发送队列 + +### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs` + +`ActualSend*` 导出改为复用最终发送队列，确保导出工件与运行时一致。 + +### 14.5 `tests/Flyshot.Core.Tests/` + +补飞拍专项单测、UTTC 回归和工件一致性测试。 + +## 15. 风险与注意事项 + +### 15.1 不能把自动拉长误解为“偷偷改 speedRatio” + +对外语义仍应保留： + +- 用户请求的是某个 `speedRatio` +- 系统为了满足离散动力学约束，自动采用了更保守的最终执行时长 + +日志里必须把这件事说清楚，不能让现场误以为参数未生效。 + +### 15.2 不能让触发绑定脱离最终队列 + +只要最终发送点数变化，触发绑定就必须重新基于最终队列计算，否则工件与执行会再次分叉。 + +### 15.3 `speedRatio = 1.0` 必须优先做回归保护 + +这是最容易被“执行侧重构”误伤的路径，必须作为首要回归项。 + +## 16. 结论 + +本次飞拍调速设计的核心结论是： + +- `planning_speed_scale` 继续只属于规划层。 +- `speedRatio` 只属于飞拍执行层，并且只对下一次启动的飞拍任务生效。 +- 飞拍执行必须先生成最终 `8ms` 发送队列，再对该队列做逐周期 `vel/acc/jerk` 校验。 +- 若 `speedRatio < 1` 导致第一轮候选队列不满足约束，则自动拉长执行时长后重建，直到通过。 +- 运行时不再对飞拍轨迹做发送前临场回采样。 +- 触发绑定、导出工件与真实发送必须全部统一到同一份最终发送队列上。 + +该方案可以在不修改 `move_joint`、不放宽校验阈值、不中断现有触发语义的前提下，把飞拍 `speedRatio` 调速从“经验插值”收敛为“可校验、可追踪、可自动保守化”的执行机制。 diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index c56e8e9..0a83817 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -1,5 +1,6 @@ using Flyshot.Core.Config; using Flyshot.Core.Domain; +using Flyshot.Core.Planning.Sampling; using Flyshot.Runtime.Common; using Microsoft.Extensions.Logging; @@ -492,6 +493,7 @@ 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); _logger?.LogInformation( "ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}", @@ -621,6 +623,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method), planningSettings, planningSettings.PlanningSpeedScale); + bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio); _logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地"); ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle); @@ -658,6 +661,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory), planningSettings, planningSettings.PlanningSpeedScale); + bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio); ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle); duration = bundle.Result.Duration; @@ -794,6 +798,37 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi _artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio); } + /// + /// 为飞拍链路预先构建最终发送队列，确保运行时只消费已经离散校验通过的 8ms 点列。 + /// + private static PlannedExecutionBundle PrepareFlyshotExecutionBundle( + RobotProfile robot, + PlannedExecutionBundle bundle, + double speedRatio) + { + var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build( + robot, + bundle.Result, + robot.ServoPeriod.TotalSeconds, + speedRatio); + var preparedResult = new TrajectoryResult( + programName: bundle.Result.ProgramName, + method: bundle.Result.Method, + isValid: bundle.Result.IsValid, + duration: bundle.Result.Duration, + shotEvents: bundle.Result.ShotEvents, + triggerTimeline: bundle.Result.TriggerTimeline, + artifacts: bundle.Result.Artifacts, + failureReason: bundle.Result.FailureReason, + usedCache: bundle.Result.UsedCache, + originalWaypointCount: bundle.Result.OriginalWaypointCount, + plannedWaypointCount: bundle.Result.PlannedWaypointCount, + triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles, + denseJointTrajectory: bundle.Result.DenseJointTrajectory, + preparedFlyshotExecution: preparedExecution); + return new PlannedExecutionBundle(bundle.PlannedTrajectory, bundle.ShotTimeline, preparedResult, preparedExecution); + } + /// /// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置；失败时返回 null。 /// diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index 3efc444..bdf7b26 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -53,6 +53,8 @@ public sealed class ControllerClientTrajectoryOrchestrator /// /// 当前机器人配置。 /// 普通轨迹关节路点。 + /// 执行参数。 + /// 规划速度倍率。 /// 包含规划轨迹、空触发时间轴和执行结果的结果包。 public PlannedExecutionBundle PlanOrdinaryTrajectory( RobotProfile robot, @@ -107,6 +109,9 @@ public sealed class ControllerClientTrajectoryOrchestrator /// /// 当前机器人配置。 /// 兼容层保存的上传轨迹。 + /// 执行参数。 + /// 兼容层机器人设置。 + /// 规划速度倍率。 /// 包含规划轨迹、触发时间轴和执行结果的结果包。 public PlannedExecutionBundle PlanUploadedFlyshot( RobotProfile robot, @@ -135,21 +140,13 @@ public sealed class ControllerClientTrajectoryOrchestrator var method = ParseFlyshotMethod(options.Method); var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale); - //if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle)) - //{ - // _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey); - // var executionTrajectory = ApplyExecutionTiming(cachedBundle.PlannedTrajectory, settings); - // var executionTimeline = _shotTimelineBuilder.Build( - // executionTrajectory, - // holdCycles: settings.IoKeepCycles, - // samplePeriod: planningRobot.ServoPeriod, - // useDo: settings.UseDo); - // // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。 - // return new PlannedExecutionBundle( - // cachedBundle.PlannedTrajectory, - // executionTimeline, - // CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false)); - //} + if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle)) + { + _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey); + + // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。 + return cachedBundle; + } var request = new TrajectoryRequest( robot: planningRobot, diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs index 513d332..7bc823f 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs @@ -101,41 +101,55 @@ public sealed class FlyshotTrajectoryArtifactWriter { ArgumentNullException.ThrowIfNull(robot); - if (result.DenseJointTrajectory is null) + var preparedExecution = result.PreparedFlyshotExecution; + if (result.DenseJointTrajectory is null && preparedExecution is null) { return; } - if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio)) + if (preparedExecution is null && (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))) { throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。"); } - - var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory( - result.DenseJointTrajectory, - result.Duration.TotalSeconds, - ActualSendServoPeriodSeconds, - speedRatio); - - - try - { - TrajectoryLimitValidator.ValidateJ519SendSamples( - robot, + var samples = preparedExecution is null + ? J519SendTrajectorySampler.SampleDenseJointTrajectory( + result.DenseJointTrajectory!, + result.Duration.TotalSeconds, + ActualSendServoPeriodSeconds, + speedRatio) + : preparedExecution.Samples.Select(static sample => new J519SendSample( + sample.SampleIndex, + sample.SendTime, + sample.TrajectoryTime, + sample.SpeedRatio, + sample.JointsDegrees)).ToArray(); + var triggerBindings = preparedExecution is null + ? TriggerSampleBinder.Bind( + result.TriggerTimeline, samples, - trajectoryName: result.ProgramName); - } - catch (Exception e) - { - _logger?.LogError(e, "ValidateJ519SendSamples 失败，program={ProgramName}", result.ProgramName); - } - var jointRows = new List>(samples.Count); - var timingRows = new List>(samples.Count); - var jerkRows = new List>(); - var triggerBindings = TriggerSampleBinder.Bind( - result.TriggerTimeline, + result.TriggerSampleIndexOffsetCycles) + : preparedExecution.TriggerBindings.Select(static binding => + new TriggerSampleBinding( + binding.Trigger, + new J519SendSample( + binding.Sample.SampleIndex, + binding.Sample.SendTime, + binding.Sample.TrajectoryTime, + binding.Sample.SpeedRatio, + binding.Sample.JointsDegrees), + binding.SampleIndex, + binding.FoundInWindow)).ToArray(); + TrajectoryLimitValidator.ValidateJ519SendSamples( + robot, samples, - result.TriggerSampleIndexOffsetCycles); + trajectoryName: result.ProgramName); + var jointRows = new List>(samples.Count); + List> timingRows = preparedExecution is null + ? new List>(samples.Count) + : preparedExecution.TimingRows.Select(static row => (IReadOnlyList)row.ToArray()).ToList(); + List> jerkRows = preparedExecution is null + ? [] + : preparedExecution.JerkRows.Select(static row => (IReadOnlyList)row.ToArray()).ToList(); double? previousSendTime = null; double[]? previousJoints = null; double[]? previousVelocity = null; @@ -144,17 +158,20 @@ public sealed class FlyshotTrajectoryArtifactWriter foreach (var sample in samples) { jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees)); - timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample)); - - if (previousSendTime is not null && previousJoints is not null) + if (preparedExecution is null) { - jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow( - previousSendTime.Value, - sample.SendTime, - previousJoints, - sample.JointsDegrees, - ref previousVelocity, - ref previousAcceleration)); + timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample)); + + if (previousSendTime is not null && previousJoints is not null) + { + jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow( + previousSendTime.Value, + sample.SendTime, + previousJoints, + sample.JointsDegrees, + ref previousVelocity, + ref previousAcceleration)); + } } previousSendTime = sample.SendTime; diff --git a/src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs b/src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs index a806df5..892e20f 100644 --- a/src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs +++ b/src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs @@ -15,11 +15,16 @@ public sealed class PlannedExecutionBundle /// 规划后的轨迹。 /// 飞拍触发时间轴。 /// 对运行时和监控层暴露的规划结果。 - public PlannedExecutionBundle(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, TrajectoryResult result) + public PlannedExecutionBundle( + PlannedTrajectory plannedTrajectory, + ShotTimeline shotTimeline, + TrajectoryResult result, + FlyshotPreparedExecution? preparedExecution = null) { PlannedTrajectory = plannedTrajectory ?? throw new ArgumentNullException(nameof(plannedTrajectory)); ShotTimeline = shotTimeline ?? throw new ArgumentNullException(nameof(shotTimeline)); Result = result ?? throw new ArgumentNullException(nameof(result)); + PreparedExecution = preparedExecution; } /// @@ -36,4 +41,9 @@ public sealed class PlannedExecutionBundle /// 获取运行时可消费的规划结果。 /// public TrajectoryResult Result { get; } + + /// + /// 获取飞拍链路预先准备好的最终发送队列；普通轨迹与 move_joint 为 null。 + /// + public FlyshotPreparedExecution? PreparedExecution { get; } } diff --git a/src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs b/src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs new file mode 100644 index 0000000..418f1da --- /dev/null +++ b/src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs @@ -0,0 +1,202 @@ +namespace Flyshot.Core.Domain; + +/// +/// 表示飞拍链路在进入运行时之前就已经准备完成的最终发送结果。 +/// +public sealed class FlyshotPreparedExecution +{ + /// + /// 初始化一份飞拍最终发送结果。 + /// + /// 最终 8ms 发送点列。 + /// 与最终发送点列对齐的触发绑定结果。 + /// 与最终发送点列一致的时间映射诊断行。 + /// 与最终发送点列一致的跃度诊断行。 + /// 请求的执行倍率。 + /// 通过离散校验后实际采用的保守倍率。 + /// 最终发送总时长，单位为秒。 + /// 自动拉长执行时长的迭代次数。 + public FlyshotPreparedExecution( + IEnumerable samples, + IEnumerable triggerBindings, + IEnumerable> timingRows, + IEnumerable> jerkRows, + double requestSpeedRatio, + double finalSpeedRatio, + double finalDurationSeconds, + int stretchIterationCount) + { + ArgumentNullException.ThrowIfNull(samples); + ArgumentNullException.ThrowIfNull(triggerBindings); + ArgumentNullException.ThrowIfNull(timingRows); + ArgumentNullException.ThrowIfNull(jerkRows); + + if (requestSpeedRatio <= 0.0 || double.IsNaN(requestSpeedRatio) || double.IsInfinity(requestSpeedRatio)) + { + throw new ArgumentOutOfRangeException(nameof(requestSpeedRatio), "请求速度倍率必须是有限正数。"); + } + + if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio)) + { + throw new ArgumentOutOfRangeException(nameof(finalSpeedRatio), "最终速度倍率必须是有限正数。"); + } + + if (finalDurationSeconds < 0.0 || double.IsNaN(finalDurationSeconds) || double.IsInfinity(finalDurationSeconds)) + { + throw new ArgumentOutOfRangeException(nameof(finalDurationSeconds), "最终发送总时长必须是有限非负数。"); + } + + if (stretchIterationCount < 0) + { + throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "拉长迭代次数必须是非负整数。"); + } + + Samples = samples.Select(static sample => sample).ToArray(); + TriggerBindings = triggerBindings.Select(static binding => binding).ToArray(); + TimingRows = timingRows.Select(static row => row.ToArray()).ToArray(); + JerkRows = jerkRows.Select(static row => row.ToArray()).ToArray(); + RequestSpeedRatio = requestSpeedRatio; + FinalSpeedRatio = finalSpeedRatio; + FinalDurationSeconds = finalDurationSeconds; + StretchIterationCount = stretchIterationCount; + } + + /// + /// 获取最终 8ms 发送点列。 + /// + public IReadOnlyList Samples { get; } + + /// + /// 获取与最终发送点列对齐的触发绑定结果。 + /// + public IReadOnlyList TriggerBindings { get; } + + /// + /// 获取与最终发送点列一致的时间映射诊断行。 + /// + public IReadOnlyList> TimingRows { get; } + + /// + /// 获取与最终发送点列一致的跃度诊断行。 + /// + public IReadOnlyList> JerkRows { get; } + + /// + /// 获取请求的执行倍率。 + /// + public double RequestSpeedRatio { get; } + + /// + /// 获取通过离散校验后实际采用的保守倍率。 + /// + public double FinalSpeedRatio { get; } + + /// + /// 获取最终发送总时长，单位为秒。 + /// + public double FinalDurationSeconds { get; } + + /// + /// 获取自动拉长执行时长的迭代次数。 + /// + public int StretchIterationCount { get; } +} + +/// +/// 表示飞拍最终发送队列中的一个 8ms 发送点。 +/// +public sealed class FlyshotPreparedSample +{ + /// + /// 初始化一条最终发送点。 + /// + /// 发送周期序号。 + /// 物理发送时间，单位为秒。 + /// 回映射到规划轨迹的采样时间，单位为秒。 + /// 生成该发送点时采用的执行倍率。 + /// J519 下发使用的角度制关节目标。 + public FlyshotPreparedSample( + long sampleIndex, + double sendTime, + double trajectoryTime, + double speedRatio, + IReadOnlyList jointsDegrees) + { + ArgumentNullException.ThrowIfNull(jointsDegrees); + SampleIndex = sampleIndex; + SendTime = sendTime; + TrajectoryTime = trajectoryTime; + SpeedRatio = speedRatio; + JointsDegrees = jointsDegrees.ToArray(); + } + + /// + /// 获取发送周期序号。 + /// + public long SampleIndex { get; } + + /// + /// 获取物理发送时间，单位为秒。 + /// + public double SendTime { get; } + + /// + /// 获取回映射到规划轨迹的采样时间，单位为秒。 + /// + public double TrajectoryTime { get; } + + /// + /// 获取生成该发送点时采用的执行倍率。 + /// + public double SpeedRatio { get; } + + /// + /// 获取 J519 下发使用的角度制关节目标。 + /// + public IReadOnlyList JointsDegrees { get; } +} + +/// +/// 表示理论触发事件最终绑定到哪个发送点的结果。 +/// +public sealed class FlyshotPreparedTriggerBinding +{ + /// + /// 初始化一条最终触发绑定结果。 + /// + /// 理论触发事件。 + /// 最终绑定的发送点。 + /// 最终绑定到的发送点索引。 + /// 是否在理论搜索窗口内找到该发送点。 + public FlyshotPreparedTriggerBinding( + TrajectoryDoEvent trigger, + FlyshotPreparedSample sample, + int sampleIndex, + bool foundInWindow) + { + Trigger = trigger ?? throw new ArgumentNullException(nameof(trigger)); + Sample = sample ?? throw new ArgumentNullException(nameof(sample)); + SampleIndex = sampleIndex; + FoundInWindow = foundInWindow; + } + + /// + /// 获取理论触发事件。 + /// + public TrajectoryDoEvent Trigger { get; } + + /// + /// 获取最终绑定的发送点。 + /// + public FlyshotPreparedSample Sample { get; } + + /// + /// 获取最终绑定到的发送点索引。 + /// + public int SampleIndex { get; } + + /// + /// 获取是否在理论搜索窗口内完成绑定。 + /// + public bool FoundInWindow { get; } +} diff --git a/src/Flyshot.Core.Domain/TrajectoryResult.cs b/src/Flyshot.Core.Domain/TrajectoryResult.cs index 3523f3d..e80f373 100644 --- a/src/Flyshot.Core.Domain/TrajectoryResult.cs +++ b/src/Flyshot.Core.Domain/TrajectoryResult.cs @@ -23,7 +23,8 @@ public sealed class TrajectoryResult int originalWaypointCount, int plannedWaypointCount, int triggerSampleIndexOffsetCycles = 0, - IEnumerable>? denseJointTrajectory = null) + IEnumerable>? denseJointTrajectory = null, + FlyshotPreparedExecution? preparedFlyshotExecution = null) { if (string.IsNullOrWhiteSpace(programName)) { @@ -73,6 +74,7 @@ public sealed class TrajectoryResult PlannedWaypointCount = plannedWaypointCount; TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles; DenseJointTrajectory = copiedDenseJointTrajectory; + PreparedFlyshotExecution = preparedFlyshotExecution; } /// @@ -153,6 +155,12 @@ public sealed class TrajectoryResult /// [JsonPropertyName("denseJointTrajectory")] public IReadOnlyList>? DenseJointTrajectory { get; } + + /// + /// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence. + /// + [JsonIgnore] + public FlyshotPreparedExecution? PreparedFlyshotExecution { get; } } /// diff --git a/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs b/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs new file mode 100644 index 0000000..15deb6d --- /dev/null +++ b/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs @@ -0,0 +1,161 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Planning.Sampling; + +/// +/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并在必要时自动拉长执行时长直到通过离散限幅校验。 +/// +public static class FlyshotExecutionSendSequenceBuilder +{ + /// + /// 自动拉长时每轮采用的保守倍率缩减系数。 + /// + private const double StretchFactor = 0.95; + + /// + /// 自动拉长尝试的最大迭代次数。 + /// + private const int MaxStretchIterations = 16; + + /// + /// 根据规划层稠密轨迹和执行层 speedRatio 构建最终发送队列。 + /// + /// 机器人关节限值配置。 + /// 规划结果。 + /// J519 物理发送周期，单位为秒。 + /// 请求的执行倍率。 + /// 通过离散校验后的飞拍最终发送结果。 + public static FlyshotPreparedExecution Build( + RobotProfile robot, + TrajectoryResult result, + double servoPeriodSeconds, + double requestedSpeedRatio) + { + ArgumentNullException.ThrowIfNull(robot); + ArgumentNullException.ThrowIfNull(result); + + if (result.DenseJointTrajectory is null) + { + throw new InvalidOperationException("飞拍执行准备前必须先生成规划层稠密关节轨迹。"); + } + + if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio)) + { + throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。"); + } + + if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds)) + { + throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。"); + } + + var effectiveSpeedRatio = requestedSpeedRatio; + InvalidOperationException? firstFailure = null; + + for (var iteration = 0; iteration <= MaxStretchIterations; iteration++) + { + var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory( + result.DenseJointTrajectory, + result.Duration.TotalSeconds, + servoPeriodSeconds, + effectiveSpeedRatio); + + try + { + TrajectoryLimitValidator.ValidateJ519SendSamples( + robot, + samples, + trajectoryName: result.ProgramName); + + return BuildPreparedExecution( + result, + samples, + requestedSpeedRatio, + effectiveSpeedRatio, + iteration); + } + catch (InvalidOperationException exception) + { + firstFailure ??= exception; + if (iteration >= MaxStretchIterations) + { + throw new InvalidOperationException( + $"飞拍最终发送队列离散限幅校验失败，已达到最大自动拉长次数 {MaxStretchIterations}。", + firstFailure); + } + + // 只在执行侧进一步保守化 trajectoryTime(sendTime) 映射，不回写规划层轨迹。 + effectiveSpeedRatio *= StretchFactor; + } + } + + throw new InvalidOperationException("飞拍最终发送队列构建失败。", firstFailure); + } + + /// + /// 将通过校验的 J519 采样点和触发绑定结果封装为稳定的飞拍执行结果。 + /// + private static FlyshotPreparedExecution BuildPreparedExecution( + TrajectoryResult result, + IReadOnlyList samples, + double requestedSpeedRatio, + double finalSpeedRatio, + int stretchIterationCount) + { + var preparedSamples = samples.Select(static sample => + new FlyshotPreparedSample( + sample.SampleIndex, + sample.SendTime, + sample.TrajectoryTime, + sample.SpeedRatio, + sample.JointsDegrees)).ToArray(); + var sampleLookup = preparedSamples.ToDictionary(static sample => sample.SampleIndex); + var triggerBindings = TriggerSampleBinder.Bind( + result.TriggerTimeline, + samples, + result.TriggerSampleIndexOffsetCycles) + .Select(binding => + new FlyshotPreparedTriggerBinding( + binding.Trigger, + sampleLookup[binding.Sample.SampleIndex], + binding.SampleIndex, + binding.FoundInWindow)) + .ToArray(); + var timingRows = samples.Select(J519SendTrajectorySampler.BuildTimingRow).ToArray(); + var jerkRows = BuildJerkRows(samples); + var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime; + + return new FlyshotPreparedExecution( + preparedSamples, + triggerBindings, + timingRows, + jerkRows, + requestedSpeedRatio, + finalSpeedRatio, + finalDurationSeconds, + stretchIterationCount); + } + + /// + /// 为最终发送点列构建跃度诊断行，保证导出工件和运行时发送复用同一份数据。 + /// + private static IReadOnlyList> BuildJerkRows(IReadOnlyList samples) + { + var jerkRows = new List>(Math.Max(0, samples.Count - 1)); + double[]? previousVelocity = null; + double[]? previousAcceleration = null; + + for (var index = 1; index < samples.Count; index++) + { + jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow( + samples[index - 1].SendTime, + samples[index].SendTime, + samples[index - 1].JointsDegrees, + samples[index].JointsDegrees, + ref previousVelocity, + ref previousAcceleration)); + } + + return jerkRows; + } +} diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index 5377286..54995ff 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -569,13 +569,20 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable var triggers = result.TriggerTimeline; var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds; var speedRatio = _speedRatio; - var trajectoryStepSeconds = servoPeriodSeconds * speedRatio; - var durationSeconds = result.Duration.TotalSeconds; - var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory( - result.DenseJointTrajectory!, - durationSeconds, - servoPeriodSeconds, - speedRatio); + var preparedExecution = result.PreparedFlyshotExecution; + var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds; + var samples = preparedExecution is null + ? J519SendTrajectorySampler.SampleDenseJointTrajectory( + result.DenseJointTrajectory!, + durationSeconds, + servoPeriodSeconds, + speedRatio) + : preparedExecution.Samples.Select(static sample => new J519SendSample( + sample.SampleIndex, + sample.SendTime, + sample.TrajectoryTime, + sample.SpeedRatio, + sample.JointsDegrees)).ToArray(); TrajectoryLimitValidator.ValidateJ519SendSamples( _robot!, samples, @@ -586,10 +593,22 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable "SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}", result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count); - var triggerBindings = TriggerSampleBinder.Bind( - triggers, - samples, - result.TriggerSampleIndexOffsetCycles); + var triggerBindings = preparedExecution is null + ? TriggerSampleBinder.Bind( + triggers, + samples, + result.TriggerSampleIndexOffsetCycles) + : preparedExecution.TriggerBindings.Select(static binding => + new TriggerSampleBinding( + binding.Trigger, + new J519SendSample( + binding.Sample.SampleIndex, + binding.Sample.SendTime, + binding.Sample.TrajectoryTime, + binding.Sample.SpeedRatio, + binding.Sample.JointsDegrees), + binding.SampleIndex, + binding.FoundInWindow)).ToArray(); var stopwatch = Stopwatch.StartNew(); ushort ioValue = 0; @@ -607,6 +626,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable double[]? previousJoints = null; double[]? previousVelocity = null; double[]? previousAcceleration = null; + var preparedJerkRows = preparedExecution?.JerkRows; try { @@ -669,7 +689,14 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue)); sentTimingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample)); - if (previousSendTime is not null && previousJoints is not null) + if (preparedJerkRows is not null) + { + if (sample.SampleIndex > 0 && sample.SampleIndex - 1 < preparedJerkRows.Count) + { + sentJerkRows.Add(preparedJerkRows[(int)sample.SampleIndex - 1]); + } + } + else if (previousSendTime is not null && previousJoints is not null) { var jerkRow = J519SendTrajectorySampler.BuildJerkRow( previousSendTime.Value, @@ -766,8 +793,16 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable return; } - retryEnableRobot(); - waitAfterRetry(); + try + { + retryEnableRobot(); + waitAfterRetry(); + } + catch (InvalidOperationException exception) + { + // 重试链路自身失败时，优先保留 J519 状态位诊断，底层连接异常作为 inner exception 继续上抛。 + throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response), exception); + } response = getLatestResponse(); if (response is null || IsJ519ReadyForDenseExecution(response)) diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs index 53485b7..e2031e0 100644 --- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -1,6 +1,7 @@ using Flyshot.Core.Domain; using Flyshot.ControllerClientCompat; using Flyshot.Core.Config; +using Flyshot.Core.Planning.Sampling; using Flyshot.Runtime.Fanuc; using Flyshot.Runtime.Fanuc.Protocol; using System.Buffers.Binary; @@ -274,8 +275,8 @@ public sealed class FanucControllerRuntimeDenseTests Assert.NotEmpty(pointsLines); Assert.NotEmpty(timingLines); Assert.NotEmpty(jerkLines); - Assert.Equal(927, pointsLines.Length); - Assert.Equal(927, timingLines.Length); + Assert.Equal(result.DenseJointTrajectory!.Count, pointsLines.Length); + Assert.Equal(result.DenseJointTrajectory.Count, timingLines.Length); var firstPoint = ParseColumns(pointsLines[0]); var secondPoint = ParseColumns(pointsLines[1]); @@ -323,6 +324,144 @@ public sealed class FanucControllerRuntimeDenseTests Assert.True(File.Exists(summaryPath)); } + /// + /// 验证真实 UTTC_MS11 轨迹在非 1 倍 speedRatio 下仍能生成并装载 J519 实发队列。 + /// + [Theory] + [InlineData(0.7)] + [InlineData(0.5)] + public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_AllowsNonOneSpeedRatio(double speedRatio) + { + using var commandClient = new FanucCommandClient(); + using var stateClient = new FanucStateClient(); + using var j519Client = new FanucJ519Client(); + using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client); + var fixture = LoadUttcMs11RuntimeFixture(); + var fullSpeedSettings = new CompatibilityRobotSettings( + useDo: fixture.Settings.UseDo, + ioAddresses: fixture.Settings.IoAddresses, + ioKeepCycles: fixture.Settings.IoKeepCycles, + triggerSampleIndexOffsetCycles: fixture.Settings.TriggerSampleIndexOffsetCycles, + accLimitScale: fixture.Settings.AccLimitScale, + jerkLimitScale: fixture.Settings.JerkLimitScale, + adaptIcspTryNum: fixture.Settings.AdaptIcspTryNum, + planningSpeedScale: 1.0, + smoothStartStopTiming: true); + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var bundle = orchestrator.PlanUploadedFlyshot( + fixture.Robot, + fixture.Uploaded, + settings: fullSpeedSettings, + planningSpeedScale: fullSpeedSettings.PlanningSpeedScale); + var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build( + fixture.Robot, + bundle.Result, + fixture.Robot.ServoPeriod.TotalSeconds, + speedRatio); + var result = WithUniqueProgramName(new TrajectoryResult( + programName: bundle.Result.ProgramName, + method: bundle.Result.Method, + isValid: bundle.Result.IsValid, + duration: bundle.Result.Duration, + shotEvents: bundle.Result.ShotEvents, + triggerTimeline: bundle.Result.TriggerTimeline, + artifacts: bundle.Result.Artifacts, + failureReason: bundle.Result.FailureReason, + usedCache: bundle.Result.UsedCache, + originalWaypointCount: bundle.Result.OriginalWaypointCount, + plannedWaypointCount: bundle.Result.PlannedWaypointCount, + triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles, + denseJointTrajectory: bundle.Result.DenseJointTrajectory, + preparedFlyshotExecution: preparedExecution), $"UTTC_MS11_speedratio_{speedRatio:F1}_{Guid.NewGuid():N}"); + var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", result.ProgramName); + + try + { + runtime.ResetRobot(fixture.Robot, fixture.Robot.Name); + j519Client.EnableCommandHistoryForTests(); + ForceRealModeEnabled(runtime, speedRatio); + + runtime.ExecuteTrajectory(result, result.DenseJointTrajectory![^1].Skip(1).ToArray()); + WaitUntilIdle(runtime); + + var commands = j519Client.GetCommandHistoryForTests(); + Assert.NotEmpty(commands); + Assert.Equal(preparedExecution.Samples.Count, commands.Count); + AssertJointDegreesEqual(result.DenseJointTrajectory[0].Skip(1).ToArray(), commands[0].TargetJoints); + } + finally + { + if (Directory.Exists(outputRoot)) + { + Directory.Delete(outputRoot, recursive: true); + } + } + } + + /// + /// 验证运行时拿到飞拍预生成发送队列后，会直接消费该队列，而不是再按当前 speedRatio 临场重采样。 + /// + [Fact] + public void ExecuteTrajectory_WithPreparedFlyshotExecution_RealMode_UsesPreparedSamplesDirectly() + { + using var commandClient = new FanucCommandClient(); + using var stateClient = new FanucStateClient(); + using var j519Client = new FanucJ519Client(); + using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client); + var robot = TestRobotFactory.CreateRobotProfile(); + runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD"); + j519Client.EnableCommandHistoryForTests(); + ForceRealModeEnabled(runtime, speedRatio: 0.5); + + var preparedSamples = new[] + { + new FlyshotPreparedSample(0, 0.0, 0.0, 1.0, [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + new FlyshotPreparedSample(1, 0.008, 0.008, 1.0, [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]), + new FlyshotPreparedSample(2, 0.016, 0.016, 1.0, [3.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + }; + var preparedExecution = new FlyshotPreparedExecution( + samples: preparedSamples, + triggerBindings: Array.Empty(), + timingRows: preparedSamples.Select(static sample => (IReadOnlyList) + [ + sample.SampleIndex, + Math.Round(sample.SendTime, 6), + Math.Round(sample.TrajectoryTime, 6), + Math.Round(sample.SpeedRatio, 6) + ]).ToArray(), + jerkRows: Array.Empty>(), + requestSpeedRatio: 1.0, + finalSpeedRatio: 1.0, + finalDurationSeconds: 0.016, + stretchIterationCount: 0); + var result = new TrajectoryResult( + programName: "prepared-demo", + method: PlanningMethod.Icsp, + isValid: true, + duration: TimeSpan.FromSeconds(0.016), + shotEvents: Array.Empty(), + triggerTimeline: Array.Empty(), + artifacts: Array.Empty(), + failureReason: null, + usedCache: false, + originalWaypointCount: 4, + plannedWaypointCount: 4, + denseJointTrajectory: + [ + 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 } + ], + preparedFlyshotExecution: preparedExecution); + + runtime.ExecuteTrajectory(result, [DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0]); + WaitUntilIdle(runtime); + + var commands = j519Client.GetCommandHistoryForTests(); + Assert.Equal(preparedSamples.Length, commands.Count); + Assert.Equal([1.0, 2.0, 3.0], commands.Select(static command => command.TargetJoints[0])); + } + /// /// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。 /// @@ -1223,7 +1362,8 @@ public sealed class FanucControllerRuntimeDenseTests usedCache: result.UsedCache, originalWaypointCount: result.OriginalWaypointCount, plannedWaypointCount: result.PlannedWaypointCount, - denseJointTrajectory: result.DenseJointTrajectory); + denseJointTrajectory: result.DenseJointTrajectory, + preparedFlyshotExecution: result.PreparedFlyshotExecution); } /// diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index 50a5efa..c08952d 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -826,6 +826,48 @@ public sealed class RuntimeOrchestrationTests } } + /// + /// 验证飞拍链路在进入运行时前就会准备最终发送队列，而不是把 speedRatio 重采样留给运行时临场处理。 + /// + [Fact] + public void ControllerClientCompatService_ExecuteTrajectoryByName_PreparesFinalSendQueueBeforeRuntime() + { + var configRoot = CreateTempConfigRoot(); + try + { + WriteRobotConfigWithDemoTrajectory(configRoot); + 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()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + service.SetActiveController(sim: false); + service.Connect("192.168.10.101"); + service.EnableRobot(2); + service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot()); + runtime.SetSpeedRatio(0.5); + + service.ExecuteTrajectoryByName( + "demo-flyshot", + new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false)); + + var result = Assert.IsType(runtime.LastExecutedResult); + var preparedExecution = Assert.IsType(result.PreparedFlyshotExecution); + Assert.NotEmpty(preparedExecution.Samples); + Assert.Equal(preparedExecution.Samples.Count, preparedExecution.TimingRows.Count); + Assert.Equal(result.TriggerTimeline.Count, preparedExecution.TriggerBindings.Count); + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + /// /// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。 /// @@ -953,14 +995,14 @@ public sealed class RuntimeOrchestrationTests var executionDuration = double.Parse( File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture); - var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1; + var minimumExpectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1; - Assert.Equal(expectedRows, pointRows.Length); - Assert.Equal(expectedRows, timingRows.Length); + Assert.Equal(pointRows.Length, timingRows.Length); + Assert.True(pointRows.Length >= minimumExpectedRows, $"最终发送点数应不少于请求倍率的首轮候选值，actual={pointRows.Length}, min={minimumExpectedRows}"); Assert.Equal(0.0, pointRows[0][0], precision: 6); Assert.Equal(0.008, pointRows[1][0], precision: 6); - Assert.Equal(0.004, timingRows[1][2], precision: 6); - Assert.Equal(0.5, timingRows[1][3], 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.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson); Assert.Contains("\"selected_sample_index\"", shotEventsJson); } @@ -1607,6 +1649,11 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime /// public RobotProfile? LastRobotProfile { get; private set; } + /// + /// 获取最近一次 ExecuteTrajectory 收到的结果对象。 + /// + public TrajectoryResult? LastExecutedResult { get; private set; } + /// public void ResetRobot(RobotProfile robot, string robotName) { @@ -1691,6 +1738,7 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime /// public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions) { + LastExecutedResult = result; } }