yunxiao.zhu
d82128da4a
* 在 RobotConfig.json 中新增 speed_ratio 配置，允许在运行时设置默认速度倍率。 * 调整 ControllerClientCompatService 以使用 speed_ratio 初始化机器人设置。 * 更新 TrajectoryLimitValidator 和 FlyshotExecutionSendSequenceBuilder，支持在飞拍链路中临时关闭 jerk 校验，仅保留速度和加速度约束。 * 新增文档记录对 UTTC_MS11 的 jerk 阻断策略调整，确保飞拍链路的执行效率。 * 增加单元测试以验证 speed_ratio 的加载和 jerk 校验的行为。
355 lines
13 KiB
C#
355 lines
13 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", "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>
|
||
/// 构造一个最小 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;
|
||
}
|
||
}
|