✨ feat(native-planning): 添加 Rust ICSP 原生规划核心及 C# 互操作集成
* 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口) * 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装) * 更新 ControllerClientCompat 适配新的规划接口 * 添加构建脚本 (Copy-NativePlanningArtifacts.ps1) * 新增互操作测试及一致性对比测试 * 更新解决方案文件和设计文档
This commit is contained in:
160
tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs
Normal file
160
tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs
Normal file
@@ -0,0 +1,160 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Native.Planning;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Rust ICSP 与现有 C# 基准实现保持逐项数值一致。
|
||||
/// </summary>
|
||||
public sealed class RustIcspPlannerParityTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证基础六自由度兼容样本的时间轴、段时长、段缩放和迭代信息完全对齐。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture()
|
||||
{
|
||||
TrajectoryRequest request = CreateCompatibilityRequest();
|
||||
PlannedTrajectory managed = new ICspPlanner().Plan(request);
|
||||
PlannedTrajectory native = new RustIcspPlanner().Plan(request);
|
||||
|
||||
Assert.Equal(PlanningMethod.Icsp, managed.Method);
|
||||
Assert.Equal(PlanningMethod.IcspRust, native.Method);
|
||||
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证关闭最终 scale 拒绝后,多自由度非均匀路点仍与托管算法逐项对齐。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RustIcspPlanner_MatchesManagedIcspPlanner_ForNonUniformFixture()
|
||||
{
|
||||
TrajectoryRequest request = CreateNonUniformRequest();
|
||||
var options = NativeIcspPlannerOptions.Default with { EnforceFinalScale = false };
|
||||
PlannedTrajectory managed = new ICspPlanner(enforceFinalScale: false).Plan(request);
|
||||
PlannedTrajectory native = new RustIcspPlanner(options).Plan(request);
|
||||
|
||||
Assert.Equal(PlanningMethod.Icsp, managed.Method);
|
||||
Assert.Equal(PlanningMethod.IcspRust, native.Method);
|
||||
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建与现有 PlanningCompatibilityTests 基础用例一致的六自由度请求。
|
||||
/// </summary>
|
||||
/// <returns>可由默认 ICSP 配置收敛的稳定规划请求。</returns>
|
||||
private static TrajectoryRequest CreateCompatibilityRequest()
|
||||
{
|
||||
return CreateRequest(
|
||||
velocityLimits: [1, 1, 1, 1, 1, 1],
|
||||
accelerationLimits: [2, 2, 2, 2, 2, 2],
|
||||
jerkLimits: [10, 10, 10, 10, 10, 10],
|
||||
waypointRows:
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.4, 0.1, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.8, 0.3, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.2, 0.0, 0.0, 0.0, 0.0]
|
||||
]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建用于覆盖多自由度、非均匀段距离和不同约束的规划请求。
|
||||
/// </summary>
|
||||
/// <returns>关闭最终 scale 拒绝后可用于数值对拍的稳定请求。</returns>
|
||||
private static TrajectoryRequest CreateNonUniformRequest()
|
||||
{
|
||||
return CreateRequest(
|
||||
velocityLimits: [1.2, 0.9, 1.5],
|
||||
accelerationLimits: [2.5, 1.8, 3.0],
|
||||
jerkLimits: [12.0, 9.0, 15.0],
|
||||
waypointRows:
|
||||
[
|
||||
[0.0, 0.0, 0.0],
|
||||
[0.2, -0.1, 0.4],
|
||||
[0.9, 0.3, 0.7],
|
||||
[1.1, 0.8, 0.2],
|
||||
[1.6, 0.4, -0.1]
|
||||
]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据逐关节限制和路点矩阵创建完整领域请求。
|
||||
/// </summary>
|
||||
/// <param name="velocityLimits">逐关节速度上限。</param>
|
||||
/// <param name="accelerationLimits">逐关节加速度上限。</param>
|
||||
/// <param name="jerkLimits">逐关节 jerk 上限。</param>
|
||||
/// <param name="waypointRows">按路点排列的关节位置矩阵。</param>
|
||||
/// <returns>可同时传给 C# 与 Rust 规划器的请求。</returns>
|
||||
private static TrajectoryRequest CreateRequest(
|
||||
IReadOnlyList<double> velocityLimits,
|
||||
IReadOnlyList<double> accelerationLimits,
|
||||
IReadOnlyList<double> jerkLimits,
|
||||
IReadOnlyList<double[]> waypointRows)
|
||||
{
|
||||
var robot = new RobotProfile(
|
||||
name: "RustParityRobot",
|
||||
modelPath: "Models/RustParity.robot",
|
||||
degreesOfFreedom: velocityLimits.Count,
|
||||
jointLimits: velocityLimits
|
||||
.Select((velocity, index) => new JointLimit(
|
||||
$"J{index + 1}",
|
||||
velocity,
|
||||
accelerationLimits[index],
|
||||
jerkLimits[index]))
|
||||
.ToArray(),
|
||||
jointCouplings: [],
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
var program = new FlyshotProgram(
|
||||
name: "rust-parity",
|
||||
waypoints: waypointRows.Select(row => new JointWaypoint(row)).ToArray(),
|
||||
shotFlags: Enumerable.Repeat(false, waypointRows.Count).ToArray(),
|
||||
offsetValues: Enumerable.Repeat(0, waypointRows.Count).ToArray(),
|
||||
addressGroups: Enumerable.Range(0, waypointRows.Count)
|
||||
.Select(_ => new IoAddressGroup([]))
|
||||
.ToArray());
|
||||
|
||||
return new TrajectoryRequest(robot, program, PlanningMethod.Icsp);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 逐字段比较两套规划实现的数值结果。
|
||||
/// </summary>
|
||||
/// <param name="managed">现有 C# ICSP 基准结果。</param>
|
||||
/// <param name="native">Rust ICSP 原生结果。</param>
|
||||
/// <param name="tolerance">所有浮点数组和 threshold 使用的绝对误差。</param>
|
||||
private static void AssertTrajectoryMatches(
|
||||
PlannedTrajectory managed,
|
||||
PlannedTrajectory native,
|
||||
double tolerance)
|
||||
{
|
||||
Assert.Equal(managed.Iterations, native.Iterations);
|
||||
Assert.Equal(managed.WaypointTimes.Count, native.WaypointTimes.Count);
|
||||
Assert.Equal(managed.SegmentDurations.Count, native.SegmentDurations.Count);
|
||||
Assert.Equal(managed.SegmentScales.Count, native.SegmentScales.Count);
|
||||
Assert.InRange(Math.Abs(managed.Threshold - native.Threshold), 0.0, tolerance);
|
||||
|
||||
AssertSequenceMatches(managed.WaypointTimes, native.WaypointTimes, tolerance);
|
||||
AssertSequenceMatches(managed.SegmentDurations, native.SegmentDurations, tolerance);
|
||||
AssertSequenceMatches(managed.SegmentScales, native.SegmentScales, tolerance);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按相同索引比较两个浮点序列的绝对误差。
|
||||
/// </summary>
|
||||
/// <param name="expected">托管基准序列。</param>
|
||||
/// <param name="actual">Rust 原生序列。</param>
|
||||
/// <param name="tolerance">允许的最大绝对误差。</param>
|
||||
private static void AssertSequenceMatches(
|
||||
IReadOnlyList<double> expected,
|
||||
IReadOnlyList<double> actual,
|
||||
double tolerance)
|
||||
{
|
||||
for (int index = 0; index < expected.Count; index++)
|
||||
{
|
||||
Assert.InRange(Math.Abs(expected[index] - actual[index]), 0.0, tolerance);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user