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

✨ feat(*): 添加触发样本偏移与实发轨迹分析导出

Browse Source 
* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置
  * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发
  * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出
  * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档
  * 补齐配置兼容、规划绑定和运行时触发相关测试
This commit is contained in:
yunxiao.zhu
2026-05-09 11:12:31 +08:00
parent 1779067b5c
commit f7e2bb0e7b
35 changed files with 5772 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -146,8 +146,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(1.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -164,7 +164,7 @@ 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(2.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
@@ -172,14 +172,17 @@ public sealed class FanucControllerRuntimeDenseTests
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(runDir, "ActualSendTiming.txt");
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
var shotEventsPath = Path.Combine(runDir, "ShotEvents.json");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
Assert.True(File.Exists(jerkPath));
Assert.True(File.Exists(shotEventsPath));
var pointLines = File.ReadAllLines(pointsPath);
var timingLines = File.ReadAllLines(timingPath);
var jerkLines = File.ReadAllLines(jerkPath);
var shotEventsJson = File.ReadAllText(shotEventsPath);
Assert.Equal(commands.Count, pointLines.Length);
Assert.Equal(commands.Count, timingLines.Length);
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
@@ -191,7 +194,7 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal(9, lastColumns.Length);
Assert.Equal(0.0, firstColumns[0], precision: 6);
Assert.Equal(0.008, secondColumns[0], precision: 6);
Assert.Equal(180.0, lastColumns[1], precision: 6);
Assert.Equal(2.0, lastColumns[1], precision: 6);
var firstTimingColumns = ParseColumns(timingLines[0]);
var secondTimingColumns = ParseColumns(timingLines[1]);
@@ -211,6 +214,8 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal(10, firstJerkColumns.Length);
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
Assert.Equal(0.008, firstJerkColumns[2], precision: 6);
Assert.Equal("[]", shotEventsJson.Trim());
}
finally
{
@@ -561,8 +566,63 @@ public sealed class FanucControllerRuntimeDenseTests
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));
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Select(static command => command.WriteIoValue));
}
/// <summary>
/// 验证同一时间窗口内存在多个候选 sample 时，会优先把 IO 挂到关节坐标最接近示教点的那一帧。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_PrefersClosestJointSampleWithinTriggerWindow()
{
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.008, DegreesToRadians(1.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.024, DegreesToRadians(3.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.012,
offsetCycles: 0,
holdCycles: 1,
addressGroup: new IoAddressGroup([1]),
referenceJointsDegrees: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0])
],
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 4,
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [DegreesToRadians(3.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)0, (ushort)1, (ushort)1], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Select(static command => command.WriteIoValue));
}
/// <summary>
@@ -677,13 +737,16 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 IO 地址组中的地址号被正确映射为 writeIoValue 的位掩码。
/// 验证 IO 地址组中的地址号按 writeIoIndex=1 的手册语义映射为 writeIoValue 位掩码。
/// </summary>
[Theory]
[InlineData(new[] { 0 }, (ushort)1)]
[InlineData(new[] { 7 }, (ushort)128)]
[InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256
[InlineData(new[] { 15 }, (ushort)32768)]
[InlineData(new[] { 1 }, (ushort)1)]
[InlineData(new[] { 2, 4 }, (ushort)10)]
[InlineData(new[] { 3, 4 }, (ushort)12)]
[InlineData(new[] { 2, 3, 4 }, (ushort)14)]
[InlineData(new[] { 8 }, (ushort)128)]
[InlineData(new[] { 8, 9 }, (ushort)384)] // 128 + 256
[InlineData(new[] { 16 }, (ushort)32768)]
[InlineData(new int[] { }, (ushort)0)]
public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected)
{
@@ -693,12 +756,12 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。
/// 验证超出 1..16 范围的地址号会被安全忽略，不会污染位掩码。
/// </summary>
[Fact]
public void ComputeIoValue_IgnoresOutOfRangeAddresses()
{
var group = new IoAddressGroup([0, 16, 7]);
var group = new IoAddressGroup([0, 1, 17, 8]);
var actual = FanucControllerRuntime.ComputeIoValue(group);
Assert.Equal((ushort)(1 | 128), actual);
}