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

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)
{