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

Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'

Browse Source
This commit is contained in:
yunxiao.zhu
2026-05-28 10:44:03 +08:00
parent 4eeaa3fef3
commit 35bdb08d11
144 changed files with 33730 additions and 392 deletions
Download Patch File Download Diff File Expand all files Collapse all files

267
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -1,6 +1,7 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
namespace Flyshot.Core.Tests;
@@ -34,6 +35,30 @@ public sealed class PlanningCompatibilityTests
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
}
/// <summary>
/// 验证普通 ICSP 在最终最优解仍超限时会显式失败，而不是返回不可执行轨迹。
/// </summary>
[Fact]
public void ICspPlanner_Throws_WhenFinalGlobalScaleExceedsOne()
{
var request = new TrajectoryRequest(
robot: CreateRobotProfile([0.1], [0.1], [0.1]),
program: CreateProgram(
new[]
{
new[] { 0.0 },
new[] { 10.0 },
new[] { 20.0 },
new[] { 30.0 }
}),
method: PlanningMethod.Icsp);
var planner = new ICspPlanner(maxIterations: 0);
var exception = Assert.Throws<InvalidOperationException>(() => planner.Plan(request));
Assert.Contains("global_scale", exception.Message);
}
/// <summary>
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
/// </summary>
@@ -42,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, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
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);
@@ -145,6 +170,241 @@ public sealed class PlanningCompatibilityTests
Assert.Equal(1, doEvent.WaypointIndex);
Assert.Equal(1, doEvent.OffsetCycles);
Assert.Equal(2, doEvent.HoldCycles);
Assert.NotNull(doEvent.ReferenceJointsDegrees);
Assert.Equal(RadiansToDegrees(1.0), doEvent.ReferenceJointsDegrees![0], precision: 6);
Assert.Equal(0.0, doEvent.ReferenceJointsDegrees![1], precision: 6);
}
/// <summary>
/// 验证时间轴会把示教点关节角保存到 DO 事件里，供运行时在近时窗内做最小关节差绑定。
/// </summary>
[Fact]
public void ShotTimelineBuilder_PopulatesReferenceJointsDegreesForRuntimeBinding()
{
var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
var program = new FlyshotProgram(
name: "demo",
waypoints:
[
new JointWaypoint([0.0, 0.0]),
new JointWaypoint([Math.PI / 6.0, -Math.PI / 4.0])
],
shotFlags: [false, true],
offsetValues: [0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup([1])
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.5],
segmentDurations: [0.5],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
.Build(trajectory, holdCycles: 1, samplePeriod: TimeSpan.FromMilliseconds(8));
var trigger = Assert.Single(timeline.TriggerTimeline);
Assert.NotNull(trigger.ReferenceJointsDegrees);
Assert.Equal(30.0, trigger.ReferenceJointsDegrees![0], precision: 6);
Assert.Equal(-45.0, trigger.ReferenceJointsDegrees![1], precision: 6);
}
/// <summary>
/// 验证稠密轨迹生成后会复核离散加速度，避免采样结果绕过 ICSP 段 scale 校验。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_Throws_WhenDenseTrajectoryAccelerationExceedsLimit()
{
var robot = CreateRobotProfile([100.0], [10.0], [1000.0]);
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
[
[0.0, 0.0],
[0.008, 0.0],
[0.016, 0.01]
],
trajectoryName: "dense-acceleration-check"));
Assert.Contains("加速度", exception.Message);
Assert.Contains("dense-acceleration-check", exception.Message);
}
/// <summary>
/// 验证飞拍链路临时关闭 jerk 复核时，仍保留速度/加速度校验，但不会再被 jerk 单独阻断。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_CanSkipJerkValidation_ForFlyshotPath()
{
var robot = CreateRobotProfile([100.0], [1000.0], [10.0]);
IReadOnlyList<IReadOnlyList<double>> rows =
[
[0.0, 0.0],
[0.008, 0.0],
[0.016, 0.01],
[0.024, 0.02]
];
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
rows,
trajectoryName: "dense-jerk-check"));
Assert.Contains("Jerk", exception.Message);
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
rows,
trajectoryName: "dense-jerk-check-skip",
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>
@@ -224,4 +484,9 @@ public sealed class PlanningCompatibilityTests
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}