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

- 改为高优先级 J519 接收线程与复用缓冲区发送链路
- 增加稠密执行前的 J519 就绪重试与状态诊断
- 修正程序状态响应字段顺序与 EnableRobot 默认参数
- 为飞拍轨迹补充平滑起停时间轴与首尾整形验证
- 补充真实运行配置、报警窗口与边界对比测试
- 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置

.gitignore

@@ -398,3 +398,5 @@ FodyWeavers.xsd
 *.sln.iml
 Config/Data/*
 .dotnet-home/*
+codex-dotnet-home/*
+.dotnet-sdk8/*

analysis/calc_jointdetail_acceleration.py

@@ -112,6 +112,23 @@ def read_joint_rows(path: Path) -> list[list[float]]:
     return rows
 
 
+def trim_rows_to_limit_count(rows: list[list[float]], limit_count: int) -> tuple[list[list[float]], str | None]:
+    joint_count = len(rows[0]) - 1
+    if joint_count == limit_count:
+        return rows, None
+
+    if joint_count < limit_count:
+        raise ValueError(f"Joint column count ({joint_count}) is smaller than robot limit count ({limit_count}).")
+
+    trimmed_rows = [row[: limit_count + 1] for row in rows]
+    ignored_joint_count = joint_count - limit_count
+    trim_note = (
+        f"ignored_joint_columns={ignored_joint_count} "
+        f"(using first {limit_count} joints out of {joint_count}; trailing columns treated as external axes/placeholders)"
+    )
+    return trimmed_rows, trim_note
+
+
 def infer_unit(rows: Iterable[list[float]], requested_unit: str) -> str:
     if requested_unit != "auto":
         return requested_unit
@@ -367,6 +384,7 @@ def build_report_text(
     unit: str,
     max_abs_joint: float,
     limit_source_text: str,
+    trim_note: str | None,
     velocity_peaks: list[PeakMetric],
     acceleration_peaks: list[PeakMetric],
     jerk_peaks: list[PeakMetric],
@@ -378,6 +396,11 @@ def build_report_text(
         f"joint_count={len(rows[0]) - 1}",
         f"inferred_unit={unit}",
         f"max_abs_joint_value={max_abs_joint:.6f}",
+    ]
+    if trim_note is not None:
+        lines.append(trim_note)
+    lines.extend(
+        [
             "",
             build_metric_section("Velocity Peaks", velocity_peaks, unit, "s", "s", "VelLimit(rad/s)"),
             "",
@@ -386,6 +409,7 @@ def build_report_text(
             build_metric_section("Jerk Peaks", jerk_peaks, unit, "s^3", "s^3", "JerkLimit(rad/s^3)"),
             "",
         ]
+    )
     return "\n".join(lines)
 
 
@@ -393,8 +417,9 @@ def main() -> int:
     args = parse_args()
     joint_detail_path = resolve_path(args.joint_detail)
     rows = read_joint_rows(joint_detail_path)
-    unit = infer_unit(rows, args.unit)
     limits_info = load_effective_limits(args.limit_csv)
+    rows, trim_note = trim_rows_to_limit_count(rows, len(limits_info.joints))
+    unit = infer_unit(rows, args.unit)
 
     velocity_peaks = calculate_velocity_peaks(rows, unit, limits_info.joints)
     acceleration_peaks = calculate_acceleration_peaks(rows, unit, limits_info.joints)
@@ -413,6 +438,7 @@ def main() -> int:
         unit=unit,
         max_abs_joint=max_abs_joint,
         limit_source_text=limit_source_text,
+        trim_note=trim_note,
         velocity_peaks=velocity_peaks,
         acceleration_peaks=acceleration_peaks,
         jerk_peaks=jerk_peaks,

docs/move-joint-jerk-comparison-20260505.md

@@ -7,6 +7,7 @@
 本文档固定记录以下三类证据，避免后续继续混用测试基线、旧文档结论和当前运行目录中的真实模型数据：
 
 - 当前运行目录 `.robot` 模型中的六轴基础 `velocity / acceleration / jerk`
+- 现场示教器读取到的六轴 `velocity / acceleration / jerk`
 - 当前运行目录 `RobotConfig.json` 中的 `acc_limit / jerk_limit`
 - 当前失败样本 `ActualSendJerkStats.txt` 中的逐轴实发跃度峰值
 
@@ -46,7 +47,29 @@
 - 当前运行目录中，`Joint1.jerk_base` 不是测试基线里常见的 `272.7`，而是 `224.22`。
 - 因此当前样本的 `Joint1` 生效 jerk 上限应按 `224.22 * 0.74 = 165.9228 rad/s^3` 计算。
 
-## 3. ActualSendJerkStats 的单位边界
+## 3. 示教器读取到的实际机器人限值
+
+2026-05-06 现场从机器人示教器读取到的速度、加速度与加加速度限制如下。示教器显示口径为角度制；下表同时记录换算后的弧度制，便于与 `.robot` / `RobotProfile` 中的基础限值直接对照。
+
+换算公式：
+
+- `rad = deg * π / 180`
+
+| Joint | velocity(deg/s) | velocity(rad/s) | acceleration(deg/s^2) | acceleration(rad/s^2) | jerk(deg/s^3) | jerk(rad/s^3) |
+| --- | ---: | ---: | ---: | ---: | ---: | ---: |
+| Joint1 | 370 | 6.457718 | 1541.667 | 26.907165 | 12847.223 | 224.226341 |
+| Joint2 | 310 | 5.410521 | 1291.667 | 22.543842 | 10763.889 | 187.865303 |
+| Joint3 | 410 | 7.155850 | 1708.333 | 29.816036 | 14236.111 | 248.467010 |
+| Joint4 | 550 | 9.599311 | 2291.667 | 39.997135 | 19097.223 | 333.309419 |
+| Joint5 | 545 | 9.512044 | 2270.833 | 39.633513 | 18923.611 | 330.279318 |
+| Joint6 | 1000 | 17.453293 | 4166.667 | 72.722058 | 34722.223 | 606.017115 |
+
+这组数据的含义：
+
+- 示教器读取值与当前运行 `.robot` 中的 `velocity_base / acceleration_base / jerk_base` 基本一致，可作为实际机器人基础限值证据。
+- 它还没有叠加当前 `RobotConfig.json` 的 `acc_limit = 0.74` 与 `jerk_limit = 0.74`；若用于本次失败样本比较，仍应使用第 2 节中的 `acc_eff / jerk_eff` 作为生效上限。
+
+## 4. ActualSendJerkStats 的单位边界
 
 `ActualSendJerkStats.txt` 的真实输入不是弧度，而是 J519 下发用的角度制关节目标：
 
@@ -60,7 +83,7 @@
 - 若要与 `.robot` / `RobotProfile` 中的 jerk limit 比较，需要先换算为 `rad/s^3`
 - 换算公式：`jerk_rad = jerk_deg * π / 180`
 
-## 4. 全文件逐轴最大跃度对比
+## 5. 全文件逐轴最大跃度对比
 
 扫描整份 `ActualSendJerkStats.txt` 后，各轴绝对值最大跃度如下：
 
@@ -79,7 +102,7 @@
 - 其余 5 个轴即使取全文件峰值，也远低于各自当前生效 jerk limit。
 - 当前样本本质上是一个“J1 主导”的跃度问题，而不是六轴普遍同时逼近上限。
 
-## 5. 报警窗口逐轴对比
+## 6. 报警窗口逐轴对比
 
 结合抓包与 J519 序号，报警前最后一个关键窗口是：
 
@@ -104,7 +127,7 @@
 - `Joint1` 在报警窗口内已经达到当前生效 jerk limit 的 `2.1454x`
 - 其余 5 轴在同一窗口仍远低于生效 jerk 上限
 
-## 6. 报警窗口与全局峰值窗口的关系
+## 7. 报警窗口与全局峰值窗口的关系
 
 本次样本不能简单理解为“最大峰值出现的位置就是首次报警位置”。
 
@@ -118,11 +141,12 @@
 1. 机器人第一次进入异常态时，`Joint1` 已经在 `0.296 -> 0.304s` 超限约 `2.15x`
 2. 即便忽略第一次报警，后续轨迹中仍存在更高的 J1 跃度峰值，说明当前 `MoveJoint` 临时轨迹整体都偏激，不只是单个孤立点异常
 
-## 7. 当前可落地的结论
+## 8. 当前可落地的结论
 
 基于当前运行目录的真实模型、配置和实发跃度文件，本次失败样本可以先固定为下面这组结论：
 
 - 当前运行模型 `Joint1.jerk_base = 224.22`，不是 `272.7`
+- 现场示教器读取到的 `Joint1.jerk = 12847.223 deg/s^3 = 224.226341 rad/s^3`，与当前运行模型基础值一致
 - 当前样本 `jerk_limit = 0.74`，所以 `Joint1.jerk_eff = 165.9228 rad/s^3`
 - `ActualSendJerkStats.txt` 需要按 `deg/s^3` 理解，再换算成 `rad/s^3` 后与模型 jerk limit 对比
 - 无论看报警窗口还是看全文件峰值，越限主体都只有 `Joint1`

global.json

@@ -0,0 +1,6 @@
+{
+  "sdk": {
+    "version": "8.0.420",
+    "rollForward": "disable"
+  }
+}

src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs

@@ -66,7 +66,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
 
         var plannedTrajectory = PlanByMethod(request, method);
         var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
-        var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
+        var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false);
 
         _logger?.LogInformation(
             "PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
@@ -112,11 +112,17 @@ public sealed class ControllerClientTrajectoryOrchestrator
         if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
         {
             _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
+            var executionTrajectory = ApplySmoothStartStopTiming(cachedBundle.PlannedTrajectory);
+            var executionTimeline = _shotTimelineBuilder.Build(
+                executionTrajectory,
+                holdCycles: settings.IoKeepCycles,
+                samplePeriod: planningRobot.ServoPeriod,
+                useDo: settings.UseDo);
             // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
             return new PlannedExecutionBundle(
                 cachedBundle.PlannedTrajectory,
-                cachedBundle.ShotTimeline,
+                executionTimeline,
-                CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
+                CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false));
         }
 
         var request = new TrajectoryRequest(
@@ -128,12 +134,13 @@ public sealed class ControllerClientTrajectoryOrchestrator
             useCache: options.UseCache);
 
         var plannedTrajectory = PlanByMethod(request, method, settings);
+        var smoothedExecutionTrajectory = ApplySmoothStartStopTiming(plannedTrajectory);
         var shotTimeline = _shotTimelineBuilder.Build(
-            plannedTrajectory,
+            smoothedExecutionTrajectory,
             holdCycles: settings.IoKeepCycles,
             samplePeriod: planningRobot.ServoPeriod,
             useDo: settings.UseDo);
-        var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
+        var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, usedCache: false, shapeTrajectoryEdges: false);
         var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
 
         _logger?.LogInformation(
@@ -357,11 +364,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
     /// <param name="plannedTrajectory">规划后的轨迹。</param>
     /// <param name="shotTimeline">触发时间轴。</param>
     /// <returns>运行时执行结果描述。</returns>
-    private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
+    private static TrajectoryResult CreateResult(
+        PlannedTrajectory plannedTrajectory,
+        ShotTimeline shotTimeline,
+        bool usedCache,
+        bool shapeTrajectoryEdges)
     {
         var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
             plannedTrajectory,
-            samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds);
+            samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds,
+            smoothStartStop: shapeTrajectoryEdges);
 
         return new TrajectoryResult(
             programName: plannedTrajectory.OriginalProgram.Name,
@@ -377,4 +389,90 @@ public sealed class ControllerClientTrajectoryOrchestrator
             plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
             denseJointTrajectory: denseJointTrajectory);
     }
+
+    /// <summary>
+    /// 为飞拍执行生成一条平滑起停的时间轴。
+    /// 保持路点位置不变，只重映射路点时刻，让起点和终点附近的速度自然收敛。
+    /// </summary>
+    private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
+    {
+        var originalTimes = plannedTrajectory.WaypointTimes;
+        if (originalTimes.Count < 3)
+        {
+            return plannedTrajectory;
+        }
+
+        var totalDuration = originalTimes[^1];
+        if (totalDuration <= 0.0)
+        {
+            return plannedTrajectory;
+        }
+
+        var smoothedTimes = new double[originalTimes.Count];
+        smoothedTimes[0] = 0.0;
+        smoothedTimes[^1] = totalDuration;
+
+        for (var index = 1; index < originalTimes.Count - 1; index++)
+        {
+            var normalizedProgress = originalTimes[index] / totalDuration;
+            smoothedTimes[index] = totalDuration * InvertSmoothStartStopProgress(normalizedProgress);
+        }
+
+        var segmentDurations = new double[smoothedTimes.Length - 1];
+        for (var index = 0; index < segmentDurations.Length; index++)
+        {
+            segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
+        }
+
+        return new PlannedTrajectory(
+            robot: plannedTrajectory.Robot,
+            originalProgram: plannedTrajectory.OriginalProgram,
+            plannedWaypoints: plannedTrajectory.PlannedWaypoints,
+            waypointTimes: smoothedTimes,
+            segmentDurations: segmentDurations,
+            segmentScales: plannedTrajectory.SegmentScales,
+            method: plannedTrajectory.Method,
+            iterations: plannedTrajectory.Iterations,
+            threshold: plannedTrajectory.Threshold);
+    }
+
+    /// <summary>
+    /// 反解 7 次 smootherstep 的时间进度，用二分法把原始线性进度映射成平滑时间轴。
+    /// </summary>
+    private static double InvertSmoothStartStopProgress(double normalizedProgress)
+    {
+        var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
+        var low = 0.0;
+        var high = 1.0;
+        for (var iteration = 0; iteration < 40; iteration++)
+        {
+            var middle = (low + high) / 2.0;
+            var progress = EvaluateSmoothStartStopProgress(middle);
+            if (progress < target)
+            {
+                low = middle;
+            }
+            else
+            {
+                high = middle;
+            }
+        }
+
+        return (low + high) / 2.0;
+    }
+
+    /// <summary>
+    /// 计算 7 次 smootherstep 进度值，用于整段平滑起停时间律。
+    /// </summary>
+    private static double EvaluateSmoothStartStopProgress(double normalizedTime)
+    {
+        var u = Math.Clamp(normalizedTime, 0.0, 1.0);
+        var u2 = u * u;
+        var u3 = u2 * u;
+        var u4 = u3 * u;
+        var u5 = u4 * u;
+        var u6 = u5 * u;
+        var u7 = u6 * u;
+        return (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
+    }
 }

src/Flyshot.ControllerClientCompat/FlyshotTrajectoryEdgeShaper.cs

@@ -0,0 +1,217 @@
+namespace Flyshot.ControllerClientCompat;
+
+/// <summary>
+/// 对飞拍稠密关节轨迹的首尾采样点做速度整形，降低启动和结束时的单步角度变化。
+/// </summary>
+internal static class FlyshotTrajectoryEdgeShaper
+{
+    /// <summary>
+    /// 首尾整形默认覆盖的采样点数（含锚点）。
+    /// </summary>
+    internal const int DefaultEdgePointCount = 10;
+
+    /// <summary>
+    /// 对稠密关节轨迹做首尾整形，时间列保持不变，首段采用 ease-in，尾段采用 ease-out。
+    /// </summary>
+    /// <param name="denseJointTrajectory">输入稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
+    /// <param name="maxEdgeStepDegrees">保留旧签名兼容调用方；当前实现不再按角度阈值扩窗。</param>
+    /// <param name="maxWindowPoints">单侧整形覆盖的采样点数（含锚点），默认首尾各 10 点。</param>
+    /// <returns>经过首尾整形后的新轨迹；若不满足整形条件则返回原轨迹副本。</returns>
+    internal static IReadOnlyList<IReadOnlyList<double>> ShapeDenseJointTrajectory(
+        IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
+        double maxEdgeStepDegrees = 0.0,
+        int maxWindowPoints = DefaultEdgePointCount)
+    {
+        ArgumentNullException.ThrowIfNull(denseJointTrajectory);
+        if (denseJointTrajectory.Count == 0)
+        {
+            return Array.Empty<IReadOnlyList<double>>();
+        }
+
+        var copiedRows = denseJointTrajectory
+            .Select(static row => row.ToArray())
+            .ToArray();
+        if (copiedRows.Length < 5 || maxWindowPoints < 2)
+        {
+            return copiedRows;
+        }
+
+        var lastIndex = copiedRows.Length - 1;
+        var window = Math.Min(maxWindowPoints, lastIndex / 2);
+        if (window < 2)
+        {
+            return copiedRows;
+        }
+
+        // 以原始轨迹为参考估计窗口边界的速度，并在位移累计量上做单段单调整形，
+        // 目标是让首尾 10 点表现为更平滑的加减速，而不是硬匹配高阶导数导致振荡。
+        var originalRows = copiedRows
+            .Select(static row => row.ToArray())
+            .ToArray();
+
+        ApplyLeadingHermiteBlend(copiedRows, originalRows, window);
+        ApplyTrailingHermiteBlend(copiedRows, originalRows, window);
+
+        return copiedRows;
+    }
+
+    /// <summary>
+    /// 对首段做单段 Hermite 累计位移整形：起点速度为 0，窗口末端按原轨迹边界速度接回中段。
+    /// </summary>
+    private static void ApplyLeadingHermiteBlend(double[][] rows, double[][] originalRows, int window)
+    {
+        var startRow = originalRows[0];
+        var endRow = originalRows[window];
+        var totalDuration = endRow[0] - startRow[0];
+        if (totalDuration <= 0.0)
+        {
+            return;
+        }
+
+        for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
+        {
+            var delta = endRow[jointIndex] - startRow[jointIndex];
+            if (Math.Abs(delta) <= 1e-12)
+            {
+                continue;
+            }
+
+            var endVelocity = EstimateVelocity(originalRows, window, jointIndex);
+            var normalizedEndSlope = ClampNormalizedSlope((endVelocity * totalDuration) / delta);
+
+            for (var index = 1; index < window; index++)
+            {
+                var normalizedTime = (rows[index][0] - startRow[0]) / totalDuration;
+                var shapedValue = startRow[jointIndex]
+                    + (delta * EvaluateHermiteProgress(normalizedTime, startSlope: 0.0, endSlope: normalizedEndSlope));
+                var blendWeight = Math.Pow(1.0 - normalizedTime, 2.0);
+                rows[index][jointIndex] = Lerp(originalRows[index][jointIndex], shapedValue, blendWeight);
+            }
+        }
+    }
+
+    /// <summary>
+    /// 对尾段做单段 Hermite 累计位移整形：窗口起点按原轨迹边界速度接入，终点速度减到 0。
+    /// </summary>
+    private static void ApplyTrailingHermiteBlend(double[][] rows, double[][] originalRows, int window)
+    {
+        var startIndex = rows.Length - 1 - window;
+        var startRow = originalRows[startIndex];
+        var endRow = originalRows[^1];
+        var totalDuration = endRow[0] - startRow[0];
+        if (totalDuration <= 0.0)
+        {
+            return;
+        }
+
+        for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
+        {
+            var delta = endRow[jointIndex] - startRow[jointIndex];
+            if (Math.Abs(delta) <= 1e-12)
+            {
+                continue;
+            }
+
+            var startVelocity = EstimateVelocity(originalRows, startIndex, jointIndex);
+            var normalizedStartSlope = ClampNormalizedSlope((startVelocity * totalDuration) / delta);
+
+            for (var index = 1; index < window; index++)
+            {
+                var normalizedTime = (rows[startIndex + index][0] - startRow[0]) / totalDuration;
+                var shapedValue = startRow[jointIndex]
+                    + (delta * EvaluateHermiteProgress(normalizedTime, startSlope: normalizedStartSlope, endSlope: 0.0));
+                var blendWeight = Math.Pow(normalizedTime, 2.0);
+                rows[startIndex + index][jointIndex] = Lerp(originalRows[startIndex + index][jointIndex], shapedValue, blendWeight);
+            }
+        }
+    }
+
+    /// <summary>
+    /// 估算给定行在原始轨迹上的一阶导，首尾退化为单边差分。
+    /// </summary>
+    private static double EstimateVelocity(double[][] rows, int index, int jointIndex)
+    {
+        if (index <= 0)
+        {
+            var dt = rows[1][0] - rows[0][0];
+            return dt <= 0.0 ? 0.0 : (rows[1][jointIndex] - rows[0][jointIndex]) / dt;
+        }
+
+        if (index >= rows.Length - 1)
+        {
+            var dt = rows[^1][0] - rows[^2][0];
+            return dt <= 0.0 ? 0.0 : (rows[^1][jointIndex] - rows[^2][jointIndex]) / dt;
+        }
+
+        var previousDt = rows[index][0] - rows[index - 1][0];
+        var nextDt = rows[index + 1][0] - rows[index][0];
+        if (previousDt <= 0.0 || nextDt <= 0.0)
+        {
+            return 0.0;
+        }
+
+        var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
+        var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
+        return (backward + forward) / 2.0;
+    }
+
+    /// <summary>
+    /// 估算给定行在原始轨迹上的二阶导，端点退化为 0 以避免放大边界噪声。
+    /// </summary>
+    private static double EstimateAcceleration(double[][] rows, int index, int jointIndex)
+    {
+        if (index <= 0 || index >= rows.Length - 1)
+        {
+            return 0.0;
+        }
+
+        var previousDt = rows[index][0] - rows[index - 1][0];
+        var nextDt = rows[index + 1][0] - rows[index][0];
+        if (previousDt <= 0.0 || nextDt <= 0.0)
+        {
+            return 0.0;
+        }
+
+        var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
+        var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
+        var averageDt = (previousDt + nextDt) / 2.0;
+        return averageDt <= 0.0 ? 0.0 : (forward - backward) / averageDt;
+    }
+
+    /// <summary>
+    /// 计算 Hermite 累计位移曲线在 0..1 归一化时间上的进度值。
+    /// </summary>
+    private static double EvaluateHermiteProgress(double normalizedTime, double startSlope, double endSlope)
+    {
+        var u = Math.Clamp(normalizedTime, 0.0, 1.0);
+        var u2 = u * u;
+        var u3 = u2 * u;
+        var h00 = (2.0 * u3) - (3.0 * u2) + 1.0;
+        var h10 = u3 - (2.0 * u2) + u;
+        var h01 = (-2.0 * u3) + (3.0 * u2);
+        var h11 = u3 - u2;
+        return (h00 * 0.0) + (h10 * startSlope) + (h01 * 1.0) + (h11 * endSlope);
+    }
+
+    /// <summary>
+    /// 把归一化边界斜率限制在单调 Hermite 常见的稳定区间内，避免过冲和窗口内振荡。
+    /// </summary>
+    private static double ClampNormalizedSlope(double normalizedSlope)
+    {
+        if (double.IsNaN(normalizedSlope) || double.IsInfinity(normalizedSlope))
+        {
+            return 0.0;
+        }
+
+        return Math.Clamp(normalizedSlope, 0.0, 3.0);
+    }
+
+    /// <summary>
+    /// 在线性插值基础上做温和混合，避免首尾窗口为了追赶锚点而产生过大的局部跃度。
+    /// </summary>
+    private static double Lerp(double originalValue, double shapedValue, double weight)
+    {
+        var clampedWeight = Math.Clamp(weight, 0.0, 1.0);
+        return originalValue + ((shapedValue - originalValue) * clampedWeight);
+    }
+}

src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs

@@ -19,7 +19,8 @@ public static class TrajectorySampler
     public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
         PlannedTrajectory trajectory,
         double samplePeriod = 0.016,
-        int decimals = 6)
+        int decimals = 6,
+        bool smoothStartStop = false)
     {
         var spline = RebuildSpline(trajectory);
         double duration = trajectory.WaypointTimes[^1];
@@ -28,7 +29,10 @@ public static class TrajectorySampler
 
         foreach (var t in times)
         {
-            var pos = spline.Evaluate(t);
+            var evaluationTime = smoothStartStop
+                ? MapSmoothStartStopEvaluationTime(t, duration)
+                : t;
+            var pos = spline.Evaluate(evaluationTime);
             var row = new List<double>(pos.Length + 1);
             row.Add(Math.Round(t, decimals));
             foreach (var value in pos)
@@ -49,7 +53,8 @@ public static class TrajectorySampler
         PlannedTrajectory trajectory,
         RobotKinematicsModel kinematicsModel,
         double samplePeriod = 0.016,
-        int decimals = 6)
+        int decimals = 6,
+        bool smoothStartStop = false)
     {
         var spline = RebuildSpline(trajectory);
         double duration = trajectory.WaypointTimes[^1];
@@ -58,7 +63,10 @@ public static class TrajectorySampler
 
         foreach (var t in times)
         {
-            var jointPos = spline.Evaluate(t);
+            var evaluationTime = smoothStartStop
+                ? MapSmoothStartStopEvaluationTime(t, duration)
+                : t;
+            var jointPos = spline.Evaluate(evaluationTime);
             var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
             var row = new List<double>(pose.Length + 1);
             row.Add(Math.Round(t, decimals));
@@ -103,4 +111,26 @@ public static class TrajectorySampler
 
         return times;
     }
+
+    /// <summary>
+    /// 把线性采样时间映射为整段平滑起停的评估时间。
+    /// 使用 7 次 smootherstep 时间律，让起点和终点的一到三阶导都自然收敛到 0。
+    /// </summary>
+    private static double MapSmoothStartStopEvaluationTime(double sampleTime, double duration)
+    {
+        if (duration <= 0.0)
+        {
+            return 0.0;
+        }
+
+        var normalizedTime = Math.Clamp(sampleTime / duration, 0.0, 1.0);
+        var u2 = normalizedTime * normalizedTime;
+        var u3 = u2 * normalizedTime;
+        var u4 = u3 * normalizedTime;
+        var u5 = u4 * normalizedTime;
+        var u6 = u5 * normalizedTime;
+        var u7 = u6 * normalizedTime;
+        var progress = (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
+        return duration * progress;
+    }
 }

src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs

@@ -187,7 +187,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
                 return;
             }
 
-            // 真机模式：走完整 RVBUSTSM 启动序列（与抓包一致）。
+            // 真机模式：按 all-reconnect.pcap 的重连序列启动 RVBUSTSM，暂不发送 StopProg。
               _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
             _commandClient.ResetRobotAsync().GetAwaiter().GetResult();
             _commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
@@ -775,25 +775,75 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
     /// </summary>
     private void EnsureJ519ReadyForDenseExecution()
     {
-        var response = _j519Client.GetLatestResponse();
+        EnsureJ519ReadyForDenseExecutionCore(
-        if (response is null)
+            () => _j519Client.GetLatestResponse(),
+            () =>
+            {
+                // 若当前状态未就绪，则先尝试一次 EnableRobot 走现场既有恢复链路。
+                EnableRobot(_bufferSize);
+            },
+            static () => Thread.Sleep(TimeSpan.FromMilliseconds(500)));
+    }
+
+    /// <summary>
+    /// 执行稠密发送前的 J519 就绪校验；未就绪时允许先做一次启机重试。
+    /// </summary>
+    /// <param name="getLatestResponse">读取最新 J519 状态的委托。</param>
+    /// <param name="retryEnableRobot">状态未就绪时触发一次 EnableRobot 重试的委托。</param>
+    /// <param name="waitAfterRetry">重试后等待状态刷新的委托。</param>
+    internal static void EnsureJ519ReadyForDenseExecutionCore(
+        Func<FanucJ519Response?> getLatestResponse,
+        Action retryEnableRobot,
+        Action waitAfterRetry)
+    {
+        ArgumentNullException.ThrowIfNull(getLatestResponse);
+        ArgumentNullException.ThrowIfNull(retryEnableRobot);
+        ArgumentNullException.ThrowIfNull(waitAfterRetry);
+
+        var response = getLatestResponse();
+        if (response is null || IsJ519ReadyForDenseExecution(response))
         {
             return;
         }
 
-        if (response.AcceptsCommand && response.SystemReady)
+        retryEnableRobot();
+        waitAfterRetry();
+
+        response = getLatestResponse();
+        if (response is null || IsJ519ReadyForDenseExecution(response))
         {
             return;
         }
 
-        throw new InvalidOperationException(
+        throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response));
-            "J519 status is not ready for dense execution: "
+    }
+
+    /// <summary>
+    /// 判断当前 J519 响应是否已经满足稠密执行前置条件。
+    /// </summary>
+    /// <param name="response">待检查的 J519 状态响应。</param>
+    /// <returns>接受命令且系统就绪时返回 true，否则返回 false。</returns>
+    internal static bool IsJ519ReadyForDenseExecution(FanucJ519Response response)
+    {
+        ArgumentNullException.ThrowIfNull(response);
+        return response.AcceptsCommand && response.SystemReady;
+    }
+
+    /// <summary>
+    /// 组装 J519 未就绪时的异常消息，便于保留现场关键状态位。
+    /// </summary>
+    /// <param name="response">最近一次收到的 J519 状态响应。</param>
+    /// <returns>包含状态位和序号的诊断消息。</returns>
+    internal static string BuildJ519DenseExecutionNotReadyMessage(FanucJ519Response response)
+    {
+        ArgumentNullException.ThrowIfNull(response);
+        return "J519 status is not ready for dense execution: "
             + $"accept_cmd={response.AcceptsCommand}, "
             + $"received_cmd={response.ReceivedCommand}, "
             + $"sysrdy={response.SystemReady}, "
             + $"rbt_inmotion={response.RobotInMotion}, "
             + $"seq={response.Sequence}, "
-            + $"status=0x{response.Status:X2}.");
+            + $"status=0x{response.Status:X2}.";
     }
 
     private static double RadiansToDegrees(double radians)

src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs

@@ -548,9 +548,9 @@ public static class FanucCommandProtocol
             throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
         }
 
-        // 抓包样本中的字段顺序为 result_code 后接 prog_status。
+        // all-reconnect.pcap 中字段顺序为 prog_status 后接 result_code。
-        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
+        var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
-        var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
+        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
         return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
     }
 

src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs

@@ -1,3 +1,4 @@
+using System.Diagnostics;
 using System.Net.Sockets;
 using Microsoft.Extensions.Logging;
 
@@ -13,7 +14,7 @@ public sealed class FanucJ519Client : IDisposable
     private readonly ILogger<FanucJ519Client>? _logger;
     private UdpClient? _udpClient;
     private CancellationTokenSource? _cts;
-    private Task? _receiveTask;
+    private Thread? _receiveThread;
     private FanucJ519Command? _currentCommand;
     private FanucJ519Command? _lastSentCommand;
     // 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。
@@ -21,6 +22,8 @@ public sealed class FanucJ519Client : IDisposable
     private TaskCompletionSource? _commandQueueDrainedCompletion;
     private List<FanucJ519Command>? _commandHistoryForTests;
     private FanucJ519Response? _latestResponse;
+    private long _slowSendCount;
+    private long _maxReceiveToSendTicks;
     // 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。
     private bool _hasPendingCommandForStart;
     private bool _motionStarted;
@@ -64,13 +67,21 @@ public sealed class FanucJ519Client : IDisposable
 
         _udpClient = new UdpClient();
         _udpClient.Connect(ip, port);
+        ConfigureRealtimeSocket(_udpClient.Client);
+        ConfigureProcessPriority();
 
         // 发送初始化包。
         await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
         _logger?.LogInformation("J519 初始化包已发送");
 
         _cts = new CancellationTokenSource();
-        _receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token);
+        _receiveThread = new Thread(ReceiveLoop)
+        {
+            IsBackground = true,
+            Name = "J519 UDP realtime loop",
+            Priority = ThreadPriority.Highest
+        };
+        _receiveThread.Start();
     }
 
     /// <summary>
@@ -205,7 +216,6 @@ public sealed class FanucJ519Client : IDisposable
         return waitTask.WaitAsync(cancellationToken);
     }
 
-    /// <summary>
     /// 判断当前是否没有等待出队的命令；仅供单元测试断言。
     /// </summary>
     /// <returns>如果队列为空或尚未装载队列，则返回 true。</returns>
@@ -285,20 +295,19 @@ public sealed class FanucJ519Client : IDisposable
 
         try
         {
-            _receiveTask?.Wait(TimeSpan.FromSeconds(1));
+            _udpClient?.Dispose();
+            _receiveThread?.Join(TimeSpan.FromSeconds(1));
         }
-        catch (AggregateException)
+        catch (ObjectDisposedException)
         {
-            // 忽略取消异常。
+            // 忽略释放期间的套接字关闭异常。
         }
 
-        _receiveTask?.Dispose();
+        _receiveThread = null;
-        _receiveTask = null;
 
         _cts?.Dispose();
         _cts = null;
 
-        _udpClient?.Dispose();
         _udpClient = null;
 
         lock (_commandLock)
@@ -332,16 +341,15 @@ public sealed class FanucJ519Client : IDisposable
 
         try
         {
-            _receiveTask?.Wait(TimeSpan.FromSeconds(1));
+            _udpClient?.Dispose();
+            _receiveThread?.Join(TimeSpan.FromSeconds(1));
         }
-        catch (AggregateException)
+        catch (ObjectDisposedException)
         {
-            // 忽略取消异常。
+            // 忽略释放期间的套接字关闭异常。
         }
 
-        _receiveTask?.Dispose();
         _cts?.Dispose();
-        _udpClient?.Dispose();
 
         lock (_commandLock)
         {
@@ -350,28 +358,50 @@ public sealed class FanucJ519Client : IDisposable
         }
     }
 
-    private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
+    private static void ConfigureProcessPriority()
     {
-        return new FanucJ519Command(
+        try
-            sequence,
+        {
-            source.TargetJoints,
+            var process = Process.GetCurrentProcess();
-            source.LastData,
+            if (process.PriorityClass < ProcessPriorityClass.High)
-            source.ReadIoType,
+            {
-            source.ReadIoIndex,
+                process.PriorityClass = ProcessPriorityClass.High;
-            source.ReadIoMask,
+            }
-            source.DataStyle,
+        }
-            source.WriteIoType,
+        catch (Exception)
-            source.WriteIoIndex,
+        {
-            source.WriteIoMask,
+            // 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。
-            source.WriteIoValue);
+        }
     }
 
     /// <summary>
-    /// 后台接收循环：持续接收 132B 响应并解析。
+    /// 配置 J519 UDP 套接字的低延迟参数。
     /// </summary>
-    private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
+    /// <param name="socket">已连接 FANUC 60015 的 UDP 套接字。</param>
+    private void ConfigureRealtimeSocket(Socket socket)
     {
-        if (_udpClient is null)
+        socket.ReceiveBufferSize = 1024 * 1024;
+        socket.SendBufferSize = 1024 * 1024;
+
+        try
+        {
+            // DSCP EF(46) 标记低延迟流量，是否生效取决于现场网卡、交换机和控制柜网络策略。
+            socket.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.TypeOfService, 0xB8);
+        }
+        catch (SocketException ex)
+        {
+            _logger?.LogWarning(ex, "J519 UDP 套接字无法设置 DSCP EF 优先级标记");
+        }
+    }
+
+    /// <summary>
+    /// 后台接收循环：在专用高优先级线程中同步接收 132B 响应并立即回发命令。
+    /// </summary>
+    private void ReceiveLoop()
+    {
+        var udpClient = _udpClient;
+        var cancellationToken = _cts?.Token ?? CancellationToken.None;
+        if (udpClient is null)
         {
             return;
         }
@@ -379,26 +409,31 @@ public sealed class FanucJ519Client : IDisposable
         _logger?.LogInformation("J519 ReceiveLoop 启动");
         long receiveCount = 0;
         FanucJ519Response? lastLoggedResponse = null;
+        var receiveBuffer = new byte[FanucJ519Protocol.ResponsePacketLength];
+        var commandBuffer = new byte[FanucJ519Protocol.CommandPacketLength];
 
         try
         {
             while (!cancellationToken.IsCancellationRequested)
             {
-                var result = await _udpClient.ReceiveAsync(cancellationToken).ConfigureAwait(false);
+                var received = udpClient.Client.Receive(receiveBuffer);
-                if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength)
+                if (received == FanucJ519Protocol.ResponsePacketLength)
                 {
-                    var response = FanucJ519Protocol.ParseResponse(result.Buffer);
+                    var receiveTicks = Stopwatch.GetTimestamp();
+                    var response = FanucJ519Protocol.ParseResponse(receiveBuffer);
+
+                    // 先按状态包节拍回发命令，再做低频日志处理，减少受信周期内的非必要工作。
+                    if (response.AcceptsCommand)
+                    {
+                        SendCommandForStatus(udpClient.Client, response, commandBuffer, receiveTicks);
+                    }
+
                     lock (_responseLock)
                     {
                         _latestResponse = response;
                     }
 
                     receiveCount++;
-                    // 判断 ready for command 条件并回发命令包。
-                    if (response.AcceptsCommand)
-                    {
-                        await SendCommandForStatusAsync(response, cancellationToken).ConfigureAwait(false);
-                    }
 
                     // 仅在状态变化时记录 Info，避免高频日志。
                     if (lastLoggedResponse is null
@@ -408,10 +443,11 @@ public sealed class FanucJ519Client : IDisposable
                         || lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
                     {
                         _logger?.LogInformation(
-                            "J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, sysrdy={SysRdy}, motion={Motion}, pose=[{Pose}], joints=[{Joints}]",
+                            "J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, received={received}, SystemReady={SystemReady}, RobotInMotion={RobotInMotion}, pose=[{Pose}], joints=[{Joints}]",
                             response.Status,
                             response.Sequence,
                             response.AcceptsCommand,
+                            response.ReceivedCommand,
                             response.SystemReady,
                             response.RobotInMotion,
                             string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
@@ -420,10 +456,27 @@ public sealed class FanucJ519Client : IDisposable
                         var lastSentTargetJoints = GetLastSentTargetJointsLogText();
                         _logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
                         lastLoggedResponse = response;
+
+                        // 如果状态从AcceptsCommand true 变为false,说明机器人报错,清空队列
+                        if (!response.AcceptsCommand)
+                        {
+                            lock (_commandLock)
+                            {
+                                _currentCommand = null;
+                                CompleteCommandQueueLocked();
+                            }
+                            _logger?.LogWarning("J519 接收状态包显示机器人不可接受命令，已清空命令队列");
+                        }
+
                     }
                     else if (receiveCount % 1000 == 0)
                     {
-                        _logger?.LogDebug("J519 已接收 {Count} 个响应包", receiveCount);
+                        var maxReceiveToSendMs = Stopwatch.GetElapsedTime(0, Interlocked.Read(ref _maxReceiveToSendTicks)).TotalMilliseconds;
+                        _logger?.LogDebug(
+                            "J519 已接收 {Count} 个响应包，receive-to-send 最大耗时约 {MaxMs:F3}ms，超过 0.5ms 次数 {SlowCount}",
+                            receiveCount,
+                            maxReceiveToSendMs,
+                            Interlocked.Read(ref _slowSendCount));
                     }
                 }
             }
@@ -436,21 +489,21 @@ public sealed class FanucJ519Client : IDisposable
         {
             _logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
         }
+        catch (SocketException ex) when (cancellationToken.IsCancellationRequested)
+        {
+            _logger?.LogInformation(ex, "J519 ReceiveLoop 因取消关闭套接字退出，共接收 {Count} 个包", receiveCount);
+        }
     }
 
     /// <summary>
     /// 按机器人状态包的 sequence 立即回发当前 J519 命令。
     /// </summary>
-    /// <param name="response">刚收到的状态包。</param>
+    /// <param name="socket">已连接的 UDP 套接字。</param>
-    /// <param name="cancellationToken">取消令牌。</param>
+    /// <param name="response">刚收到的机器人状态包。</param>
-    private async Task SendCommandForStatusAsync(FanucJ519Response response, CancellationToken cancellationToken)
+    /// <param name="commandBuffer">可复用的 64B 命令包缓冲区。</param>
+    /// <param name="receiveTicks">收到状态包后的时间戳。</param>
+    private void SendCommandForStatus(Socket socket, FanucJ519Response response, byte[] commandBuffer, long receiveTicks)
     {
-        var udpClient = _udpClient;
-        if (udpClient is null)
-        {
-            return;
-        }
-
         FanucJ519Command? command;
         var willDrainQueue = false;
         lock (_commandLock)
@@ -465,13 +518,11 @@ public sealed class FanucJ519Client : IDisposable
                 var queuedCommand = queue.Dequeue();
                 _currentCommand = queuedCommand;
                 willDrainQueue = queue.Count == 0;
-                command = WithSequence(queuedCommand, response.Sequence);
+                command = queuedCommand;
             }
             else
             {
-                command = _currentCommand is null
+                command = _currentCommand;
-                    ? null
-                    : WithSequence(_currentCommand, response.Sequence);
             }
         }
 
@@ -480,14 +531,15 @@ public sealed class FanucJ519Client : IDisposable
             return;
         }
 
-        var packet = FanucJ519Protocol.PackCommandPacket(command);
+        FanucJ519Protocol.PackCommandPacket(command, response.Sequence, commandBuffer);
-        await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
+        socket.Send(commandBuffer);
-        _logger?.LogDebug("J519 已回发命令包，seq={Seq}", command.Sequence);
+        TrackReceiveToSendLatency(receiveTicks);
-        _logger?.LogTrace(
+        // _logger?.LogDebug("J519 已回发命令包，seq={Seq}", sequence);
-            "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
+        // _logger?.LogDebug(
-            command.TargetJoints,
+        //     "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
-            command.WriteIoMask,
+        //     command.TargetJoints,
-            command.WriteIoValue);
+        //     command.WriteIoMask,
+        //     command.WriteIoValue);
         lock (_commandLock)
         {
             _lastSentCommand = command;
@@ -498,6 +550,26 @@ public sealed class FanucJ519Client : IDisposable
         }
     }
 
+    /// <summary>
+    /// 记录状态包到命令包发出的最大耗时和慢发送次数，供低频诊断日志观察调度抖动。
+    /// </summary>
+    /// <param name="receiveTicks">收到状态包后的时间戳。</param>
+    private void TrackReceiveToSendLatency(long receiveTicks)
+    {
+        var elapsedTicks = Stopwatch.GetTimestamp() - receiveTicks;
+        var currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
+        while (elapsedTicks > currentMax
+            && Interlocked.CompareExchange(ref _maxReceiveToSendTicks, elapsedTicks, currentMax) != currentMax)
+        {
+            currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
+        }
+
+        if (Stopwatch.GetElapsedTime(0, elapsedTicks) > TimeSpan.FromMilliseconds(0.5))
+        {
+            Interlocked.Increment(ref _slowSendCount);
+        }
+    }
+
     /// <summary>
     /// 清空当前命令队列，并唤醒等待队列结束的运行时任务。
     /// </summary>

src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs

@@ -301,28 +301,45 @@ public static class FanucJ519Protocol
         ArgumentNullException.ThrowIfNull(command);
 
         var packet = new byte[CommandPacketLength];
-        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, sizeof(uint)), 1);
+        PackCommandPacket(command, command.Sequence, packet);
-        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1);
+        return packet;
-        BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence);
+    }
+
+    /// <summary>
+    /// 将 J519 64 字节命令包写入调用方提供的缓冲区。
+    /// </summary>
+    /// <param name="command">命令数据。</param>
+    /// <param name="sequence">本次出包使用的机器人状态包序号。</param>
+    /// <param name="packet">长度至少为 64 字节的命令包缓冲区。</param>
+    public static void PackCommandPacket(FanucJ519Command command, uint sequence, Span<byte> packet)
+    {
+        ArgumentNullException.ThrowIfNull(command);
+        if (packet.Length < CommandPacketLength)
+        {
+            throw new ArgumentException("J519 命令包缓冲区长度不足。", nameof(packet));
+        }
+
+        packet.Slice(0, CommandPacketLength).Clear();
+        BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x00, sizeof(uint)), 1);
+        BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x04, sizeof(uint)), 1);
+        BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x08, sizeof(uint)), sequence);
         packet[0x0c] = command.LastData;
         packet[0x0d] = command.ReadIoType;
-        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, sizeof(ushort)), command.ReadIoIndex);
+        BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort)), command.ReadIoIndex);
-        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask);
+        BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x10, sizeof(ushort)), command.ReadIoMask);
         packet[0x12] = command.DataStyle;
         packet[0x13] = command.WriteIoType;
-        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x14, sizeof(ushort)), command.WriteIoIndex);
+        BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x14, sizeof(ushort)), command.WriteIoIndex);
-        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask);
+        BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x16, sizeof(ushort)), command.WriteIoMask);
-        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue);
+        BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x18, sizeof(ushort)), command.WriteIoValue);
-        BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0);
+        BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x1a, sizeof(ushort)), 0);
 
         // J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
         for (var index = 0; index < 9; index++)
         {
             var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
-            BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
+            BinaryPrimitives.WriteSingleBigEndian(packet.Slice(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
         }
-
-        return packet;
     }
 
     /// <summary>

src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs

@@ -52,12 +52,12 @@ public sealed class FanucStateClientOptions
     /// <summary>
     /// 获取或设置重连等待时间的上限。
     /// </summary>
-    public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(2);
+    public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(5);
 
     /// <summary>
     /// 获取或设置单次 TCP 建连允许的最长时间。
     /// </summary>
-    public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(2);
+    public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(5);
 }
 
 /// <summary>

src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs

@@ -154,12 +154,12 @@ public sealed class LegacyHttpApiController : ControllerBase
     }
 
     /// <summary>
-    /// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状。
+    /// 兼容旧 `EnableRobot(buffer_size=4)` 参数形状。
     /// </summary>
     /// <param name="buffer_size">控制器执行缓冲区大小。</param>
     /// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
     [HttpGet("/enable_robot/")]
-    public IActionResult EnableRobot([FromQuery] int buffer_size = 2)
+    public IActionResult EnableRobot([FromQuery] int buffer_size = 4)
     {
         _logger.LogInformation("EnableRobot 调用: buffer_size={BufferSize}", buffer_size);
         try

tests/Flyshot.Core.Tests/FanucCommandClientTests.cs

@@ -99,7 +99,7 @@ public sealed class FanucCommandClientTests : IDisposable
     {
         using var client = new FanucCommandClient();
         var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
-        var responseFrame = Convert.FromHexString("646f7a000000160000200300000000000000017a6f64");
+        var responseFrame = Convert.FromHexString("646f7a000000160000200300000001000000007a6f64");
         var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
 
         await client.ConnectAsync("127.0.0.1", Port, _cts.Token);

tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs

@@ -221,6 +221,101 @@ public sealed class FanucControllerRuntimeDenseTests
         }
     }
 
+    /// <summary>
+    /// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 轨迹执行一次完整 1x 稠密下发，
+    /// 并把 0.088s 报警窗口附近的实发时间、关节与跃度摘要落盘，便于继续对照现场报警帧。
+    /// </summary>
+    [Fact]
+    public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_WritesDenseSendDebugWindowAtOneX()
+    {
+        using var commandClient = new FanucCommandClient();
+        using var stateClient = new FanucStateClient();
+        using var j519Client = new FanucJ519Client();
+        using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
+        var fixture = LoadUttcMs11RuntimeFixture();
+        var orchestrator = new ControllerClientTrajectoryOrchestrator();
+        var bundle = orchestrator.PlanUploadedFlyshot(
+            fixture.Robot,
+            fixture.Uploaded,
+            settings: fixture.Settings,
+            planningSpeedScale: 1.0);
+        var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", bundle.Result.ProgramName);
+        var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
+        var beforeRunDirectories = Directory.Exists(denseSendRoot)
+            ? Directory.GetDirectories(denseSendRoot).ToHashSet(StringComparer.OrdinalIgnoreCase)
+            : new HashSet<string>(StringComparer.OrdinalIgnoreCase);
+
+        runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
+        j519Client.EnableCommandHistoryForTests();
+        ForceRealModeEnabled(runtime, speedRatio: 1.0);
+
+        runtime.ExecuteTrajectory(bundle.Result, bundle.Result.DenseJointTrajectory![0].Skip(1).ToArray());
+        WaitUntilIdle(runtime);
+
+        var runDirectory = GetNewDenseSendRunDirectory(outputRoot, beforeRunDirectories);
+        var pointsPath = Path.Combine(runDirectory, "ActualSendJointTraj.txt");
+        var timingPath = Path.Combine(runDirectory, "ActualSendTiming.txt");
+        var jerkPath = Path.Combine(runDirectory, "ActualSendJerkStats.txt");
+        var summaryPath = Path.Combine(runDirectory, "AlarmWindow_0p088s_Summary.txt");
+
+        Assert.True(File.Exists(pointsPath));
+        Assert.True(File.Exists(timingPath));
+        Assert.True(File.Exists(jerkPath));
+
+        var pointsLines = File.ReadAllLines(pointsPath);
+        var timingLines = File.ReadAllLines(timingPath);
+        var jerkLines = File.ReadAllLines(jerkPath);
+
+        Assert.NotEmpty(pointsLines);
+        Assert.NotEmpty(timingLines);
+        Assert.NotEmpty(jerkLines);
+
+        var firstPoint = ParseColumns(pointsLines[0]);
+        var secondPoint = ParseColumns(pointsLines[1]);
+        Assert.Equal(0.0, firstPoint[0], precision: 6);
+        Assert.Equal(0.008, secondPoint[0], precision: 6);
+
+        var firstStepJ1 = Math.Abs(secondPoint[1] - firstPoint[1]);
+        Assert.True(firstStepJ1 > 1e-6, $"UTTC_MS11 实发首步不应被压成 0，actual={firstStepJ1:F9}deg");
+
+        const double targetSendTime = 0.088;
+        const double windowHalfWidth = 0.024;
+        var summaryLines = new List<string>
+        {
+            $"program={bundle.Result.ProgramName}",
+            $"send_time_target_seconds={targetSendTime:F6}",
+            $"window_half_width_seconds={windowHalfWidth:F6}",
+            $"points_path={pointsPath}",
+            $"timing_path={timingPath}",
+            $"jerk_path={jerkPath}",
+            "timing_window:"
+        };
+
+        foreach (var line in timingLines.Select(ParseColumns))
+        {
+            var sendTime = line[1];
+            if (Math.Abs(sendTime - targetSendTime) <= windowHalfWidth + 1e-9)
+            {
+                summaryLines.Add(
+                    $"timing send_index={line[0]:F0} send_time={line[1]:F6} trajectory_time={line[2]:F6} speed_ratio={line[3]:F6}");
+            }
+        }
+
+        summaryLines.Add("jerk_window:");
+        foreach (var line in jerkLines.Select(ParseColumns))
+        {
+            var endTime = line[1];
+            if (Math.Abs(endTime - targetSendTime) <= windowHalfWidth + 1e-9)
+            {
+                summaryLines.Add(
+                    $"jerk end_time={line[1]:F6} dt={line[2]:F6} j1={line[3]:F6} j2={line[4]:F6} j3={line[5]:F6} j4={line[6]:F6} j5={line[7]:F6} j6={line[8]:F6} max_abs={line.Skip(3).Max(static value => Math.Abs(value)):F6}");
+            }
+        }
+
+        File.WriteAllLines(summaryPath, summaryLines);
+        Assert.True(File.Exists(summaryPath));
+    }
+
     /// <summary>
     /// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
     /// </summary>
@@ -509,6 +604,58 @@ public sealed class FanucControllerRuntimeDenseTests
         Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
     }
 
+    /// <summary>
+    /// 验证稠密执行前若 J519 首次未就绪，会先尝试一次 EnableRobot，再在 500ms 后复查状态。
+    /// </summary>
+    [Fact]
+    public void EnsureJ519ReadyForDenseExecutionCore_RetriesEnableRobotOnceBeforePassing()
+    {
+        var responses = new Queue<FanucJ519Response?>(
+        [
+            CreateJ519Response(status: 0b0001, sequence: 11),
+            CreateJ519Response(status: 0b0101, sequence: 12)
+        ]);
+        var enableRobotRetryCount = 0;
+        var waitCount = 0;
+
+        var exception = Record.Exception(
+            () => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
+                () => responses.Dequeue(),
+                () => enableRobotRetryCount++,
+                () => waitCount++));
+
+        Assert.Null(exception);
+        Assert.Equal(1, enableRobotRetryCount);
+        Assert.Equal(1, waitCount);
+    }
+
+    /// <summary>
+    /// 验证重试 EnableRobot 之后若 J519 仍未就绪，会继续抛出带状态位的异常。
+    /// </summary>
+    [Fact]
+    public void EnsureJ519ReadyForDenseExecutionCore_ThrowsWhenStillNotReadyAfterRetry()
+    {
+        var responses = new Queue<FanucJ519Response?>(
+        [
+            CreateJ519Response(status: 0b0000, sequence: 21),
+            CreateJ519Response(status: 0b0010, sequence: 22)
+        ]);
+        var enableRobotRetryCount = 0;
+        var waitCount = 0;
+
+        var exception = Assert.Throws<InvalidOperationException>(
+            () => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
+                () => responses.Dequeue(),
+                () => enableRobotRetryCount++,
+                () => waitCount++));
+
+        Assert.Equal(1, enableRobotRetryCount);
+        Assert.Equal(1, waitCount);
+        Assert.Contains("accept_cmd=False", exception.Message, StringComparison.Ordinal);
+        Assert.Contains("sysrdy=False", exception.Message, StringComparison.Ordinal);
+        Assert.Contains("seq=22", exception.Message, StringComparison.Ordinal);
+    }
+
     /// <summary>
     /// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
     /// </summary>
@@ -728,6 +875,30 @@ public sealed class FanucControllerRuntimeDenseTests
         }
     }
 
+    /// <summary>
+    /// 创建用于就绪状态测试的最小 J519 响应。
+    /// </summary>
+    /// <param name="status">待注入的 J519 状态位。</param>
+    /// <param name="sequence">待注入的响应序号。</param>
+    /// <returns>包含最小字段集合的测试响应。</returns>
+    private static FanucJ519Response CreateJ519Response(byte status, uint sequence)
+    {
+        return new FanucJ519Response(
+            messageType: 0,
+            version: 1,
+            sequence: sequence,
+            status: status,
+            readIoType: 0,
+            readIoIndex: 0,
+            readIoMask: 0,
+            readIoValue: 0,
+            timestamp: 0,
+            pose: new double[6],
+            externalAxes: new double[3],
+            jointDegrees: new double[6],
+            motorCurrents: new double[6]);
+    }
+
     private static RobotProfile CreateMoveJointReferenceRobotProfile()
     {
         return new RobotProfile(
@@ -906,13 +1077,14 @@ public sealed class FanucControllerRuntimeDenseTests
         UdpClient server,
         IPEndPoint clientEndpoint,
         uint sequence,
-        CancellationToken cancellationToken)
+        CancellationToken cancellationToken,
+        byte status = 0b0111)
     {
         var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
         BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
         BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
         BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
-        responsePacket[0x0c] = 0b0111;
+        responsePacket[0x0c] = status;
         await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
     }
 
@@ -929,6 +1101,54 @@ public sealed class FanucControllerRuntimeDenseTests
         return runDirectories[0];
     }
 
+    /// <summary>
+    /// 获取本次测试刚生成的新稠密发送记录目录，避免误读历史运行产物。
+    /// </summary>
+    private static string GetNewDenseSendRunDirectory(string outputRoot, IReadOnlySet<string> beforeRunDirectories)
+    {
+        var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
+        Assert.True(Directory.Exists(denseSendRoot));
+
+        var newDirectories = Directory.GetDirectories(denseSendRoot)
+            .Where(path => !beforeRunDirectories.Contains(path))
+            .ToArray();
+        Assert.Single(newDirectories);
+        return newDirectories[0];
+    }
+
+    /// <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>
     /// 解析空格分隔的纯文本数值列。
     /// </summary>
@@ -984,4 +1204,13 @@ public sealed class FanucControllerRuntimeDenseTests
         Assert.NotNull(field);
         field.SetValue(client, response);
     }
+
+    /// <summary>
+    /// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
+    /// </summary>
+    private sealed record UttcMs11RuntimeFixture(
+        string ConfigRoot,
+        CompatibilityRobotSettings Settings,
+        ControllerClientCompatUploadedTrajectory Uploaded,
+        RobotProfile Robot);
 }

tests/Flyshot.Core.Tests/FanucProtocolTests.cs

@@ -30,7 +30,7 @@ public sealed class FanucProtocolTests
         var stopResponse = FanucCommandProtocol.ParseResultResponse(
             Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
         var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
-            Convert.FromHexString("646f7a000000160000200300000000000000017a6f64"));
+            Convert.FromHexString("646f7a000000160000200300000001000000007a6f64"));
 
         Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
         Assert.True(stopResponse.IsSuccess);
@@ -183,6 +183,24 @@ public sealed class FanucProtocolTests
         Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
     }
 
+    /// <summary>
+    /// 验证实时发送路径可以复用命令对象，并用机器人状态包序号覆盖出包序号。
+    /// </summary>
+    [Fact]
+    public void J519Protocol_PacksCommandWithOverrideSequenceIntoReusableBuffer()
+    {
+        var command = new FanucJ519Command(
+            sequence: 2,
+            targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
+        var packet = new byte[FanucJ519Protocol.CommandPacketLength];
+
+        FanucJ519Protocol.PackCommandPacket(command, sequence: 456, packet);
+
+        Assert.Equal(456u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
+        Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
+        Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
+    }
+
     /// <summary>
     /// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
     /// </summary>

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>