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