feat(fanuc): 打通飞拍轨迹完整执行链路

* 增加 J519 稠密发送采样校验与保姿回发逻辑
* 调整 saveTrajectory 导出与 sequence buffer 行为
* 补充 10010 解析脚本、ICSP 说明和回归测试
This commit is contained in:
2026-05-08 13:25:02 +08:00
parent c6829d214a
commit 1779067b5c
13 changed files with 1363 additions and 33 deletions

View File

@@ -914,7 +914,7 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位,并应用当前 speed_ratio。
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位,并按执行侧稠密轨迹时长应用当前 speed_ratio。
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
@@ -945,8 +945,10 @@ public sealed class RuntimeOrchestrationTests
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var duration = double.Parse(File.ReadLines(Path.Combine(outputDir, "JointTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture);
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (duration / (0.008 * 0.5)) - 1e-9)) + 1;
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;
Assert.Equal(expectedRows, pointRows.Length);
Assert.Equal(expectedRows, timingRows.Length);
@@ -961,6 +963,54 @@ public sealed class RuntimeOrchestrationTests
}
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图,
/// 而不是再次从 PlannedTrajectory 独立重采样。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample()
{
var fixture = LoadUttcMs11RuntimeFixture();
var configRoot = CreateTempConfigRoot();
try
{
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader());
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0);
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!,
samplePeriodSeconds: 0.016)
.Select(static row => row.ToArray())
.ToArray();
Assert.Equal(expectedRows.Length, exportedRows.Length);
for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++)
{
Assert.Equal(expectedRows[rowIndex].Length, exportedRows[rowIndex].Length);
for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++)
{
Assert.Equal(expectedRows[rowIndex][columnIndex], exportedRows[rowIndex][columnIndex], precision: 6);
}
}
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。
/// </summary>
@@ -1276,6 +1326,40 @@ public sealed class RuntimeOrchestrationTests
.ToArray();
}
/// <summary>
/// 将 8ms 执行稠密轨迹按目标周期抽稀为低频视图,并始终保留终点。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> DownsampleDenseRows(
IReadOnlyList<IReadOnlyList<double>> denseRows,
double samplePeriodSeconds)
{
var result = new List<IReadOnlyList<double>>();
var epsilon = 1e-6;
var nextSampleTime = 0.0;
foreach (var row in denseRows)
{
var sampleTime = row[0];
if (sampleTime + epsilon < nextSampleTime)
{
continue;
}
if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0))
{
result.Add(row);
nextSampleTime += samplePeriodSeconds;
}
}
if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1]))
{
result.Add(denseRows[^1]);
}
return result;
}
/// <summary>
/// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线,返回相对首点时间的系数。
/// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0单位保持输入文件的角度制。