✨ feat(fanuc): 统一 speedRatio 执行倍率语义
* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user