yunxiao.zhu
d120ef2a39
* 将 speedRatio 前移到规划/准备阶段，运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
342 lines
15 KiB
C#
342 lines
15 KiB
C#
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Core.Planning;
|
||
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;
|
||
|
||
/// <summary>
|
||
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
|
||
/// </summary>
|
||
public sealed class FlyshotTrajectoryArtifactWriter
|
||
{
|
||
/// <summary>
|
||
/// 旧 Data 明细点位文件使用的默认采样周期，单位为秒。
|
||
/// </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;
|
||
|
||
/// <summary>
|
||
/// 初始化规划结果点位导出器。
|
||
/// </summary>
|
||
/// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
|
||
/// <param name="robotModelLoader">机器人模型加载器，用于生成笛卡尔点位。</param>
|
||
/// <param name="logger">日志记录器；允许 null。</param>
|
||
public FlyshotTrajectoryArtifactWriter(
|
||
ControllerClientCompatOptions options,
|
||
RobotModelLoader robotModelLoader,
|
||
ILogger<FlyshotTrajectoryArtifactWriter>? logger = null)
|
||
{
|
||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||
_logger = logger;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将飞拍规划结果导出到 Config/Data/name。
|
||
/// </summary>
|
||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||
/// <param name="robot">当前机器人配置。</param>
|
||
/// <param name="bundle">规划结果包；旧规划导出和实发诊断使用同一份视图。</param>
|
||
/// <param name="speedRatio">请求速度倍率，仅作为诊断列保留；最终采样倍率已在规划阶段处理。</param>
|
||
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
|
||
{
|
||
WriteUploadedFlyshot(trajectoryName, robot, bundle, bundle, speedRatio);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将飞拍规划结果导出到 Config/Data/name，并显式区分旧规划导出视图和实发诊断视图。
|
||
/// </summary>
|
||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||
/// <param name="robot">当前机器人配置。</param>
|
||
/// <param name="exportPlanningBundle">旧格式规划导出使用的结果包，不应包含运行时 speedRatio 折算。</param>
|
||
/// <param name="executionBundle">实发诊断使用的结果包，应与运行时最终发送队列保持一致。</param>
|
||
/// <param name="speedRatio">请求速度倍率，仅作为 ActualSend 诊断列保留。</param>
|
||
public void WriteUploadedFlyshot(
|
||
string trajectoryName,
|
||
RobotProfile robot,
|
||
PlannedExecutionBundle exportPlanningBundle,
|
||
PlannedExecutionBundle executionBundle,
|
||
double speedRatio = 1.0)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||
{
|
||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||
}
|
||
|
||
ArgumentNullException.ThrowIfNull(robot);
|
||
ArgumentNullException.ThrowIfNull(exportPlanningBundle);
|
||
ArgumentNullException.ThrowIfNull(executionBundle);
|
||
|
||
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
||
Directory.CreateDirectory(outputDir);
|
||
if (exportPlanningBundle.Result.DenseJointTrajectory is null)
|
||
{
|
||
throw new InvalidOperationException("导出飞拍旧格式规划工件前必须先生成规划明细轨迹。");
|
||
}
|
||
|
||
// 旧格式规划文件固定使用不含运行时 speedRatio 的规划视图，避免和 ActualSend 实发诊断混在一起。
|
||
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
||
var jointTrajectory = BuildJointRows(exportPlanningBundle.PlannedTrajectory);
|
||
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
|
||
var executionDenseTrajectory = exportPlanningBundle.Result.DenseJointTrajectory;
|
||
var jointDetailTrajectory = DownsampleDenseRows(
|
||
executionDenseTrajectory,
|
||
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
|
||
var cartTrajectory = BuildCartesianRows(exportPlanningBundle.PlannedTrajectory, kinematicsModel);
|
||
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);
|
||
WriteActualSendArtifacts(outputDir, robot, executionBundle.Result, speedRatio);
|
||
|
||
_logger?.LogInformation(
|
||
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
|
||
trajectoryName,
|
||
outputDir,
|
||
jointTrajectory.Count,
|
||
jointDetailTrajectory.Count,
|
||
speedRatio);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。
|
||
/// </summary>
|
||
private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(robot);
|
||
|
||
var preparedExecution = result.PreparedFlyshotExecution;
|
||
if (result.DenseJointTrajectory is null && preparedExecution is null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var samples = preparedExecution is null
|
||
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||
result.DenseJointTrajectory!,
|
||
result.Duration.TotalSeconds,
|
||
ActualSendServoPeriodSeconds,
|
||
speedRatio: 1.0)
|
||
: preparedExecution.Samples.Select(static sample => new J519SendSample(
|
||
sample.SampleIndex,
|
||
sample.SendTime,
|
||
sample.TrajectoryTime,
|
||
sample.SpeedRatio,
|
||
sample.JointsDegrees)).ToArray();
|
||
var triggerBindings = preparedExecution is null
|
||
? TriggerSampleBinder.Bind(
|
||
result.TriggerTimeline,
|
||
samples,
|
||
result.TriggerSampleIndexOffsetCycles)
|
||
: preparedExecution.TriggerBindings.Select(static binding =>
|
||
new TriggerSampleBinding(
|
||
binding.Trigger,
|
||
new J519SendSample(
|
||
binding.Sample.SampleIndex,
|
||
binding.Sample.SendTime,
|
||
binding.Sample.TrajectoryTime,
|
||
binding.Sample.SpeedRatio,
|
||
binding.Sample.JointsDegrees),
|
||
binding.SampleIndex,
|
||
binding.FoundInWindow)).ToArray();
|
||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||
robot,
|
||
samples,
|
||
trajectoryName: result.ProgramName,
|
||
validateJerk: false);
|
||
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
|
||
List<IReadOnlyList<double>> timingRows = preparedExecution is null
|
||
? new List<IReadOnlyList<double>>(samples.Count)
|
||
: preparedExecution.TimingRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
|
||
List<IReadOnlyList<double>> jerkRows = preparedExecution is null
|
||
? []
|
||
: preparedExecution.JerkRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
|
||
double? previousSendTime = null;
|
||
double[]? previousJoints = null;
|
||
double[]? previousVelocity = null;
|
||
double[]? previousAcceleration = null;
|
||
|
||
foreach (var sample in samples)
|
||
{
|
||
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
|
||
if (preparedExecution is null)
|
||
{
|
||
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample, speedRatio, stretchIterationCount: 0));
|
||
|
||
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);
|
||
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
|
||
}
|
||
|
||
/// <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>
|
||
/// 构造 JointTraj.txt 行数据，格式为 time + 关节弧度。
|
||
/// </summary>
|
||
private static IReadOnlyList<IReadOnlyList<double>> BuildJointRows(PlannedTrajectory trajectory)
|
||
{
|
||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||
{
|
||
var row = new List<double>(trajectory.PlannedWaypoints[index].Positions.Count + 1)
|
||
{
|
||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||
};
|
||
row.AddRange(trajectory.PlannedWaypoints[index].Positions.Select(static value => Math.Round(value, 6)));
|
||
rows.Add(row);
|
||
}
|
||
|
||
return rows;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造 CartTraj.txt 行数据，格式为 time + x/y/z/qx/qy/qz/qw。
|
||
/// </summary>
|
||
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRows(
|
||
PlannedTrajectory trajectory,
|
||
RobotKinematicsModel kinematicsModel)
|
||
{
|
||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||
{
|
||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, trajectory.PlannedWaypoints[index].Positions.ToArray());
|
||
var row = new List<double>(pose.Length + 1)
|
||
{
|
||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||
};
|
||
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
|
||
rows.Add(row);
|
||
}
|
||
|
||
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>
|
||
private static string SanitizeDirectoryName(string name)
|
||
{
|
||
var invalidChars = Path.GetInvalidFileNameChars();
|
||
var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
|
||
return new string(chars);
|
||
}
|
||
}
|