✨ feat(fanuc): 优化 J519 实时下发与飞拍起停整形
- 改为高优先级 J519 接收线程与复用缓冲区发送链路 - 增加稠密执行前的 J519 就绪重试与状态诊断 - 修正程序状态响应字段顺序与 EnableRobot 默认参数 - 为飞拍轨迹补充平滑起停时间轴与首尾整形验证 - 补充真实运行配置、报警窗口与边界对比测试 - 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
This commit is contained in:
2
.gitignore
vendored
2
.gitignore
vendored
@@ -398,3 +398,5 @@ FodyWeavers.xsd
|
||||
*.sln.iml
|
||||
Config/Data/*
|
||||
.dotnet-home/*
|
||||
codex-dotnet-home/*
|
||||
.dotnet-sdk8/*
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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`
|
||||
|
||||
6
global.json
Normal file
6
global.json
Normal file
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"sdk": {
|
||||
"version": "8.0.420",
|
||||
"rollForward": "disable"
|
||||
}
|
||||
}
|
||||
@@ -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,
|
||||
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
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
if (response is null)
|
||||
EnsureJ519ReadyForDenseExecutionCore(
|
||||
() => _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(
|
||||
"J519 status is not ready for dense execution: "
|
||||
throw new InvalidOperationException(BuildJ519DenseExecutionNotReadyMessage(response));
|
||||
}
|
||||
|
||||
/// <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)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
_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)
|
||||
{
|
||||
// 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。
|
||||
}
|
||||
}
|
||||
|
||||
/// <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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="response">刚收到的状态包。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
private async Task SendCommandForStatusAsync(FanucJ519Response response, CancellationToken cancellationToken)
|
||||
/// <param name="socket">已连接的 UDP 套接字。</param>
|
||||
/// <param name="response">刚收到的机器人状态包。</param>
|
||||
/// <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
|
||||
? 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
|
||||
}
|
||||
}
|
||||
|
||||
/// <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>
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
/// <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.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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user