diff --git a/.gitignore b/.gitignore index 8224f3a..72a39a3 100644 --- a/.gitignore +++ b/.gitignore @@ -398,3 +398,5 @@ FodyWeavers.xsd *.sln.iml Config/Data/* .dotnet-home/* +codex-dotnet-home/* +.dotnet-sdk8/* diff --git a/analysis/calc_jointdetail_acceleration.py b/analysis/calc_jointdetail_acceleration.py index d0d719f..5156b81 100644 --- a/analysis/calc_jointdetail_acceleration.py +++ b/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,14 +396,20 @@ def build_report_text( f"joint_count={len(rows[0]) - 1}", f"inferred_unit={unit}", f"max_abs_joint_value={max_abs_joint:.6f}", - "", - build_metric_section("Velocity Peaks", velocity_peaks, unit, "s", "s", "VelLimit(rad/s)"), - "", - build_metric_section("Acceleration Peaks", acceleration_peaks, unit, "s^2", "s^2", "AccLimit(rad/s^2)"), - "", - build_metric_section("Jerk Peaks", jerk_peaks, unit, "s^3", "s^3", "JerkLimit(rad/s^3)"), - "", ] + 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)"), + "", + build_metric_section("Acceleration Peaks", acceleration_peaks, unit, "s^2", "s^2", "AccLimit(rad/s^2)"), + "", + 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, diff --git a/docs/move-joint-jerk-comparison-20260505.md b/docs/move-joint-jerk-comparison-20260505.md index 61a6eaa..126476c 100644 --- a/docs/move-joint-jerk-comparison-20260505.md +++ b/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` diff --git a/global.json b/global.json new file mode 100644 index 0000000..6661367 --- /dev/null +++ b/global.json @@ -0,0 +1,6 @@ +{ + "sdk": { + "version": "8.0.420", + "rollForward": "disable" + } +} diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index 164359c..dfb27fa 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -66,7 +66,7 @@ public sealed class ControllerClientTrajectoryOrchestrator var plannedTrajectory = PlanByMethod(request, method); var shotTimeline = new ShotTimeline(Array.Empty(), Array.Empty()); - 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, - CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true)); + executionTimeline, + 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 /// 规划后的轨迹。 /// 触发时间轴。 /// 运行时执行结果描述。 - 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); } + + /// + /// 为飞拍执行生成一条平滑起停的时间轴。 + /// 保持路点位置不变，只重映射路点时刻，让起点和终点附近的速度自然收敛。 + /// + 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); + } + + /// + /// 反解 7 次 smootherstep 的时间进度，用二分法把原始线性进度映射成平滑时间轴。 + /// + 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; + } + + /// + /// 计算 7 次 smootherstep 进度值，用于整段平滑起停时间律。 + /// + 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); + } } diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryEdgeShaper.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryEdgeShaper.cs new file mode 100644 index 0000000..9515930 --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryEdgeShaper.cs @@ -0,0 +1,217 @@ +namespace Flyshot.ControllerClientCompat; + +/// +/// 对飞拍稠密关节轨迹的首尾采样点做速度整形，降低启动和结束时的单步角度变化。 +/// +internal static class FlyshotTrajectoryEdgeShaper +{ + /// + /// 首尾整形默认覆盖的采样点数（含锚点）。 + /// + internal const int DefaultEdgePointCount = 10; + + /// + /// 对稠密关节轨迹做首尾整形，时间列保持不变，首段采用 ease-in，尾段采用 ease-out。 + /// + /// 输入稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。 + /// 保留旧签名兼容调用方；当前实现不再按角度阈值扩窗。 + /// 单侧整形覆盖的采样点数（含锚点），默认首尾各 10 点。 + /// 经过首尾整形后的新轨迹；若不满足整形条件则返回原轨迹副本。 + internal static IReadOnlyList> ShapeDenseJointTrajectory( + IReadOnlyList> denseJointTrajectory, + double maxEdgeStepDegrees = 0.0, + int maxWindowPoints = DefaultEdgePointCount) + { + ArgumentNullException.ThrowIfNull(denseJointTrajectory); + if (denseJointTrajectory.Count == 0) + { + return Array.Empty>(); + } + + 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; + } + + /// + /// 对首段做单段 Hermite 累计位移整形：起点速度为 0，窗口末端按原轨迹边界速度接回中段。 + /// + 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); + } + } + } + + /// + /// 对尾段做单段 Hermite 累计位移整形：窗口起点按原轨迹边界速度接入，终点速度减到 0。 + /// + 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); + } + } + } + + /// + /// 估算给定行在原始轨迹上的一阶导，首尾退化为单边差分。 + /// + 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; + } + + /// + /// 估算给定行在原始轨迹上的二阶导，端点退化为 0 以避免放大边界噪声。 + /// + 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; + } + + /// + /// 计算 Hermite 累计位移曲线在 0..1 归一化时间上的进度值。 + /// + 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); + } + + /// + /// 把归一化边界斜率限制在单调 Hermite 常见的稳定区间内，避免过冲和窗口内振荡。 + /// + private static double ClampNormalizedSlope(double normalizedSlope) + { + if (double.IsNaN(normalizedSlope) || double.IsInfinity(normalizedSlope)) + { + return 0.0; + } + + return Math.Clamp(normalizedSlope, 0.0, 3.0); + } + + /// + /// 在线性插值基础上做温和混合，避免首尾窗口为了追赶锚点而产生过大的局部跃度。 + /// + private static double Lerp(double originalValue, double shapedValue, double weight) + { + var clampedWeight = Math.Clamp(weight, 0.0, 1.0); + return originalValue + ((shapedValue - originalValue) * clampedWeight); + } +} diff --git a/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs b/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs index 35f682c..1d28dff 100644 --- a/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs +++ b/src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs @@ -19,7 +19,8 @@ public static class TrajectorySampler public static IReadOnlyList> 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(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(pose.Length + 1); row.Add(Math.Round(t, decimals)); @@ -103,4 +111,26 @@ public static class TrajectorySampler return times; } + + /// + /// 把线性采样时间映射为整段平滑起停的评估时间。 + /// 使用 7 次 smootherstep 时间律，让起点和终点的一到三阶导都自然收敛到 0。 + /// + 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; + } } diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index 570f10f..13d05ba 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -187,8 +187,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable return; } - // 真机模式：走完整 RVBUSTSM 启动序列（与抓包一致）。 - _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); + // 真机模式：按 all-reconnect.pcap 的重连序列启动 RVBUSTSM，暂不发送 StopProg。 + _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); _commandClient.ResetRobotAsync().GetAwaiter().GetResult(); _commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult(); _commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); @@ -683,7 +683,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable if (_j519Client.IsConnected) { _j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult(); - } + } _logger?.LogInformation( "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 队列装载耗时={ElapsedMs}ms", sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds); @@ -775,25 +775,75 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable /// private void EnsureJ519ReadyForDenseExecution() { - var response = _j519Client.GetLatestResponse(); - if (response is null) + EnsureJ519ReadyForDenseExecutionCore( + () => _j519Client.GetLatestResponse(), + () => + { + // 若当前状态未就绪，则先尝试一次 EnableRobot 走现场既有恢复链路。 + EnableRobot(_bufferSize); + }, + static () => Thread.Sleep(TimeSpan.FromMilliseconds(500))); + } + + /// + /// 执行稠密发送前的 J519 就绪校验；未就绪时允许先做一次启机重试。 + /// + /// 读取最新 J519 状态的委托。 + /// 状态未就绪时触发一次 EnableRobot 重试的委托。 + /// 重试后等待状态刷新的委托。 + internal static void EnsureJ519ReadyForDenseExecutionCore( + Func 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( - "J519 status is not ready for dense execution: " + throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response)); + } + + /// + /// 判断当前 J519 响应是否已经满足稠密执行前置条件。 + /// + /// 待检查的 J519 状态响应。 + /// 接受命令且系统就绪时返回 true，否则返回 false。 + internal static bool IsJ519ReadyForDenseExecution(FanucJ519Response response) + { + ArgumentNullException.ThrowIfNull(response); + return response.AcceptsCommand && response.SystemReady; + } + + /// + /// 组装 J519 未就绪时的异常消息，便于保留现场关键状态位。 + /// + /// 最近一次收到的 J519 状态响应。 + /// 包含状态位和序号的诊断消息。 + 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) diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs index 463e0a2..5f29dc2 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs @@ -548,9 +548,9 @@ public static class FanucCommandProtocol throw new InvalidDataException("FANUC 程序状态响应体长度不足。"); } - // 抓包样本中的字段顺序为 result_code 后接 prog_status。 - var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]); - var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint))); + // all-reconnect.pcap 中字段顺序为 prog_status 后接 result_code。 + var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]); + var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint))); return new FanucProgramStatusResponse(messageId, resultCode, programStatus); } diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs index ff22c0f..c5ba285 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs +++ b/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? _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? _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(); } /// @@ -205,7 +216,6 @@ public sealed class FanucJ519Client : IDisposable return waitTask.WaitAsync(cancellationToken); } - /// /// 判断当前是否没有等待出队的命令；仅供单元测试断言。 /// /// 如果队列为空或尚未装载队列，则返回 true。 @@ -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(); - _receiveTask = null; + _receiveThread = 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( - sequence, - source.TargetJoints, - source.LastData, - source.ReadIoType, - source.ReadIoIndex, - source.ReadIoMask, - source.DataStyle, - source.WriteIoType, - source.WriteIoIndex, - source.WriteIoMask, - source.WriteIoValue); + try + { + var process = Process.GetCurrentProcess(); + if (process.PriorityClass < ProcessPriorityClass.High) + { + process.PriorityClass = ProcessPriorityClass.High; + } + } + catch (Exception) + { + // 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。 + } } /// - /// 后台接收循环：持续接收 132B 响应并解析。 + /// 配置 J519 UDP 套接字的低延迟参数。 /// - private async Task ReceiveLoopAsync(CancellationToken cancellationToken) + /// 已连接 FANUC 60015 的 UDP 套接字。 + 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 优先级标记"); + } + } + + /// + /// 后台接收循环：在专用高优先级线程中同步接收 132B 响应并立即回发命令。 + /// + 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); - if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength) + var received = udpClient.Client.Receive(receiveBuffer); + 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); + } } /// /// 按机器人状态包的 sequence 立即回发当前 J519 命令。 /// - /// 刚收到的状态包。 - /// 取消令牌。 - private async Task SendCommandForStatusAsync(FanucJ519Response response, CancellationToken cancellationToken) + /// 已连接的 UDP 套接字。 + /// 刚收到的机器人状态包。 + /// 可复用的 64B 命令包缓冲区。 + /// 收到状态包后的时间戳。 + 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 - ? null - : WithSequence(_currentCommand, response.Sequence); + command = _currentCommand; } } @@ -480,14 +531,15 @@ public sealed class FanucJ519Client : IDisposable return; } - var packet = FanucJ519Protocol.PackCommandPacket(command); - await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false); - _logger?.LogDebug("J519 已回发命令包，seq={Seq}", command.Sequence); - _logger?.LogTrace( - "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}", - command.TargetJoints, - command.WriteIoMask, - command.WriteIoValue); + FanucJ519Protocol.PackCommandPacket(command, response.Sequence, commandBuffer); + socket.Send(commandBuffer); + TrackReceiveToSendLatency(receiveTicks); + // _logger?.LogDebug("J519 已回发命令包，seq={Seq}", sequence); + // _logger?.LogDebug( + // "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}", + // command.TargetJoints, + // command.WriteIoMask, + // command.WriteIoValue); lock (_commandLock) { _lastSentCommand = command; @@ -498,6 +550,26 @@ public sealed class FanucJ519Client : IDisposable } } + /// + /// 记录状态包到命令包发出的最大耗时和慢发送次数，供低频诊断日志观察调度抖动。 + /// + /// 收到状态包后的时间戳。 + 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); + } + } + /// /// 清空当前命令队列，并唤醒等待队列结束的运行时任务。 /// diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs index d84409e..cb13f70 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs +++ b/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); - BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1); - BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence); + PackCommandPacket(command, command.Sequence, packet); + return packet; + } + + /// + /// 将 J519 64 字节命令包写入调用方提供的缓冲区。 + /// + /// 命令数据。 + /// 本次出包使用的机器人状态包序号。 + /// 长度至少为 64 字节的命令包缓冲区。 + public static void PackCommandPacket(FanucJ519Command command, uint sequence, Span 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.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask); + BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort)), command.ReadIoIndex); + 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.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask); - BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue); - BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0); + BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x14, sizeof(ushort)), command.WriteIoIndex); + BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x16, sizeof(ushort)), command.WriteIoMask); + BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x18, sizeof(ushort)), command.WriteIoValue); + 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; } /// diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs index 080c3b1..5b522b5 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs @@ -52,12 +52,12 @@ public sealed class FanucStateClientOptions /// /// 获取或设置重连等待时间的上限。 /// - public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(2); + public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(5); /// /// 获取或设置单次 TCP 建连允许的最长时间。 /// - public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(2); + public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(5); } /// diff --git a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs index a94065a..55f5812 100644 --- a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs +++ b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs @@ -154,12 +154,12 @@ public sealed class LegacyHttpApiController : ControllerBase } /// - /// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状。 + /// 兼容旧 `EnableRobot(buffer_size=4)` 参数形状。 /// /// 控制器执行缓冲区大小。 /// 旧 FastAPI 层风格的布尔状态响应。 [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 diff --git a/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs b/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs index a08dacc..60dfcc1 100644 --- a/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs +++ b/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); diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs index 4482c4b..be70e27 100644 --- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -221,6 +221,101 @@ public sealed class FanucControllerRuntimeDenseTests } } + /// + /// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 轨迹执行一次完整 1x 稠密下发， + /// 并把 0.088s 报警窗口附近的实发时间、关节与跃度摘要落盘，便于继续对照现场报警帧。 + /// + [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(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 + { + $"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)); + } + /// /// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。 /// @@ -509,6 +604,58 @@ public sealed class FanucControllerRuntimeDenseTests Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions); } + /// + /// 验证稠密执行前若 J519 首次未就绪，会先尝试一次 EnableRobot，再在 500ms 后复查状态。 + /// + [Fact] + public void EnsureJ519ReadyForDenseExecutionCore_RetriesEnableRobotOnceBeforePassing() + { + var responses = new Queue( + [ + 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); + } + + /// + /// 验证重试 EnableRobot 之后若 J519 仍未就绪，会继续抛出带状态位的异常。 + /// + [Fact] + public void EnsureJ519ReadyForDenseExecutionCore_ThrowsWhenStillNotReadyAfterRetry() + { + var responses = new Queue( + [ + CreateJ519Response(status: 0b0000, sequence: 21), + CreateJ519Response(status: 0b0010, sequence: 22) + ]); + var enableRobotRetryCount = 0; + var waitCount = 0; + + var exception = Assert.Throws( + () => 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); + } + /// /// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。 /// @@ -728,6 +875,30 @@ public sealed class FanucControllerRuntimeDenseTests } } + /// + /// 创建用于就绪状态测试的最小 J519 响应。 + /// + /// 待注入的 J519 状态位。 + /// 待注入的响应序号。 + /// 包含最小字段集合的测试响应。 + 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]; } + /// + /// 获取本次测试刚生成的新稠密发送记录目录，避免误读历史运行产物。 + /// + private static string GetNewDenseSendRunDirectory(string outputRoot, IReadOnlySet 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]; + } + + /// + /// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。 + /// + 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); + } + /// /// 解析空格分隔的纯文本数值列。 /// @@ -984,4 +1204,13 @@ public sealed class FanucControllerRuntimeDenseTests Assert.NotNull(field); field.SetValue(client, response); } + + /// + /// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。 + /// + private sealed record UttcMs11RuntimeFixture( + string ConfigRoot, + CompatibilityRobotSettings Settings, + ControllerClientCompatUploadedTrajectory Uploaded, + RobotProfile Robot); } diff --git a/tests/Flyshot.Core.Tests/FanucProtocolTests.cs b/tests/Flyshot.Core.Tests/FanucProtocolTests.cs index 9f2eec3..2b331b2 100644 --- a/tests/Flyshot.Core.Tests/FanucProtocolTests.cs +++ b/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))); } + /// + /// 验证实时发送路径可以复用命令对象，并用机器人状态包序号覆盖出包序号。 + /// + [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))); + } + /// /// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。 /// diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index 56811e6..e63a0cb 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -1,9 +1,13 @@ using Flyshot.ControllerClientCompat; using Flyshot.Core.Config; using Flyshot.Core.Domain; +using Flyshot.Core.Planning; +using Flyshot.Core.Planning.Sampling; using Flyshot.Runtime.Common; using Flyshot.Runtime.Fanuc; using Flyshot.Runtime.Fanuc.Protocol; +using System.Globalization; +using System.Reflection; namespace Flyshot.Core.Tests; @@ -270,6 +274,364 @@ public sealed class RuntimeOrchestrationTests Assert.Equal(0.008, firstDt, precision: 3); } + /// + /// 验证可直接使用宿主输出目录 Config/RobotConfig.json 中的真实 UTTC_MS11 示教点， + /// 在 1x 规划速度下生成一条完整飞拍轨迹。 + /// + [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()); + } + + /// + /// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 示教点，导出首尾 10 点整形前后的逐点对比数据， + /// 便于和现场报警时间段逐项核对关节、速度、加速度与跃度。 + /// + [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 + { + $"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)); + } + + /// + /// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会降低首尾窗口最大跃度，而不是只把第一步压小。 + /// + [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}"); + } + + /// + /// 验证运行时 UTTC_MS11 在 1x 下的平滑起停会缩小首尾第一步， + /// 但不能把首尾直接压成静止平台，否则现场执行会变成先停住再起步。 + /// + [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}"); + } + + /// + /// 对比真实可运行轨迹前 0.04s 反推出的局部三次边界特征与当前首段样条系数， + /// 用于判断当前 clamped-zero 边界是否已经和现场可运行轨迹明显偏离。 + /// + [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(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(fieldH!.GetValue(spline)); + var coeffA = Assert.IsType(fieldA!.GetValue(spline)); + var coeffB = Assert.IsType(fieldB!.GetValue(spline)); + var coeffC = Assert.IsType(fieldC!.GetValue(spline)); + + var lines = new List + { + $"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)); + } + + /// + /// 验证飞拍规划结果会应用平滑起停时间轴，而不是直接复用原始线性时间轴采样。 + /// + [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); + } + + /// + /// 验证旧的首尾整形工具在独立调用时仍会改动首尾窗口，供对比和回归保留。 + /// + [Fact] + public void FlyshotTrajectoryEdgeShaper_ShapesLeadingAndTrailingStepsWithoutFlatteningMiddleSection() + { + var denseTrajectory = new IReadOnlyList[] + { + [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"); + } + + /// + /// 验证飞拍首尾整形只覆盖首尾各 10 个点，锚点之外的中段采样保持不变。 + /// + [Fact] + public void FlyshotTrajectoryEdgeShaper_ShapesOnlyLeadingAndTrailingTenPoints() + { + var denseTrajectory = Enumerable.Range(0, 31) + .Select(index => (IReadOnlyList) + [ + 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); + } + /// /// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。 /// @@ -564,8 +926,369 @@ public sealed class RuntimeOrchestrationTests } """); } + + /// + /// 角度转弧度，供轨迹整形测试构造输入。 + /// + private static double DegreesToRadians(double degrees) + { + return degrees * Math.PI / 180.0; + } + + /// + /// 弧度转角度，供轨迹整形测试验证单步变化量。 + /// + private static double RadiansToDegrees(double radians) + { + return radians * 180.0 / Math.PI; + } + + /// + /// 验证稠密轨迹端点仍与原始示教点一致。 + /// + private static void AssertJointRadiansEqual(IReadOnlyList expectedRadians, IReadOnlyList actualRadians) + { + Assert.Equal(expectedRadians.Count, actualRadians.Count); + for (var index = 0; index < expectedRadians.Count; index++) + { + Assert.Equal(expectedRadians[index], actualRadians[index], precision: 6); + } + } + + /// + /// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。 + /// + 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); + } + + /// + /// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。 + /// + private static void WriteEdgeComparisonCsv( + string path, + IReadOnlyList> rawDense, + IReadOnlyList> shapedDense, + int startIndex, + int count) + { + var lines = new List + { + BuildEdgeComparisonHeader() + }; + + for (var index = startIndex; index < startIndex + count && index < rawDense.Count && index < shapedDense.Count; index++) + { + var columns = new List + { + 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); + } + + /// + /// 构造首尾整形逐点对比 CSV 表头。 + /// + private static string BuildEdgeComparisonHeader() + { + var columns = new List + { + "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); + } + + /// + /// 追加 J1..J6 表头列名。 + /// + private static void AppendJointHeader(List columns, string prefix) + { + for (var jointIndex = 1; jointIndex <= 6; jointIndex++) + { + columns.Add($"{prefix}_j{jointIndex}"); + } + } + + /// + /// 追加某一行对应的六轴数值列。 + /// + private static void AppendJointColumns( + List columns, + string prefix, + int rowIndex, + IReadOnlyList> rows, + Func>, int, int, double> selector) + { + for (var jointIndex = 0; jointIndex < 6; jointIndex++) + { + columns.Add(selector(rows, rowIndex, jointIndex).ToString("F6", CultureInfo.InvariantCulture)); + } + } + + /// + /// 计算某一行相对上一行的单步角度变化量。 + /// + private static double ComputeStepDegrees(IReadOnlyList> rows, int rowIndex, int jointIndex) + { + if (rowIndex <= 0) + { + return 0.0; + } + + return RadiansToDegrees(rows[rowIndex][jointIndex + 1] - rows[rowIndex - 1][jointIndex + 1]); + } + + /// + /// 计算某一行的角速度，单位 deg/s。 + /// + private static double ComputeVelocityDegrees(IReadOnlyList> 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; + } + + /// + /// 计算某一行的角加速度，单位 deg/s^2。 + /// + private static double ComputeAccelerationDegrees(IReadOnlyList> 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; + } + + /// + /// 计算某一行的角跃度，单位 deg/s^3。 + /// + private static double ComputeJerkDegrees(IReadOnlyList> 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; + } + + /// + /// 统计给定窗口内所有关节的最大绝对值跃度，便于快速看首尾尖峰是否被压低。 + /// + private static double ComputeWindowMaxAbsJerkDegrees( + IReadOnlyList> 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; + } + + /// + /// 解析空格分隔的关节轨迹行。 + /// + private static double[] ParseSpaceSeparatedDoubles(string line) + { + return line + .Split(' ', StringSplitOptions.RemoveEmptyEntries) + .Select(static value => double.Parse(value, CultureInfo.InvariantCulture)) + .ToArray(); + } + + /// + /// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线，返回相对首点时间的系数。 + /// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0，单位保持输入文件的角度制。 + /// + private static LocalCubicFit FitCubicFromFirstFourRows(IReadOnlyList 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); + } + + /// + /// 用高斯消元求解 4x4 线性方程组。 + /// + 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; + } } +/// +/// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。 +/// +internal sealed record UttcMs11RuntimeFixture( + string ConfigRoot, + CompatibilityRobotSettings Settings, + ControllerClientCompatUploadedTrajectory Uploaded, + RobotProfile Robot); + +/// +/// 记录基于真实轨迹前 4 点拟合得到的局部三次曲线系数。 +/// +internal sealed record LocalCubicFit( + double Coefficient3, + double Coefficient2, + double Coefficient1, + double Coefficient0, + double LastLocalTime); + /// /// 为运行时编排测试构造稳定的最小领域对象。 ///