xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
dev
FlyShotHost/docs/fanuc-stream-motion-manual-notes.md
yunxiao.zhu ac41d04a54 ✨ feat(server): 添加服务启动自动初始化机器人功能 
* 在 ControllerClientCompatOptions 中新增 AutoInitOptions 配置模型
* 创建 AutoInitRobotHostedService 实现启动时自动初始化链路
* 在 appsettings.json 的 ControllerClientCompat.AutoInit 节配置
  RobotName 和 RobotIp
* 启动时依次执行 SetUpRobot → SetActiveController → Connect →
  EnableRobot；配置缺失或连接失败不阻止服务启动
* 更新 README.md 和现场工作流文档
2026-06-22 13:29:04 +08:00

14 KiB
Raw Permalink Blame History

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 作为禁用、异常中断或断开前的强制停止路径。正常稠密运动结束由内部开关 EndCommandStreamAfterQueueDrained 控制，默认会在队列末尾追加一帧 LastData=1 的结束命令；该帧发出后客户端进入仅监控状态，继续接收 J519 状态包用于位置监控，不再持续回发保姿命令。关闭该开关时保留旧行为：队列结束后继续保持最后目标或反馈位置。LastData 是单帧 command packet 字段，不是全局状态；下一轮运动重新装载的新队列会从 LastData=0 的普通命令帧开始。

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 结束帧；结束后只监控状态，不发送 packet type 2。旧保持路径仍可通过内部开关回退。

序号和节拍已经按手册方向校准；停止语义仍需在真实 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 下发时间轴缩放 已实现，J519 实发时间为 t_send = k * 0.008，原轨迹采样时间为 t_traj = t_send * 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. 默认正常运动结束已经通过追加 LastData=1 结束帧靠近手册语义；packet type 2 仍仅作为强制停止状态输出路径。
  3. 当前未实现 packet type 3 的速度/加速度/jerk 限制表查询，也未实现 payload/speed 20 档动态限制算法。
  4. 当前没有自动校验 AUTO / 100% OVERRIDE / brake control / resume offset / payload 等控制器前置状态。
  5. 当前没有报警码读取和 MOTN-* / PRIO-* 映射。

建议后续联调优先级：

  1. 联调验证 LastData=1 后机器人是否稳定退出命令消费并继续输出状态包；如现场程序仍要求持续命令包，可通过内部开关回退旧保持路径。
  2. 抓一次报警现场包，确认 MOTN-600/603/617 等是否能从包序号与状态位直接定位。
  3. 如果后续追求更稳的真实机运行，补 packet type 3 限制表查询，并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。