我们将 RapidCode API 和 RMP 与 ROS 2 control 集成，控制一台华沿机器人（Huayan Robotics）Elfin5 协作机器人（6 轴，5 kg 负载）进行绘图。绘图分为两种形式：
- 笛卡尔配方
- 实时绘图（含不落笔的“跟随”模式）
笛卡尔配方由 Wikimedia Commons 上的 SVG 文件转换而来：将图形转换为直线、圆弧和曲线，每段由按可配置间隔采样的点组成。实时绘图则是即时生成的——Elfin5 机器人在绘图平面上模仿用户的笔画（用户可使用鼠标光标或数位板绘制）。在实时绘图过程中，用户还可以命令机器人“跟随”鼠标光标。跟随时，机器人沿最短路径移向光标当前位置（该位置对应笛卡尔空间中的一个位姿），用户因此可以方便地重新定位机械臂，而不会画出多余的笔画。
为实现上述功能，我们在 ROS 2 control 节点中实现了两个主要组件：
- 直通轨迹控制器（passthrough trajectory controller）
- RapidCode System Hardware
直通轨迹控制器
直通轨迹控制器的功能与 ROS 2 自带的 Joint Trajectory Controller（JTC）类似，命令运动的接口也相近。它通过 action ~/follow_joint_trajectory 接收轨迹目标，也可以通过话题 ~/joint_trajectory 接收连续的关节轨迹消息流，例如 MoveIt Servo 提供的消息。不同之处在于，JTC 在每个控制周期只从已存储的轨迹中采样并发送一个点，而直通轨迹控制器按 RMP 固件的采样率对轨迹进行采样，再将采样后的轨迹分成若干“块”发送给 RapidCode System Hardware。直通轨迹控制器还提供一些附加服务，例如 ~/stop（停止机器人运动并丢弃所有排队的目标），以及 ~/reset_fault（清除 RapidCode 端锁存的故障）。
直通轨迹控制器本身不做任何运动学计算。它接收的关节轨迹通常来自 MoveIt 的标准组件：/compute_cartesian_path 服务为笔画中的每个点求解逆运动学；Pilz 在关节空间中规划轨迹，将机器人移动到就绪位置；MoveIt Servo 为“跟随”模式提供实时关节数据流。
RapidCode System Hardware
RapidCode System Hardware 使用 RapidCode API 配置机器人的驱动器、命令其运动并读取其状态。它负责初始化 RMP 固件，同时配置并持有 RapidCode 对象。RMP 固件是一个周期性收发 EtherCAT 数据包的进程，这些数据包包含物理驱动器的命令和状态信息。RapidCode System Hardware 从直通轨迹控制器接收轨迹点形式的命令（包含时间、位置和速度），再将其送入 RapidCode 的流式调用 MovePVT()。用户可以通过 RapidCode System Hardware 的状态接口，查看流式运动的执行进度、当前正在执行的轨迹块以及 RMP 本身的状态。如果想在没有实体机器人的情况下测试运动，还可以使用 RapidCode 虚拟轴（phantom axes）来控制一台虚拟机器人。此时 RapidCode System Hardware 状态接口的上报方式保持不变，因此 RViz 等可视化工具依然能准确显示机器人的位姿。
为什么不使用 ROS 2 自带的 Joint Trajectory Controller？
RapidCode System Hardware 可以直接配合 ROS 2 自带的 JTC 使用，但我们发现这种方式更容易出错：JTC 默认的实时配置不如 RMP 严格，会导致运动不平顺，或触发“out of frames”错误使机器人完全停止。如果让 JTC 的循环周期与 RMP 的采样率一致，并在每个周期向 RapidCode API 的 MovePVT() 发送一个点，系统就会受制于 ROS 2 Controller Manager 的抖动，而且两者的职责也不必要地重叠。RMP 本身就能将通过 MovePVT() 提供的运动帧排队，并在每个采样周期向驱动器发送正确的位置，因此控制器没有必要把轨迹点一直保留到即将执行的时刻才发送。不过在“跟随”模式下，直通轨迹控制器只会下发有限数量的运动帧，以便用户取消运动或随时改变方向。
将用户界面与 MoveIt 连接
除了用于通过 RapidCode 和 ROS 2 命令运动的两个核心组件之外，我们还编写了一些辅助 Python 脚本，负责把笛卡尔配方和实时绘图依次传递给 MoveIt 和直通轨迹控制器。用户主要通过绘图面客户端（surface client）这个 Python 程序进行操作，它可以在未安装 ROS 2 的系统上运行，用户可以在其中创建配方、加载已有配方或进行实时绘图。该客户端连接到一个运行在 ROS 2 系统上的通信桥，并把用户命令的所有机器人位姿发送给它。通信桥再根据运动类型，将这些位姿分派给相应的 MoveIt 组件。最常见的情况是，由配方或实时绘图中的目标笛卡尔位姿组成的路径会被拆分为短小且首尾相连的片段，每个片段交给 MoveIt 的 /compute_cartesian_path 求解逆运动学。求解得到的关节轨迹经过时间和速度调整以保持连续，然后发送给直通轨迹控制器。将轨迹拆分后，系统可以在已规划的运动执行期间并行规划后续运动。