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

✨ feat(*): 添加 J519 实发重采样与 JSON 机型模型

Browse Source 
* 新增 J519 实发采样器，按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换
* 兼容层产物导出补充 speedRatio，规划编排补齐 smoothStartStopTiming 与日志透传
* 配置与机型加载切换到运行目录 JSON 模型，并补齐 7L 展开模型与相关单元测试
This commit is contained in:
yunxiao.zhu
2026-05-07 17:08:32 +08:00
parent 70b0ccd414
commit c6829d214a
26 changed files with 1417 additions and 409 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -287,7 +287,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
Assert.Equal("UTTC_MS11", bundle.Result.ProgramName);
@@ -304,6 +304,45 @@ public sealed class RuntimeOrchestrationTests
AssertJointRadiansEqual(fixture.Uploaded.Waypoints[^1], bundle.Result.DenseJointTrajectory[^1].Skip(1).ToArray());
}
/// <summary>
/// 验证现场 UTTC_MS11 配置使用通用规划速度倍率拉长时间轴，并关闭二次平滑时间重映射。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromRuntimeLegacyFitConfig_UsesPlanningScaleWithoutSecondRemap()
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings);
var baselineBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
Assert.Equal(0.7422771653721995, fixture.Settings.PlanningSpeedScale, precision: 12);
Assert.False(fixture.Settings.SmoothStartStopTiming);
Assert.Equal(fixture.Uploaded.Waypoints.Count, bundle.PlannedTrajectory.WaypointTimes.Count);
Assert.Equal(
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / fixture.Settings.PlanningSpeedScale,
bundle.PlannedTrajectory.WaypointTimes[^1],
precision: 6);
Assert.Equal(7.403046, bundle.PlannedTrajectory.WaypointTimes[^1], precision: 3);
// 关闭二次时间重映射时，运行时稠密点应直接使用规划样条采样，避免再次改变通用规划时间轴。
Assert.Equal(rawDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(rawDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
Assert.True(
Math.Abs(bundle.PlannedTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留规划终点时间，planned={bundle.PlannedTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
}
/// <summary>
/// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 示教点，导出首尾 10 点整形前后的逐点对比数据，
/// 便于和现场报警时间段逐项核对关节、速度、加速度与跃度。
@@ -347,6 +386,7 @@ public sealed class RuntimeOrchestrationTests
{
$"program=UTTC_MS11",
$"planning_speed_scale=1.000000",
$"smooth_start_stop_timing=true",
$"servo_period_seconds={fixture.Robot.ServoPeriod.TotalSeconds.ToString("F6", CultureInfo.InvariantCulture)}",
$"dense_point_count={shapedDense.Count}",
$"leading_first_step_raw_deg={ComputeStepDegrees(rawDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
@@ -380,7 +420,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
@@ -412,7 +452,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
@@ -874,7 +914,55 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并应用当前 speed_ratio。
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var runtime = new RecordingControllerRuntime();
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
runtime.SetSpeedRatio(0.5);
service.SaveTrajectoryInfo("demo-flyshot");
var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
var pointsPath = Path.Combine(outputDir, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(outputDir, "ActualSendTiming.txt");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var duration = double.Parse(File.ReadLines(Path.Combine(outputDir, "JointTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture);
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (duration / (0.008 * 0.5)) - 1e-9)) + 1;
Assert.Equal(expectedRows, pointRows.Length);
Assert.Equal(expectedRows, timingRows.Length);
Assert.Equal(0.0, pointRows[0][0], precision: 6);
Assert.Equal(0.008, pointRows[1][0], precision: 6);
Assert.Equal(0.004, timingRows[1][2], precision: 6);
Assert.Equal(0.5, timingRows[1][3], precision: 6);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。
/// </summary>
private static string CreateTempConfigRoot()
{
@@ -886,8 +974,8 @@ public sealed class RuntimeOrchestrationTests
TestRobotFactory.GetReplacementRoot(),
"Config",
"Models",
"LR_Mate_200iD_7L.robot");
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
"LR_Mate_200iD_7L.json");
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));
return configRoot;
}
@@ -960,16 +1048,8 @@ public sealed class RuntimeOrchestrationTests
/// </summary>
private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
{
var configPath = Path.Combine(
TestRobotFactory.GetReplacementRoot(),
"src",
"Flyshot.Server.Host",
"bin",
"Debug",
"net8.0",
"Config",
"RobotConfig.json");
var configRoot = Path.GetDirectoryName(configPath)!;
var configRoot = TestRobotFactory.GetConfigRoot();
var configPath = Path.Combine(configRoot, "RobotConfig.json");
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
var program = loaded.Programs["UTTC_MS11"];
var uploaded = new ControllerClientCompatUploadedTrajectory(
@@ -988,6 +1068,22 @@ public sealed class RuntimeOrchestrationTests
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
}
/// <summary>
/// 为首尾平滑对比测试显式打开二次时间重映射，避免受现场 legacy-fit 配置影响。
/// </summary>
private static CompatibilityRobotSettings EnableSmoothStartStopTiming(CompatibilityRobotSettings settings)
{
return new CompatibilityRobotSettings(
useDo: settings.UseDo,
ioAddresses: settings.IoAddresses,
ioKeepCycles: settings.IoKeepCycles,
accLimitScale: settings.AccLimitScale,
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
smoothStartStopTiming: true);
}
/// <summary>
/// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。
/// </summary>
@@ -1412,6 +1508,8 @@ internal static class TestRobotFactory
/// </summary>
internal sealed class RecordingControllerRuntime : IControllerRuntime
{
private double _speedRatio = 1.0;
/// <summary>
/// 获取最近一次 ResetRobot 收到的机器人配置。
/// </summary>
@@ -1454,11 +1552,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
}
/// <inheritdoc />
public double GetSpeedRatio() => 1.0;
public double GetSpeedRatio() => _speedRatio;
/// <inheritdoc />
public void SetSpeedRatio(double ratio)
{
_speedRatio = ratio;
}
/// <inheritdoc />
@@ -1491,7 +1590,7 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
connectionState: "Connected",
isEnabled: true,
isInMotion: false,
speedRatio: 1.0,
speedRatio: _speedRatio,
jointPositions: Array.Empty<double>(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());