✨ feat(config): 更新 RobotConfig.json 以支持运行时速度倍率配置
* 在 RobotConfig.json 中新增 speed_ratio 配置,允许在运行时设置默认速度倍率。 * 调整 ControllerClientCompatService 以使用 speed_ratio 初始化机器人设置。 * 更新 TrajectoryLimitValidator 和 FlyshotExecutionSendSequenceBuilder,支持在飞拍链路中临时关闭 jerk 校验,仅保留速度和加速度约束。 * 新增文档记录对 UTTC_MS11 的 jerk 阻断策略调整,确保飞拍链路的执行效率。 * 增加单元测试以验证 speed_ratio 的加载和 jerk 校验的行为。
This commit is contained in:
@@ -1,17 +1,18 @@
|
|||||||
{
|
{
|
||||||
"robot": {
|
"robot": {
|
||||||
"use_do": true,
|
"use_do": true,
|
||||||
"io_addr": [
|
"io_addr": [
|
||||||
7,
|
7,
|
||||||
8
|
8
|
||||||
],
|
],
|
||||||
"io_keep_cycles": 2,
|
"io_keep_cycles": 2,
|
||||||
"trigger_sample_index_offset_cycles": 6,
|
"trigger_sample_index_offset_cycles": 6,
|
||||||
"acc_limit": 1,
|
"acc_limit": 1,
|
||||||
"jerk_limit": 1,
|
"jerk_limit": 1,
|
||||||
"adapt_icsp_try_num": 5,
|
"adapt_icsp_try_num": 5,
|
||||||
|
"speed_ratio": 1,
|
||||||
"planning_speed_scale": 0.74227,
|
"planning_speed_scale": 0.74227,
|
||||||
"smooth_start_stop_timing": false
|
"smooth_start_stop_timing": true
|
||||||
},
|
},
|
||||||
"flying_shots": {
|
"flying_shots": {
|
||||||
"UTTC_MS11": {
|
"UTTC_MS11": {
|
||||||
@@ -301,4 +302,4 @@
|
|||||||
]
|
]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -21,6 +21,7 @@
|
|||||||
- 2026-04-28 UTTC 抓包确认:UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`,`JointDetialTraj.txt` 为弧度制 `rad`,`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸;2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长,当前实际生成约 `7.4s` 轨迹。
|
- 2026-04-28 UTTC 抓包确认:UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`,`JointDetialTraj.txt` 为弧度制 `rad`,`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸;2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长,当前实际生成约 `7.4s` 轨迹。
|
||||||
- 2026-04-30 本机 `50001/TCP+JSON` 抓包确认:`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数,不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路,新系统按 replacement-only 内部参数限制规划加速度。
|
- 2026-04-30 本机 `50001/TCP+JSON` 抓包确认:`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数,不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路,新系统按 replacement-only 内部参数限制规划加速度。
|
||||||
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送能力;稠密轨迹下发已按 `speed_ratio` 做执行时间缩放,并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。
|
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送能力;稠密轨迹下发已按 `speed_ratio` 做执行时间缩放,并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。
|
||||||
|
- 2026-05-11 对 `UTTC_MS11` 的独立 `ConfigRoot` 重新生成确认:flyshot 链路若继续用离散 `jerk` 作为阻断条件,会把新结果拉长到约 `16.576s / 33.152s`;只保留速度、加速度校验并跳过 flyshot jerk 阻断后,新 `1.0x / 0.5x` 时长回落到约 `7.552s / 15.104s`,与旧抓包主段 `7.408s / 14.808s` 的差异已很小,当前按可忽略处理。详见 `docs/uttc-ms11-jerk-validation-decision-20260511.md`。
|
||||||
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹:当前关节到目标关节的关节空间直线,五次 smoothstep 起停,按 `status=15` 运动窗口复现 `40/55/77` 点,并由 J519 层完成 `rad -> deg` 下发。
|
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹:当前关节到目标关节的关节空间直线,五次 smoothstep 起停,按 `status=15` 运动窗口复现 `40/55/77` 点,并由 J519 层完成 `rad -> deg` 下发。
|
||||||
- 单程序只对应一台机器人,上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`,不再创建独立轨迹存储文件。
|
- 单程序只对应一台机器人,上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`,不再创建独立轨迹存储文件。
|
||||||
|
|
||||||
|
|||||||
151
docs/uttc-ms11-jerk-validation-decision-20260511.md
Normal file
151
docs/uttc-ms11-jerk-validation-decision-20260511.md
Normal file
@@ -0,0 +1,151 @@
|
|||||||
|
# UTTC_MS11 跳过 flyshot jerk 阻断的结论记录
|
||||||
|
|
||||||
|
## 背景
|
||||||
|
|
||||||
|
2026-05-11 对 `UTTC_MS11` 做了两轮独立 `ConfigRoot` 重新生成与对比:
|
||||||
|
|
||||||
|
1. 保持 flyshot 离散校验中的 `vel/acc/jerk` 全部生效。
|
||||||
|
2. 临时只对 flyshot 链路跳过 `jerk` 阻断,仍保留速度与加速度校验。
|
||||||
|
|
||||||
|
两轮都使用当前程序重新生成:
|
||||||
|
|
||||||
|
- `POST /set_speedRatio/`
|
||||||
|
- `POST /save_traj_info/`
|
||||||
|
|
||||||
|
并与以下旧抓包侧结果做对比:
|
||||||
|
|
||||||
|
- `../Rvbust/uttc-20260428/2026042802-1_joint_segments/segment_02/JointDetialTraj.txt`
|
||||||
|
- `../Rvbust/uttc-20260428/2026042802-0.5_joint_segments/segment_02/JointDetialTraj.txt`
|
||||||
|
- `../Rvbust/uttc-20260428/1倍速度 角度坐标点/真实轨迹JointDetialTraj.txt`
|
||||||
|
|
||||||
|
## 直接结论
|
||||||
|
|
||||||
|
对当前 `UTTC_MS11` 现场样本,flyshot 链路若继续用离散 `jerk` 作为阻断条件,会触发大量时间轴 stretch,使新结果远慢于旧抓包。
|
||||||
|
|
||||||
|
当 flyshot 链路只保留速度、加速度校验并跳过 `jerk` 阻断后:
|
||||||
|
|
||||||
|
- 新 `1.0x` 实发时长从 `16.576s` 回落到 `7.552s`
|
||||||
|
- 新 `0.5x` 实发时长从 `33.152s` 回落到 `15.104s`
|
||||||
|
- 与旧抓包主运动段相比,只剩很小的时长差异,可接受并忽略
|
||||||
|
|
||||||
|
因此,当前项目对 `UTTC_MS11` 的实际结论定为:
|
||||||
|
|
||||||
|
- flyshot 链路临时不再把离散 `jerk` 作为阻断条件
|
||||||
|
- 仍保留速度、加速度校验
|
||||||
|
- `jerk` 诊断文件继续保留,用于分析,不再作为该路径是否可运行的唯一否决条件
|
||||||
|
|
||||||
|
## 证据
|
||||||
|
|
||||||
|
### 1. 开启 jerk 阻断时的现象
|
||||||
|
|
||||||
|
重新生成结果目录:
|
||||||
|
|
||||||
|
- `../temp/regen-20260511-110950/`
|
||||||
|
|
||||||
|
关键结果:
|
||||||
|
|
||||||
|
- `1.0x ActualSendTiming.txt`:`2073` 行,`send_duration=16.576s`
|
||||||
|
- `0.5x ActualSendTiming.txt`:`4145` 行,`send_duration=33.152s`
|
||||||
|
|
||||||
|
对比旧抓包主段:
|
||||||
|
|
||||||
|
- 旧 `1.0x`:`7.408109s`
|
||||||
|
- 旧 `0.5x`:`14.808342s`
|
||||||
|
|
||||||
|
可见新结果明显被拉长。
|
||||||
|
|
||||||
|
### 2. 触发 stretch 的第一条失败证据
|
||||||
|
|
||||||
|
日志文件:
|
||||||
|
|
||||||
|
- `../temp/regen-20260511-110950/host.log`
|
||||||
|
|
||||||
|
第一条离散失败不是加速度,而是开头窗口的 `Joint1 Jerk`:
|
||||||
|
|
||||||
|
- `轨迹 UTTC_MS11 第 3 行 Joint1 Jerk超限`
|
||||||
|
- `time=0.008000->0.016000s`
|
||||||
|
- `actual=-1732.421875`
|
||||||
|
- `limit=91.698044`
|
||||||
|
- `ratio=18.8927`
|
||||||
|
|
||||||
|
后续 `CreateLimitCompliantDenseTrajectory(...)` 按 `1.01` 倍持续拉长时间轴,直到通过离散校验,最终把总时长推到约 `16.57s`。
|
||||||
|
|
||||||
|
### 3. 只跳过 flyshot jerk 阻断后的结果
|
||||||
|
|
||||||
|
重新生成结果目录:
|
||||||
|
|
||||||
|
- `../temp/regen-nojerk-20260511-113258/`
|
||||||
|
|
||||||
|
关键结果:
|
||||||
|
|
||||||
|
- `1.0x ActualSendTiming.txt`:`945` 行,`send_duration=7.552s`
|
||||||
|
- `0.5x ActualSendTiming.txt`:`1889` 行,`send_duration=15.104s`
|
||||||
|
|
||||||
|
与旧抓包主段对比:
|
||||||
|
|
||||||
|
| 项目 | 旧抓包时长 | 新结果时长 | 差值 |
|
||||||
|
| --- | ---: | ---: | ---: |
|
||||||
|
| `1.0x` | `7.408109s` | `7.552s` | `0.143891s` |
|
||||||
|
| `0.5x` | `14.808342s` | `15.104s` | `0.295658s` |
|
||||||
|
|
||||||
|
该差异已经很小,当前按“可忽略”处理。
|
||||||
|
|
||||||
|
## 轨迹误差变化
|
||||||
|
|
||||||
|
在同一套对比脚本口径下:
|
||||||
|
|
||||||
|
### 开启 jerk 阻断时
|
||||||
|
|
||||||
|
- `JointDetialTraj` 新 vs 旧真实轨迹:全局 RMS 约 `46.664 deg`
|
||||||
|
- `ActualSendJointTraj 1.0x` 新 vs 旧抓包:全局 RMS 约 `46.665 deg`
|
||||||
|
- `ActualSendJointTraj 0.5x` 新 vs 旧抓包:全局 RMS 约 `46.667 deg`
|
||||||
|
|
||||||
|
### 跳过 flyshot jerk 阻断后
|
||||||
|
|
||||||
|
- `JointDetialTraj` 新 vs 旧真实轨迹:全局 RMS 约 `8.411 deg`
|
||||||
|
- `ActualSendJointTraj 1.0x` 新 vs 旧抓包:全局 RMS 约 `8.412 deg`
|
||||||
|
- `ActualSendJointTraj 0.5x` 新 vs 旧抓包:全局 RMS 约 `8.412 deg`
|
||||||
|
|
||||||
|
说明这次差异收敛主要来自去掉 flyshot jerk 阻断后的 stretch,而不是 `speed_ratio` 链路本身被重新设计。
|
||||||
|
|
||||||
|
## 对 jerk 的解释
|
||||||
|
|
||||||
|
这次调整不等于“现场 jerk 完全合规”,而是明确区分:
|
||||||
|
|
||||||
|
1. `jerk` 仍然继续计算和导出
|
||||||
|
2. `jerk` 仍然可作为诊断指标
|
||||||
|
3. 但对当前 `UTTC_MS11` flyshot 现场样本,不再把 `jerk` 作为阻断执行的唯一判据
|
||||||
|
|
||||||
|
重新生成后的分析文件仍显示:
|
||||||
|
|
||||||
|
- 加速度 ratio 明显在限值内
|
||||||
|
- `jerk` ratio 依然可能大于 `1`
|
||||||
|
|
||||||
|
这与旧抓包整理结果的观察一致:当前现场样本里,“加速度合规但跃度超限”是可接受现象,不能单凭 jerk 超限就否决 flyshot 轨迹。
|
||||||
|
|
||||||
|
## 当前代码决策范围
|
||||||
|
|
||||||
|
本次代码调整范围刻意收窄为:
|
||||||
|
|
||||||
|
- 只对 flyshot 链路关闭 `jerk` 阻断
|
||||||
|
- 保留 flyshot 的速度、加速度校验
|
||||||
|
- 不修改 `move_joint` 的 jerk 约束逻辑
|
||||||
|
|
||||||
|
对应入口包括:
|
||||||
|
|
||||||
|
- `ControllerClientTrajectoryOrchestrator.CreateLimitCompliantDenseTrajectory(...)`
|
||||||
|
- `FlyshotExecutionSendSequenceBuilder.Build(...)`
|
||||||
|
- `FlyshotTrajectoryArtifactWriter.WriteActualSendArtifacts(...)`
|
||||||
|
|
||||||
|
`move_joint` 仍保持原先的 jerk 受限实现,不共享这次豁免策略。
|
||||||
|
|
||||||
|
## 当前验收判断
|
||||||
|
|
||||||
|
对 `UTTC_MS11`:
|
||||||
|
|
||||||
|
- 只保留 flyshot 速度/加速度校验
|
||||||
|
- 跳过 flyshot jerk 阻断
|
||||||
|
- 新 `1.0x / 0.5x` 与旧抓包主段的剩余时长差异已经很小
|
||||||
|
- 当前判断:这点差异可以忽略
|
||||||
|
|
||||||
|
后续除非现场再次提供新的反证,否则本结论作为当前项目实现与验收的记录保留。
|
||||||
@@ -134,6 +134,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
_activeRobotProfile = robotProfile;
|
_activeRobotProfile = robotProfile;
|
||||||
_uploadedTrajectories.Clear();
|
_uploadedTrajectories.Clear();
|
||||||
_runtime.ResetRobot(robotProfile, robotName);
|
_runtime.ResetRobot(robotProfile, robotName);
|
||||||
|
// RobotConfig.json 的 speed_ratio 是启动默认值;在线 set_speedRatio 仍可覆盖后续执行。
|
||||||
|
_runtime.SetSpeedRatio(robotSettings.SpeedRatio);
|
||||||
_robotSettings = robotSettings;
|
_robotSettings = robotSettings;
|
||||||
|
|
||||||
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
|
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
|
||||||
@@ -145,11 +147,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, 恢复轨迹数={TrajCount}",
|
"SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, speedRatio={SpeedRatio}, 恢复轨迹数={TrajCount}",
|
||||||
robotName,
|
robotName,
|
||||||
robotProfile.DegreesOfFreedom,
|
robotProfile.DegreesOfFreedom,
|
||||||
robotSettings.AccLimitScale,
|
robotSettings.AccLimitScale,
|
||||||
robotSettings.JerkLimitScale,
|
robotSettings.JerkLimitScale,
|
||||||
|
robotSettings.SpeedRatio,
|
||||||
_uploadedTrajectories.Count);
|
_uploadedTrajectories.Count);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -452,7 +452,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
||||||
executionTrajectory.Robot,
|
executionTrajectory.Robot,
|
||||||
denseJointTrajectory,
|
denseJointTrajectory,
|
||||||
trajectoryName: executionTrajectory.OriginalProgram.Name);
|
trajectoryName: executionTrajectory.OriginalProgram.Name,
|
||||||
|
validateJerk: false);
|
||||||
return denseJointTrajectory;
|
return denseJointTrajectory;
|
||||||
}
|
}
|
||||||
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
|
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
|
||||||
|
|||||||
@@ -142,7 +142,8 @@ public sealed class FlyshotTrajectoryArtifactWriter
|
|||||||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||||
robot,
|
robot,
|
||||||
samples,
|
samples,
|
||||||
trajectoryName: result.ProgramName);
|
trajectoryName: result.ProgramName,
|
||||||
|
validateJerk: false);
|
||||||
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
|
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
|
||||||
List<IReadOnlyList<double>> timingRows = preparedExecution is null
|
List<IReadOnlyList<double>> timingRows = preparedExecution is null
|
||||||
? new List<IReadOnlyList<double>>(samples.Count)
|
? new List<IReadOnlyList<double>>(samples.Count)
|
||||||
|
|||||||
@@ -198,6 +198,7 @@ public sealed class JsonFlyshotTrajectoryStore
|
|||||||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
["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),
|
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
|
||||||
|
["speed_ratio"] = JsonValue.Create(settings.SpeedRatio),
|
||||||
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
|
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -21,6 +21,7 @@ public sealed class CompatibilityRobotSettings
|
|||||||
double jerkLimitScale,
|
double jerkLimitScale,
|
||||||
int adaptIcspTryNum,
|
int adaptIcspTryNum,
|
||||||
double planningSpeedScale = 1.0,
|
double planningSpeedScale = 1.0,
|
||||||
|
double speedRatio = 1.0,
|
||||||
bool smoothStartStopTiming = true)
|
bool smoothStartStopTiming = true)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(ioAddresses);
|
ArgumentNullException.ThrowIfNull(ioAddresses);
|
||||||
@@ -50,6 +51,11 @@ public sealed class CompatibilityRobotSettings
|
|||||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (speedRatio <= 0.0 || speedRatio > 1.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(speedRatio), "运行时速度倍率必须在 (0, 1] 范围内。");
|
||||||
|
}
|
||||||
|
|
||||||
if (adaptIcspTryNum < 0)
|
if (adaptIcspTryNum < 0)
|
||||||
{
|
{
|
||||||
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
|
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
|
||||||
@@ -70,6 +76,7 @@ public sealed class CompatibilityRobotSettings
|
|||||||
JerkLimitScale = jerkLimitScale;
|
JerkLimitScale = jerkLimitScale;
|
||||||
AdaptIcspTryNum = adaptIcspTryNum;
|
AdaptIcspTryNum = adaptIcspTryNum;
|
||||||
PlanningSpeedScale = planningSpeedScale;
|
PlanningSpeedScale = planningSpeedScale;
|
||||||
|
SpeedRatio = speedRatio;
|
||||||
SmoothStartStopTiming = smoothStartStopTiming;
|
SmoothStartStopTiming = smoothStartStopTiming;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -109,6 +116,11 @@ public sealed class CompatibilityRobotSettings
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
public double PlanningSpeedScale { get; }
|
public double PlanningSpeedScale { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取运行时默认速度倍率,会在 SetUpRobot 时同步到控制器运行时,供 MoveJoint 和后续执行链路使用。
|
||||||
|
/// </summary>
|
||||||
|
public double SpeedRatio { get; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
|
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -202,6 +214,7 @@ public sealed class RobotConfigLoader
|
|||||||
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
|
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
|
||||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 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),
|
||||||
|
speedRatio: ReadDouble(robotElement, "speed_ratio", defaultValue: 1.0),
|
||||||
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
|
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
|
||||||
|
|
||||||
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
|
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
|
||||||
@@ -213,8 +226,8 @@ public sealed class RobotConfigLoader
|
|||||||
}
|
}
|
||||||
|
|
||||||
_logger?.LogInformation(
|
_logger?.LogInformation(
|
||||||
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
|
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
|
||||||
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
|
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SpeedRatio, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
|
||||||
|
|
||||||
return new LoadedRobotConfig(
|
return new LoadedRobotConfig(
|
||||||
sourcePath: resolvedConfigPath,
|
sourcePath: resolvedConfigPath,
|
||||||
|
|||||||
@@ -65,7 +65,8 @@ public static class FlyshotExecutionSendSequenceBuilder
|
|||||||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||||
robot,
|
robot,
|
||||||
samples,
|
samples,
|
||||||
trajectoryName: result.ProgramName);
|
trajectoryName: result.ProgramName,
|
||||||
|
validateJerk: false);
|
||||||
|
|
||||||
return BuildPreparedExecution(
|
return BuildPreparedExecution(
|
||||||
result,
|
result,
|
||||||
|
|||||||
@@ -19,16 +19,18 @@ public static class TrajectoryLimitValidator
|
|||||||
/// <param name="rows">稠密轨迹行,格式为 time + 关节弧度。</param>
|
/// <param name="rows">稠密轨迹行,格式为 time + 关节弧度。</param>
|
||||||
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
|
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
|
||||||
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
||||||
|
/// <param name="validateJerk">是否校验离散 jerk;飞拍链路可临时关闭,仅保留速度/加速度约束。</param>
|
||||||
public static void ValidateDenseJointTrajectory(
|
public static void ValidateDenseJointTrajectory(
|
||||||
RobotProfile robot,
|
RobotProfile robot,
|
||||||
IReadOnlyList<IReadOnlyList<double>> rows,
|
IReadOnlyList<IReadOnlyList<double>> rows,
|
||||||
double toleranceMultiplier = DefaultLimitTolerance,
|
double toleranceMultiplier = DefaultLimitTolerance,
|
||||||
string? trajectoryName = null)
|
string? trajectoryName = null,
|
||||||
|
bool validateJerk = true)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(robot);
|
ArgumentNullException.ThrowIfNull(robot);
|
||||||
ArgumentNullException.ThrowIfNull(rows);
|
ArgumentNullException.ThrowIfNull(rows);
|
||||||
ValidateTolerance(toleranceMultiplier);
|
ValidateTolerance(toleranceMultiplier);
|
||||||
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory");
|
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory", validateJerk);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -38,11 +40,13 @@ public static class TrajectoryLimitValidator
|
|||||||
/// <param name="samples">J519 发送采样点,关节单位为角度。</param>
|
/// <param name="samples">J519 发送采样点,关节单位为角度。</param>
|
||||||
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
|
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
|
||||||
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
||||||
|
/// <param name="validateJerk">是否校验离散 jerk;飞拍链路可临时关闭,仅保留速度/加速度约束。</param>
|
||||||
public static void ValidateJ519SendSamples(
|
public static void ValidateJ519SendSamples(
|
||||||
RobotProfile robot,
|
RobotProfile robot,
|
||||||
IReadOnlyList<J519SendSample> samples,
|
IReadOnlyList<J519SendSample> samples,
|
||||||
double toleranceMultiplier = DefaultLimitTolerance,
|
double toleranceMultiplier = DefaultLimitTolerance,
|
||||||
string? trajectoryName = null)
|
string? trajectoryName = null,
|
||||||
|
bool validateJerk = true)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(robot);
|
ArgumentNullException.ThrowIfNull(robot);
|
||||||
ArgumentNullException.ThrowIfNull(samples);
|
ArgumentNullException.ThrowIfNull(samples);
|
||||||
@@ -61,7 +65,7 @@ public static class TrajectoryLimitValidator
|
|||||||
rows.Add(row);
|
rows.Add(row);
|
||||||
}
|
}
|
||||||
|
|
||||||
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory");
|
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory", validateJerk);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -82,7 +86,8 @@ public static class TrajectoryLimitValidator
|
|||||||
RobotProfile robot,
|
RobotProfile robot,
|
||||||
IReadOnlyList<IReadOnlyList<double>> rows,
|
IReadOnlyList<IReadOnlyList<double>> rows,
|
||||||
double toleranceMultiplier,
|
double toleranceMultiplier,
|
||||||
string trajectoryName)
|
string trajectoryName,
|
||||||
|
bool validateJerk)
|
||||||
{
|
{
|
||||||
double? previousTime = null;
|
double? previousTime = null;
|
||||||
double[]? previousPositions = null;
|
double[]? previousPositions = null;
|
||||||
@@ -145,7 +150,7 @@ public static class TrajectoryLimitValidator
|
|||||||
jointLimit.AccelerationLimit,
|
jointLimit.AccelerationLimit,
|
||||||
toleranceMultiplier);
|
toleranceMultiplier);
|
||||||
|
|
||||||
if (previousAccelerations is not null)
|
if (validateJerk && previousAccelerations is not null)
|
||||||
{
|
{
|
||||||
var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
|
var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
|
||||||
ThrowIfExceeded(
|
ThrowIfExceeded(
|
||||||
|
|||||||
@@ -75,6 +75,7 @@ public sealed class ConfigCompatibilityTests
|
|||||||
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
|
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
|
||||||
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
|
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
|
||||||
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
||||||
|
Assert.Equal(1.0, loaded.Robot.SpeedRatio);
|
||||||
Assert.True(loaded.Robot.SmoothStartStopTiming);
|
Assert.True(loaded.Robot.SmoothStartStopTiming);
|
||||||
Assert.Equal([0, 0, 0], program.OffsetValues);
|
Assert.Equal([0, 0, 0], program.OffsetValues);
|
||||||
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
|
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
|
||||||
@@ -85,6 +86,46 @@ public sealed class ConfigCompatibilityTests
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 RobotConfig.json 可以显式配置运行时默认速度倍率,供 MoveJoint 和后续执行链路初始化使用。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void RobotConfigLoader_LoadsRuntimeSpeedRatio()
|
||||||
|
{
|
||||||
|
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,
|
||||||
|
"speed_ratio": 0.65
|
||||||
|
},
|
||||||
|
"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.Equal(0.65, loaded.Robot.SpeedRatio, precision: 6);
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
Directory.Delete(tempRoot, recursive: true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 验证 RobotConfig.json 可以显式配置规划限速倍率,且该倍率独立于运行时 J519 速度倍率。
|
/// 验证 RobotConfig.json 可以显式配置规划限速倍率,且该倍率独立于运行时 J519 速度倍率。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -74,7 +74,8 @@ public sealed class ControllerClientCompatConfigRootTests
|
|||||||
triggerSampleIndexOffsetCycles: 7,
|
triggerSampleIndexOffsetCycles: 7,
|
||||||
accLimitScale: 1.0,
|
accLimitScale: 1.0,
|
||||||
jerkLimitScale: 1.0,
|
jerkLimitScale: 1.0,
|
||||||
adaptIcspTryNum: 5);
|
adaptIcspTryNum: 5,
|
||||||
|
speedRatio: 0.65);
|
||||||
var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||||
|
|
||||||
store.Save("FANUC_LR_Mate_200iD", settings, trajectory);
|
store.Save("FANUC_LR_Mate_200iD", settings, trajectory);
|
||||||
@@ -85,6 +86,7 @@ public sealed class ControllerClientCompatConfigRootTests
|
|||||||
var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings);
|
var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings);
|
||||||
Assert.NotNull(loadedSettings);
|
Assert.NotNull(loadedSettings);
|
||||||
Assert.Equal(7, loadedSettings.TriggerSampleIndexOffsetCycles);
|
Assert.Equal(7, loadedSettings.TriggerSampleIndexOffsetCycles);
|
||||||
|
Assert.Equal(0.65, loadedSettings.SpeedRatio, precision: 6);
|
||||||
Assert.Contains(trajectory.Name, loaded);
|
Assert.Contains(trajectory.Name, loaded);
|
||||||
|
|
||||||
store.Delete("FANUC_LR_Mate_200iD", trajectory.Name);
|
store.Delete("FANUC_LR_Mate_200iD", trajectory.Name);
|
||||||
|
|||||||
@@ -239,6 +239,36 @@ public sealed class PlanningCompatibilityTests
|
|||||||
Assert.Contains("dense-acceleration-check", exception.Message);
|
Assert.Contains("dense-acceleration-check", exception.Message);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证飞拍链路临时关闭 jerk 复核时,仍保留速度/加速度校验,但不会再被 jerk 单独阻断。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void TrajectoryLimitValidator_CanSkipJerkValidation_ForFlyshotPath()
|
||||||
|
{
|
||||||
|
var robot = CreateRobotProfile([100.0], [1000.0], [10.0]);
|
||||||
|
IReadOnlyList<IReadOnlyList<double>> rows =
|
||||||
|
[
|
||||||
|
[0.0, 0.0],
|
||||||
|
[0.008, 0.0],
|
||||||
|
[0.016, 0.01],
|
||||||
|
[0.024, 0.02]
|
||||||
|
];
|
||||||
|
|
||||||
|
var exception = Assert.Throws<InvalidOperationException>(() =>
|
||||||
|
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
||||||
|
robot,
|
||||||
|
rows,
|
||||||
|
trajectoryName: "dense-jerk-check"));
|
||||||
|
|
||||||
|
Assert.Contains("Jerk", exception.Message);
|
||||||
|
|
||||||
|
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
||||||
|
robot,
|
||||||
|
rows,
|
||||||
|
trajectoryName: "dense-jerk-check-skip",
|
||||||
|
validateJerk: false);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 构造一个最小 RobotProfile,便于规划层单元测试聚焦在时间轴逻辑上。
|
/// 构造一个最小 RobotProfile,便于规划层单元测试聚焦在时间轴逻辑上。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -887,7 +887,8 @@ public sealed class RuntimeOrchestrationTests
|
|||||||
"io_keep_cycles": 4,
|
"io_keep_cycles": 4,
|
||||||
"acc_limit": 0.5,
|
"acc_limit": 0.5,
|
||||||
"jerk_limit": 0.25,
|
"jerk_limit": 0.25,
|
||||||
"adapt_icsp_try_num": 3
|
"adapt_icsp_try_num": 3,
|
||||||
|
"speed_ratio": 0.65
|
||||||
},
|
},
|
||||||
"flying_shots": {}
|
"flying_shots": {}
|
||||||
}
|
}
|
||||||
@@ -907,6 +908,7 @@ public sealed class RuntimeOrchestrationTests
|
|||||||
var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
|
var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
|
||||||
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
|
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
|
||||||
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
||||||
|
Assert.Equal(0.65, runtime.GetSpeedRatio(), precision: 6);
|
||||||
}
|
}
|
||||||
finally
|
finally
|
||||||
{
|
{
|
||||||
|
|||||||
Reference in New Issue
Block a user