yunxiao.zhu
d120ef2a39
* 将 speedRatio 前移到规划/准备阶段，运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
493 lines
19 KiB
C#
493 lines
19 KiB
C#
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;
|
||
|
||
/// <summary>
|
||
/// 锁定 Task 4 的最小兼容面，覆盖 ICSP、自适应补点以及飞拍触发时间轴。
|
||
/// </summary>
|
||
public sealed class PlanningCompatibilityTests
|
||
{
|
||
/// <summary>
|
||
/// 验证 ICSP 规划至少会生成严格递增的 waypoint 时间轴。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ICspPlanner_ReturnsMonotonicWaypointTimes()
|
||
{
|
||
var request = new TrajectoryRequest(
|
||
robot: CreateRobotProfile([1, 1, 1, 1, 1, 1], [2, 2, 2, 2, 2, 2], [10, 10, 10, 10, 10, 10]),
|
||
program: CreateProgram(
|
||
new[]
|
||
{
|
||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.4, 0.1, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 0.8, 0.3, 0.0, 0.0, 0.0, 0.0 },
|
||
new[] { 1.0, 0.2, 0.0, 0.0, 0.0, 0.0 }
|
||
}),
|
||
method: PlanningMethod.Icsp);
|
||
|
||
var trajectory = new ICspPlanner().Plan(request);
|
||
|
||
Assert.Equal(4, trajectory.WaypointTimes.Count);
|
||
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>
|
||
[Fact]
|
||
public void SelfAdaptIcspPlanner_InsertsMidpoints_ForEol9Case()
|
||
{
|
||
var workspaceRoot = GetWorkspaceRoot();
|
||
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.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);
|
||
var constrainedProfile = ScaleRobotProfile(baseProfile, velocityScale: 0.9, accelerationScale: 0.9 * 0.9, jerkScale: 0.9 * 0.9 * 0.9);
|
||
|
||
var request = new TrajectoryRequest(
|
||
robot: constrainedProfile,
|
||
program: config.Programs["EOL9_EAU_90"],
|
||
method: PlanningMethod.SelfAdaptIcsp);
|
||
|
||
var trajectory = new SelfAdaptIcspPlanner().Plan(request, adaptIcspTryNum: config.Robot.AdaptIcspTryNum);
|
||
|
||
Assert.True(trajectory.InsertedWaypointCount > 0);
|
||
Assert.True(trajectory.PlannedWaypointCount > trajectory.OriginalWaypointCount);
|
||
Assert.True(trajectory.SegmentScales.Max() <= 1.0005);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证补点后仍然能按原始示教点顺序找回时间戳，而不是错误地绑定到新增中点。
|
||
/// </summary>
|
||
[Fact]
|
||
public void WaypointTimestampResolver_UsesOriginalTeachPointsAfterInsertion()
|
||
{
|
||
var originalProgram = CreateProgram(
|
||
new[]
|
||
{
|
||
new[] { 0.0, 0.0 },
|
||
new[] { 1.0, 0.0 },
|
||
new[] { 2.0, 0.0 },
|
||
new[] { 3.0, 0.0 }
|
||
});
|
||
|
||
var trajectory = new PlannedTrajectory(
|
||
robot: CreateRobotProfile([1, 1], [1, 1], [1, 1]),
|
||
originalProgram: originalProgram,
|
||
plannedWaypoints:
|
||
[
|
||
new JointWaypoint([0.0, 0.0]),
|
||
new JointWaypoint([0.5, 0.0]),
|
||
new JointWaypoint([1.0, 0.0]),
|
||
new JointWaypoint([2.0, 0.0]),
|
||
new JointWaypoint([2.5, 0.0]),
|
||
new JointWaypoint([3.0, 0.0])
|
||
],
|
||
waypointTimes: [0.0, 0.25, 0.5, 1.0, 1.5, 2.0],
|
||
segmentDurations: [0.25, 0.25, 0.5, 0.5, 0.5],
|
||
segmentScales: [1.0, 1.0, 1.0, 1.0, 1.0],
|
||
method: PlanningMethod.SelfAdaptIcsp,
|
||
iterations: 3,
|
||
threshold: 0.0);
|
||
|
||
var timestamps = new WaypointTimestampResolver().Resolve(trajectory);
|
||
|
||
Assert.Equal([0.0, 0.5, 1.0, 2.0], timestamps);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证触发时间轴会使用原始 waypoint 时间、offset 周期和地址组生成 ShotEvent/TrajectoryDoEvent。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ShotTimelineBuilder_MapsOffsetsToShotEventsAndTriggerTimeline()
|
||
{
|
||
var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
|
||
var program = new FlyshotProgram(
|
||
name: "demo",
|
||
waypoints:
|
||
[
|
||
new JointWaypoint([0.0, 0.0]),
|
||
new JointWaypoint([1.0, 0.0]),
|
||
new JointWaypoint([2.0, 0.0])
|
||
],
|
||
shotFlags: [false, true, false],
|
||
offsetValues: [0, 1, 0],
|
||
addressGroups:
|
||
[
|
||
new IoAddressGroup(Array.Empty<int>()),
|
||
new IoAddressGroup([2, 4]),
|
||
new IoAddressGroup(Array.Empty<int>())
|
||
]);
|
||
|
||
var trajectory = new PlannedTrajectory(
|
||
robot: robot,
|
||
originalProgram: program,
|
||
plannedWaypoints: program.Waypoints,
|
||
waypointTimes: [0.0, 0.5, 1.0],
|
||
segmentDurations: [0.5, 0.5],
|
||
segmentScales: [1.0, 1.0],
|
||
method: PlanningMethod.Icsp,
|
||
iterations: 1,
|
||
threshold: 0.0);
|
||
|
||
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
|
||
.Build(trajectory, holdCycles: 2, samplePeriod: TimeSpan.FromMilliseconds(16));
|
||
|
||
var shotEvent = Assert.Single(timeline.ShotEvents);
|
||
Assert.Equal(0.508, shotEvent.TriggerTime, precision: 3);
|
||
Assert.Equal([2, 4], shotEvent.AddressGroup.Addresses);
|
||
|
||
var doEvent = Assert.Single(timeline.TriggerTimeline);
|
||
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>
|
||
/// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
|
||
/// </summary>
|
||
private static RobotProfile CreateRobotProfile(
|
||
IReadOnlyList<double> velocityLimits,
|
||
IReadOnlyList<double> accelerationLimits,
|
||
IReadOnlyList<double> jerkLimits)
|
||
{
|
||
return new RobotProfile(
|
||
name: "TestRobot",
|
||
modelPath: "Models/Test.robot",
|
||
degreesOfFreedom: velocityLimits.Count,
|
||
jointLimits: velocityLimits
|
||
.Select((velocity, index) => new JointLimit(
|
||
$"J{index + 1}",
|
||
velocity,
|
||
accelerationLimits[index],
|
||
jerkLimits[index]))
|
||
.ToArray(),
|
||
jointCouplings: Array.Empty<JointCoupling>(),
|
||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造一个默认不带拍照点的最小 FlyshotProgram。
|
||
/// </summary>
|
||
private static FlyshotProgram CreateProgram(IEnumerable<double[]> waypointRows)
|
||
{
|
||
var rows = waypointRows.ToArray();
|
||
return new FlyshotProgram(
|
||
name: "demo",
|
||
waypoints: rows.Select(row => new JointWaypoint(row)).ToArray(),
|
||
shotFlags: Enumerable.Repeat(false, rows.Length).ToArray(),
|
||
offsetValues: Enumerable.Repeat(0, rows.Length).ToArray(),
|
||
addressGroups: Enumerable.Range(0, rows.Length).Select(_ => new IoAddressGroup(Array.Empty<int>())).ToArray());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在不丢失 couple 元数据的前提下，按比例缩放机器人关节限制。
|
||
/// </summary>
|
||
private static RobotProfile ScaleRobotProfile(RobotProfile source, double velocityScale, double accelerationScale, double jerkScale)
|
||
{
|
||
return new RobotProfile(
|
||
name: source.Name,
|
||
modelPath: source.ModelPath,
|
||
degreesOfFreedom: source.DegreesOfFreedom,
|
||
jointLimits: source.JointLimits
|
||
.Select(limit => new JointLimit(
|
||
limit.JointName,
|
||
limit.VelocityLimit * velocityScale,
|
||
limit.AccelerationLimit * accelerationScale,
|
||
limit.JerkLimit * jerkScale))
|
||
.ToArray(),
|
||
jointCouplings: source.JointCouplings,
|
||
servoPeriod: source.ServoPeriod,
|
||
triggerPeriod: source.TriggerPeriod);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位父工作区根目录，读取真实的 EOL9 样本和机器人模型。
|
||
/// </summary>
|
||
private static string GetWorkspaceRoot()
|
||
{
|
||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||
while (current is not null)
|
||
{
|
||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||
{
|
||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||
}
|
||
|
||
current = current.Parent;
|
||
}
|
||
|
||
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
|
||
}
|
||
|
||
private static double RadiansToDegrees(double radians)
|
||
{
|
||
return radians * 180.0 / Math.PI;
|
||
}
|
||
}
|