yunxiao.zhu
f7e2bb0e7b
* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置 * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发 * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出 * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档 * 补齐配置兼容、规划绑定和运行时触发相关测试
1983 lines
77 KiB
C#
1983 lines
77 KiB
C#
using Flyshot.ControllerClientCompat;
|
||
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Core.Planning;
|
||
using Flyshot.Core.Planning.Sampling;
|
||
using Flyshot.Runtime.Common;
|
||
using Flyshot.Runtime.Fanuc;
|
||
using Flyshot.Runtime.Fanuc.Protocol;
|
||
using System.Globalization;
|
||
using System.Reflection;
|
||
|
||
namespace Flyshot.Core.Tests;
|
||
|
||
/// <summary>
|
||
/// 验证最小运行时编排链路会把规划结果交给控制器运行时，而不是停留在兼容层内存状态。
|
||
/// </summary>
|
||
public sealed class RuntimeOrchestrationTests
|
||
{
|
||
/// <summary>
|
||
/// 验证 FANUC 最小运行时执行轨迹后会更新状态快照与最终关节位置。
|
||
/// </summary>
|
||
[Fact]
|
||
public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
|
||
{
|
||
var runtime = new FanucControllerRuntime();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||
runtime.SetActiveController(sim: true);
|
||
runtime.Connect("192.168.10.101");
|
||
runtime.EnableRobot(bufferSize: 2);
|
||
|
||
var result = new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(1.2),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4);
|
||
|
||
runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||
|
||
var snapshot = runtime.GetSnapshot();
|
||
Assert.Equal("Connected", snapshot.ConnectionState);
|
||
Assert.False(snapshot.IsInMotion);
|
||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
|
||
/// </summary>
|
||
[Theory]
|
||
[InlineData(FanucStateConnectionState.Connected, false, "Connected")]
|
||
[InlineData(FanucStateConnectionState.Connected, true, "StateTimeout")]
|
||
[InlineData(FanucStateConnectionState.TimedOut, true, "StateTimeout")]
|
||
[InlineData(FanucStateConnectionState.Reconnecting, true, "Reconnecting")]
|
||
[InlineData(FanucStateConnectionState.Disconnected, false, "Disconnected")]
|
||
public void FanucControllerRuntime_ResolveRealConnectionState_ReflectsStateChannelHealth(
|
||
FanucStateConnectionState state,
|
||
bool isFrameStale,
|
||
string expected)
|
||
{
|
||
var status = new FanucStateClientStatus(
|
||
state,
|
||
isFrameStale,
|
||
lastFrameAt: null,
|
||
reconnectAttemptCount: 0,
|
||
lastErrorMessage: null);
|
||
|
||
var actual = FanucControllerRuntime.ResolveRealConnectionState(status);
|
||
|
||
Assert.Equal(expected, actual);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证只有已连接且未陈旧的 TCP 10010 帧会被 runtime 当作当前机器人状态使用。
|
||
/// </summary>
|
||
[Theory]
|
||
[InlineData(FanucStateConnectionState.Connected, false, true)]
|
||
[InlineData(FanucStateConnectionState.Connected, true, false)]
|
||
[InlineData(FanucStateConnectionState.Reconnecting, false, false)]
|
||
[InlineData(FanucStateConnectionState.TimedOut, false, false)]
|
||
[InlineData(FanucStateConnectionState.Disconnected, false, false)]
|
||
public void FanucControllerRuntime_ShouldUseStateFrame_RequiresConnectedFreshState(
|
||
FanucStateConnectionState state,
|
||
bool isFrameStale,
|
||
bool expected)
|
||
{
|
||
var status = new FanucStateClientStatus(
|
||
state,
|
||
isFrameStale,
|
||
lastFrameAt: null,
|
||
reconnectAttemptCount: 0,
|
||
lastErrorMessage: null);
|
||
|
||
var actual = FanucControllerRuntime.ShouldUseStateFrame(status);
|
||
|
||
Assert.Equal(expected, actual);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证普通轨迹会先进入 ICSP 规划，并沿用 ICSP 对示教点数量的约束。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
|
||
void Act() =>
|
||
orchestrator.PlanOrdinaryTrajectory(
|
||
robot,
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
]);
|
||
|
||
Assert.Throws<ArgumentException>(Act);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证已上传飞拍轨迹会经过 self-adapt-icsp 并生成拍照触发时间轴。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||
|
||
Assert.True(bundle.Result.IsValid);
|
||
Assert.Single(bundle.Result.ShotEvents);
|
||
Assert.Single(bundle.Result.TriggerTimeline);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_AppliesPlanningSpeedScaleToLimits()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
|
||
var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 1.0);
|
||
var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 0.5);
|
||
|
||
Assert.True(
|
||
halfSpeed.Result.Duration.TotalSeconds > fullSpeed.Result.Duration.TotalSeconds * 1.9,
|
||
$"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_CacheKeyIncludesPlanningSpeedScale()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
var options = new FlyshotExecutionOptions(useCache: true);
|
||
|
||
var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 1.0);
|
||
var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 0.5);
|
||
|
||
Assert.False(halfSpeed.Result.UsedCache);
|
||
Assert.True(halfSpeed.Result.Duration > fullSpeed.Result.Duration);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRobotSettingsForHoldCycles()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
var settings = new CompatibilityRobotSettings(
|
||
useDo: true,
|
||
ioAddresses: [7, 8],
|
||
ioKeepCycles: 4,
|
||
triggerSampleIndexOffsetCycles: 7,
|
||
accLimitScale: 1.0,
|
||
jerkLimitScale: 1.0,
|
||
adaptIcspTryNum: 5);
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
|
||
|
||
var doEvent = Assert.Single(bundle.Result.TriggerTimeline);
|
||
Assert.Equal(4, doEvent.HoldCycles);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
var settings = new CompatibilityRobotSettings(
|
||
useDo: false,
|
||
ioAddresses: [7, 8],
|
||
ioKeepCycles: 4,
|
||
triggerSampleIndexOffsetCycles: 7,
|
||
accLimitScale: 1.0,
|
||
jerkLimitScale: 1.0,
|
||
adaptIcspTryNum: 5);
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
|
||
|
||
Assert.Single(bundle.Result.ShotEvents);
|
||
Assert.Empty(bundle.Result.TriggerTimeline);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证普通轨迹规划后会生成稠密关节采样序列。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_ReturnsDenseJointTrajectory()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
|
||
var bundle = orchestrator.PlanOrdinaryTrajectory(
|
||
robot,
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
]);
|
||
|
||
Assert.NotNull(bundle.Result.DenseJointTrajectory);
|
||
Assert.NotEmpty(bundle.Result.DenseJointTrajectory);
|
||
|
||
// 验证时间单调递增。
|
||
var times = bundle.Result.DenseJointTrajectory.Select(static row => row[0]).ToArray();
|
||
for (var i = 1; i < times.Length; i++)
|
||
{
|
||
Assert.True(times[i] > times[i - 1], $"采样时间点应在索引 {i} 处单调递增。");
|
||
}
|
||
|
||
// 验证每行包含时间 + 6 个关节值。
|
||
Assert.All(bundle.Result.DenseJointTrajectory, row => Assert.Equal(7, row.Count));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍轨迹规划后的稠密采样时间轴与伺服周期一致。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_DenseTrajectoryUsesServoPeriod()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||
|
||
Assert.NotNull(bundle.Result.DenseJointTrajectory);
|
||
Assert.True(bundle.Result.DenseJointTrajectory.Count > 1);
|
||
|
||
// 采样周期应为 8ms（伺服周期）。
|
||
var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
|
||
Assert.Equal(0.008, firstDt, precision: 3);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证可直接使用宿主输出目录 Config/RobotConfig.json 中的真实 UTTC_MS11 示教点，
|
||
/// 在 1x 规划速度下生成一条完整飞拍轨迹。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_GeneratesCompleteTrajectoryAtOneX()
|
||
{
|
||
var fixture = LoadUttcMs11RuntimeFixture();
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||
fixture.Robot,
|
||
fixture.Uploaded,
|
||
settings: EnableSmoothStartStopTiming(fixture.Settings),
|
||
planningSpeedScale: 1.0);
|
||
|
||
Assert.Equal("UTTC_MS11", bundle.Result.ProgramName);
|
||
Assert.NotNull(bundle.Result.DenseJointTrajectory);
|
||
Assert.True(bundle.Result.DenseJointTrajectory.Count > fixture.Uploaded.Waypoints.Count);
|
||
Assert.True(bundle.Result.Duration.TotalSeconds > 0.0);
|
||
Assert.Equal(fixture.Uploaded.ShotFlags.Count(static flag => flag), bundle.Result.ShotEvents.Count);
|
||
Assert.Equal(fixture.Uploaded.ShotFlags.Count(static flag => flag), bundle.Result.TriggerTimeline.Count);
|
||
|
||
var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
|
||
Assert.Equal(fixture.Robot.ServoPeriod.TotalSeconds, firstDt, precision: 6);
|
||
|
||
AssertJointRadiansEqual(fixture.Uploaded.Waypoints[0], bundle.Result.DenseJointTrajectory[0].Skip(1).ToArray());
|
||
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 点整形前后的逐点对比数据，
|
||
/// 便于和现场报警时间段逐项核对关节、速度、加速度与跃度。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ExportsEdgeComparisonAtOneX()
|
||
{
|
||
const int edgePointCount = 10;
|
||
var fixture = LoadUttcMs11RuntimeFixture();
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||
fixture.Robot,
|
||
fixture.Uploaded,
|
||
settings: fixture.Settings,
|
||
planningSpeedScale: 1.0);
|
||
var rawDense = TrajectorySampler.SampleJointTrajectory(
|
||
bundle.PlannedTrajectory,
|
||
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
|
||
var shapedDense = bundle.Result.DenseJointTrajectory!;
|
||
|
||
var outputDir = Path.Combine(
|
||
fixture.ConfigRoot,
|
||
"Data",
|
||
"UTTC_MS11_EdgeShapeDebug_1x",
|
||
DateTime.Now.ToString("yyyyMMdd_HHmmss_fff", CultureInfo.InvariantCulture));
|
||
Directory.CreateDirectory(outputDir);
|
||
|
||
var leadingPath = Path.Combine(outputDir, "Leading10Comparison.csv");
|
||
var trailingPath = Path.Combine(outputDir, "Trailing10Comparison.csv");
|
||
var summaryPath = Path.Combine(outputDir, "Summary.txt");
|
||
|
||
WriteEdgeComparisonCsv(leadingPath, rawDense, shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count));
|
||
WriteEdgeComparisonCsv(
|
||
trailingPath,
|
||
rawDense,
|
||
shapedDense,
|
||
Math.Max(0, shapedDense.Count - edgePointCount),
|
||
Math.Min(edgePointCount, shapedDense.Count));
|
||
|
||
var summaryLines = new List<string>
|
||
{
|
||
$"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)}",
|
||
$"leading_first_step_shaped_deg={ComputeStepDegrees(shapedDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"trailing_last_step_raw_deg={ComputeStepDegrees(rawDense, rawDense.Count - 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"trailing_last_step_shaped_deg={ComputeStepDegrees(shapedDense, shapedDense.Count - 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"leading_window_max_abs_raw_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(rawDense, 0, Math.Min(edgePointCount, rawDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"leading_window_max_abs_shaped_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"trailing_window_max_abs_raw_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(rawDense, Math.Max(0, rawDense.Count - edgePointCount), Math.Min(edgePointCount, rawDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"trailing_window_max_abs_shaped_jerk_deg_s3={ComputeWindowMaxAbsJerkDegrees(shapedDense, Math.Max(0, shapedDense.Count - edgePointCount), Math.Min(edgePointCount, shapedDense.Count)).ToString("F6", CultureInfo.InvariantCulture)}",
|
||
$"leading_csv={leadingPath}",
|
||
$"trailing_csv={trailingPath}"
|
||
};
|
||
|
||
File.WriteAllLines(summaryPath, summaryLines);
|
||
|
||
Assert.True(File.Exists(leadingPath));
|
||
Assert.True(File.Exists(trailingPath));
|
||
Assert.True(File.Exists(summaryPath));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会降低首尾窗口最大跃度，而不是只把第一步压小。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ReducesEdgeWindowJerkAtOneX()
|
||
{
|
||
const int edgePointCount = 10;
|
||
var fixture = LoadUttcMs11RuntimeFixture();
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||
fixture.Robot,
|
||
fixture.Uploaded,
|
||
settings: EnableSmoothStartStopTiming(fixture.Settings),
|
||
planningSpeedScale: 1.0);
|
||
var rawDense = TrajectorySampler.SampleJointTrajectory(
|
||
bundle.PlannedTrajectory,
|
||
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
|
||
var shapedDense = bundle.Result.DenseJointTrajectory!;
|
||
|
||
var rawLeadingMaxJerk = ComputeWindowMaxAbsJerkDegrees(rawDense, 0, Math.Min(edgePointCount, rawDense.Count));
|
||
var shapedLeadingMaxJerk = ComputeWindowMaxAbsJerkDegrees(shapedDense, 0, Math.Min(edgePointCount, shapedDense.Count));
|
||
var rawTrailingMaxJerk = ComputeWindowMaxAbsJerkDegrees(rawDense, Math.Max(0, rawDense.Count - edgePointCount), Math.Min(edgePointCount, rawDense.Count));
|
||
var shapedTrailingMaxJerk = ComputeWindowMaxAbsJerkDegrees(shapedDense, Math.Max(0, shapedDense.Count - edgePointCount), Math.Min(edgePointCount, shapedDense.Count));
|
||
|
||
Assert.True(
|
||
shapedLeadingMaxJerk < rawLeadingMaxJerk,
|
||
$"首段最大跃度应下降，raw={rawLeadingMaxJerk:F6}, shaped={shapedLeadingMaxJerk:F6}");
|
||
Assert.True(
|
||
shapedTrailingMaxJerk < rawTrailingMaxJerk,
|
||
$"尾段最大跃度应下降，raw={rawTrailingMaxJerk:F6}, shaped={shapedTrailingMaxJerk:F6}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会缩小首尾第一步，
|
||
/// 但不能把首尾直接压成静止平台，否则现场执行会变成先停住再起步。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ReducesEdgeStepWithoutFlatteningToZeroAtOneX()
|
||
{
|
||
var fixture = LoadUttcMs11RuntimeFixture();
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||
fixture.Robot,
|
||
fixture.Uploaded,
|
||
settings: EnableSmoothStartStopTiming(fixture.Settings),
|
||
planningSpeedScale: 1.0);
|
||
var rawDense = TrajectorySampler.SampleJointTrajectory(
|
||
bundle.PlannedTrajectory,
|
||
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
|
||
var shapedDense = bundle.Result.DenseJointTrajectory!;
|
||
|
||
var rawLeadingFirstStep = Math.Abs(ComputeStepDegrees(rawDense, 1, 0));
|
||
var shapedLeadingFirstStep = Math.Abs(ComputeStepDegrees(shapedDense, 1, 0));
|
||
var rawTrailingLastStep = Math.Abs(ComputeStepDegrees(rawDense, rawDense.Count - 1, 0));
|
||
var shapedTrailingLastStep = Math.Abs(ComputeStepDegrees(shapedDense, shapedDense.Count - 1, 0));
|
||
|
||
Assert.True(
|
||
shapedLeadingFirstStep < rawLeadingFirstStep,
|
||
$"首段第一步应变小，raw={rawLeadingFirstStep:F6}, shaped={shapedLeadingFirstStep:F6}");
|
||
Assert.True(
|
||
shapedTrailingLastStep < rawTrailingLastStep,
|
||
$"尾段最后一步应变小，raw={rawTrailingLastStep:F6}, shaped={shapedTrailingLastStep:F6}");
|
||
Assert.True(
|
||
shapedLeadingFirstStep > 1e-6,
|
||
$"首段第一步不应被压成 0，actual={shapedLeadingFirstStep:F9}");
|
||
Assert.True(
|
||
shapedTrailingLastStep > 1e-6,
|
||
$"尾段最后一步不应被压成 0，actual={shapedTrailingLastStep:F9}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 对比真实可运行轨迹前 0.04s 反推出的局部三次边界特征与当前首段样条系数，
|
||
/// 用于判断当前 clamped-zero 边界是否已经和现场可运行轨迹明显偏离。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromHostRuntimeRobotConfig_ExportsLeadingBoundaryComparison()
|
||
{
|
||
var fixture = LoadUttcMs11RuntimeFixture();
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||
fixture.Robot,
|
||
fixture.Uploaded,
|
||
settings: fixture.Settings,
|
||
planningSpeedScale: 1.0);
|
||
var applyMethod = typeof(ControllerClientTrajectoryOrchestrator).GetMethod(
|
||
"ApplySmoothStartStopTiming",
|
||
BindingFlags.Static | BindingFlags.NonPublic);
|
||
Assert.NotNull(applyMethod);
|
||
var executionTrajectory = Assert.IsType<PlannedTrajectory>(applyMethod!.Invoke(null, [bundle.PlannedTrajectory]));
|
||
var spline = new CubicSplineInterpolator(
|
||
executionTrajectory.WaypointTimes.ToArray(),
|
||
executionTrajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray());
|
||
|
||
var comparisonPath = Path.Combine(
|
||
fixture.ConfigRoot,
|
||
"Data",
|
||
"UTTC_MS11_EdgeShapeDebug_1x",
|
||
DateTime.Now.ToString("yyyyMMdd_HHmmss_fff", CultureInfo.InvariantCulture),
|
||
"LeadingBoundaryComparison.txt");
|
||
Directory.CreateDirectory(Path.GetDirectoryName(comparisonPath)!);
|
||
|
||
var realPath = Path.Combine(
|
||
TestRobotFactory.GetReplacementRoot(),
|
||
"..",
|
||
"Rvbust",
|
||
"前两个点正常 飞拍失败的运行",
|
||
"真实可运行的飞拍轨迹点位JointDetialTraj.txt");
|
||
var realRows = File.ReadLines(realPath)
|
||
.TakeWhile(static line => !string.IsNullOrWhiteSpace(line))
|
||
.Select(ParseSpaceSeparatedDoubles)
|
||
.Take(4)
|
||
.ToArray();
|
||
Assert.Equal(4, realRows.Length);
|
||
|
||
var fieldH = typeof(CubicSplineInterpolator).GetField("_h", BindingFlags.Instance | BindingFlags.NonPublic);
|
||
var fieldA = typeof(CubicSplineInterpolator).GetField("_a", BindingFlags.Instance | BindingFlags.NonPublic);
|
||
var fieldB = typeof(CubicSplineInterpolator).GetField("_b", BindingFlags.Instance | BindingFlags.NonPublic);
|
||
var fieldC = typeof(CubicSplineInterpolator).GetField("_c", BindingFlags.Instance | BindingFlags.NonPublic);
|
||
Assert.NotNull(fieldH);
|
||
Assert.NotNull(fieldA);
|
||
Assert.NotNull(fieldB);
|
||
Assert.NotNull(fieldC);
|
||
|
||
var segmentDurations = Assert.IsType<double[]>(fieldH!.GetValue(spline));
|
||
var coeffA = Assert.IsType<double[][]>(fieldA!.GetValue(spline));
|
||
var coeffB = Assert.IsType<double[][]>(fieldB!.GetValue(spline));
|
||
var coeffC = Assert.IsType<double[][]>(fieldC!.GetValue(spline));
|
||
|
||
var lines = new List<string>
|
||
{
|
||
$"real_path={Path.GetFullPath(realPath)}",
|
||
$"current_segment0_h_seconds={segmentDurations[0].ToString("F9", CultureInfo.InvariantCulture)}",
|
||
$"real_window_t0={realRows[0][0].ToString("F9", CultureInfo.InvariantCulture)}",
|
||
$"real_window_t1={realRows[1][0].ToString("F9", CultureInfo.InvariantCulture)}",
|
||
$"real_window_t2={realRows[2][0].ToString("F9", CultureInfo.InvariantCulture)}",
|
||
$"real_window_t3={realRows[3][0].ToString("F9", CultureInfo.InvariantCulture)}"
|
||
};
|
||
|
||
for (var jointIndex = 0; jointIndex < 6; jointIndex++)
|
||
{
|
||
var currentStartVelocityDeg = RadiansToDegrees(coeffC[0][jointIndex]);
|
||
var currentStartAccelerationDeg = RadiansToDegrees(2.0 * coeffB[0][jointIndex]);
|
||
var currentJerkDeg = RadiansToDegrees(6.0 * coeffA[0][jointIndex]);
|
||
var currentEndVelocityDeg = RadiansToDegrees(
|
||
(3.0 * coeffA[0][jointIndex] * segmentDurations[0] * segmentDurations[0])
|
||
+ (2.0 * coeffB[0][jointIndex] * segmentDurations[0])
|
||
+ coeffC[0][jointIndex]);
|
||
|
||
var localRealFit = FitCubicFromFirstFourRows(realRows, jointIndex + 1);
|
||
var realStartVelocityDeg = localRealFit.Coefficient1;
|
||
var realStartAccelerationDeg = 2.0 * localRealFit.Coefficient2;
|
||
var realJerkDeg = 6.0 * localRealFit.Coefficient3;
|
||
var realEndVelocityDeg = (3.0 * localRealFit.Coefficient3 * localRealFit.LastLocalTime * localRealFit.LastLocalTime)
|
||
+ (2.0 * localRealFit.Coefficient2 * localRealFit.LastLocalTime)
|
||
+ localRealFit.Coefficient1;
|
||
|
||
lines.Add(
|
||
$"J{jointIndex + 1}: current_start_v_deg_s={currentStartVelocityDeg:F9}, real_start_v_deg_s={realStartVelocityDeg:F9}, current_start_a_deg_s2={currentStartAccelerationDeg:F9}, real_start_a_deg_s2={realStartAccelerationDeg:F9}, current_jerk_deg_s3={currentJerkDeg:F9}, real_jerk_deg_s3={realJerkDeg:F9}, current_end_v_deg_s={currentEndVelocityDeg:F9}, real_end_v_deg_s={realEndVelocityDeg:F9}");
|
||
}
|
||
|
||
File.WriteAllLines(comparisonPath, lines);
|
||
Assert.True(File.Exists(comparisonPath));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍规划结果会应用平滑起停时间轴，而不是直接复用原始线性时间轴采样。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_ShapesDenseTrajectoryEdges()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||
var rawDense = TrajectorySampler.SampleJointTrajectory(
|
||
bundle.PlannedTrajectory,
|
||
samplePeriod: robot.ServoPeriod.TotalSeconds);
|
||
|
||
Assert.NotNull(bundle.Result.DenseJointTrajectory);
|
||
Assert.True(bundle.Result.DenseJointTrajectory.Count > 4);
|
||
|
||
var dense = bundle.Result.DenseJointTrajectory;
|
||
Assert.NotEqual(rawDense[1][1], dense[1][1]);
|
||
Assert.NotEqual(rawDense[^2][1], dense[^2][1]);
|
||
Assert.Equal(rawDense[0][1], dense[0][1], precision: 6);
|
||
Assert.Equal(rawDense[^1][1], dense[^1][1], precision: 6);
|
||
Assert.Equal(rawDense.Count, dense.Count);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证旧的首尾整形工具在独立调用时仍会改动首尾窗口，供对比和回归保留。
|
||
/// </summary>
|
||
[Fact]
|
||
public void FlyshotTrajectoryEdgeShaper_ShapesLeadingAndTrailingStepsWithoutFlatteningMiddleSection()
|
||
{
|
||
var denseTrajectory = new IReadOnlyList<double>[]
|
||
{
|
||
[0.000, DegreesToRadians(60.546226), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.008, DegreesToRadians(60.541482), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.016, DegreesToRadians(60.536738), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.024, DegreesToRadians(60.531994), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.032, DegreesToRadians(60.527250), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.040, DegreesToRadians(60.522506), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.048, DegreesToRadians(60.517762), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.056, DegreesToRadians(60.513018), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.064, DegreesToRadians(60.508274), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.072, DegreesToRadians(60.503530), 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.080, DegreesToRadians(60.498786), 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
};
|
||
|
||
var shaped = FlyshotTrajectoryEdgeShaper.ShapeDenseJointTrajectory(denseTrajectory);
|
||
|
||
Assert.Equal(denseTrajectory.Length, shaped.Count);
|
||
Assert.Equal(denseTrajectory[0][0], shaped[0][0], precision: 6);
|
||
Assert.Equal(denseTrajectory[^1][0], shaped[^1][0], precision: 6);
|
||
Assert.Equal(denseTrajectory[0][1], shaped[0][1], precision: 12);
|
||
Assert.Equal(denseTrajectory[^1][1], shaped[^1][1], precision: 12);
|
||
|
||
var firstStepDegrees = Math.Abs(RadiansToDegrees(shaped[1][1] - shaped[0][1]));
|
||
var lastStepDegrees = Math.Abs(RadiansToDegrees(shaped[^1][1] - shaped[^2][1]));
|
||
var originalFirstStepDegrees = Math.Abs(RadiansToDegrees(denseTrajectory[1][1] - denseTrajectory[0][1]));
|
||
var originalLastStepDegrees = Math.Abs(RadiansToDegrees(denseTrajectory[^1][1] - denseTrajectory[^2][1]));
|
||
Assert.NotEqual(originalFirstStepDegrees, firstStepDegrees);
|
||
Assert.NotEqual(originalLastStepDegrees, lastStepDegrees);
|
||
|
||
var middleStepDegrees = Math.Abs(RadiansToDegrees(shaped[5][1] - shaped[4][1]));
|
||
Assert.True(middleStepDegrees > 0.002, $"中段单步变化不应被整体压平，actual={middleStepDegrees:F6}deg");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍首尾整形只覆盖首尾各 10 个点，锚点之外的中段采样保持不变。
|
||
/// </summary>
|
||
[Fact]
|
||
public void FlyshotTrajectoryEdgeShaper_ShapesOnlyLeadingAndTrailingTenPoints()
|
||
{
|
||
var denseTrajectory = Enumerable.Range(0, 31)
|
||
.Select(index => (IReadOnlyList<double>)
|
||
[
|
||
index * 0.008,
|
||
DegreesToRadians(60.0 - index),
|
||
DegreesToRadians(index * 0.1),
|
||
DegreesToRadians(-index * 0.5),
|
||
DegreesToRadians(index * 0.02),
|
||
DegreesToRadians(index * 0.2),
|
||
DegreesToRadians(index * 0.05)
|
||
])
|
||
.ToArray();
|
||
|
||
var shaped = FlyshotTrajectoryEdgeShaper.ShapeDenseJointTrajectory(denseTrajectory);
|
||
|
||
Assert.Equal(denseTrajectory.Length, shaped.Count);
|
||
|
||
Assert.NotEqual(denseTrajectory[1][1], shaped[1][1]);
|
||
Assert.NotEqual(denseTrajectory[9][1], shaped[9][1]);
|
||
Assert.Equal(denseTrajectory[10][1], shaped[10][1], precision: 12);
|
||
Assert.Equal(denseTrajectory[11][1], shaped[11][1], precision: 12);
|
||
Assert.Equal(denseTrajectory[19][1], shaped[19][1], precision: 12);
|
||
Assert.Equal(denseTrajectory[20][1], shaped[20][1], precision: 12);
|
||
Assert.NotEqual(denseTrajectory[21][1], shaped[21][1]);
|
||
Assert.NotEqual(denseTrajectory[29][1], shaped[29][1]);
|
||
Assert.Equal(denseTrajectory[30][1], shaped[30][1], precision: 12);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
|
||
{
|
||
var service = TestRobotFactory.CreateCompatService();
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: true);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
|
||
void Act() =>
|
||
service.ExecuteTrajectory(
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
]);
|
||
|
||
Assert.Throws<ArgumentException>(Act);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectoryByName_MoveToStartWaitsBeforeFlyshot()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = configRoot
|
||
};
|
||
var runtime = new DelayedCompletionControllerRuntime(
|
||
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: false);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||
|
||
service.ExecuteTrajectoryByName(
|
||
"demo-flyshot",
|
||
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false));
|
||
|
||
Assert.True(runtime.ExecuteCalls.Count >= 2);
|
||
Assert.NotNull(runtime.ExecuteCalls[0].Result.DenseJointTrajectory);
|
||
Assert.True(runtime.ExecuteCalls[0].Result.DenseJointTrajectory!.Count > 1);
|
||
Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证正式飞拍前若真实反馈仍未接近起点，则兼容层会拒绝继续执行。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectoryByName_RejectsFlyshotWhenFeedbackIsNotNearStartAfterMoveToStart()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = configRoot
|
||
};
|
||
var runtime = new StickyFeedbackControllerRuntime(
|
||
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(20));
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: false);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||
|
||
var exception = Assert.Throws<InvalidOperationException>(() =>
|
||
service.ExecuteTrajectoryByName(
|
||
"demo-flyshot",
|
||
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false)));
|
||
|
||
Assert.Contains("not near flyshot start", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||
Assert.Single(runtime.ExecuteCalls);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectoryByName_WaitTrueWaitsForFlyshotCompletion()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = configRoot
|
||
};
|
||
var runtime = new DelayedCompletionControllerRuntime(
|
||
initialJointPositions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: false);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||
|
||
service.ExecuteTrajectoryByName(
|
||
"demo-flyshot",
|
||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true));
|
||
|
||
Assert.Single(runtime.ExecuteCalls);
|
||
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
File.WriteAllText(
|
||
Path.Combine(configRoot, "RobotConfig.json"),
|
||
"""
|
||
{
|
||
"robot": {
|
||
"use_do": true,
|
||
"io_addr": [7, 8],
|
||
"io_keep_cycles": 4,
|
||
"acc_limit": 0.5,
|
||
"jerk_limit": 0.25,
|
||
"adapt_icsp_try_num": 3
|
||
},
|
||
"flying_shots": {}
|
||
}
|
||
""");
|
||
|
||
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");
|
||
|
||
var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
|
||
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
|
||
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_IsFlyshotTrajectoryValid_SaveTrajectoryExportsPlannedData()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
WriteRobotConfigWithDemoTrajectory(configRoot);
|
||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
new RecordingControllerRuntime(),
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
|
||
var valid = service.IsFlyshotTrajectoryValid(
|
||
out var duration,
|
||
"demo-flyshot",
|
||
method: "icsp",
|
||
saveTrajectory: true);
|
||
|
||
var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
|
||
Assert.True(valid);
|
||
Assert.True(duration > TimeSpan.Zero);
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "JointTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "CartTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.json")));
|
||
Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt")));
|
||
Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "CartDetialTraj.txt")));
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 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");
|
||
var shotEventsPath = Path.Combine(outputDir, "ShotEvents.json");
|
||
Assert.True(File.Exists(pointsPath));
|
||
Assert.True(File.Exists(timingPath));
|
||
Assert.True(File.Exists(shotEventsPath));
|
||
|
||
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
|
||
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
|
||
var shotEventsJson = File.ReadAllText(shotEventsPath);
|
||
var executionDuration = double.Parse(
|
||
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
|
||
CultureInfo.InvariantCulture);
|
||
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (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);
|
||
Assert.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson);
|
||
Assert.Contains("\"selected_sample_index\"", shotEventsJson);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图，
|
||
/// 而不是再次从 PlannedTrajectory 独立重采样。
|
||
/// </summary>
|
||
[Fact]
|
||
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample()
|
||
{
|
||
var fixture = LoadUttcMs11RuntimeFixture();
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||
var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader());
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||
fixture.Robot,
|
||
fixture.Uploaded,
|
||
settings: EnableSmoothStartStopTiming(fixture.Settings),
|
||
planningSpeedScale: 1.0);
|
||
|
||
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0);
|
||
|
||
var outputDir = Path.Combine(configRoot, "Data", "UTTC_MS11");
|
||
var exportedRows = File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt"))
|
||
.Select(ParseSpaceSeparatedDoubles)
|
||
.ToArray();
|
||
var expectedRows = DownsampleDenseRows(
|
||
bundle.Result.DenseJointTrajectory!,
|
||
samplePeriodSeconds: 0.016)
|
||
.Select(static row => row.ToArray())
|
||
.ToArray();
|
||
|
||
Assert.Equal(expectedRows.Length, exportedRows.Length);
|
||
for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++)
|
||
{
|
||
Assert.Equal(expectedRows[rowIndex].Length, exportedRows[rowIndex].Length);
|
||
for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++)
|
||
{
|
||
Assert.Equal(expectedRows[rowIndex][columnIndex], exportedRows[rowIndex][columnIndex], precision: 6);
|
||
}
|
||
}
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。
|
||
/// </summary>
|
||
private static string CreateTempConfigRoot()
|
||
{
|
||
var configRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"), "Config");
|
||
var modelDir = Path.Combine(configRoot, "Models");
|
||
Directory.CreateDirectory(modelDir);
|
||
|
||
var sourceModel = Path.Combine(
|
||
TestRobotFactory.GetReplacementRoot(),
|
||
"Config",
|
||
"Models",
|
||
"LR_Mate_200iD_7L.json");
|
||
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));
|
||
|
||
return configRoot;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。
|
||
/// </summary>
|
||
/// <param name="configRoot">测试运行配置根。</param>
|
||
private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
|
||
{
|
||
File.WriteAllText(
|
||
Path.Combine(configRoot, "RobotConfig.json"),
|
||
"""
|
||
{
|
||
"robot": {
|
||
"use_do": true,
|
||
"io_addr": [7, 8],
|
||
"io_keep_cycles": 2,
|
||
"acc_limit": 1.0,
|
||
"jerk_limit": 1.0,
|
||
"adapt_icsp_try_num": 5
|
||
},
|
||
"flying_shots": {
|
||
"demo-flyshot": {
|
||
"traj_waypoints": [
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
],
|
||
"shot_flags": [false, true, false, false],
|
||
"offset_values": [0, 1, 0, 0],
|
||
"addr": [[], [7, 8], [], []]
|
||
}
|
||
}
|
||
}
|
||
""");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 角度转弧度，供轨迹整形测试构造输入。
|
||
/// </summary>
|
||
private static double DegreesToRadians(double degrees)
|
||
{
|
||
return degrees * Math.PI / 180.0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 弧度转角度，供轨迹整形测试验证单步变化量。
|
||
/// </summary>
|
||
private static double RadiansToDegrees(double radians)
|
||
{
|
||
return radians * 180.0 / Math.PI;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证稠密轨迹端点仍与原始示教点一致。
|
||
/// </summary>
|
||
private static void AssertJointRadiansEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualRadians)
|
||
{
|
||
Assert.Equal(expectedRadians.Count, actualRadians.Count);
|
||
for (var index = 0; index < expectedRadians.Count; index++)
|
||
{
|
||
Assert.Equal(expectedRadians[index], actualRadians[index], precision: 6);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。
|
||
/// </summary>
|
||
private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
|
||
{
|
||
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(
|
||
name: program.Name,
|
||
waypoints: program.Waypoints.Select(static waypoint => waypoint.Positions),
|
||
shotFlags: program.ShotFlags,
|
||
offsetValues: program.OffsetValues,
|
||
addressGroups: program.AddressGroups.Select(static group => group.Addresses));
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = configRoot
|
||
};
|
||
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
|
||
.LoadProfile("FANUC_LR_Mate_200iD", loaded.Robot.AccLimitScale, loaded.Robot.JerkLimitScale);
|
||
|
||
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,
|
||
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles,
|
||
accLimitScale: settings.AccLimitScale,
|
||
jerkLimitScale: settings.JerkLimitScale,
|
||
adaptIcspTryNum: settings.AdaptIcspTryNum,
|
||
planningSpeedScale: settings.PlanningSpeedScale,
|
||
smoothStartStopTiming: true);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。
|
||
/// </summary>
|
||
private static void WriteEdgeComparisonCsv(
|
||
string path,
|
||
IReadOnlyList<IReadOnlyList<double>> rawDense,
|
||
IReadOnlyList<IReadOnlyList<double>> shapedDense,
|
||
int startIndex,
|
||
int count)
|
||
{
|
||
var lines = new List<string>
|
||
{
|
||
BuildEdgeComparisonHeader()
|
||
};
|
||
|
||
for (var index = startIndex; index < startIndex + count && index < rawDense.Count && index < shapedDense.Count; index++)
|
||
{
|
||
var columns = new List<string>
|
||
{
|
||
index.ToString(CultureInfo.InvariantCulture),
|
||
rawDense[index][0].ToString("F6", CultureInfo.InvariantCulture)
|
||
};
|
||
|
||
AppendJointColumns(columns, "raw_j", index, rawDense, static (rows, rowIndex, jointIndex) => RadiansToDegrees(rows[rowIndex][jointIndex + 1]));
|
||
AppendJointColumns(columns, "shaped_j", index, shapedDense, static (rows, rowIndex, jointIndex) => RadiansToDegrees(rows[rowIndex][jointIndex + 1]));
|
||
AppendJointColumns(columns, "raw_step", index, rawDense, ComputeStepDegrees);
|
||
AppendJointColumns(columns, "shaped_step", index, shapedDense, ComputeStepDegrees);
|
||
AppendJointColumns(columns, "raw_velocity", index, rawDense, ComputeVelocityDegrees);
|
||
AppendJointColumns(columns, "shaped_velocity", index, shapedDense, ComputeVelocityDegrees);
|
||
AppendJointColumns(columns, "raw_acceleration", index, rawDense, ComputeAccelerationDegrees);
|
||
AppendJointColumns(columns, "shaped_acceleration", index, shapedDense, ComputeAccelerationDegrees);
|
||
AppendJointColumns(columns, "raw_jerk", index, rawDense, ComputeJerkDegrees);
|
||
AppendJointColumns(columns, "shaped_jerk", index, shapedDense, ComputeJerkDegrees);
|
||
lines.Add(string.Join(",", columns));
|
||
}
|
||
|
||
File.WriteAllLines(path, lines);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造首尾整形逐点对比 CSV 表头。
|
||
/// </summary>
|
||
private static string BuildEdgeComparisonHeader()
|
||
{
|
||
var columns = new List<string>
|
||
{
|
||
"sample_index",
|
||
"time_seconds"
|
||
};
|
||
|
||
AppendJointHeader(columns, "raw_j");
|
||
AppendJointHeader(columns, "shaped_j");
|
||
AppendJointHeader(columns, "raw_step");
|
||
AppendJointHeader(columns, "shaped_step");
|
||
AppendJointHeader(columns, "raw_velocity");
|
||
AppendJointHeader(columns, "shaped_velocity");
|
||
AppendJointHeader(columns, "raw_acceleration");
|
||
AppendJointHeader(columns, "shaped_acceleration");
|
||
AppendJointHeader(columns, "raw_jerk");
|
||
AppendJointHeader(columns, "shaped_jerk");
|
||
|
||
return string.Join(",", columns);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 追加 J1..J6 表头列名。
|
||
/// </summary>
|
||
private static void AppendJointHeader(List<string> columns, string prefix)
|
||
{
|
||
for (var jointIndex = 1; jointIndex <= 6; jointIndex++)
|
||
{
|
||
columns.Add($"{prefix}_j{jointIndex}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 追加某一行对应的六轴数值列。
|
||
/// </summary>
|
||
private static void AppendJointColumns(
|
||
List<string> columns,
|
||
string prefix,
|
||
int rowIndex,
|
||
IReadOnlyList<IReadOnlyList<double>> rows,
|
||
Func<IReadOnlyList<IReadOnlyList<double>>, int, int, double> selector)
|
||
{
|
||
for (var jointIndex = 0; jointIndex < 6; jointIndex++)
|
||
{
|
||
columns.Add(selector(rows, rowIndex, jointIndex).ToString("F6", CultureInfo.InvariantCulture));
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算某一行相对上一行的单步角度变化量。
|
||
/// </summary>
|
||
private static double ComputeStepDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
|
||
{
|
||
if (rowIndex <= 0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
return RadiansToDegrees(rows[rowIndex][jointIndex + 1] - rows[rowIndex - 1][jointIndex + 1]);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算某一行的角速度，单位 deg/s。
|
||
/// </summary>
|
||
private static double ComputeVelocityDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
|
||
{
|
||
if (rowIndex <= 0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
|
||
if (dt <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
return ComputeStepDegrees(rows, rowIndex, jointIndex) / dt;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算某一行的角加速度，单位 deg/s^2。
|
||
/// </summary>
|
||
private static double ComputeAccelerationDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
|
||
{
|
||
if (rowIndex <= 1)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
|
||
if (dt <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
return (ComputeVelocityDegrees(rows, rowIndex, jointIndex) - ComputeVelocityDegrees(rows, rowIndex - 1, jointIndex)) / dt;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算某一行的角跃度，单位 deg/s^3。
|
||
/// </summary>
|
||
private static double ComputeJerkDegrees(IReadOnlyList<IReadOnlyList<double>> rows, int rowIndex, int jointIndex)
|
||
{
|
||
if (rowIndex <= 2)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var dt = rows[rowIndex][0] - rows[rowIndex - 1][0];
|
||
if (dt <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
return (ComputeAccelerationDegrees(rows, rowIndex, jointIndex) - ComputeAccelerationDegrees(rows, rowIndex - 1, jointIndex)) / dt;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 统计给定窗口内所有关节的最大绝对值跃度，便于快速看首尾尖峰是否被压低。
|
||
/// </summary>
|
||
private static double ComputeWindowMaxAbsJerkDegrees(
|
||
IReadOnlyList<IReadOnlyList<double>> rows,
|
||
int startIndex,
|
||
int count)
|
||
{
|
||
var maxAbsJerk = 0.0;
|
||
for (var rowIndex = startIndex; rowIndex < startIndex + count && rowIndex < rows.Count; rowIndex++)
|
||
{
|
||
for (var jointIndex = 0; jointIndex < 6; jointIndex++)
|
||
{
|
||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(ComputeJerkDegrees(rows, rowIndex, jointIndex)));
|
||
}
|
||
}
|
||
|
||
return maxAbsJerk;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析空格分隔的关节轨迹行。
|
||
/// </summary>
|
||
private static double[] ParseSpaceSeparatedDoubles(string line)
|
||
{
|
||
return line
|
||
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
|
||
.Select(static value => double.Parse(value, CultureInfo.InvariantCulture))
|
||
.ToArray();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将 8ms 执行稠密轨迹按目标周期抽稀为低频视图，并始终保留终点。
|
||
/// </summary>
|
||
private static IReadOnlyList<IReadOnlyList<double>> DownsampleDenseRows(
|
||
IReadOnlyList<IReadOnlyList<double>> denseRows,
|
||
double samplePeriodSeconds)
|
||
{
|
||
var result = new List<IReadOnlyList<double>>();
|
||
var epsilon = 1e-6;
|
||
var nextSampleTime = 0.0;
|
||
|
||
foreach (var row in denseRows)
|
||
{
|
||
var sampleTime = row[0];
|
||
if (sampleTime + epsilon < nextSampleTime)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0))
|
||
{
|
||
result.Add(row);
|
||
nextSampleTime += samplePeriodSeconds;
|
||
}
|
||
}
|
||
|
||
if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1]))
|
||
{
|
||
result.Add(denseRows[^1]);
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线，返回相对首点时间的系数。
|
||
/// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0，单位保持输入文件的角度制。
|
||
/// </summary>
|
||
private static LocalCubicFit FitCubicFromFirstFourRows(IReadOnlyList<double[]> rows, int valueColumn)
|
||
{
|
||
var t0 = rows[0][0];
|
||
var matrix = new double[4][];
|
||
var rhs = new double[4];
|
||
for (var index = 0; index < 4; index++)
|
||
{
|
||
var localTime = rows[index][0] - t0;
|
||
matrix[index] =
|
||
[
|
||
localTime * localTime * localTime,
|
||
localTime * localTime,
|
||
localTime,
|
||
1.0
|
||
];
|
||
rhs[index] = rows[index][valueColumn];
|
||
}
|
||
|
||
var solution = SolveLinearSystem4x4(matrix, rhs);
|
||
return new LocalCubicFit(
|
||
Coefficient3: solution[0],
|
||
Coefficient2: solution[1],
|
||
Coefficient1: solution[2],
|
||
Coefficient0: solution[3],
|
||
LastLocalTime: rows[3][0] - t0);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 用高斯消元求解 4x4 线性方程组。
|
||
/// </summary>
|
||
private static double[] SolveLinearSystem4x4(double[][] matrix, double[] rhs)
|
||
{
|
||
var a = matrix.Select(static row => row.ToArray()).ToArray();
|
||
var b = rhs.ToArray();
|
||
|
||
for (var pivot = 0; pivot < 4; pivot++)
|
||
{
|
||
var bestRow = pivot;
|
||
for (var row = pivot + 1; row < 4; row++)
|
||
{
|
||
if (Math.Abs(a[row][pivot]) > Math.Abs(a[bestRow][pivot]))
|
||
{
|
||
bestRow = row;
|
||
}
|
||
}
|
||
|
||
if (Math.Abs(a[bestRow][pivot]) <= 1e-12)
|
||
{
|
||
throw new InvalidOperationException("真实轨迹前 4 点无法稳定拟合局部三次曲线。");
|
||
}
|
||
|
||
if (bestRow != pivot)
|
||
{
|
||
(a[pivot], a[bestRow]) = (a[bestRow], a[pivot]);
|
||
(b[pivot], b[bestRow]) = (b[bestRow], b[pivot]);
|
||
}
|
||
|
||
var divisor = a[pivot][pivot];
|
||
for (var column = pivot; column < 4; column++)
|
||
{
|
||
a[pivot][column] /= divisor;
|
||
}
|
||
|
||
b[pivot] /= divisor;
|
||
|
||
for (var row = 0; row < 4; row++)
|
||
{
|
||
if (row == pivot)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var factor = a[row][pivot];
|
||
for (var column = pivot; column < 4; column++)
|
||
{
|
||
a[row][column] -= factor * a[pivot][column];
|
||
}
|
||
|
||
b[row] -= factor * b[pivot];
|
||
}
|
||
}
|
||
|
||
return b;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
|
||
/// </summary>
|
||
internal sealed record UttcMs11RuntimeFixture(
|
||
string ConfigRoot,
|
||
CompatibilityRobotSettings Settings,
|
||
ControllerClientCompatUploadedTrajectory Uploaded,
|
||
RobotProfile Robot);
|
||
|
||
/// <summary>
|
||
/// 记录基于真实轨迹前 4 点拟合得到的局部三次曲线系数。
|
||
/// </summary>
|
||
internal sealed record LocalCubicFit(
|
||
double Coefficient3,
|
||
double Coefficient2,
|
||
double Coefficient1,
|
||
double Coefficient0,
|
||
double LastLocalTime);
|
||
|
||
/// <summary>
|
||
/// 为运行时编排测试构造稳定的最小领域对象。
|
||
/// </summary>
|
||
internal static class TestRobotFactory
|
||
{
|
||
/// <summary>
|
||
/// 构造六轴测试机器人配置，避免运行时测试依赖真实 .robot 文件。
|
||
/// </summary>
|
||
/// <returns>可用于规划和运行时状态校验的机器人配置。</returns>
|
||
public static RobotProfile CreateRobotProfile()
|
||
{
|
||
return new RobotProfile(
|
||
name: "TestRobot",
|
||
modelPath: "Models/Test.robot",
|
||
degreesOfFreedom: 6,
|
||
jointLimits: Enumerable.Range(1, 6)
|
||
.Select(static index => new JointLimit($"J{index}", 10.0, 20.0, 100.0))
|
||
.ToArray(),
|
||
jointCouplings: Array.Empty<JointCoupling>(),
|
||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造一条含单个拍照点的上传飞拍轨迹。
|
||
/// </summary>
|
||
/// <returns>可用于触发时间轴测试的上传轨迹。</returns>
|
||
public static ControllerClientCompatUploadedTrajectory CreateUploadedTrajectoryWithSingleShot()
|
||
{
|
||
return new ControllerClientCompatUploadedTrajectory(
|
||
name: "demo-flyshot",
|
||
waypoints:
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
],
|
||
shotFlags: [false, true, false, false],
|
||
offsetValues: [0, 1, 0, 0],
|
||
addressGroups:
|
||
[
|
||
Array.Empty<int>(),
|
||
[7, 8],
|
||
Array.Empty<int>(),
|
||
Array.Empty<int>()
|
||
]);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造一份真实依赖注入等价的兼容服务，覆盖运行时和编排器协作。
|
||
/// </summary>
|
||
/// <returns>可执行 ControllerClient 兼容语义的服务实例。</returns>
|
||
public static ControllerClientCompatService CreateCompatService()
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = GetConfigRoot()
|
||
};
|
||
|
||
return new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
new FanucControllerRuntime(),
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位 replacement 仓库内的运行配置根目录。
|
||
/// </summary>
|
||
/// <returns>当前仓库 Config 目录。</returns>
|
||
public static string GetConfigRoot()
|
||
{
|
||
return Path.Combine(GetReplacementRoot(), "Config");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
|
||
/// </summary>
|
||
/// <returns>replacement 仓库根目录。</returns>
|
||
public static string GetReplacementRoot()
|
||
{
|
||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||
while (current is not null)
|
||
{
|
||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||
{
|
||
return current.FullName;
|
||
}
|
||
|
||
current = current.Parent;
|
||
}
|
||
|
||
throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位父工作区根目录，供兼容服务加载真实机器人模型。
|
||
/// </summary>
|
||
/// <returns>父工作区根目录。</returns>
|
||
public 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.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 记录 ResetRobot 入参的测试运行时，用于验证兼容服务传递的机器人配置。
|
||
/// </summary>
|
||
internal sealed class RecordingControllerRuntime : IControllerRuntime
|
||
{
|
||
private double _speedRatio = 1.0;
|
||
|
||
/// <summary>
|
||
/// 获取最近一次 ResetRobot 收到的机器人配置。
|
||
/// </summary>
|
||
public RobotProfile? LastRobotProfile { get; private set; }
|
||
|
||
/// <inheritdoc />
|
||
public void ResetRobot(RobotProfile robot, string robotName)
|
||
{
|
||
LastRobotProfile = robot;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetActiveController(bool sim)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Connect(string robotIp)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Disconnect()
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void EnableRobot(int bufferSize)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void DisableRobot()
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void StopMove()
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public double GetSpeedRatio() => _speedRatio;
|
||
|
||
/// <inheritdoc />
|
||
public void SetSpeedRatio(double ratio)
|
||
{
|
||
_speedRatio = ratio;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
|
||
|
||
/// <inheritdoc />
|
||
public void SetTcp(double x, double y, double z)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public bool GetIo(int port, string ioType) => false;
|
||
|
||
/// <inheritdoc />
|
||
public void SetIo(int port, bool value, string ioType)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetJointPositions() => Array.Empty<double>();
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||
|
||
/// <inheritdoc />
|
||
public ControllerStateSnapshot GetSnapshot()
|
||
{
|
||
return new ControllerStateSnapshot(
|
||
capturedAt: DateTimeOffset.UtcNow,
|
||
connectionState: "Connected",
|
||
isEnabled: true,
|
||
isInMotion: false,
|
||
speedRatio: _speedRatio,
|
||
jointPositions: Array.Empty<double>(),
|
||
cartesianPose: Array.Empty<double>(),
|
||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||
{
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。
|
||
/// </summary>
|
||
internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
|
||
{
|
||
private readonly object _lock = new();
|
||
private readonly TimeSpan _firstMotionCompletionDelay;
|
||
private double[] _jointPositions;
|
||
private bool _isEnabled;
|
||
private bool _isInMotion;
|
||
private bool _firstMotionCompleted;
|
||
|
||
/// <summary>
|
||
/// 初始化可延迟完成第一段运动的测试运行时。
|
||
/// </summary>
|
||
/// <param name="initialJointPositions">运行时报告的初始关节位置。</param>
|
||
/// <param name="firstMotionCompletionDelay">第一段运动完成前保持忙碌的时间。</param>
|
||
public DelayedCompletionControllerRuntime(
|
||
IReadOnlyList<double> initialJointPositions,
|
||
TimeSpan firstMotionCompletionDelay)
|
||
{
|
||
_jointPositions = initialJointPositions.ToArray();
|
||
_firstMotionCompletionDelay = firstMotionCompletionDelay;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取所有 ExecuteTrajectory 调用记录。
|
||
/// </summary>
|
||
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
|
||
|
||
/// <summary>
|
||
/// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。
|
||
/// </summary>
|
||
public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; }
|
||
|
||
/// <inheritdoc />
|
||
public void ResetRobot(RobotProfile robot, string robotName)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetActiveController(bool sim)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Connect(string robotIp)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Disconnect()
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void EnableRobot(int bufferSize)
|
||
{
|
||
_isEnabled = true;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void DisableRobot()
|
||
{
|
||
_isEnabled = false;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void StopMove()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_isInMotion = false;
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public double GetSpeedRatio() => 1.0;
|
||
|
||
/// <inheritdoc />
|
||
public void SetSpeedRatio(double ratio)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
|
||
|
||
/// <inheritdoc />
|
||
public void SetTcp(double x, double y, double z)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public bool GetIo(int port, string ioType) => false;
|
||
|
||
/// <inheritdoc />
|
||
public void SetIo(int port, bool value, string ioType)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetJointPositions()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return _jointPositions.ToArray();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||
|
||
/// <inheritdoc />
|
||
public ControllerStateSnapshot GetSnapshot()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return new ControllerStateSnapshot(
|
||
capturedAt: DateTimeOffset.UtcNow,
|
||
connectionState: "Connected",
|
||
isEnabled: _isEnabled,
|
||
isInMotion: _isInMotion,
|
||
speedRatio: 1.0,
|
||
jointPositions: _jointPositions.ToArray(),
|
||
cartesianPose: Array.Empty<double>(),
|
||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||
{
|
||
lock (_lock)
|
||
{
|
||
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
|
||
if (ExecuteCalls.Count == 1)
|
||
{
|
||
_isInMotion = true;
|
||
_ = Task.Run(async () =>
|
||
{
|
||
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
|
||
lock (_lock)
|
||
{
|
||
_jointPositions = finalJointPositions.ToArray();
|
||
_isInMotion = false;
|
||
_firstMotionCompleted = true;
|
||
}
|
||
});
|
||
return;
|
||
}
|
||
|
||
if (!_firstMotionCompleted)
|
||
{
|
||
SecondTrajectoryStartedBeforeFirstMotionCompleted = true;
|
||
}
|
||
|
||
_jointPositions = finalJointPositions.ToArray();
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 模拟 move_to_start 逻辑完成后，真实反馈仍停留在旧位置的测试运行时。
|
||
/// </summary>
|
||
internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
|
||
{
|
||
private readonly object _lock = new();
|
||
private readonly TimeSpan _firstMotionCompletionDelay;
|
||
private double[] _jointPositions;
|
||
private bool _isEnabled;
|
||
private bool _isInMotion;
|
||
private bool _firstMotionCompleted;
|
||
|
||
public StickyFeedbackControllerRuntime(
|
||
IReadOnlyList<double> initialJointPositions,
|
||
TimeSpan firstMotionCompletionDelay)
|
||
{
|
||
_jointPositions = initialJointPositions.ToArray();
|
||
_firstMotionCompletionDelay = firstMotionCompletionDelay;
|
||
}
|
||
|
||
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
|
||
|
||
public void ResetRobot(RobotProfile robot, string robotName)
|
||
{
|
||
}
|
||
|
||
public void SetActiveController(bool sim)
|
||
{
|
||
}
|
||
|
||
public void Connect(string robotIp)
|
||
{
|
||
}
|
||
|
||
public void Disconnect()
|
||
{
|
||
}
|
||
|
||
public void EnableRobot(int bufferSize)
|
||
{
|
||
_isEnabled = true;
|
||
}
|
||
|
||
public void DisableRobot()
|
||
{
|
||
_isEnabled = false;
|
||
}
|
||
|
||
public void StopMove()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_isInMotion = false;
|
||
}
|
||
}
|
||
|
||
public double GetSpeedRatio() => 1.0;
|
||
|
||
public void SetSpeedRatio(double ratio)
|
||
{
|
||
}
|
||
|
||
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
|
||
|
||
public void SetTcp(double x, double y, double z)
|
||
{
|
||
}
|
||
|
||
public bool GetIo(int port, string ioType) => false;
|
||
|
||
public void SetIo(int port, bool value, string ioType)
|
||
{
|
||
}
|
||
|
||
public IReadOnlyList<double> GetJointPositions()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return _jointPositions.ToArray();
|
||
}
|
||
}
|
||
|
||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||
|
||
public ControllerStateSnapshot GetSnapshot()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return new ControllerStateSnapshot(
|
||
capturedAt: DateTimeOffset.UtcNow,
|
||
connectionState: "Connected",
|
||
isEnabled: _isEnabled,
|
||
isInMotion: _isInMotion,
|
||
speedRatio: 1.0,
|
||
jointPositions: _jointPositions.ToArray(),
|
||
cartesianPose: Array.Empty<double>(),
|
||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||
}
|
||
}
|
||
|
||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||
{
|
||
lock (_lock)
|
||
{
|
||
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
|
||
if (!_firstMotionCompleted)
|
||
{
|
||
_isInMotion = true;
|
||
_ = Task.Run(async () =>
|
||
{
|
||
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
|
||
lock (_lock)
|
||
{
|
||
// 模拟控制链逻辑上结束，但真实反馈仍卡在旧位置。
|
||
_isInMotion = false;
|
||
_firstMotionCompleted = true;
|
||
}
|
||
});
|
||
return;
|
||
}
|
||
|
||
_jointPositions = finalJointPositions.ToArray();
|
||
}
|
||
}
|
||
}
|