✨ feat: 初始化飞拍替换方案仓库骨架
* 建立 .NET 8 解决方案及分层项目结构 * 添加 Flyshot.Core.Domain 领域模型（机器人、轨迹、运动学） * 添加 Flyshot.Core.Planning 规划层（ICSP、CubicSpline、采样器） * 添加 Flyshot.Core.Triggering 触发时序与 IO 时间轴 * 添加 Flyshot.Core.Config 配置兼容与 .robot 解析 * 添加 Flyshot.Server.Host 最小宿主及 /healthz 端点 * 补充单元测试与集成测试项目 * 添加 CLAUDE.md、AGENTS.md、README.md 项目规范
This commit is contained in:
176
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
Normal file
176
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
Normal file
@@ -0,0 +1,176 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 锁定 Task 3 的兼容输入行为，确保旧配置、.robot 元数据和路径策略都能被稳定加载。
|
||||
/// </summary>
|
||||
public sealed class ConfigCompatibilityTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证现有 RobotConfig.json 能被加载，并保持关键机器人参数与飞拍程序内容不变。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms()
|
||||
{
|
||||
var workspaceRoot = GetWorkspaceRoot();
|
||||
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json");
|
||||
|
||||
var loaded = new RobotConfigLoader().Load(configPath);
|
||||
|
||||
Assert.True(loaded.Robot.UseDo);
|
||||
Assert.Equal([7, 8], loaded.Robot.IoAddresses);
|
||||
Assert.Equal(2, loaded.Robot.IoKeepCycles);
|
||||
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
|
||||
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
|
||||
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
|
||||
|
||||
var program = Assert.Contains("001", loaded.Programs);
|
||||
Assert.Equal("001", program.Name);
|
||||
Assert.Equal(5, program.Waypoints.Count);
|
||||
Assert.Equal(3, program.ShotWaypointCount);
|
||||
Assert.Empty(program.AddressGroups[0].Addresses);
|
||||
Assert.Equal([8, 7], program.AddressGroups[1].Addresses);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证旧配置缺少 offset_values 和 addr 字段时，会自动回填与旧系统一致的默认值。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotConfigLoader_FillsLegacyDefaults_WhenOptionalFieldsAreMissing()
|
||||
{
|
||||
var tempRoot = CreateTempDirectory();
|
||||
try
|
||||
{
|
||||
var configPath = Path.Combine(tempRoot, "legacy.json");
|
||||
File.WriteAllText(
|
||||
configPath,
|
||||
"""
|
||||
{
|
||||
"robot": {
|
||||
"use_do": false,
|
||||
"io_keep_cycles": 3,
|
||||
"acc_limit": 0.5,
|
||||
"jerk_limit": 0.25
|
||||
},
|
||||
"flying_shots": {
|
||||
"demo": {
|
||||
"traj_waypoints": [[0, 1], [2, 3], [4, 5]],
|
||||
"shot_flags": [0, 1, 0]
|
||||
}
|
||||
}
|
||||
}
|
||||
""");
|
||||
|
||||
var loaded = new RobotConfigLoader().Load(configPath);
|
||||
var program = Assert.Contains("demo", loaded.Programs);
|
||||
|
||||
Assert.False(loaded.Robot.UseDo);
|
||||
Assert.Empty(loaded.Robot.IoAddresses);
|
||||
Assert.Equal(3, loaded.Robot.IoKeepCycles);
|
||||
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
|
||||
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
|
||||
Assert.Equal([0, 0, 0], program.OffsetValues);
|
||||
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(tempRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
|
||||
{
|
||||
var workspaceRoot = GetWorkspaceRoot();
|
||||
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
||||
|
||||
var profile = new RobotModelLoader().LoadProfile(modelPath);
|
||||
|
||||
Assert.Equal("FANUC_LR_Mate_200iD_7L", profile.Name);
|
||||
Assert.Equal(modelPath, profile.ModelPath);
|
||||
Assert.Equal(6, profile.DegreesOfFreedom);
|
||||
Assert.Equal(6.45, profile.JointLimits[0].VelocityLimit, precision: 2);
|
||||
Assert.Equal(29.81, profile.JointLimits[2].AccelerationLimit, precision: 2);
|
||||
|
||||
var coupling = Assert.Single(profile.JointCouplings);
|
||||
Assert.Equal("Joint3", coupling.SlaveJointName);
|
||||
Assert.Equal("Joint2", coupling.MasterJointName);
|
||||
Assert.Equal(1.0, coupling.Multiplier);
|
||||
Assert.Equal(0.0, coupling.Offset);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
|
||||
{
|
||||
var workspaceRoot = GetWorkspaceRoot();
|
||||
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
||||
|
||||
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
|
||||
|
||||
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
|
||||
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证路径兼容层既能补旧目录候选，也能按平台策略生成默认用户数据目录。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void PathCompatibility_ResolvesLegacyCandidates_AndBuildsUserDataRoots()
|
||||
{
|
||||
var tempRoot = CreateTempDirectory();
|
||||
try
|
||||
{
|
||||
var legacyConfigPath = Path.Combine(tempRoot, "Rvbust", "Install", "FlyingShot", "Config", "sample.json");
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(legacyConfigPath)!);
|
||||
File.WriteAllText(legacyConfigPath, "{}");
|
||||
|
||||
var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot);
|
||||
|
||||
Assert.Equal(legacyConfigPath, resolved);
|
||||
Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix));
|
||||
Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows));
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(tempRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位当前工作区根目录，便于复用父仓库中的真实样本。
|
||||
/// </summary>
|
||||
private static string GetWorkspaceRoot()
|
||||
{
|
||||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (current is not null)
|
||||
{
|
||||
var slnPath = Path.Combine(current.FullName, "FlyshotReplacement.sln");
|
||||
if (File.Exists(slnPath))
|
||||
{
|
||||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建当前测试专用的临时目录，避免不同测试之间相互污染。
|
||||
/// </summary>
|
||||
private static string CreateTempDirectory()
|
||||
{
|
||||
var tempPath = Path.Combine(Path.GetTempPath(), "flyshot-config-tests", Guid.NewGuid().ToString("N"));
|
||||
Directory.CreateDirectory(tempPath);
|
||||
return tempPath;
|
||||
}
|
||||
}
|
||||
207
tests/Flyshot.Core.Tests/DomainModelTests.cs
Normal file
207
tests/Flyshot.Core.Tests/DomainModelTests.cs
Normal file
@@ -0,0 +1,207 @@
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// Verifies the Task 2 domain contracts before planning and runtime code depend on them.
|
||||
/// </summary>
|
||||
public sealed class DomainModelTests
|
||||
{
|
||||
/// <summary>
|
||||
/// Ensures robot profiles keep a stable copy of joint limits and reject invalid dimensions.
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotProfile_CopiesJointLimits_AndRejectsMismatchedDof()
|
||||
{
|
||||
var jointLimits = new[]
|
||||
{
|
||||
new JointLimit("J1", 7.85, 32.72, 272.7),
|
||||
new JointLimit("J2", 6.63, 27.63, 230.28)
|
||||
};
|
||||
|
||||
var profile = new RobotProfile(
|
||||
name: "LR_Mate_200iD_7L",
|
||||
modelPath: "Models/LR_Mate_200iD_7L.robot",
|
||||
degreesOfFreedom: 2,
|
||||
jointLimits: jointLimits,
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
|
||||
Assert.Equal(2, profile.DegreesOfFreedom);
|
||||
Assert.NotSame(jointLimits, profile.JointLimits);
|
||||
|
||||
// The planner must not accept a profile whose DOF and limits disagree.
|
||||
Assert.Throws<ArgumentException>(() => new RobotProfile(
|
||||
name: "InvalidRobot",
|
||||
modelPath: "Models/Invalid.robot",
|
||||
degreesOfFreedom: 3,
|
||||
jointLimits: jointLimits,
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ensures uploaded flyshot programs keep their shot metadata aligned with teach waypoints.
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void FlyshotProgram_RejectsMisalignedShotMetadata()
|
||||
{
|
||||
var waypoints = new[]
|
||||
{
|
||||
new JointWaypoint(new[] { 0.0, 1.0 }),
|
||||
new JointWaypoint(new[] { 2.0, 3.0 })
|
||||
};
|
||||
|
||||
var validProgram = new FlyshotProgram(
|
||||
name: "EOL10_EAU_0",
|
||||
waypoints: waypoints,
|
||||
shotFlags: new[] { true, false },
|
||||
offsetValues: new[] { 0, 1 },
|
||||
addressGroups: new[]
|
||||
{
|
||||
new IoAddressGroup(new[] { 100 }),
|
||||
new IoAddressGroup(Array.Empty<int>())
|
||||
});
|
||||
|
||||
Assert.Equal(1, validProgram.ShotWaypointCount);
|
||||
|
||||
// The gateway cannot recover from count mismatches after this point, so fail fast here.
|
||||
Assert.Throws<ArgumentException>(() => new FlyshotProgram(
|
||||
name: "BrokenProgram",
|
||||
waypoints: waypoints,
|
||||
shotFlags: new[] { true },
|
||||
offsetValues: new[] { 0, 1 },
|
||||
addressGroups: new[]
|
||||
{
|
||||
new IoAddressGroup(new[] { 100 }),
|
||||
new IoAddressGroup(Array.Empty<int>())
|
||||
}));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ensures execution requests start from predictable defaults for compatibility paths.
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void TrajectoryRequest_UsesExpectedDefaults()
|
||||
{
|
||||
var request = new TrajectoryRequest(
|
||||
robot: CreateRobotProfile(),
|
||||
program: CreateProgram(),
|
||||
method: PlanningMethod.Icsp);
|
||||
|
||||
Assert.False(request.MoveToStart);
|
||||
Assert.False(request.SaveTrajectoryArtifacts);
|
||||
Assert.False(request.UseCache);
|
||||
Assert.Equal(PlanningMethod.Icsp, request.Method);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ensures runtime snapshots expose safe empty collections before the controller connects.
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerStateSnapshot_InitializesEmptyRuntimeCollections()
|
||||
{
|
||||
var snapshot = new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"),
|
||||
connectionState: "Disconnected",
|
||||
isEnabled: false,
|
||||
isInMotion: false,
|
||||
speedRatio: 100.0);
|
||||
|
||||
Assert.Empty(snapshot.JointPositions);
|
||||
Assert.Empty(snapshot.CartesianPose);
|
||||
Assert.Empty(snapshot.ActiveAlarms);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ensures JSON payloads keep stable enum and property names for downstream SDKs.
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void DomainObjects_SerializeStableContract()
|
||||
{
|
||||
var result = new TrajectoryResult(
|
||||
programName: "EOL10_EAU_0",
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(1.25),
|
||||
shotEvents: new[]
|
||||
{
|
||||
new ShotEvent(
|
||||
waypointIndex: 0,
|
||||
triggerTime: 0.5,
|
||||
sampleIndex: 62,
|
||||
sampleTime: 0.496,
|
||||
addressGroup: new IoAddressGroup(new[] { 100 }))
|
||||
},
|
||||
triggerTimeline: new[]
|
||||
{
|
||||
new TrajectoryDoEvent(
|
||||
waypointIndex: 0,
|
||||
triggerTime: 0.5,
|
||||
offsetCycles: 0,
|
||||
holdCycles: 1,
|
||||
addressGroup: new IoAddressGroup(new[] { 100 }))
|
||||
},
|
||||
artifacts: new[]
|
||||
{
|
||||
new TrajectoryArtifact(
|
||||
kind: TrajectoryArtifactKind.JointDenseTrajectory,
|
||||
logicalName: "JointDetialTraj.txt",
|
||||
relativePath: "artifacts/JointDetialTraj.txt")
|
||||
},
|
||||
failureReason: null,
|
||||
usedCache: true,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 5);
|
||||
|
||||
var json = JsonSerializer.Serialize(result);
|
||||
|
||||
Assert.Contains("\"programName\":\"EOL10_EAU_0\"", json);
|
||||
Assert.Contains("\"method\":\"SelfAdaptIcsp\"", json);
|
||||
Assert.Contains("\"kind\":\"JointDenseTrajectory\"", json);
|
||||
Assert.Contains("\"usedCache\":true", json);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a representative robot profile for request-level domain tests.
|
||||
/// </summary>
|
||||
private static RobotProfile CreateRobotProfile()
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: "LR_Mate_200iD_7L",
|
||||
modelPath: "Models/LR_Mate_200iD_7L.robot",
|
||||
degreesOfFreedom: 2,
|
||||
jointLimits: new[]
|
||||
{
|
||||
new JointLimit("J1", 7.85, 32.72, 272.7),
|
||||
new JointLimit("J2", 6.63, 27.63, 230.28)
|
||||
},
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a representative uploaded program for request-level domain tests.
|
||||
/// </summary>
|
||||
private static FlyshotProgram CreateProgram()
|
||||
{
|
||||
return new FlyshotProgram(
|
||||
name: "EOL10_EAU_0",
|
||||
waypoints: new[]
|
||||
{
|
||||
new JointWaypoint(new[] { 0.0, 1.0 }),
|
||||
new JointWaypoint(new[] { 2.0, 3.0 })
|
||||
},
|
||||
shotFlags: new[] { true, false },
|
||||
offsetValues: new[] { 0, 1 },
|
||||
addressGroups: new[]
|
||||
{
|
||||
new IoAddressGroup(new[] { 100 }),
|
||||
new IoAddressGroup(Array.Empty<int>())
|
||||
});
|
||||
}
|
||||
}
|
||||
32
tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj
Normal file
32
tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj
Normal file
@@ -0,0 +1,32 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
|
||||
<IsPackable>false</IsPackable>
|
||||
<IsTestProject>true</IsTestProject>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.6.0" />
|
||||
<PackageReference Include="xunit" Version="2.4.2" />
|
||||
<PackageReference Include="xunit.runner.visualstudio" Version="2.4.5">
|
||||
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
||||
<PrivateAssets>all</PrivateAssets>
|
||||
</PackageReference>
|
||||
<PackageReference Include="coverlet.collector" Version="6.0.0">
|
||||
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
||||
<PrivateAssets>all</PrivateAssets>
|
||||
</PackageReference>
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
1
tests/Flyshot.Core.Tests/GlobalUsings.cs
Normal file
1
tests/Flyshot.Core.Tests/GlobalUsings.cs
Normal file
@@ -0,0 +1 @@
|
||||
global using Xunit;
|
||||
154
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
Normal file
154
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
Normal file
@@ -0,0 +1,154 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Export;
|
||||
using Flyshot.Core.Planning.Kinematics;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Triggering;
|
||||
using Xunit.Abstractions;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 离线轨迹生成入口，与 Python demo 的 CLI 对齐。
|
||||
///
|
||||
/// 为什么放在测试项目里？
|
||||
/// ---
|
||||
/// 测试项目已经引用了所有需要的模块（Config、Domain、Planning、Triggering），
|
||||
/// 不需要额外创建 Console 项目。通过 dotnet test --filter 可以直接调用，
|
||||
/// 也能利用 xUnit 的断言做结果验证。
|
||||
/// </summary>
|
||||
public sealed class OfflinePlanTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", false, 1.0)]
|
||||
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", true, 0.9)]
|
||||
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL10_EAU_0", false, 1.0)]
|
||||
public void GenerateTrajectory_MatchesPythonDemo(
|
||||
string configPath,
|
||||
string robotModelPath,
|
||||
string trajName,
|
||||
bool useSelfAdapt,
|
||||
double speedRatio)
|
||||
{
|
||||
var workspaceRoot = GetWorkspaceRoot();
|
||||
var outputDir = Path.Combine(workspaceRoot, "analysis", "output", "dotnet", $"{trajName}_sr{speedRatio:F2}_{(useSelfAdapt ? "adapt" : "icsp")}");
|
||||
Directory.CreateDirectory(outputDir);
|
||||
|
||||
// 1. 加载配置和模型。
|
||||
var loadedConfig = new RobotConfigLoader().Load(configPath, repoRoot: workspaceRoot);
|
||||
var program = loadedConfig.Programs[trajName];
|
||||
var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath);
|
||||
var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale);
|
||||
var kinematicsModel = new RobotModelLoader().LoadKinematicsModel(resolvedRobotModelPath);
|
||||
|
||||
// 2. 应用 speed_ratio 缩放。
|
||||
var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);
|
||||
|
||||
// 3. 规划轨迹。
|
||||
var request = new TrajectoryRequest(
|
||||
robot: scaledProfile,
|
||||
program: program,
|
||||
method: useSelfAdapt ? PlanningMethod.SelfAdaptIcsp : PlanningMethod.Icsp);
|
||||
|
||||
PlannedTrajectory trajectory;
|
||||
if (useSelfAdapt)
|
||||
{
|
||||
trajectory = new SelfAdaptIcspPlanner().Plan(request, loadedConfig.Robot.AdaptIcspTryNum);
|
||||
}
|
||||
else
|
||||
{
|
||||
trajectory = new ICspPlanner().Plan(request);
|
||||
}
|
||||
|
||||
// 4. 生成触发时间轴。
|
||||
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
|
||||
.Build(trajectory, holdCycles: loadedConfig.Robot.IoKeepCycles, samplePeriod: TimeSpan.FromMilliseconds(16));
|
||||
|
||||
// 5. 稠密采样。
|
||||
double samplePeriod = 0.016;
|
||||
var jointDense = TrajectorySampler.SampleJointTrajectory(trajectory, samplePeriod);
|
||||
var cartDense = TrajectorySampler.SampleCartesianTrajectory(trajectory, kinematicsModel, samplePeriod);
|
||||
|
||||
// 6. 导出文件。
|
||||
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.demo.txt"), jointDense);
|
||||
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.demo.txt"), cartDense);
|
||||
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.demo.json"), timeline.ShotEvents);
|
||||
|
||||
// 7. 打印统计信息到测试输出。
|
||||
_testOutputHelper.WriteLine($"traj_name={trajName}");
|
||||
_testOutputHelper.WriteLine($"speed_ratio={speedRatio:F6}");
|
||||
_testOutputHelper.WriteLine($"duration={trajectory.WaypointTimes[^1]:F6}");
|
||||
_testOutputHelper.WriteLine($"joint_dense_rows={jointDense.Count}");
|
||||
_testOutputHelper.WriteLine($"cart_dense_rows={cartDense.Count}");
|
||||
_testOutputHelper.WriteLine($"shot_events={timeline.ShotEvents.Count}");
|
||||
_testOutputHelper.WriteLine($"output_dir={outputDir}");
|
||||
|
||||
// 最小验证：输出文件存在且非空。
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.demo.txt")));
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.demo.txt")));
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.demo.json")));
|
||||
Assert.True(jointDense.Count > 0);
|
||||
Assert.True(cartDense.Count > 0);
|
||||
}
|
||||
|
||||
private readonly ITestOutputHelper _testOutputHelper;
|
||||
|
||||
public OfflinePlanTests(ITestOutputHelper testOutputHelper)
|
||||
{
|
||||
_testOutputHelper = testOutputHelper;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按 speed_ratio 缩放机器人关节限制。
|
||||
///
|
||||
/// 缩放律：
|
||||
/// - 速度按一次方：speedRatio^1
|
||||
/// - 加速度按平方：speedRatio^2
|
||||
/// - jerk 按立方：speedRatio^3
|
||||
///
|
||||
/// 为什么加速度是平方？
|
||||
/// ---
|
||||
/// 如果把时间轴整体缩放 1/speedRatio 倍，速度按 speedRatio 缩放，加速度按 speedRatio^2 缩放。
|
||||
/// 所以降低 speed_ratio 意味着降低速度、更大幅度地降低加速度和 jerk，轨迹更平滑。
|
||||
/// </summary>
|
||||
private static RobotProfile ScaleRobotProfile(RobotProfile source, double speedRatio)
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: source.Name,
|
||||
modelPath: source.ModelPath,
|
||||
degreesOfFreedom: source.DegreesOfFreedom,
|
||||
jointLimits: source.JointLimits
|
||||
.Select(limit => new JointLimit(
|
||||
limit.JointName,
|
||||
limit.VelocityLimit * speedRatio,
|
||||
limit.AccelerationLimit * speedRatio * speedRatio,
|
||||
limit.JerkLimit * speedRatio * speedRatio * speedRatio))
|
||||
.ToArray(),
|
||||
jointCouplings: source.JointCouplings,
|
||||
servoPeriod: source.ServoPeriod,
|
||||
triggerPeriod: source.TriggerPeriod);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位父工作区根目录。
|
||||
/// </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.");
|
||||
}
|
||||
}
|
||||
227
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
Normal file
227
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
Normal file
@@ -0,0 +1,227 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
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>
|
||||
/// 验证 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, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/// <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.");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user