✨ feat(*): 完善 FANUC J519 闭环、MoveJoint 与现场抓包验证
* 划分 J519 发送循环与稠密轨迹循环职责边界， FanucJ519Client 负责 UDP 周期发送， FanucControllerRuntime 按轨迹时间更新下一帧命令 * 执行时将规划输出 rad 转为 J519 deg 目标， 并按 speed_ratio 调整 8ms 发送时间尺度 * 补齐 accept_cmd/received_cmd/sysrdy/rbt_inmotion 状态位解析与启动前闭环检查 * MoveJoint 改为关节空间直线 + smoothstep 进度 的临时 PTP 稠密轨迹，按 status=15 运动窗口复现 * 新增 UTTC 2026-04-28 三份抓包 golden tests， 覆盖 0.5/0.7/1.0 speed_ratio 下的 J519 命令、 IO 脉冲与响应滞后 * 状态通道补充超时重连策略与退避逻辑 * TCP 10012 命令响应统一检查 result_code * 状态页扩展 J519 状态位与快照诊断信息 * 新增 docs/fanuc-field-runtime-workflow.md 现场工作流 * 补充 LR Mate 200iD 模型、RobotConfig.json 与 workpiece
This commit is contained in:
@@ -1,5 +1,9 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using System.Reflection;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
@@ -8,6 +12,302 @@ namespace Flyshot.Core.Tests;
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntimeDenseTests
|
||||
{
|
||||
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
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.5);
|
||||
|
||||
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 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(5, 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]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(1.0)]
|
||||
[InlineData(0.7)]
|
||||
[InlineData(0.5)]
|
||||
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var service = CreateCompatService(runtime);
|
||||
var startJoints = new[]
|
||||
{
|
||||
1.056731,
|
||||
0.011664811,
|
||||
-0.017892333,
|
||||
-0.01516874,
|
||||
0.021492079,
|
||||
0.009567846
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
0.8532358,
|
||||
0.03837953,
|
||||
-0.19235304,
|
||||
0.0071595116,
|
||||
0.109054826,
|
||||
0.040055145
|
||||
};
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio);
|
||||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||||
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
WorkspaceRoot = TestRobotFactory.GetWorkspaceRoot()
|
||||
};
|
||||
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
|
||||
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
|
||||
var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio);
|
||||
|
||||
service.MoveJoint(targetJoints);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
|
||||
AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
|
||||
AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);
|
||||
|
||||
var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
|
||||
Assert.InRange(middleAlpha, 0.45, 0.55);
|
||||
|
||||
var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
|
||||
Assert.InRange(earlyAlpha, 0.0, 0.02);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
|
||||
var targetJoints = new[] { 0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145 };
|
||||
|
||||
var fullSpeed = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var speed07 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var speed05 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.5);
|
||||
|
||||
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
||||
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
||||
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
|
||||
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
|
||||
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var service = CreateCompatService(runtime);
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||||
|
||||
service.MoveJoint(targetJoints);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var currentCommand = j519Client.GetCurrentCommand();
|
||||
Assert.NotNull(currentCommand);
|
||||
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
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 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机模式下若 J519 响应明确显示伺服侧未就绪，则拒绝启动稠密轨迹发送。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsNotReadyJ519Status()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetLatestJ519Response(j519Client, status: 0b0011);
|
||||
|
||||
var result = CreateDenseResult(
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
],
|
||||
durationSeconds: 0.008);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => runtime.ExecuteTrajectory(result, [Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
|
||||
Assert.Contains("J519 status is not ready", exception.Message);
|
||||
Assert.Contains("sysrdy=False", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证控制器快照暴露最近一次 J519 响应中的四个状态位，便于状态页和诊断接口显示。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void GetSnapshot_RealMode_IncludesLatestJ519StatusBits()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetLatestJ519Response(j519Client, status: 0b0111);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
|
||||
Assert.Equal((byte)0b0111, snapshot.J519Status);
|
||||
Assert.Equal(10u, snapshot.J519Sequence);
|
||||
Assert.True(snapshot.J519AcceptsCommand);
|
||||
Assert.True(snapshot.J519ReceivedCommand);
|
||||
Assert.True(snapshot.J519SystemReady);
|
||||
Assert.False(snapshot.J519RobotInMotion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍 IO 脉冲按轨迹时间轴嵌入 J519 命令，并在保持周期后用同一 mask 清零。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_EmbedsIoPulseOnTrajectoryTimeline()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.024),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline:
|
||||
[
|
||||
new TrajectoryDoEvent(
|
||||
waypointIndex: 1,
|
||||
triggerTime: 0.008,
|
||||
offsetCycles: 0,
|
||||
holdCycles: 2,
|
||||
addressGroup: new IoAddressGroup([1, 3]))
|
||||
],
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(4, commands.Count);
|
||||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)10], commands.Select(static command => command.WriteIoMask));
|
||||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)0], commands.Select(static command => command.WriteIoValue));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。
|
||||
/// </summary>
|
||||
@@ -93,4 +393,287 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
var actual = FanucControllerRuntime.ComputeIoValue(group);
|
||||
Assert.Equal((ushort)(1 | 128), actual);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[]
|
||||
{
|
||||
DegreesToRadians(60.546227),
|
||||
DegreesToRadians(0.668344),
|
||||
DegreesToRadians(-1.025155),
|
||||
DegreesToRadians(-0.869105),
|
||||
DegreesToRadians(1.231405),
|
||||
DegreesToRadians(0.548197)
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
DegreesToRadians(48.886810),
|
||||
DegreesToRadians(2.198985),
|
||||
DegreesToRadians(-11.021017),
|
||||
DegreesToRadians(0.410210),
|
||||
DegreesToRadians(6.248381),
|
||||
DegreesToRadians(2.294991)
|
||||
};
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.Equal(41, rows.Count);
|
||||
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
|
||||
|
||||
var expectedAlpha = new[]
|
||||
{
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
};
|
||||
|
||||
for (var index = 0; index < rows.Count; index++)
|
||||
{
|
||||
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
|
||||
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
|
||||
}
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_RejectsUnrepresentableSampleCountForTinySpeedRatio()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1e-12));
|
||||
|
||||
Assert.Contains("sample count", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_StretchesLongMoveFromJointLimitsInsteadOfKeepingFortyCycles()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||||
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||||
var expectedMinimumDuration = new[]
|
||||
{
|
||||
0.320,
|
||||
expectedVelocityDuration,
|
||||
expectedAccelerationDuration,
|
||||
expectedJerkDuration
|
||||
}.Max();
|
||||
var expectedCountFromDuration = (int)Math.Floor(result.Duration.TotalSeconds / robot.ServoPeriod.TotalSeconds + 1e-9) + 1;
|
||||
|
||||
Assert.True(rows.Count > 41, $"Expected long MoveJoint to produce more than 41 points, got {rows.Count}.");
|
||||
Assert.True(
|
||||
result.Duration.TotalSeconds >= expectedMinimumDuration,
|
||||
$"Expected duration >= {expectedMinimumDuration:F6}s from joint limits, got {result.Duration.TotalSeconds:F6}s.");
|
||||
Assert.Equal(expectedCountFromDuration, rows.Count);
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
|
||||
private static void ForceRealModeEnabled(FanucControllerRuntime runtime, double speedRatio)
|
||||
{
|
||||
SetPrivateField(runtime, "_activeControllerIsSimulation", false);
|
||||
SetPrivateField(runtime, "_connectedRobotIp", "127.0.0.1");
|
||||
SetPrivateField(runtime, "_isEnabled", true);
|
||||
SetPrivateField(runtime, "_bufferSize", 2);
|
||||
SetPrivateField(runtime, "_speedRatio", speedRatio);
|
||||
}
|
||||
|
||||
private static ControllerClientCompatService CreateCompatService(FanucControllerRuntime runtime)
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
WorkspaceRoot = TestRobotFactory.GetWorkspaceRoot()
|
||||
};
|
||||
|
||||
return new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader(),
|
||||
new InMemoryFlyshotTrajectoryStore());
|
||||
}
|
||||
|
||||
private static double ComputeLineAlpha(
|
||||
IReadOnlyList<double> actualDegrees,
|
||||
IReadOnlyList<double> startRadians,
|
||||
IReadOnlyList<double> targetRadians)
|
||||
{
|
||||
var numerator = 0.0;
|
||||
var denominator = 0.0;
|
||||
for (var index = 0; index < startRadians.Count; index++)
|
||||
{
|
||||
var startDegrees = RadiansToDegrees(startRadians[index]);
|
||||
var deltaDegrees = RadiansToDegrees(targetRadians[index]) - startDegrees;
|
||||
numerator += (actualDegrees[index] - startDegrees) * deltaDegrees;
|
||||
denominator += deltaDegrees * deltaDegrees;
|
||||
}
|
||||
|
||||
return denominator <= 0.0 ? 0.0 : numerator / denominator;
|
||||
}
|
||||
|
||||
private static void AssertJointDegreesEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualDegrees)
|
||||
{
|
||||
Assert.Equal(expectedRadians.Count, actualDegrees.Count);
|
||||
for (var index = 0; index < expectedRadians.Count; index++)
|
||||
{
|
||||
Assert.Equal(RadiansToDegrees(expectedRadians[index]), actualDegrees[index], precision: 4);
|
||||
}
|
||||
}
|
||||
|
||||
private static RobotProfile CreateMoveJointReferenceRobotProfile()
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: "FANUC_LR_Mate_200iD",
|
||||
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||||
degreesOfFreedom: 6,
|
||||
jointLimits:
|
||||
[
|
||||
new JointLimit("J1", 7.85, 32.72, 272.7),
|
||||
new JointLimit("J2", 6.63, 27.63, 230.28),
|
||||
new JointLimit("J3", 9.07, 37.81, 315.12),
|
||||
new JointLimit("J4", 9.59, 39.99, 333.3),
|
||||
new JointLimit("J5", 9.51, 39.63, 330.27),
|
||||
new JointLimit("J6", 17.45, 72.72, 606.01)
|
||||
],
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
private static double DegreesToRadians(double degrees)
|
||||
{
|
||||
return degrees * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
private static void WaitUntilIdle(FanucControllerRuntime runtime)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
if (!runtime.GetSnapshot().IsInMotion)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Thread.Sleep(1);
|
||||
}
|
||||
|
||||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||||
}
|
||||
|
||||
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
||||
{
|
||||
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
Assert.NotNull(field);
|
||||
field.SetValue(runtime, value);
|
||||
}
|
||||
|
||||
private static TrajectoryResult CreateDenseResult(IReadOnlyList<IReadOnlyList<double>> denseTrajectory, double durationSeconds)
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
}
|
||||
|
||||
private static void SetLatestJ519Response(FanucJ519Client client, byte status)
|
||||
{
|
||||
var response = new FanucJ519Response(
|
||||
messageType: 0,
|
||||
version: 1,
|
||||
sequence: 10,
|
||||
status: status,
|
||||
readIoType: 2,
|
||||
readIoIndex: 1,
|
||||
readIoMask: 255,
|
||||
readIoValue: 0,
|
||||
timestamp: 1234,
|
||||
pose: new double[6],
|
||||
externalAxes: new double[3],
|
||||
jointDegrees: new double[9],
|
||||
motorCurrents: new double[9]);
|
||||
var field = typeof(FanucJ519Client).GetField("_latestResponse", BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
Assert.NotNull(field);
|
||||
field.SetValue(client, response);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -72,7 +72,7 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
// 接收至少一个命令包。
|
||||
var commandResult = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
@@ -156,18 +156,86 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
client.StartMotion();
|
||||
|
||||
var result1 = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
client.UpdateCommand(command2);
|
||||
|
||||
var result2 = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证重复保持同一命令时实际 J519 包序号仍按客户端全局递增。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_IncrementsSequenceForRepeatedHoldPackets()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await _server.ReceiveAsync(_cts.Token); // init
|
||||
|
||||
var command = new FanucJ519Command(sequence: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
var packets = new List<byte[]>();
|
||||
for (var index = 0; index < 4; index++)
|
||||
{
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
packets.Add(result.Buffer);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
var sequences = packets
|
||||
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
|
||||
.ToArray();
|
||||
Assert.Equal([0u, 1u, 2u, 3u], sequences);
|
||||
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证停止运动后可在同一连接内重启发送，且包序号不重置。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_CanRestartAfterStopMotionWithoutResettingSequence()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await _server.ReceiveAsync(_cts.Token); // init
|
||||
|
||||
client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
client.StartMotion();
|
||||
|
||||
var first = await _server.ReceiveAsync(_cts.Token);
|
||||
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
|
||||
Assert.Equal(0u, firstSequence);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
byte[] packet;
|
||||
do
|
||||
{
|
||||
packet = (await _server.ReceiveAsync(_cts.Token)).Buffer;
|
||||
}
|
||||
while (packet.Length != FanucJ519Protocol.ControlPacketLength);
|
||||
|
||||
client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
client.StartMotion();
|
||||
|
||||
var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
||||
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
|
||||
Assert.True(BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)) > firstSequence);
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
|
||||
/// </summary>
|
||||
@@ -179,10 +247,10 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。
|
||||
/// 验证发送循环能持续按协议周期输出命令包。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_MaintainsSubMillisecondPeriod()
|
||||
public async Task StartMotion_MaintainsPeriodicCommandStream()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
@@ -192,28 +260,32 @@ public sealed class FanucJ519ClientTests : IDisposable
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
// 收集 5 个命令包到达时间戳。
|
||||
// 收集 5 个命令包到达时间戳和序号。
|
||||
var timestamps = new List<DateTimeOffset>();
|
||||
var sequences = new List<uint>();
|
||||
for (var i = 0; i < 5; i++)
|
||||
{
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||
timestamps.Add(DateTimeOffset.UtcNow);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
// 计算相邻包间隔并断言最大抖动。
|
||||
Assert.Equal([0u, 1u, 2u, 3u, 4u], sequences);
|
||||
|
||||
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
|
||||
var intervals = new List<TimeSpan>();
|
||||
for (var i = 1; i < timestamps.Count; i++)
|
||||
{
|
||||
intervals.Add(timestamps[i] - timestamps[i - 1]);
|
||||
}
|
||||
|
||||
// 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。
|
||||
Assert.All(intervals, interval =>
|
||||
{
|
||||
Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。");
|
||||
Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||
Assert.True(interval > TimeSpan.Zero, $"间隔 {interval.TotalMilliseconds:F2}ms 必须为正。");
|
||||
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs
Normal file
526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs
Normal file
@@ -0,0 +1,526 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Globalization;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 使用 2026-04-28 UTTC 真实抓包验证 J519 主运行点位与 JointDetialTraj 重采样规则一致。
|
||||
/// </summary>
|
||||
public sealed class UttcJ519GoldenTests
|
||||
{
|
||||
private const int JointCount = 6;
|
||||
private const int RobotJ519Port = 60015;
|
||||
private const double ServoPeriodSeconds = 0.008;
|
||||
|
||||
public static IEnumerable<object[]> SpeedSweepCases()
|
||||
{
|
||||
yield return ["2026042802-0.5.pcap", 0.5, 1851, 14.800309];
|
||||
yield return ["2026042802-0.7.pcap", 0.7, 1322, 10.568313];
|
||||
yield return ["2026042802-1.pcap", 1.0, 926, 7.400125];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed=0.5/0.7/1.0 三份真实抓包都符合当前运行时采用的发送点生成规则。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[MemberData(nameof(SpeedSweepCases))]
|
||||
public void CapturedJ519Run_ReplaysJointDetailTrajectoryWithSpeedRatio(
|
||||
string pcapFileName,
|
||||
double speedRatio,
|
||||
int expectedPointCount,
|
||||
double expectedSendDurationSeconds)
|
||||
{
|
||||
var repositoryRoot = FindRepositoryRoot();
|
||||
var pcapPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", pcapFileName);
|
||||
var jointDetailPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
|
||||
|
||||
var packets = ParsePcapUdp(pcapPath);
|
||||
var hostPort = DetectHostJ519Port(packets);
|
||||
var commands = ParseJ519Commands(packets, hostPort);
|
||||
var responses = ParseJ519Responses(packets, hostPort);
|
||||
var responseSegment = LongestStatusSegment(responses, status: 15);
|
||||
var jointRows = ReadJointDetail(jointDetailPath);
|
||||
var expected = GenerateExpectedPoints(jointRows, speedRatio);
|
||||
|
||||
var commandBySequence = new Dictionary<uint, CapturedJ519Command>();
|
||||
var duplicateSequenceCount = 0;
|
||||
foreach (var command in commands)
|
||||
{
|
||||
if (!commandBySequence.TryAdd(command.Sequence, command))
|
||||
{
|
||||
duplicateSequenceCount++;
|
||||
}
|
||||
}
|
||||
|
||||
var (startSequence, windowRms) = FindBestCommandWindow(commandBySequence, expected, responseSegment, searchRadius: 32);
|
||||
var actualRun = Enumerable
|
||||
.Range(0, expected.Length)
|
||||
.Select(index => commandBySequence[startSequence + (uint)index])
|
||||
.ToArray();
|
||||
var comparison = Compare(actualRun, expected);
|
||||
|
||||
Assert.Equal(464, jointRows.Length);
|
||||
Assert.Equal(expectedPointCount, expected.Length);
|
||||
Assert.Equal(expectedPointCount, actualRun.Length);
|
||||
Assert.Equal(0, duplicateSequenceCount);
|
||||
Assert.Equal(17, comparison.IoSetPulses);
|
||||
Assert.Equal(17, comparison.IoClearFrames);
|
||||
Assert.Equal(
|
||||
new ushort[] { 10, 12, 14 },
|
||||
actualRun
|
||||
.Where(static command => command.WriteIoMask != 0)
|
||||
.Select(static command => command.WriteIoMask)
|
||||
.Distinct()
|
||||
.Order()
|
||||
.ToArray());
|
||||
Assert.True(responseSegment.Length >= expectedPointCount - 1, $"status=15 segment too short: {responseSegment.Length}");
|
||||
Assert.InRange((long)responseSegment[0].Sequence - startSequence, 2, 8);
|
||||
Assert.All(actualRun, static command => Assert.Equal(0, command.LastData));
|
||||
for (var index = 0; index < actualRun.Length; index++)
|
||||
{
|
||||
Assert.Equal(startSequence + (uint)index, actualRun[index].Sequence);
|
||||
}
|
||||
|
||||
Assert.True(windowRms < 0.012, $"J519 global RMS {windowRms:F9} deg exceeds tolerance.");
|
||||
Assert.True(comparison.GlobalMaxAbsDeg < 0.07, $"J519 max abs diff {comparison.GlobalMaxAbsDeg:F9} deg exceeds tolerance.");
|
||||
|
||||
var sendDuration = actualRun[^1].TimeRelativeSeconds - actualRun[0].TimeRelativeSeconds;
|
||||
Assert.InRange(sendDuration, expectedSendDurationSeconds - 0.04, expectedSendDurationSeconds + 0.04);
|
||||
}
|
||||
|
||||
private static string FindRepositoryRoot()
|
||||
{
|
||||
var directory = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (directory is not null)
|
||||
{
|
||||
var jointDetailPath = Path.Combine(directory.FullName, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
|
||||
if (File.Exists(jointDetailPath))
|
||||
{
|
||||
return directory.FullName;
|
||||
}
|
||||
|
||||
directory = directory.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Cannot locate repository root containing Rvbust/uttc-20260428/Data/JointDetialTraj.txt.");
|
||||
}
|
||||
|
||||
private static IReadOnlyList<UdpPacket> ParsePcapUdp(string path)
|
||||
{
|
||||
using var stream = File.OpenRead(path);
|
||||
var header = new byte[24];
|
||||
stream.ReadExactly(header);
|
||||
|
||||
var magic = header.AsSpan(0, 4);
|
||||
var bigEndian = false;
|
||||
var timestampScale = 1_000_000.0;
|
||||
if (magic.SequenceEqual(new byte[] { 0xd4, 0xc3, 0xb2, 0xa1 }))
|
||||
{
|
||||
bigEndian = false;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0xc3, 0xd4 }))
|
||||
{
|
||||
bigEndian = true;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0x4d, 0x3c, 0xb2, 0xa1 }))
|
||||
{
|
||||
bigEndian = false;
|
||||
timestampScale = 1_000_000_000.0;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0x3c, 0x4d }))
|
||||
{
|
||||
bigEndian = true;
|
||||
timestampScale = 1_000_000_000.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidDataException($"Unsupported pcap magic: {Convert.ToHexString(header.AsSpan(0, 4))}");
|
||||
}
|
||||
|
||||
var linkType = ReadUInt32(header.AsSpan(20, 4), bigEndian);
|
||||
if (linkType != 1)
|
||||
{
|
||||
throw new InvalidDataException($"Only Ethernet pcap is supported, got linktype {linkType}.");
|
||||
}
|
||||
|
||||
var packets = new List<UdpPacket>();
|
||||
var recordHeader = new byte[16];
|
||||
double? firstTimestamp = null;
|
||||
var frameNumber = 0;
|
||||
while (ReadFullOrEnd(stream, recordHeader))
|
||||
{
|
||||
frameNumber++;
|
||||
var tsSec = ReadUInt32(recordHeader.AsSpan(0, 4), bigEndian);
|
||||
var tsFraction = ReadUInt32(recordHeader.AsSpan(4, 4), bigEndian);
|
||||
var includedLength = ReadUInt32(recordHeader.AsSpan(8, 4), bigEndian);
|
||||
var packet = new byte[includedLength];
|
||||
stream.ReadExactly(packet);
|
||||
|
||||
var timestamp = tsSec + (tsFraction / timestampScale);
|
||||
firstTimestamp ??= timestamp;
|
||||
var udp = ParseEthernetIpv4Udp(packet, frameNumber, timestamp - firstTimestamp.Value);
|
||||
if (udp is not null)
|
||||
{
|
||||
packets.Add(udp);
|
||||
}
|
||||
}
|
||||
|
||||
return packets;
|
||||
}
|
||||
|
||||
private static bool ReadFullOrEnd(Stream stream, byte[] buffer)
|
||||
{
|
||||
var offset = 0;
|
||||
while (offset < buffer.Length)
|
||||
{
|
||||
var read = stream.Read(buffer, offset, buffer.Length - offset);
|
||||
if (read == 0)
|
||||
{
|
||||
if (offset == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
throw new EndOfStreamException("Truncated pcap record header.");
|
||||
}
|
||||
|
||||
offset += read;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static UdpPacket? ParseEthernetIpv4Udp(byte[] packet, int frameNumber, double timeRelativeSeconds)
|
||||
{
|
||||
if (packet.Length < 14)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var offset = 14;
|
||||
var etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(12, 2));
|
||||
if (etherType == 0x8100)
|
||||
{
|
||||
if (packet.Length < 18)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(16, 2));
|
||||
offset = 18;
|
||||
}
|
||||
|
||||
if (etherType != 0x0800 || packet.Length < offset + 20)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var versionIhl = packet[offset];
|
||||
var version = versionIhl >> 4;
|
||||
var ihl = (versionIhl & 0x0f) * 4;
|
||||
if (version != 4 || ihl < 20 || packet.Length < offset + ihl)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var protocol = packet[offset + 9];
|
||||
if (protocol != 17)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var totalLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(offset + 2, 2));
|
||||
var udpOffset = offset + ihl;
|
||||
if (packet.Length < udpOffset + 8)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var sourcePort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset, 2));
|
||||
var destinationPort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 2, 2));
|
||||
var udpLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 4, 2));
|
||||
var payloadOffset = udpOffset + 8;
|
||||
var payloadLength = Math.Max(0, Math.Min(udpLength - 8, totalLength - ihl - 8));
|
||||
if (packet.Length < payloadOffset + payloadLength)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return new UdpPacket(
|
||||
frameNumber,
|
||||
timeRelativeSeconds,
|
||||
sourcePort,
|
||||
destinationPort,
|
||||
packet.AsSpan(payloadOffset, payloadLength).ToArray());
|
||||
}
|
||||
|
||||
private static ushort DetectHostJ519Port(IEnumerable<UdpPacket> packets)
|
||||
{
|
||||
return packets
|
||||
.Where(static packet => packet.DestinationPort == RobotJ519Port && packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
|
||||
.GroupBy(static packet => packet.SourcePort)
|
||||
.OrderByDescending(static group => group.Count())
|
||||
.Select(static group => group.Key)
|
||||
.First();
|
||||
}
|
||||
|
||||
private static CapturedJ519Command[] ParseJ519Commands(IEnumerable<UdpPacket> packets, ushort hostPort)
|
||||
{
|
||||
return packets
|
||||
.Where(packet =>
|
||||
packet.SourcePort == hostPort
|
||||
&& packet.DestinationPort == RobotJ519Port
|
||||
&& packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
|
||||
.Select(ParseCommand)
|
||||
.Where(static command => command is not null)
|
||||
.Cast<CapturedJ519Command>()
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static CapturedJ519Command? ParseCommand(UdpPacket packet)
|
||||
{
|
||||
var payload = packet.Payload;
|
||||
var messageType = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x00, 4));
|
||||
var version = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x04, 4));
|
||||
if (messageType != 1 || version != 1)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var targets = new double[9];
|
||||
for (var index = 0; index < targets.Length; index++)
|
||||
{
|
||||
targets[index] = BinaryPrimitives.ReadSingleBigEndian(payload.AsSpan(0x1c + (index * 4), 4));
|
||||
}
|
||||
|
||||
return new CapturedJ519Command(
|
||||
packet.FrameNumber,
|
||||
packet.TimeRelativeSeconds,
|
||||
BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x08, 4)),
|
||||
payload[0x0c],
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x16, 2)),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x18, 2)),
|
||||
targets);
|
||||
}
|
||||
|
||||
private static FanucJ519Response[] ParseJ519Responses(IEnumerable<UdpPacket> packets, ushort hostPort)
|
||||
{
|
||||
return packets
|
||||
.Where(packet =>
|
||||
packet.SourcePort == RobotJ519Port
|
||||
&& packet.DestinationPort == hostPort
|
||||
&& packet.Payload.Length == FanucJ519Protocol.ResponsePacketLength)
|
||||
.Select(packet => FanucJ519Protocol.ParseResponse(packet.Payload))
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static FanucJ519Response[] LongestStatusSegment(IEnumerable<FanucJ519Response> responses, byte status)
|
||||
{
|
||||
var best = new List<FanucJ519Response>();
|
||||
var current = new List<FanucJ519Response>();
|
||||
foreach (var response in responses)
|
||||
{
|
||||
if (response.Status == status)
|
||||
{
|
||||
current.Add(response);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (current.Count > best.Count)
|
||||
{
|
||||
best = current;
|
||||
}
|
||||
|
||||
current = [];
|
||||
}
|
||||
|
||||
return (current.Count > best.Count ? current : best).ToArray();
|
||||
}
|
||||
|
||||
private static JointRow[] ReadJointDetail(string path)
|
||||
{
|
||||
return File.ReadLines(path)
|
||||
.Where(static line => !string.IsNullOrWhiteSpace(line) && !line.TrimStart().StartsWith('#'))
|
||||
.Select(static line =>
|
||||
{
|
||||
var values = line
|
||||
.Split((char[]?)null, StringSplitOptions.RemoveEmptyEntries)
|
||||
.Select(value => double.Parse(value, CultureInfo.InvariantCulture))
|
||||
.ToArray();
|
||||
return new JointRow(values[0], values.Skip(1).Take(JointCount).ToArray());
|
||||
})
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static ExpectedPoint[] GenerateExpectedPoints(IReadOnlyList<JointRow> rows, double speedRatio)
|
||||
{
|
||||
var durationSeconds = rows[^1].TimeSeconds;
|
||||
var trajectoryStepSeconds = ServoPeriodSeconds * speedRatio;
|
||||
var sampleCount = (int)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
|
||||
var points = new ExpectedPoint[sampleCount];
|
||||
var segmentIndex = 0;
|
||||
|
||||
for (var index = 0; index < sampleCount; index++)
|
||||
{
|
||||
var trajectoryTime = Math.Min(index * trajectoryStepSeconds, durationSeconds);
|
||||
var jointsRad = Interpolate(rows, trajectoryTime, ref segmentIndex);
|
||||
points[index] = new ExpectedPoint(
|
||||
index,
|
||||
trajectoryTime,
|
||||
jointsRad.Select(static value => value * 180.0 / Math.PI).ToArray());
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
private static double[] Interpolate(IReadOnlyList<JointRow> rows, double trajectoryTime, ref int segmentIndex)
|
||||
{
|
||||
if (rows.Count == 1 || trajectoryTime <= rows[0].TimeSeconds)
|
||||
{
|
||||
return rows[0].JointsRad.ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = rows.Count - 1;
|
||||
if (trajectoryTime >= rows[lastIndex].TimeSeconds)
|
||||
{
|
||||
return rows[lastIndex].JointsRad.ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && rows[segmentIndex + 1].TimeSeconds < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = rows[segmentIndex];
|
||||
var end = rows[segmentIndex + 1];
|
||||
var duration = end.TimeSeconds - start.TimeSeconds;
|
||||
var alpha = duration <= 0.0 ? 0.0 : (trajectoryTime - start.TimeSeconds) / duration;
|
||||
var joints = new double[JointCount];
|
||||
for (var index = 0; index < joints.Length; index++)
|
||||
{
|
||||
joints[index] = start.JointsRad[index] + ((end.JointsRad[index] - start.JointsRad[index]) * alpha);
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
private static (uint StartSequence, double RmsDeg) FindBestCommandWindow(
|
||||
IReadOnlyDictionary<uint, CapturedJ519Command> commandBySequence,
|
||||
IReadOnlyList<ExpectedPoint> expected,
|
||||
IReadOnlyList<FanucJ519Response> responseSegment,
|
||||
int searchRadius)
|
||||
{
|
||||
if (responseSegment.Count == 0)
|
||||
{
|
||||
throw new InvalidDataException("No status=15 response segment found.");
|
||||
}
|
||||
|
||||
var responseStartSequence = (long)responseSegment[0].Sequence;
|
||||
uint? bestStartSequence = null;
|
||||
var bestRms = double.PositiveInfinity;
|
||||
|
||||
for (var startSequence = responseStartSequence - searchRadius; startSequence <= responseStartSequence + searchRadius; startSequence++)
|
||||
{
|
||||
if (startSequence < 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var differences = new List<double>(expected.Count * JointCount);
|
||||
var completeWindow = true;
|
||||
for (var index = 0; index < expected.Count; index++)
|
||||
{
|
||||
var sequence = (uint)(startSequence + index);
|
||||
if (!commandBySequence.TryGetValue(sequence, out var command))
|
||||
{
|
||||
completeWindow = false;
|
||||
break;
|
||||
}
|
||||
|
||||
for (var joint = 0; joint < JointCount; joint++)
|
||||
{
|
||||
differences.Add(command.TargetDegrees[joint] - expected[index].JointsDeg[joint]);
|
||||
}
|
||||
}
|
||||
|
||||
if (!completeWindow)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var rms = Rms(differences);
|
||||
if (rms < bestRms)
|
||||
{
|
||||
bestRms = rms;
|
||||
bestStartSequence = (uint)startSequence;
|
||||
}
|
||||
}
|
||||
|
||||
if (bestStartSequence is null)
|
||||
{
|
||||
throw new InvalidDataException("No complete command window found near the status=15 response segment.");
|
||||
}
|
||||
|
||||
return (bestStartSequence.Value, bestRms);
|
||||
}
|
||||
|
||||
private static ComparisonSummary Compare(IReadOnlyList<CapturedJ519Command> actual, IReadOnlyList<ExpectedPoint> expected)
|
||||
{
|
||||
var differences = new List<double>(actual.Count * JointCount);
|
||||
var maxAbs = 0.0;
|
||||
for (var index = 0; index < actual.Count; index++)
|
||||
{
|
||||
for (var joint = 0; joint < JointCount; joint++)
|
||||
{
|
||||
var difference = actual[index].TargetDegrees[joint] - expected[index].JointsDeg[joint];
|
||||
differences.Add(difference);
|
||||
maxAbs = Math.Max(maxAbs, Math.Abs(difference));
|
||||
}
|
||||
}
|
||||
|
||||
var ioSetPulses = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue != 0);
|
||||
var ioClearFrames = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue == 0);
|
||||
return new ComparisonSummary(Rms(differences), maxAbs, ioSetPulses, ioClearFrames);
|
||||
}
|
||||
|
||||
private static double Rms(IEnumerable<double> values)
|
||||
{
|
||||
var sum = 0.0;
|
||||
var count = 0;
|
||||
foreach (var value in values)
|
||||
{
|
||||
sum += value * value;
|
||||
count++;
|
||||
}
|
||||
|
||||
return count == 0 ? 0.0 : Math.Sqrt(sum / count);
|
||||
}
|
||||
|
||||
private static uint ReadUInt32(ReadOnlySpan<byte> value, bool bigEndian)
|
||||
{
|
||||
return bigEndian ? BinaryPrimitives.ReadUInt32BigEndian(value) : BinaryPrimitives.ReadUInt32LittleEndian(value);
|
||||
}
|
||||
|
||||
private sealed record UdpPacket(
|
||||
int FrameNumber,
|
||||
double TimeRelativeSeconds,
|
||||
ushort SourcePort,
|
||||
ushort DestinationPort,
|
||||
byte[] Payload);
|
||||
|
||||
private sealed record CapturedJ519Command(
|
||||
int FrameNumber,
|
||||
double TimeRelativeSeconds,
|
||||
uint Sequence,
|
||||
byte LastData,
|
||||
ushort WriteIoMask,
|
||||
ushort WriteIoValue,
|
||||
IReadOnlyList<double> TargetDegrees);
|
||||
|
||||
private sealed record JointRow(double TimeSeconds, IReadOnlyList<double> JointsRad);
|
||||
|
||||
private sealed record ExpectedPoint(int Index, double TrajectoryTimeSeconds, IReadOnlyList<double> JointsDeg);
|
||||
|
||||
private sealed record ComparisonSummary(double GlobalRmsDeg, double GlobalMaxAbsDeg, int IoSetPulses, int IoClearFrames);
|
||||
}
|
||||
Reference in New Issue
Block a user