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

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

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

108
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
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;