# FANUC Stream Motion 文档要点与实现差异

本文记录 `../FANUC_stream_motion.pdf` 中与本仓库 `Flyshot.Runtime.Fanuc` 直接相关的重点，并对照当前实现状态。

读取时间：2026-05-03

## 1. 文档定位

`FANUC_stream_motion.pdf` 对应 FANUC `Stream motion` 功能，选项号为 `A05B-2600-J519`。它描述的是外部设备通过以太网实时发送期望位置，让机器人按外部生成路径运动的控制方式。

文档明确要求外部设备自行生成满足机器人约束的路径，包括速度、加速度、jerk、可达性、姿态连续性等。FANUC 不提供完整运动学、逆解和碰撞检测公式。

## 2. 使用前提与示教程序

1. 机器人侧需要安装 J519 stream motion 选项。
2. 物理网口通过 `$STMO.$PHYS_PORT` 选择，`1` 表示 `CD38A`，`2` 表示 `CD38B`。
3. 机器人程序必须包含成对的 `IBGN start[*]` 和 `IBGN end[*]` 指令，二者编号必须一致，`start` 必须在 `end` 前一行。
4. `IBGN start[*]` 执行期间，机器人根据外部设备发来的期望位置运动；`IBGN end[*]` 之后程序继续执行。
5. 执行时要求 `AUTO` 模式和 `100% OVERRIDE`。

当前实现中的 `FanucControllerRuntime.EnableRobot()` 会按现场抓包流程启动 `RVBUSTSM` 程序，并随后允许 J519 在收到机器人 UDP status 包后回发命令。是否满足 `AUTO / 100% OVERRIDE / IBGN start` 已到位，当前只通过 J519 状态位和现场程序行为间接判断，没有在代码里读取或设置这些控制器状态。

## 3. UDP 60015 协议结构

协议使用 UDP，大端字节序，机器人侧端口为 `60015`。通信周期通常为 `8ms`，部分机型支持 `4ms`。状态包输出可以在任意时间通过 start/stop 控制包启停，不要求已经进入 `IBGN start[*]`。

### 3.1 状态输出 start 包

外部设备发给机器人：

| 字段 | 长度 | 值 |
| --- | --- | --- |
| Packet type | 4B | `0` |
| Version | 4B | `1` |

当前实现：`FanucJ519Protocol.PackInitPacket()` 已按 8B 大端控制包实现，`FanucJ519Client.ConnectAsync()` 连接后立即发送。

### 3.2 状态包

机器人发给外部设备，长度为 `132B`：

| 偏移 | 字段 | 含义 |
| --- | --- | --- |
| `0x00` | Packet type | `0` |
| `0x04` | Version | `1` |
| `0x08` | Sequence No. | 状态包序号，发送 start 包后从 `1` 重新开始 |
| `0x0c` | Status | bit0 接受命令、bit1 已收到命令、bit2 SYSRDY、bit3 运动中 |
| `0x0d..0x12` | Read I/O 回显和值 | 回显命令包中的读取 IO 类型、索引、掩码，并返回 16 点 IO 值 |
| `0x14` | Timestamp | ms 单位，2ms 分辨率 |
| `0x18..0x38` | Cartesian / external axis | `X/Y/Z/W/P/R` 加 3 个扩展轴 |
| `0x3c..0x5c` | Joint | `J1..J9`，单位 degree |
| `0x60..0x80` | Motor current | `J1..J9` 电流，单位 A |

当前实现：`FanucJ519Protocol.ParseResponse()` 已解析 `132B` 状态包，并暴露 `AcceptsCommand`、`ReceivedCommand`、`SystemReady`、`RobotInMotion` 四个状态位。`FanucControllerRuntime.GetSnapshot()` 也会把最新 J519 状态写进快照。

### 3.3 命令包

外部设备发给机器人，长度为 `64B`：

| 偏移 | 字段 | 含义 |
| --- | --- | --- |
| `0x00` | Packet type | `1` |
| `0x04` | Version | `1` |
| `0x08` | Sequence No. | 第一包应等于刚收到的状态包序号，后续逐包递增 |
| `0x0c` | Last data | 正常为 `0`；结束外部控制时最后一包设为 `1` |
| `0x0d..0x11` | Reading I/O | 可读取最多 16 个连续 IO 点 |
| `0x12` | Data format | `0` 笛卡尔，`1` 关节 |
| `0x13..0x19` | Writing I/O | 可写入最多 16 个连续 IO 点 |
| `0x1a` | unused | 2B |
| `0x1c..0x3c` | target[9] | 9 个 f32 目标值；关节格式时单位 degree |

当前实现：`FanucJ519Protocol.PackCommandPacket()` 已按上述布局打包，默认 `dataStyle=1`，也就是关节格式。运行时会把规划输出的弧度制关节轨迹转换为 degree 后下发。

### 3.4 状态输出 stop 包

外部设备发给机器人：

| 字段 | 长度 | 值 |
| --- | --- | --- |
| Packet type | 4B | `2` |
| Version | 4B | `1` |

文档把它定义为“停止状态包输出”的控制包，不是命令流正常终止的首选动作。命令流结束应通过 command packet 的 `Last data=1` 表达。

当前实现：`FanucJ519Client.StopMotionAsync()` 当前会停止状态包驱动发送并发送 packet type `2`，而稠密轨迹执行期间保持 `LastData=0`。这是与 FANUC 文档最明显的语义差异之一；已有多数 UTTC 抓包显示主运行窗口 `LastData=0`，但 `../j519 协议.pcap` 中存在 1 个 `LastData=1` 后紧跟 packet type `2` 的样本，后续应单独校准停止语义。

## 4. 通信时序重点

文档推荐的时序是：

1. 外部设备发送状态输出 start 包。
2. 机器人每个通信周期输出状态包。
3. 机器人程序执行到 `IBGN start[*]` 后，状态包 bit0 变为 `1`，表示等待命令包。
4. 外部设备收到 bit0 为 `1` 的状态包后，立即发送第一帧命令包，第一帧命令序号应等于刚收到的状态包序号。
5. 后续每收到一个状态包，外部设备应立即发送下一帧命令包。
6. 结束命令通信时，发送 `Last data=1` 的最后一帧命令包。

当前实现对照：

1. `FanucJ519Client` 已改为收到机器人 132B status 包后立即回发当前命令，不再由上位机本地固定 8ms 发送循环主动发包。
2. 命令包 sequence 已按刚收到的 status packet sequence 写入，避免第一帧从本地 `0` 起步。
3. `FanucControllerRuntime.ExecuteTrajectory()` 启动前会检查已有 J519 响应中的 `AcceptsCommand` 和 `SystemReady`；但如果还没收到状态包，则会放行，后续命令仍要等第一帧 status 到达才会发出。
4. 当前稠密轨迹结束不发送 `LastData=1`，而是依赖停止 J519 状态包驱动发送和 packet type `2` stop 控制包。

序号和节拍已经按手册方向校准；停止语义仍需在真实 R30iB 联调中继续确认。

## 5. 命令缓冲

文档说明机器人可以缓冲提前到达的 command packet。默认启用，缓冲上限为 `$STMO.$PKT_STACK - 1`，`$PKT_STACK` 默认 `10`，可配置范围 `2..10`。`$STMO.$START_MOVE` 决定积累多少未处理命令包后开始运动，默认 `1`。

注意事项：

1. 只有 command packet 会进入缓冲。
2. status output stop packet 会立即处理。
3. 如果 command buffer 中还有未处理包，不应发送 status output stop packet。
4. 使用 `Last data=1` 时，机器人会先处理完缓冲里的命令包，再结束外部控制。

当前实现没有显式预填 `$PKT_STACK` 缓冲，也没有读取 `$START_MOVE`。`FanucJ519Client` 只保存一个“当前命令”，由后台循环持续发送；`FanucControllerRuntime.SendDenseTrajectory()` 另一个 8ms 循环负责按轨迹时间更新这条当前命令。这与文档的“按状态包响应并可提前发多包缓冲”模型不同。

## 6. 可执行运动条件

文档列出的主要运动约束：

1. 目标点必须可达。
2. 笛卡尔格式下目标点对应的关节解必须唯一，且 configuration 要与 `IBGN start` 开始时一致。
3. 各轴必须满足上下限。
4. 不能发生自碰撞。
5. 必须考虑 FANUC J3 轴定义：J3 不是相对 J2 臂的夹角，而是机器人视角下相对水平面的 J3 臂角度。
6. 外部设备必须控制每轴速度、加速度、jerk 不超过 `$STMO_GRP` 下的限制。
7. 状态包中的当前位置是 servo feedback position，不是 command position。轨迹起点应平滑连接到机器人 command position，而不能简单用当前 servo position 直接起步。
8. reducer load 超限也会导致停机，负载相关计算不公开。

当前实现对这些条件的覆盖：

| 条件 | 当前状态 |
| --- | --- |
| 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 |
| `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 |
| `speed_ratio` 下发时间轴缩放 | 已实现，规则为 `t_traj = k * 0.008 * speed_ratio` |
| IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` |
| 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 |
| J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 |
| command position 起步 | `MoveJoint` 会用当前运行时记录的关节作为起点生成 PTP 稠密轨迹；但没有通过 FANUC HMI 通信读取 command position |
| reducer load | 未建模，依赖保守规划和现场报警反馈 |
| 笛卡尔格式限制 | 运行时不走笛卡尔 J519 目标格式，暂不覆盖 configuration 变化报警 |

## 7. 系统变量

与本仓库后续最相关的变量：

| 变量 | 默认/含义 |
| --- | --- |
| `$STMO.$PHYS_PORT` | 物理口，`1=CD38A`，`2=CD38B` |
| `$STMO.$COM_INT` | 通信周期，单位 ms，通常 `8`，只读 |
| `$STMO.$PKT_STACK` | command buffer 最大保留量，默认 `10` |
| `$STMO.$START_MOVE` | 缓冲中积累多少未处理命令后开始运动，默认 `1` |
| `$STMO_GRP.$JNT_VEL_LIM[*]` | 各轴速度上限，degree/s，只读 |
| `$STMO_GRP.$JNT_ACC_LIM[*]` | 各轴加速度上限，degree/s^2，只读 |
| `$STMO_GRP.$JNT_JRK_LIM[*]` | 各轴 jerk 上限，degree/s^3，只读 |
| `$STMO_GRP.$LMT_MODE` | 加速度/jerk 限制计算模式 |
| `$STMO_GRP.$WARN_LIM` | 接近限制时报警阈值，默认 `80%` |
| `$STMO_GRP.$FLTR_LN` | 命令目标移动平均滤波窗口 |
| `$STMO_GRP.$MAX_SPD` | 用于限制计算的 flange center 最大速度 |

当前实现没有读取或设置上述系统变量。`RobotProfile.ServoPeriod` 当前决定运行时发送周期；对当前现场而言应继续确认它与 `$COM_INT` 一致。

## 8. 附录 B：加速度和 jerk 限制

文档说明，在 `$STMO_GRP[].$LMT_MODE=0` 时，加速度和 jerk 的允许上限会根据 flange center speed 与 payload 计算：

1. 以 `$MAX_SPD` 分成 20 档速度区间。
2. 每个轴、每种限制类型都有无负载和最大负载两张 20 档表。
3. 实际 payload 通过线性插值得到限制表。
4. 实际 flange center speed 在相邻速度档之间线性插值。
5. 限制值不是每个通信周期都更新，而是在超过 `Vmax/20` 到再次跌回阈值的整段时间内，以该段观测到的 `Vpeak` 决定。
6. 如果长时间不跌回阈值，会按中间检查时间做临时判断。

文档还提供了 packet type `3` 的限制表查询协议：

| 包 | 方向 | 重点字段 |
| --- | --- | --- |
| 请求 | 外部设备 -> 机器人 | packet type `3`、version `1`、axis `1..9`、limit type `0=velocity/1=acceleration/2=jerk` |
| 响应 | 机器人 -> 外部设备 | packet type `3`、version `1`、axis、limit type、`Vmax`、中间检查时间、无负载 20 档、最大负载 20 档 |

当前实现没有 packet type `3` 查询，也没有实现手册描述的动态限制表算法。现阶段规划时长和保守程度主要依赖 replacement 自身参数与现场抓包对齐。

## 9. 报警与诊断

文档中与实现最相关的报警：

| 报警 | 含义 |
| --- | --- |
| `MOTN-600` | 命令序号与机器人期望不一致 |
| `MOTN-602` | data format 非法 |
| `MOTN-603` | 后续命令包未在通信周期内到达 |
| `MOTN-604` | 命令包过多，超出缓冲 |
| `MOTN-605` | 目标位置包含 NaN 或 infinity |
| `MOTN-606` | 非 AUTO 或 override 不是 100% |
| `MOTN-607` | 协议版本不匹配 |
| `MOTN-609/610/611` | 速度、加速度、jerk 超限 |
| `MOTN-612/613/614` | 接近速度、加速度、jerk 限制 |
| `MOTN-617` | 目标点与当前位置不连续 |
| `MOTN-619` | 当前机型不支持笛卡尔目标格式 |
| `PRIO-023` | 读写的 IO 类型或索引未分配 |

当前 `ControllerStateSnapshot.ActiveAlarms` 仍为空，Web 状态页也尚未接入 FANUC 报警列表。后续现场联调如果出现报警，应优先按上述表格关联 J519 包序号、目标数据、IO 字段、发送间隔和状态包 bit。

## 10. 与当前代码的结论

已基本对齐：

1. UDP 60015、大端、start/stop 控制包、64B command packet、132B status packet 的基础二进制布局。
2. `Data format=1` 的关节目标下发。
3. 状态位 bit0..bit3 的解析和快照暴露。
4. 规划输出 `rad` 转 J519 `deg`。
5. 根据 `speed_ratio` 做运行期时间轴缩放，而不是改变规划文件时间。
6. 飞拍 IO 触发通过 command packet 的写 IO 字段下发。
7. 命令发送按机器人 UDP status 包驱动，并使用最新 status sequence 回发。

主要差异/风险：

1. 当前未实现命令缓冲预填，也未读取 `$PKT_STACK / $START_MOVE`。
2. 当前停止运动依赖 packet type `2` stop 控制包，没有稳定发送 `LastData=1` 的最后 command packet；这与手册标准结束语义不同。
3. 当前未实现 packet type `3` 的速度/加速度/jerk 限制表查询，也未实现 payload/speed 20 档动态限制算法。
4. 当前没有自动校验 `AUTO / 100% OVERRIDE / brake control / resume offset / payload` 等控制器前置状态。
5. 当前没有报警码读取和 `MOTN-* / PRIO-*` 映射。

建议后续联调优先级：

1. 验证运动结束是否必须补 `LastData=1`；如果当前 stop 控制包能稳定工作，也应在文档中标为现场兼容路径，而不是手册标准路径。
2. 抓一次报警现场包，确认 `MOTN-600/603/617` 等是否能从包序号与状态位直接定位。
3. 如果后续追求更稳的真实机运行，补 packet type `3` 限制表查询，并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。