✨ feat(*): 添加 J519 实发重采样与 JSON 机型模型
* 新增 J519 实发采样器,按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换 * 兼容层产物导出补充 speedRatio,规划编排补齐 smoothStartStopTiming 与日志透传 * 配置与机型加载切换到运行目录 JSON 模型,并补齐 7L 展开模型与相关单元测试
This commit is contained in:
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user