✨ feat(fanuc): 优化 J519 实时下发与飞拍起停整形

- 改为高优先级 J519 接收线程与复用缓冲区发送链路 - 增加稠密执行前的 J519 就绪重试与状态诊断 - 修正程序状态响应字段顺序与 EnableRobot 默认参数 - 为飞拍轨迹补充平滑起停时间轴与首尾整形验证 - 补充真实运行配置、报警窗口与边界对比测试 - 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
2026-05-06 22:37:31 +08:00
parent 783716ff44
commit 70b0ccd414
17 changed files with 1629 additions and 117 deletions
--- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
+++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
@@ -1,9 +1,13 @@
 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;

@@ -270,6 +274,364 @@ public sealed class RuntimeOrchestrationTests
        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: 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>
+    /// 使用运行时 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",
+            $"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: 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: 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>
@@ -564,8 +926,369 @@ public sealed class RuntimeOrchestrationTests
            }
            """);
    }
+
+    /// <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 configPath = Path.Combine(
+            TestRobotFactory.GetReplacementRoot(),
+            "src",
+            "Flyshot.Server.Host",
+            "bin",
+            "Debug",
+            "net8.0",
+            "Config",
+            "RobotConfig.json");
+        var configRoot = Path.GetDirectoryName(configPath)!;
+        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>
+    /// 把指定窗口内的整形前后逐点数据导出为 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>
+    /// 用真实可运行轨迹前 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>