✨ feat(*): 添加轨迹产物导出与规划速度倍率隔离
* 新增 FlyshotTrajectoryArtifactWriter,支持 saveTrajectory 将规划结果导出到 Config/Data/name(JointTraj、CartTraj、 ShotEvents 等) * RobotConfig 新增 PlanningSpeedScale,区分规划阶段限速倍率 与运行时 J519 下发倍率 * 轨迹缓存键纳入 planningSpeedScale,避免降速规划误用缓存 * 完善 FanucCommandClient 命令参数日志与状态通道重连 * 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离 * 更新 README 进度文档
This commit is contained in:
@@ -0,0 +1,136 @@
|
||||
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;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
|
||||
/// </summary>
|
||||
public sealed class FlyshotTrajectoryArtifactWriter
|
||||
{
|
||||
/// <summary>
|
||||
/// 旧 Data 明细点位文件使用的默认采样周期,单位为秒。
|
||||
/// </summary>
|
||||
private const double LegacyDetailSamplePeriodSeconds = 0.016;
|
||||
|
||||
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>
|
||||
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(bundle);
|
||||
|
||||
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
||||
Directory.CreateDirectory(outputDir);
|
||||
|
||||
// 明细文件对齐旧 Data 目录的 16ms 采样;运行时 J519 仍可使用自己的 8ms 伺服采样。
|
||||
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
||||
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
|
||||
var jointDetailTrajectory = TrajectorySampler.SampleJointTrajectory(
|
||||
bundle.PlannedTrajectory,
|
||||
samplePeriod: LegacyDetailSamplePeriodSeconds);
|
||||
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
|
||||
var cartDetailTrajectory = TrajectorySampler.SampleCartesianTrajectory(
|
||||
bundle.PlannedTrajectory,
|
||||
kinematicsModel,
|
||||
samplePeriod: LegacyDetailSamplePeriodSeconds);
|
||||
|
||||
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);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}",
|
||||
trajectoryName,
|
||||
outputDir,
|
||||
jointTrajectory.Count,
|
||||
jointDetailTrajectory.Count);
|
||||
}
|
||||
|
||||
/// <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>
|
||||
/// 将轨迹名转换为可用目录名,避免 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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user