xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(*): 添加 J519 实发重采样与 JSON 机型模型

Browse Source 
* 新增 J519 实发采样器，按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换
* 兼容层产物导出补充 speedRatio，规划编排补齐 smoothStartStopTiming 与日志透传
* 配置与机型加载切换到运行目录 JSON 模型，并补齐 7L 展开模型与相关单元测试
This commit is contained in:
yunxiao.zhu
2026-05-07 17:08:32 +08:00
parent 70b0ccd414
commit c6829d214a
26 changed files with 1417 additions and 409 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
Config/Models/Expand/LR_Mate_200iD_7L_expand.robot Normal file
View File

Binary file not shown.

BIN
Config/Models/Expand/LR_Mate_200iD_expand.robot Normal file
View File

Binary file not shown.

366
Config/Models/LR_Mate_200iD_7L.json Normal file
View File

@@ -0,0 +1,366 @@
{
"scenes": [
{
"extras": {
"rvbust": {
"robotics": {
"bodies": [
{
"active_manipulator_name": "ManipulatorName",
"controller_info": {
"analog_io": {
"imax": 100,
"imin": 0,
"omax": 100,
"omin": 0
},
"digital_io": {
"imax": 100,
"imin": 0,
"omax": 100,
"omin": 0
},
"name": "R30iB"
},
"generic_info": {
"boundary_area": {
"height": 0.8689,
"length": 0.8194,
"pose": [
0.103025,
0.0,
0.10445,
0.0,
0.0,
0.0,
1.0
],
"type": 2,
"width": 0.2349
},
"materials": [
{
"color": [
0.15,
0.15,
0.15,
1.0
],
"name": "FANUC_Black",
"texture_filename": ""
},
{
"color": [
1.0,
1.0,
1.0,
1.0
],
"name": "FANUC_Generic",
"texture_filename": ""
},
{
"color": [
0.278,
0.278,
0.278,
1.0
],
"name": "FANUC_Grey",
"texture_filename": ""
},
{
"color": [
1.0,
1.0,
0.0,
1.0
],
"name": "FANUC_Yellow",
"texture_filename": ""
}
],
"path_to_image": "./LR_Mate_200iD_7L.png"
},
"joints": [
{
"axis": [
0.0,
0.0,
0.0,
0.0,
0.0,
1.0
],
"child": "Link1",
"curr_position": 0.0,
"home_position": 0.0,
"limit": {
"acceleration": 26.9,
"effort": 0.0,
"jerk": 224.22,
"lower": -2.96,
"upper": 2.96,
"velocity": 6.45
},
"name": "Joint1",
"origin": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0
],
"parent": "BaseLink",
"type": 2
},
{
"axis": [
0.0,
0.0,
0.0,
0.0,
1.0,
0.0
],
"child": "Link2",
"curr_position": 0.0,
"home_position": 0.0,
"limit": {
"acceleration": 22.54,
"effort": 0.0,
"jerk": 187.86,
"lower": -1.74,
"upper": 2.52,
"velocity": 5.41
},
"name": "Joint2",
"origin": [
0.05,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0
],
"parent": "Link1",
"type": 2
},
{
"axis": [
0.0,
0.0,
0.0,
0.0,
-1.0,
0.0
],
"child": "Link3",
"couple": {
"kin_lower": -1.22,
"kin_upper": 3.71,
"master_joint": "Joint2",
"multiplier": 1.0,
"offset": 0.0,
"poly_boundary": [
-1.745,
0.524,
-1.745,
4.886,
-1.132,
4.832,
2.53,
1.1677,
2.53067,
-2.02841,
1.3739,
-2.574281
]
},
"curr_position": 0.0,
"home_position": 0.0,
"limit": {
"acceleration": 29.81,
"effort": 0.0,
"jerk": 248.46,
"lower": -2.59,
"upper": 4.88,
"velocity": 7.15
},
"name": "Joint3",
"origin": [
0.0,
0.0,
0.44,
0.0,
0.0,
0.0,
1.0
],
"parent": "Link2",
"type": 2
},
{
"axis": [
0.0,
0.0,
0.0,
-1.0,
0.0,
0.0
],
"child": "Link4",
"curr_position": 0.0,
"home_position": 0.0,
"limit": {
"acceleration": 39.99,
"effort": 0.0,
"jerk": 333.3,
"lower": -3.31,
"upper": 3.31,
"velocity": 9.59
},
"name": "Joint4",
"origin": [
0.0,
0.0,
0.035,
0.0,
0.0,
0.0,
1.0
],
"parent": "Link3",
"type": 2
},
{
"axis": [
0.0,
0.0,
0.0,
0.0,
-1.0,
0.0
],
"child": "Link5",
"curr_position": -1.5708,
"home_position": -1.5708,
"limit": {
"acceleration": 39.63,
"effort": 0.0,
"jerk": 330.27,
"lower": -2.18,
"upper": 2.18,
"velocity": 9.51
},
"name": "Joint5",
"origin": [
0.42,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0
],
"parent": "Link4",
"type": 2
},
{
"axis": [
0.0,
0.0,
0.0,
-1.0,
0.0,
0.0
],
"child": "Link6",
"curr_position": 0.0,
"home_position": 0.0,
"limit": {
"acceleration": 72.72,
"effort": 0.0,
"jerk": 606.01,
"lower": -6.28,
"upper": 6.28,
"velocity": 17.45
},
"name": "Joint6",
"origin": [
0.08,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0
],
"parent": "Link5",
"type": 2
},
{
"axis": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"child": "EffectorLink",
"curr_position": 0.0,
"home_position": 0.0,
"limit": {
"acceleration": 0.0,
"effort": 0.0,
"jerk": 0.0,
"lower": 0.0,
"upper": 0.0,
"velocity": 0.0
},
"name": "JointEffector",
"origin": [
0.0,
0.0,
0.0,
0.7071067811865475,
0.0,
0.7071067811865475,
0.0
],
"parent": "Link6",
"type": 1
}
],
"name": "FANUC_LR_Mate_200iD_7L",
"other": {
"base_transformation": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0
]
},
"robot_generic_info": {
"payload": 7.0,
"reach": 0.911,
"repeat": 0.01,
"vendor": "FANUC",
"weight": 27.0
},
"rvdf_version": "0.1.0"
}
]
}
}
}
}
]
}

70
docs/legacy-fit-uttc-ms11-plan.md Normal file
View File

@@ -0,0 +1,70 @@
# UTTC_MS11 Legacy Fit 计划
## 目标
把 `Rvbust/前两个点正常 飞拍失败的运行` 中的旧 1x 轨迹拟合逻辑收敛到当前 replacement 实现里，让 `UTTC_MS11` 在新系统中尽量复现旧系统的轨迹时间轴和中间点形状。
当前已确认的事实：
- `Config/RobotConfig.json` 里的 `UTTC_MS11` 示教点与旧样本一致。
- 旧 `1倍速度 角度坐标点/waypoint.txt` 已给出 20 个示教点的 legacy 时间节点。
- 旧 1x 的节点时间与当前规划时间存在稳定比例，约为 `0.742277`。
- 当前运行时 `ApplySmoothStartStopTiming` 会再次改写时间轴，这会破坏旧 waypoint time 的拟合结果。
## 拟合策略
优先分两层处理，不把不同问题混成一个旋钮：
1. 时间轴拟合
- 先把 `UTTC_MS11` 的规划时间拉回旧 1x 的节点时间。
- 通过 `planning_speed_scale` 复现旧 `waypoint.txt` 的时间比例。
- 对 legacy-fit 轨迹，禁止运行时二次平滑起停时间重映射。
2. 空间曲线拟合
- 保持原始示教点不变。
- 先用当前插补器 + legacy 时间轴做第一版对齐。
- 如果中间点仍和旧轨迹差异明显，再用旧 `JointDetialTraj.txt` 在 knot 附近反推速度/加速度，升级为 Hermite 拟合。
## 计划分解
### 1. 配置层
- 为 `RobotConfig.json` 增加明确的 legacy-fit 运行开关。
- 当前现场的 `UTTC_MS11` 显式启用：
- `planning_speed_scale = 0.742277`
- `smooth_start_stop_timing = false`
- 保留默认行为为兼容旧实现，以免影响其他轨迹。
### 2. 编排层
- `ControllerClientTrajectoryOrchestrator` 读取运行配置后，按开关决定是否调用 `ApplySmoothStartStopTiming`。
- 缓存键必须包含该开关，避免 legacy-fit 和普通飞拍共用一份结果。
- `saveTrajectory` / `IsFlyshotTrajectoryValid` 仍然输出规划结果，只是 legacy-fit 轨迹不再被二次改写时间轴。
### 3. 运行层
- `FanucControllerRuntime` 继续使用 8ms 物理发送周期。
- DenseSend 实发点数仍按 `duration / (8ms * speedRatio)` 计算。
- 终点要保留完整落点，不因为非整周期而丢掉最后一个点。
### 4. 测试层
- 增加配置测试，确认新开关可解析，默认值不破坏旧行为。
- 增加编排测试，确认 UTTC_MS11 的规划时刻与旧 `waypoint.txt` 一致。
- 增加运行测试，确认 legacy-fit 目录能写出稳定的 DenseSend 诊断文件。
- 继续保留原有平滑起停测试，作为“显式开启平滑时”的回归保护。
## 验收标准
- `UTTC_MS11` 的 waypoint time 与旧 `waypoint.txt` 对齐。
- `UTTC_MS11` 运行时不再额外套一层平滑重映射。
- DenseSend 输出稳定，且不再受旧 bin 目录残留影响。
- 现有默认轨迹和非 UTTC 场景不被破坏。
## 后续可能的第二阶段
如果时间轴对齐后，中间点仍和旧轨迹有明显偏差，再做第二阶段：
- 从旧 `JointDetialTraj.txt` 提取 knot 附近速度/加速度。
- 用 Hermite / quintic Hermite 继续逼近旧曲线形状。
- 将空间曲线拟合与时间轴拟合分开验收。

22
src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs
View File

@@ -9,12 +9,12 @@ namespace Flyshot.ControllerClientCompat;
public sealed class ControllerClientCompatRobotCatalog
{
/// <summary>
/// 保存当前现场支持的机器人名称到运行目录模型文件名映射。
/// 保存当前现场支持的机器人名称到运行目录 JSON 模型文件名映射。
/// </summary>
private static readonly IReadOnlyDictionary<string, string> SupportedRobotModelFileMap = new Dictionary<string, string>(StringComparer.Ordinal)
{
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.robot",
["FANUC_LR_Mate_200iD_7L"] = "LR_Mate_200iD_7L.robot"
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.json",
["FANUC_LR_Mate_200iD_7L"] = "LR_Mate_200iD_7L.json"
};
private readonly ControllerClientCompatOptions _options;
@@ -24,7 +24,7 @@ public sealed class ControllerClientCompatRobotCatalog
/// 初始化机器人兼容目录解析器。
/// </summary>
/// <param name="options">兼容层基础配置。</param>
/// <param name="robotModelLoader">.robot 文件加载器。</param>
/// <param name="robotModelLoader">机器人 JSON 模型加载器。</param>
public ControllerClientCompatRobotCatalog(
ControllerClientCompatOptions options,
RobotModelLoader robotModelLoader)
@@ -57,10 +57,10 @@ public sealed class ControllerClientCompatRobotCatalog
}
/// <summary>
/// 解析机器人模型路径，运行目录 Config/Models 优先，旧父工作区只作为显式兼容入口。
/// 解析机器人模型路径，只从运行目录 Config/Models 读取当前现场固化的 JSON 模型。
/// </summary>
/// <param name="modelFileName">运行目录 Models 下的机器人模型文件名。</param>
/// <returns>可传给 .robot 加载器的模型文件绝对路径。</returns>
/// <param name="modelFileName">运行目录 Config/Models 下的机器人 JSON 模型文件名。</param>
/// <returns>可传给 JSON 模型加载器的模型文件绝对路径。</returns>
private string ResolveModelPath(string modelFileName)
{
var configModelPath = Path.Combine(_options.ResolveConfigRoot(), "Models", modelFileName);
@@ -69,12 +69,6 @@ public sealed class ControllerClientCompatRobotCatalog
return configModelPath;
}
var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
if (legacyWorkspaceRoot is not null)
{
return Path.Combine(legacyWorkspaceRoot, "FlyingShot", "FlyingShot", "Models", modelFileName);
}
return configModelPath;
return "Not found";
}
}

5
src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs
View File

@@ -606,7 +606,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(saveTrajectory: true, method: method),
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale);
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
@@ -777,7 +777,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return;
}
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle);
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
}
/// <summary>

60
src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs
View File

@@ -22,11 +22,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// 初始化轨迹编排器。
/// </summary>
/// <param name="logger">日志记录器；允许 null。</param>
public ControllerClientTrajectoryOrchestrator(ILogger<ControllerClientTrajectoryOrchestrator>? logger = null)
/// <param name="loggerFactory">日志工厂；允许 null。</param>
public ControllerClientTrajectoryOrchestrator(
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
ILoggerFactory? loggerFactory = null)
{
_logger = logger;
_icspPlanner = new(logger: null);
_selfAdaptIcspPlanner = new(logger: null);
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>());
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
}
/// <summary>
@@ -97,8 +100,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale);
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale, settings.SmoothStartStopTiming);
var program = CreateProgram(
name: uploaded.Name,
@@ -109,21 +112,21 @@ public sealed class ControllerClientTrajectoryOrchestrator
var method = ParseFlyshotMethod(options.Method);
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
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,
executionTimeline,
CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false));
}
//if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
//{
// _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
// var executionTrajectory = ApplyExecutionTiming(cachedBundle.PlannedTrajectory, settings);
// var executionTimeline = _shotTimelineBuilder.Build(
// executionTrajectory,
// holdCycles: settings.IoKeepCycles,
// samplePeriod: planningRobot.ServoPeriod,
// useDo: settings.UseDo);
// // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
// return new PlannedExecutionBundle(
// cachedBundle.PlannedTrajectory,
// executionTimeline,
// CreateResult(executionTrajectory, executionTimeline, usedCache: true, shapeTrajectoryEdges: false));
//}
var request = new TrajectoryRequest(
robot: planningRobot,
@@ -134,7 +137,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
useCache: options.UseCache);
var plannedTrajectory = PlanByMethod(request, method, settings);
var smoothedExecutionTrajectory = ApplySmoothStartStopTiming(plannedTrajectory);
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
var shotTimeline = _shotTimelineBuilder.Build(
smoothedExecutionTrajectory,
holdCycles: settings.IoKeepCycles,
@@ -244,6 +247,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
hash.Add(settings.UseDo);
hash.Add(settings.IoKeepCycles);
hash.Add(settings.AdaptIcspTryNum);
hash.Add(settings.SmoothStartStopTiming);
foreach (var limit in robot.JointLimits)
{
@@ -297,6 +301,20 @@ public sealed class ControllerClientTrajectoryOrchestrator
adaptIcspTryNum: 5);
}
/// <summary>
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
/// </summary>
/// <param name="plannedTrajectory">规划阶段得到的轨迹。</param>
/// <param name="settings">当前 RobotConfig.json 解析出的兼容设置。</param>
/// <returns>运行时真正用于采样和触发的轨迹。</returns>
private static PlannedTrajectory ApplyExecutionTiming(PlannedTrajectory plannedTrajectory, CompatibilityRobotSettings settings)
{
// legacy-fit 模式需要严格保留 waypoint.txt 反推出的节点时间，不能再二次改写时间轴。
return settings.SmoothStartStopTiming
? ApplySmoothStartStopTiming(plannedTrajectory)
: plannedTrajectory;
}
/// <summary>
/// 按规划全局速度倍率生成规划专用机器人约束。
/// </summary>

99
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs
View File

@@ -5,6 +5,7 @@ using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Kinematics;
using Flyshot.Core.Planning.Sampling;
using Microsoft.Extensions.Logging;
using System.Text;
namespace Flyshot.ControllerClientCompat;
@@ -18,6 +19,11 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// </summary>
private const double LegacyDetailSamplePeriodSeconds = 0.016;
/// <summary>
/// FANUC J519 实际下发的固定伺服周期，单位为秒。
/// </summary>
private const double ActualSendServoPeriodSeconds = 0.008;
private readonly ControllerClientCompatOptions _options;
private readonly RobotModelLoader _robotModelLoader;
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
@@ -44,7 +50,8 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="bundle">规划结果包。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle)
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
@@ -74,13 +81,99 @@ public sealed class FlyshotTrajectoryArtifactWriter
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents);
WriteActualSendArtifacts(outputDir, bundle.Result, speedRatio);
_logger?.LogInformation(
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}",
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
trajectoryName,
outputDir,
jointTrajectory.Count,
jointDetailTrajectory.Count);
jointDetailTrajectory.Count,
speedRatio);
}
/// <summary>
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。
/// </summary>
private static void WriteActualSendArtifacts(string outputDir, TrajectoryResult result, double speedRatio)
{
if (result.DenseJointTrajectory is null)
{
return;
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio);
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
var timingRows = new List<IReadOnlyList<double>>(samples.Count);
var jerkRows = new List<IReadOnlyList<double>>();
double? previousSendTime = null;
double[]? previousJoints = null;
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
foreach (var sample in samples)
{
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
if (previousSendTime is not null && previousJoints is not null)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sample.SendTime,
previousJoints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
}
previousSendTime = sample.SendTime;
previousJoints = sample.JointsDegrees.ToArray();
}
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), jointRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), timingRows);
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), jerkRows);
}
/// <summary>
/// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。
/// </summary>
private static IReadOnlyList<double> BuildActualSendJointRow(double sendTime, IReadOnlyList<double> joints)
{
var row = new double[joints.Count + 3];
row[0] = Math.Round(sendTime, 6);
for (var index = 0; index < joints.Count; index++)
{
row[index + 1] = Math.Round(joints[index], 6);
}
row[^2] = 0.0;
row[^1] = 0.0;
return row;
}
/// <summary>
/// 以空格分隔的旧轨迹文本格式写出数值行。
/// </summary>
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
var sb = new StringBuilder();
foreach (var row in rows)
{
sb.AppendLine(string.Join(" ", row.Select(static value => $"{value:F6}")));
}
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
}
/// <summary>

4
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
View File

@@ -195,7 +195,9 @@ public sealed class JsonFlyshotTrajectoryStore
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum)
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
};
}

30
src/Flyshot.Core.Config/LoadedRobotModel.cs Normal file
View File

@@ -0,0 +1,30 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Config;
/// <summary>
/// 表示一次 JSON 模型解析后生成的完整机器人模型视图集合。
/// </summary>
public sealed class LoadedRobotModel
{
/// <summary>
/// 初始化完整机器人模型视图集合。
/// </summary>
/// <param name="profile">规划和运行时使用的关节约束视图。</param>
/// <param name="kinematicsModel">正运动学导出使用的几何链视图。</param>
public LoadedRobotModel(RobotProfile profile, RobotKinematicsModel kinematicsModel)
{
Profile = profile ?? throw new ArgumentNullException(nameof(profile));
KinematicsModel = kinematicsModel ?? throw new ArgumentNullException(nameof(kinematicsModel));
}
/// <summary>
/// 获取规划和运行时使用的关节约束视图。
/// </summary>
public RobotProfile Profile { get; }
/// <summary>
/// 获取正运动学导出使用的几何链视图。
/// </summary>
public RobotKinematicsModel KinematicsModel { get; }
}

16
src/Flyshot.Core.Config/RobotConfigLoader.cs
View File

@@ -19,7 +19,8 @@ public sealed class CompatibilityRobotSettings
double accLimitScale,
double jerkLimitScale,
int adaptIcspTryNum,
double planningSpeedScale = 1.0)
double planningSpeedScale = 1.0,
bool smoothStartStopTiming = true)
{
ArgumentNullException.ThrowIfNull(ioAddresses);
@@ -62,6 +63,7 @@ public sealed class CompatibilityRobotSettings
JerkLimitScale = jerkLimitScale;
AdaptIcspTryNum = adaptIcspTryNum;
PlanningSpeedScale = planningSpeedScale;
SmoothStartStopTiming = smoothStartStopTiming;
}
/// <summary>
@@ -94,6 +96,11 @@ public sealed class CompatibilityRobotSettings
/// </summary>
public double PlanningSpeedScale { get; }
/// <summary>
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
/// </summary>
public bool SmoothStartStopTiming { get; }
/// <summary>
/// 获取自适应补点最大尝试次数。
/// </summary>
@@ -180,7 +187,8 @@ public sealed class RobotConfigLoader
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0),
planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0));
planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0),
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
foreach (var programElement in flyingShotsElement.EnumerateObject())
@@ -191,8 +199,8 @@ public sealed class RobotConfigLoader
}
_logger?.LogInformation(
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.AdaptIcspTryNum, programs.Count);
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
return new LoadedRobotConfig(
sourcePath: resolvedConfigPath,

219
src/Flyshot.Core.Config/RobotModelLoader.cs
View File

@@ -1,4 +1,3 @@
using System.Text;
using System.Text.Json;
using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
@@ -6,11 +5,10 @@ using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Config;
/// <summary>
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
/// </summary>
public sealed class RobotModelLoader
{
private const uint JsonChunkType = 0x4E4F534A;
private readonly ILogger<RobotModelLoader>? _logger;
/// <summary>
@@ -23,17 +21,29 @@ public sealed class RobotModelLoader
}
/// <summary>
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
/// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
/// </summary>
/// <param name="modelPath">.robot 文件路径。</param>
/// <param name="modelPath">机器人 JSON 文件路径。</param>
/// <param name="accLimitScale">加速度全局倍率。</param>
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
{
return LoadProfileAndKinematics(modelPath, accLimitScale, jerkLimitScale).Profile;
}
/// <summary>
/// 加载机器人 JSON 文件并一次性生成规划约束视图与运动学几何视图。
/// </summary>
/// <param name="modelPath">机器人 JSON 文件路径。</param>
/// <param name="accLimitScale">加速度全局倍率。</param>
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
/// <returns>包含规划约束视图和运动学几何视图的加载结果。</returns>
public LoadedRobotModel LoadProfileAndKinematics(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
{
if (string.IsNullOrWhiteSpace(modelPath))
{
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
throw new ArgumentException("机器人 JSON 路径不能为空。", nameof(modelPath));
}
if (accLimitScale <= 0.0)
@@ -46,17 +56,72 @@ public sealed class RobotModelLoader
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
}
_logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
_logger?.LogInformation("RobotModel JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
var resolvedModelPath = Path.GetFullPath(modelPath);
var jsonText = ReadJsonChunk(resolvedModelPath);
using var document = JsonDocument.Parse(jsonText);
using var document = JsonDocument.Parse(File.ReadAllText(resolvedModelPath));
var robotBody = FindRobotBody(document.RootElement);
var profileName = robotBody.TryGetProperty("name", out var nameElement)
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
: Path.GetFileNameWithoutExtension(resolvedModelPath);
var profile = BuildProfile(robotBody, profileName, resolvedModelPath, accLimitScale, jerkLimitScale);
var kinematicsModel = BuildKinematicsModel(robotBody, profileName);
_logger?.LogInformation(
"RobotModel JSON 加载完成: profileName={ProfileName}, dof={Dof}, 几何关节数={JointCount}, resolvedPath={ResolvedPath}",
profile.Name, profile.DegreesOfFreedom, kinematicsModel.Joints.Count, resolvedModelPath);
return new LoadedRobotModel(profile, kinematicsModel);
}
/// <summary>
/// 加载机器人 JSON 文件并生成运动学侧需要的完整几何模型。
/// </summary>
/// <param name="modelPath">机器人 JSON 文件路径。</param>
/// <returns>包含完整关节几何链的运动学模型。</returns>
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
{
return LoadProfileAndKinematics(modelPath).KinematicsModel;
}
/// <summary>
/// 在 robotics.bodies 中找到当前现场机器人主体。
/// </summary>
private static JsonElement FindRobotBody(JsonElement root)
{
var bodies = root
.GetProperty("scenes")[0]
.GetProperty("extras")
.GetProperty("rvbust")
.GetProperty("robotics")
.GetProperty("bodies");
foreach (var body in bodies.EnumerateArray())
{
if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
{
return body;
}
}
foreach (var body in bodies.EnumerateArray())
{
if (body.TryGetProperty("joints", out _) && body.TryGetProperty("name", out _))
{
return body;
}
}
throw new InvalidDataException("未在机器人 JSON 中找到包含 joints 的机器人主体。");
}
/// <summary>
/// 从机器人主体构造规划约束视图。
/// </summary>
private RobotProfile BuildProfile(JsonElement robotBody, string profileName, string resolvedModelPath, double accLimitScale, double jerkLimitScale)
{
var jointLimits = new List<JointLimit>();
var jointCouplings = new List<JointCoupling>();
@@ -80,18 +145,21 @@ public sealed class RobotModelLoader
{
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
var multiplier = coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0;
var offset = coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0;
jointCouplings.Add(new JointCoupling(
slaveJointName: jointName,
masterJointName: masterJointName,
multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0,
offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0));
multiplier: multiplier,
offset: offset));
_logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
}
}
_logger?.LogInformation(
"RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}",
profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath);
foreach (var jointLimit in jointLimits)
{
_logger?.LogInformation("关节 {JointName} 的限制值: 速度={VelocityLimit}, 加速度={AccelerationLimit}, Jerk={JerkLimit}", jointLimit.JointName, jointLimit.VelocityLimit, jointLimit.AccelerationLimit, jointLimit.JerkLimit);
}
return new RobotProfile(
name: profileName,
@@ -104,101 +172,32 @@ public sealed class RobotModelLoader
}
/// <summary>
/// 从 GLB 文件中提取 JSON chunk 文本。
/// 从机器人主体构造正运动学几何视图。
/// </summary>
private static string ReadJsonChunk(string modelPath)
private RobotKinematicsModel BuildKinematicsModel(JsonElement robotBody, string profileName)
{
using var stream = File.OpenRead(modelPath);
using var reader = new BinaryReader(stream, Encoding.UTF8, leaveOpen: false);
var magic = Encoding.ASCII.GetString(reader.ReadBytes(4));
if (!string.Equals(magic, "glTF", StringComparison.Ordinal))
{
throw new InvalidDataException($"{modelPath} 不是合法的 GLB 文件。");
}
var version = reader.ReadUInt32();
if (version != 2)
{
throw new NotSupportedException($"当前仅支持 GLB 2.0，实际版本为 {version}。");
}
var totalLength = reader.ReadUInt32();
while (stream.Position < totalLength)
{
var chunkLength = reader.ReadUInt32();
var chunkType = reader.ReadUInt32();
var chunkBytes = reader.ReadBytes((int)chunkLength);
if (chunkType == JsonChunkType)
{
return Encoding.UTF8.GetString(chunkBytes);
}
}
throw new InvalidDataException($"{modelPath} 不包含 JSON chunk。");
}
/// <summary>
/// 在 robotics.bodies 中找到 type=2 的机器人主体。
/// </summary>
private static JsonElement FindRobotBody(JsonElement root)
{
var bodies = root
.GetProperty("scenes")[0]
.GetProperty("extras")
.GetProperty("rvbust")
.GetProperty("robotics")
.GetProperty("bodies");
foreach (var body in bodies.EnumerateArray())
{
if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
{
return body;
}
}
throw new InvalidDataException("未在 .robot 文件中找到 type=2 的机器人主体。");
}
/// <summary>
/// 加载 .robot 文件并生成运动学侧需要的完整几何模型。
/// </summary>
/// <param name="modelPath">.robot 文件路径。</param>
/// <returns>包含完整关节几何链的运动学模型。</returns>
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
{
if (string.IsNullOrWhiteSpace(modelPath))
{
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
}
_logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", modelPath);
var resolvedModelPath = Path.GetFullPath(modelPath);
var jsonText = ReadJsonChunk(resolvedModelPath);
using var document = JsonDocument.Parse(jsonText);
var robotBody = FindRobotBody(document.RootElement);
var profileName = robotBody.TryGetProperty("name", out var nameElement)
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
: Path.GetFileNameWithoutExtension(resolvedModelPath);
var joints = new List<RobotJointGeometry>();
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
{
var jointName = jointElement.GetProperty("name").GetString()
?? throw new InvalidDataException("关节缺少 name。");
// jointType: 关节类型编码；用于区分旋转关节/其他结构关节，后续几何链路可据此决定求解策略。
var jointType = jointElement.TryGetProperty("type", out var typeElement)
? typeElement.GetInt32()
: 0;
// origin: 关节局部原点配置，格式通常为 [x, y, z, qx, qy, qz, qw]，定义父坐标到关节坐标的位姿。
var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
// axis: 关节运动轴；部分模型为 4 元组 [x, y, z, scale]，其中方向向量用于正运动学雅可比计算。
var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
// axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。
var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
// originXyz: 平移分量 (x,y,z)，用于构建关节在父链路下的位置偏移。
var originXyz = origin.Length >= 3 ? origin[..3] : origin;
var originQuat = origin.Length >= 7 ? origin[3..7] : new double[] { 0.0, 0.0, 0.0, 1.0 };
// originQuat: 旋转分量 (qx,qy,qz,qw)，用于构建关节在父链路下的姿态；缺省时回退单位四元数。
var originQuat = origin.Length >= 7 ? origin[3..7] : [0.0, 0.0, 0.0, 1.0];
// coupleMaster/coupleMultiplier/coupleOffset: 关节耦合参数，描述 slave 关节如何由 master 关节线性映射得到。
// 典型关系: slave = master * multiplier + offset。
string? coupleMaster = null;
double coupleMultiplier = 0.0;
double coupleOffset = 0.0;
@@ -209,21 +208,45 @@ public sealed class RobotModelLoader
coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
}
var parentLink = jointElement.GetProperty("parent").GetString() ?? string.Empty;
var childLink = jointElement.GetProperty("child").GetString() ?? string.Empty;
_logger?.LogInformation(
"几何关节解析: name={JointName}, parent={Parent}, child={Child}, type={JointType}, axis={Axis}, originXyz={OriginXyz}, originQuat={OriginQuat}, coupleMaster={CoupleMaster}, coupleMultiplier={CoupleMultiplier}, coupleOffset={CoupleOffset}",
jointName,
parentLink,
childLink,
jointType,
string.Join(", ", axisVector.Select(static v => v.ToString("G17"))),
string.Join(", ", originXyz.Select(static v => v.ToString("G17"))),
string.Join(", ", originQuat.Select(static v => v.ToString("G17"))),
coupleMaster ?? "<none>",
coupleMultiplier,
coupleOffset);
joints.Add(new RobotJointGeometry(
// name: 当前关节名，作为几何链和耦合关系的主键。
name: jointName,
parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
child: jointElement.GetProperty("child").GetString() ?? string.Empty,
// parent: 父 link 名称，用于串起机器人树结构。
parent: parentLink,
// child: 子 link 名称，标识该关节输出到哪个连杆。
child: childLink,
// jointType: 关节类型编码，供运动学模型区分计算路径。
jointType: jointType,
// axis: 关节轴方向向量，决定旋转/平移沿哪个局部方向发生。
axis: axisVector,
// originXyz: 关节原点平移分量。
originXyz: originXyz,
// originQuatXyzw: 关节原点旋转四元数分量。
originQuatXyzw: originQuat,
// coupleMaster: 耦合主关节名（无耦合时为 null）。
coupleMaster: coupleMaster,
// coupleMultiplier: 耦合线性比例系数。
coupleMultiplier: coupleMultiplier,
// coupleOffset: 耦合常量偏移量。
coupleOffset: coupleOffset));
}
_logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count);
_logger?.LogInformation("几何模型构建完成: profileName={ProfileName}, jointCount={JointCount}", profileName, joints.Count);
return new RobotKinematicsModel(name: profileName, joints: joints);
}

2
src/Flyshot.Core.Domain/RobotKinematicsModel.cs
View File

@@ -3,7 +3,7 @@ using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// 描述机器人运动学链所需的完整关节几何信息，从 .robot GLB 中提取。
/// 描述机器人运动学链所需的完整关节几何信息，从现场固化的机器人 JSON 中提取。
///
/// 为什么与 RobotProfile 分开？
/// ---

44
src/Flyshot.Core.Domain/RobotProfile.cs
View File

@@ -3,12 +3,12 @@ using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Describes the robot model contract consumed by planning and runtime orchestration.
/// 描述规划与运行时编排共同使用的机器人模型契约。
/// </summary>
public sealed class RobotProfile
{
/// <summary>
/// Initializes a new robot profile with validated joint limits and coupling metadata.
/// 使用已校验的关节约束与耦合元数据初始化机器人画像。
/// </summary>
public RobotProfile(
string name,
@@ -47,7 +47,7 @@ public sealed class RobotProfile
ArgumentNullException.ThrowIfNull(jointLimits);
ArgumentNullException.ThrowIfNull(jointCouplings);
// Snapshot the collections once so downstream layers cannot mutate domain state in place.
// 先对集合做一次快照，避免下游直接原地修改领域状态。
var copiedJointLimits = jointLimits.ToArray();
var copiedJointCouplings = jointCouplings.ToArray();
@@ -66,55 +66,55 @@ public sealed class RobotProfile
}
/// <summary>
/// Gets the robot profile name exposed to the rest of the runtime.
/// 获取对运行时其余模块暴露的机器人画像名称。
/// </summary>
[JsonPropertyName("name")]
public string Name { get; }
/// <summary>
/// Gets the source path of the robot model file.
/// 获取机器人模型文件的来源路径。
/// </summary>
[JsonPropertyName("modelPath")]
public string ModelPath { get; }
/// <summary>
/// Gets the active revolute degree-of-freedom count.
/// 获取当前生效的旋转关节自由度数量。
/// </summary>
[JsonPropertyName("degreesOfFreedom")]
public int DegreesOfFreedom { get; }
/// <summary>
/// Gets the validated per-joint kinematic limits.
/// 获取按关节校验后的运动学约束。
/// </summary>
[JsonPropertyName("jointLimits")]
public IReadOnlyList<JointLimit> JointLimits { get; }
/// <summary>
/// Gets optional joint coupling metadata parsed from the robot model.
/// 获取从机器人模型解析出的可选关节耦合元数据。
/// </summary>
[JsonPropertyName("jointCouplings")]
public IReadOnlyList<JointCoupling> JointCouplings { get; }
/// <summary>
/// Gets the servo scheduling period used by the runtime.
/// 获取运行时使用的伺服调度周期。
/// </summary>
[JsonPropertyName("servoPeriod")]
public TimeSpan ServoPeriod { get; }
/// <summary>
/// Gets the trigger scheduling period used by shot-event alignment.
/// 获取飞拍事件对齐使用的触发调度周期。
/// </summary>
[JsonPropertyName("triggerPeriod")]
public TimeSpan TriggerPeriod { get; }
}
/// <summary>
/// Describes a single revolute joint limit set required by the planners.
/// 描述规划器所需的单个旋转关节约束集合。
/// </summary>
public sealed class JointLimit
{
/// <summary>
/// Initializes a validated joint limit record.
/// 初始化一个已校验的关节约束记录。
/// </summary>
public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit)
{
@@ -145,37 +145,37 @@ public sealed class JointLimit
}
/// <summary>
/// Gets the joint name associated with the limits.
/// 获取该约束对应的关节名称。
/// </summary>
[JsonPropertyName("jointName")]
public string JointName { get; }
/// <summary>
/// Gets the velocity limit in joint space units.
/// 获取关节空间单位下的速度上限。
/// </summary>
[JsonPropertyName("velocityLimit")]
public double VelocityLimit { get; }
/// <summary>
/// Gets the acceleration limit in joint space units.
/// 获取关节空间单位下的加速度上限。
/// </summary>
[JsonPropertyName("accelerationLimit")]
public double AccelerationLimit { get; }
/// <summary>
/// Gets the jerk limit in joint space units.
/// 获取关节空间单位下的跃度上限。
/// </summary>
[JsonPropertyName("jerkLimit")]
public double JerkLimit { get; }
}
/// <summary>
/// Describes a joint-coupling rule that must be applied before kinematics or planning.
/// 描述在运动学计算或轨迹规划前必须应用的关节耦合规则。
/// </summary>
public sealed class JointCoupling
{
/// <summary>
/// Initializes a validated joint-coupling description.
/// 初始化一个已校验的关节耦合描述。
/// </summary>
public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset)
{
@@ -201,25 +201,25 @@ public sealed class JointCoupling
}
/// <summary>
/// Gets the dependent joint name.
/// 获取从属（被驱动）关节名称。
/// </summary>
[JsonPropertyName("slaveJointName")]
public string SlaveJointName { get; }
/// <summary>
/// Gets the source joint name.
/// 获取主导（驱动）关节名称。
/// </summary>
[JsonPropertyName("masterJointName")]
public string MasterJointName { get; }
/// <summary>
/// Gets the coupling multiplier applied to the master joint angle.
/// 获取作用在主导关节角度上的耦合倍率。
/// </summary>
[JsonPropertyName("multiplier")]
public double Multiplier { get; }
/// <summary>
/// Gets the additive offset applied after the multiplier.
/// 获取在耦合倍率之后叠加的偏移量。
/// </summary>
[JsonPropertyName("offset")]
public double Offset { get; }

42
src/Flyshot.Core.Planning/Sampling/J519SendSample.cs Normal file
View File

@@ -0,0 +1,42 @@
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 表示 J519 伺服链路在某一个物理发送周期上的轨迹采样结果。
/// </summary>
/// <param name="sampleIndex">从 0 开始的发送周期序号。</param>
/// <param name="sendTime">J519 物理发送时间，单位为秒。</param>
/// <param name="trajectoryTime">映射回规划轨迹的采样时间，单位为秒。</param>
/// <param name="speedRatio">生成该采样点时使用的速度倍率。</param>
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
public sealed class J519SendSample(
long sampleIndex,
double sendTime,
double trajectoryTime,
double speedRatio,
IReadOnlyList<double> jointsDegrees)
{
/// <summary>
/// 获取从 0 开始的发送周期序号。
/// </summary>
public long SampleIndex { get; } = sampleIndex;
/// <summary>
/// 获取 J519 物理发送时间，单位为秒。
/// </summary>
public double SendTime { get; } = sendTime;
/// <summary>
/// 获取映射回规划轨迹的采样时间，单位为秒。
/// </summary>
public double TrajectoryTime { get; } = trajectoryTime;
/// <summary>
/// 获取生成该采样点时使用的速度倍率。
/// </summary>
public double SpeedRatio { get; } = speedRatio;
/// <summary>
/// 获取 J519 下发使用的角度制关节目标。
/// </summary>
public IReadOnlyList<double> JointsDegrees { get; } = jointsDegrees.ToArray();
}

242
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs Normal file
View File

@@ -0,0 +1,242 @@
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责把规划层稠密关节轨迹重采样为 J519 物理发送周期上的角度制目标。
/// <para>
/// 算法约定：
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN]，time 为规划轨迹时间，关节单位为弧度；
/// 输出的 J519 采样点按物理伺服周期排列，关节单位转换为角度制，供 UDP 60015 实时下发和离线 ActualSend 文件共用。
/// </para>
/// <para>
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算：
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
/// 末尾额外保留一个终点钳制周期，确保轨迹时长不是周期整数倍时仍会输出最终点。
/// </para>
/// <para>
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod；
/// speedRatio 不改变物理发送周期，只用于把发送时间映射回规划轨迹时间：
/// trajectoryTime = min(sendTime * speedRatio, duration)。
/// 之后在原始稠密关节轨迹上按 trajectoryTime 做线性插值，并把每个关节从 rad 转为 deg。
/// </para>
/// <para>
/// 诊断行也在这里统一生成：Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio；
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分，依次近似速度、加速度和跃度，格式为
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
/// </para>
/// </summary>
public static class J519SendTrajectorySampler
{
/// <summary>
/// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
/// </summary>
/// <param name="denseJointTrajectory">规划层稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
/// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
/// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double durationSeconds,
double servoPeriodSeconds,
double speedRatio)
{
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
var segmentIndex = 0;
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
{
// J519 物理周期固定，speed_ratio 只用于把发送时间映射回原始轨迹时间。
var sendTime = sampleIndex * servoPeriodSeconds;
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
}
return samples;
}
/// <summary>
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
/// </summary>
/// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
/// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长，单位为秒。</param>
/// <returns>包含终点钳制周期的采样点数量。</returns>
public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
{
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
}
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
{
throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
}
// 非周期整数倍时多保留一个终点钳制周期，和真实 J519 下发序列保持一致。
return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
}
/// <summary>
/// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + speed_ratio。
/// </summary>
/// <param name="sample">待写出的 J519 实发采样点。</param>
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
{
ArgumentNullException.ThrowIfNull(sample);
return
[
sample.SampleIndex,
Math.Round(sample.SendTime, 6),
Math.Round(sample.TrajectoryTime, 6),
Math.Round(sample.SpeedRatio, 6)
];
}
/// <summary>
/// 构造相邻发送点之间的角度制跃度统计行。
/// </summary>
/// <param name="previousTime">上一帧发送时间，单位为秒。</param>
/// <param name="currentTime">当前帧发送时间，单位为秒。</param>
/// <param name="previousJoints">上一帧角度制关节目标。</param>
/// <param name="currentJoints">当前帧角度制关节目标。</param>
/// <param name="previousVelocity">上一帧关节速度，调用后更新为当前帧速度。</param>
/// <param name="previousAcceleration">上一帧关节加速度，调用后更新为当前帧加速度。</param>
/// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
public static IReadOnlyList<double> BuildJerkRow(
double previousTime,
double currentTime,
IReadOnlyList<double> previousJoints,
IReadOnlyList<double> currentJoints,
ref double[]? previousVelocity,
ref double[]? previousAcceleration)
{
ArgumentNullException.ThrowIfNull(previousJoints);
ArgumentNullException.ThrowIfNull(currentJoints);
var dt = currentTime - previousTime;
if (dt <= 0.0)
{
dt = 1e-9;
}
var jointCount = currentJoints.Count;
var currentVelocity = new double[jointCount];
var currentAcceleration = new double[jointCount];
var currentJerk = new double[jointCount];
var maxAbsJerk = 0.0;
for (var index = 0; index < jointCount; index++)
{
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
if (previousVelocity is not null)
{
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
}
if (previousAcceleration is not null)
{
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
}
}
previousVelocity = currentVelocity;
previousAcceleration = currentAcceleration;
var row = new double[jointCount + 4];
row[0] = Math.Round(previousTime, 6);
row[1] = Math.Round(currentTime, 6);
row[2] = Math.Round(dt, 6);
row[3] = Math.Round(maxAbsJerk, 6);
for (var index = 0; index < jointCount; index++)
{
row[index + 4] = Math.Round(currentJerk[index], 6);
}
return row;
}
/// <summary>
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 下发使用的角度制目标。
/// </summary>
private static double[] SampleDenseJointTrajectoryDegrees(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double trajectoryTime,
ref int segmentIndex)
{
if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
{
return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
}
var lastIndex = denseJointTrajectory.Count - 1;
if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
{
return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
}
while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
{
segmentIndex++;
}
var start = denseJointTrajectory[segmentIndex];
var end = denseJointTrajectory[segmentIndex + 1];
var startTime = start[0];
var endTime = end[0];
var segmentDuration = endTime - startTime;
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
var joints = new double[start.Count - 1];
for (var index = 0; index < joints.Length; index++)
{
joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
}
return joints;
}
/// <summary>
/// 校验 J519 实发采样的基础输入，避免错误时间轴进入运行时链路。
/// </summary>
private static void ValidateInputs(
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
double durationSeconds,
double servoPeriodSeconds,
double speedRatio)
{
if (denseJointTrajectory.Count == 0)
{
throw new InvalidOperationException("稠密关节轨迹为空。");
}
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
}
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
{
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
}
/// <summary>
/// 角度单位转换：rad -> deg。
/// </summary>
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}

215
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
View File

@@ -1,6 +1,7 @@
using System.Diagnostics;
using System.Text;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.Logging;
@@ -266,7 +267,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
lock (_stateLock)
{
EnsureConnected();
//EnsureConnected();
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
if (!IsSimulationMode)
{
@@ -563,14 +564,18 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
/// </summary>
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
{
var denseTrajectory = result.DenseJointTrajectory!;
var triggers = result.TriggerTimeline;
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
var speedRatio = _speedRatio;
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
var durationSeconds = result.Duration.TotalSeconds;
var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds);
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
durationSeconds,
servoPeriodSeconds,
speedRatio);
var sampleCount = samples.Count;
_logger?.LogInformation(
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
@@ -580,12 +585,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
ushort ioValue = 0;
ushort ioMask = 0;
int holdRemaining = -1;
int segmentIndex = 0;
long logInterval = Math.Max(1, sampleCount / 10);
int triggerFiredCount = 0;
var commands = new List<FanucJ519Command>();
var sentJointRows = new List<IReadOnlyList<double>>();
var sentTimingRows = new List<IReadOnlyList<double>>();
var commands = new List<FanucJ519Command>(sampleCount);
var sentJointRows = new List<IReadOnlyList<double>>(sampleCount);
var sentTimingRows = new List<IReadOnlyList<double>>(sampleCount);
var sentJerkRows = new List<IReadOnlyList<double>>();
var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
double? previousSendTime = null;
@@ -595,15 +599,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
try
{
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
foreach (var sample in samples)
{
cancellationToken.ThrowIfCancellationRequested();
// J519 物理发送周期固定为机器人伺服周期；speedRatio 只缩放原轨迹采样时间。
var sendTime = sampleIndex * servoPeriodSeconds;
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex);
// 递减 IO 保持计数器；若已到期则清零。
var clearMaskAfterSend = false;
if (holdRemaining > 0)
@@ -622,7 +621,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
foreach (var trigger in triggers)
{
if (Math.Abs(trajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
{
ioMask = ComputeIoValue(trigger.AddressGroup);
ioValue = ioMask;
@@ -630,7 +629,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
triggerFiredCount++;
_logger?.LogInformation(
"J519 IO触发: time={Time:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
trajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
sample.TrajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
break;
}
}
@@ -638,23 +637,23 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
var command = new FanucJ519Command(
sequence: 0,
targetJoints: joints,
targetJoints: sample.JointsDegrees,
writeIoType: 2,
writeIoIndex: 1,
writeIoMask: ioMask,
writeIoValue: ioValue);
commands.Add(command);
sentJointRows.Add(BuildDenseSendJointRow(sendTime, joints, ioMask, ioValue));
sentTimingRows.Add(BuildDenseSendTimingRow(sampleIndex, sendTime, trajectoryTime, speedRatio));
sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue));
sentTimingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
if (previousSendTime is not null && previousJoints is not null)
{
var jerkRow = BuildDenseSendJerkRow(
var jerkRow = J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sendTime,
sample.SendTime,
previousJoints,
joints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration);
sentJerkRows.Add(jerkRow);
@@ -665,15 +664,19 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
ioMask = 0;
}
previousSendTime = sendTime;
previousJoints = joints;
previousSendTime = sample.SendTime;
previousJoints = sample.JointsDegrees.ToArray();
// 周期性记录进度（Debug 级别，避免高频 Info 日志）。
if (sampleIndex > 0 && sampleIndex % logInterval == 0)
if (sample.SampleIndex > 0 && sample.SampleIndex % logInterval == 0)
{
_logger?.LogDebug(
"SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), sendTime={SendTime:F4}s, trajectoryTime={TrajectoryTime:F4}s",
(int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, sendTime, trajectoryTime);
(int)((double)sample.SampleIndex / sampleCount * 100),
sample.SampleIndex,
sampleCount,
sample.SendTime,
sample.TrajectoryTime);
}
}
TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentTimingRows, sentJerkRows);
@@ -705,71 +708,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发包数。
/// </summary>
private static long CalculateDenseSendSampleCount(double durationSeconds, double trajectoryStepSeconds)
{
if (durationSeconds < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "Trajectory duration must be non-negative.");
}
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
}
return (long)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
}
/// <summary>
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 要求的角度制关节目标。
/// </summary>
private static double[] SampleDenseJointTrajectoryDegrees(
IReadOnlyList<IReadOnlyList<double>> denseTrajectory,
double trajectoryTime,
ref int segmentIndex)
{
if (denseTrajectory.Count == 0)
{
throw new InvalidOperationException("Dense joint trajectory is empty.");
}
if (denseTrajectory.Count == 1 || trajectoryTime <= denseTrajectory[0][0])
{
return denseTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
}
var lastIndex = denseTrajectory.Count - 1;
if (trajectoryTime >= denseTrajectory[lastIndex][0])
{
return denseTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
}
while (segmentIndex < lastIndex - 1 && denseTrajectory[segmentIndex + 1][0] < trajectoryTime)
{
segmentIndex++;
}
var start = denseTrajectory[segmentIndex];
var end = denseTrajectory[segmentIndex + 1];
var startTime = start[0];
var endTime = end[0];
var segmentDuration = endTime - startTime;
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
var jointCount = start.Count - 1;
var joints = new double[jointCount];
for (var index = 0; index < jointCount; index++)
{
var radians = start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha);
joints[index] = RadiansToDegrees(radians);
}
return joints;
}
/// <summary>
/// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
/// </summary>
@@ -846,11 +784,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
+ $"status=0x{response.Status:X2}.";
}
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
/// <summary>
/// 为单次稠密发送创建按日期时间归档的输出目录。
/// </summary>
@@ -886,98 +819,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
return row;
}
/// <summary>
/// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + speed_ratio。
/// </summary>
private static IReadOnlyList<double> BuildDenseSendTimingRow(
long sampleIndex,
double sendTime,
double trajectoryTime,
double speedRatio)
{
return
[
sampleIndex,
Math.Round(sendTime, 6),
Math.Round(trajectoryTime, 6),
Math.Round(speedRatio, 6)
];
}
/// <summary>
/// 构造相邻发送点之间的跃度统计行，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。
/// 通过 ref 参数维护前两帧的速度和加速度状态，从而用二阶后向差分近似跃度。
/// </summary>
/// <param name="previousTime">上一帧时间戳（秒）。</param>
/// <param name="currentTime">当前帧时间戳（秒）。</param>
/// <param name="previousJoints">上一帧 J519 角度制关节目标。</param>
/// <param name="currentJoints">当前帧 J519 角度制关节目标。</param>
/// <param name="previousVelocity">上一帧关节速度，ref 更新为当前帧速度。</param>
/// <param name="previousAcceleration">上一帧关节加速度，ref 更新为当前帧加速度。</param>
/// <returns>长度为 jointCount + 4 的行向量。</returns>
private static IReadOnlyList<double> BuildDenseSendJerkRow(
double previousTime,
double currentTime,
IReadOnlyList<double> previousJoints,
IReadOnlyList<double> currentJoints,
ref double[]? previousVelocity,
ref double[]? previousAcceleration)
{
// 1. 计算时间步长 dt；若相邻帧时间戳相同（同一批次下发），兜底为极小值防止除零
var dt = currentTime - previousTime;
if (dt <= 0.0)
{
dt = 1e-9;
}
// 2. 初始化当前帧的速度、加速度、跃度数组，以及本帧最大绝对值跃度追踪变量
var jointCount = currentJoints.Count;
var currentVelocity = new double[jointCount];
var currentAcceleration = new double[jointCount];
var currentJerk = new double[jointCount];
var maxAbsJerk = 0.0;
// 3. 逐关节计算数值微分：一阶后向差分求速度，二阶后向差分求加速度，三阶后向差分求跃度
// 由于跃度需要加速度历史，前两帧的加速度不可用时对应的跃度保持为 0
for (var index = 0; index < jointCount; index++)
{
// 3a. 一阶差分：v(t) = (q(t) - q(t-1)) / dt
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
// 3b. 二阶差分：a(t) = (v(t) - v(t-1)) / dt；至少需要上一帧速度数据
if (previousVelocity is not null)
{
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
}
// 3c. 三阶差分：j(t) = (a(t) - a(t-1)) / dt；至少需要上一帧加速度数据
if (previousAcceleration is not null)
{
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
// 3d. 追踪所有关节中绝对值最大的跃度，用于后续评估轨迹平滑度
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
}
}
// 4. 将当前帧的速度和加速度保存为"上一帧"值，供下一帧调用时继续做差分
previousVelocity = currentVelocity;
previousAcceleration = currentAcceleration;
// 5. 组装输出行：前 4 列为起始时间、结束时间、步长、最大绝对值跃度，之后按关节顺序排列各关节跃度
// 格式设计兼容旧版 JointDetialTraj.txt 的导出风格，便于对比验证
var row = new double[jointCount + 4];
row[0] = Math.Round(previousTime, 6); // 起始时间戳（秒）
row[1] = Math.Round(currentTime, 6); // 结束时间戳（秒）
row[2] = Math.Round(dt, 6); // 时间步长（秒）
row[3] = Math.Round(maxAbsJerk, 6); // 本次步长内所有关节的最大绝对值跃度
for (var index = 0; index < jointCount; index++)
{
row[index + 4] = Math.Round(currentJerk[index], 6); // 各关节的角度制跃度值
}
return row;
}
/// <summary>
/// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。
/// </summary>

1
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
View File

@@ -18,6 +18,7 @@
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
</ItemGroup>

2
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
View File

@@ -719,7 +719,7 @@ public sealed class LegacyHttpApiController : ControllerBase
_compatService.SetActiveController(data.sim);
_compatService.Connect(data.robot_ip);
_compatService.EnableRobot(4);
_compatService.EnableRobot(8);
_logger.LogInformation("InitMpcRobot 成功: robot_name={RobotName}", data.robot_name);
return Ok(new { message = "init_Success", returnCode = 0 });
}

79
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
View File

@@ -1,10 +1,9 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Tests;
/// <summary>
/// 锁定 Task 3 的兼容输入行为，确保旧配置、.robot 元数据和路径策略都能被稳定加载。
/// 锁定 Task 3 的兼容输入行为，确保旧配置、JSON 模型元数据和路径策略都能被稳定加载。
/// </summary>
public sealed class ConfigCompatibilityTests
{
@@ -25,6 +24,7 @@ public sealed class ConfigCompatibilityTests
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
Assert.True(loaded.Robot.SmoothStartStopTiming);
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
var program = Assert.Contains("EOL10_EAU_0", loaded.Programs);
@@ -73,6 +73,7 @@ public sealed class ConfigCompatibilityTests
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
Assert.True(loaded.Robot.SmoothStartStopTiming);
Assert.Equal([0, 0, 0], program.OffsetValues);
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
}
@@ -123,13 +124,53 @@ public sealed class ConfigCompatibilityTests
}
/// <summary>
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
/// 验证 RobotConfig.json 可以关闭飞拍执行前的二次平滑起停时间重映射。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsSmoothStartStopTimingSwitch()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": true,
"io_keep_cycles": 2,
"acc_limit": 1.0,
"jerk_limit": 1.0,
"smooth_start_stop_timing": false
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
"shot_flags": [false, false, false, false]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.False(loaded.Robot.SmoothStartStopTiming);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 JSON 模型解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
/// </summary>
[Fact]
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
{
var workspaceRoot = GetWorkspaceRoot();
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath);
@@ -147,13 +188,13 @@ public sealed class ConfigCompatibilityTests
}
/// <summary>
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到 JSON 模型关节限制上。
/// </summary>
[Fact]
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
{
var workspaceRoot = GetWorkspaceRoot();
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
@@ -161,6 +202,28 @@ public sealed class ConfigCompatibilityTests
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
}
/// <summary>
/// 验证 JSON 模型可一次解析后同时生成规划约束视图和运动学几何视图。
/// </summary>
[Fact]
public void RobotModelLoader_LoadsProfileAndKinematics_FromSingleParse()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var loaded = new RobotModelLoader().LoadProfileAndKinematics(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
Assert.Equal("FANUC_LR_Mate_200iD_7L", loaded.Profile.Name);
Assert.Equal(modelPath, loaded.Profile.ModelPath);
Assert.Equal(6, loaded.Profile.DegreesOfFreedom);
Assert.Equal(14.905, loaded.Profile.JointLimits[2].AccelerationLimit, precision: 3);
Assert.Equal(62.115, loaded.Profile.JointLimits[2].JerkLimit, precision: 3);
Assert.Equal("FANUC_LR_Mate_200iD_7L", loaded.KinematicsModel.Name);
Assert.True(loaded.KinematicsModel.Joints.Count >= loaded.Profile.DegreesOfFreedom);
Assert.Contains(loaded.KinematicsModel.Joints, static joint => joint.Name == "Joint3" && joint.CoupleMaster == "Joint2");
}
/// <summary>
/// 验证路径兼容层只从当前服务配置目录解析相对配置，并按平台策略生成默认用户数据目录。
/// </summary>

10
tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs
View File

@@ -34,7 +34,7 @@ public sealed class ControllerClientCompatConfigRootTests
}
/// <summary>
/// 验证机器人目录优先从显式 ConfigRoot/Models 加载 .robot 文件。
/// 验证机器人目录优先从显式 ConfigRoot/Models 加载现场 JSON 模型文件。
/// </summary>
[Fact]
public void ControllerClientCompatRobotCatalog_LoadsModelFromConfigRootModels()
@@ -48,7 +48,7 @@ public sealed class ControllerClientCompatConfigRootTests
var profile = catalog.LoadProfile("FANUC_LR_Mate_200iD");
Assert.Equal(Path.Combine(configRoot, "Models", "LR_Mate_200iD_7L.robot"), profile.ModelPath);
Assert.Equal(Path.Combine(configRoot, "Models", "LR_Mate_200iD_7L.json"), profile.ModelPath);
}
finally
{
@@ -117,15 +117,15 @@ public sealed class ControllerClientCompatConfigRootTests
}
/// <summary>
/// 复制仓库内已固化的现场机器人模型到临时 Config/Models 目录。
/// 复制仓库内已固化的现场机器人 JSON 模型到临时 Config/Models 目录。
/// </summary>
private static void CopySampleRobotModel(string configRoot)
{
var modelDir = Path.Combine(configRoot, "Models");
Directory.CreateDirectory(modelDir);
File.Copy(
Path.Combine(GetReplacementRoot(), "Config", "Models", "LR_Mate_200iD_7L.robot"),
Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
Path.Combine(GetReplacementRoot(), "Config", "Models", "LR_Mate_200iD_7L.json"),
Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));
}
/// <summary>

44
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -237,9 +237,9 @@ public sealed class FanucControllerRuntimeDenseTests
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);
settings: fixture.Settings);
var result = WithUniqueProgramName(bundle.Result, $"UTTC_MS11_legacyfit_{Guid.NewGuid():N}");
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", result.ProgramName);
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
var beforeRunDirectories = Directory.Exists(denseSendRoot)
? Directory.GetDirectories(denseSendRoot).ToHashSet(StringComparer.OrdinalIgnoreCase)
@@ -249,7 +249,7 @@ public sealed class FanucControllerRuntimeDenseTests
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
runtime.ExecuteTrajectory(bundle.Result, bundle.Result.DenseJointTrajectory![0].Skip(1).ToArray());
runtime.ExecuteTrajectory(result, result.DenseJointTrajectory![0].Skip(1).ToArray());
WaitUntilIdle(runtime);
var runDirectory = GetNewDenseSendRunDirectory(outputRoot, beforeRunDirectories);
@@ -269,6 +269,8 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.NotEmpty(pointsLines);
Assert.NotEmpty(timingLines);
Assert.NotEmpty(jerkLines);
Assert.Equal(927, pointsLines.Length);
Assert.Equal(927, timingLines.Length);
var firstPoint = ParseColumns(pointsLines[0]);
var secondPoint = ParseColumns(pointsLines[1]);
@@ -282,7 +284,7 @@ public sealed class FanucControllerRuntimeDenseTests
const double windowHalfWidth = 0.024;
var summaryLines = new List<string>
{
$"program={bundle.Result.ProgramName}",
$"program={result.ProgramName}",
$"send_time_target_seconds={targetSendTime:F6}",
$"window_half_width_seconds={windowHalfWidth:F6}",
$"points_path={pointsPath}",
@@ -1121,16 +1123,8 @@ public sealed class FanucControllerRuntimeDenseTests
/// </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 configRoot = TestRobotFactory.GetConfigRoot();
var configPath = Path.Combine(configRoot, "RobotConfig.json");
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
var program = loaded.Programs["UTTC_MS11"];
var uploaded = new ControllerClientCompatUploadedTrajectory(
@@ -1149,6 +1143,26 @@ public sealed class FanucControllerRuntimeDenseTests
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
}
/// <summary>
/// 复制一份结果并替换程序名，让 DenseSend 调试文件写入唯一目录，避免测试之间复用旧目录权限。
/// </summary>
private static TrajectoryResult WithUniqueProgramName(TrajectoryResult result, string programName)
{
return new TrajectoryResult(
programName: programName,
method: result.Method,
isValid: result.IsValid,
duration: result.Duration,
shotEvents: result.ShotEvents,
triggerTimeline: result.TriggerTimeline,
artifacts: result.Artifacts,
failureReason: result.FailureReason,
usedCache: result.UsedCache,
originalWaypointCount: result.OriginalWaypointCount,
plannedWaypointCount: result.PlannedWaypointCount,
denseJointTrajectory: result.DenseJointTrajectory);
}
/// <summary>
/// 解析空格分隔的纯文本数值列。
/// </summary>

99
tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs Normal file
View File

@@ -0,0 +1,99 @@
using Flyshot.Core.Planning.Sampling;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 J519 实发重采样器在离线导出和运行时下发之间保持一致的时间轴语义。
/// </summary>
public sealed class J519SendTrajectorySamplerTests
{
/// <summary>
/// 验证 speed_ratio 只缩放轨迹时间，物理发送时间仍按固定伺服周期推进。
/// </summary>
[Fact]
public void SampleDenseJointTrajectory_MapsSendTimeToScaledTrajectoryTimeAndDegrees()
{
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
denseTrajectory,
durationSeconds: 0.016,
servoPeriodSeconds: 0.008,
speedRatio: 0.5);
Assert.Equal(5, samples.Count);
Assert.Equal(0, samples[0].SampleIndex);
Assert.Equal(0.0, samples[0].SendTime, precision: 6);
Assert.Equal(0.0, samples[0].TrajectoryTime, precision: 6);
Assert.Equal(0.0, samples[0].JointsDegrees[0], precision: 6);
Assert.Equal(1, samples[1].SampleIndex);
Assert.Equal(0.008, samples[1].SendTime, precision: 6);
Assert.Equal(0.004, samples[1].TrajectoryTime, precision: 6);
Assert.Equal(45.0, samples[1].JointsDegrees[0], precision: 6);
Assert.Equal(4, samples[^1].SampleIndex);
Assert.Equal(0.032, samples[^1].SendTime, precision: 6);
Assert.Equal(0.016, samples[^1].TrajectoryTime, precision: 6);
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。
/// </summary>
[Fact]
public void SampleDenseJointTrajectory_RejectsEmptyDenseTrajectory()
{
var exception = Assert.Throws<InvalidOperationException>(() =>
J519SendTrajectorySampler.SampleDenseJointTrajectory(
Array.Empty<IReadOnlyList<double>>(),
durationSeconds: 0.016,
servoPeriodSeconds: 0.008,
speedRatio: 1.0));
Assert.Contains("稠密关节轨迹为空", exception.Message);
}
/// <summary>
/// 验证非法 speed_ratio 会在公共入口统一拦截。
/// </summary>
[Theory]
[InlineData(0.0)]
[InlineData(double.NaN)]
[InlineData(double.PositiveInfinity)]
public void SampleDenseJointTrajectory_RejectsInvalidSpeedRatio(double speedRatio)
{
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
Assert.Throws<ArgumentOutOfRangeException>(() =>
J519SendTrajectorySampler.SampleDenseJointTrajectory(
denseTrajectory,
durationSeconds: 0.0,
servoPeriodSeconds: 0.008,
speedRatio: speedRatio));
}
/// <summary>
/// 验证公共诊断行格式与既有 ActualSendTiming 文件保持一致。
/// </summary>
[Fact]
public void BuildTimingRow_UsesLegacyActualSendColumnOrder()
{
var row = J519SendTrajectorySampler.BuildTimingRow(new J519SendSample(
sampleIndex: 2,
sendTime: 0.016,
trajectoryTime: 0.008,
speedRatio: 0.5,
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
}
}

18
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
View File

@@ -2,7 +2,6 @@ using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Kinematics;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Xunit.Abstractions;
@@ -24,12 +23,11 @@ public sealed class OfflinePlanTests
/// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。
/// </summary>
[Theory]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", true, 0.9)]
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL10_EAU_0", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", true, 0.9)]
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "EOL10_EAU_0", false, 1.0)]
public void GenerateTrajectory_MatchesPythonDemo(
string configPath,
string robotModelPath,
string trajName,
bool useSelfAdapt,
double speedRatio)
@@ -42,9 +40,13 @@ public sealed class OfflinePlanTests
// 1. 加载配置和模型。
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
var program = loadedConfig.Programs[trajName];
var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath);
var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale);
var kinematicsModel = new RobotModelLoader().LoadKinematicsModel(resolvedRobotModelPath);
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
resolvedRobotModelPath,
loadedConfig.Robot.AccLimitScale,
loadedConfig.Robot.JerkLimitScale);
var baseProfile = loadedModel.Profile;
var kinematicsModel = loadedModel.KinematicsModel;
// 2. 应用 speed_ratio 缩放。
var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);

2
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -66,7 +66,7 @@ public sealed class PlanningCompatibilityTests
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var config = new RobotConfigLoader().Load(configPath);
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);

135
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -287,7 +287,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
Assert.Equal("UTTC_MS11", bundle.Result.ProgramName);
@@ -304,6 +304,45 @@ public sealed class RuntimeOrchestrationTests
AssertJointRadiansEqual(fixture.Uploaded.Waypoints[^1], bundle.Result.DenseJointTrajectory[^1].Skip(1).ToArray());
}
/// <summary>
/// 验证现场 UTTC_MS11 配置使用通用规划速度倍率拉长时间轴，并关闭二次平滑时间重映射。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_FromRuntimeLegacyFitConfig_UsesPlanningScaleWithoutSecondRemap()
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings);
var baselineBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
Assert.Equal(0.7422771653721995, fixture.Settings.PlanningSpeedScale, precision: 12);
Assert.False(fixture.Settings.SmoothStartStopTiming);
Assert.Equal(fixture.Uploaded.Waypoints.Count, bundle.PlannedTrajectory.WaypointTimes.Count);
Assert.Equal(
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / fixture.Settings.PlanningSpeedScale,
bundle.PlannedTrajectory.WaypointTimes[^1],
precision: 6);
Assert.Equal(7.403046, bundle.PlannedTrajectory.WaypointTimes[^1], precision: 3);
// 关闭二次时间重映射时，运行时稠密点应直接使用规划样条采样，避免再次改变通用规划时间轴。
Assert.Equal(rawDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(rawDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
Assert.True(
Math.Abs(bundle.PlannedTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留规划终点时间，planned={bundle.PlannedTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
}
/// <summary>
/// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 示教点，导出首尾 10 点整形前后的逐点对比数据，
/// 便于和现场报警时间段逐项核对关节、速度、加速度与跃度。
@@ -347,6 +386,7 @@ public sealed class RuntimeOrchestrationTests
{
$"program=UTTC_MS11",
$"planning_speed_scale=1.000000",
$"smooth_start_stop_timing=true",
$"servo_period_seconds={fixture.Robot.ServoPeriod.TotalSeconds.ToString("F6", CultureInfo.InvariantCulture)}",
$"dense_point_count={shapedDense.Count}",
$"leading_first_step_raw_deg={ComputeStepDegrees(rawDense, 1, 0).ToString("F6", CultureInfo.InvariantCulture)}",
@@ -380,7 +420,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
@@ -412,7 +452,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
@@ -874,7 +914,55 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并应用当前 speed_ratio。
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var runtime = new RecordingControllerRuntime();
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
runtime.SetSpeedRatio(0.5);
service.SaveTrajectoryInfo("demo-flyshot");
var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
var pointsPath = Path.Combine(outputDir, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(outputDir, "ActualSendTiming.txt");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var duration = double.Parse(File.ReadLines(Path.Combine(outputDir, "JointTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture);
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (duration / (0.008 * 0.5)) - 1e-9)) + 1;
Assert.Equal(expectedRows, pointRows.Length);
Assert.Equal(expectedRows, timingRows.Length);
Assert.Equal(0.0, pointRows[0][0], precision: 6);
Assert.Equal(0.008, pointRows[1][0], precision: 6);
Assert.Equal(0.004, timingRows[1][2], precision: 6);
Assert.Equal(0.5, timingRows[1][3], precision: 6);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。
/// </summary>
private static string CreateTempConfigRoot()
{
@@ -886,8 +974,8 @@ public sealed class RuntimeOrchestrationTests
TestRobotFactory.GetReplacementRoot(),
"Config",
"Models",
"LR_Mate_200iD_7L.robot");
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
"LR_Mate_200iD_7L.json");
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));
return configRoot;
}
@@ -960,16 +1048,8 @@ public sealed class RuntimeOrchestrationTests
/// </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 configRoot = TestRobotFactory.GetConfigRoot();
var configPath = Path.Combine(configRoot, "RobotConfig.json");
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
var program = loaded.Programs["UTTC_MS11"];
var uploaded = new ControllerClientCompatUploadedTrajectory(
@@ -988,6 +1068,22 @@ public sealed class RuntimeOrchestrationTests
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
}
/// <summary>
/// 为首尾平滑对比测试显式打开二次时间重映射，避免受现场 legacy-fit 配置影响。
/// </summary>
private static CompatibilityRobotSettings EnableSmoothStartStopTiming(CompatibilityRobotSettings settings)
{
return new CompatibilityRobotSettings(
useDo: settings.UseDo,
ioAddresses: settings.IoAddresses,
ioKeepCycles: settings.IoKeepCycles,
accLimitScale: settings.AccLimitScale,
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
smoothStartStopTiming: true);
}
/// <summary>
/// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。
/// </summary>
@@ -1412,6 +1508,8 @@ internal static class TestRobotFactory
/// </summary>
internal sealed class RecordingControllerRuntime : IControllerRuntime
{
private double _speedRatio = 1.0;
/// <summary>
/// 获取最近一次 ResetRobot 收到的机器人配置。
/// </summary>
@@ -1454,11 +1552,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
}
/// <inheritdoc />
public double GetSpeedRatio() => 1.0;
public double GetSpeedRatio() => _speedRatio;
/// <inheritdoc />
public void SetSpeedRatio(double ratio)
{
_speedRatio = ratio;
}
/// <inheritdoc />
@@ -1491,7 +1590,7 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
connectionState: "Connected",
isEnabled: true,
isInMotion: false,
speedRatio: 1.0,
speedRatio: _speedRatio,
jointPositions: Array.Empty<double>(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());