xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(fanuc): 优化 J519 实时下发与飞拍起停整形

Browse Source 
- 改为高优先级 J519 接收线程与复用缓冲区发送链路
- 增加稠密执行前的 J519 就绪重试与状态诊断
- 修正程序状态响应字段顺序与 EnableRobot 默认参数
- 为飞拍轨迹补充平滑起停时间轴与首尾整形验证
- 补充真实运行配置、报警窗口与边界对比测试
- 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
This commit is contained in:
yunxiao.zhu
2026-05-06 22:37:31 +08:00
parent 783716ff44
commit 70b0ccd414
17 changed files with 1629 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
tests/Flyshot.Core.Tests/FanucCommandClientTests.cs
View File

@@ -99,7 +99,7 @@ public sealed class FanucCommandClientTests : IDisposable
{
using var client = new FanucCommandClient();
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
var responseFrame = Convert.FromHexString("646f7a000000160000200300000000000000017a6f64");
var responseFrame = Convert.FromHexString("646f7a000000160000200300000001000000007a6f64");
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);

233
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -221,6 +221,101 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
/// <summary>
/// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 轨迹执行一次完整 1x 稠密下发，
/// 并把 0.088s 报警窗口附近的实发时间、关节与跃度摘要落盘，便于继续对照现场报警帧。
/// </summary>
[Fact]
public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_WritesDenseSendDebugWindowAtOneX()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", bundle.Result.ProgramName);
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
var beforeRunDirectories = Directory.Exists(denseSendRoot)
? Directory.GetDirectories(denseSendRoot).ToHashSet(StringComparer.OrdinalIgnoreCase)
: new HashSet<string>(StringComparer.OrdinalIgnoreCase);
runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
runtime.ExecuteTrajectory(bundle.Result, bundle.Result.DenseJointTrajectory![0].Skip(1).ToArray());
WaitUntilIdle(runtime);
var runDirectory = GetNewDenseSendRunDirectory(outputRoot, beforeRunDirectories);
var pointsPath = Path.Combine(runDirectory, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(runDirectory, "ActualSendTiming.txt");
var jerkPath = Path.Combine(runDirectory, "ActualSendJerkStats.txt");
var summaryPath = Path.Combine(runDirectory, "AlarmWindow_0p088s_Summary.txt");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
Assert.True(File.Exists(jerkPath));
var pointsLines = File.ReadAllLines(pointsPath);
var timingLines = File.ReadAllLines(timingPath);
var jerkLines = File.ReadAllLines(jerkPath);
Assert.NotEmpty(pointsLines);
Assert.NotEmpty(timingLines);
Assert.NotEmpty(jerkLines);
var firstPoint = ParseColumns(pointsLines[0]);
var secondPoint = ParseColumns(pointsLines[1]);
Assert.Equal(0.0, firstPoint[0], precision: 6);
Assert.Equal(0.008, secondPoint[0], precision: 6);
var firstStepJ1 = Math.Abs(secondPoint[1] - firstPoint[1]);
Assert.True(firstStepJ1 > 1e-6, $"UTTC_MS11 实发首步不应被压成 0，actual={firstStepJ1:F9}deg");
const double targetSendTime = 0.088;
const double windowHalfWidth = 0.024;
var summaryLines = new List<string>
{
$"program={bundle.Result.ProgramName}",
$"send_time_target_seconds={targetSendTime:F6}",
$"window_half_width_seconds={windowHalfWidth:F6}",
$"points_path={pointsPath}",
$"timing_path={timingPath}",
$"jerk_path={jerkPath}",
"timing_window:"
};
foreach (var line in timingLines.Select(ParseColumns))
{
var sendTime = line[1];
if (Math.Abs(sendTime - targetSendTime) <= windowHalfWidth + 1e-9)
{
summaryLines.Add(
$"timing send_index={line[0]:F0} send_time={line[1]:F6} trajectory_time={line[2]:F6} speed_ratio={line[3]:F6}");
}
}
summaryLines.Add("jerk_window:");
foreach (var line in jerkLines.Select(ParseColumns))
{
var endTime = line[1];
if (Math.Abs(endTime - targetSendTime) <= windowHalfWidth + 1e-9)
{
summaryLines.Add(
$"jerk end_time={line[1]:F6} dt={line[2]:F6} j1={line[3]:F6} j2={line[4]:F6} j3={line[5]:F6} j4={line[6]:F6} j5={line[7]:F6} j6={line[8]:F6} max_abs={line.Skip(3).Max(static value => Math.Abs(value)):F6}");
}
}
File.WriteAllLines(summaryPath, summaryLines);
Assert.True(File.Exists(summaryPath));
}
/// <summary>
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
/// </summary>
@@ -509,6 +604,58 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
}
/// <summary>
/// 验证稠密执行前若 J519 首次未就绪，会先尝试一次 EnableRobot，再在 500ms 后复查状态。
/// </summary>
[Fact]
public void EnsureJ519ReadyForDenseExecutionCore_RetriesEnableRobotOnceBeforePassing()
{
var responses = new Queue<FanucJ519Response?>(
[
CreateJ519Response(status: 0b0001, sequence: 11),
CreateJ519Response(status: 0b0101, sequence: 12)
]);
var enableRobotRetryCount = 0;
var waitCount = 0;
var exception = Record.Exception(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
Assert.Null(exception);
Assert.Equal(1, enableRobotRetryCount);
Assert.Equal(1, waitCount);
}
/// <summary>
/// 验证重试 EnableRobot 之后若 J519 仍未就绪，会继续抛出带状态位的异常。
/// </summary>
[Fact]
public void EnsureJ519ReadyForDenseExecutionCore_ThrowsWhenStillNotReadyAfterRetry()
{
var responses = new Queue<FanucJ519Response?>(
[
CreateJ519Response(status: 0b0000, sequence: 21),
CreateJ519Response(status: 0b0010, sequence: 22)
]);
var enableRobotRetryCount = 0;
var waitCount = 0;
var exception = Assert.Throws<InvalidOperationException>(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
Assert.Equal(1, enableRobotRetryCount);
Assert.Equal(1, waitCount);
Assert.Contains("accept_cmd=False", exception.Message, StringComparison.Ordinal);
Assert.Contains("sysrdy=False", exception.Message, StringComparison.Ordinal);
Assert.Contains("seq=22", exception.Message, StringComparison.Ordinal);
}
/// <summary>
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
/// </summary>
@@ -728,6 +875,30 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
/// <summary>
/// 创建用于就绪状态测试的最小 J519 响应。
/// </summary>
/// <param name="status">待注入的 J519 状态位。</param>
/// <param name="sequence">待注入的响应序号。</param>
/// <returns>包含最小字段集合的测试响应。</returns>
private static FanucJ519Response CreateJ519Response(byte status, uint sequence)
{
return new FanucJ519Response(
messageType: 0,
version: 1,
sequence: sequence,
status: status,
readIoType: 0,
readIoIndex: 0,
readIoMask: 0,
readIoValue: 0,
timestamp: 0,
pose: new double[6],
externalAxes: new double[3],
jointDegrees: new double[6],
motorCurrents: new double[6]);
}
private static RobotProfile CreateMoveJointReferenceRobotProfile()
{
return new RobotProfile(
@@ -906,13 +1077,14 @@ public sealed class FanucControllerRuntimeDenseTests
UdpClient server,
IPEndPoint clientEndpoint,
uint sequence,
CancellationToken cancellationToken)
CancellationToken cancellationToken,
byte status = 0b0111)
{
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
responsePacket[0x0c] = 0b0111;
responsePacket[0x0c] = status;
await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
}
@@ -929,6 +1101,54 @@ public sealed class FanucControllerRuntimeDenseTests
return runDirectories[0];
}
/// <summary>
/// 获取本次测试刚生成的新稠密发送记录目录，避免误读历史运行产物。
/// </summary>
private static string GetNewDenseSendRunDirectory(string outputRoot, IReadOnlySet<string> beforeRunDirectories)
{
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
Assert.True(Directory.Exists(denseSendRoot));
var newDirectories = Directory.GetDirectories(denseSendRoot)
.Where(path => !beforeRunDirectories.Contains(path))
.ToArray();
Assert.Single(newDirectories);
return newDirectories[0];
}
/// <summary>
/// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。
/// </summary>
private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
{
var configPath = Path.Combine(
TestRobotFactory.GetReplacementRoot(),
"src",
"Flyshot.Server.Host",
"bin",
"Debug",
"net8.0",
"Config",
"RobotConfig.json");
var configRoot = Path.GetDirectoryName(configPath)!;
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
var program = loaded.Programs["UTTC_MS11"];
var uploaded = new ControllerClientCompatUploadedTrajectory(
name: program.Name,
waypoints: program.Waypoints.Select(static waypoint => waypoint.Positions),
shotFlags: program.ShotFlags,
offsetValues: program.OffsetValues,
addressGroups: program.AddressGroups.Select(static group => group.Addresses));
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", loaded.Robot.AccLimitScale, loaded.Robot.JerkLimitScale);
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
}
/// <summary>
/// 解析空格分隔的纯文本数值列。
/// </summary>
@@ -984,4 +1204,13 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.NotNull(field);
field.SetValue(client, response);
}
/// <summary>
/// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
/// </summary>
private sealed record UttcMs11RuntimeFixture(
string ConfigRoot,
CompatibilityRobotSettings Settings,
ControllerClientCompatUploadedTrajectory Uploaded,
RobotProfile Robot);
}

20
tests/Flyshot.Core.Tests/FanucProtocolTests.cs
View File

@@ -30,7 +30,7 @@ public sealed class FanucProtocolTests
var stopResponse = FanucCommandProtocol.ParseResultResponse(
Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
Convert.FromHexString("646f7a000000160000200300000000000000017a6f64"));
Convert.FromHexString("646f7a000000160000200300000001000000007a6f64"));
Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
Assert.True(stopResponse.IsSuccess);
@@ -183,6 +183,24 @@ public sealed class FanucProtocolTests
Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
}
/// <summary>
/// 验证实时发送路径可以复用命令对象，并用机器人状态包序号覆盖出包序号。
/// </summary>
[Fact]
public void J519Protocol_PacksCommandWithOverrideSequenceIntoReusableBuffer()
{
var command = new FanucJ519Command(
sequence: 2,
targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
var packet = new byte[FanucJ519Protocol.CommandPacketLength];
FanucJ519Protocol.PackCommandPacket(command, sequence: 456, packet);
Assert.Equal(456u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
}
/// <summary>
/// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
/// </summary>

723
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -1,9 +1,13 @@
using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
using Flyshot.Runtime.Fanuc.Protocol;
using System.Globalization;
using System.Reflection;
namespace Flyshot.Core.Tests;
@@ -270,6 +274,364 @@ public sealed class RuntimeOrchestrationTests
Assert.Equal(0.008, firstDt, precision: 3);
}
/// <summary>
/// 验证可直接使用宿主输出目录 Config/RobotConfig.json 中的真实 UTTC_MS11 示教点，
/// 在 1x 规划速度下生成一条完整飞拍轨迹。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_GeneratesCompleteTrajectoryAtOneX()
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
Assert.Equal("UTTC_MS11", bundle.Result.ProgramName);
Assert.NotNull(bundle.Result.DenseJointTrajectory);
Assert.True(bundle.Result.DenseJointTrajectory.Count > fixture.Uploaded.Waypoints.Count);
Assert.True(bundle.Result.Duration.TotalSeconds > 0.0);
Assert.Equal(fixture.Uploaded.ShotFlags.Count(static flag => flag), bundle.Result.ShotEvents.Count);
Assert.Equal(fixture.Uploaded.ShotFlags.Count(static flag => flag), bundle.Result.TriggerTimeline.Count);
var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
Assert.Equal(fixture.Robot.ServoPeriod.TotalSeconds, firstDt, precision: 6);
AssertJointRadiansEqual(fixture.Uploaded.Waypoints[0], bundle.Result.DenseJointTrajectory[0].Skip(1).ToArray());
AssertJointRadiansEqual(fixture.Uploaded.Waypoints[^1], bundle.Result.DenseJointTrajectory[^1].Skip(1).ToArray());
}
/// <summary>
/// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 示教点，导出首尾 10 点整形前后的逐点对比数据，
/// 便于和现场报警时间段逐项核对关节、速度、加速度与跃度。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ExportsEdgeComparisonAtOneX()
{
const int edgePointCount = 10;
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
var shapedDense = bundle.Result.DenseJointTrajectory!;
var outputDir = Path.Combine(
fixture.ConfigRoot,
"Data",
"UTTC_MS11_EdgeShapeDebug_1x",
DateTime.Now.ToString("yyyyMMdd_HHmmss_fff", CultureInfo.InvariantCulture));
Directory.CreateDirectory(outputDir);
var leadingPath = Path.Combine(outputDir, "Leading10Comparison.csv");
var trailingPath = Path.Combine(outputDir, "Trailing10Comparison.csv");
var summaryPath = Path.Combine(outputDir, "Summary.txt");
WriteEdgeComparisonCsv(leadingPath, rawDense, shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count));
WriteEdgeComparisonCsv(
trailingPath,
rawDense,
shapedDense,
Math.Max(0, shapedDense.Count - edgePointCount),
Math.Min(edgePointCount, shapedDense.Count));
var summaryLines = new List<string>
{
$"program=UTTC_MS11",
$"planning_speed_scale=1.000000",
$"servo_period_seconds={fixture.Robot.ServoPeriod.TotalSeconds.ToString("F6", CultureInfo.InvariantCulture)}",
$"dense_point_count={shapedDense.Count}",
$"leading_first_step_raw_deg={ComputeStepDegrees(rawDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
$"leading_first_step_shaped_deg={ComputeStepDegrees(shapedDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
$"trailing_last_step_raw_deg={ComputeStepDegrees(rawDense, rawDense.Count - 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
$"trailing_last_step_shaped_deg={ComputeStepDegrees(shapedDense, shapedDense.Count - 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
$"leading_window_max_abs_raw_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(rawDense, 0, Math.Min(edgePointCount, rawDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
$"leading_window_max_abs_shaped_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
$"trailing_window_max_abs_raw_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(rawDense, Math.Max(0, rawDense.Count - edgePointCount), Math.Min(edgePointCount, rawDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
$"trailing_window_max_abs_shaped_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(shapedDense, Math.Max(0, shapedDense.Count - edgePointCount), Math.Min(edgePointCount, shapedDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
$"leading_csv={leadingPath}",
$"trailing_csv={trailingPath}"
};
File.WriteAllLines(summaryPath, summaryLines);
Assert.True(File.Exists(leadingPath));
Assert.True(File.Exists(trailingPath));
Assert.True(File.Exists(summaryPath));
}
/// <summary>
/// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会降低首尾窗口最大跃度，而不是只把第一步压小。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ReducesEdgeWindowJerkAtOneX()
{
const int edgePointCount = 10;
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
var shapedDense = bundle.Result.DenseJointTrajectory!;
var rawLeadingMaxJerk = ComputeWindowMaxAbsJerkDegrees(rawDense, 0, Math.Min(edgePointCount, rawDense.Count));
var shapedLeadingMaxJerk = ComputeWindowMaxAbsJerkDegrees(shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count));
var rawTrailingMaxJerk = ComputeWindowMaxAbsJerkDegrees(rawDense, Math.Max(0, rawDense.Count - edgePointCount), Math.Min(edgePointCount, rawDense.Count));
var shapedTrailingMaxJerk = ComputeWindowMaxAbsJerkDegrees(shapedDense, Math.Max(0, shapedDense.Count - edgePointCount), Math.Min(edgePointCount, shapedDense.Count));
Assert.True(
shapedLeadingMaxJerk < rawLeadingMaxJerk,
$"首段最大跃度应下降，raw={rawLeadingMaxJerk:F6}, shaped={shapedLeadingMaxJerk:F6}");
Assert.True(
shapedTrailingMaxJerk < rawTrailingMaxJerk,
$"尾段最大跃度应下降，raw={rawTrailingMaxJerk:F6}, shaped={shapedTrailingMaxJerk:F6}");
}
/// <summary>
/// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会缩小首尾第一步，
/// 但不能把首尾直接压成静止平台，否则现场执行会变成先停住再起步。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ReducesEdgeStepWithoutFlatteningToZeroAtOneX()
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
var shapedDense = bundle.Result.DenseJointTrajectory!;
var rawLeadingFirstStep = Math.Abs(ComputeStepDegrees(rawDense, 1, 0));
var shapedLeadingFirstStep = Math.Abs(ComputeStepDegrees(shapedDense, 1, 0));
var rawTrailingLastStep = Math.Abs(ComputeStepDegrees(rawDense, rawDense.Count - 1, 0));
var shapedTrailingLastStep = Math.Abs(ComputeStepDegrees(shapedDense, shapedDense.Count - 1, 0));
Assert.True(
shapedLeadingFirstStep < rawLeadingFirstStep,
$"首段第一步应变小，raw={rawLeadingFirstStep:F6}, shaped={shapedLeadingFirstStep:F6}");
Assert.True(
shapedTrailingLastStep < rawTrailingLastStep,
$"尾段最后一步应变小，raw={rawTrailingLastStep:F6}, shaped={shapedTrailingLastStep:F6}");
Assert.True(
shapedLeadingFirstStep > 1e-6,
$"首段第一步不应被压成 0，actual={shapedLeadingFirstStep:F9}");
Assert.True(
shapedTrailingLastStep > 1e-6,
$"尾段最后一步不应被压成 0，actual={shapedTrailingLastStep:F9}");
}
/// <summary>
/// 对比真实可运行轨迹前 0.04s 反推出的局部三次边界特征与当前首段样条系数，
/// 用于判断当前 clamped-zero 边界是否已经和现场可运行轨迹明显偏离。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ExportsLeadingBoundaryComparison()
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
var applyMethod = typeof(ControllerClientTrajectoryOrchestrator).GetMethod(
"ApplySmoothStartStopTiming",
BindingFlags.Static | BindingFlags.NonPublic);
Assert.NotNull(applyMethod);
var executionTrajectory = Assert.IsType<PlannedTrajectory>(applyMethod!.Invoke(null, [bundle.PlannedTrajectory]));
var spline = new CubicSplineInterpolator(
executionTrajectory.WaypointTimes.ToArray(),
executionTrajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray());
var comparisonPath = Path.Combine(
fixture.ConfigRoot,
"Data",
"UTTC_MS11_EdgeShapeDebug_1x",
DateTime.Now.ToString("yyyyMMdd_HHmmss_fff", CultureInfo.InvariantCulture),
"LeadingBoundaryComparison.txt");
Directory.CreateDirectory(Path.GetDirectoryName(comparisonPath)!);
var realPath = Path.Combine(
TestRobotFactory.GetReplacementRoot(),
"..",
"Rvbust",
"前两个点正常 飞拍失败的运行",
"真实可运行的飞拍轨迹点位JointDetialTraj.txt");
var realRows = File.ReadLines(realPath)
.TakeWhile(static line => !string.IsNullOrWhiteSpace(line))
.Select(ParseSpaceSeparatedDoubles)
.Take(4)
.ToArray();
Assert.Equal(4, realRows.Length);
var fieldH = typeof(CubicSplineInterpolator).GetField("_h", BindingFlags.Instance | BindingFlags.NonPublic);
var fieldA = typeof(CubicSplineInterpolator).GetField("_a", BindingFlags.Instance | BindingFlags.NonPublic);
var fieldB = typeof(CubicSplineInterpolator).GetField("_b", BindingFlags.Instance | BindingFlags.NonPublic);
var fieldC = typeof(CubicSplineInterpolator).GetField("_c", BindingFlags.Instance | BindingFlags.NonPublic);
Assert.NotNull(fieldH);
Assert.NotNull(fieldA);
Assert.NotNull(fieldB);
Assert.NotNull(fieldC);
var segmentDurations = Assert.IsType<double[]>(fieldH!.GetValue(spline));
var coeffA = Assert.IsType<double[][]>(fieldA!.GetValue(spline));
var coeffB = Assert.IsType<double[][]>(fieldB!.GetValue(spline));
var coeffC = Assert.IsType<double[][]>(fieldC!.GetValue(spline));
var lines = new List<string>
{
$"real_path={Path.GetFullPath(realPath)}",
$"current_segment0_h_seconds={segmentDurations[0].ToString("F9", CultureInfo.InvariantCulture)}",
$"real_window_t0={realRows[0][0].ToString("F9", CultureInfo.InvariantCulture)}",
$"real_window_t1={realRows[1][0].ToString("F9", CultureInfo.InvariantCulture)}",
$"real_window_t2={realRows[2][0].ToString("F9", CultureInfo.InvariantCulture)}",
$"real_window_t3={realRows[3][0].ToString("F9", CultureInfo.InvariantCulture)}"
};
for (var jointIndex = 0; jointIndex < 6; jointIndex++)
{
var currentStartVelocityDeg = RadiansToDegrees(coeffC[0][jointIndex]);
var currentStartAccelerationDeg = RadiansToDegrees(2.0 * coeffB[0][jointIndex]);
var currentJerkDeg = RadiansToDegrees(6.0 * coeffA[0][jointIndex]);
var currentEndVelocityDeg = RadiansToDegrees(
(3.0 * coeffA[0][jointIndex] * segmentDurations[0] * segmentDurations[0])
+ (2.0 * coeffB[0][jointIndex] * segmentDurations[0])
+ coeffC[0][jointIndex]);
var localRealFit = FitCubicFromFirstFourRows(realRows, jointIndex + 1);
var realStartVelocityDeg = localRealFit.Coefficient1;
var realStartAccelerationDeg = 2.0 * localRealFit.Coefficient2;
var realJerkDeg = 6.0 * localRealFit.Coefficient3;
var realEndVelocityDeg = (3.0 * localRealFit.Coefficient3 * localRealFit.LastLocalTime * localRealFit.LastLocalTime)
+ (2.0 * localRealFit.Coefficient2 * localRealFit.LastLocalTime)
+ localRealFit.Coefficient1;
lines.Add(
$"J{jointIndex + 1}: current_start_v_deg_s={currentStartVelocityDeg:F9}, real_start_v_deg_s={realStartVelocityDeg:F9}, current_start_a_deg_s2={currentStartAccelerationDeg:F9}, real_start_a_deg_s2={realStartAccelerationDeg:F9}, current_jerk_deg_s3={currentJerkDeg:F9}, real_jerk_deg_s3={realJerkDeg:F9}, current_end_v_deg_s={currentEndVelocityDeg:F9}, real_end_v_deg_s={realEndVelocityDeg:F9}");
}
File.WriteAllLines(comparisonPath, lines);
Assert.True(File.Exists(comparisonPath));
}
/// <summary>
/// 验证飞拍规划结果会应用平滑起停时间轴，而不是直接复用原始线性时间轴采样。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_ShapesDenseTrajectoryEdges()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: robot.ServoPeriod.TotalSeconds);
Assert.NotNull(bundle.Result.DenseJointTrajectory);
Assert.True(bundle.Result.DenseJointTrajectory.Count > 4);
var dense = bundle.Result.DenseJointTrajectory;
Assert.NotEqual(rawDense[1][1], dense[1][1]);
Assert.NotEqual(rawDense[^2][1], dense[^2][1]);
Assert.Equal(rawDense[0][1], dense[0][1], precision: 6);
Assert.Equal(rawDense[^1][1], dense[^1][1], precision: 6);
Assert.Equal(rawDense.Count, dense.Count);
}
/// <summary>
/// 验证旧的首尾整形工具在独立调用时仍会改动首尾窗口，供对比和回归保留。
/// </summary>
[Fact]
public void FlyshotTrajectoryEdgeShaper_ShapesLeadingAndTrailingStepsWithoutFlatteningMiddleSection()
{
var denseTrajectory = new IReadOnlyList<double>[]
{
[0.000, DegreesToRadians(60.546226), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.008, DegreesToRadians(60.541482), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.016, DegreesToRadians(60.536738), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.024, DegreesToRadians(60.531994), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.032, DegreesToRadians(60.527250), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.040, DegreesToRadians(60.522506), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.048, DegreesToRadians(60.517762), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.056, DegreesToRadians(60.513018), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.064, DegreesToRadians(60.508274), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.072, DegreesToRadians(60.503530), 0.0, 0.0, 0.0, 0.0, 0.0],
[0.080, DegreesToRadians(60.498786), 0.0, 0.0, 0.0, 0.0, 0.0]
};
var shaped = FlyshotTrajectoryEdgeShaper.ShapeDenseJointTrajectory(denseTrajectory);
Assert.Equal(denseTrajectory.Length, shaped.Count);
Assert.Equal(denseTrajectory[0][0], shaped[0][0], precision: 6);
Assert.Equal(denseTrajectory[^1][0], shaped[^1][0], precision: 6);
Assert.Equal(denseTrajectory[0][1], shaped[0][1], precision: 12);
Assert.Equal(denseTrajectory[^1][1], shaped[^1][1], precision: 12);
var firstStepDegrees = Math.Abs(RadiansToDegrees(shaped[1][1] - shaped[0][1]));
var lastStepDegrees = Math.Abs(RadiansToDegrees(shaped[^1][1] - shaped[^2][1]));
var originalFirstStepDegrees = Math.Abs(RadiansToDegrees(denseTrajectory[1][1] - denseTrajectory[0][1]));
var originalLastStepDegrees = Math.Abs(RadiansToDegrees(denseTrajectory[^1][1] - denseTrajectory[^2][1]));
Assert.NotEqual(originalFirstStepDegrees, firstStepDegrees);
Assert.NotEqual(originalLastStepDegrees, lastStepDegrees);
var middleStepDegrees = Math.Abs(RadiansToDegrees(shaped[5][1] - shaped[4][1]));
Assert.True(middleStepDegrees > 0.002, $"中段单步变化不应被整体压平，actual={middleStepDegrees:F6}deg");
}
/// <summary>
/// 验证飞拍首尾整形只覆盖首尾各 10 个点，锚点之外的中段采样保持不变。
/// </summary>
[Fact]
public void FlyshotTrajectoryEdgeShaper_ShapesOnlyLeadingAndTrailingTenPoints()
{
var denseTrajectory = Enumerable.Range(0, 31)
.Select(index => (IReadOnlyList<double>)
[
index * 0.008,
DegreesToRadians(60.0 - index),
DegreesToRadians(index * 0.1),
DegreesToRadians(-index * 0.5),
DegreesToRadians(index * 0.02),
DegreesToRadians(index * 0.2),
DegreesToRadians(index * 0.05)
])
.ToArray();
var shaped = FlyshotTrajectoryEdgeShaper.ShapeDenseJointTrajectory(denseTrajectory);
Assert.Equal(denseTrajectory.Length, shaped.Count);
Assert.NotEqual(denseTrajectory[1][1], shaped[1][1]);
Assert.NotEqual(denseTrajectory[9][1], shaped[9][1]);
Assert.Equal(denseTrajectory[10][1], shaped[10][1], precision: 12);
Assert.Equal(denseTrajectory[11][1], shaped[11][1], precision: 12);
Assert.Equal(denseTrajectory[19][1], shaped[19][1], precision: 12);
Assert.Equal(denseTrajectory[20][1], shaped[20][1], precision: 12);
Assert.NotEqual(denseTrajectory[21][1], shaped[21][1]);
Assert.NotEqual(denseTrajectory[29][1], shaped[29][1]);
Assert.Equal(denseTrajectory[30][1], shaped[30][1], precision: 12);
}
/// <summary>
/// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。
/// </summary>
@@ -564,8 +926,369 @@ public sealed class RuntimeOrchestrationTests
}
""");
}
/// <summary>
/// 角度转弧度，供轨迹整形测试构造输入。
/// </summary>
private static double DegreesToRadians(double degrees)
{
return degrees * Math.PI / 180.0;
}
/// <summary>
/// 弧度转角度，供轨迹整形测试验证单步变化量。
/// </summary>
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
/// <summary>
/// 验证稠密轨迹端点仍与原始示教点一致。
/// </summary>
private static void AssertJointRadiansEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualRadians)
{
Assert.Equal(expectedRadians.Count, actualRadians.Count);
for (var index = 0; index < expectedRadians.Count; index++)
{
Assert.Equal(expectedRadians[index], actualRadians[index], precision: 6);
}
}
/// <summary>
/// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。
/// </summary>
private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
{
var configPath = Path.Combine(
TestRobotFactory.GetReplacementRoot(),
"src",
"Flyshot.Server.Host",
"bin",
"Debug",
"net8.0",
"Config",
"RobotConfig.json");
var configRoot = Path.GetDirectoryName(configPath)!;
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
var program = loaded.Programs["UTTC_MS11"];
var uploaded = new ControllerClientCompatUploadedTrajectory(
name: program.Name,
waypoints: program.Waypoints.Select(static waypoint => waypoint.Positions),
shotFlags: program.ShotFlags,
offsetValues: program.OffsetValues,
addressGroups: program.AddressGroups.Select(static group => group.Addresses));
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", loaded.Robot.AccLimitScale, loaded.Robot.JerkLimitScale);
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
}
/// <summary>
/// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。
/// </summary>
private static void WriteEdgeComparisonCsv(
string path,
IReadOnlyList<IReadOnlyList<double>> rawDense,
IReadOnlyList<IReadOnlyList<double>> shapedDense,
int startIndex,
int count)
{
var lines = new List<string>
{
BuildEdgeComparisonHeader()
};
for (var index = startIndex; index < startIndex + count && index < rawDense.Count && index < shapedDense.Count; index++)
{
var columns = new List<string>
{
index.ToString(CultureInfo.InvariantCulture),
rawDense[index][0].ToString("F6", CultureInfo.InvariantCulture)
};
AppendJointColumns(columns, "raw_j", index, rawDense, static (rows, rowIndex, jointIndex) => RadiansToDegrees(rows[rowIndex][jointIndex + 1]));
AppendJointColumns(columns, "shaped_j", index, shapedDense, static (rows, rowIndex, jointIndex) => RadiansToDegrees(rows[rowIndex][jointIndex + 1]));
AppendJointColumns(columns, "raw_step", index, rawDense, ComputeStepDegrees);
AppendJointColumns(columns, "shaped_step", index, shapedDense, ComputeStepDegrees);
AppendJointColumns(columns, "raw_velocity", index, rawDense, ComputeVelocityDegrees);
AppendJointColumns(columns, "shaped_velocity", index, shapedDense, ComputeVelocityDegrees);
AppendJointColumns(columns, "raw_acceleration", index, rawDense, ComputeAccelerationDegrees);
AppendJointColumns(columns, "shaped_acceleration", index, shapedDense, ComputeAccelerationDegrees);
AppendJointColumns(columns, "raw_jerk", index, rawDense, ComputeJerkDegrees);
AppendJointColumns(columns, "shaped_jerk", index, shapedDense, ComputeJerkDegrees);
lines.Add(string.Join(",", columns));
}
File.WriteAllLines(path, lines);
}
/// <summary>
/// 构造首尾整形逐点对比 CSV 表头。
/// </summary>
private static string BuildEdgeComparisonHeader()
{
var columns = new List<string>
{
"sample_index",
"time_seconds"
};
AppendJointHeader(columns, "raw_j");
AppendJointHeader(columns, "shaped_j");
AppendJointHeader(columns, "raw_step");
AppendJointHeader(columns, "shaped_step");
AppendJointHeader(columns, "raw_velocity");
AppendJointHeader(columns, "shaped_velocity");
AppendJointHeader(columns, "raw_acceleration");
AppendJointHeader(columns, "shaped_acceleration");
AppendJointHeader(columns, "raw_jerk");
AppendJointHeader(columns, "shaped_jerk");
return string.Join(",", columns);
}
/// <summary>
/// 追加 J1..J6 表头列名。
/// </summary>
private static void AppendJointHeader(List<string> columns, string prefix)
{
for (var jointIndex = 1; jointIndex <= 6; jointIndex++)
{
columns.Add($"{prefix}_j{jointIndex}");
}
}
/// <summary>
/// 追加某一行对应的六轴数值列。
/// </summary>
private static void AppendJointColumns(
List<string> columns,
string prefix,
int rowIndex,
IReadOnlyList<IReadOnlyList<double>> rows,
Func<IReadOnlyList<IReadOnlyList<double>>, int, int, double> selector)
{
for (var jointIndex = 0; jointIndex < 6; jointIndex++)
{
columns.Add(selector(rows, rowIndex, jointIndex).ToString("F6", CultureInfo.InvariantCulture));
}
}
/// <summary>
/// 计算某一行相对上一行的单步角度变化量。
/// </summary>
private static double ComputeStepDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
{
if (rowIndex <= 0)
{
return 0.0;
}
return RadiansToDegrees(rows[rowIndex][jointIndex + 1] - rows[rowIndex - 1][jointIndex + 1]);
}
/// <summary>
/// 计算某一行的角速度，单位 deg/s。
/// </summary>
private static double ComputeVelocityDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
{
if (rowIndex <= 0)
{
return 0.0;
}
var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
if (dt <= 0.0)
{
return 0.0;
}
return ComputeStepDegrees(rows, rowIndex, jointIndex) / dt;
}
/// <summary>
/// 计算某一行的角加速度，单位 deg/s^2。
/// </summary>
private static double ComputeAccelerationDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
{
if (rowIndex <= 1)
{
return 0.0;
}
var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
if (dt <= 0.0)
{
return 0.0;
}
return (ComputeVelocityDegrees(rows, rowIndex, jointIndex) - ComputeVelocityDegrees(rows, rowIndex - 1, jointIndex)) / dt;
}
/// <summary>
/// 计算某一行的角跃度，单位 deg/s^3。
/// </summary>
private static double ComputeJerkDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
{
if (rowIndex <= 2)
{
return 0.0;
}
var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
if (dt <= 0.0)
{
return 0.0;
}
return (ComputeAccelerationDegrees(rows, rowIndex, jointIndex) - ComputeAccelerationDegrees(rows, rowIndex - 1, jointIndex)) / dt;
}
/// <summary>
/// 统计给定窗口内所有关节的最大绝对值跃度，便于快速看首尾尖峰是否被压低。
/// </summary>
private static double ComputeWindowMaxAbsJerkDegrees(
IReadOnlyList<IReadOnlyList<double>> rows,
int startIndex,
int count)
{
var maxAbsJerk = 0.0;
for (var rowIndex = startIndex; rowIndex < startIndex + count && rowIndex < rows.Count; rowIndex++)
{
for (var jointIndex = 0; jointIndex < 6; jointIndex++)
{
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(ComputeJerkDegrees(rows, rowIndex, jointIndex)));
}
}
return maxAbsJerk;
}
/// <summary>
/// 解析空格分隔的关节轨迹行。
/// </summary>
private static double[] ParseSpaceSeparatedDoubles(string line)
{
return line
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
.Select(static value => double.Parse(value, CultureInfo.InvariantCulture))
.ToArray();
}
/// <summary>
/// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线，返回相对首点时间的系数。
/// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0，单位保持输入文件的角度制。
/// </summary>
private static LocalCubicFit FitCubicFromFirstFourRows(IReadOnlyList<double[]> rows, int valueColumn)
{
var t0 = rows[0][0];
var matrix = new double[4][];
var rhs = new double[4];
for (var index = 0; index < 4; index++)
{
var localTime = rows[index][0] - t0;
matrix[index] =
[
localTime * localTime * localTime,
localTime * localTime,
localTime,
1.0
];
rhs[index] = rows[index][valueColumn];
}
var solution = SolveLinearSystem4x4(matrix, rhs);
return new LocalCubicFit(
Coefficient3: solution[0],
Coefficient2: solution[1],
Coefficient1: solution[2],
Coefficient0: solution[3],
LastLocalTime: rows[3][0] - t0);
}
/// <summary>
/// 用高斯消元求解 4x4 线性方程组。
/// </summary>
private static double[] SolveLinearSystem4x4(double[][] matrix, double[] rhs)
{
var a = matrix.Select(static row => row.ToArray()).ToArray();
var b = rhs.ToArray();
for (var pivot = 0; pivot < 4; pivot++)
{
var bestRow = pivot;
for (var row = pivot + 1; row < 4; row++)
{
if (Math.Abs(a[row][pivot]) > Math.Abs(a[bestRow][pivot]))
{
bestRow = row;
}
}
if (Math.Abs(a[bestRow][pivot]) <= 1e-12)
{
throw new InvalidOperationException("真实轨迹前 4 点无法稳定拟合局部三次曲线。");
}
if (bestRow != pivot)
{
(a[pivot], a[bestRow]) = (a[bestRow], a[pivot]);
(b[pivot], b[bestRow]) = (b[bestRow], b[pivot]);
}
var divisor = a[pivot][pivot];
for (var column = pivot; column < 4; column++)
{
a[pivot][column] /= divisor;
}
b[pivot] /= divisor;
for (var row = 0; row < 4; row++)
{
if (row == pivot)
{
continue;
}
var factor = a[row][pivot];
for (var column = pivot; column < 4; column++)
{
a[row][column] -= factor * a[pivot][column];
}
b[row] -= factor * b[pivot];
}
}
return b;
}
}
/// <summary>
/// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
/// </summary>
internal sealed record UttcMs11RuntimeFixture(
string ConfigRoot,
CompatibilityRobotSettings Settings,
ControllerClientCompatUploadedTrajectory Uploaded,
RobotProfile Robot);
/// <summary>
/// 记录基于真实轨迹前 4 点拟合得到的局部三次曲线系数。
/// </summary>
internal sealed record LocalCubicFit(
double Coefficient3,
double Coefficient2,
double Coefficient1,
double Coefficient0,
double LastLocalTime);
/// <summary>
/// 为运行时编排测试构造稳定的最小领域对象。
/// </summary>