✨ feat(*): 添加 J519 实发重采样与 JSON 机型模型
* 新增 J519 实发采样器，按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换 * 兼容层产物导出补充 speedRatio，规划编排补齐 smoothStartStopTiming 与日志透传 * 配置与机型加载切换到运行目录 JSON 模型，并补齐 7L 展开模型与相关单元测试
This commit is contained in:
BIN
Config/Models/Expand/LR_Mate_200iD_7L_expand.robot
Normal file
BIN
Config/Models/Expand/LR_Mate_200iD_7L_expand.robot
Normal file
Binary file not shown.
BIN
Config/Models/Expand/LR_Mate_200iD_expand.robot
Normal file
BIN
Config/Models/Expand/LR_Mate_200iD_expand.robot
Normal file
Binary file not shown.
366
Config/Models/LR_Mate_200iD_7L.json
Normal file
366
Config/Models/LR_Mate_200iD_7L.json
Normal 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
70
docs/legacy-fit-uttc-ms11-plan.md
Normal 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 继续逼近旧曲线形状。
|
||||
- 将空间曲线拟合与时间轴拟合分开验收。
|
||||
@@ -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";
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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
30
src/Flyshot.Core.Config/LoadedRobotModel.cs
Normal 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; }
|
||||
}
|
||||
@@ -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,
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@ using System.Text.Json.Serialization;
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// 描述机器人运动学链所需的完整关节几何信息，从 .robot GLB 中提取。
|
||||
/// 描述机器人运动学链所需的完整关节几何信息，从现场固化的机器人 JSON 中提取。
|
||||
///
|
||||
/// 为什么与 RobotProfile 分开？
|
||||
/// ---
|
||||
|
||||
@@ -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
42
src/Flyshot.Core.Planning/Sampling/J519SendSample.cs
Normal 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
242
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
|
||||
@@ -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 });
|
||||
}
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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
99
tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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>());
|
||||
|
||||
Reference in New Issue
Block a user