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

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

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;