diff --git a/Config/Models/Expand/LR_Mate_200iD_7L_expand.robot b/Config/Models/Expand/LR_Mate_200iD_7L_expand.robot new file mode 100644 index 0000000..28248e6 Binary files /dev/null and b/Config/Models/Expand/LR_Mate_200iD_7L_expand.robot differ diff --git a/Config/Models/Expand/LR_Mate_200iD_expand.robot b/Config/Models/Expand/LR_Mate_200iD_expand.robot new file mode 100644 index 0000000..6cbeda6 Binary files /dev/null and b/Config/Models/Expand/LR_Mate_200iD_expand.robot differ diff --git a/Config/Models/LR_Mate_200iD_7L.json b/Config/Models/LR_Mate_200iD_7L.json new file mode 100644 index 0000000..0c65b17 --- /dev/null +++ b/Config/Models/LR_Mate_200iD_7L.json @@ -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" + } + ] + } + } + } + } + ] +} \ No newline at end of file diff --git a/docs/legacy-fit-uttc-ms11-plan.md b/docs/legacy-fit-uttc-ms11-plan.md new file mode 100644 index 0000000..9b20962 --- /dev/null +++ b/docs/legacy-fit-uttc-ms11-plan.md @@ -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 继续逼近旧曲线形状。 +- 将空间曲线拟合与时间轴拟合分开验收。 diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs index 5f97ddf..e5a4e73 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs @@ -9,12 +9,12 @@ namespace Flyshot.ControllerClientCompat; public sealed class ControllerClientCompatRobotCatalog { /// - /// 保存当前现场支持的机器人名称到运行目录模型文件名映射。 + /// 保存当前现场支持的机器人名称到运行目录 JSON 模型文件名映射。 /// private static readonly IReadOnlyDictionary SupportedRobotModelFileMap = new Dictionary(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 /// 初始化机器人兼容目录解析器。 /// /// 兼容层基础配置。 - /// .robot 文件加载器。 + /// 机器人 JSON 模型加载器。 public ControllerClientCompatRobotCatalog( ControllerClientCompatOptions options, RobotModelLoader robotModelLoader) @@ -57,10 +57,10 @@ public sealed class ControllerClientCompatRobotCatalog } /// - /// 解析机器人模型路径，运行目录 Config/Models 优先，旧父工作区只作为显式兼容入口。 + /// 解析机器人模型路径，只从运行目录 Config/Models 读取当前现场固化的 JSON 模型。 /// - /// 运行目录 Models 下的机器人模型文件名。 - /// 可传给 .robot 加载器的模型文件绝对路径。 + /// 运行目录 Config/Models 下的机器人 JSON 模型文件名。 + /// 可传给 JSON 模型加载器的模型文件绝对路径。 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"; } } diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index f65e97e..05984a8 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -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); } /// diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index dfb27fa..0d13841 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -22,11 +22,14 @@ public sealed class ControllerClientTrajectoryOrchestrator /// 初始化轨迹编排器。 /// /// 日志记录器；允许 null。 - public ControllerClientTrajectoryOrchestrator(ILogger? logger = null) + /// 日志工厂；允许 null。 + public ControllerClientTrajectoryOrchestrator( + ILogger? logger = null, + ILoggerFactory? loggerFactory = null) { _logger = logger; - _icspPlanner = new(logger: null); - _selfAdaptIcspPlanner = new(logger: null); + _icspPlanner = new(logger: loggerFactory?.CreateLogger()); + _selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger()); } /// @@ -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); } + /// + /// 按运行配置决定是否对规划结果做执行前时间轴重映射。 + /// + /// 规划阶段得到的轨迹。 + /// 当前 RobotConfig.json 解析出的兼容设置。 + /// 运行时真正用于采样和触发的轨迹。 + private static PlannedTrajectory ApplyExecutionTiming(PlannedTrajectory plannedTrajectory, CompatibilityRobotSettings settings) + { + // legacy-fit 模式需要严格保留 waypoint.txt 反推出的节点时间，不能再二次改写时间轴。 + return settings.SmoothStartStopTiming + ? ApplySmoothStartStopTiming(plannedTrajectory) + : plannedTrajectory; + } + /// /// 按规划全局速度倍率生成规划专用机器人约束。 /// diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs index d25312c..7b8f88d 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs @@ -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 /// private const double LegacyDetailSamplePeriodSeconds = 0.016; + /// + /// FANUC J519 实际下发的固定伺服周期，单位为秒。 + /// + private const double ActualSendServoPeriodSeconds = 0.008; + private readonly ControllerClientCompatOptions _options; private readonly RobotModelLoader _robotModelLoader; private readonly ILogger? _logger; @@ -44,7 +50,8 @@ public sealed class FlyshotTrajectoryArtifactWriter /// 飞拍轨迹名称。 /// 当前机器人配置。 /// 规划结果包。 - public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle) + /// 导出 J519 实发采样点时使用的速度倍率。 + 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); + } + + /// + /// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。 + /// + 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>(samples.Count); + var timingRows = new List>(samples.Count); + var jerkRows = new List>(); + 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); + } + + /// + /// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。 + /// + private static IReadOnlyList BuildActualSendJointRow(double sendTime, IReadOnlyList 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; + } + + /// + /// 以空格分隔的旧轨迹文本格式写出数值行。 + /// + private static void WriteDenseRows(string path, IReadOnlyList> 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)); } /// diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs index 29444a2..6eaf159 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs @@ -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) }; } diff --git a/src/Flyshot.Core.Config/LoadedRobotModel.cs b/src/Flyshot.Core.Config/LoadedRobotModel.cs new file mode 100644 index 0000000..9a78086 --- /dev/null +++ b/src/Flyshot.Core.Config/LoadedRobotModel.cs @@ -0,0 +1,30 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.Core.Config; + +/// +/// 表示一次 JSON 模型解析后生成的完整机器人模型视图集合。 +/// +public sealed class LoadedRobotModel +{ + /// + /// 初始化完整机器人模型视图集合。 + /// + /// 规划和运行时使用的关节约束视图。 + /// 正运动学导出使用的几何链视图。 + public LoadedRobotModel(RobotProfile profile, RobotKinematicsModel kinematicsModel) + { + Profile = profile ?? throw new ArgumentNullException(nameof(profile)); + KinematicsModel = kinematicsModel ?? throw new ArgumentNullException(nameof(kinematicsModel)); + } + + /// + /// 获取规划和运行时使用的关节约束视图。 + /// + public RobotProfile Profile { get; } + + /// + /// 获取正运动学导出使用的几何链视图。 + /// + public RobotKinematicsModel KinematicsModel { get; } +} diff --git a/src/Flyshot.Core.Config/RobotConfigLoader.cs b/src/Flyshot.Core.Config/RobotConfigLoader.cs index 99a2875..90dcaf9 100644 --- a/src/Flyshot.Core.Config/RobotConfigLoader.cs +++ b/src/Flyshot.Core.Config/RobotConfigLoader.cs @@ -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; } /// @@ -94,6 +96,11 @@ public sealed class CompatibilityRobotSettings /// public double PlanningSpeedScale { get; } + /// + /// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。 + /// + public bool SmoothStartStopTiming { get; } + /// /// 获取自适应补点最大尝试次数。 /// @@ -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(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, diff --git a/src/Flyshot.Core.Config/RobotModelLoader.cs b/src/Flyshot.Core.Config/RobotModelLoader.cs index 701e68b..638a81d 100644 --- a/src/Flyshot.Core.Config/RobotModelLoader.cs +++ b/src/Flyshot.Core.Config/RobotModelLoader.cs @@ -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; /// -/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。 +/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。 /// public sealed class RobotModelLoader { - private const uint JsonChunkType = 0x4E4F534A; private readonly ILogger? _logger; /// @@ -23,17 +21,29 @@ public sealed class RobotModelLoader } /// - /// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。 + /// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。 /// - /// .robot 文件路径。 + /// 机器人 JSON 文件路径。 /// 加速度全局倍率。 /// Jerk 全局倍率。 /// 包含关节限制和 couple 信息的 RobotProfile。 public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0) + { + return LoadProfileAndKinematics(modelPath, accLimitScale, jerkLimitScale).Profile; + } + + /// + /// 加载机器人 JSON 文件并一次性生成规划约束视图与运动学几何视图。 + /// + /// 机器人 JSON 文件路径。 + /// 加速度全局倍率。 + /// Jerk 全局倍率。 + /// 包含规划约束视图和运动学几何视图的加载结果。 + 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); + } + + /// + /// 加载机器人 JSON 文件并生成运动学侧需要的完整几何模型。 + /// + /// 机器人 JSON 文件路径。 + /// 包含完整关节几何链的运动学模型。 + public RobotKinematicsModel LoadKinematicsModel(string modelPath) + { + return LoadProfileAndKinematics(modelPath).KinematicsModel; + } + + /// + /// 在 robotics.bodies 中找到当前现场机器人主体。 + /// + 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 的机器人主体。"); + } + + /// + /// 从机器人主体构造规划约束视图。 + /// + private RobotProfile BuildProfile(JsonElement robotBody, string profileName, string resolvedModelPath, double accLimitScale, double jerkLimitScale) + { var jointLimits = new List(); var jointCouplings = new List(); @@ -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 } /// - /// 从 GLB 文件中提取 JSON chunk 文本。 + /// 从机器人主体构造正运动学几何视图。 /// - 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。"); - } - - /// - /// 在 robotics.bodies 中找到 type=2 的机器人主体。 - /// - 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 的机器人主体。"); - } - - /// - /// 加载 .robot 文件并生成运动学侧需要的完整几何模型。 - /// - /// .robot 文件路径。 - /// 包含完整关节几何链的运动学模型。 - 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(); 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 ?? "", + 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); } diff --git a/src/Flyshot.Core.Domain/RobotKinematicsModel.cs b/src/Flyshot.Core.Domain/RobotKinematicsModel.cs index c06c7f4..9afb373 100644 --- a/src/Flyshot.Core.Domain/RobotKinematicsModel.cs +++ b/src/Flyshot.Core.Domain/RobotKinematicsModel.cs @@ -3,7 +3,7 @@ using System.Text.Json.Serialization; namespace Flyshot.Core.Domain; /// -/// 描述机器人运动学链所需的完整关节几何信息，从 .robot GLB 中提取。 +/// 描述机器人运动学链所需的完整关节几何信息，从现场固化的机器人 JSON 中提取。 /// /// 为什么与 RobotProfile 分开？ /// --- diff --git a/src/Flyshot.Core.Domain/RobotProfile.cs b/src/Flyshot.Core.Domain/RobotProfile.cs index 05f92b8..a7f623a 100644 --- a/src/Flyshot.Core.Domain/RobotProfile.cs +++ b/src/Flyshot.Core.Domain/RobotProfile.cs @@ -3,12 +3,12 @@ using System.Text.Json.Serialization; namespace Flyshot.Core.Domain; /// -/// Describes the robot model contract consumed by planning and runtime orchestration. +/// 描述规划与运行时编排共同使用的机器人模型契约。 /// public sealed class RobotProfile { /// - /// Initializes a new robot profile with validated joint limits and coupling metadata. + /// 使用已校验的关节约束与耦合元数据初始化机器人画像。 /// 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 } /// - /// Gets the robot profile name exposed to the rest of the runtime. + /// 获取对运行时其余模块暴露的机器人画像名称。 /// [JsonPropertyName("name")] public string Name { get; } /// - /// Gets the source path of the robot model file. + /// 获取机器人模型文件的来源路径。 /// [JsonPropertyName("modelPath")] public string ModelPath { get; } /// - /// Gets the active revolute degree-of-freedom count. + /// 获取当前生效的旋转关节自由度数量。 /// [JsonPropertyName("degreesOfFreedom")] public int DegreesOfFreedom { get; } /// - /// Gets the validated per-joint kinematic limits. + /// 获取按关节校验后的运动学约束。 /// [JsonPropertyName("jointLimits")] public IReadOnlyList JointLimits { get; } /// - /// Gets optional joint coupling metadata parsed from the robot model. + /// 获取从机器人模型解析出的可选关节耦合元数据。 /// [JsonPropertyName("jointCouplings")] public IReadOnlyList JointCouplings { get; } /// - /// Gets the servo scheduling period used by the runtime. + /// 获取运行时使用的伺服调度周期。 /// [JsonPropertyName("servoPeriod")] public TimeSpan ServoPeriod { get; } /// - /// Gets the trigger scheduling period used by shot-event alignment. + /// 获取飞拍事件对齐使用的触发调度周期。 /// [JsonPropertyName("triggerPeriod")] public TimeSpan TriggerPeriod { get; } } /// -/// Describes a single revolute joint limit set required by the planners. +/// 描述规划器所需的单个旋转关节约束集合。 /// public sealed class JointLimit { /// - /// Initializes a validated joint limit record. + /// 初始化一个已校验的关节约束记录。 /// public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit) { @@ -145,37 +145,37 @@ public sealed class JointLimit } /// - /// Gets the joint name associated with the limits. + /// 获取该约束对应的关节名称。 /// [JsonPropertyName("jointName")] public string JointName { get; } /// - /// Gets the velocity limit in joint space units. + /// 获取关节空间单位下的速度上限。 /// [JsonPropertyName("velocityLimit")] public double VelocityLimit { get; } /// - /// Gets the acceleration limit in joint space units. + /// 获取关节空间单位下的加速度上限。 /// [JsonPropertyName("accelerationLimit")] public double AccelerationLimit { get; } /// - /// Gets the jerk limit in joint space units. + /// 获取关节空间单位下的跃度上限。 /// [JsonPropertyName("jerkLimit")] public double JerkLimit { get; } } /// -/// Describes a joint-coupling rule that must be applied before kinematics or planning. +/// 描述在运动学计算或轨迹规划前必须应用的关节耦合规则。 /// public sealed class JointCoupling { /// - /// Initializes a validated joint-coupling description. + /// 初始化一个已校验的关节耦合描述。 /// public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset) { @@ -201,25 +201,25 @@ public sealed class JointCoupling } /// - /// Gets the dependent joint name. + /// 获取从属（被驱动）关节名称。 /// [JsonPropertyName("slaveJointName")] public string SlaveJointName { get; } /// - /// Gets the source joint name. + /// 获取主导（驱动）关节名称。 /// [JsonPropertyName("masterJointName")] public string MasterJointName { get; } /// - /// Gets the coupling multiplier applied to the master joint angle. + /// 获取作用在主导关节角度上的耦合倍率。 /// [JsonPropertyName("multiplier")] public double Multiplier { get; } /// - /// Gets the additive offset applied after the multiplier. + /// 获取在耦合倍率之后叠加的偏移量。 /// [JsonPropertyName("offset")] public double Offset { get; } diff --git a/src/Flyshot.Core.Planning/Sampling/J519SendSample.cs b/src/Flyshot.Core.Planning/Sampling/J519SendSample.cs new file mode 100644 index 0000000..e707e2f --- /dev/null +++ b/src/Flyshot.Core.Planning/Sampling/J519SendSample.cs @@ -0,0 +1,42 @@ +namespace Flyshot.Core.Planning.Sampling; + +/// +/// 表示 J519 伺服链路在某一个物理发送周期上的轨迹采样结果。 +/// +/// 从 0 开始的发送周期序号。 +/// J519 物理发送时间，单位为秒。 +/// 映射回规划轨迹的采样时间，单位为秒。 +/// 生成该采样点时使用的速度倍率。 +/// J519 下发使用的角度制关节目标。 +public sealed class J519SendSample( + long sampleIndex, + double sendTime, + double trajectoryTime, + double speedRatio, + IReadOnlyList jointsDegrees) +{ + /// + /// 获取从 0 开始的发送周期序号。 + /// + public long SampleIndex { get; } = sampleIndex; + + /// + /// 获取 J519 物理发送时间，单位为秒。 + /// + public double SendTime { get; } = sendTime; + + /// + /// 获取映射回规划轨迹的采样时间，单位为秒。 + /// + public double TrajectoryTime { get; } = trajectoryTime; + + /// + /// 获取生成该采样点时使用的速度倍率。 + /// + public double SpeedRatio { get; } = speedRatio; + + /// + /// 获取 J519 下发使用的角度制关节目标。 + /// + public IReadOnlyList JointsDegrees { get; } = jointsDegrees.ToArray(); +} diff --git a/src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs b/src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs new file mode 100644 index 0000000..937eac6 --- /dev/null +++ b/src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs @@ -0,0 +1,242 @@ +namespace Flyshot.Core.Planning.Sampling; + +/// +/// 负责把规划层稠密关节轨迹重采样为 J519 物理发送周期上的角度制目标。 +/// +/// 算法约定： +/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN]，time 为规划轨迹时间，关节单位为弧度； +/// 输出的 J519 采样点按物理伺服周期排列，关节单位转换为角度制，供 UDP 60015 实时下发和离线 ActualSend 文件共用。 +/// +/// +/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算： +/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。 +/// 末尾额外保留一个终点钳制周期，确保轨迹时长不是周期整数倍时仍会输出最终点。 +/// +/// +/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod； +/// speedRatio 不改变物理发送周期，只用于把发送时间映射回规划轨迹时间： +/// trajectoryTime = min(sendTime * speedRatio, duration)。 +/// 之后在原始稠密关节轨迹上按 trajectoryTime 做线性插值，并把每个关节从 rad 转为 deg。 +/// +/// +/// 诊断行也在这里统一生成：Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio； +/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分，依次近似速度、加速度和跃度，格式为 +/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。 +/// +/// +public static class J519SendTrajectorySampler +{ + /// + /// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。 + /// + /// 规划层稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。 + /// 规划轨迹总时长，单位为秒。 + /// J519 物理发送周期，单位为秒。 + /// 速度倍率；只缩放轨迹采样时间，不改变物理发送周期。 + /// 按 J519 发送周期排列的角度制采样序列。 + public static IReadOnlyList SampleDenseJointTrajectory( + IReadOnlyList> 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((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; + } + + /// + /// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。 + /// + /// 规划轨迹总时长，单位为秒。 + /// 每个物理发送周期对应的轨迹时间步长，单位为秒。 + /// 包含终点钳制周期的采样点数量。 + 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; + } + + /// + /// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + speed_ratio。 + /// + /// 待写出的 J519 实发采样点。 + /// 与 ActualSendTiming.txt 兼容的数值行。 + public static IReadOnlyList BuildTimingRow(J519SendSample sample) + { + ArgumentNullException.ThrowIfNull(sample); + return + [ + sample.SampleIndex, + Math.Round(sample.SendTime, 6), + Math.Round(sample.TrajectoryTime, 6), + Math.Round(sample.SpeedRatio, 6) + ]; + } + + /// + /// 构造相邻发送点之间的角度制跃度统计行。 + /// + /// 上一帧发送时间，单位为秒。 + /// 当前帧发送时间，单位为秒。 + /// 上一帧角度制关节目标。 + /// 当前帧角度制关节目标。 + /// 上一帧关节速度，调用后更新为当前帧速度。 + /// 上一帧关节加速度，调用后更新为当前帧加速度。 + /// 与 ActualSendJerkStats.txt 兼容的数值行。 + public static IReadOnlyList BuildJerkRow( + double previousTime, + double currentTime, + IReadOnlyList previousJoints, + IReadOnlyList 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; + } + + /// + /// 在稠密关节轨迹上按时间线性插值，并转换成 J519 下发使用的角度制目标。 + /// + private static double[] SampleDenseJointTrajectoryDegrees( + IReadOnlyList> 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; + } + + /// + /// 校验 J519 实发采样的基础输入，避免错误时间轴进入运行时链路。 + /// + private static void ValidateInputs( + IReadOnlyList> 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 必须是有限正数。"); + } + } + + /// + /// 角度单位转换：rad -> deg。 + /// + private static double RadiansToDegrees(double radians) + { + return radians * 180.0 / Math.PI; + } +} diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index 13d05ba..fdf1e73 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -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 /// private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList 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(); - var sentJointRows = new List>(); - var sentTimingRows = new List>(); + var commands = new List(sampleCount); + var sentJointRows = new List>(sampleCount); + var sentTimingRows = new List>(sampleCount); var sentJerkRows = new List>(); 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 } } - /// - /// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发包数。 - /// - 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; - } - - /// - /// 在稠密关节轨迹上按时间线性插值，并转换成 J519 要求的角度制关节目标。 - /// - private static double[] SampleDenseJointTrajectoryDegrees( - IReadOnlyList> 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; - } - /// /// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。 /// @@ -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; - } - /// /// 为单次稠密发送创建按日期时间归档的输出目录。 /// @@ -886,98 +819,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable return row; } - /// - /// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + speed_ratio。 - /// - private static IReadOnlyList BuildDenseSendTimingRow( - long sampleIndex, - double sendTime, - double trajectoryTime, - double speedRatio) - { - return - [ - sampleIndex, - Math.Round(sendTime, 6), - Math.Round(trajectoryTime, 6), - Math.Round(speedRatio, 6) - ]; - } - - /// - /// 构造相邻发送点之间的跃度统计行，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。 - /// 通过 ref 参数维护前两帧的速度和加速度状态，从而用二阶后向差分近似跃度。 - /// - /// 上一帧时间戳（秒）。 - /// 当前帧时间戳（秒）。 - /// 上一帧 J519 角度制关节目标。 - /// 当前帧 J519 角度制关节目标。 - /// 上一帧关节速度，ref 更新为当前帧速度。 - /// 上一帧关节加速度，ref 更新为当前帧加速度。 - /// 长度为 jointCount + 4 的行向量。 - private static IReadOnlyList BuildDenseSendJerkRow( - double previousTime, - double currentTime, - IReadOnlyList previousJoints, - IReadOnlyList 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; - } - /// /// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。 /// diff --git a/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj b/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj index fb0f8ff..487c72c 100644 --- a/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj +++ b/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj @@ -18,6 +18,7 @@ + diff --git a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs index 55f5812..483ba07 100644 --- a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs +++ b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs @@ -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 }); } diff --git a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs index ed2a3ba..36bd11f 100644 --- a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs +++ b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs @@ -1,10 +1,9 @@ using Flyshot.Core.Config; -using Flyshot.Core.Domain; namespace Flyshot.Core.Tests; /// -/// 锁定 Task 3 的兼容输入行为，确保旧配置、.robot 元数据和路径策略都能被稳定加载。 +/// 锁定 Task 3 的兼容输入行为，确保旧配置、JSON 模型元数据和路径策略都能被稳定加载。 /// 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 } /// - /// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。 + /// 验证 RobotConfig.json 可以关闭飞拍执行前的二次平滑起停时间重映射。 + /// + [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); + } + } + + /// + /// 验证 JSON 模型解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。 /// [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 } /// - /// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。 + /// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到 JSON 模型关节限制上。 /// [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); } + /// + /// 验证 JSON 模型可一次解析后同时生成规划约束视图和运动学几何视图。 + /// + [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"); + } + /// /// 验证路径兼容层只从当前服务配置目录解析相对配置，并按平台策略生成默认用户数据目录。 /// diff --git a/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs b/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs index f4c7938..dd1df01 100644 --- a/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs +++ b/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs @@ -34,7 +34,7 @@ public sealed class ControllerClientCompatConfigRootTests } /// - /// 验证机器人目录优先从显式 ConfigRoot/Models 加载 .robot 文件。 + /// 验证机器人目录优先从显式 ConfigRoot/Models 加载现场 JSON 模型文件。 /// [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 } /// - /// 复制仓库内已固化的现场机器人模型到临时 Config/Models 目录。 + /// 复制仓库内已固化的现场机器人 JSON 模型到临时 Config/Models 目录。 /// 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")); } /// diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs index be70e27..c71b565 100644 --- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -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 { - $"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 /// 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); } + /// + /// 复制一份结果并替换程序名，让 DenseSend 调试文件写入唯一目录，避免测试之间复用旧目录权限。 + /// + 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); + } + /// /// 解析空格分隔的纯文本数值列。 /// diff --git a/tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs b/tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs new file mode 100644 index 0000000..a89fde2 --- /dev/null +++ b/tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs @@ -0,0 +1,99 @@ +using Flyshot.Core.Planning.Sampling; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 J519 实发重采样器在离线导出和运行时下发之间保持一致的时间轴语义。 +/// +public sealed class J519SendTrajectorySamplerTests +{ + /// + /// 验证 speed_ratio 只缩放轨迹时间，物理发送时间仍按固定伺服周期推进。 + /// + [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); + } + + /// + /// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。 + /// + [Fact] + public void SampleDenseJointTrajectory_RejectsEmptyDenseTrajectory() + { + var exception = Assert.Throws(() => + J519SendTrajectorySampler.SampleDenseJointTrajectory( + Array.Empty>(), + durationSeconds: 0.016, + servoPeriodSeconds: 0.008, + speedRatio: 1.0)); + + Assert.Contains("稠密关节轨迹为空", exception.Message); + } + + /// + /// 验证非法 speed_ratio 会在公共入口统一拦截。 + /// + [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(() => + J519SendTrajectorySampler.SampleDenseJointTrajectory( + denseTrajectory, + durationSeconds: 0.0, + servoPeriodSeconds: 0.008, + speedRatio: speedRatio)); + } + + /// + /// 验证公共诊断行格式与既有 ActualSendTiming 文件保持一致。 + /// + [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); + } +} diff --git a/tests/Flyshot.Core.Tests/OfflinePlanTests.cs b/tests/Flyshot.Core.Tests/OfflinePlanTests.cs index dc92379..74aeb7b 100644 --- a/tests/Flyshot.Core.Tests/OfflinePlanTests.cs +++ b/tests/Flyshot.Core.Tests/OfflinePlanTests.cs @@ -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/。 /// [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); diff --git a/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs b/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs index 468740f..ac4a9c0 100644 --- a/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs +++ b/tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs @@ -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); diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index e63a0cb..41f4d1f 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -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()); } + /// + /// 验证现场 UTTC_MS11 配置使用通用规划速度倍率拉长时间轴，并关闭二次平滑时间重映射。 + /// + [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}"); + } + /// /// 使用运行时 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 } /// - /// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。 + /// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并应用当前 speed_ratio。 + /// + [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); + } + } + + /// + /// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。 /// 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 /// 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); } + /// + /// 为首尾平滑对比测试显式打开二次时间重映射，避免受现场 legacy-fit 配置影响。 + /// + 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); + } + /// /// 把指定窗口内的整形前后逐点数据导出为 CSV，包含关节、步长、速度、加速度与跃度。 /// @@ -1412,6 +1508,8 @@ internal static class TestRobotFactory /// internal sealed class RecordingControllerRuntime : IControllerRuntime { + private double _speedRatio = 1.0; + /// /// 获取最近一次 ResetRobot 收到的机器人配置。 /// @@ -1454,11 +1552,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime } /// - public double GetSpeedRatio() => 1.0; + public double GetSpeedRatio() => _speedRatio; /// public void SetSpeedRatio(double ratio) { + _speedRatio = ratio; } /// @@ -1491,7 +1590,7 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime connectionState: "Connected", isEnabled: true, isInMotion: false, - speedRatio: 1.0, + speedRatio: _speedRatio, jointPositions: Array.Empty(), cartesianPose: Array.Empty(), activeAlarms: Array.Empty());