✨ 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

@@ -63,25 +63,28 @@ public sealed class FlyshotTrajectoryArtifactWriter
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
Directory.CreateDirectory(outputDir);
if (bundle.Result.DenseJointTrajectory is null)
{
throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。");
}
// 明细文件对齐旧 Data 目录的 16ms 采样;运行时 J519 仍可使用自己的 8ms 伺服采样。
// 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”,避免再次从 PlannedTrajectory 生成另一条轨迹。
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
var jointDetailTrajectory = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: LegacyDetailSamplePeriodSeconds);
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
var executionDenseTrajectory = bundle.Result.DenseJointTrajectory;
var jointDetailTrajectory = DownsampleDenseRows(
executionDenseTrajectory,
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
var cartDetailTrajectory = TrajectorySampler.SampleCartesianTrajectory(
bundle.PlannedTrajectory,
kinematicsModel,
samplePeriod: LegacyDetailSamplePeriodSeconds);
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);
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents);
WriteActualSendArtifacts(outputDir, bundle.Result, speedRatio);
WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio);
_logger?.LogInformation(
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
@@ -95,8 +98,10 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// <summary>
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点,供 saveTrajectory 离线对比真实下发序列。
/// </summary>
private static void WriteActualSendArtifacts(string outputDir, TrajectoryResult result, double speedRatio)
private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio)
{
ArgumentNullException.ThrowIfNull(robot);
if (result.DenseJointTrajectory is null)
{
return;
@@ -112,6 +117,19 @@ public sealed class FlyshotTrajectoryArtifactWriter
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName);
}
catch (Exception e)
{
_logger?.LogError(e, "ValidateJ519SendSamples 失败,program={ProgramName}", result.ProgramName);
}
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
var timingRows = new List<IReadOnlyList<double>>(samples.Count);
var jerkRows = new List<IReadOnlyList<double>>();
@@ -217,6 +235,63 @@ public sealed class FlyshotTrajectoryArtifactWriter
return rows;
}
/// <summary>
/// 基于执行侧稠密关节轨迹生成笛卡尔导出行,保持与 JointDetialTraj.txt 同一来源。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRowsFromJointDense(
IReadOnlyList<IReadOnlyList<double>> jointDenseRows,
RobotKinematicsModel kinematicsModel)
{
var rows = new List<IReadOnlyList<double>>(jointDenseRows.Count);
foreach (var jointRow in jointDenseRows)
{
var jointPositions = jointRow.Skip(1).ToArray();
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPositions);
var row = new List<double>(pose.Length + 1)
{
Math.Round(jointRow[0], 6)
};
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
rows.Add(row);
}
return rows;
}
/// <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>
/// 将轨迹名转换为可用目录名,避免 HTTP 输入中的路径字符污染输出目录。
/// </summary>