✨ feat(*): 添加 J519 实发重采样与 JSON 机型模型

* 新增 J519 实发采样器,按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换
* 兼容层产物导出补充 speedRatio,规划编排补齐 smoothStartStopTiming 与日志透传
* 配置与机型加载切换到运行目录 JSON 模型,并补齐 7L 展开模型与相关单元测试
This commit is contained in:
2026-05-07 17:08:32 +08:00
parent 70b0ccd414
commit c6829d214a
26 changed files with 1417 additions and 409 deletions

View File

@@ -5,6 +5,7 @@ using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Kinematics;
using Flyshot.Core.Planning.Sampling;
using Microsoft.Extensions.Logging;
using System.Text;
namespace Flyshot.ControllerClientCompat;
@@ -18,6 +19,11 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// </summary>
private const double LegacyDetailSamplePeriodSeconds = 0.016;
/// <summary>
/// FANUC J519 实际下发的固定伺服周期,单位为秒。
/// </summary>
private const double ActualSendServoPeriodSeconds = 0.008;
private readonly ControllerClientCompatOptions _options;
private readonly RobotModelLoader _robotModelLoader;
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
@@ -44,7 +50,8 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="bundle">规划结果包。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle)
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
@@ -74,13 +81,99 @@ public sealed class FlyshotTrajectoryArtifactWriter
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);
_logger?.LogInformation(
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}",
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
trajectoryName,
outputDir,
jointTrajectory.Count,
jointDetailTrajectory.Count);
jointDetailTrajectory.Count,
speedRatio);
}
/// <summary>
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点,供 saveTrajectory 离线对比真实下发序列。
/// </summary>
private static void WriteActualSendArtifacts(string outputDir, TrajectoryResult result, double speedRatio)
{
if (result.DenseJointTrajectory is null)
{
return;
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio);
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
var timingRows = new List<IReadOnlyList<double>>(samples.Count);
var jerkRows = new List<IReadOnlyList<double>>();
double? previousSendTime = null;
double[]? previousJoints = null;
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
foreach (var sample in samples)
{
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
if (previousSendTime is not null && previousJoints is not null)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sample.SendTime,
previousJoints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
}
previousSendTime = sample.SendTime;
previousJoints = sample.JointsDegrees.ToArray();
}
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), jointRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), timingRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), jerkRows);
}
/// <summary>
/// 构造实际发送点位文本行,格式为 send_time + 关节角度 + io_mask + io_value。
/// </summary>
private static IReadOnlyList<double> BuildActualSendJointRow(double sendTime, IReadOnlyList<double> joints)
{
var row = new double[joints.Count + 3];
row[0] = Math.Round(sendTime, 6);
for (var index = 0; index < joints.Count; index++)
{
row[index + 1] = Math.Round(joints[index], 6);
}
row[^2] = 0.0;
row[^1] = 0.0;
return row;
}
/// <summary>
/// 以空格分隔的旧轨迹文本格式写出数值行。
/// </summary>
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
var sb = new StringBuilder();
foreach (var row in rows)
{
sb.AppendLine(string.Join(" ", row.Select(static value => $"{value:F6}")));
}
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
}
/// <summary>