✨ feat(fanuc): 打通飞拍轨迹完整执行链路
* 增加 J519 稠密发送采样校验与保姿回发逻辑 * 调整 saveTrajectory 导出与 sequence buffer 行为 * 补充 10010 解析脚本、ICSP 说明和回归测试
This commit is contained in:
@@ -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,单位保持输入文件的角度制。
|
||||
|
||||
Reference in New Issue
Block a user