✨ feat(fanuc): 统一 speedRatio 执行倍率语义
* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
This commit is contained in:
@@ -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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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],
|
||||
|
||||
Reference in New Issue
Block a user