feat(fanuc): 统一 speedRatio 执行倍率语义

* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列
* 区分旧格式规划导出与 ActualSend 实发诊断工件
* 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
2026-05-11 17:21:18 +08:00
parent d82128da4a
commit d120ef2a39
17 changed files with 1395 additions and 534 deletions

View File

@@ -8,13 +8,13 @@ namespace Flyshot.Core.Tests;
public sealed class ConfigCompatibilityTests
{
/// <summary>
/// 验证现有 RobotConfig.json 能被加载,并保持关键机器人参数与飞拍程序内容不变。
/// 验证现有旧样本配置能被加载,并保持关键机器人参数与飞拍程序内容不变。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms()
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json");
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "EOL10_EAU_0.json");
var loaded = new RobotConfigLoader().Load(configPath);
@@ -253,7 +253,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath);
@@ -277,7 +277,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
@@ -292,7 +292,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_LoadsProfileAndKinematics_FromSingleParse()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var loaded = new RobotModelLoader().LoadProfileAndKinematics(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);

View File

@@ -21,10 +21,10 @@ public sealed class FanucControllerRuntimeDenseTests
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列,并输出角度制目标
/// 验证真机 J519 会直接消费已经按 8ms 生成好的稠密轨迹,不再按运行时 speedRatio 临场重采样
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_UsesPlannedDenseRowsAndConvertsRadiansToDegrees()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -38,8 +38,8 @@ public sealed class FanucControllerRuntimeDenseTests
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -56,13 +56,19 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(5, commands.Count);
Assert.Equal(3, commands.Count);
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
var expectedJ1Targets = new[] { 0.0, 0.08, 0.16 };
var actualJ1Targets = commands.Select(static command => command.TargetJoints[0]).ToArray();
Assert.Equal(expectedJ1Targets.Length, actualJ1Targets.Length);
for (var index = 0; index < expectedJ1Targets.Length; index++)
{
Assert.Equal(expectedJ1Targets[index], actualJ1Targets[index], precision: 6);
}
Assert.False(j519Client.IsCommandQueueDrainedForTests());
}
@@ -90,8 +96,8 @@ public sealed class FanucControllerRuntimeDenseTests
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
programName: "wait-drain",
@@ -107,7 +113,7 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
// 只有机器人状态包把队列全部取出后ExecuteTrajectory 才能向上层返回。
@@ -200,14 +206,16 @@ public sealed class FanucControllerRuntimeDenseTests
var firstTimingColumns = ParseColumns(timingLines[0]);
var secondTimingColumns = ParseColumns(timingLines[1]);
var lastTimingColumns = ParseColumns(timingLines[^1]);
Assert.Equal(4, firstTimingColumns.Length);
Assert.Equal(6, firstTimingColumns.Length);
Assert.Equal(0.0, firstTimingColumns[0], precision: 6);
Assert.Equal(0.0, firstTimingColumns[1], precision: 6);
Assert.Equal(0.0, firstTimingColumns[2], precision: 6);
Assert.Equal(0.5, firstTimingColumns[3], precision: 6);
Assert.Equal(1.0, firstTimingColumns[3], precision: 6);
Assert.Equal(0.5, firstTimingColumns[4], precision: 6);
Assert.Equal(0.0, firstTimingColumns[5], precision: 6);
Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
Assert.Equal(0.004, secondTimingColumns[2], precision: 6);
Assert.Equal(0.008, secondTimingColumns[2], precision: 6);
Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
@@ -355,6 +363,7 @@ public sealed class FanucControllerRuntimeDenseTests
planningSpeedScale: fullSpeedSettings.PlanningSpeedScale);
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
fixture.Robot,
bundle.ExecutionTrajectory,
bundle.Result,
fixture.Robot.ServoPeriod.TotalSeconds,
speedRatio);
@@ -387,7 +396,9 @@ public sealed class FanucControllerRuntimeDenseTests
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.Equal(preparedExecution.Samples.Count, commands.Count);
AssertJointDegreesEqual(result.DenseJointTrajectory[0].Skip(1).ToArray(), commands[0].TargetJoints);
Assert.Equal(speedRatio, preparedExecution.FinalSpeedRatio, precision: 6);
Assert.Equal(0, preparedExecution.StretchIterationCount);
Assert.Equal(preparedExecution.Samples[0].JointsDegrees, commands[0].TargetJoints);
}
finally
{
@@ -463,13 +474,13 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标,并继续叠加 speed_ratio 重采样
/// 验证 MoveJoint 会在生成阶段折算 speed_ratio运行时只发送已经规划好的 8ms 稠密点列
/// </summary>
[Theory]
[InlineData(1.0)]
[InlineData(0.7)]
[InlineData(0.5)]
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryWithSpeedRatioAlreadyPlanned(double speedRatio)
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -524,7 +535,7 @@ public sealed class FanucControllerRuntimeDenseTests
}
[Fact]
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
public void MoveJointTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
@@ -536,9 +547,12 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
AssertDenseRowsUseServoPeriod(fullSpeed.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed07.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed05.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 1.0));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5));
}
[Fact]
@@ -565,10 +579,10 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务
/// 验证运行时稠密发送不再依赖当前 speed_ratio倍率合法性应在上游规划/生成阶段处理
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_IgnoresRuntimeSpeedRatioForDenseFallback()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -576,12 +590,14 @@ public sealed class FanucControllerRuntimeDenseTests
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 0.0);
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -598,9 +614,12 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
var exception = Assert.Throws<InvalidOperationException>(
() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(3, commands.Count);
Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
}
/// <summary>
@@ -819,8 +838,10 @@ public sealed class FanucControllerRuntimeDenseTests
var enableRobotRetryCount = 0;
var waitCount = 0;
using var runtime = new FanucControllerRuntime();
var exception = Record.Exception(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => runtime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
@@ -844,8 +865,10 @@ public sealed class FanucControllerRuntimeDenseTests
var enableRobotRetryCount = 0;
var waitCount = 0;
using var runtime = new FanucControllerRuntime();
var exception = Assert.Throws<InvalidOperationException>(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => runtime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
@@ -980,8 +1003,9 @@ public sealed class FanucControllerRuntimeDenseTests
var rows = result.DenseJointTrajectory!;
Assert.True(
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7),
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
AssertDenseRowsUseServoPeriod(rows, robot.ServoPeriod.TotalSeconds);
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
}
@@ -1207,8 +1231,14 @@ public sealed class FanucControllerRuntimeDenseTests
private static double ExpectedSmoothPtpDuration(
RobotProfile robot,
IReadOnlyList<double> startJoints,
IReadOnlyList<double> targetJoints)
IReadOnlyList<double> targetJoints,
double speedRatio = 1.0)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var duration = 0.0;
for (var index = 0; index < robot.DegreesOfFreedom; index++)
{
@@ -1219,15 +1249,27 @@ public sealed class FanucControllerRuntimeDenseTests
}
var limit = robot.JointLimits[index];
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / (limit.VelocityLimit * speedRatio);
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / (limit.AccelerationLimit * speedRatio * speedRatio));
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / (limit.JerkLimit * speedRatio * speedRatio * speedRatio));
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
/// <summary>
/// 验证 MoveJoint 稠密轨迹已经是物理伺服周期,不把 speed_ratio 编码到采样间隔里。
/// </summary>
private static void AssertDenseRowsUseServoPeriod(IReadOnlyList<IReadOnlyList<double>> rows, double servoPeriodSeconds)
{
Assert.True(rows.Count > 1);
for (var index = 1; index < rows.Count; index++)
{
Assert.Equal(servoPeriodSeconds, rows[index][0] - rows[index - 1][0], precision: 6);
}
}
private static double DegreesToRadians(double degrees)
{
return degrees * Math.PI / 180.0;

View File

@@ -1,4 +1,6 @@
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Core.Tests;
@@ -43,6 +45,58 @@ public sealed class J519SendTrajectorySamplerTests
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证飞拍 speed_ratio 安全队列从连续规划样条直接采样,而不是先把规划轨迹离散成稠密点后再线性插值。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_DirectlyEvaluatesContinuousSplineForSpeedRatio()
{
var trajectory = CreateSingleJointTrajectory();
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.5);
var sample = Assert.Single(samples.Where(static item => Math.Abs(item.TrajectoryTime - 0.004) < 1e-9));
Assert.Equal(0.008, sample.SendTime, precision: 6);
Assert.Equal(0.004, sample.TrajectoryTime, precision: 6);
Assert.NotEqual(45.0, sample.JointsDegrees[0], precision: 3);
var expected = new CubicSplineInterpolator(
trajectory.WaypointTimes.ToArray(),
trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray())
.Evaluate(0.004)[0] * 180.0 / Math.PI;
Assert.Equal(expected, sample.JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证 speed_ratio 等价于执行侧时间轴拉长:原轨迹 0.8 倍执行应与时间整体拉长 1/0.8 的轨迹 1 倍执行一致。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_SpeedRatioMatchesUniformPlanningTimeStretch()
{
var trajectory = CreateSingleJointTrajectory();
var stretchedTrajectory = StretchTrajectoryTime(trajectory, stretchFactor: 1.0 / 0.8);
var ratioSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.8);
var stretchedSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
stretchedTrajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Equal(stretchedSamples.Count, ratioSamples.Count);
for (var index = 0; index < ratioSamples.Count; index++)
{
Assert.Equal(stretchedSamples[index].SendTime, ratioSamples[index].SendTime, precision: 6);
Assert.Equal(stretchedSamples[index].TrajectoryTime * 0.8, ratioSamples[index].TrajectoryTime, precision: 6);
Assert.Equal(stretchedSamples[index].JointsDegrees[0], ratioSamples[index].JointsDegrees[0], precision: 6);
}
}
/// <summary>
/// 验证空稠密轨迹会直接暴露为调用错误,避免生成无意义下发点。
/// </summary>
@@ -96,4 +150,84 @@ public sealed class J519SendTrajectorySamplerTests
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
}
/// <summary>
/// 验证倍率诊断行在保留旧 4 列的同时追加请求倍率和历史改写次数。
/// </summary>
[Fact]
public void BuildTimingRow_WithRatioDiagnostics_AppendsRequestedRatioAndIterationCount()
{
var sample = new J519SendSample(
sampleIndex: 2,
sendTime: 0.016,
trajectoryTime: 0.0076,
speedRatio: 0.475,
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var row = J519SendTrajectorySampler.BuildTimingRow(
sample,
requestedSpeedRatio: 0.5,
stretchIterationCount: 1);
Assert.Equal([2.0, 0.016, 0.0076, 0.475, 0.5, 1.0], row);
}
/// <summary>
/// 构造一个最小单关节规划轨迹,便于验证 speed_ratio 直接样条采样语义。
/// </summary>
private static PlannedTrajectory CreateSingleJointTrajectory()
{
var robot = new RobotProfile(
name: "TestRobot",
modelPath: "Models/Test.robot",
degreesOfFreedom: 1,
jointLimits: [new JointLimit("J1", 100.0, 1000.0, 10000.0)],
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
var program = new FlyshotProgram(
name: "spline-sample",
waypoints:
[
new JointWaypoint([0.0]),
new JointWaypoint([Math.PI / 2.0]),
new JointWaypoint([Math.PI])
],
shotFlags: [false, false, false],
offsetValues: [0, 0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>())
]);
return new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.008, 0.016],
segmentDurations: [0.008, 0.008],
segmentScales: [1.0, 1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
}
/// <summary>
/// 构造统一拉长时间轴后的轨迹,模拟更低 planning_speed_scale 生成的等几何时间结果。
/// </summary>
private static PlannedTrajectory StretchTrajectoryTime(PlannedTrajectory trajectory, double stretchFactor)
{
return new PlannedTrajectory(
robot: trajectory.Robot,
originalProgram: trajectory.OriginalProgram,
plannedWaypoints: trajectory.PlannedWaypoints,
waypointTimes: trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(),
segmentDurations: trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(),
segmentScales: trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(),
method: trajectory.Method,
iterations: trajectory.Iterations,
threshold: trajectory.Threshold);
}
}

View File

@@ -40,7 +40,7 @@ public sealed class OfflinePlanTests
// 1. 加载配置和模型。
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
var program = loadedConfig.Programs[trajName];
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
resolvedRobotModelPath,
loadedConfig.Robot.AccLimitScale,

View File

@@ -67,7 +67,7 @@ public sealed class PlanningCompatibilityTests
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var config = new RobotConfigLoader().Load(configPath);
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
@@ -269,6 +269,144 @@ public sealed class PlanningCompatibilityTests
validateJerk: false);
}
/// <summary>
/// 验证飞拍最终 J519 队列的 jerk 硬约束使用严格限值,不再沿用历史上放大 4 倍的宽容口径。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_ThrowsForJ519Jerk_WhenStrictFinalQueueValidationIsEnabled()
{
var robot = CreateRobotProfile([100.0], [1000.0], [100.0]);
var samples = new[]
{
new J519SendSample(0, 0.000, 0.000, 1.0, [0.0]),
new J519SendSample(1, 0.008, 0.008, 1.0, [0.0]),
new J519SendSample(2, 0.016, 0.016, 1.0, [0.003754]),
new J519SendSample(3, 0.024, 0.024, 1.0, [0.007508])
};
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: "strict-final-j519",
validateJerk: true));
Assert.Contains("Jerk", exception.Message);
Assert.Contains("strict-final-j519", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列在请求倍率下严格 jerk 超限时直接拒绝执行,而不是自动改写 speed_ratio。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_ThrowsInsteadOfRewritingRequestedSpeedRatio()
{
var robot = CreateRobotProfile([100.0], [10000.0], [800.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "strict-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var initialSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
initialSamples,
trajectoryName: "strict-builder-initial",
validateJerk: true));
var exception = Assert.Throws<InvalidOperationException>(() =>
FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 1.0));
Assert.Contains("speed_ratio=1.000000", exception.Message);
Assert.Contains("planning_speed_scale", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列通过严格校验时,请求 speed_ratio 会原样成为最终倍率。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_KeepsRequestedSpeedRatioWhenQueuePasses()
{
var robot = CreateRobotProfile([100.0], [10000.0], [10000.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "rewrite-speedratio-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var prepared = FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 0.8);
Assert.Equal(0.8, prepared.RequestSpeedRatio, precision: 6);
Assert.Equal(0.8, prepared.FinalSpeedRatio, precision: 6);
Assert.Equal(0, prepared.StretchIterationCount);
Assert.All(prepared.Samples, sample => Assert.Equal(0.8, sample.SpeedRatio, precision: 6));
Assert.All(prepared.TimingRows, row =>
{
Assert.Equal(0.8, row[3], precision: 6);
Assert.Equal(0.8, row[4], precision: 6);
Assert.Equal(0.0, row[5], precision: 6);
});
}
/// <summary>
/// 构造一个最小 RobotProfile便于规划层单元测试聚焦在时间轴逻辑上。
/// </summary>

View File

@@ -158,6 +158,83 @@ public sealed class RuntimeOrchestrationTests
$"半速规划时长应接近全速的 2 倍,实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
}
/// <summary>
/// 验证飞拍生成轨迹时会把 speed_ratio 折算进有效规划倍率,使倍率乘积相同的配置生成等价轨迹。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesSpeedRatioAsPlanningScaleFactor()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var planningSideSlowdown = CreateFlyshotSettings(planningSpeedScale: 0.45, speedRatio: 1.0);
var runtimeSideSlowdown = CreateFlyshotSettings(planningSpeedScale: 0.9, speedRatio: 0.5);
var planningSideBundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: planningSideSlowdown);
var runtimeSideBundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: runtimeSideSlowdown);
Assert.Equal(planningSideBundle.Result.Duration.TotalSeconds, runtimeSideBundle.Result.Duration.TotalSeconds, precision: 6);
Assert.Equal(planningSideBundle.ExecutionTrajectory.WaypointTimes.Count, runtimeSideBundle.ExecutionTrajectory.WaypointTimes.Count);
for (var index = 0; index < planningSideBundle.ExecutionTrajectory.WaypointTimes.Count; index++)
{
Assert.Equal(
planningSideBundle.ExecutionTrajectory.WaypointTimes[index],
runtimeSideBundle.ExecutionTrajectory.WaypointTimes[index],
precision: 6);
}
Assert.Equal(planningSideBundle.Result.DenseJointTrajectory!.Count, runtimeSideBundle.Result.DenseJointTrajectory!.Count);
for (var rowIndex = 0; rowIndex < planningSideBundle.Result.DenseJointTrajectory.Count; rowIndex++)
{
var planningSideRow = planningSideBundle.Result.DenseJointTrajectory[rowIndex];
var runtimeSideRow = runtimeSideBundle.Result.DenseJointTrajectory[rowIndex];
Assert.Equal(planningSideRow.Count, runtimeSideRow.Count);
for (var columnIndex = 0; columnIndex < planningSideRow.Count; columnIndex++)
{
Assert.Equal(planningSideRow[columnIndex], runtimeSideRow[columnIndex], precision: 6);
}
}
}
/// <summary>
/// 验证普通轨迹生成也会把 speed_ratio 折算进规划倍率,避免把限速留到发送阶段处理。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_UsesSpeedRatioAsPlanningScaleFactor()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.001, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.002, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.003, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var planningSideBundle = orchestrator.PlanOrdinaryTrajectory(
robot,
waypoints,
planningSpeedScale: 0.45);
var runtimeSideBundle = orchestrator.PlanOrdinaryTrajectory(
robot,
waypoints,
planningSpeedScale: 0.9,
speedRatio: 0.5);
Assert.Equal(planningSideBundle.Result.Duration.TotalSeconds, runtimeSideBundle.Result.Duration.TotalSeconds, precision: 6);
Assert.Equal(planningSideBundle.Result.DenseJointTrajectory!.Count, runtimeSideBundle.Result.DenseJointTrajectory!.Count);
for (var rowIndex = 0; rowIndex < planningSideBundle.Result.DenseJointTrajectory.Count; rowIndex++)
{
var planningSideRow = planningSideBundle.Result.DenseJointTrajectory[rowIndex];
var runtimeSideRow = runtimeSideBundle.Result.DenseJointTrajectory[rowIndex];
for (var columnIndex = 0; columnIndex < planningSideRow.Count; columnIndex++)
{
Assert.Equal(planningSideRow[columnIndex], runtimeSideRow[columnIndex], precision: 6);
}
}
}
/// <summary>
/// 验证飞拍缓存键包含规划限速倍率,避免降速验证时误用 100% 速度下的规划结果。
/// </summary>
@@ -314,35 +391,37 @@ public sealed class RuntimeOrchestrationTests
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var legacyFitSettings = DisableSmoothStartStopTiming(fixture.Settings);
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings);
settings: legacyFitSettings);
var baselineBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
var executionDense = TrajectorySampler.SampleJointTrajectory(
bundle.ExecutionTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
Assert.Equal(0.7422771653721995, fixture.Settings.PlanningSpeedScale, precision: 12);
Assert.False(fixture.Settings.SmoothStartStopTiming);
Assert.Equal(0.74227, legacyFitSettings.PlanningSpeedScale, precision: 6);
Assert.False(legacyFitSettings.SmoothStartStopTiming);
Assert.Equal(fixture.Uploaded.Waypoints.Count, bundle.PlannedTrajectory.WaypointTimes.Count);
Assert.Equal(
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / fixture.Settings.PlanningSpeedScale,
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / legacyFitSettings.PlanningSpeedScale,
bundle.PlannedTrajectory.WaypointTimes[^1],
precision: 6);
Assert.Equal(7.403046, bundle.PlannedTrajectory.WaypointTimes[^1], precision: 3);
// 关闭二次时间重映射时,运行时稠密点应直接使用规划样条采样,避免再次改变通用规划时间轴
Assert.Equal(rawDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(rawDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
// 关闭二次时间重映射时,稠密点应和最终执行时间轴自洽;若离散限幅额外拉长,也要反映在 ExecutionTrajectory 上
Assert.Equal(executionDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(executionDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
Assert.True(bundle.ExecutionTrajectory.WaypointTimes[^1] >= bundle.PlannedTrajectory.WaypointTimes[^1]);
Assert.True(
Math.Abs(bundle.PlannedTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留规划终点时间planned={bundle.PlannedTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
Math.Abs(bundle.ExecutionTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留最终执行时间轴终点execution={bundle.ExecutionTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
}
/// <summary>
@@ -697,6 +776,54 @@ public sealed class RuntimeOrchestrationTests
Assert.Throws<ArgumentException>(Act);
}
/// <summary>
/// 验证兼容服务执行普通轨迹时会把运行时 speed_ratio 前移到规划阶段。
/// </summary>
[Fact]
public void ControllerClientCompatService_ExecuteTrajectory_PassesSpeedRatioIntoOrdinaryPlanning()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot, planningSpeedScale: 0.9, speedRatio: 1.0);
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var runtime = new RecordingControllerRuntime();
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
var waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.001, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.002, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.003, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
runtime.SetSpeedRatio(0.5);
service.ExecuteTrajectory(waypoints);
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
var expected = new ControllerClientTrajectoryOrchestrator()
.PlanOrdinaryTrajectory(robot, waypoints, planningSpeedScale: 0.45);
var actual = Assert.IsType<TrajectoryResult>(runtime.LastExecutedResult);
Assert.Equal(expected.Result.Duration.TotalSeconds, actual.Duration.TotalSeconds, precision: 6);
Assert.Equal(expected.Result.DenseJointTrajectory!.Count, actual.DenseJointTrajectory!.Count);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点,并等待该段运动完成后再启动飞拍轨迹。
/// </summary>
@@ -706,6 +833,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -750,6 +878,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -793,6 +922,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -960,7 +1090,7 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位,并按执行侧稠密轨迹时长应用当前 speed_ratio。
/// 验证 SaveTrajectoryInfo 会导出按 J519 8ms 实发周期重采样的点位,并保留当前 speed_ratio 诊断信息
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
@@ -994,17 +1124,21 @@ public sealed class RuntimeOrchestrationTests
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var shotEventsJson = File.ReadAllText(shotEventsPath);
var executionDuration = double.Parse(
var planningExportDuration = double.Parse(
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
CultureInfo.InvariantCulture);
var minimumExpectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1;
var planningExportRowsAtServoPeriod = (int)Math.Ceiling(Math.Max(0.0, (planningExportDuration / 0.008) - 1e-9)) + 1;
Assert.Equal(pointRows.Length, timingRows.Length);
Assert.True(pointRows.Length >= minimumExpectedRows, $"最终发送点数应不少于请求倍率的首轮候选值actual={pointRows.Length}, min={minimumExpectedRows}");
Assert.True(
pointRows.Length > planningExportRowsAtServoPeriod,
$"ActualSend 应按当前 speedRatio 的执行视图导出,不能再用旧 JointDetialTraj 规划时长约束点数。actual={pointRows.Length}, planningRows={planningExportRowsAtServoPeriod}");
Assert.Equal(0.0, pointRows[0][0], precision: 6);
Assert.Equal(0.008, pointRows[1][0], precision: 6);
Assert.True(timingRows[1][2] <= 0.004 + 1e-6, $"自动拉长后 trajectory_time 推进不应快于请求倍率actual={timingRows[1][2]:F6}");
Assert.True(timingRows[1][3] <= 0.5 + 1e-6, $"最终采用倍率不应快于请求倍率actual={timingRows[1][3]:F6}");
Assert.Equal(0.008, timingRows[1][2], precision: 6);
Assert.Equal(1.0, timingRows[1][3], precision: 6);
Assert.Equal(0.5, timingRows[1][4], precision: 6);
Assert.Equal(0.0, timingRows[1][5], precision: 6);
Assert.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson);
Assert.Contains("\"selected_sample_index\"", shotEventsJson);
}
@@ -1015,11 +1149,56 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图,
/// 而不是再次从 PlannedTrajectory 独立重采样。
/// 验证 SaveTrajectoryInfo 的旧格式规划导出只受 planning_speed_scale 影响,不跟随运行时 speed_ratio 改变。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample()
public void ControllerClientCompatService_SaveTrajectoryInfo_KeepsLegacyPlanningExportsIndependentFromSpeedRatio()
{
var fullSpeedRoot = CreateTempConfigRoot();
var halfSpeedRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(fullSpeedRoot, planningSpeedScale: 0.45, speedRatio: 1.0);
WriteRobotConfigWithDemoTrajectory(halfSpeedRoot, planningSpeedScale: 0.45, speedRatio: 0.5);
SaveDemoTrajectoryInfo(fullSpeedRoot);
SaveDemoTrajectoryInfo(halfSpeedRoot);
var fullSpeedOutput = Path.Combine(fullSpeedRoot, "Data", "demo-flyshot");
var halfSpeedOutput = Path.Combine(halfSpeedRoot, "Data", "demo-flyshot");
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "JointTraj.txt"),
Path.Combine(halfSpeedOutput, "JointTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "JointDetialTraj.txt"),
Path.Combine(halfSpeedOutput, "JointDetialTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "CartTraj.txt"),
Path.Combine(halfSpeedOutput, "CartTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "CartDetialTraj.txt"),
Path.Combine(halfSpeedOutput, "CartDetialTraj.txt"));
var fullSpeedActualSend = File.ReadAllLines(Path.Combine(fullSpeedOutput, "ActualSendJointTraj.txt"));
var halfSpeedActualSend = File.ReadAllLines(Path.Combine(halfSpeedOutput, "ActualSendJointTraj.txt"));
var fullSpeedTiming = File.ReadAllLines(Path.Combine(fullSpeedOutput, "ActualSendTiming.txt"));
var halfSpeedTiming = File.ReadAllLines(Path.Combine(halfSpeedOutput, "ActualSendTiming.txt"));
Assert.NotEqual(fullSpeedActualSend.Length, halfSpeedActualSend.Length);
Assert.NotEqual(fullSpeedTiming.Length, halfSpeedTiming.Length);
}
finally
{
Directory.Delete(fullSpeedRoot, recursive: true);
Directory.Delete(halfSpeedRoot, recursive: true);
}
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自旧规划导出视图的 16ms 明细轨迹,
/// 而不是来自 speed_ratio 后的实发执行视图。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExportPlanningDenseDownsample()
{
var fixture = LoadUttcMs11RuntimeFixture();
var configRoot = CreateTempConfigRoot();
@@ -1028,20 +1207,26 @@ public sealed class RuntimeOrchestrationTests
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader());
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
var exportBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var executionBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0,
speedRatio: 0.5);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, exportBundle, executionBundle, speedRatio: 0.5);
var outputDir = Path.Combine(configRoot, "Data", "UTTC_MS11");
var exportedRows = File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt"))
.Select(ParseSpaceSeparatedDoubles)
.ToArray();
var expectedRows = DownsampleDenseRows(
bundle.Result.DenseJointTrajectory!,
exportBundle.Result.DenseJointTrajectory!,
samplePeriodSeconds: 0.016)
.Select(static row => row.ToArray())
.ToArray();
@@ -1085,16 +1270,21 @@ public sealed class RuntimeOrchestrationTests
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json供兼容服务从统一配置恢复轨迹。
/// </summary>
/// <param name="configRoot">测试运行配置根。</param>
private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
private static void WriteRobotConfigWithDemoTrajectory(
string configRoot,
double planningSpeedScale = 0.45,
double speedRatio = 1.0)
{
File.WriteAllText(
Path.Combine(configRoot, "RobotConfig.json"),
"""
$$"""
{
"robot": {
"use_do": true,
"io_addr": [7, 8],
"io_keep_cycles": 2,
"speed_ratio": {{speedRatio.ToString(CultureInfo.InvariantCulture)}},
"planning_speed_scale": {{planningSpeedScale.ToString(CultureInfo.InvariantCulture)}},
"acc_limit": 1.0,
"jerk_limit": 1.0,
"adapt_icsp_try_num": 5
@@ -1103,9 +1293,9 @@ public sealed class RuntimeOrchestrationTests
"demo-flyshot": {
"traj_waypoints": [
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
],
"shot_flags": [false, true, false, false],
"offset_values": [0, 1, 0, 0],
@@ -1116,6 +1306,42 @@ public sealed class RuntimeOrchestrationTests
""");
}
/// <summary>
/// 使用指定运行配置根执行 demo-flyshot 的 SaveTrajectoryInfo。
/// </summary>
private static void SaveDemoTrajectoryInfo(string configRoot)
{
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
new RecordingControllerRuntime(),
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SaveTrajectoryInfo("demo-flyshot");
}
/// <summary>
/// 按数值逐行比较两个轨迹文本文件,避免纯文本格式差异影响判断。
/// </summary>
private static void AssertNumericFilesEqual(string expectedPath, string actualPath)
{
var expectedRows = File.ReadAllLines(expectedPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var actualRows = File.ReadAllLines(actualPath).Select(ParseSpaceSeparatedDoubles).ToArray();
Assert.Equal(expectedRows.Length, actualRows.Length);
for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++)
{
Assert.Equal(expectedRows[rowIndex].Length, actualRows[rowIndex].Length);
for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++)
{
Assert.Equal(expectedRows[rowIndex][columnIndex], actualRows[rowIndex][columnIndex], precision: 6);
}
}
}
/// <summary>
/// 角度转弧度,供轨迹整形测试构造输入。
/// </summary>
@@ -1183,6 +1409,43 @@ public sealed class RuntimeOrchestrationTests
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
speedRatio: settings.SpeedRatio,
smoothStartStopTiming: true);
}
/// <summary>
/// 为 legacy-fit 对比测试显式关闭二次时间重映射。
/// </summary>
private static CompatibilityRobotSettings DisableSmoothStartStopTiming(CompatibilityRobotSettings settings)
{
return new CompatibilityRobotSettings(
useDo: settings.UseDo,
ioAddresses: settings.IoAddresses,
ioKeepCycles: settings.IoKeepCycles,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles,
accLimitScale: settings.AccLimitScale,
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
speedRatio: settings.SpeedRatio,
smoothStartStopTiming: false);
}
/// <summary>
/// 构造飞拍倍率测试使用的最小兼容设置。
/// </summary>
private static CompatibilityRobotSettings CreateFlyshotSettings(double planningSpeedScale, double speedRatio)
{
return new CompatibilityRobotSettings(
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 2,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5,
planningSpeedScale: planningSpeedScale,
speedRatio: speedRatio,
smoothStartStopTiming: true);
}
@@ -1555,9 +1818,9 @@ internal static class TestRobotFactory
waypoints:
[
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
],
shotFlags: [false, true, false, false],
offsetValues: [0, 1, 0, 0],