4 Commits

Author SHA1 Message Date
0952958af7 ✨ feat(status): 添加点动摇杆请求日志
* 将状态页直角坐标点动改为 XYZ/WPR 双摇杆布局
* 添加 MovePose 请求日志与清空入口,便于现场观察下发内容
* 扩展状态页集成测试,覆盖摇杆控件和日志资源
2026-05-19 11:17:53 +08:00
2cd42f04e5 ✨ feat(fanuc): 添加直角坐标点动功能与相关接口
* 新增 `MovePose` 方法,支持以直角坐标执行点到点移动。
* 引入 `LegacyCartesianPoseRequest` 类,处理直角位姿请求体的解析与验证。
* 更新 `LegacyHttpApiController`,实现 `/move_pose/` 路由以支持新功能。
* 增强状态快照元数据,提供机器人初始化状态与已上传轨迹信息。
* 更新前端状态页面,增加直角坐标点动控制面板与步长设置选项。
* 相关文档与测试用例同步更新,确保新功能的完整性与稳定性。
2026-05-14 17:46:42 +08:00
d120ef2a39 ✨ feat(fanuc): 统一 speedRatio 执行倍率语义
* 将 speedRatio 前移到规划/准备阶段,运行时只消费已生成的 8ms 队列
* 区分旧格式规划导出与 ActualSend 实发诊断工件
* 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
2026-05-11 17:21:18 +08:00
d82128da4a ✨ feat(config): 更新 RobotConfig.json 以支持运行时速度倍率配置
* 在 RobotConfig.json 中新增 speed_ratio 配置,允许在运行时设置默认速度倍率。
* 调整 ControllerClientCompatService 以使用 speed_ratio 初始化机器人设置。
* 更新 TrajectoryLimitValidator 和 FlyshotExecutionSendSequenceBuilder,支持在飞拍链路中临时关闭 jerk 校验,仅保留速度和加速度约束。
* 新增文档记录对 UTTC_MS11 的 jerk 阻断策略调整,确保飞拍链路的执行效率。
* 增加单元测试以验证 speed_ratio 的加载和 jerk 校验的行为。
2026-05-11 14:44:58 +08:00
42 changed files with 4740 additions and 630 deletions

View File

@@ -1,17 +1,18 @@
{
"robot": {
"use_do": true,
"io_addr": [
7,
8
],
"io_keep_cycles": 2,
"trigger_sample_index_offset_cycles": 6,
"acc_limit": 1,
"jerk_limit": 1,
"use_do": true,
"io_addr": [
7,
8
],
"io_keep_cycles": 2,
"trigger_sample_index_offset_cycles": 6,
"acc_limit": 1,
"jerk_limit": 1,
"adapt_icsp_try_num": 5,
"speed_ratio": 1,
"planning_speed_scale": 0.74227,
"smooth_start_stop_timing": false
"smooth_start_stop_timing": true
},
"flying_shots": {
"UTTC_MS11": {
@@ -301,4 +302,4 @@
]
}
}
}
}

View File

@@ -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-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 全流程现场联调仍需执行。
- 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` 下发。
- 单程序只对应一台机器人,上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`,不再创建独立轨迹存储文件。

View File

@@ -0,0 +1,200 @@
#!/usr/bin/env python3
"""读取 FANUC J519 允许速度/加速度/jerk 上限表。
本脚本只做 UDP packet type=3 查询,不启动运行时服务,也不写入机器人参数。
响应中的 limit 值保留为控制器原始单位:速度 deg/s、加速度 deg/s^2、jerk deg/s^3。
"""
from __future__ import annotations
import argparse
import csv
import socket
import struct
import sys
from dataclasses import dataclass
from pathlib import Path
LIMIT_TYPES = {
0: ("velocity", "deg/s"),
1: ("acceleration", "deg/s^2"),
2: ("jerk", "deg/s^3"),
}
@dataclass(frozen=True)
class LimitTable:
"""保存单个轴、单种限制类型的 20 档 no-payload / max-payload 上限表。"""
axis: int
limit_type: int
vmax_mm_s: int
intermediate_check_s: int
no_payload: tuple[float, ...]
max_payload: tuple[float, ...]
def pack_request(axis: int, limit_type: int) -> bytes:
"""按手册 Table 5 封装允许上限表查询请求。"""
return struct.pack(">IIII", 3, 1, axis, limit_type)
def parse_response(packet: bytes, expected_axis: int, expected_type: int) -> LimitTable:
"""按手册 Table 6 解析允许上限表响应。"""
if len(packet) != 184:
raise ValueError(f"响应长度应为 184 字节,实际为 {len(packet)} 字节。")
packet_type, version, axis, limit_type, vmax, check_time = struct.unpack_from(">IIIIII", packet, 0)
if packet_type != 3 or version != 1:
raise ValueError(f"响应头不正确:packet_type={packet_type}, version={version}。")
if axis != expected_axis or limit_type != expected_type:
raise ValueError(
f"响应与请求不匹配:期望 axis={expected_axis}, type={expected_type},"
f"实际 axis={axis}, type={limit_type}。"
)
values = struct.unpack_from(">40f", packet, 24)
return LimitTable(
axis=axis,
limit_type=limit_type,
vmax_mm_s=vmax,
intermediate_check_s=check_time,
no_payload=tuple(values[:20]),
max_payload=tuple(values[20:]),
)
def query_table(sock: socket.socket, robot_ip: str, port: int, axis: int, limit_type: int) -> LimitTable:
"""发送单个查询请求并等待对应响应。"""
sock.sendto(pack_request(axis, limit_type), (robot_ip, port))
packet, address = sock.recvfrom(2048)
if address[0] != robot_ip:
raise ValueError(f"收到非目标机器人响应:{address[0]}:{address[1]}。")
return parse_response(packet, axis, limit_type)
def iter_requested_tables(
robot_ip: str,
port: int,
timeout_s: float,
axes: range,
limit_types: tuple[int, ...],
) -> list[LimitTable]:
"""按轴和限制类型顺序读取所有上限表。"""
tables: list[LimitTable] = []
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
sock.settimeout(timeout_s)
for axis in axes:
for limit_type in limit_types:
tables.append(query_table(sock, robot_ip, port, axis, limit_type))
return tables
def write_csv(path: Path, tables: list[LimitTable]) -> None:
"""把 20 档速度区间和两套 payload 表写成扁平 CSV。"""
with path.open("w", newline="", encoding="utf-8") as file:
writer = csv.writer(file)
writer.writerow(
[
"axis",
"limit_type",
"unit",
"vmax_mm_s",
"intermediate_check_s",
"speed_bin_index",
"speed_bin_upper_mm_s",
"no_payload_limit",
"max_payload_limit",
]
)
for table in tables:
limit_name, unit = LIMIT_TYPES[table.limit_type]
for index, (no_payload, max_payload) in enumerate(zip(table.no_payload, table.max_payload), start=1):
writer.writerow(
[
table.axis,
limit_name,
unit,
table.vmax_mm_s,
table.intermediate_check_s,
index,
table.vmax_mm_s * index / 20.0,
f"{no_payload:.9g}",
f"{max_payload:.9g}",
]
)
def print_summary(tables: list[LimitTable]) -> None:
"""打印每张表的首末档,便于现场快速确认返回值是否正常。"""
for table in tables:
limit_name, unit = LIMIT_TYPES[table.limit_type]
print(
f"axis={table.axis} type={limit_name:<12} unit={unit:<8} "
f"vmax={table.vmax_mm_s}mm/s check={table.intermediate_check_s}s "
f"no_payload[1]={table.no_payload[0]:.6g} no_payload[20]={table.no_payload[-1]:.6g} "
f"max_payload[1]={table.max_payload[0]:.6g} max_payload[20]={table.max_payload[-1]:.6g}"
)
def parse_args() -> argparse.Namespace:
"""解析命令行参数。"""
parser = argparse.ArgumentParser(
description="Read FANUC J519 packet type=3 allowable upper limit tables.",
)
parser.add_argument("robot_ip", help="机器人控制柜 IP,例如 192.168.10.11。")
parser.add_argument("--port", type=int, default=60015, help="J519 UDP 端口,默认 60015。")
parser.add_argument("--timeout", type=float, default=2.0, help="单次响应超时秒数,默认 2.0。")
parser.add_argument("--axis-start", type=int, default=1, help="起始轴号,默认 1。")
parser.add_argument("--axis-end", type=int, default=9, help="结束轴号,默认 9。")
parser.add_argument(
"--types",
default="0,1,2",
help="限制类型列表:0=velocity, 1=acceleration, 2=jerk;默认 0,1,2。",
)
parser.add_argument(
"--csv",
type=Path,
default=Path("analysis/fanuc_allowable_limit_tables.csv"),
help="CSV 输出路径。",
)
return parser.parse_args()
def main() -> int:
"""脚本入口。"""
args = parse_args()
limit_types = tuple(int(item.strip()) for item in args.types.split(",") if item.strip())
unknown_types = [item for item in limit_types if item not in LIMIT_TYPES]
if unknown_types:
print(f"未知限制类型:{unknown_types}", file=sys.stderr)
return 2
if not 1 <= args.axis_start <= args.axis_end <= 9:
print("轴范围必须满足 1 <= axis-start <= axis-end <= 9。", file=sys.stderr)
return 2
tables = iter_requested_tables(
args.robot_ip,
args.port,
args.timeout,
range(args.axis_start, args.axis_end + 1),
limit_types,
)
args.csv.parent.mkdir(parents=True, exist_ok=True)
write_csv(args.csv, tables)
print_summary(tables)
print(f"\nCSV 已写入:{args.csv}")
return 0
if __name__ == "__main__":
raise SystemExit(main())

BIN
docs/0.7x.pcap Normal file

Binary file not shown.

View File

@@ -47,10 +47,10 @@
本设计目标如下:
- 保持 `planning_speed_scale` 只属于规划层。
- `speedRatio` 只作用于飞拍执行层,且仅对下一次启动的飞拍任务生效。
- 任意有效 `speedRatio` 下,最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
- `speedRatio` 是运行时统一执行倍率,可影响下一次启动的飞拍、普通轨迹和 `MoveJoint`。
- 飞拍链路在任意有效 `speedRatio` 下,最终真实发送的 `8ms` 点列必须满足逐周期 `vel/acc/jerk` 约束。
- 不依赖“插值后自然会平滑”的经验假设。
- 当 `speedRatio = 0.5` 等较慢倍率导致候选点列不满足约束时,系统自动拉长执行时长,直到点列通过校验。
- 当请求 `speedRatio` 下的最终发送队列不满足约束时,拒绝执行并输出超限诊断;不在执行侧偷偷改写本次请求倍率。
- 不破坏现有 `shot timeline / sample offset` 语义。
- `speedRatio = 1.0` 时行为应与当前基线一致或可解释等价。
@@ -58,7 +58,7 @@
本次设计明确不包含以下内容:
- 不修改 `move_joint` 的调速和生成机制。
- 不恢复旧的发送阶段临场回采样机制;普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段折算。
- 不讨论 GUI、多机器人或旧 `50001/TCP+JSON` 网关恢复。
- 不修改 `planning_speed_scale` 的含义。
- 不在任务执行中途支持 `speedRatio` 热切换。
@@ -77,7 +77,7 @@
- 输入规划层稠密轨迹、执行时读取到的 `speedRatio`、机器人关节限位和触发时间轴。
- 生成最终 `8ms` 发送队列。
- 对最终队列做逐周期 `vel/acc/jerk` 校验。
- 如果不通过,则自动拉长执行时长后重建队列,直到通过。
- 如果不通过,则拒绝执行,提示降低 `planning_speed_scale` 重新规划或显式设置更低的 `speedRatio`。
3. 发送段
- 运行时只消费已经准备好的最终 `8ms` 队列。
- 运行时不再根据 `speedRatio` 对 `DenseJointTrajectory` 临场做线性回采样。
@@ -90,7 +90,7 @@
## 6. 统一语义
本次只改飞拍任务,但仍需把飞拍内部语义说清楚,避免再次混淆规划倍率和执行倍率。
本次以飞拍任务为主线,但 `speedRatio` 允许作为运行时统一执行倍率影响普通轨迹和 `MoveJoint`,需要把三类入口的边界说清楚,避免再次混淆规划倍率和执行倍率。
### 6.1 `planning_speed_scale`
@@ -101,17 +101,17 @@
### 6.2 `speedRatio`
- 只影响飞拍执行准备阶段。
- 只对下一次启动的飞拍任务生效。
- 表示“用户期望的执行层时间推进速度”。
- 该值先用于构建候选发送队列。
- 如果候选发送队列在离散 `8ms` 点列上不满足约束,则系统自动进一步拉长执行时长,直到满足约束。
- 影响下一次启动的运动任务,不在任务执行中途热切换。
- 对飞拍任务,表示用户期望的执行层时间推进速度,可前移到规划/执行准备阶段生成最终发送队列。
- 对普通轨迹和 `MoveJoint`,允许在规划或临时 PTP 轨迹生成阶段折算进有效关节限位,使运行时只消费已经生成好的 `8ms` 稠密点列。
- `speedRatio = 0.8` 表示执行节奏接近把同几何轨迹时间轴整体拉长 `1 / 0.8`。
- 例如 `planning_speed_scale = 0.9` 且 `speedRatio = 0.8` 时,最终执行节奏应接近同几何轨迹在 `planning_speed_scale ≈ 0.72` 下的结果。
也就是说:
- `speedRatio` 是执行目标倍率。
- 但最终真实执行时长允许比该目标更保守。
- 保守的原因不是放宽标准,而是严格保证离散动力学约束。
- `speedRatio` 是确定的执行倍率,不是校验失败后的自动搜索变量。
- `finalSpeedRatio` 正常必须等于请求值。
- 若该倍率下最终队列超限,问题暴露给调用方,而不是由执行侧自动改成另一个倍率。
## 7. 最终发送序列生成
@@ -124,54 +124,33 @@
职责:
- 输入规划层 `DenseJointTrajectory`。
- 输入 `durationSeconds`、`servoPeriodSeconds`、`speedRatio`。
- 输入规划层连续 `PlannedTrajectory`,优先使用经过执行侧定时处理后的 `ExecutionTrajectory`。
- 输入 `servoPeriodSeconds`、`speedRatio`。
- 输出最终真实发送的 `8ms` 点列。
- 输出与该点列一致的触发绑定结果。
- 输出校验与自动拉长过程中的诊断信息。
- 输出请求倍率、最终倍率和历史倍率改写次数等诊断信息;当前语义下最终倍率等于请求倍率、改写次数为 0。
### 7.2 第一版候选队列
### 7.2 最终请求倍率队列
候选队列仍可以沿用当前时间轴语义作为起点:
最终队列按固定时间轴语义生成:
- `sendTime = sampleIndex * 0.008`
- `trajectoryTime = min(sendTime * speedRatio, durationSeconds)`
- 关节目标从连续样条直接取点,并从 `rad` 转为 J519 需要的 `deg`
但该候选队列只作为“第一轮尝试”,不能直接视为最终执行结果。
该队列就是本次请求倍率下的最终发送队列,后续只做校验和触发绑定,不再进行二次滤波或倍率搜索。
### 7.3 自动拉长策略
### 7.3 校验失败策略
当候选队列的离散 `vel/acc/jerk` 校验失败时:
当最终队列的离散 `vel/acc/jerk` 校验失败时:
- 不修改规划层轨迹。
- 不修改 `planning_speed_scale`。
- 不放宽校验阈值。
- 只在执行侧拉长最终发送时长,然后重新构建候选队列。
- 不降低本次 `speedRatio`。
- 直接拒绝执行并保留首个超限诊断。
可实现为等价的两种方式之一:
1. 延长物理发送总点数,使发送总时长变长。
2. 在保持 `8ms` 周期不变的前提下,降低执行侧有效轨迹时间推进速度。
本质上二者等价,建议统一落成第二种表达:
- 对外仍说“自动拉长执行时长”。
- 对内通过更保守的 `trajectoryTime(sendTime)` 映射来实现。
### 7.4 自动拉长的迭代规则
建议采用单调保守策略:
1. 先按请求 `speedRatio` 构建第 1 版候选队列。
2. 对候选队列做离散校验。
3. 若失败,则按固定步长或倍率逐轮拉长,再次构建候选队列。
4. 一旦通过,立即停止迭代,产出最终队列。
5. 若超过安全迭代上限仍未通过,则拒绝执行并输出首个超限诊断。
这样可以保证:
- 自动拉长过程是可解释、可记录的。
- 不会因为局部修补而引入新的不可预测尖峰。
这样可以避免把 `speedRatio` 误实现成安全优化器。现场若希望从 `planning_speed_scale = 0.9` 的 10s 轨迹降到接近 `planning_speed_scale = 0.7` 的执行效果,应显式设置类似 `speedRatio = 0.8`,而不是让系统在失败后自动猜一个更低倍率。
## 8. 逐周期约束校验
@@ -201,10 +180,11 @@
### 8.3 校验失败的处理
飞拍链路按本次确认采用 A:
飞拍链路按当前修正语义采用“确定倍率 + 失败拒绝”:
- 若失败,优先自动拉长执行时长重试。
- 只有在超过拉长上限后仍失败时才拒绝执行。
- 若请求倍率下失败,立即拒绝执行。
- 不进行执行侧自动倍率搜索。
- 调用方可以选择降低 `planning_speed_scale` 重新规划,或显式设置更低的 `speedRatio` 后再次执行。
拒绝执行时必须输出:
@@ -212,7 +192,8 @@
- 时间窗
- 指标类型
- `actual / limit / ratio`
- 失败发生在哪一轮自动拉长尝试中
- 请求 `speedRatio`
- 当前 `finalSpeedRatio`,正常等于请求值
## 9. 触发时序与绑定
@@ -238,15 +219,15 @@
- 触发绑定和真实发送完全一致。
- 导出工件中的 `ShotEvents.json` 与真实执行时序一致。
- 自动拉长后触发点仍可被精确追溯到最终发送点索引。
- 触发点可被精确追溯到本次请求倍率下的最终发送点索引。
### 9.3 关于触发数量与顺序
自动拉长执行时长时:
请求倍率改变最终发送时长时:
- 触发数量不能变。
- 触发顺序不能变。
- 只允许触发在最终发送队列中的绑定索引后移或保持可解释等价。
- 触发在最终发送队列中的绑定索引应随时间轴拉长后移或保持可解释等价。
## 10. 运行时职责调整
@@ -284,9 +265,19 @@
## 11. 导出工件与日志
### 11.1 工件统一基于最终发送队列
### 11.1 旧规划导出和实发诊断必须分开
以下文件必须全部从最终发送队列生成:
`saveTrajectory` 目录中存在两类不同用途的文件,不能再混成同一条时间轴:
第一类是旧格式规划导出,用于和旧 RVBUST 导出的规划轨迹、离线分析脚本做对比。它们只受 `planning_speed_scale` 和规划/整形参数影响,不受运行时 `speedRatio` 影响:
- `JointTraj.txt`
- `JointDetialTraj.txt`
- `CartTraj.txt`
- `CartDetialTraj.txt`
- `JointDetialTraj.analysis.txt`
第二类是实发诊断,用于观察当前 `speedRatio` 下真实准备发送的 8ms 队列和触发绑定。它们必须全部从最终发送队列生成:
- `ActualSendJointTraj.txt`
- `ActualSendTiming.txt`
@@ -295,7 +286,8 @@
不允许再出现:
- 导出文件基于一套样本,
- 旧规划导出被运行时 `speedRatio` 改写,
- `ActualSend*` 基于一套样本,
- 真实发送队列又基于另一套样本。
### 11.2 推荐新增日志字段
@@ -304,8 +296,8 @@
- 请求 `speedRatio`
- 规划层轨迹时长
- 第一轮候选队列点数
- 第一轮候选队列是否通过校验
- 最终发送队列点数
- 最终发送队列是否通过校验
- 若失败,首个失败窗口的:
- 关节轴
- 时间区间
@@ -313,7 +305,7 @@
- `actual`
- `limit`
- `ratio`
- 自动拉长轮数
- 倍率改写次数,当前正常为 0
- 最终采用的执行时长
- 最终发送队列点数
- 最终触发绑定数量
@@ -324,16 +316,16 @@
- `Information`
- 记录执行请求、最终采用结果、最终通过结论
- `Warning`
- 记录第一轮候选失败与自动拉长启动
- 记录请求倍率下最终发送队列校验失败
- `Debug`
- 记录每一轮拉长的中间参数与详细差分统计
- 记录最终发送队列的时间映射与详细差分统计
## 12. 验收口径
### 12.1 功能验收
- 飞拍 `speedRatio` 可在线设置。
- 该值对下一次启动的飞拍任务生效。
- `speedRatio` 可在线设置。
- 该值对下一次启动的飞拍、普通轨迹或 `MoveJoint` 生效。
- 不需要修改 `planning_speed_scale`。
### 12.2 约束验收
@@ -345,8 +337,9 @@
### 12.3 工件与日志验收
- `JointTraj.txt / JointDetialTraj.txt / CartTraj.txt / CartDetialTraj.txt` 不随运行时 `speedRatio` 改变。
- `ActualSend*` 文件能反映最终真实发送点位与时间映射。
- 日志能定位自动拉长前后的关键参数和校验结果。
- 日志能定位请求倍率、最终发送队列和校验失败窗口。
### 12.4 回归验收
@@ -362,8 +355,10 @@
- 飞拍执行侧最终发送序列生成器测试
- `speedRatio` 非法边界值测试
- 第一轮候选失败后自动拉长成功测试
- 请求倍率下队列超限时拒绝执行测试
- `speedRatio = 1.0 / 0.8 / 0.5` 的逐周期限值通过测试
- `planning_speed_scale = 0.9` 加 `speedRatio = 0.8` 的执行时间轴等价于约 `0.72` 规划倍率的回归测试
- 普通轨迹和 `MoveJoint` 在倍率乘积相同时生成等价或可解释等价的稠密执行轨迹
- 触发绑定始终与最终发送队列一致的测试
### 13.2 编排测试
@@ -372,6 +367,8 @@
- `ExecuteFlyShotTraj` 进入运行时前,已经拿到最终发送队列
- `FanucControllerRuntime` 不再自行按 `_speedRatio` 对飞拍轨迹回采样
- `ExecuteTrajectory` 和 `MoveJoint` 的调速在规划/生成阶段完成,运行时不再做临场倍率重采样
- `SaveTrajectoryInfo` 的旧格式规划导出不随运行时 `speedRatio` 改变,而 `ActualSend*` 仍随运行时 `speedRatio` 改变
### 13.3 集成与黄金样本
@@ -401,13 +398,13 @@
新增飞拍执行侧最终发送序列生成器及相关结果类型,负责:
- 候选队列构建
- 自动拉长
- 离散校验
- 触发绑定输入准备
### 14.2 `src/Flyshot.ControllerClientCompat/`
在飞拍执行准备阶段调用该生成器,把最终发送队列放入执行结果或新的执行上下文对象。
普通轨迹规划入口和 `MoveJoint` 临时轨迹生成入口可以读取当前 `speedRatio`,并在进入运行时前折算成已调速的稠密轨迹。
### 14.3 `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
@@ -418,7 +415,10 @@
### 14.4 `src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs`
`ActualSend*` 导出改为复用最终发送队列,确保导出工件与运行时一致。
`FlyshotTrajectoryArtifactWriter` 接收两份视图:
- 旧格式规划导出视图:写 `JointTraj.txt`、`JointDetialTraj.txt`、`CartTraj.txt`、`CartDetialTraj.txt`
- 实发诊断视图:写 `ActualSendJointTraj.txt`、`ActualSendTiming.txt`、`ActualSendJerkStats.txt`、`ShotEvents.json`
### 14.5 `tests/Flyshot.Core.Tests/`
@@ -426,14 +426,15 @@
## 15. 风险与注意事项
### 15.1 不能把自动拉长误解为“偷偷改 speedRatio”
### 15.1 不能把 speedRatio 做成安全优化器
对外语义仍应保留:
对外语义必须保持:
- 用户请求的是某个 `speedRatio`
- 系统为了满足离散动力学约束,自动采用了更保守的最终执行时长
- 系统按这个 `speedRatio` 构建最终发送队列
- 若队列超限,则拒绝执行并要求调用方显式选择新的规划倍率或执行倍率
日志里必须把这件事说清楚,不能让现场误以为参数未生效。
不能因为校验失败就在执行侧自动把 `0.8` 改成 `0.2`;这会让 `speedRatio` 和 `planning_speed_scale` 的关系失去可解释性。
### 15.2 不能让触发绑定脱离最终队列
@@ -448,10 +449,12 @@
本次飞拍调速设计的核心结论是:
- `planning_speed_scale` 继续只属于规划层。
- `speedRatio` 只属于飞拍执行层,并且只对下一次启动的飞拍任务生效。
- `speedRatio` 是运行时统一执行倍率,并且只对下一次启动的运动任务生效。
- 普通轨迹和 `MoveJoint` 允许把 `speedRatio` 前移到规划或临时轨迹生成阶段,运行时只发送已生成好的 `8ms` 点列。
- 飞拍执行必须先生成最终 `8ms` 发送队列,再对该队列做逐周期 `vel/acc/jerk` 校验。
- 若 `speedRatio < 1` 导致第一轮候选队列不满足约束,则自动拉长执行时长后重建,直到通过。
- `speedRatio` 按 `trajectoryTime = sendTime * speedRatio` 确定性拉长执行时间轴,`finalSpeedRatio` 不应被自动改写。
- 若请求倍率下最终队列不满足约束,则拒绝执行并提示降低 `planning_speed_scale` 重新规划或显式设置更低 `speedRatio`。
- 运行时不再对飞拍轨迹做发送前临场回采样。
- 触发绑定、导出工件与真实发送必须全部统一到同一份最终发送队列上。
- 旧格式规划导出必须保持不受运行时 `speedRatio` 影响;`ActualSend*` 和 `ShotEvents.json` 必须统一到真实发送队列上。
该方案可以在不修改 `move_joint`、不放宽校验阈值、不中断现有触发语义的前提下,把飞拍 `speedRatio` 调速从“经验插值”收敛为“可校验、可追踪、可自动保守化”的执行机制。
该方案可以在不放宽校验阈值、不中断现有触发语义的前提下,把 `speedRatio` 调速从“发送阶段经验插值”收敛为“规划/准备阶段完成、运行时确定发送”的执行机制。

View File

@@ -0,0 +1,657 @@
<!doctype html>
<html lang="zh-CN">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>状态页摇杆点动 Demo</title>
<style>
:root {
color-scheme: light;
--bg: #f5f7fb;
--surface: #ffffff;
--line: #d8dee9;
--text: #172033;
--muted: #5b667a;
--accent: #007c89;
--accent-dark: #005d67;
--steel: #2f3b4f;
--steel-soft: #e7ebf1;
--good: #12805c;
--warn: #b7791f;
}
* {
box-sizing: border-box;
}
body {
margin: 0;
min-height: 100vh;
background: var(--bg);
color: var(--text);
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
font-size: 15px;
letter-spacing: 0;
}
header {
border-bottom: 1px solid var(--line);
background: var(--surface);
}
.topbar {
display: flex;
align-items: center;
justify-content: space-between;
gap: 16px;
width: min(1180px, calc(100% - 32px));
margin: 0 auto;
padding: 18px 0;
}
h1 {
margin: 0;
font-size: 22px;
font-weight: 650;
}
.status-pill {
display: inline-flex;
align-items: center;
gap: 8px;
min-height: 36px;
padding: 0 12px;
border: 1px solid rgba(18, 128, 92, 0.28);
border-radius: 999px;
background: rgba(18, 128, 92, 0.08);
color: var(--good);
font-weight: 650;
}
.dot {
width: 10px;
height: 10px;
border-radius: 999px;
background: var(--good);
}
main {
width: min(1180px, calc(100% - 32px));
margin: 22px auto;
}
.summary {
display: grid;
grid-template-columns: repeat(4, minmax(0, 1fr));
gap: 12px;
margin-bottom: 16px;
}
.metric,
.panel {
border: 1px solid var(--line);
border-radius: 8px;
background: var(--surface);
}
.metric {
min-height: 86px;
padding: 14px;
}
.label {
color: var(--muted);
font-size: 13px;
}
.value {
margin-top: 8px;
overflow-wrap: anywhere;
font-size: 24px;
font-weight: 650;
}
.panel h2 {
margin: 0;
padding: 14px 16px;
border-bottom: 1px solid var(--line);
font-size: 16px;
font-weight: 650;
}
.jog-panel {
overflow: hidden;
}
.jog-body {
display: grid;
grid-template-columns: minmax(0, 1fr) minmax(0, 1fr);
gap: 18px;
padding: 16px;
}
.jog-toolbar {
display: grid;
grid-template-columns: repeat(2, minmax(180px, 1fr)) minmax(220px, 1fr);
gap: 12px;
padding: 16px;
border-bottom: 1px solid var(--line);
background: #fbfcfe;
}
.step-field {
display: grid;
gap: 6px;
color: var(--muted);
font-size: 13px;
}
.step-field input {
min-height: 38px;
width: 100%;
padding: 0 10px;
border: 1px solid var(--line);
border-radius: 6px;
background: #ffffff;
color: var(--text);
font: inherit;
}
.jog-readout {
display: flex;
align-items: center;
justify-content: flex-end;
min-height: 38px;
color: var(--muted);
font-family: Consolas, "Cascadia Mono", monospace;
text-align: right;
}
.stick-card {
min-width: 0;
border: 1px solid #ccd4df;
border-radius: 8px;
background: linear-gradient(180deg, #ffffff 0%, #f3f6fa 100%);
box-shadow: inset 0 1px 0 rgba(255, 255, 255, 0.9);
}
.stick-head {
display: flex;
align-items: center;
justify-content: space-between;
gap: 12px;
padding: 14px 16px 0;
}
.stick-title {
margin: 0;
font-size: 16px;
font-weight: 650;
}
.stick-unit {
color: var(--muted);
font-size: 13px;
font-family: Consolas, "Cascadia Mono", monospace;
}
.stick-console {
display: grid;
grid-template-columns: 72px 1fr 72px;
grid-template-rows: 72px 72px 72px;
gap: 8px;
width: min(100%, 344px);
margin: 18px auto 16px;
padding: 14px;
border: 1px solid #c2cad7;
border-radius: 999px;
background:
radial-gradient(circle at 50% 50%, #ffffff 0 26%, #e6ebf2 27% 48%, #cdd5e0 49% 50%, #edf1f6 51% 100%);
}
.axis-button {
min-width: 0;
min-height: 64px;
border: 1px solid #99a8ba;
border-radius: 999px;
background: linear-gradient(180deg, #ffffff 0%, #e5ebf2 100%);
color: var(--steel);
font: inherit;
font-weight: 750;
cursor: pointer;
touch-action: none;
user-select: none;
box-shadow: 0 2px 0 #b7c1ce;
}
.axis-button:hover,
.axis-button.active {
border-color: var(--accent);
background: linear-gradient(180deg, #10a4b3 0%, var(--accent) 100%);
color: #ffffff;
box-shadow: 0 2px 0 var(--accent-dark);
}
.axis-button:focus-visible {
outline: 3px solid rgba(0, 124, 137, 0.3);
outline-offset: 2px;
}
.axis-button.up {
grid-column: 2;
grid-row: 1;
}
.axis-button.left {
grid-column: 1;
grid-row: 2;
}
.axis-button.right {
grid-column: 3;
grid-row: 2;
}
.axis-button.down {
grid-column: 2;
grid-row: 3;
}
.center-stack {
display: grid;
grid-column: 2;
grid-row: 2;
grid-template-columns: 1fr 1fr;
gap: 6px;
place-items: center;
padding: 8px;
border-radius: 999px;
background: rgba(47, 59, 79, 0.08);
}
.mini-button {
min-width: 44px;
min-height: 44px;
border: 1px solid #99a8ba;
border-radius: 999px;
background: var(--surface);
color: var(--steel);
font: inherit;
font-weight: 750;
cursor: pointer;
touch-action: none;
user-select: none;
}
.mini-button:hover,
.mini-button.active {
border-color: var(--warn);
background: #b7791f;
color: #ffffff;
}
.hint-row {
display: grid;
grid-template-columns: repeat(3, minmax(0, 1fr));
gap: 8px;
padding: 0 16px 16px;
color: var(--muted);
font-size: 13px;
text-align: center;
}
.hint-row span {
min-height: 28px;
padding: 5px 8px;
border-radius: 999px;
background: var(--steel-soft);
white-space: nowrap;
}
.log-panel {
margin-top: 16px;
overflow: hidden;
}
.log-head {
display: flex;
align-items: center;
justify-content: space-between;
gap: 12px;
padding: 14px 16px;
border-bottom: 1px solid var(--line);
}
.log-head h2 {
margin: 0;
padding: 0;
border-bottom: 0;
}
.clear-button {
min-height: 34px;
padding: 0 12px;
border: 1px solid var(--line);
border-radius: 6px;
background: #ffffff;
color: var(--muted);
font: inherit;
cursor: pointer;
}
.clear-button:hover {
border-color: var(--accent);
color: var(--accent);
}
.request-log {
display: grid;
gap: 8px;
max-height: 300px;
margin: 0;
padding: 12px 16px 16px;
overflow: auto;
background: #fbfcfe;
font-family: Consolas, "Cascadia Mono", monospace;
font-size: 13px;
}
.log-entry {
display: grid;
grid-template-columns: 86px 96px minmax(0, 1fr);
gap: 10px;
align-items: start;
padding: 9px 10px;
border: 1px solid #e1e7ef;
border-radius: 6px;
background: #ffffff;
}
.log-entry:first-child {
border-color: rgba(0, 124, 137, 0.35);
background: rgba(0, 124, 137, 0.04);
}
.log-time,
.log-axis {
color: var(--muted);
white-space: nowrap;
}
.log-payload {
overflow-wrap: anywhere;
color: var(--text);
}
@media (max-width: 920px) {
.summary,
.jog-body,
.jog-toolbar {
grid-template-columns: 1fr;
}
.jog-readout {
justify-content: flex-start;
text-align: left;
}
}
@media (max-width: 520px) {
.topbar {
align-items: flex-start;
flex-direction: column;
}
.stick-console {
grid-template-columns: 60px 1fr 60px;
grid-template-rows: 60px 60px 60px;
padding: 10px;
}
.axis-button {
min-height: 56px;
}
.log-entry {
grid-template-columns: 1fr;
}
}
</style>
</head>
<body>
<header>
<div class="topbar">
<h1>Flyshot Replacement 状态监控</h1>
<div class="status-pill"><span class="dot"></span><span>Connected</span></div>
</div>
</header>
<main>
<div class="summary">
<div class="metric">
<div class="label">机器人</div>
<div class="value">FANUC</div>
</div>
<div class="metric">
<div class="label">速度倍率</div>
<div class="value">0.70</div>
</div>
<div class="metric">
<div class="label">运动中</div>
<div class="value">否</div>
</div>
<div class="metric">
<div class="label">TCP 位姿</div>
<div class="value" style="font-size: 16px;">120.0, 80.0, 320.0</div>
</div>
</div>
<section class="panel jog-panel">
<h2>直角坐标点动</h2>
<div class="jog-toolbar">
<label class="step-field" for="linear-step">
<span>平移步长 mm</span>
<input id="linear-step" type="number" min="0.1" max="10" step="0.1" value="1">
</label>
<label class="step-field" for="angular-step">
<span>姿态步长 deg</span>
<input id="angular-step" type="number" min="0.1" max="10" step="0.1" value="1">
</label>
<div id="readout" class="jog-readout">等待点动输入</div>
</div>
<div class="jog-body">
<article class="stick-card">
<div class="stick-head">
<h3 class="stick-title">XYZ 平移摇杆</h3>
<span class="stick-unit">mm</span>
</div>
<div class="stick-console" aria-label="XYZ 平移摇杆">
<button class="axis-button up" type="button" data-axis="y" data-direction="1">Y+</button>
<button class="axis-button left" type="button" data-axis="x" data-direction="-1">X-</button>
<div class="center-stack">
<button class="mini-button" type="button" data-axis="z" data-direction="1">Z+</button>
<button class="mini-button" type="button" data-axis="z" data-direction="-1">Z-</button>
</div>
<button class="axis-button right" type="button" data-axis="x" data-direction="1">X+</button>
<button class="axis-button down" type="button" data-axis="y" data-direction="-1">Y-</button>
</div>
<div class="hint-row">
<span>X 左右</span>
<span>Y 上下</span>
<span>Z 中心</span>
</div>
</article>
<article class="stick-card">
<div class="stick-head">
<h3 class="stick-title">WPR 姿态摇杆</h3>
<span class="stick-unit">deg</span>
</div>
<div class="stick-console" aria-label="WPR 姿态摇杆">
<button class="axis-button up" type="button" data-axis="p" data-direction="1">P+</button>
<button class="axis-button left" type="button" data-axis="w" data-direction="-1">W-</button>
<div class="center-stack">
<button class="mini-button" type="button" data-axis="r" data-direction="-1">R-</button>
<button class="mini-button" type="button" data-axis="r" data-direction="1">R+</button>
</div>
<button class="axis-button right" type="button" data-axis="w" data-direction="1">W+</button>
<button class="axis-button down" type="button" data-axis="p" data-direction="-1">P-</button>
</div>
<div class="hint-row">
<span>W 左右</span>
<span>P 上下</span>
<span>R 中心</span>
</div>
</article>
</div>
</section>
<section class="panel log-panel">
<div class="log-head">
<h2>请求日志</h2>
<button id="clear-log" class="clear-button" type="button">清空</button>
</div>
<div id="request-log" class="request-log" aria-live="polite">
<div class="log-entry">
<span class="log-time">--:--:--</span>
<span class="log-axis">等待输入</span>
<span class="log-payload">按住任意摇杆方向后,这里会记录每次模拟 MovePose 请求。</span>
</div>
</div>
</section>
</main>
<script>
const readout = document.getElementById("readout");
const buttons = Array.from(document.querySelectorAll("[data-axis]"));
const linearStepInput = document.getElementById("linear-step");
const angularStepInput = document.getElementById("angular-step");
const requestLog = document.getElementById("request-log");
const clearLog = document.getElementById("clear-log");
const axisIndexes = { x: 0, y: 1, z: 2, w: 3, p: 4, r: 5 };
const poseKeys = ["x", "y", "z", "w", "p", "r"];
const maxLogCount = 80;
let timer = null;
let activeButton = null;
let requestCount = 0;
let currentPose = {
x: 120.0,
y: 80.0,
z: 320.0,
w: 0.0,
p: 45.0,
r: 0.0
};
function labelFor(button) {
const axis = button.dataset.axis.toUpperCase();
const sign = Number(button.dataset.direction) > 0 ? "+" : "-";
return axis + sign;
}
function clampStep(input, min, max) {
const value = Number(input.value);
if (!Number.isFinite(value)) {
input.value = String(min);
return min;
}
const clamped = Math.min(Math.max(value, min), max);
input.value = String(clamped);
return clamped;
}
function stepFor(axis) {
return axis === "x" || axis === "y" || axis === "z"
? clampStep(linearStepInput, 0.1, 10)
: clampStep(angularStepInput, 0.1, 10);
}
function buildMovePoseBody(button) {
const axis = button.dataset.axis;
const direction = Number(button.dataset.direction);
const key = poseKeys[axisIndexes[axis]];
currentPose[key] = Number((currentPose[key] + direction * stepFor(axis)).toFixed(6));
return {
x: currentPose.x,
y: currentPose.y,
z: currentPose.z,
w: currentPose.w,
p: currentPose.p,
r: currentPose.r
};
}
function appendLog(button, body) {
requestCount += 1;
if (requestCount === 1) {
requestLog.replaceChildren();
}
const row = document.createElement("div");
row.className = "log-entry";
const time = document.createElement("span");
time.className = "log-time";
time.textContent = new Date().toLocaleTimeString("zh-CN", { hour12: false });
const axis = document.createElement("span");
axis.className = "log-axis";
axis.textContent = "#" + String(requestCount).padStart(3, "0") + " " + labelFor(button);
const payload = document.createElement("span");
payload.className = "log-payload";
payload.textContent = "POST /move_pose/ " + JSON.stringify(body);
row.append(time, axis, payload);
requestLog.prepend(row);
while (requestLog.children.length > maxLogCount) {
requestLog.lastElementChild.remove();
}
}
function tick(button) {
const body = buildMovePoseBody(button);
readout.textContent = "模拟发送: " + labelFor(button) + " -> " + JSON.stringify(body);
appendLog(button, body);
}
function stop() {
if (timer !== null) {
window.clearInterval(timer);
timer = null;
}
if (activeButton) {
activeButton.classList.remove("active");
activeButton = null;
}
}
buttons.forEach(function (button) {
button.addEventListener("pointerdown", function (event) {
event.preventDefault();
stop();
activeButton = button;
button.classList.add("active");
tick(button);
timer = window.setInterval(function () {
tick(button);
}, 250);
});
button.addEventListener("pointerup", stop);
button.addEventListener("pointerleave", stop);
button.addEventListener("pointercancel", stop);
});
clearLog.addEventListener("click", function () {
requestCount = 0;
requestLog.innerHTML = '<div class="log-entry"><span class="log-time">--:--:--</span><span class="log-axis">已清空</span><span class="log-payload">新的点动请求会继续记录在这里。</span></div>';
readout.textContent = "等待点动输入";
});
window.addEventListener("blur", stop);
</script>
</body>
</html>

View 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` 与旧抓包主段的剩余时长差异已经很小
- 当前判断:这点差异可以忽略
后续除非现场再次提供新的反证,否则本结论作为当前项目实现与验收的记录保留。

View File

@@ -12,6 +12,7 @@ namespace Flyshot.ControllerClientCompat;
public sealed class ControllerClientCompatService : IControllerClientCompatService
{
private readonly object _stateLock = new();
private readonly object _motionLock = new();
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
private readonly ControllerClientCompatOptions _options;
private readonly ControllerClientCompatRobotCatalog _robotCatalog;
@@ -134,6 +135,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_activeRobotProfile = robotProfile;
_uploadedTrajectories.Clear();
_runtime.ResetRobot(robotProfile, robotName);
// RobotConfig.json 的 speed_ratio 是启动默认值;在线 set_speedRatio 仍可覆盖后续执行。
_runtime.SetSpeedRatio(robotSettings.SpeedRatio);
_robotSettings = robotSettings;
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
@@ -145,11 +148,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
_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,
robotProfile.DegreesOfFreedom,
robotSettings.AccLimitScale,
robotSettings.JerkLimitScale,
robotSettings.SpeedRatio,
_uploadedTrajectories.Count);
}
@@ -194,8 +198,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetActiveController(sim);
}
_runtime.SetActiveController(sim);
}
/// <inheritdoc />
@@ -211,9 +216,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.Connect(robotIp);
}
_runtime.Connect(robotIp);
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
}
@@ -223,8 +229,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.Disconnect();
}
_runtime.Disconnect();
}
/// <inheritdoc />
@@ -234,8 +241,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.EnableRobot(bufferSize);
}
_runtime.EnableRobot(bufferSize);
_logger?.LogInformation("EnableRobot 完成");
}
@@ -246,8 +253,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.DisableRobot();
}
// DisableRobot 是中断/控制命令,不能被长时间运动串行锁挡住。
_runtime.DisableRobot();
_logger?.LogInformation("DisableRobot 完成");
}
@@ -258,8 +267,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.StopMove();
}
// StopMove 必须能在运动执行期间直接进入 runtime 取消发送任务。
_runtime.StopMove();
_logger?.LogInformation("StopMove 完成");
}
@@ -269,14 +280,31 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return _runtime.GetSnapshot();
}
/// <inheritdoc />
public ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata()
{
lock (_stateLock)
{
var isSetup = _activeRobotProfile is not null;
return new ControllerClientStatusSnapshotMetadata(
isSetup,
isSetup ? _configuredRobotName : null,
isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0,
isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty<string>(),
GetServerVersion(),
GetClientVersion());
}
}
/// <inheritdoc />
public double GetSpeedRatio()
{
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetSpeedRatio();
}
return _runtime.GetSpeedRatio();
}
/// <inheritdoc />
@@ -285,8 +313,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetSpeedRatio(ratio);
}
_runtime.SetSpeedRatio(ratio);
}
/// <inheritdoc />
@@ -295,8 +324,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetIo(port, value, ioType);
}
_runtime.SetIo(port, value, ioType);
}
/// <inheritdoc />
@@ -305,8 +335,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetIo(port, ioType);
}
return _runtime.GetIo(port, ioType);
}
/// <inheritdoc />
@@ -338,8 +369,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetTcp(x, y, z);
}
_runtime.SetTcp(x, y, z);
}
/// <inheritdoc />
@@ -348,8 +380,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetTcp();
}
return _runtime.GetTcp();
}
/// <inheritdoc />
@@ -358,8 +391,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetJointPositions();
}
return _runtime.GetJointPositions();
}
/// <inheritdoc />
@@ -370,16 +404,44 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
RobotProfile robot;
lock (_stateLock)
{
var robot = RequireActiveRobot();
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
}
lock (_motionLock)
{
ExecuteMoveJointAndWait(robot, jointPositions, "MoveJoint");
}
_logger?.LogInformation("MoveJoint 完成");
}
/// <inheritdoc />
public void MovePose(IReadOnlyList<double> pose)
{
ArgumentNullException.ThrowIfNull(pose);
_logger?.LogInformation("MovePose 开始: 目标位姿维度={PoseCount}", pose.Count);
_logger?.LogDebug("MovePose 目标位姿: {Pose}", string.Join(", ", pose.Select(v => v.ToString("F4"))));
RobotProfile robot;
lock (_stateLock)
{
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
}
lock (_motionLock)
{
ExecuteMovePoseAndWait(robot, pose, "MovePose");
}
_logger?.LogInformation("MovePose 完成");
}
/// <inheritdoc />
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
{
@@ -395,21 +457,29 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
RobotProfile robot;
CompatibilityRobotSettings settings;
lock (_stateLock)
{
var robot = RequireActiveRobot();
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
settings = RequireRobotSettings();
}
lock (_motionLock)
{
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
var planningSpeedScale = settings.PlanningSpeedScale;
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
_logger?.LogInformation(
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
bundle.Result.Method,
bundle.Result.Duration.TotalSeconds,
bundle.Result.IsValid,
bundle.Result.DenseJointTrajectory?.Count ?? 0,
planningSpeedScale);
planningSpeedScale,
speedRatio);
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
}
@@ -423,8 +493,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetPose();
}
return _runtime.GetPose();
}
/// <inheritdoc />
@@ -438,16 +509,20 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
trajectory.Waypoints.Count,
trajectory.ShotFlags.Count(static f => f));
string robotName;
CompatibilityRobotSettings settings;
lock (_stateLock)
{
EnsureRuntimeEnabled();
_uploadedTrajectories[trajectory.Name] = trajectory;
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
var settings = _robotSettings ?? CreateDefaultRobotSettings();
_trajectoryStore.Save(robotName, settings, trajectory);
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
settings = _robotSettings ?? CreateDefaultRobotSettings();
}
// RobotConfig.json 持久化是文件 I/O,放在状态锁外,避免状态页轮询被磁盘写入拖住。
_trajectoryStore.Save(robotName, settings, trajectory);
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
}
@@ -473,51 +548,68 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
RobotProfile robot;
CompatibilityRobotSettings settings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock)
{
var robot = RequireActiveRobot();
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist.");
}
// 飞拍执行只拿上传轨迹的瞬时副本,后续规划/导出都不再依赖字典锁。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
if (trajectory.Waypoints.Count == 0)
{
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
}
settings = RequireRobotSettings();
}
lock (_motionLock)
{
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
var settings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
options.SaveTrajectory,
robot,
trajectory,
options,
settings,
bundle,
settings.PlanningSpeedScale,
speedRatio);
_logger?.LogInformation(
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
name,
bundle.Result.Method,
bundle.Result.Duration.TotalSeconds,
bundle.Result.TriggerTimeline.Count,
bundle.Result.UsedCache,
settings.PlanningSpeedScale);
settings.PlanningSpeedScale,
speedRatio);
if (options.MoveToStart)
{
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
ExecuteMoveJointAndWait(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
}
else
{
//检验当前机械臂的关节坐标与计划轨迹的第一个点之前的差异,如果差异过大.就不报警,不执行下去
var currentJointPositions = _runtime.GetJointPositions();
var targetJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[0].Positions;
var diff = currentJointPositions.Zip(targetJointPositions, (c, t) => Math.Abs(c - t)).Sum();
if (diff > 0.01)
// 正式飞拍前必须确认机器人反馈已经在轨迹起点附近,避免 J519 目标突变。
if (!IsFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name))
{
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之前的差异过大 name={Name}", name);
return;
}
}
@@ -526,7 +618,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
if (options.Wait)
{
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
var executionDuration = bundle.PreparedExecution is null
? bundle.Result.Duration
: TimeSpan.FromSeconds(bundle.PreparedExecution.FinalDurationSeconds);
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", executionDuration);
}
}
@@ -539,7 +634,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <param name="robot">当前机器人模型。</param>
/// <param name="targetJointPositions">目标关节位置,单位为弧度。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
private void ExecuteMoveJointAndWait(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
{
var currentJointPositions = _runtime.GetJointPositions();
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
@@ -558,6 +653,57 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 从当前 TCP 位姿生成临时直角 PTP 稠密轨迹并阻塞等待运行时完成。
/// </summary>
/// <param name="robot">当前机器人模型,用于取得 J519 伺服周期。</param>
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMovePoseAndWait(RobotProfile robot, IReadOnlyList<double> targetPose, string operationName)
{
var currentPose = NormalizeRuntimePose(_runtime.GetPose());
EnsurePoseVector(targetPose, nameof(targetPose));
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MovePoseTrajectoryGenerator.CreateResult(currentPose, targetPose, robot.ServoPeriod, speedRatio, _logger);
_logger?.LogInformation(
"{OperationName} 直角PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
operationName,
speedRatio,
moveResult.Duration.TotalSeconds,
moveResult.DenseCartesianTrajectory?.Count ?? 0);
_runtime.ExecuteCartesianTrajectory(moveResult, targetPose);
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 校验当前反馈是否接近飞拍起点;不接近时直接抛出兼容错误。
/// </summary>
private void EnsureFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
{
if (!IsFeedbackNearFlyshotStart(targetJointPositions, name))
{
throw new InvalidOperationException("Robot feedback is not near flyshot start.");
}
}
/// <summary>
/// 检查当前机械臂关节反馈与计划轨迹第一个点之间的差异。
/// </summary>
private bool IsFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
{
var currentJointPositions = _runtime.GetJointPositions();
var diff = currentJointPositions.Zip(targetJointPositions, (current, target) => Math.Abs(current - target)).Sum();
if (diff <= 0.01)
{
return true;
}
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之间的差异过大 name={Name}, diff={Diff}", name, diff);
return false;
}
/// <summary>
/// 等待运行时报告当前运动结束,用于把 move_to_start 与正式飞拍轨迹串行化。
/// </summary>
@@ -606,32 +752,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
RobotProfile robot;
CompatibilityRobotSettings planningSettings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock)
{
var robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist.");
}
// 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。
var planningSettings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
// _trajectoryStore.Save(robotName, settings, trajectory);
// 保存轨迹信息会执行规划和文件导出,先复制上传数据再释放状态锁。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
planningSettings = RequireRobotSettings();
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
}
@@ -645,31 +804,48 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
RobotProfile robot;
CompatibilityRobotSettings planningSettings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock)
{
var robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist.");
}
var planningSettings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, _runtime.GetSnapshot().SpeedRatio);
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
duration = bundle.Result.Duration;
_logger?.LogInformation(
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
name, bundle.Result.IsValid, duration.TotalSeconds);
return bundle.Result.IsValid;
// 有效性检查只消费当前快照,不要求和后续上传/删除形成长事务。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
planningSettings = RequireRobotSettings();
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
name, bundle.Result.IsValid, duration.TotalSeconds);
return bundle.Result.IsValid;
}
/// <inheritdoc />
@@ -682,6 +858,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
string robotName;
lock (_stateLock)
{
if (!_uploadedTrajectories.Remove(name))
@@ -690,10 +867,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
throw new InvalidOperationException("DeleteFlyShotTraj failed");
}
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
_trajectoryStore.Delete(robotName, name);
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
}
// 删除持久化文件不占用状态锁,状态页只需要看到内存字典的即时快照。
_trajectoryStore.Delete(robotName, name);
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
}
@@ -733,6 +912,21 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return _robotSettings ?? CreateDefaultRobotSettings();
}
/// <summary>
/// 复制一份上传轨迹快照,避免锁外规划期间观察到可变集合引用。
/// </summary>
/// <param name="trajectory">待复制的上传轨迹。</param>
/// <returns>上传轨迹副本。</returns>
private static ControllerClientCompatUploadedTrajectory CloneUploadedTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
{
return new ControllerClientCompatUploadedTrajectory(
trajectory.Name,
trajectory.Waypoints,
trajectory.ShotFlags,
trajectory.OffsetValues,
trajectory.AddressGroups);
}
/// <summary>
/// 校验机器人已经完成初始化。
/// </summary>
@@ -776,26 +970,87 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
}
/// <summary>
/// 校验直角位姿为 [x,y,z,w,p,r] 六维有限数。
/// </summary>
/// <param name="pose">待校验位姿,单位为 mm/deg。</param>
/// <param name="paramName">调用方参数名。</param>
private static void EnsurePoseVector(IReadOnlyList<double> pose, string paramName)
{
if (pose.Count != 6)
{
throw new ArgumentException("位姿必须为 [x,y,z,w,p,r] 六维数组。", paramName);
}
for (var index = 0; index < pose.Count; index++)
{
var value = pose[index];
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个位姿值必须是有限数值。");
}
}
}
/// <summary>
/// 将运行时位姿快照归一化为 J519 直角命令需要的 [x,y,z,w,p,r] 六维。
/// </summary>
/// <param name="pose">运行时返回的位姿数组。</param>
/// <returns>六维直角位姿。</returns>
private static IReadOnlyList<double> NormalizeRuntimePose(IReadOnlyList<double> pose)
{
if (pose.Count < 6)
{
throw new InvalidOperationException("Runtime pose must contain at least [x,y,z,w,p,r].");
}
var normalized = pose.Take(6).ToArray();
EnsurePoseVector(normalized, nameof(pose));
return normalized;
}
/// <summary>
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
/// </summary>
/// <param name="name">飞拍轨迹名称。</param>
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
/// <param name="robot">当前机器人模型。</param>
/// <param name="bundle">规划结果包。</param>
/// <param name="uploaded">已上传的飞拍轨迹。</param>
/// <param name="options">本次规划选项。</param>
/// <param name="settings">当前机器人兼容设置。</param>
/// <param name="executionBundle">运行时和 ActualSend 诊断使用的规划结果包。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次运行时速度倍率。</param>
private void ExportFlyshotArtifactsIfRequested(
string name,
bool saveTrajectory,
RobotProfile robot,
PlannedExecutionBundle bundle)
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions options,
CompatibilityRobotSettings settings,
PlannedExecutionBundle executionBundle,
double planningSpeedScale,
double speedRatio)
{
if (!saveTrajectory)
{
return;
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
// 旧格式 Joint/Cart 导出必须只反映 planning_speed_scale;ActualSend* 才反映当前运行时 speedRatio。
var exportPlanningBundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
uploaded,
new FlyshotExecutionOptions(
moveToStart: options.MoveToStart,
useCache: false,
saveTrajectory: options.SaveTrajectory,
method: options.Method,
wait: options.Wait),
settings,
planningSpeedScale,
speedRatio: 1.0);
_artifactWriter.WriteUploadedFlyshot(name, robot, exportPlanningBundle, executionBundle, speedRatio);
}
/// <summary>
@@ -808,9 +1063,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
{
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
robot,
bundle.ExecutionTrajectory,
bundle.Result,
robot.ServoPeriod.TotalSeconds,
speedRatio);
requestedSpeedRatio: speedRatio,
samplingSpeedRatio: 1.0);
var preparedResult = new TrajectoryResult(
programName: bundle.Result.ProgramName,
method: bundle.Result.Method,
@@ -826,7 +1083,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
denseJointTrajectory: bundle.Result.DenseJointTrajectory,
preparedFlyshotExecution: preparedExecution);
return new PlannedExecutionBundle(bundle.PlannedTrajectory, bundle.ShotTimeline, preparedResult, preparedExecution);
return new PlannedExecutionBundle(
bundle.PlannedTrajectory,
bundle.ShotTimeline,
preparedResult,
preparedExecution,
bundle.ExecutionTrajectory);
}
/// <summary>

View File

@@ -0,0 +1,62 @@
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// 保存状态页需要的 ControllerClient 兼容层元数据快照,避免状态页连续读取多个受保护字段。
/// </summary>
public sealed record ControllerClientStatusSnapshotMetadata
{
/// <summary>
/// 初始化一份兼容层状态元数据快照。
/// </summary>
/// <param name="isSetup">当前是否已经完成机器人初始化。</param>
/// <param name="robotName">当前机器人名称;未初始化时为空。</param>
/// <param name="degreesOfFreedom">当前机器人自由度;未初始化时为 0。</param>
/// <param name="uploadedTrajectories">当前已上传轨迹名称快照。</param>
/// <param name="serverVersion">兼容服务端版本。</param>
/// <param name="clientVersion">兼容客户端版本。</param>
public ControllerClientStatusSnapshotMetadata(
bool isSetup,
string? robotName,
int degreesOfFreedom,
IReadOnlyList<string> uploadedTrajectories,
string serverVersion,
string clientVersion)
{
IsSetup = isSetup;
RobotName = robotName;
DegreesOfFreedom = degreesOfFreedom;
UploadedTrajectories = uploadedTrajectories.ToArray();
ServerVersion = serverVersion;
ClientVersion = clientVersion;
}
/// <summary>
/// 获取当前是否已经完成机器人初始化。
/// </summary>
public bool IsSetup { get; }
/// <summary>
/// 获取当前机器人名称;未初始化时为空。
/// </summary>
public string? RobotName { get; }
/// <summary>
/// 获取当前机器人自由度;未初始化时为 0。
/// </summary>
public int DegreesOfFreedom { get; }
/// <summary>
/// 获取已上传轨迹名称数组快照。
/// </summary>
public IReadOnlyList<string> UploadedTrajectories { get; }
/// <summary>
/// 获取兼容服务端版本。
/// </summary>
public string ServerVersion { get; }
/// <summary>
/// 获取兼容客户端版本。
/// </summary>
public string ClientVersion { get; }
}

View File

@@ -55,21 +55,24 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="waypoints">普通轨迹关节路点。</param>
/// <param name="options">执行参数。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次执行速度倍率;普通轨迹在规划阶段折算进有效限速倍率。</param>
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanOrdinaryTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> waypoints,
TrajectoryExecutionOptions? options = null,
double planningSpeedScale = 1.0)
double planningSpeedScale = 1.0,
double speedRatio = 1.0)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions();
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(planningSpeedScale, speedRatio, "普通轨迹");
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale);
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}",
waypoints.Count, options.Method, planningSpeedScale, speedRatio, effectivePlanningSpeedScale);
var program = CreateProgram(
name: "ordinary-trajectory",
@@ -101,7 +104,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
result.Duration.TotalSeconds,
result.DenseJointTrajectory?.Count ?? 0);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: executionTrajectory);
}
/// <summary>
@@ -112,24 +115,35 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="options">执行参数。</param>
/// <param name="settings">兼容层机器人设置。</param>
/// <param name="planningSpeedScale">规划速度倍率。</param>
/// <param name="speedRatio">本次飞拍使用的速度倍率;为空时使用配置默认值。</param>
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanUploadedFlyshot(
RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions? options = null,
CompatibilityRobotSettings? settings = null,
double? planningSpeedScale = null)
double? planningSpeedScale = null,
double? speedRatio = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(uploaded);
options ??= new FlyshotExecutionOptions();
settings ??= CreateDefaultRobotSettings();
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var requestedSpeedRatio = speedRatio ?? settings.SpeedRatio;
var configuredPlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(configuredPlanningSpeedScale, requestedSpeedRatio, "飞拍");
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation(
"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);
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
uploaded.Name,
uploaded.Waypoints.Count,
options.Method,
options.UseCache,
configuredPlanningSpeedScale,
requestedSpeedRatio,
effectivePlanningSpeedScale,
settings.SmoothStartStopTiming);
var program = CreateProgram(
name: uploaded.Name,
@@ -170,7 +184,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
denseJointTrajectory,
usedCache: false,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: smoothedExecutionTrajectory);
_logger?.LogInformation(
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
@@ -329,6 +343,23 @@ public sealed class ControllerClientTrajectoryOrchestrator
adaptIcspTryNum: 5);
}
/// <summary>
/// 解析飞拍生成轨迹时真正使用的规划速度倍率。
/// </summary>
/// <param name="planningSpeedScale">配置或调用方传入的规划倍率。</param>
/// <param name="speedRatio">本次执行速度倍率。</param>
/// <returns>用于缩放规划关节限制的有效倍率。</returns>
private static double ResolveEffectivePlanningSpeedScale(double planningSpeedScale, double speedRatio, string trajectoryKind)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), $"{trajectoryKind}速度倍率必须是有限正数。");
}
// 生成轨迹时把执行倍率折算进规划限制,保证 planning_speed_scale * speed_ratio 相同的配置产出同一条轨迹。
return planningSpeedScale * speedRatio;
}
/// <summary>
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
/// </summary>
@@ -361,7 +392,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
return robot;
}
// RVBUST 规划阶段会用独立限速倍率缩放有效限制;运行时 speedRatio 仍只负责 J519 下发重采样。
// speed_ratio 已在调用方折算到规划倍率中;运行时只消费已经生成好的 8ms 稠密轨迹。
var scaledLimits = robot.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
@@ -452,7 +483,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
executionTrajectory.Robot,
denseJointTrajectory,
trajectoryName: executionTrajectory.OriginalProgram.Name);
trajectoryName: executionTrajectory.OriginalProgram.Name,
validateJerk: false);
return denseJointTrajectory;
}
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)

View File

@@ -49,9 +49,27 @@ public sealed class FlyshotTrajectoryArtifactWriter
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="bundle">规划结果包。</param>
/// <param name="speedRatio">导出 J519 实发采样点时使用的速度倍率。</param>
/// <param name="bundle">规划结果包;旧规划导出和实发诊断使用同一份视图。</param>
/// <param name="speedRatio">请求速度倍率,仅作为诊断列保留;最终采样倍率已在规划阶段处理。</param>
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
{
WriteUploadedFlyshot(trajectoryName, robot, bundle, bundle, speedRatio);
}
/// <summary>
/// 将飞拍规划结果导出到 Config/Data/name,并显式区分旧规划导出视图和实发诊断视图。
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="exportPlanningBundle">旧格式规划导出使用的结果包,不应包含运行时 speedRatio 折算。</param>
/// <param name="executionBundle">实发诊断使用的结果包,应与运行时最终发送队列保持一致。</param>
/// <param name="speedRatio">请求速度倍率,仅作为 ActualSend 诊断列保留。</param>
public void WriteUploadedFlyshot(
string trajectoryName,
RobotProfile robot,
PlannedExecutionBundle exportPlanningBundle,
PlannedExecutionBundle executionBundle,
double speedRatio = 1.0)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
@@ -59,31 +77,32 @@ public sealed class FlyshotTrajectoryArtifactWriter
}
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(bundle);
ArgumentNullException.ThrowIfNull(exportPlanningBundle);
ArgumentNullException.ThrowIfNull(executionBundle);
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
Directory.CreateDirectory(outputDir);
if (bundle.Result.DenseJointTrajectory is null)
if (exportPlanningBundle.Result.DenseJointTrajectory is null)
{
throw new InvalidOperationException("导出飞拍轨迹工件前必须先生成执行侧稠密轨迹。");
throw new InvalidOperationException("导出飞拍旧格式规划工件前必须先生成规划明细轨迹。");
}
// 明细文件现在定义为“执行侧 8ms 稠密轨迹的 16ms 低频视图”,避免再次从 PlannedTrajectory 生成另一条轨迹。
// 旧格式规划文件固定使用不含运行时 speedRatio 的规划视图,避免和 ActualSend 实发诊断混在一起。
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
var jointTrajectory = BuildJointRows(exportPlanningBundle.PlannedTrajectory);
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
var executionDenseTrajectory = bundle.Result.DenseJointTrajectory;
var executionDenseTrajectory = exportPlanningBundle.Result.DenseJointTrajectory;
var jointDetailTrajectory = DownsampleDenseRows(
executionDenseTrajectory,
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
var cartTrajectory = BuildCartesianRows(exportPlanningBundle.PlannedTrajectory, kinematicsModel);
var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
WriteActualSendArtifacts(outputDir, robot, bundle.Result, speedRatio);
WriteActualSendArtifacts(outputDir, robot, executionBundle.Result, speedRatio);
_logger?.LogInformation(
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
@@ -107,16 +126,12 @@ public sealed class FlyshotTrajectoryArtifactWriter
return;
}
if (preparedExecution is null && (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio)))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var samples = preparedExecution is null
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio)
speedRatio: 1.0)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
@@ -142,7 +157,8 @@ public sealed class FlyshotTrajectoryArtifactWriter
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName);
trajectoryName: result.ProgramName,
validateJerk: false);
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
List<IReadOnlyList<double>> timingRows = preparedExecution is null
? new List<IReadOnlyList<double>>(samples.Count)
@@ -160,7 +176,7 @@ public sealed class FlyshotTrajectoryArtifactWriter
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
if (preparedExecution is null)
{
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample, speedRatio, stretchIterationCount: 0));
if (previousSendTime is not null && previousJoints is not null)
{

View File

@@ -198,6 +198,7 @@ public sealed class JsonFlyshotTrajectoryStore
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
["speed_ratio"] = JsonValue.Create(settings.SpeedRatio),
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
};
}

View File

@@ -95,6 +95,12 @@ public interface IControllerClientCompatService
/// <returns>控制器运行时状态快照。</returns>
ControllerStateSnapshot GetControllerSnapshot();
/// <summary>
/// 读取状态页所需的兼容层元数据快照。
/// </summary>
/// <returns>兼容层元数据快照。</returns>
ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata();
/// <summary>
/// 获取当前速度倍率。
/// </summary>
@@ -157,6 +163,12 @@ public interface IControllerClientCompatService
/// <param name="jointPositions">目标关节位置。</param>
void MoveJoint(IReadOnlyList<double> jointPositions);
/// <summary>
/// 按直角坐标移动到目标 TCP 位姿。
/// </summary>
/// <param name="pose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
void MovePose(IReadOnlyList<double> pose);
/// <summary>
/// 执行普通轨迹。
/// </summary>

View File

@@ -53,7 +53,7 @@ internal static class MoveJointTrajectoryGenerator
///
/// 处理流程:
/// 1. 根据关节限位计算连续时间律理论最短时长
/// 2. 按 speedRatio 换算轨迹采样周期,并将时长对齐到整数个采样间隔
/// 2. 按 speedRatio 缩放规划限位,并将时长对齐到固定伺服采样间隔
/// 3. 用 7 阶平滑点到点时间律生成稠密轨迹点
/// 4. 按离散点反查速度、加速度和 jerk,必要时拉长时长重算
/// 5. 封装为 TrajectoryResult 返回
@@ -61,7 +61,7 @@ internal static class MoveJointTrajectoryGenerator
/// <param name="robot">机器人配置,含自由度数和关节限位。</param>
/// <param name="startJoints">起始关节角(弧度)。</param>
/// <param name="targetJoints">目标关节角(弧度)。</param>
/// <param name="speedRatio">速度倍率,必须大于 0;当前链路中用于换算轨迹采样周期。</param>
/// <param name="speedRatio">速度倍率,必须大于 0;在生成阶段折算进有效关节限位。</param>
/// <param name="logger">可选的诊断日志。</param>
public static TrajectoryResult CreateResult(
RobotProfile robot,
@@ -84,13 +84,14 @@ internal static class MoveJointTrajectoryGenerator
throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
}
var requestedDurationSeconds = ResolveDurationSeconds(robot, startJoints, targetJoints);
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds * speedRatio;
var planningRobot = ApplySpeedRatioToJointLimits(robot, speedRatio);
var requestedDurationSeconds = ResolveDurationSeconds(planningRobot, startJoints, targetJoints);
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds;
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
var stretchCount = 0;
while (!SatisfiesDiscreteJointLimits(robot, denseJointTrajectory))
while (!SatisfiesDiscreteJointLimits(planningRobot, denseJointTrajectory))
{
stretchCount++;
if (stretchCount > MaxDiscreteLimitStretchIterations)
@@ -127,6 +128,34 @@ internal static class MoveJointTrajectoryGenerator
denseJointTrajectory: denseJointTrajectory);
}
/// <summary>
/// 将执行速度倍率折算为 MoveJoint 规划限位,保持最终稠密轨迹仍按物理 8ms 周期生成。
/// </summary>
private static RobotProfile ApplySpeedRatioToJointLimits(RobotProfile robot, double speedRatio)
{
if (Math.Abs(speedRatio - 1.0) < 1e-12)
{
return robot;
}
var scaledLimits = robot.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
limit.VelocityLimit * speedRatio,
limit.AccelerationLimit * speedRatio * speedRatio,
limit.JerkLimit * speedRatio * speedRatio * speedRatio))
.ToArray();
return new RobotProfile(
name: robot.Name,
modelPath: robot.ModelPath,
degreesOfFreedom: robot.DegreesOfFreedom,
jointLimits: scaledLimits,
jointCouplings: robot.JointCouplings,
servoPeriod: robot.ServoPeriod,
triggerPeriod: robot.TriggerPeriod);
}
/// <summary>
/// 根据 7 阶平滑点到点时间律和每轴限位,计算 MoveJoint 理论最短时长。
///

View File

@@ -0,0 +1,353 @@
using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// MovePose 直角坐标轨迹生成器。
/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动,按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。
/// </summary>
internal static class MovePoseTrajectoryGenerator
{
/// <summary>
/// 直角位置轴最大速度,单位为 mm/s。
/// </summary>
private const double LinearVelocityLimit = 50.0;
/// <summary>
/// 直角位置轴最大加速度,单位为 mm/s^2。
/// </summary>
private const double LinearAccelerationLimit = 250.0;
/// <summary>
/// 直角位置轴最大 jerk,单位为 mm/s^3。
/// </summary>
private const double LinearJerkLimit = 1250.0;
/// <summary>
/// 姿态轴最大速度,单位为 deg/s。
/// </summary>
private const double AngularVelocityLimit = 10.0;
/// <summary>
/// 姿态轴最大加速度,单位为 deg/s^2。
/// </summary>
private const double AngularAccelerationLimit = 50.0;
/// <summary>
/// 姿态轴最大 jerk,单位为 deg/s^3。
/// </summary>
private const double AngularJerkLimit = 250.0;
/// <summary>
/// 7 阶平滑点到点时间律的一阶导数最大值。
/// </summary>
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
/// <summary>
/// 7 阶平滑点到点时间律的二阶导数最大值。
/// </summary>
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
/// <summary>
/// 7 阶平滑点到点时间律的三阶导数最大值。
/// </summary>
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 单次 MovePose 最大采样点数上限,避免极低速度倍率生成过大的队列。
/// </summary>
private const int MaxMovePoseSampleCount = 1_000_000;
/// <summary>
/// 离散限位校验允许的浮点容差。
/// </summary>
private const double DiscreteLimitTolerance = 1.000001;
/// <summary>
/// 离散限位校验失败时最多拉长的采样周期次数。
/// </summary>
private const int MaxDiscreteLimitStretchIterations = 10_000;
/// <summary>
/// 计算 MovePose 轨迹的完整结果。
/// </summary>
/// <param name="startPose">起始位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r],单位为 mm/deg。</param>
/// <param name="servoPeriod">J519 伺服发送周期。</param>
/// <param name="speedRatio">速度倍率,必须大于 0。</param>
/// <param name="logger">可选诊断日志。</param>
/// <returns>包含稠密直角轨迹的规划结果。</returns>
public static TrajectoryResult CreateResult(
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose,
TimeSpan servoPeriod,
double speedRatio,
ILogger? logger = null)
{
ArgumentNullException.ThrowIfNull(startPose);
ArgumentNullException.ThrowIfNull(targetPose);
EnsurePoseVector(startPose, nameof(startPose));
EnsurePoseVector(targetPose, nameof(targetPose));
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose execution.");
}
var samplePeriodSeconds = servoPeriod.TotalSeconds;
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
{
throw new InvalidOperationException("MovePose servo period must be a finite positive duration.");
}
var requestedDurationSeconds = ResolveDurationSeconds(startPose, targetPose, speedRatio);
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
var denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
var stretchCount = 0;
while (!SatisfiesDefaultCartesianLimits(denseCartesianTrajectory, speedRatio))
{
stretchCount++;
if (stretchCount > MaxDiscreteLimitStretchIterations)
{
throw new InvalidOperationException("MovePose duration cannot be stretched enough to satisfy Cartesian limits.");
}
// 离散差分以真实下发点为准;不满足时逐周期拉长后重采样。
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
}
logger?.LogDebug(
"MovePoseTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
requestedDurationSeconds,
durationSeconds,
speedRatio,
samplePeriodSeconds,
denseCartesianTrajectory.Count,
stretchCount);
return new TrajectoryResult(
programName: "move-pose",
method: PlanningMethod.Doubles,
isValid: true,
duration: TimeSpan.FromSeconds(durationSeconds),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: denseCartesianTrajectory.Count,
denseCartesianTrajectory: denseCartesianTrajectory);
}
/// <summary>
/// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。
/// </summary>
/// <param name="rows">稠密轨迹行,每行为 [time,x,y,z,w,p,r]。</param>
/// <param name="speedRatio">生成该轨迹时使用的速度倍率。</param>
/// <returns>满足限制时返回 true,否则返回 false。</returns>
public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList<IReadOnlyList<double>> rows, double speedRatio)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose limit validation.");
}
double? previousTime = null;
double[]? previousPositions = null;
double[]? previousVelocities = null;
double[]? previousAccelerations = null;
foreach (var row in rows)
{
if (row.Count != 7)
{
throw new InvalidOperationException("MovePose dense trajectory rows must contain time plus six pose values.");
}
var currentTime = row[0];
var currentPositions = row.Skip(1).Take(6).ToArray();
if (previousTime is not null && previousPositions is not null)
{
var dt = currentTime - previousTime.Value;
if (dt <= 0.0)
{
throw new InvalidOperationException("MovePose dense trajectory timestamps must be strictly increasing.");
}
var velocities = new double[6];
var accelerations = new double[6];
for (var index = 0; index < 6; index++)
{
var limits = GetAxisLimits(index, speedRatio);
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
if (Math.Abs(velocities[index]) > limits.Velocity * DiscreteLimitTolerance)
{
return false;
}
accelerations[index] = previousVelocities is null
? 0.0
: (velocities[index] - previousVelocities[index]) / dt;
if (Math.Abs(accelerations[index]) > limits.Acceleration * DiscreteLimitTolerance)
{
return false;
}
if (previousAccelerations is not null)
{
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
if (Math.Abs(jerk) > limits.Jerk * DiscreteLimitTolerance)
{
return false;
}
}
}
previousVelocities = velocities;
previousAccelerations = accelerations;
}
previousTime = currentTime;
previousPositions = currentPositions;
}
return true;
}
/// <summary>
/// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。
/// </summary>
private static double ResolveDurationSeconds(IReadOnlyList<double> startPose, IReadOnlyList<double> targetPose, double speedRatio)
{
var duration = 0.0;
for (var index = 0; index < 6; index++)
{
var distance = Math.Abs(targetPose[index] - startPose[index]);
if (distance <= 0.0)
{
continue;
}
var limits = GetAxisLimits(index, speedRatio);
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limits.Velocity;
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limits.Acceleration);
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limits.Jerk);
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
/// <summary>
/// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose,
double durationSeconds,
double samplePeriodSeconds)
{
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
for (var index = 0L; index < sampleCount; index++)
{
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
rows.Add(CreateRow(time, durationSeconds, startPose, targetPose));
}
return rows;
}
/// <summary>
/// 将请求时长向上对齐到整数个伺服周期。
/// </summary>
private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
{
return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds;
}
/// <summary>
/// 计算时长对应的采样间隔数,轨迹至少包含起点和终点两帧。
/// </summary>
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
{
var rawIntervals = durationSeconds / samplePeriodSeconds;
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
{
throw new InvalidOperationException("MovePose sample count is not representable.");
}
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
intervals = Math.Max(1, intervals);
if (intervals + 1 > MaxMovePoseSampleCount)
{
throw new InvalidOperationException($"MovePose sample count must be between 2 and {MaxMovePoseSampleCount}.");
}
return intervals;
}
/// <summary>
/// 构造单个轨迹行:[time_seconds,x,y,z,w,p,r]。
/// </summary>
private static IReadOnlyList<double> CreateRow(
double timeSeconds,
double durationSeconds,
IReadOnlyList<double> startPose,
IReadOnlyList<double> targetPose)
{
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
var scale = MoveJointTrajectoryGenerator.EvaluateSmoothPtpPositionScale(u);
var row = new double[7];
row[0] = Math.Round(timeSeconds, 9);
for (var index = 0; index < 6; index++)
{
row[index + 1] = startPose[index] + ((targetPose[index] - startPose[index]) * scale);
}
return row;
}
/// <summary>
/// 获取指定直角轴在当前速度倍率下的有效限制。
/// </summary>
private static CartesianAxisLimit GetAxisLimits(int index, double speedRatio)
{
var linearAxis = index < 3;
var velocity = linearAxis ? LinearVelocityLimit : AngularVelocityLimit;
var acceleration = linearAxis ? LinearAccelerationLimit : AngularAccelerationLimit;
var jerk = linearAxis ? LinearJerkLimit : AngularJerkLimit;
return new CartesianAxisLimit(
velocity * speedRatio,
acceleration * speedRatio * speedRatio,
jerk * speedRatio * speedRatio * speedRatio);
}
/// <summary>
/// 校验位姿向量必须为六维有限数。
/// </summary>
private static void EnsurePoseVector(IReadOnlyList<double> pose, string parameterName)
{
if (pose.Count != 6)
{
throw new ArgumentException("MovePose expects pose [x,y,z,w,p,r].", parameterName);
}
if (pose.Any(static value => double.IsNaN(value) || double.IsInfinity(value)))
{
throw new ArgumentException("MovePose pose values must be finite.", parameterName);
}
}
/// <summary>
/// 表示单个直角轴的有效速度、加速度和 jerk 限制。
/// </summary>
private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk);
}

View File

@@ -12,26 +12,35 @@ public sealed class PlannedExecutionBundle
/// <summary>
/// 初始化一份执行规划结果包。
/// </summary>
/// <param name="plannedTrajectory">规划后的轨迹。</param>
/// <param name="plannedTrajectory">规划后的原始连续轨迹。</param>
/// <param name="shotTimeline">飞拍触发时间轴。</param>
/// <param name="result">对运行时和监控层暴露的规划结果。</param>
/// <param name="preparedExecution">飞拍链路预先准备好的最终发送队列。</param>
/// <param name="executionTrajectory">与运行时结果同口径的连续执行轨迹;为空时复用 plannedTrajectory。</param>
public PlannedExecutionBundle(
PlannedTrajectory plannedTrajectory,
ShotTimeline shotTimeline,
TrajectoryResult result,
FlyshotPreparedExecution? preparedExecution = null)
FlyshotPreparedExecution? preparedExecution = null,
PlannedTrajectory? executionTrajectory = null)
{
PlannedTrajectory = plannedTrajectory ?? throw new ArgumentNullException(nameof(plannedTrajectory));
ShotTimeline = shotTimeline ?? throw new ArgumentNullException(nameof(shotTimeline));
Result = result ?? throw new ArgumentNullException(nameof(result));
PreparedExecution = preparedExecution;
ExecutionTrajectory = executionTrajectory ?? plannedTrajectory;
}
/// <summary>
/// 获取规划后的轨迹。
/// 获取规划后的原始连续轨迹。
/// </summary>
public PlannedTrajectory PlannedTrajectory { get; }
/// <summary>
/// 获取与运行时结果同口径的连续执行轨迹,用于构建最终 8ms 发送队列。
/// </summary>
public PlannedTrajectory ExecutionTrajectory { get; }
/// <summary>
/// 获取飞拍触发时间轴。
/// </summary>

View File

@@ -21,6 +21,7 @@ public sealed class CompatibilityRobotSettings
double jerkLimitScale,
int adaptIcspTryNum,
double planningSpeedScale = 1.0,
double speedRatio = 1.0,
bool smoothStartStopTiming = true)
{
ArgumentNullException.ThrowIfNull(ioAddresses);
@@ -50,6 +51,11 @@ public sealed class CompatibilityRobotSettings
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)
{
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
@@ -70,6 +76,7 @@ public sealed class CompatibilityRobotSettings
JerkLimitScale = jerkLimitScale;
AdaptIcspTryNum = adaptIcspTryNum;
PlanningSpeedScale = planningSpeedScale;
SpeedRatio = speedRatio;
SmoothStartStopTiming = smoothStartStopTiming;
}
@@ -109,6 +116,11 @@ public sealed class CompatibilityRobotSettings
/// </summary>
public double PlanningSpeedScale { get; }
/// <summary>
/// 获取运行时默认速度倍率,会在 SetUpRobot 时同步到控制器运行时,供 MoveJoint 和后续执行链路使用。
/// </summary>
public double SpeedRatio { get; }
/// <summary>
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
/// </summary>
@@ -202,6 +214,7 @@ public sealed class RobotConfigLoader
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),
speedRatio: ReadDouble(robotElement, "speed_ratio", defaultValue: 1.0),
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
@@ -213,8 +226,8 @@ public sealed class RobotConfigLoader
}
_logger?.LogInformation(
"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);
"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.SpeedRatio, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
return new LoadedRobotConfig(
sourcePath: resolvedConfigPath,

View File

@@ -13,9 +13,9 @@ public sealed class FlyshotPreparedExecution
/// <param name="timingRows">与最终发送点列一致的时间映射诊断行。</param>
/// <param name="jerkRows">与最终发送点列一致的跃度诊断行。</param>
/// <param name="requestSpeedRatio">请求的执行倍率。</param>
/// <param name="finalSpeedRatio">通过离散校验后实际采用的保守倍率。</param>
/// <param name="finalSpeedRatio">最终发送队列采用的采样倍率;请求倍率已折算进规划时为 1。</param>
/// <param name="finalDurationSeconds">最终发送总时长,单位为秒。</param>
/// <param name="stretchIterationCount">自动拉长执行时长的迭代次数。</param>
/// <param name="stretchIterationCount">历史诊断字段;当前执行侧不自动改写 speed_ratio,因此正常为 0。</param>
public FlyshotPreparedExecution(
IEnumerable<FlyshotPreparedSample> samples,
IEnumerable<FlyshotPreparedTriggerBinding> triggerBindings,
@@ -48,7 +48,7 @@ public sealed class FlyshotPreparedExecution
if (stretchIterationCount < 0)
{
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "拉长迭代次数必须是非负整数。");
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写迭代次数必须是非负整数。");
}
Samples = samples.Select(static sample => sample).ToArray();
@@ -87,7 +87,7 @@ public sealed class FlyshotPreparedExecution
public double RequestSpeedRatio { get; }
/// <summary>
/// 获取通过离散校验后实际采用的保守倍率。
/// 获取最终发送队列采用的采样倍率;请求倍率保留在 <see cref="RequestSpeedRatio"/> 中用于诊断。
/// </summary>
public double FinalSpeedRatio { get; }
@@ -97,7 +97,7 @@ public sealed class FlyshotPreparedExecution
public double FinalDurationSeconds { get; }
/// <summary>
/// 获取自动拉长执行时长的迭代次数。
/// 获取历史诊断字段;当前执行侧不自动改写 speed_ratio,因此正常为 0。
/// </summary>
public int StretchIterationCount { get; }
}
@@ -113,7 +113,7 @@ public sealed class FlyshotPreparedSample
/// <param name="sampleIndex">发送周期序号。</param>
/// <param name="sendTime">物理发送时间,单位为秒。</param>
/// <param name="trajectoryTime">回映射到规划轨迹的采样时间,单位为秒。</param>
/// <param name="speedRatio">生成该发送点时采用的执行倍率。</param>
/// <param name="speedRatio">生成该发送点时采用的队列采样倍率,已规划调速路径通常为 1。</param>
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
public FlyshotPreparedSample(
long sampleIndex,
@@ -146,7 +146,7 @@ public sealed class FlyshotPreparedSample
public double TrajectoryTime { get; }
/// <summary>
/// 获取生成该发送点时采用的执行倍率。
/// 获取生成该发送点时采用的队列采样倍率。
/// </summary>
public double SpeedRatio { get; }

View File

@@ -24,7 +24,8 @@ public sealed class TrajectoryResult
int plannedWaypointCount,
int triggerSampleIndexOffsetCycles = 0,
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null,
FlyshotPreparedExecution? preparedFlyshotExecution = null)
FlyshotPreparedExecution? preparedFlyshotExecution = null,
IEnumerable<IReadOnlyList<double>>? denseCartesianTrajectory = null)
{
if (string.IsNullOrWhiteSpace(programName))
{
@@ -60,6 +61,7 @@ public sealed class TrajectoryResult
var copiedTriggerTimeline = triggerTimeline.ToArray();
var copiedArtifacts = artifacts.ToArray();
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
var copiedDenseCartesianTrajectory = denseCartesianTrajectory?.Select(static row => row.ToArray()).ToArray();
ProgramName = programName;
Method = method;
@@ -74,6 +76,7 @@ public sealed class TrajectoryResult
PlannedWaypointCount = plannedWaypointCount;
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
DenseJointTrajectory = copiedDenseJointTrajectory;
DenseCartesianTrajectory = copiedDenseCartesianTrajectory;
PreparedFlyshotExecution = preparedFlyshotExecution;
}
@@ -156,6 +159,13 @@ public sealed class TrajectoryResult
[JsonPropertyName("denseJointTrajectory")]
public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; }
/// <summary>
/// 获取稠密直角坐标轨迹采样点,每行格式为 [time, x, y, z, w, p, r]。
/// Null 表示本结果不是直角流式运动。
/// </summary>
[JsonPropertyName("denseCartesianTrajectory")]
public IReadOnlyList<IReadOnlyList<double>>? DenseCartesianTrajectory { get; }
/// <summary>
/// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence.
/// </summary>

View File

@@ -3,42 +3,32 @@ using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列,并在必要时自动拉长执行时长直到通过离散限幅校验。
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列,并执行发送口径的离散限幅校验。
/// </summary>
public static class FlyshotExecutionSendSequenceBuilder
{
/// <summary>
/// 自动拉长时每轮采用的保守倍率缩减系数。
/// </summary>
private const double StretchFactor = 0.95;
/// <summary>
/// 自动拉长尝试的最大迭代次数。
/// </summary>
private const int MaxStretchIterations = 16;
/// <summary>
/// 根据规划层稠密轨迹和执行层 speedRatio 构建最终发送队列。
/// 根据连续规划轨迹构建最终发送队列;请求 speedRatio 只作为诊断信息保留。
/// </summary>
/// <param name="robot">机器人关节限值配置。</param>
/// <param name="plannedTrajectory">规划阶段产出的连续轨迹。</param>
/// <param name="result">规划结果。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
/// <param name="requestedSpeedRatio">请求的执行倍率。</param>
/// <param name="requestedSpeedRatio">请求的执行倍率,必须已经在规划阶段折算进轨迹。</param>
/// <param name="samplingSpeedRatio">最终队列采样倍率;为空时使用 1.0,表示直接采样已调速后的规划轨迹。</param>
/// <returns>通过离散校验后的飞拍最终发送结果。</returns>
public static FlyshotPreparedExecution Build(
RobotProfile robot,
PlannedTrajectory plannedTrajectory,
TrajectoryResult result,
double servoPeriodSeconds,
double requestedSpeedRatio)
double requestedSpeedRatio,
double? samplingSpeedRatio = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(plannedTrajectory);
ArgumentNullException.ThrowIfNull(result);
if (result.DenseJointTrajectory is null)
{
throw new InvalidOperationException("飞拍执行准备前必须先生成规划层稠密关节轨迹。");
}
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。");
@@ -49,47 +39,40 @@ public static class FlyshotExecutionSendSequenceBuilder
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。");
}
var effectiveSpeedRatio = requestedSpeedRatio;
InvalidOperationException? firstFailure = null;
for (var iteration = 0; iteration <= MaxStretchIterations; iteration++)
var finalSpeedRatio = samplingSpeedRatio ?? 1.0;
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
{
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory,
result.Duration.TotalSeconds,
servoPeriodSeconds,
effectiveSpeedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName);
return BuildPreparedExecution(
result,
samples,
requestedSpeedRatio,
effectiveSpeedRatio,
iteration);
}
catch (InvalidOperationException exception)
{
firstFailure ??= exception;
if (iteration >= MaxStretchIterations)
{
throw new InvalidOperationException(
$"飞拍最终发送队列离散限幅校验失败,已达到最大自动拉长次数 {MaxStretchIterations}。",
firstFailure);
}
// 只在执行侧进一步保守化 trajectoryTime(sendTime) 映射,不回写规划层轨迹。
effectiveSpeedRatio *= StretchFactor;
}
throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。");
}
throw new InvalidOperationException("飞拍最终发送队列构建失败。", firstFailure);
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
plannedTrajectory,
servoPeriodSeconds,
finalSpeedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: result.ProgramName,
validateJerk: false,
strictJerkTolerance: true);
}
catch (InvalidOperationException exception)
{
throw new InvalidOperationException(
$"飞拍最终发送队列在请求 speed_ratio={requestedSpeedRatio:F6} 下离散限幅校验失败。" +
"请降低 planning_speed_scale 或 speed_ratio 后重新规划;执行侧不会在发送阶段临场重采样。",
exception);
}
return BuildPreparedExecution(
result,
samples,
requestedSpeedRatio,
finalSpeedRatio,
stretchIterationCount: 0);
}
/// <summary>
@@ -121,7 +104,9 @@ public static class FlyshotExecutionSendSequenceBuilder
binding.SampleIndex,
binding.FoundInWindow))
.ToArray();
var timingRows = samples.Select(J519SendTrajectorySampler.BuildTimingRow).ToArray();
var timingRows = samples
.Select(sample => J519SendTrajectorySampler.BuildTimingRow(sample, requestedSpeedRatio, stretchIterationCount))
.ToArray();
var jerkRows = BuildJerkRows(samples);
var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;

View File

@@ -1,242 +1,352 @@
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;
}
}
using Flyshot.Core.Planning;
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>
/// 飞拍最终发送队列优先从 <see cref="PlannedTrajectory"/> 重建连续三次样条并直接取点;
/// 旧稠密点入口仅保留给运行时 fallback 和兼容导出。
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod;
/// speedRatio 不改变物理发送周期,只用于把发送时间映射回规划轨迹时间:
/// trajectoryTime = min(sendTime * speedRatio, duration)。
/// 之后在连续样条或旧稠密轨迹上求取关节,并把每个关节从 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 伺服周期生成完整实发采样序列。
/// </summary>
/// <param name="trajectory">规划阶段产出的连续轨迹信息。</param>
/// <param name="servoPeriodSeconds">J519 物理发送周期,单位为秒。</param>
/// <param name="speedRatio">速度倍率;只缩放轨迹采样时间,不改变物理发送周期。</param>
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
public static IReadOnlyList<J519SendSample> SamplePlannedTrajectory(
PlannedTrajectory trajectory,
double servoPeriodSeconds,
double speedRatio)
{
ArgumentNullException.ThrowIfNull(trajectory);
ValidatePlannedTrajectoryInputs(trajectory, servoPeriodSeconds, speedRatio);
var durationSeconds = trajectory.WaypointTimes[^1];
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
var spline = RebuildSpline(trajectory);
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
{
// 直接在连续样条上按 trajectoryTime 取点,避免先离散再线性回采样引入局部跃度尖峰。
var sendTime = sampleIndex * servoPeriodSeconds;
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
var joints = spline.Evaluate(trajectoryTime).Select(RadiansToDegrees).ToArray();
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
}
return samples;
}
/// <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 + final_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="sample">待写出的 J519 实发采样点。</param>
/// <param name="requestedSpeedRatio">用户请求的执行倍率。</param>
/// <param name="stretchIterationCount">历史诊断字段;当前执行侧不自动改写 speed_ratio,因此正常为 0。</param>
/// <returns>前 4 列兼容 ActualSendTiming.txt,后续列记录请求倍率和倍率改写次数。</returns>
public static IReadOnlyList<double> BuildTimingRow(
J519SendSample sample,
double requestedSpeedRatio,
int stretchIterationCount)
{
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
{
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "请求 speed_ratio 必须是有限正数。");
}
if (stretchIterationCount < 0)
{
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写次数不能为负数。");
}
var legacyRow = BuildTimingRow(sample);
return
[
legacyRow[0],
legacyRow[1],
legacyRow[2],
legacyRow[3],
Math.Round(requestedSpeedRatio, 6),
stretchIterationCount
];
}
/// <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>
/// 校验连续规划轨迹采样入口的基础输入,避免异常时间轴进入最终发送队列。
/// </summary>
private static void ValidatePlannedTrajectoryInputs(
PlannedTrajectory trajectory,
double servoPeriodSeconds,
double speedRatio)
{
if (trajectory.WaypointTimes.Count < 2)
{
throw new InvalidOperationException("规划轨迹至少需要两个路点才能重建连续样条。");
}
if (trajectory.WaypointTimes[^1] < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(trajectory), "规划轨迹总时长不能为负数。");
}
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>
/// 从规划轨迹重建连续三次样条,供飞拍最终发送队列直接采样。
/// </summary>
private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
{
var times = trajectory.WaypointTimes.ToArray();
var values = trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray();
return new CubicSplineInterpolator(times, values);
}
/// <summary>
/// 角度单位转换:rad -> deg。
/// </summary>
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}

View File

@@ -19,16 +19,18 @@ public static class TrajectoryLimitValidator
/// <param name="rows">稠密轨迹行,格式为 time + 关节弧度。</param>
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
/// <param name="trajectoryName">诊断用轨迹名称。</param>
/// <param name="validateJerk">是否校验离散 jerk;飞拍链路可临时关闭,仅保留速度/加速度约束。</param>
public static void ValidateDenseJointTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> rows,
double toleranceMultiplier = DefaultLimitTolerance,
string? trajectoryName = null)
string? trajectoryName = null,
bool validateJerk = true)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(rows);
ValidateTolerance(toleranceMultiplier);
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory");
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory", validateJerk);
}
/// <summary>
@@ -38,11 +40,15 @@ public static class TrajectoryLimitValidator
/// <param name="samples">J519 发送采样点,关节单位为角度。</param>
/// <param name="toleranceMultiplier">限值容差倍率,用于过滤浮点舍入误差。</param>
/// <param name="trajectoryName">诊断用轨迹名称。</param>
/// <param name="validateJerk">是否校验离散 jerk;飞拍链路可临时关闭,仅保留速度/加速度约束。</param>
/// <param name="strictJerkTolerance">是否按原始 jerk 限值做硬约束;false 时保留历史宽容口径。</param>
public static void ValidateJ519SendSamples(
RobotProfile robot,
IReadOnlyList<J519SendSample> samples,
double toleranceMultiplier = DefaultLimitTolerance,
string? trajectoryName = null)
string? trajectoryName = null,
bool validateJerk = false,
bool strictJerkTolerance = true)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(samples);
@@ -61,7 +67,13 @@ public static class TrajectoryLimitValidator
rows.Add(row);
}
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "j519-send-trajectory");
ValidateRows(
robot,
rows,
toleranceMultiplier,
trajectoryName ?? "j519-send-trajectory",
validateJerk,
strictJerkTolerance);
}
/// <summary>
@@ -82,7 +94,9 @@ public static class TrajectoryLimitValidator
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> rows,
double toleranceMultiplier,
string trajectoryName)
string trajectoryName,
bool validateJerk,
bool strictJerkTolerance = false)
{
double? previousTime = null;
double[]? previousPositions = null;
@@ -145,7 +159,7 @@ public static class TrajectoryLimitValidator
jointLimit.AccelerationLimit,
toleranceMultiplier);
if (previousAccelerations is not null)
if (validateJerk && previousAccelerations is not null)
{
var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
ThrowIfExceeded(
@@ -157,7 +171,7 @@ public static class TrajectoryLimitValidator
"Jerk",
jerk,
jointLimit.JerkLimit,
toleranceMultiplier*4);
strictJerkTolerance ? toleranceMultiplier : toleranceMultiplier * 4.0);
}
}

View File

@@ -113,4 +113,11 @@ public interface IControllerRuntime
/// <param name="result">规划结果。</param>
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
/// <summary>
/// 执行一条已经完成规划的直角坐标轨迹,并更新最终 TCP 位姿。
/// </summary>
/// <param name="result">规划结果,必须包含稠密直角坐标轨迹。</param>
/// <param name="finalPose">轨迹执行结束后的 [x,y,z,w,p,r] 位姿。</param>
void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose);
}

View File

@@ -487,11 +487,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
if (!IsSimulationMode && result.DenseJointTrajectory is not null)
{
if (_speedRatio <= 0.0)
{
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
}
EnsureJ519ReadyForDenseExecution();
// 真机模式且存在稠密路点:准备可被 StopMove 取消的同步发送任务。
@@ -543,6 +538,78 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
ArgumentNullException.ThrowIfNull(result);
ArgumentNullException.ThrowIfNull(finalPose);
CancellationToken denseSendCancellationToken = default;
CancellationTokenSource? denseSendCancellationSource = null;
var shouldRunDenseTrajectory = false;
_logger?.LogInformation(
"ExecuteCartesianTrajectory 开始: program={ProgramName}, 时长={Duration}s, 稠密采样={HasDense}, speedRatio={SpeedRatio}",
result.ProgramName, result.Duration.TotalSeconds, result.DenseCartesianTrajectory is not null, _speedRatio);
lock (_stateLock)
{
EnsureEnabled();
EnsureValidTrajectory(result);
EnsurePoseCount(finalPose.Count);
CancelSendTaskLocked();
if (!IsSimulationMode && result.DenseCartesianTrajectory is not null)
{
EnsureJ519ReadyForDenseExecution();
// 真机直角模式同样预装完整队列,由机器人状态包节拍驱动出队。
_isInMotion = true;
_sendCts = new CancellationTokenSource();
denseSendCancellationSource = _sendCts;
denseSendCancellationToken = _sendCts.Token;
shouldRunDenseTrajectory = true;
_logger?.LogInformation("ExecuteCartesianTrajectory 开始同步直角稠密发送任务");
}
else
{
if (!IsSimulationMode)
{
var command = new FanucJ519Command(
sequence: 0,
targetValues: BuildCartesianTargetValues(finalPose),
dataStyle: 0);
_j519Client.UpdateCommand(command);
}
_isInMotion = true;
_pose = MergeFinalCartesianPose(finalPose);
_isInMotion = false;
_logger?.LogInformation("ExecuteCartesianTrajectory 完成(单点模式)");
}
}
if (shouldRunDenseTrajectory)
{
try
{
SendDenseCartesianTrajectory(result, finalPose, denseSendCancellationToken);
_logger?.LogInformation("ExecuteCartesianTrajectory 完成(稠密模式)");
}
finally
{
lock (_stateLock)
{
if (ReferenceEquals(_sendCts, denseSendCancellationSource))
{
denseSendCancellationSource?.Dispose();
_sendCts = null;
}
}
}
}
}
/// <summary>
/// 释放运行时持有的所有 Socket 客户端。
/// </summary>
@@ -562,13 +629,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
/// <summary>
/// 稠密轨迹发送任务:预生成完整 J519 命令队列,并等待机器人状态包按 speed_ratio 推进到执行完成。
/// 稠密轨迹发送任务:预生成完整 J519 命令队列,并等待机器人状态包驱动队列执行完成。
/// </summary>
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
{
var triggers = result.TriggerTimeline;
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
var speedRatio = _speedRatio;
var preparedExecution = result.PreparedFlyshotExecution;
var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds;
var samples = preparedExecution is null
@@ -576,7 +642,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
result.DenseJointTrajectory!,
durationSeconds,
servoPeriodSeconds,
speedRatio)
speedRatio: 1.0)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
@@ -590,8 +656,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
var sampleCount = samples.Count;
_logger?.LogInformation(
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, runtimeSpeedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
result.ProgramName, sampleCount, durationSeconds, _speedRatio, servoPeriodSeconds * 1000, triggers.Count);
var triggerBindings = preparedExecution is null
? TriggerSampleBinder.Bind(
@@ -627,6 +693,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
var preparedJerkRows = preparedExecution?.JerkRows;
var diagnosticRequestedSpeedRatio = _speedRatio > 0.0 && !double.IsNaN(_speedRatio) && !double.IsInfinity(_speedRatio)
? _speedRatio
: 1.0;
try
{
@@ -687,7 +756,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
commands.Add(command);
sentJointRows.Add(BuildDenseSendJointRow(sample.SendTime, sample.JointsDegrees, ioMask, ioValue));
sentTimingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
sentTimingRows.Add(preparedExecution is null
? J519SendTrajectorySampler.BuildTimingRow(sample, diagnosticRequestedSpeedRatio, stretchIterationCount: 0)
: J519SendTrajectorySampler.BuildTimingRow(
sample,
preparedExecution.RequestSpeedRatio,
preparedExecution.StretchIterationCount));
if (preparedJerkRows is not null)
{
@@ -757,6 +831,71 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 直角稠密轨迹发送任务:预生成 Data format=0 的 J519 命令队列,并等待状态包驱动执行完成。
/// </summary>
private void SendDenseCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose, CancellationToken cancellationToken)
{
var rows = result.DenseCartesianTrajectory ?? throw new InvalidOperationException("Cartesian trajectory requires dense Cartesian samples.");
var commands = new List<FanucJ519Command>(rows.Count);
var sentPoseRows = new List<IReadOnlyList<double>>(rows.Count);
var outputDir = CreateDenseSendOutputDirectory(result.ProgramName);
var stopwatch = Stopwatch.StartNew();
try
{
foreach (var row in rows)
{
cancellationToken.ThrowIfCancellationRequested();
if (row.Count != 7)
{
throw new InvalidOperationException("Cartesian dense trajectory rows must contain time plus six pose values.");
}
var pose = row.Skip(1).Take(6).ToArray();
var command = new FanucJ519Command(
sequence: 0,
targetValues: BuildCartesianTargetValues(pose),
dataStyle: 0,
writeIoType: 2,
writeIoIndex: 1,
writeIoMask: 0,
writeIoValue: 0);
commands.Add(command);
sentPoseRows.Add(BuildDenseSendPoseRow(row[0], pose));
}
TryWriteDenseCartesianSendArtifacts(outputDir, sentPoseRows);
_j519Client.LoadCommandQueue(commands);
if (_j519Client.IsConnected)
{
_j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult();
}
_logger?.LogInformation(
"SendDenseCartesianTrajectory 正常完成: 采样数={SampleCount}, 队列装载耗时={ElapsedMs}ms",
commands.Count,
stopwatch.ElapsedMilliseconds);
}
catch (OperationCanceledException)
{
_logger?.LogWarning(
"SendDenseCartesianTrajectory 被取消: 已生成 {Current}/{Total} 条命令",
commands.Count,
rows.Count);
}
finally
{
lock (_stateLock)
{
_isInMotion = false;
_pose = MergeFinalCartesianPose(finalPose);
}
}
}
/// <summary>
/// 若已有 J519 响应,则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
/// </summary>
@@ -778,7 +917,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
/// <param name="getLatestResponse">读取最新 J519 状态的委托。</param>
/// <param name="retryEnableRobot">状态未就绪时触发一次 EnableRobot 重试的委托。</param>
/// <param name="waitAfterRetry">重试后等待状态刷新的委托。</param>
internal static void EnsureJ519ReadyForDenseExecutionCore(
internal void EnsureJ519ReadyForDenseExecutionCore(
Func<FanucJ519Response?> getLatestResponse,
Action retryEnableRobot,
Action waitAfterRetry)
@@ -788,11 +927,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
ArgumentNullException.ThrowIfNull(waitAfterRetry);
var response = getLatestResponse();
if (response is null || IsJ519ReadyForDenseExecution(response))
{
return;
}
_logger?.LogInformation("当前机器人不满足执行条件,重新启用机器人");
try
{
retryEnableRobot();
@@ -876,6 +1016,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
return row;
}
/// <summary>
/// 构造实际发送直角位姿文本行,格式为 send_time + X/Y/Z/W/P/R。
/// </summary>
private static IReadOnlyList<double> BuildDenseSendPoseRow(double sendTime, IReadOnlyList<double> pose)
{
var row = new double[pose.Count + 1];
row[0] = Math.Round(sendTime, 6);
for (var index = 0; index < pose.Count; index++)
{
row[index + 1] = Math.Round(pose[index], 6);
}
return row;
}
/// <summary>
/// 构造 J519 直角命令的 9 个目标槽位,当前现场无扩展轴时 E1/E2/E3 补零。
/// </summary>
private static double[] BuildCartesianTargetValues(IReadOnlyList<double> pose)
{
EnsurePoseCount(pose.Count);
return [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], 0.0, 0.0, 0.0];
}
/// <summary>
/// 将六维 WPR 直角目标合并回运行时缓存,保留旧 GetPose 仿真路径的 7 维外形。
/// </summary>
private double[] MergeFinalCartesianPose(IReadOnlyList<double> pose)
{
EnsurePoseCount(pose.Count);
return _pose.Length >= 7
? [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], _pose[6]]
: pose.ToArray();
}
/// <summary>
/// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件;若落盘失败,只记录日志,不影响运动主流程。
/// </summary>
@@ -907,6 +1082,25 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 尝试把直角实际发送点位写入文本文件;若落盘失败,只记录日志,不影响运动主流程。
/// </summary>
private void TryWriteDenseCartesianSendArtifacts(string outputDir, IReadOnlyList<IReadOnlyList<double>> sentPoseRows)
{
try
{
WriteDenseRows(Path.Combine(outputDir, "ActualSendPoseTraj.txt"), sentPoseRows);
_logger?.LogInformation(
"SendDenseCartesianTrajectory 已写出实际发送记录: outputDir={OutputDir}, poseRows={PoseRows}",
outputDir,
sentPoseRows.Count);
}
catch (Exception exception)
{
_logger?.LogWarning(exception, "SendDenseCartesianTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir);
}
}
/// <summary>
/// 以旧轨迹文本兼容的空格分隔格式写出数值行。
/// </summary>
@@ -1086,6 +1280,17 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
}
}
/// <summary>
/// 校验直角位姿固定为 [x,y,z,w,p,r] 六维。
/// </summary>
private static void EnsurePoseCount(int poseCount)
{
if (poseCount != 6)
{
throw new InvalidOperationException($"Expected 6 Cartesian pose values but received {poseCount}.");
}
}
/// <summary>
/// 校验机器人已经完成初始化。
/// </summary>

View File

@@ -7,7 +7,7 @@ namespace Flyshot.Runtime.Fanuc.Protocol;
/// </summary>
public sealed class FanucJ519Command
{
private readonly double[] _targetJoints;
private readonly double[] _targetValues;
/// <summary>
/// 初始化 J519 命令数据。
@@ -35,11 +35,53 @@ public sealed class FanucJ519Command
ushort writeIoIndex = 1,
ushort writeIoMask = 0,
ushort writeIoValue = 0)
: this(
sequence,
targetValues: targetJoints,
lastData,
readIoType,
readIoIndex,
readIoMask,
dataStyle,
writeIoType,
writeIoIndex,
writeIoMask,
writeIoValue)
{
ArgumentNullException.ThrowIfNull(targetJoints);
if (targetJoints.Count is <= 0 or > 9)
}
/// <summary>
/// 初始化 J519 命令数据。
/// </summary>
/// <param name="sequence">命令序号。</param>
/// <param name="targetValues">J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3,关节模式为 J1..J9。</param>
/// <param name="lastData">是否为最后一帧数据。</param>
/// <param name="readIoType">读取 IO 类型。</param>
/// <param name="readIoIndex">读取 IO 起始索引。</param>
/// <param name="readIoMask">读取 IO 掩码。</param>
/// <param name="dataStyle">目标数据类型,0 为直角坐标,1 为关节坐标。</param>
/// <param name="writeIoType">写入 IO 类型。</param>
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
/// <param name="writeIoMask">写入 IO 掩码。</param>
/// <param name="writeIoValue">写入 IO 数值。</param>
public FanucJ519Command(
uint sequence,
IEnumerable<double> targetValues,
byte lastData = 0,
byte readIoType = 2,
ushort readIoIndex = 1,
ushort readIoMask = 255,
byte dataStyle = 1,
byte writeIoType = 2,
ushort writeIoIndex = 1,
ushort writeIoMask = 0,
ushort writeIoValue = 0)
{
ArgumentNullException.ThrowIfNull(targetValues);
var copiedTargetValues = targetValues.ToArray();
if (copiedTargetValues.Length is <= 0 or > 9)
{
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
throw new ArgumentOutOfRangeException(nameof(targetValues), "J519 目标数据必须包含 1 到 9 个槽位。");
}
Sequence = sequence;
@@ -52,7 +94,7 @@ public sealed class FanucJ519Command
WriteIoIndex = writeIoIndex;
WriteIoMask = writeIoMask;
WriteIoValue = writeIoValue;
_targetJoints = targetJoints.ToArray();
_targetValues = copiedTargetValues;
}
/// <summary>
@@ -108,7 +150,12 @@ public sealed class FanucJ519Command
/// <summary>
/// 获取目标关节或扩展轴数据。
/// </summary>
public IReadOnlyList<double> TargetJoints => _targetJoints;
public IReadOnlyList<double> TargetJoints => _targetValues;
/// <summary>
/// 获取 J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3,关节模式为 J1..J9。
/// </summary>
public IReadOnlyList<double> TargetValues => _targetValues;
}
/// <summary>

View File

@@ -431,6 +431,32 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 以直角坐标 `[x,y,z,w,p,r]` 执行点到点移动。
/// </summary>
/// <param name="pose_data">直角位姿请求体。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/move_pose/")]
public IActionResult MovePose([FromBody] JsonElement pose_data)
{
try
{
var poseRequest = LegacyCartesianPoseRequest.FromJson(pose_data);
var pose = poseRequest.ToPoseArray();
_logger.LogInformation("MovePose 调用: x={X}, y={Y}, z={Z}, w={W}, p={P}, r={R}",
poseRequest.x, poseRequest.y, poseRequest.z, poseRequest.w, poseRequest.p, poseRequest.r);
_compatService.MovePose(pose);
_logger.LogInformation("MovePose 成功");
return Ok(new { status = "robot moved" });
}
catch (Exception exception)
{
_logger.LogError(exception, "MovePose 失败");
return LegacyBadRequest("MovePose failed");
}
}
/// <summary>
/// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。
/// </summary>
@@ -874,6 +900,136 @@ public sealed class LegacyJointPositionRequest
public List<double> joints { get; init; } = [];
}
/// <summary>
/// 表示 `/move_pose/` 路由使用的直角位姿请求体。
/// </summary>
public sealed class LegacyCartesianPoseRequest
{
/// <summary>
/// MovePose 第一版入口硬限制:TCP X/Y 最大绝对值,单位 mm。
/// </summary>
private const double MaxHorizontalMillimeters = 1000.0;
/// <summary>
/// MovePose 第一版入口硬限制:TCP Z 最小值,单位 mm。
/// </summary>
private const double MinZMillimeters = 0.0;
/// <summary>
/// MovePose 第一版入口硬限制:TCP Z 最大值,单位 mm。
/// </summary>
private const double MaxZMillimeters = 1200.0;
/// <summary>
/// MovePose 第一版入口硬限制:W/R 姿态角最大绝对值,单位 deg。
/// </summary>
private const double MaxRollYawDegrees = 180.0;
/// <summary>
/// MovePose 第一版入口硬限制:P 姿态角最大绝对值,单位 deg。
/// </summary>
private const double MaxPitchDegrees = 90.0;
/// <summary>
/// 获取或设置 TCP X,单位为 mm。
/// </summary>
public double x { get; init; }
/// <summary>
/// 获取或设置 TCP Y,单位为 mm。
/// </summary>
public double y { get; init; }
/// <summary>
/// 获取或设置 TCP Z,单位为 mm。
/// </summary>
public double z { get; init; }
/// <summary>
/// 获取或设置姿态 W,单位为 deg。
/// </summary>
public double w { get; init; }
/// <summary>
/// 获取或设置姿态 P,单位为 deg。
/// </summary>
public double p { get; init; }
/// <summary>
/// 获取或设置姿态 R,单位为 deg。
/// </summary>
public double r { get; init; }
/// <summary>
/// 从原始 JSON 请求体解析直角位姿,并显式拒绝缺字段、null 和非有限数。
/// </summary>
/// <param name="json">原始 JSON 请求体。</param>
/// <returns>解析后的直角位姿请求。</returns>
public static LegacyCartesianPoseRequest FromJson(JsonElement json)
{
if (json.ValueKind != JsonValueKind.Object)
{
throw new ArgumentException("MovePose request body must be an object.");
}
// 旧接口要求请求体必须完整提供 x/y/z/w/p/r,不能让模型绑定把缺字段静默补 0。
return new LegacyCartesianPoseRequest
{
x = ReadRequiredFiniteDouble(json, "x", -MaxHorizontalMillimeters, MaxHorizontalMillimeters),
y = ReadRequiredFiniteDouble(json, "y", -MaxHorizontalMillimeters, MaxHorizontalMillimeters),
z = ReadRequiredFiniteDouble(json, "z", MinZMillimeters, MaxZMillimeters),
w = ReadRequiredFiniteDouble(json, "w", -MaxRollYawDegrees, MaxRollYawDegrees),
p = ReadRequiredFiniteDouble(json, "p", -MaxPitchDegrees, MaxPitchDegrees),
r = ReadRequiredFiniteDouble(json, "r", -MaxRollYawDegrees, MaxRollYawDegrees)
};
}
/// <summary>
/// 转换为兼容层使用的六维位姿数组。
/// </summary>
/// <returns>[x,y,z,w,p,r] 数组。</returns>
public IReadOnlyList<double> ToPoseArray()
{
return [x, y, z, w, p, r];
}
/// <summary>
/// 读取必填有限数值字段。
/// </summary>
/// <param name="json">请求体 JSON 对象。</param>
/// <param name="propertyName">字段名。</param>
/// <param name="minInclusive">允许的最小值。</param>
/// <param name="maxInclusive">允许的最大值。</param>
/// <returns>字段对应的有限 double 数值。</returns>
private static double ReadRequiredFiniteDouble(
JsonElement json,
string propertyName,
double minInclusive,
double maxInclusive)
{
if (!json.TryGetProperty(propertyName, out var property) || property.ValueKind != JsonValueKind.Number)
{
throw new ArgumentException($"MovePose request field '{propertyName}' is required and must be a number.");
}
var value = property.GetDouble();
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(propertyName, "MovePose request values must be finite.");
}
if (value < minInclusive || value > maxInclusive)
{
throw new ArgumentOutOfRangeException(
propertyName,
value,
$"MovePose request field '{propertyName}' must be between {minInclusive} and {maxInclusive}.");
}
return value;
}
}
/// <summary>
/// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。
/// </summary>

View File

@@ -49,23 +49,18 @@ public sealed class StatusController : ControllerBase
public IActionResult GetSnapshot()
{
var snapshot = _compatService.GetControllerSnapshot();
var isSetup = _compatService.IsSetUp;
// 状态页需要在机器人未初始化时仍能打开,因此只有初始化后才读取机器人元数据。
var robotName = isSetup ? _compatService.GetRobotName() : null;
var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0;
var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty<string>();
var metadata = _compatService.GetStatusSnapshotMetadata();
return Ok(new
{
Status = "ok",
Service = "flyshot-server-host",
ServerVersion = _compatService.GetServerVersion(),
ClientVersion = _compatService.GetClientVersion(),
IsSetup = isSetup,
RobotName = robotName,
DegreesOfFreedom = degreesOfFreedom,
UploadedTrajectories = uploadedTrajectories,
ServerVersion = metadata.ServerVersion,
ClientVersion = metadata.ClientVersion,
IsSetup = metadata.IsSetup,
RobotName = metadata.RobotName,
DegreesOfFreedom = metadata.DegreesOfFreedom,
UploadedTrajectories = metadata.UploadedTrajectories,
Snapshot = snapshot
});
}

View File

@@ -16,6 +16,17 @@ const state = {
history: []
};
const requestBodySamples = {
"POST /move_pose/": {
x: 100.0,
y: 200.0,
z: 300.0,
w: 0.0,
p: 45.0,
r: 0.0
}
};
/** 简单的 escape:把任意字符串安全嵌入 textContent 之外的位置时使用。 */
function escapeHtml(value) {
return String(value).replace(/[&<>"']/g, function (ch) {
@@ -137,6 +148,11 @@ function storageKey(op) {
return STORAGE_PREFIX + op.method + ":" + op.path;
}
/** 返回指定端点的手工调试样例,处理 JsonElement 等 OpenAPI 无法自动推断字段的接口。 */
function getRequestBodySample(op) {
return requestBodySamples[op.method + " " + op.path] || null;
}
/** 读取本端点最近一次输入;解析失败则当作空。 */
function loadInputs(op) {
try {
@@ -281,7 +297,7 @@ function renderBodyEditor(container, op, savedBody) {
if (savedBody !== undefined && savedBody !== null) {
initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2);
} else {
const sample = buildSampleFromSchema(op.bodySchema, 0);
const sample = getRequestBodySample(op) || buildSampleFromSchema(op.bodySchema, 0);
initialText = sample === null ? "" : JSON.stringify(sample, null, 2);
}
textarea.value = initialText;

View File

@@ -6,6 +6,9 @@
--text: #172033;
--muted: #5b667a;
--accent: #007c89;
--accent-dark: #005d67;
--steel: #2f3b4f;
--steel-soft: #e7ebf1;
--good: #12805c;
--warn: #b7791f;
--bad: #b42318;
@@ -188,6 +191,289 @@ dd {
font-family: inherit;
}
.jog-panel {
grid-column: 1 / -1;
overflow: hidden;
}
.jog-toolbar {
display: grid;
grid-template-columns: repeat(2, minmax(180px, 1fr)) minmax(220px, 1fr);
gap: 12px;
padding: 16px;
border-bottom: 1px solid var(--line);
background: #fbfcfe;
}
.step-field {
display: grid;
gap: 6px;
color: var(--muted);
font-size: 13px;
}
.step-field input {
min-height: 38px;
width: 100%;
padding: 0 10px;
border: 1px solid var(--line);
border-radius: 6px;
background: #ffffff;
color: var(--text);
font: inherit;
}
.jog-body {
display: grid;
grid-template-columns: minmax(0, 1fr) minmax(0, 1fr);
gap: 18px;
padding: 16px;
}
.stick-card {
min-width: 0;
border: 1px solid #ccd4df;
border-radius: 8px;
background: linear-gradient(180deg, #ffffff 0%, #f3f6fa 100%);
box-shadow: inset 0 1px 0 rgba(255, 255, 255, 0.9);
}
.stick-head {
display: flex;
align-items: center;
justify-content: space-between;
gap: 12px;
padding: 14px 16px 0;
}
.stick-head h3 {
margin: 0;
font-size: 16px;
font-weight: 650;
}
.stick-head span {
color: var(--muted);
font-family: Consolas, "Cascadia Mono", monospace;
font-size: 13px;
}
.stick-console {
display: grid;
grid-template-columns: 72px 1fr 72px;
grid-template-rows: 72px 72px 72px;
gap: 8px;
width: min(100%, 344px);
margin: 18px auto 16px;
padding: 14px;
border: 1px solid #c2cad7;
border-radius: 999px;
background:
radial-gradient(circle at 50% 50%, #ffffff 0 26%, #e6ebf2 27% 48%, #cdd5e0 49% 50%, #edf1f6 51% 100%);
}
.jog-button {
min-width: 44px;
min-height: 44px;
color: var(--steel);
font-weight: 650;
touch-action: none;
user-select: none;
}
.axis-button,
.mini-button {
border: 1px solid #99a8ba;
border-radius: 999px;
background: linear-gradient(180deg, #ffffff 0%, #e5ebf2 100%);
box-shadow: 0 2px 0 #b7c1ce;
}
.axis-button {
min-width: 0;
min-height: 64px;
padding: 0 8px;
}
.mini-button {
padding: 0 10px;
background: var(--surface);
}
.axis-button:hover,
.axis-button.active {
border-color: var(--accent);
background: linear-gradient(180deg, #10a4b3 0%, var(--accent) 100%);
color: #ffffff;
box-shadow: 0 2px 0 var(--accent-dark);
}
.mini-button:hover,
.mini-button.active {
border-color: var(--warn);
background: var(--warn);
color: #ffffff;
box-shadow: 0 2px 0 #8a5a13;
}
.jog-button:focus-visible {
outline: 3px solid rgba(0, 124, 137, 0.3);
outline-offset: 2px;
}
.jog-button.active {
transform: translateY(1px);
}
.axis-button.up {
grid-column: 2;
grid-row: 1;
}
.axis-button.left {
grid-column: 1;
grid-row: 2;
}
.axis-button.right {
grid-column: 3;
grid-row: 2;
}
.axis-button.down {
grid-column: 2;
grid-row: 3;
}
.center-stack {
display: grid;
grid-column: 2;
grid-row: 2;
grid-template-columns: 1fr 1fr;
gap: 6px;
place-items: center;
padding: 8px;
border-radius: 999px;
background: rgba(47, 59, 79, 0.08);
}
.hint-row {
display: grid;
grid-template-columns: repeat(3, minmax(0, 1fr));
gap: 8px;
padding: 0 16px 16px;
color: var(--muted);
font-size: 13px;
text-align: center;
}
.hint-row span {
min-height: 28px;
padding: 5px 8px;
border-radius: 999px;
background: var(--steel-soft);
white-space: nowrap;
}
.jog-status {
display: flex;
align-items: center;
justify-content: flex-end;
min-height: 38px;
color: var(--muted);
font-family: Consolas, "Cascadia Mono", monospace;
text-align: right;
}
.jog-status.error {
color: var(--bad);
}
.log-panel {
margin: 0 16px 16px;
overflow: hidden;
border: 1px solid var(--line);
border-radius: 8px;
background: #fbfcfe;
}
.log-head {
display: flex;
align-items: center;
justify-content: space-between;
gap: 12px;
padding: 12px 14px;
border-bottom: 1px solid var(--line);
background: var(--surface);
}
.log-head h3 {
margin: 0;
font-size: 15px;
font-weight: 650;
}
.clear-button {
min-height: 34px;
padding: 0 12px;
border-color: var(--line);
background: #ffffff;
color: var(--muted);
}
.clear-button:hover {
border-color: var(--accent);
color: var(--accent);
}
.request-log {
display: grid;
gap: 8px;
max-height: 300px;
min-height: 54px;
margin: 0;
padding: 12px 14px 14px;
overflow: auto;
font-family: Consolas, "Cascadia Mono", monospace;
font-size: 13px;
}
.log-entry {
display: grid;
grid-template-columns: 88px 96px minmax(0, 1fr);
gap: 10px;
align-items: start;
padding: 9px 10px;
border: 1px solid #e1e7ef;
border-radius: 6px;
background: #ffffff;
}
.log-entry:first-child {
border-color: rgba(0, 124, 137, 0.35);
background: rgba(0, 124, 137, 0.04);
}
.log-entry.error {
border-color: rgba(180, 35, 24, 0.35);
background: rgba(180, 35, 24, 0.06);
}
.log-time,
.log-axis {
color: var(--muted);
white-space: nowrap;
}
.log-payload {
overflow-wrap: anywhere;
color: var(--text);
}
.log-empty {
color: var(--muted);
}
@media (max-width: 820px) {
.topbar {
align-items: flex-start;
@@ -199,6 +485,16 @@ dd {
grid-template-columns: 1fr;
}
.jog-toolbar,
.jog-body {
grid-template-columns: 1fr;
}
.jog-status {
justify-content: flex-start;
text-align: left;
}
dl {
grid-template-columns: 1fr;
}
@@ -212,3 +508,19 @@ dd {
padding-top: 2px;
}
}
@media (max-width: 520px) {
.stick-console {
grid-template-columns: 60px 1fr 60px;
grid-template-rows: 60px 60px 60px;
padding: 10px;
}
.axis-button {
min-height: 56px;
}
.log-entry {
grid-template-columns: 1fr;
}
}

View File

@@ -15,9 +15,37 @@ const fields = {
joints: document.getElementById("joints"),
pose: document.getElementById("pose"),
trajectories: document.getElementById("trajectories"),
refresh: document.getElementById("refresh")
refresh: document.getElementById("refresh"),
linearStep: document.getElementById("linear-step"),
angularStep: document.getElementById("angular-step"),
jogStatus: document.getElementById("jog-status"),
requestLog: document.getElementById("request-log"),
clearLog: document.getElementById("clear-log"),
jogButtons: Array.from(document.querySelectorAll(".jog-button"))
};
const axisIndexes = {
x: 0,
y: 1,
z: 2,
w: 3,
p: 4,
r: 5
};
// 点动状态集中保存,确保按住按钮时不会并发发送多条 MovePose 请求。
const jogState = {
timer: null,
activeButton: null,
inFlight: false,
lastSnapshot: null,
sequence: 0,
logs: []
};
const maxJogLogs = 80;
const jogIntervalMs = 250;
function formatArray(values) {
if (!Array.isArray(values) || values.length === 0) {
return "--";
@@ -56,12 +84,202 @@ function setDot(connectionState) {
}
}
function clampStep(input, min, max) {
const value = Number(input.value);
if (!Number.isFinite(value)) {
input.value = String(min);
return min;
}
const clamped = Math.min(Math.max(value, min), max);
input.value = String(clamped);
return clamped;
}
function getStepForAxis(axis) {
// 平移和姿态使用不同单位,但共享 0.1 到 20 的现场可调范围。
return axis === "x" || axis === "y" || axis === "z"
? clampStep(fields.linearStep, 0.1, 20)
: clampStep(fields.angularStep, 0.1, 20);
}
function setJogStatus(message, isError) {
fields.jogStatus.textContent = message;
fields.jogStatus.classList.toggle("error", Boolean(isError));
}
function formatDirection(axis, direction) {
return `${axis.toUpperCase()}${direction > 0 ? "+" : "-"}`;
}
function renderJogLogs() {
fields.requestLog.replaceChildren();
if (jogState.logs.length === 0) {
const empty = document.createElement("div");
empty.className = "log-empty";
empty.textContent = "暂无请求记录";
fields.requestLog.appendChild(empty);
return;
}
jogState.logs.forEach(function (entry) {
const row = document.createElement("div");
row.className = `log-entry${entry.ok ? "" : " error"}`;
const time = document.createElement("span");
time.className = "log-time";
time.textContent = entry.time;
const axis = document.createElement("span");
axis.className = "log-axis";
axis.textContent = `#${entry.sequence} ${entry.direction}`;
const payload = document.createElement("span");
payload.className = "log-payload";
payload.textContent = `step=${entry.step} ${entry.unit}; body=${entry.body}; ${entry.result}`;
row.append(time, axis, payload);
fields.requestLog.appendChild(row);
});
}
function addJogLog(entry) {
jogState.logs.unshift(entry);
if (jogState.logs.length > maxJogLogs) {
jogState.logs.length = maxJogLogs;
}
renderJogLogs();
}
async function loadSnapshotForJog() {
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
if (!response.ok) {
throw new Error(`状态快照读取失败: HTTP ${response.status}`);
}
const payload = await response.json();
jogState.lastSnapshot = payload.snapshot;
return payload.snapshot;
}
function buildJogPose(snapshot, axis, direction) {
// 每次点动都从最新 TCP 位姿出发,只修改一个轴,避免连续按压时累积本地误差。
const sourcePose = Array.isArray(snapshot.cartesianPose) ? snapshot.cartesianPose : [];
if (sourcePose.length < 6) {
throw new Error("当前 TCP 位姿不足 6 维,无法点动。");
}
const pose = sourcePose.slice(0, 6).map(Number);
if (pose.some(value => !Number.isFinite(value))) {
throw new Error("当前 TCP 位姿包含非数值,无法点动。");
}
const axisIndex = axisIndexes[axis];
pose[axisIndex] = Number((pose[axisIndex] + direction * getStepForAxis(axis)).toFixed(6));
return {
x: pose[0],
y: pose[1],
z: pose[2],
w: pose[3],
p: pose[4],
r: pose[5]
};
}
async function sendJog(button) {
// MovePose 底层会生成完整直角轨迹;前端这里只负责构造增量目标位姿。
if (jogState.inFlight) {
return;
}
const axis = button.dataset.axis;
const direction = Number(button.dataset.direction);
if (!Object.prototype.hasOwnProperty.call(axisIndexes, axis) || !Number.isFinite(direction)) {
return;
}
jogState.inFlight = true;
const step = getStepForAxis(axis);
const directionLabel = formatDirection(axis, direction);
const sequence = ++jogState.sequence;
let bodyText = "{}";
let resultText = "";
let ok = false;
try {
const snapshot = await loadSnapshotForJog();
const pose = buildJogPose(snapshot, axis, direction);
bodyText = JSON.stringify(pose);
const response = await fetch("/move_pose/", {
method: "POST",
headers: { "Content-Type": "application/json" },
body: bodyText
});
if (!response.ok) {
throw new Error(`MovePose 调用失败: HTTP ${response.status}`);
}
ok = true;
resultText = `HTTP ${response.status}`;
setJogStatus(`${directionLabel} 已发送`, false);
await refreshStatus();
} catch (error) {
resultText = error instanceof Error ? error.message : "点动失败";
setJogStatus(resultText, true);
stopJog();
} finally {
addJogLog({
time: new Date().toLocaleTimeString(),
sequence,
direction: directionLabel,
step,
unit: axis === "x" || axis === "y" || axis === "z" ? "mm" : "deg",
body: bodyText,
result: resultText,
ok
});
jogState.inFlight = false;
}
}
function startJog(event) {
const button = event.currentTarget;
if (!button || button.disabled) {
return;
}
event.preventDefault();
stopJog();
jogState.activeButton = button;
button.classList.add("active");
sendJog(button);
jogState.timer = window.setInterval(function () {
sendJog(button);
}, jogIntervalMs);
}
function stopJog() {
if (jogState.timer !== null) {
window.clearInterval(jogState.timer);
jogState.timer = null;
}
if (jogState.activeButton) {
jogState.activeButton.classList.remove("active");
jogState.activeButton = null;
}
}
async function refreshStatus() {
fields.refresh.disabled = true;
try {
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
const payload = await response.json();
const snapshot = payload.snapshot;
jogState.lastSnapshot = snapshot;
fields.connectionState.textContent = snapshot.connectionState;
fields.robotName.textContent = payload.robotName || "--";
@@ -88,5 +306,28 @@ async function refreshStatus() {
}
fields.refresh.addEventListener("click", refreshStatus);
fields.linearStep.addEventListener("change", function () {
clampStep(fields.linearStep, 0.1, 20);
});
fields.angularStep.addEventListener("change", function () {
clampStep(fields.angularStep, 0.1, 20);
});
fields.clearLog.addEventListener("click", function () {
jogState.logs = [];
renderJogLogs();
});
fields.jogButtons.forEach(function (button) {
button.addEventListener("pointerdown", startJog);
button.addEventListener("pointerup", stopJog);
button.addEventListener("pointercancel", stopJog);
button.addEventListener("pointerleave", stopJog);
});
window.addEventListener("blur", stopJog);
window.addEventListener("keyup", function (event) {
if (event.key === "Escape") {
stopJog();
}
});
renderJogLogs();
refreshStatus();
window.setInterval(refreshStatus, 2000);

View File

@@ -57,6 +57,73 @@
<dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd>
</dl>
</section>
<section class="jog-panel">
<h2>直角坐标点动</h2>
<div class="jog-toolbar">
<label class="step-field" for="linear-step">
<span>平移步长 mm</span>
<input id="linear-step" type="number" min="0.1" max="20" step="0.1" value="1">
</label>
<label class="step-field" for="angular-step">
<span>姿态步长 deg</span>
<input id="angular-step" type="number" min="0.1" max="20" step="0.1" value="1">
</label>
<div id="jog-status" class="jog-status">等待点动输入</div>
</div>
<div class="jog-body">
<article class="stick-card">
<div class="stick-head">
<h3>XYZ 平移摇杆</h3>
<span>mm</span>
</div>
<div class="stick-console" aria-label="XYZ 平移摇杆">
<button class="jog-button axis-button up" type="button" data-axis="y" data-direction="1">Y+</button>
<button class="jog-button axis-button left" type="button" data-axis="x" data-direction="-1">X-</button>
<div class="center-stack">
<button class="jog-button mini-button" type="button" data-axis="z" data-direction="1">Z+</button>
<button class="jog-button mini-button" type="button" data-axis="z" data-direction="-1">Z-</button>
</div>
<button class="jog-button axis-button right" type="button" data-axis="x" data-direction="1">X+</button>
<button class="jog-button axis-button down" type="button" data-axis="y" data-direction="-1">Y-</button>
</div>
<div class="hint-row">
<span>X 左右</span>
<span>Y 上下</span>
<span>Z 中心</span>
</div>
</article>
<article class="stick-card">
<div class="stick-head">
<h3>WPR 姿态摇杆</h3>
<span>deg</span>
</div>
<div class="stick-console" aria-label="WPR 姿态摇杆">
<button class="jog-button axis-button up" type="button" data-axis="p" data-direction="1">P+</button>
<button class="jog-button axis-button left" type="button" data-axis="w" data-direction="-1">W-</button>
<div class="center-stack">
<button class="jog-button mini-button" type="button" data-axis="r" data-direction="-1">R-</button>
<button class="jog-button mini-button" type="button" data-axis="r" data-direction="1">R+</button>
</div>
<button class="jog-button axis-button right" type="button" data-axis="w" data-direction="1">W+</button>
<button class="jog-button axis-button down" type="button" data-axis="p" data-direction="-1">P-</button>
</div>
<div class="hint-row">
<span>W 左右</span>
<span>P 上下</span>
<span>R 中心</span>
</div>
</article>
</div>
<div class="log-panel">
<div class="log-head">
<h3>请求日志</h3>
<button id="clear-log" class="clear-button" type="button">清空</button>
</div>
<div id="request-log" class="request-log" aria-live="polite">
<div class="log-empty">暂无请求记录</div>
</div>
</div>
</section>
</div>
</main>
<script src="/assets/status.js" defer></script>

View File

@@ -8,13 +8,13 @@ namespace Flyshot.Core.Tests;
public sealed class ConfigCompatibilityTests
{
/// <summary>
/// 验证现有 RobotConfig.json 能被加载,并保持关键机器人参数与飞拍程序内容不变。
/// 验证现有旧样本配置能被加载,并保持关键机器人参数与飞拍程序内容不变。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms()
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json");
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "EOL10_EAU_0.json");
var loaded = new RobotConfigLoader().Load(configPath);
@@ -75,6 +75,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.Equal(1.0, loaded.Robot.SpeedRatio);
Assert.True(loaded.Robot.SmoothStartStopTiming);
Assert.Equal([0, 0, 0], program.OffsetValues);
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>
/// 验证 RobotConfig.json 可以显式配置规划限速倍率,且该倍率独立于运行时 J519 速度倍率。
/// </summary>
@@ -212,7 +253,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath);
@@ -236,7 +277,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
@@ -251,7 +292,7 @@ public sealed class ConfigCompatibilityTests
public void RobotModelLoader_LoadsProfileAndKinematics_FromSingleParse()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var loaded = new RobotModelLoader().LoadProfileAndKinematics(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);

View File

@@ -74,7 +74,8 @@ public sealed class ControllerClientCompatConfigRootTests
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
adaptIcspTryNum: 5,
speedRatio: 0.65);
var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
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);
Assert.NotNull(loadedSettings);
Assert.Equal(7, loadedSettings.TriggerSampleIndexOffsetCycles);
Assert.Equal(0.65, loadedSettings.SpeedRatio, precision: 6);
Assert.Contains(trajectory.Name, loaded);
store.Delete("FANUC_LR_Mate_200iD", trajectory.Name);

View File

@@ -21,10 +21,10 @@ public sealed class FanucControllerRuntimeDenseTests
private const double SmoothPtpJerkShapeCoefficient = 52.5;
/// <summary>
/// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列,并输出角度制目标。
/// 验证真机 J519 会直接消费已经按 8ms 生成好的稠密轨迹,不再按运行时 speedRatio 临场重采样。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_UsesPlannedDenseRowsAndConvertsRadiansToDegrees()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -38,8 +38,8 @@ public sealed class FanucControllerRuntimeDenseTests
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 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -56,13 +56,19 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(5, commands.Count);
Assert.Equal(3, commands.Count);
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
var expectedJ1Targets = new[] { 0.0, 0.08, 0.16 };
var actualJ1Targets = commands.Select(static command => command.TargetJoints[0]).ToArray();
Assert.Equal(expectedJ1Targets.Length, actualJ1Targets.Length);
for (var index = 0; index < expectedJ1Targets.Length; index++)
{
Assert.Equal(expectedJ1Targets[index], actualJ1Targets[index], precision: 6);
}
Assert.False(j519Client.IsCommandQueueDrainedForTests());
}
@@ -90,8 +96,8 @@ public sealed class FanucControllerRuntimeDenseTests
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 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
programName: "wait-drain",
@@ -107,7 +113,7 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
// 只有机器人状态包把队列全部取出后,ExecuteTrajectory 才能向上层返回。
@@ -200,14 +206,16 @@ public sealed class FanucControllerRuntimeDenseTests
var firstTimingColumns = ParseColumns(timingLines[0]);
var secondTimingColumns = ParseColumns(timingLines[1]);
var lastTimingColumns = ParseColumns(timingLines[^1]);
Assert.Equal(4, firstTimingColumns.Length);
Assert.Equal(6, firstTimingColumns.Length);
Assert.Equal(0.0, firstTimingColumns[0], precision: 6);
Assert.Equal(0.0, firstTimingColumns[1], precision: 6);
Assert.Equal(0.0, firstTimingColumns[2], precision: 6);
Assert.Equal(0.5, firstTimingColumns[3], precision: 6);
Assert.Equal(1.0, firstTimingColumns[3], precision: 6);
Assert.Equal(0.5, firstTimingColumns[4], precision: 6);
Assert.Equal(0.0, firstTimingColumns[5], precision: 6);
Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
Assert.Equal(0.004, secondTimingColumns[2], precision: 6);
Assert.Equal(0.008, secondTimingColumns[2], precision: 6);
Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
@@ -355,6 +363,7 @@ public sealed class FanucControllerRuntimeDenseTests
planningSpeedScale: fullSpeedSettings.PlanningSpeedScale);
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
fixture.Robot,
bundle.ExecutionTrajectory,
bundle.Result,
fixture.Robot.ServoPeriod.TotalSeconds,
speedRatio);
@@ -387,7 +396,9 @@ public sealed class FanucControllerRuntimeDenseTests
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.Equal(preparedExecution.Samples.Count, commands.Count);
AssertJointDegreesEqual(result.DenseJointTrajectory[0].Skip(1).ToArray(), commands[0].TargetJoints);
Assert.Equal(speedRatio, preparedExecution.FinalSpeedRatio, precision: 6);
Assert.Equal(0, preparedExecution.StretchIterationCount);
Assert.Equal(preparedExecution.Samples[0].JointsDegrees, commands[0].TargetJoints);
}
finally
{
@@ -463,13 +474,13 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标,并继续叠加 speed_ratio 重采样。
/// 验证 MoveJoint 会在生成阶段折算 speed_ratio,运行时只发送已经规划好的 8ms 稠密点列。
/// </summary>
[Theory]
[InlineData(1.0)]
[InlineData(0.7)]
[InlineData(0.5)]
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryWithSpeedRatioAlreadyPlanned(double speedRatio)
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -524,7 +535,7 @@ public sealed class FanucControllerRuntimeDenseTests
}
[Fact]
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
public void MoveJointTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
var robot = CreateMoveJointReferenceRobotProfile();
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
@@ -536,9 +547,36 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints));
AssertDenseRowsUseServoPeriod(fullSpeed.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed07.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed05.DenseJointTrajectory, robot.ServoPeriod.TotalSeconds);
Assert.True(fullSpeed.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 1.0));
Assert.True(speed07.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7));
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5));
}
/// <summary>
/// 验证 MovePose 低速倍率仍保持固定伺服周期,并通过拉长时长降低直角运动速度。
/// </summary>
[Fact]
public void MovePoseTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
var servoPeriod = TimeSpan.FromMilliseconds(8);
var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
var targetPose = new[] { 140.0, 260.0, 330.0, 8.0, 10.0, 12.0 };
var fullSpeed = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 1.0);
var speed07 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.7);
var speed05 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.5);
Assert.True(speed07.DenseCartesianTrajectory!.Count > fullSpeed.DenseCartesianTrajectory!.Count);
Assert.True(speed05.DenseCartesianTrajectory!.Count > speed07.DenseCartesianTrajectory!.Count);
AssertDenseRowsUseServoPeriod(fullSpeed.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed07.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed05.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertPoseEqual(startPose, fullSpeed.DenseCartesianTrajectory[0].Skip(1).ToArray());
AssertPoseEqual(targetPose, fullSpeed.DenseCartesianTrajectory[^1].Skip(1).ToArray());
Assert.True(MovePoseTrajectoryGenerator.SatisfiesDefaultCartesianLimits(speed05.DenseCartesianTrajectory, speedRatio: 0.5));
}
[Fact]
@@ -565,10 +603,45 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
/// 验证 MovePose 会生成直角坐标 J519 队列,并使用 Data format=0 下发 X/Y/Z/W/P/R。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
public void MovePose_RealMode_GeneratesCartesianJ519Queue()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var service = CreateCompatService(runtime);
var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
var targetPose = new[] { 110.0, 220.0, 315.0, 4.0, 5.0, 6.0 };
service.SetUpRobot("FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
SetPrivateField(runtime, "_pose", startPose);
service.MovePose(targetPose);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.All(commands, static command => Assert.Equal(0, command.DataStyle));
AssertPoseEqual(startPose, commands[0].TargetValues.Take(6).ToArray());
AssertPoseEqual(targetPose, commands[^1].TargetValues.Take(6).ToArray());
Assert.All(commands, static command =>
{
Assert.Equal(0.0, command.TargetValues[6], precision: 6);
Assert.Equal(0.0, command.TargetValues[7], precision: 6);
Assert.Equal(0.0, command.TargetValues[8], precision: 6);
});
}
/// <summary>
/// 验证运行时稠密发送不再依赖当前 speed_ratio;倍率合法性应在上游规划/生成阶段处理。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_IgnoresRuntimeSpeedRatioForDenseFallback()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
@@ -576,12 +649,14 @@ public sealed class FanucControllerRuntimeDenseTests
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 0.0);
var denseTrajectory = new[]
{
new[] { 0.0, 0.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 }
new[] { 0.008, DegreesToRadians(0.08), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -598,9 +673,12 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
var exception = Assert.Throws<InvalidOperationException>(
() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(3, commands.Count);
Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
}
/// <summary>
@@ -819,8 +897,10 @@ public sealed class FanucControllerRuntimeDenseTests
var enableRobotRetryCount = 0;
var waitCount = 0;
using var runtime = new FanucControllerRuntime();
var exception = Record.Exception(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => runtime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
@@ -844,8 +924,10 @@ public sealed class FanucControllerRuntimeDenseTests
var enableRobotRetryCount = 0;
var waitCount = 0;
using var runtime = new FanucControllerRuntime();
var exception = Assert.Throws<InvalidOperationException>(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => runtime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
@@ -980,8 +1062,9 @@ public sealed class FanucControllerRuntimeDenseTests
var rows = result.DenseJointTrajectory!;
Assert.True(
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints),
result.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.7),
$"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
AssertDenseRowsUseServoPeriod(rows, robot.ServoPeriod.TotalSeconds);
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
}
@@ -1079,6 +1162,15 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
private static void AssertPoseEqual(IReadOnlyList<double> expected, IReadOnlyList<double> actual)
{
Assert.Equal(expected.Count, actual.Count);
for (var index = 0; index < expected.Count; index++)
{
Assert.Equal(expected[index], actual[index], precision: 6);
}
}
/// <summary>
/// 创建用于就绪状态测试的最小 J519 响应。
/// </summary>
@@ -1207,8 +1299,14 @@ public sealed class FanucControllerRuntimeDenseTests
private static double ExpectedSmoothPtpDuration(
RobotProfile robot,
IReadOnlyList<double> startJoints,
IReadOnlyList<double> targetJoints)
IReadOnlyList<double> targetJoints,
double speedRatio = 1.0)
{
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var duration = 0.0;
for (var index = 0; index < robot.DegreesOfFreedom; index++)
{
@@ -1219,15 +1317,27 @@ public sealed class FanucControllerRuntimeDenseTests
}
var limit = robot.JointLimits[index];
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / (limit.VelocityLimit * speedRatio);
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / (limit.AccelerationLimit * speedRatio * speedRatio));
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / (limit.JerkLimit * speedRatio * speedRatio * speedRatio));
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
}
return duration;
}
/// <summary>
/// 验证 MoveJoint 稠密轨迹已经是物理伺服周期,不把 speed_ratio 编码到采样间隔里。
/// </summary>
private static void AssertDenseRowsUseServoPeriod(IReadOnlyList<IReadOnlyList<double>> rows, double servoPeriodSeconds)
{
Assert.True(rows.Count > 1);
for (var index = 1; index < rows.Count; index++)
{
Assert.Equal(servoPeriodSeconds, rows[index][0] - rows[index - 1][0], precision: 6);
}
}
private static double DegreesToRadians(double degrees)
{
return degrees * Math.PI / 180.0;

View File

@@ -163,6 +163,28 @@ public sealed class FanucJ519ClientTests : IDisposable
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证直角坐标命令会把 Data format 写为 0,并按通用目标槽位写入 X/Y/Z/W/P/R。
/// </summary>
[Fact]
public void PackCommandPacket_WritesCartesianDataFormatAndTargetValues()
{
var command = new FanucJ519Command(
sequence: 7,
targetValues: [100.0, 200.0, 300.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0],
dataStyle: 0);
var packet = FanucJ519Protocol.PackCommandPacket(command);
Assert.Equal(0, packet[0x12]);
Assert.Equal(100.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
Assert.Equal(200.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x20, 4)));
Assert.Equal(300.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x24, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x28, 4)));
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x2c, 4)));
Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
}
/// <summary>
/// 验证配置 J519 buffer size 后,实际回发命令序号会在状态包序号基础上增加该缓冲深度。
/// </summary>

View File

@@ -1,4 +1,6 @@
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Core.Tests;
@@ -43,6 +45,58 @@ public sealed class J519SendTrajectorySamplerTests
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证飞拍 speed_ratio 安全队列从连续规划样条直接采样,而不是先把规划轨迹离散成稠密点后再线性插值。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_DirectlyEvaluatesContinuousSplineForSpeedRatio()
{
var trajectory = CreateSingleJointTrajectory();
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.5);
var sample = Assert.Single(samples.Where(static item => Math.Abs(item.TrajectoryTime - 0.004) < 1e-9));
Assert.Equal(0.008, sample.SendTime, precision: 6);
Assert.Equal(0.004, sample.TrajectoryTime, precision: 6);
Assert.NotEqual(45.0, sample.JointsDegrees[0], precision: 3);
var expected = new CubicSplineInterpolator(
trajectory.WaypointTimes.ToArray(),
trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray())
.Evaluate(0.004)[0] * 180.0 / Math.PI;
Assert.Equal(expected, sample.JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证 speed_ratio 等价于执行侧时间轴拉长:原轨迹 0.8 倍执行应与时间整体拉长 1/0.8 的轨迹 1 倍执行一致。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_SpeedRatioMatchesUniformPlanningTimeStretch()
{
var trajectory = CreateSingleJointTrajectory();
var stretchedTrajectory = StretchTrajectoryTime(trajectory, stretchFactor: 1.0 / 0.8);
var ratioSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.8);
var stretchedSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
stretchedTrajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Equal(stretchedSamples.Count, ratioSamples.Count);
for (var index = 0; index < ratioSamples.Count; index++)
{
Assert.Equal(stretchedSamples[index].SendTime, ratioSamples[index].SendTime, precision: 6);
Assert.Equal(stretchedSamples[index].TrajectoryTime * 0.8, ratioSamples[index].TrajectoryTime, precision: 6);
Assert.Equal(stretchedSamples[index].JointsDegrees[0], ratioSamples[index].JointsDegrees[0], precision: 6);
}
}
/// <summary>
/// 验证空稠密轨迹会直接暴露为调用错误,避免生成无意义下发点。
/// </summary>
@@ -96,4 +150,84 @@ public sealed class J519SendTrajectorySamplerTests
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
}
/// <summary>
/// 验证倍率诊断行在保留旧 4 列的同时追加请求倍率和历史改写次数。
/// </summary>
[Fact]
public void BuildTimingRow_WithRatioDiagnostics_AppendsRequestedRatioAndIterationCount()
{
var sample = new J519SendSample(
sampleIndex: 2,
sendTime: 0.016,
trajectoryTime: 0.0076,
speedRatio: 0.475,
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var row = J519SendTrajectorySampler.BuildTimingRow(
sample,
requestedSpeedRatio: 0.5,
stretchIterationCount: 1);
Assert.Equal([2.0, 0.016, 0.0076, 0.475, 0.5, 1.0], row);
}
/// <summary>
/// 构造一个最小单关节规划轨迹,便于验证 speed_ratio 直接样条采样语义。
/// </summary>
private static PlannedTrajectory CreateSingleJointTrajectory()
{
var robot = new RobotProfile(
name: "TestRobot",
modelPath: "Models/Test.robot",
degreesOfFreedom: 1,
jointLimits: [new JointLimit("J1", 100.0, 1000.0, 10000.0)],
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
var program = new FlyshotProgram(
name: "spline-sample",
waypoints:
[
new JointWaypoint([0.0]),
new JointWaypoint([Math.PI / 2.0]),
new JointWaypoint([Math.PI])
],
shotFlags: [false, false, false],
offsetValues: [0, 0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>())
]);
return new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.008, 0.016],
segmentDurations: [0.008, 0.008],
segmentScales: [1.0, 1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
}
/// <summary>
/// 构造统一拉长时间轴后的轨迹,模拟更低 planning_speed_scale 生成的等几何时间结果。
/// </summary>
private static PlannedTrajectory StretchTrajectoryTime(PlannedTrajectory trajectory, double stretchFactor)
{
return new PlannedTrajectory(
robot: trajectory.Robot,
originalProgram: trajectory.OriginalProgram,
plannedWaypoints: trajectory.PlannedWaypoints,
waypointTimes: trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(),
segmentDurations: trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(),
segmentScales: trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(),
method: trajectory.Method,
iterations: trajectory.Iterations,
threshold: trajectory.Threshold);
}
}

View File

@@ -40,7 +40,7 @@ public sealed class OfflinePlanTests
// 1. 加载配置和模型。
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
var program = loadedConfig.Programs[trajName];
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
resolvedRobotModelPath,
loadedConfig.Robot.AccLimitScale,

View File

@@ -67,7 +67,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, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var config = new RobotConfigLoader().Load(configPath);
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
@@ -239,6 +239,174 @@ public sealed class PlanningCompatibilityTests
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>
/// 验证飞拍最终 J519 队列的 jerk 硬约束使用严格限值,不再沿用历史上放大 4 倍的宽容口径。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_ThrowsForJ519Jerk_WhenStrictFinalQueueValidationIsEnabled()
{
var robot = CreateRobotProfile([100.0], [1000.0], [100.0]);
var samples = new[]
{
new J519SendSample(0, 0.000, 0.000, 1.0, [0.0]),
new J519SendSample(1, 0.008, 0.008, 1.0, [0.0]),
new J519SendSample(2, 0.016, 0.016, 1.0, [0.003754]),
new J519SendSample(3, 0.024, 0.024, 1.0, [0.007508])
};
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: "strict-final-j519",
validateJerk: true));
Assert.Contains("Jerk", exception.Message);
Assert.Contains("strict-final-j519", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列在请求倍率下严格 jerk 超限时直接拒绝执行,而不是自动改写 speed_ratio。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_ThrowsInsteadOfRewritingRequestedSpeedRatio()
{
var robot = CreateRobotProfile([100.0], [10000.0], [800.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "strict-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var initialSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
initialSamples,
trajectoryName: "strict-builder-initial",
validateJerk: true));
var exception = Assert.Throws<InvalidOperationException>(() =>
FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 1.0));
Assert.Contains("speed_ratio=1.000000", exception.Message);
Assert.Contains("planning_speed_scale", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列通过严格校验时,请求 speed_ratio 会原样成为最终倍率。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_KeepsRequestedSpeedRatioWhenQueuePasses()
{
var robot = CreateRobotProfile([100.0], [10000.0], [10000.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "rewrite-speedratio-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var prepared = FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 0.8);
Assert.Equal(0.8, prepared.RequestSpeedRatio, precision: 6);
Assert.Equal(0.8, prepared.FinalSpeedRatio, precision: 6);
Assert.Equal(0, prepared.StretchIterationCount);
Assert.All(prepared.Samples, sample => Assert.Equal(0.8, sample.SpeedRatio, precision: 6));
Assert.All(prepared.TimingRows, row =>
{
Assert.Equal(0.8, row[3], precision: 6);
Assert.Equal(0.8, row[4], precision: 6);
Assert.Equal(0.0, row[5], precision: 6);
});
}
/// <summary>
/// 构造一个最小 RobotProfile,便于规划层单元测试聚焦在时间轴逻辑上。
/// </summary>

View File

@@ -158,6 +158,83 @@ public sealed class RuntimeOrchestrationTests
$"半速规划时长应接近全速的 2 倍,实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
}
/// <summary>
/// 验证飞拍生成轨迹时会把 speed_ratio 折算进有效规划倍率,使倍率乘积相同的配置生成等价轨迹。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesSpeedRatioAsPlanningScaleFactor()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var planningSideSlowdown = CreateFlyshotSettings(planningSpeedScale: 0.45, speedRatio: 1.0);
var runtimeSideSlowdown = CreateFlyshotSettings(planningSpeedScale: 0.9, speedRatio: 0.5);
var planningSideBundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: planningSideSlowdown);
var runtimeSideBundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: runtimeSideSlowdown);
Assert.Equal(planningSideBundle.Result.Duration.TotalSeconds, runtimeSideBundle.Result.Duration.TotalSeconds, precision: 6);
Assert.Equal(planningSideBundle.ExecutionTrajectory.WaypointTimes.Count, runtimeSideBundle.ExecutionTrajectory.WaypointTimes.Count);
for (var index = 0; index < planningSideBundle.ExecutionTrajectory.WaypointTimes.Count; index++)
{
Assert.Equal(
planningSideBundle.ExecutionTrajectory.WaypointTimes[index],
runtimeSideBundle.ExecutionTrajectory.WaypointTimes[index],
precision: 6);
}
Assert.Equal(planningSideBundle.Result.DenseJointTrajectory!.Count, runtimeSideBundle.Result.DenseJointTrajectory!.Count);
for (var rowIndex = 0; rowIndex < planningSideBundle.Result.DenseJointTrajectory.Count; rowIndex++)
{
var planningSideRow = planningSideBundle.Result.DenseJointTrajectory[rowIndex];
var runtimeSideRow = runtimeSideBundle.Result.DenseJointTrajectory[rowIndex];
Assert.Equal(planningSideRow.Count, runtimeSideRow.Count);
for (var columnIndex = 0; columnIndex < planningSideRow.Count; columnIndex++)
{
Assert.Equal(planningSideRow[columnIndex], runtimeSideRow[columnIndex], precision: 6);
}
}
}
/// <summary>
/// 验证普通轨迹生成也会把 speed_ratio 折算进规划倍率,避免把限速留到发送阶段处理。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_UsesSpeedRatioAsPlanningScaleFactor()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.001, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.002, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.003, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var planningSideBundle = orchestrator.PlanOrdinaryTrajectory(
robot,
waypoints,
planningSpeedScale: 0.45);
var runtimeSideBundle = orchestrator.PlanOrdinaryTrajectory(
robot,
waypoints,
planningSpeedScale: 0.9,
speedRatio: 0.5);
Assert.Equal(planningSideBundle.Result.Duration.TotalSeconds, runtimeSideBundle.Result.Duration.TotalSeconds, precision: 6);
Assert.Equal(planningSideBundle.Result.DenseJointTrajectory!.Count, runtimeSideBundle.Result.DenseJointTrajectory!.Count);
for (var rowIndex = 0; rowIndex < planningSideBundle.Result.DenseJointTrajectory.Count; rowIndex++)
{
var planningSideRow = planningSideBundle.Result.DenseJointTrajectory[rowIndex];
var runtimeSideRow = runtimeSideBundle.Result.DenseJointTrajectory[rowIndex];
for (var columnIndex = 0; columnIndex < planningSideRow.Count; columnIndex++)
{
Assert.Equal(planningSideRow[columnIndex], runtimeSideRow[columnIndex], precision: 6);
}
}
}
/// <summary>
/// 验证飞拍缓存键包含规划限速倍率,避免降速验证时误用 100% 速度下的规划结果。
/// </summary>
@@ -314,35 +391,37 @@ public sealed class RuntimeOrchestrationTests
{
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var legacyFitSettings = DisableSmoothStartStopTiming(fixture.Settings);
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings);
settings: legacyFitSettings);
var baselineBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var rawDense = TrajectorySampler.SampleJointTrajectory(
bundle.PlannedTrajectory,
var executionDense = TrajectorySampler.SampleJointTrajectory(
bundle.ExecutionTrajectory,
samplePeriod: fixture.Robot.ServoPeriod.TotalSeconds);
Assert.Equal(0.7422771653721995, fixture.Settings.PlanningSpeedScale, precision: 12);
Assert.False(fixture.Settings.SmoothStartStopTiming);
Assert.Equal(0.74227, legacyFitSettings.PlanningSpeedScale, precision: 6);
Assert.False(legacyFitSettings.SmoothStartStopTiming);
Assert.Equal(fixture.Uploaded.Waypoints.Count, bundle.PlannedTrajectory.WaypointTimes.Count);
Assert.Equal(
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / fixture.Settings.PlanningSpeedScale,
baselineBundle.PlannedTrajectory.WaypointTimes[^1] / legacyFitSettings.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);
// 关闭二次时间重映射时,稠密点应和最终执行时间轴自洽;若离散限幅额外拉长,也要反映在 ExecutionTrajectory 上。
Assert.Equal(executionDense.Count, bundle.Result.DenseJointTrajectory!.Count);
Assert.Equal(executionDense[1][1], bundle.Result.DenseJointTrajectory[1][1], precision: 12);
Assert.True(bundle.ExecutionTrajectory.WaypointTimes[^1] >= bundle.PlannedTrajectory.WaypointTimes[^1]);
Assert.True(
Math.Abs(bundle.PlannedTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留规划终点时间,planned={bundle.PlannedTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
Math.Abs(bundle.ExecutionTrajectory.WaypointTimes[^1] - bundle.Result.Duration.TotalSeconds) < 1e-6,
$"执行时长应保留最终执行时间轴终点,execution={bundle.ExecutionTrajectory.WaypointTimes[^1]}, result={bundle.Result.Duration.TotalSeconds}");
}
/// <summary>
@@ -697,6 +776,54 @@ public sealed class RuntimeOrchestrationTests
Assert.Throws<ArgumentException>(Act);
}
/// <summary>
/// 验证兼容服务执行普通轨迹时会把运行时 speed_ratio 前移到规划阶段。
/// </summary>
[Fact]
public void ControllerClientCompatService_ExecuteTrajectory_PassesSpeedRatioIntoOrdinaryPlanning()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot, planningSpeedScale: 0.9, speedRatio: 1.0);
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());
var waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.001, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.002, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.003, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
runtime.SetSpeedRatio(0.5);
service.ExecuteTrajectory(waypoints);
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
var expected = new ControllerClientTrajectoryOrchestrator()
.PlanOrdinaryTrajectory(robot, waypoints, planningSpeedScale: 0.45);
var actual = Assert.IsType<TrajectoryResult>(runtime.LastExecutedResult);
Assert.Equal(expected.Result.Duration.TotalSeconds, actual.Duration.TotalSeconds, precision: 6);
Assert.Equal(expected.Result.DenseJointTrajectory!.Count, actual.DenseJointTrajectory!.Count);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点,并等待该段运动完成后再启动飞拍轨迹。
/// </summary>
@@ -706,6 +833,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -750,6 +878,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -793,6 +922,7 @@ public sealed class RuntimeOrchestrationTests
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
@@ -826,6 +956,111 @@ public sealed class RuntimeOrchestrationTests
}
}
/// <summary>
/// 验证飞拍执行阻塞在运行时时,状态页元数据快照仍能通过短锁快速返回。
/// </summary>
[Fact]
public async Task ControllerClientCompatService_GetStatusSnapshotMetadata_DoesNotWaitForRunningFlyshot()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
var executing = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
var metadataTask = Task.Run(() => service.GetStatusSnapshotMetadata());
var completed = await Task.WhenAny(metadataTask, Task.Delay(TimeSpan.FromMilliseconds(150)));
runtime.ReleaseExecution();
await executing;
Assert.Same(metadataTask, completed);
var metadata = await metadataTask;
Assert.True(metadata.IsSetup);
Assert.Equal("FANUC_LR_Mate_200iD", metadata.RobotName);
Assert.Equal(["demo-flyshot"], metadata.UploadedTrajectories);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证两个飞拍执行命令必须串行进入 runtime,避免 J519 队列被并发执行覆盖。
/// </summary>
[Fact]
public async Task ControllerClientCompatService_ExecuteTrajectoryByName_SerializesConcurrentExecutionCommands()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
var first = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
var second = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
await Task.Delay(TimeSpan.FromMilliseconds(100));
Assert.Equal(1, runtime.ExecuteCallCount);
runtime.ReleaseExecution();
await first;
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2), expectedCallCount: 2));
runtime.ReleaseExecution();
await second;
Assert.Equal(2, runtime.ExecuteCallCount);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证飞拍链路在进入运行时前就会准备最终发送队列,而不是把 speedRatio 重采样留给运行时临场处理。
/// </summary>
@@ -887,7 +1122,8 @@ public sealed class RuntimeOrchestrationTests
"io_keep_cycles": 4,
"acc_limit": 0.5,
"jerk_limit": 0.25,
"adapt_icsp_try_num": 3
"adapt_icsp_try_num": 3,
"speed_ratio": 0.65
},
"flying_shots": {}
}
@@ -907,6 +1143,7 @@ public sealed class RuntimeOrchestrationTests
var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
Assert.Equal(0.65, runtime.GetSpeedRatio(), precision: 6);
}
finally
{
@@ -958,7 +1195,7 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位,并按执行侧稠密轨迹时长应用当前 speed_ratio。
/// 验证 SaveTrajectoryInfo 会导出按 J519 8ms 实发周期重采样的点位,并保留当前 speed_ratio 诊断信息。
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
@@ -992,17 +1229,21 @@ public sealed class RuntimeOrchestrationTests
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var shotEventsJson = File.ReadAllText(shotEventsPath);
var executionDuration = double.Parse(
var planningExportDuration = double.Parse(
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
CultureInfo.InvariantCulture);
var minimumExpectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1;
var planningExportRowsAtServoPeriod = (int)Math.Ceiling(Math.Max(0.0, (planningExportDuration / 0.008) - 1e-9)) + 1;
Assert.Equal(pointRows.Length, timingRows.Length);
Assert.True(pointRows.Length >= minimumExpectedRows, $"最终发送点数应不少于请求倍率的首轮候选值,actual={pointRows.Length}, min={minimumExpectedRows}");
Assert.True(
pointRows.Length > planningExportRowsAtServoPeriod,
$"ActualSend 应按当前 speedRatio 的执行视图导出,不能再用旧 JointDetialTraj 规划时长约束点数。actual={pointRows.Length}, planningRows={planningExportRowsAtServoPeriod}");
Assert.Equal(0.0, pointRows[0][0], precision: 6);
Assert.Equal(0.008, pointRows[1][0], precision: 6);
Assert.True(timingRows[1][2] <= 0.004 + 1e-6, $"自动拉长后 trajectory_time 推进不应快于请求倍率,actual={timingRows[1][2]:F6}");
Assert.True(timingRows[1][3] <= 0.5 + 1e-6, $"最终采用倍率不应快于请求倍率,actual={timingRows[1][3]:F6}");
Assert.Equal(0.008, timingRows[1][2], precision: 6);
Assert.Equal(1.0, timingRows[1][3], precision: 6);
Assert.Equal(0.5, timingRows[1][4], precision: 6);
Assert.Equal(0.0, timingRows[1][5], precision: 6);
Assert.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson);
Assert.Contains("\"selected_sample_index\"", shotEventsJson);
}
@@ -1013,11 +1254,56 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图,
/// 而不是再次从 PlannedTrajectory 独立重采样。
/// 验证 SaveTrajectoryInfo 的旧格式规划导出只受 planning_speed_scale 影响,不跟随运行时 speed_ratio 改变。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample()
public void ControllerClientCompatService_SaveTrajectoryInfo_KeepsLegacyPlanningExportsIndependentFromSpeedRatio()
{
var fullSpeedRoot = CreateTempConfigRoot();
var halfSpeedRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(fullSpeedRoot, planningSpeedScale: 0.45, speedRatio: 1.0);
WriteRobotConfigWithDemoTrajectory(halfSpeedRoot, planningSpeedScale: 0.45, speedRatio: 0.5);
SaveDemoTrajectoryInfo(fullSpeedRoot);
SaveDemoTrajectoryInfo(halfSpeedRoot);
var fullSpeedOutput = Path.Combine(fullSpeedRoot, "Data", "demo-flyshot");
var halfSpeedOutput = Path.Combine(halfSpeedRoot, "Data", "demo-flyshot");
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "JointTraj.txt"),
Path.Combine(halfSpeedOutput, "JointTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "JointDetialTraj.txt"),
Path.Combine(halfSpeedOutput, "JointDetialTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "CartTraj.txt"),
Path.Combine(halfSpeedOutput, "CartTraj.txt"));
AssertNumericFilesEqual(
Path.Combine(fullSpeedOutput, "CartDetialTraj.txt"),
Path.Combine(halfSpeedOutput, "CartDetialTraj.txt"));
var fullSpeedActualSend = File.ReadAllLines(Path.Combine(fullSpeedOutput, "ActualSendJointTraj.txt"));
var halfSpeedActualSend = File.ReadAllLines(Path.Combine(halfSpeedOutput, "ActualSendJointTraj.txt"));
var fullSpeedTiming = File.ReadAllLines(Path.Combine(fullSpeedOutput, "ActualSendTiming.txt"));
var halfSpeedTiming = File.ReadAllLines(Path.Combine(halfSpeedOutput, "ActualSendTiming.txt"));
Assert.NotEqual(fullSpeedActualSend.Length, halfSpeedActualSend.Length);
Assert.NotEqual(fullSpeedTiming.Length, halfSpeedTiming.Length);
}
finally
{
Directory.Delete(fullSpeedRoot, recursive: true);
Directory.Delete(halfSpeedRoot, recursive: true);
}
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自旧规划导出视图的 16ms 明细轨迹,
/// 而不是来自 speed_ratio 后的实发执行视图。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExportPlanningDenseDownsample()
{
var fixture = LoadUttcMs11RuntimeFixture();
var configRoot = CreateTempConfigRoot();
@@ -1026,20 +1312,26 @@ public sealed class RuntimeOrchestrationTests
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader());
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
var exportBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
var executionBundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0,
speedRatio: 0.5);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, exportBundle, executionBundle, speedRatio: 0.5);
var outputDir = Path.Combine(configRoot, "Data", "UTTC_MS11");
var exportedRows = File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt"))
.Select(ParseSpaceSeparatedDoubles)
.ToArray();
var expectedRows = DownsampleDenseRows(
bundle.Result.DenseJointTrajectory!,
exportBundle.Result.DenseJointTrajectory!,
samplePeriodSeconds: 0.016)
.Select(static row => row.ToArray())
.ToArray();
@@ -1083,16 +1375,21 @@ public sealed class RuntimeOrchestrationTests
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json,供兼容服务从统一配置恢复轨迹。
/// </summary>
/// <param name="configRoot">测试运行配置根。</param>
private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
private static void WriteRobotConfigWithDemoTrajectory(
string configRoot,
double planningSpeedScale = 0.45,
double speedRatio = 1.0)
{
File.WriteAllText(
Path.Combine(configRoot, "RobotConfig.json"),
"""
$$"""
{
"robot": {
"use_do": true,
"io_addr": [7, 8],
"io_keep_cycles": 2,
"speed_ratio": {{speedRatio.ToString(CultureInfo.InvariantCulture)}},
"planning_speed_scale": {{planningSpeedScale.ToString(CultureInfo.InvariantCulture)}},
"acc_limit": 1.0,
"jerk_limit": 1.0,
"adapt_icsp_try_num": 5
@@ -1101,9 +1398,9 @@ public sealed class RuntimeOrchestrationTests
"demo-flyshot": {
"traj_waypoints": [
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
],
"shot_flags": [false, true, false, false],
"offset_values": [0, 1, 0, 0],
@@ -1114,6 +1411,42 @@ public sealed class RuntimeOrchestrationTests
""");
}
/// <summary>
/// 使用指定运行配置根执行 demo-flyshot 的 SaveTrajectoryInfo。
/// </summary>
private static void SaveDemoTrajectoryInfo(string configRoot)
{
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
new RecordingControllerRuntime(),
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SaveTrajectoryInfo("demo-flyshot");
}
/// <summary>
/// 按数值逐行比较两个轨迹文本文件,避免纯文本格式差异影响判断。
/// </summary>
private static void AssertNumericFilesEqual(string expectedPath, string actualPath)
{
var expectedRows = File.ReadAllLines(expectedPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var actualRows = File.ReadAllLines(actualPath).Select(ParseSpaceSeparatedDoubles).ToArray();
Assert.Equal(expectedRows.Length, actualRows.Length);
for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++)
{
Assert.Equal(expectedRows[rowIndex].Length, actualRows[rowIndex].Length);
for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++)
{
Assert.Equal(expectedRows[rowIndex][columnIndex], actualRows[rowIndex][columnIndex], precision: 6);
}
}
}
/// <summary>
/// 角度转弧度,供轨迹整形测试构造输入。
/// </summary>
@@ -1181,6 +1514,43 @@ public sealed class RuntimeOrchestrationTests
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
speedRatio: settings.SpeedRatio,
smoothStartStopTiming: true);
}
/// <summary>
/// 为 legacy-fit 对比测试显式关闭二次时间重映射。
/// </summary>
private static CompatibilityRobotSettings DisableSmoothStartStopTiming(CompatibilityRobotSettings settings)
{
return new CompatibilityRobotSettings(
useDo: settings.UseDo,
ioAddresses: settings.IoAddresses,
ioKeepCycles: settings.IoKeepCycles,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles,
accLimitScale: settings.AccLimitScale,
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,
planningSpeedScale: settings.PlanningSpeedScale,
speedRatio: settings.SpeedRatio,
smoothStartStopTiming: false);
}
/// <summary>
/// 构造飞拍倍率测试使用的最小兼容设置。
/// </summary>
private static CompatibilityRobotSettings CreateFlyshotSettings(double planningSpeedScale, double speedRatio)
{
return new CompatibilityRobotSettings(
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 2,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5,
planningSpeedScale: planningSpeedScale,
speedRatio: speedRatio,
smoothStartStopTiming: true);
}
@@ -1553,9 +1923,9 @@ internal static class TestRobotFactory
waypoints:
[
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
[0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.002, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.003, 0.0, 0.0, 0.0, 0.0, 0.0]
],
shotFlags: [false, true, false, false],
offsetValues: [0, 1, 0, 0],
@@ -1740,6 +2110,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
{
LastExecutedResult = result;
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
LastExecutedResult = result;
}
}
/// <summary>
@@ -1901,6 +2277,182 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray();
}
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
}
}
/// <summary>
/// 模拟 runtime 执行入口长期占用的测试运行时,用于验证兼容层锁边界。
/// </summary>
internal sealed class BlockingExecutionControllerRuntime : IControllerRuntime
{
private readonly object _lock = new();
private readonly ManualResetEventSlim _executionStarted = new(false);
private readonly ManualResetEventSlim _releaseExecution = new(false);
private readonly double[] _jointPositions;
private bool _isEnabled;
private bool _isInMotion;
private int _executeCallCount;
/// <summary>
/// 初始化一份会阻塞 ExecuteTrajectory 的测试运行时。
/// </summary>
/// <param name="initialJointPositions">初始关节反馈。</param>
public BlockingExecutionControllerRuntime(IReadOnlyList<double> initialJointPositions)
{
_jointPositions = initialJointPositions.ToArray();
}
/// <summary>
/// 获取 runtime 执行入口被调用的次数。
/// </summary>
public int ExecuteCallCount
{
get
{
lock (_lock)
{
return _executeCallCount;
}
}
}
/// <summary>
/// 等待指定序号的执行调用进入 runtime。
/// </summary>
/// <param name="timeout">最长等待时间。</param>
/// <param name="expectedCallCount">期望已经进入的执行次数。</param>
/// <returns>是否在超时前等到。</returns>
public bool WaitForExecutionStarted(TimeSpan timeout, int expectedCallCount = 1)
{
var deadline = DateTimeOffset.UtcNow.Add(timeout);
while (DateTimeOffset.UtcNow < deadline)
{
lock (_lock)
{
if (_executeCallCount >= expectedCallCount)
{
return true;
}
}
_executionStarted.Wait(TimeSpan.FromMilliseconds(10));
}
return false;
}
/// <summary>
/// 释放当前阻塞的执行调用。
/// </summary>
public void ReleaseExecution()
{
_releaseExecution.Set();
}
public void ResetRobot(RobotProfile robot, string robotName)
{
}
public void SetActiveController(bool sim)
{
}
public void Connect(string robotIp)
{
}
public void Disconnect()
{
}
public void EnableRobot(int bufferSize)
{
_isEnabled = true;
}
public void DisableRobot()
{
_isEnabled = false;
}
public void StopMove()
{
lock (_lock)
{
_isInMotion = false;
}
ReleaseExecution();
}
public double GetSpeedRatio() => 1.0;
public void SetSpeedRatio(double ratio)
{
}
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
public void SetTcp(double x, double y, double z)
{
}
public bool GetIo(int port, string ioType) => false;
public void SetIo(int port, bool value, string ioType)
{
}
public IReadOnlyList<double> GetJointPositions()
{
return _jointPositions.ToArray();
}
public IReadOnlyList<double> GetPose() => [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
public ControllerStateSnapshot GetSnapshot()
{
lock (_lock)
{
return new ControllerStateSnapshot(
capturedAt: DateTimeOffset.UtcNow,
connectionState: "Connected",
isEnabled: _isEnabled,
isInMotion: _isInMotion,
speedRatio: 1.0,
jointPositions: _jointPositions.ToArray(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());
}
}
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
{
lock (_lock)
{
_executeCallCount++;
_isInMotion = true;
_executionStarted.Set();
_releaseExecution.Reset();
}
_releaseExecution.Wait();
lock (_lock)
{
_isInMotion = false;
_executionStarted.Reset();
}
}
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
ExecuteTrajectory(result, _jointPositions);
}
}
/// <summary>
@@ -2027,4 +2579,8 @@ internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray();
}
}
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
}
}

View File

@@ -44,6 +44,28 @@ public sealed class DebugConsoleEndpointTests(FlyshotServerFactory factory) : IC
Assert.Contains("/api/debug/config", script, StringComparison.Ordinal);
}
/// <summary>
/// 调试页应当为 MovePose 提供可直接发送的六字段请求体模板。
/// </summary>
[Fact]
public async Task GetDebugScript_ContainsMovePoseRequestSample()
{
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
using var client = configuredFactory.CreateClient();
using var scriptResponse = await client.GetAsync("/assets/debug.js");
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
var script = await scriptResponse.Content.ReadAsStringAsync();
Assert.Contains("/move_pose/", script, StringComparison.Ordinal);
Assert.Contains("x: 100.0", script, StringComparison.Ordinal);
Assert.Contains("y: 200.0", script, StringComparison.Ordinal);
Assert.Contains("z: 300.0", script, StringComparison.Ordinal);
Assert.Contains("w: 0.0", script, StringComparison.Ordinal);
Assert.Contains("p: 45.0", script, StringComparison.Ordinal);
Assert.Contains("r: 0.0", script, StringComparison.Ordinal);
}
/// <summary>
/// 当 Swagger 启用时,调试配置 API 应当返回实际 Swagger JSON 地址。
/// </summary>

View File

@@ -1,5 +1,6 @@
using System.Net;
using System.Net.Http.Json;
using System.Text;
using System.Text.Json;
namespace Flyshot.Server.IntegrationTests;
@@ -131,6 +132,13 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.Equal("robot moved", moveJointJson.RootElement.GetProperty("status").GetString());
}
using (var movePoseResponse = await client.PostAsJsonAsync("/move_pose/", new { x = 100.0, y = 200.0, z = 300.0, w = 1.0, p = 2.0, r = 3.0 }))
{
Assert.Equal(HttpStatusCode.OK, movePoseResponse.StatusCode);
using var movePoseJson = await ReadJsonAsync(movePoseResponse);
Assert.Equal("robot moved", movePoseJson.RootElement.GetProperty("status").GetString());
}
using (var getJointPositionResponse = await client.GetAsync("/get_joint_position/"))
{
Assert.Equal(HttpStatusCode.OK, getJointPositionResponse.StatusCode);
@@ -165,6 +173,32 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
}
}
/// <summary>
/// 验证 MovePose 请求必须显式提供六个有限直角坐标字段,避免缺字段被模型绑定静默补 0。
/// </summary>
[Theory]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0}""")]
[InlineData("null")]
[InlineData("""{"x":1e999,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":1000.1,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":-1000.1,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":-0.1,"w":1.0,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":-180.1,"p":2.0,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":90.1,"r":3.0}""")]
[InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":180.1}""")]
public async Task MovePose_InvalidPayload_ReturnsLegacyBadRequest(string payload)
{
using var client = factory.CreateClient();
await InitializeRobotAsync(client);
using var content = new StringContent(payload, Encoding.UTF8, "application/json");
using var response = await client.PostAsync("/move_pose/", content);
Assert.Equal(HttpStatusCode.BadRequest, response.StatusCode);
using var json = await ReadJsonAsync(response);
Assert.Equal("MovePose failed", json.RootElement.GetProperty("detail").GetString());
}
/// <summary>
/// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。
/// </summary>

View File

@@ -34,6 +34,60 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi
Assert.Contains("/api/status/snapshot", script, StringComparison.Ordinal);
}
/// <summary>
/// 状态页应当提供双摇杆直角坐标点动面板,并复用现有 MovePose HTTP 接口。
/// </summary>
[Fact]
public async Task GetStatusPageAssets_ExposeCartesianJogControls()
{
using var client = factory.CreateClient();
using var htmlResponse = await client.GetAsync("/status.html");
Assert.Equal(HttpStatusCode.OK, htmlResponse.StatusCode);
var html = await htmlResponse.Content.ReadAsStringAsync();
Assert.Contains("直角坐标点动", html, StringComparison.Ordinal);
Assert.Contains("XYZ 平移摇杆", html, StringComparison.Ordinal);
Assert.Contains("WPR 姿态摇杆", html, StringComparison.Ordinal);
Assert.Contains("id=\"linear-step\"", html, StringComparison.Ordinal);
Assert.Contains("id=\"angular-step\"", html, StringComparison.Ordinal);
Assert.Contains("max=\"20\"", html, StringComparison.Ordinal);
Assert.Contains("请求日志", html, StringComparison.Ordinal);
Assert.Contains("id=\"request-log\"", html, StringComparison.Ordinal);
Assert.Contains("id=\"clear-log\"", html, StringComparison.Ordinal);
foreach (var (axis, negative, positive) in new[]
{
("x", "X-", "X+"),
("y", "Y-", "Y+"),
("z", "Z-", "Z+"),
("w", "W-", "W+"),
("p", "P-", "P+"),
("r", "R-", "R+")
})
{
Assert.Contains($"data-axis=\"{axis}\"", html, StringComparison.Ordinal);
Assert.Contains($"data-axis=\"{axis}\" data-direction=\"1\"", html, StringComparison.Ordinal);
Assert.Contains($"data-axis=\"{axis}\" data-direction=\"-1\"", html, StringComparison.Ordinal);
Assert.Contains(negative, html, StringComparison.Ordinal);
Assert.Contains(positive, html, StringComparison.Ordinal);
}
using var scriptResponse = await client.GetAsync("/assets/status.js");
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
var script = await scriptResponse.Content.ReadAsStringAsync();
Assert.Contains("/api/status/snapshot", script, StringComparison.Ordinal);
Assert.Contains("/move_pose/", script, StringComparison.Ordinal);
Assert.Contains("cartesianPose", script, StringComparison.Ordinal);
Assert.Contains("pointerdown", script, StringComparison.Ordinal);
Assert.Contains("pointerup", script, StringComparison.Ordinal);
Assert.Contains("pointerleave", script, StringComparison.Ordinal);
Assert.Contains("blur", script, StringComparison.Ordinal);
Assert.Contains("request-log", script, StringComparison.Ordinal);
Assert.Contains("clear-log", script, StringComparison.Ordinal);
}
/// <summary>
/// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。
/// </summary>