IB_Robot 中 SO-101 机械臂硬件驱动包深度指南:C++ ros2_control 插件与 Python 工具链实战

原创2026-10-14 14:17:091,444 阅读
文章标签:人工智能具身智能机器人AI Agent模型推理服务多模态计算机视觉语音本地部署

IB_Robot 中 SO-101 机械臂硬件驱动包深度指南:C++ ros2_control 插件与 Python 工具链实战

本文全面剖析 openEuler Embedded 具身智能机器人框架 IB_Robot 中 so101_hardware 硬件驱动包:它通过 C++ ros2_control 插件直连 Feetech 舵机总线完成高实时控制,同时以 Python 工具链支撑标定、示教数据采集与诊断。读完本文,你将掌握该包的编译安装、URDF 插件配置、标定与数据迁移流程、激活回滚与初始同步读等 fail-closed 安全机制,并能在真机上独立完成从标定到控制回路运行的完整实战。

一、包定位:一条硬件、两条技术路线

so101_hardware 位于仓库 src/robots/so101/so101_hardware,是 SO-101 机械臂的完整硬件驱动解决方案,支持两种互补的工作模式:

  • C++ ros2_control 插件(SO101SystemHardware):通过 FTServo SDK 与 Feetech 舵机直接通信,延迟低、性能高,面向生产控制、强化学习与轨迹执行场景;
  • Python 工具集:覆盖标定(calibrate_arm)、示教/数据采集(Leader 臂发布器)与诊断(电机测试、标定迁移/核验),面向开发调试场景。

两者的关系可用包内 README 给出的架构图概括:

ros2_control (Controller Manager)
      ↓
SO101SystemHardware (C++ Plugin)  ←──┐
      ↓                              │
FTServo SDK (C++)                    │
      ↓                              │
Feetech Servos (Hardware)            │
      ↑                              │
Python Utilities (Scripts) ──────────┘

插件经 pluginlib 以 hardware_interface::SystemInterface 基类导出,注册声明见 so101_hardware_plugin.xml:class 名为 so101_hardware/SO101SystemHardware,C++ 类型为 so101_hardware::SO101SystemHardware。从源码结构看,该插件是一个"薄适配层":所有协议处理、标定加载/校验、激活回滚与单位换算都封装在 so101_sdk 与 feetech_sdk 中,插件只负责把 ros2_control 的接口缓冲映射到 SDK 的命名关节 API,并发布关节电流——这一点在头文件 so101_system_hardware.hpp 的注释中有明确说明。

二、依赖与混合编译

2.1 依赖清单

  • Git 子模块:FTServo_Linux SDK 以 git submodule 引入,首次拉取后需执行:
git submodule update --init --recursive
  • 系统依赖(见 package.xml):
    • ROS 2 Humble(hardware_interface、ros2_control、ros2_controllers、pluginlib、rclcpp、rclcpp_lifecycle)
    • nlohmann_json 库
    • ibrobot_msgs(电流消息定义)、feetech_sdk、so101_sdk(C++ 依赖)
    • pyserial(Python 驱动)、rclpy、sensor_msgs

2.2 编译步骤

cd ~/Research/lerobot_ros2/src/ros2/ros2_ws
source /opt/ros/humble/setup.zsh
# 确保混合编译成功:显式设置 PYTHONPATH
PYTHONPATH=/usr/lib/python3/dist-packages:$PYTHONPATH colcon build --packages-select so101_hardware
source install/setup.zsh

本包采用 ament_cmake_python 混合构建:C++ 插件编译为共享库 so101_hardware_interface,Python 脚本则以无 .py 后缀的形式安装到 lib/so101_hardware(见 CMakeLists.txt 中的 install(PROGRAMS ... RENAME ...) 段落),从而保证 ros2 run so101_hardware <工具名> 可正常解析。目前安装的脚本包括 calibrate_arm、arm_calibration_transfer、arm_calibration_checker、leader_arm_pub、motor_test、motor_bridge、read_motor_steps。

三、机械臂标定:首次上电前的必经步骤

首次使用前必须执行标定,生成 ~/.calibrate/ 下的 JSON 文件。标定工具同时支持 Leader 臂与 Follower 臂:

# 校准 Follower 臂
ros2 run so101_hardware calibrate_arm --arm follower --port /dev/ttyACM0
# 校准 Leader 臂(默认端口为 /dev/ttyACM1)
ros2 run so101_hardware calibrate_arm --arm leader --port /dev/ttyACM1
# 自定义标定文件输出路径
ros2 run so101_hardware calibrate_arm --arm follower --port /dev/ttyACM0 --calib-file /path/to/custom.json

参数说明(来自 calibrate_arm.py):

参数 默认值 说明
--arm follower 可选 leader 或 follower
--port follower 用 /dev/ttyACM0,leader 用 /dev/ttyACM1 串口设备
--calib-file 见下文常量 覆盖标定 JSON 输出路径

3.1 标定交互流程

标定的核心逻辑在 interactive.py 的 run_interactive_calibration 中,是一个需要人工介入的两步过程:

  1. 关闭全部舵机力矩(disable_torque),并把每个电机写入位置模式(Operating_Mode = POSITION);
  2. 提示操作者把机械臂移动到量程中位,回车后记录各关节的 homing_offset(零位偏移);
  3. 提示操作者将除 5 号电机外的所有关节走满全程,回车停止记录,得到各关节的 range_min / range_max;
  4. 5 号电机是 wrist_roll 全回转关节,直接取满量程 range_min = 0、range_max = 4095;
  5. 组装每个关节的 id / drive_mode / homing_offset / range_min / range_max,先写入舵机固件(bus.write_calibration),再保存为 JSON 文件(save_calibration)。

标定数据落盘结构示例(以关节键 "1"~"6" 为索引):

{
  "1": {
    "id": 1,
    "drive_mode": 0,
    "homing_offset": 2048,
    "range_min": 0,
    "range_max": 4095
  }
}

3.2 共享常量

标定相关的默认常量集中在 constants.py,Python 侧可按需导入:

from so101_hardware.calibration.constants import MOTOR_IDS, JOINT_NAMES, DEFAULT_SERIAL_PORT

其中值得关注的常量:

  • CALIB_DIR = ~/.calibrate;Leader 标定文件 so101_leader_calibrate.json,Follower 标定文件 so101_follower_calibrate.json;
  • MOTOR_IDS = [1..6],JOINT_NAMES = ["joint_1".."joint_6"],FULL_TURN_MOTOR_ID = 5;
  • 步数范围 MIN_STEP = 0、MAX_STEP = 4095;
  • 默认串口 /dev/ttyACM0;
  • 默认发布频率:Leader 50 Hz、控制 100 Hz;
  • 电流换算常量 CURRENT_RAW_TO_AMPERE = 0.0065(即 STS3215 的 1 LSB = 6.5 mA)。

配套的 YAML 模板 calibration_params.yaml 记录了同样的电机模型配置(1~6 号均为 sts3215,5 号标记 full_turn: true)。

四、C++ ros2_control 插件:URDF 配置与参数全解

在 URDF 的 <ros2_control> 段中,通过 <hardware> 声明插件并传入参数:

<ros2_control name="so101_system" type="system">
  <hardware>
    <plugin>so101_hardware/SO101SystemHardware</plugin>
    <param name="port">/dev/ttyACM0</param>
    <param name="calib_file">$(env HOME)/.calibrate/so101_follower_calibrate.json</param>
    <!-- 可选:启动时的安全姿态(JSON 格式,单位为弧度) -->
    <param name="reset_positions">{"1": 0.0, "2": 0.0}</param>
  </hardware>
  <joint name="1">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <param name="id">1</param>
  </joint>
  <!-- 关节 2..6 类似;关节 6(夹爪)可加 -->
  <joint name="6">
    ...
    <param name="gripper_opening_direction">1</param>
  </joint>
</ros2_control>

4.1 硬件参数详解

插件在 on_init 中解析这些参数并构造 SDK 配置,相关实现见 so101_system_hardware.cpp:

参数 默认值 说明
port /dev/ttyACM0 舵机总线串口
calib_file 空 标定 JSON 路径,缺失时加载报错
reset_positions 不配置 JSON 对象,键为关节名、值为弧度;仅在配置时生效
update_rate 100.0 控制频率(Hz),必须是有限正数,用于推导写重试预算
simulated false 置 true 时使用 SDK 的 fake bus 代替串口,可在无硬件环境下跑完整控制器栈(运行时一致性验证)
protection_timeout 1(秒) 夹爪过载保护的事件升级期限,必须是有限正数

关节级参数(<joint> 内):id(电机 ID,必须为纯数字,SO-101 中默认关节名即电机 ID)与 gripper_opening_direction(1 或 -1,标记夹爪的张开方向)。

从源码可以看到几个关键校验与派生行为:

  • 每个关节必须恰好声明 1 个 position 命令接口,以及 position + velocity 两个状态接口,否则 on_init 直接报错;
  • update_rate 决定写重试预算:write_retry_budget_ = 0.4 / update_rate(WRITE_RETRY_BUDGET_RATIO = 0.4,见头文件常量),即写操作在一帧周期内最多占用 40% 时间用于重试;
  • reset_positions 支持 "''" / "\"\"" 空串转义以显式禁用。

4.2 真机 URDF 参考

仓库中 SO-101 的真机 xacro 模板 so101_ros2_control.xacro 即按上述结构声明硬件参数(use_sim=false 分支);双臂版 so101_dual.urdf.xacro 则为左右臂分别实例化插件并传入各自的 port、calib_file、reset_positions。

4.3 通过 robot_config 驱动

在 IB_Robot 的 robot_config 层(YAML 单一事实来源),硬件参数以结构化形式配置,例如 robot_config README 中的示例:

robot:
  name: so101_single_arm
  type: so101
  ros2_control:
    hardware_plugin: so101_hardware/SO101SystemHardware
    port: /dev/ttyACM0
    calib_file: $(env HOME)/.calibrate/so101_follower_calibrate.json
    reset_positions:
      "1": 0.0813
      "2": 3.7905

对应的运行时配置可参考 so101_single_arm.yaml。控制器层则由 so101_controllers.yaml 定义:位置控制器(arm_position_controller 管理关节 1-5、gripper_position_controller 管理关节 6)、轨迹控制器(MoveIt 模式)以及共用的 joint_state_broadcaster,controller_manager 的 update_rate 为 100 Hz;双臂场景使用 so101_dual_controllers.yaml 中的左右臂控制器对。

五、Leader 臂数据采集与示教

leader_arm_pub 用于把 Leader 臂的关节状态发布为话题,供示教数据录制或遥操作消费:

ros2 run so101_hardware leader_arm_pub --port /dev/ttyACM0 --publish_rate 50.0

该节点(leader_arm_pub.py)可通过 ROS 参数完整配置:port、baudrate(默认 1000000)、calibration_file、calibration_version(必须显式置 1 以确认已完成标定 provision)、joint_order(默认 1~6)、gripper_joint(默认 6)、source_topic(默认 /inputs/so101_leader/state)、publish_rate(默认 50.0)。发布的消息头 frame_id 固定为 leader_radians_gripper_ratio_v1,即非夹爪关节的单位是弧度,夹爪关节的单位是开合比例。

leader_arm_pub.launch.py 提供 launch 包装,要求显式传入 calibration_version:=1 才会启动节点。底层采集在 leader_input.py 中有完善的健康防护:

  • 标定文件缺失直接抛错,提示先执行 calibrate_arm provision;
  • 读取结果必须覆盖全部关节名、所有值有限(finite),否则丢弃本帧;
  • 角度传感器故障位(protection 位 0x02)会直接判帧无效——即使位置值解码为有限数,这种姿态也绝不进入遥操作;
  • 夹爪开合比例越界(不在 0.0~1.0)同样丢弃。

Leader 臂配置模板见 leader_arm.yaml(端口、50 Hz 发布率、标定文件、so101_leader 命名空间)。

六、关节电流反馈:从舵机原值到 observation.current

插件在 read() 周期内通过 publish_currents() 发布电流(so101_system_hardware.cpp),话题为 /so101_follower/joint_currents(Follower)或 /so101_leader/joint_currents(Leader),消息类型为 ibrobot_msgs/msg/JointCurrent,字段包括带时间戳的 header、关节名数组 name 与电流数组 current(单位安培)。该话题供数据集转换导出 observation.current。

换算要点:

  • 舵机上报的是原始 Present_Current 寄存器值,按 STS3215 规格 1 LSB = 6.5 mA 换算为安培;
  • LeRobot SDK 并不解码该寄存器(Present_Current 未出现在 STS_SMS_SERIES 编码表中),因此仓库在 motor_current.py 中自行处理 bit 15 符号位(sign-magnitude 编码)后再乘换算系数;
  • Python 侧 motor_bridge 亦复用 read_motor_currents,读取失败时按 0.0 兜底并以 5 秒节流告警。

七、实现细节:坐标转换、启动位置保护与生命周期

7.1 步数与弧度互转

插件内部自动完成设备刻度与弧度的转换,公式在 so101_system_hardware.cpp 的注释中明确给出(range = range_max - range_min):

  • 读取(设备→弧度):radians = ((steps - range_min) / range - 0.5) * 2.0 * PI
  • 写入(弧度→设备):steps = (radians / (2.0 * PI) + 0.5) * range + range_min

标定文件中的 range_min/range_max 正是这一换算的边界依据。

7.2 启动位置保护(reset_positions)

reset_positions 用于指定启动时的初始关节位置,其行为对移动平台(如机器狗背负式机械臂)至关重要:

  • 配置了该参数:激活时机械臂平滑移动到指定姿态;
  • 未配置(默认):机械臂保持当前舵机位置,不发生任何动作,从而避免启动瞬间回零导致的剧烈跳动。

7.3 生命周期管理

插件完整实现标准 ros2_control 生命周期状态机:

回调 行为
on_init 解析 URDF 硬件参数并构造 so101::Arm,初始化接口缓冲;参数非法返回 ERROR
on_configure 加载标定文件、按标定构造 feetech::Bus 并打开串口、把臂挂到共享总线上
on_activate 总线 ping 探活 → 预读反馈 → arm_->activate()(内部含初始同步读)→ 发布电流话题
on_deactivate 只释放力矩(Torque Off),保持总线打开,便于 inactive→active 快速重入
on_cleanup / on_shutdown / on_error 关闭总线、复位发布器,并尽力释放力矩

读写回路还有两套容错机制(见头文件 so101_system_hardware.hpp 常量):

  • 读失败:短暂失败保持上一帧数值并节流告警;连续失败达到 FAILURE_TOLERANCE = 200 ms 后上报 ERROR;
  • 写失败:每帧最多占用 0.4 × 控制周期 的预算进行 500 µs 间隔重试,持续失败超 200 ms 上报 ERROR。

同步读的超时采用 LeRobot Feetech 的延迟余量(报文传输时间 + 3 字节时间 + 50 ms),当前 6 电机、1 Mbps、每电机 15 字节反馈对应约 51.29 ms,向上取整为 52 ms;每周期只尝试一次同步读(等价 LeRobot 默认 num_retry=0),不因单帧超 10 ms 就误判失败。

八、激活回滚与初始同步读:fail-closed 安全机制

8.1 激活回滚(Activation Rollback)

on_activate 在配置舵机过程中失败时,执行严格 fail-closed 的回滚流程(原 README 中记为 detail::rollback_activation,按头文件注释其实现已下沉至 feetech_sdk 的 test_rollback.cpp 覆盖):

  1. 先卸力矩:对所有电机执行 EnableTorque(id, 0)(带重试)。这是安全默认动作,与 EPROM 状态无关;卸力矩必须优先于 relock,因为部分 Feetech 舵机只在力矩关闭时才接受 EPROM 锁定指令;
  2. 再 relock EPROM:仅对回滚开始时仍处于解锁状态的电机(unlocked_motors 集合)尝试 LockEprom(id)(带重试),并在关闭串口之前完成。正常流程中每完成一个电机的配置即从解锁集合移除,因此只有中途失败残留的电机才会进入 relock;
  3. 最后关闭串口:sms_sts_.end()。

两类失败结果独立汇报、互不掩盖:

  • 力矩卸载失败 → Failed to disable torque for one or more motors during activation abort;
  • EPROM relock 失败 → Failed to relock EPROM for N motor(s) during activation abort; persistent parameters may be unprotected,并在 relock_failures 中列出具体电机 ID。

即使力矩卸载失败,relock 仍会执行(均为 best-effort),调用方据此判断持久参数是否需要人工复位。

8.2 初始同步读(Initial Sync Read)

使能力矩之后,on_activate 会执行一轮 sync-read,把真实反馈写入 hw_commands_/hw_positions_/hw_velocities_/hw_currents_。这一步同样受激活回滚保护(原实现 detail::perform_initial_sync_feedback,按头文件注释已下沉至 so101_sdk 的 Arm::activate),设有两道门禁:

  1. 发送/总线应答(syncReadPacketTx):返回收到 SDK 缓冲区的字节数,<= 0 表示发送失败或无应答(超时)。注意 syncReadBegin 只返回 void(分配接收缓冲区、记录超时),不是门禁,真正的门禁是 Tx 返回值;
  2. 每电机返回包(syncReadPacketRx):返回内存字节数表示成功、0 表示失败。必须全部电机都返回完整且 CRC 通过的包,才会初始化状态并解除回滚守卫返回 SUCCESS。

任一门禁失败即调用 abort_activation()(力矩 off / EPROM relock / 关闭串口),绝不在状态未完整初始化的前提下放行。对应失败日志:

  • 发送/总线失败 → Initial sync read transmit failed; aborting activation;
  • 某电机 Rx 失败 → Initial sync read for motor ID <id> failed; aborting activation。

该 helper 以回调形式注入 Tx/Rx 操作,因此可在无真机条件下用 gtest 覆盖 Tx 失败、单电机 Rx 失败与全成功三条路径——这是 test_so101_hardware.cpp 之外、SDK 侧测试(如 ActivationAbortLeavesSafeState、ActivationWithResetPositionsSeedsConfiguredCommands)的覆盖重点。

九、标定迁移与真机核验工具

包内提供两个经 ros2 run 调用的标定周边工具,配套文档位于 docs/tools/arm_calibration_transfer.md 与 docs/tools/arm_calibration_checker.md。

9.1 arm_calibration_transfer:旧标定数据迁移

当旧数据/旧策略是按旧机械臂的标定参数录制或训练、而新机械臂已按当前版本完成标定时,该工具把旧文件的 homing_offset、drive_mode 映射到当前标定文件格式,生成可直接挂接的新校准文件。输入目录约定:old_dir_path 放旧文件(形如 <old_arm_id>.json),new_dir_path 放新模板(形如 <new_arm_id>.json),输出为 new_dir_path/finetune_<new_arm_id>.json。

单组映射:

cd /path/to/IB_Robot
source .shrc_local
ros2 run so101_hardware arm_calibration_transfer \
  --arms_name_list='[["single_follower_arm","legacy_follower_arm"]]' \
  --new_dir_path /tmp/calibration/new \
  --old_dir_path /tmp/calibration/old

多组映射只需扩展 JSON 列表:

ros2 run so101_hardware arm_calibration_transfer \
  --arms_name_list='[["left_follower","legacy_left"],["right_follower","legacy_right"]]' \
  --new_dir_path /data/new_calibration \
  --old_dir_path /data/old_calibration

迁移结果接入机器人配置有两种方式:直接替换默认文件 ~/.calibrate/so101_follower_calibrate.json,或在 robot_config 中显式指定 calib_file 指向新文件(如 src/robot_config/config/robots/so101_single_arm.yaml)。注意:工具只负责生成新标定文件,不会自动改写机器人配置。

9.2 arm_calibration_checker:真机标定核验

该工具按固定顺序驱动 Follower 臂到一组预设姿态,用于确认标定可信度与装配漂移:

  1. 30° 测量动作 → 2. 60° 测量动作 → 3. 90° 测量动作 → 4. 回到 0 位 → 5. wrist_roll 转到"镜头在正下方" → 6. wrist_roll 转到"镜头在正上方" → 7. 夹爪完全打开 → 8. 回到初始位置 → 9. 完成收臂动作
cd /path/to/IB_Robot
source .shrc_local
ros2 run so101_hardware arm_calibration_checker \
  --port /dev/ttyACM0 \
  --calib-file ~/.calibrate/so101_follower_calibrate.json

工具兼容旧参数风格(--robot.type so101_follower、--robot.port、--robot.id、--robot.calibration_dir),并同时兼容两类标定文件格式:IB_Robot 当前格式(关节键 "1"~"6")与旧 LeRobot 格式(关节键 shoulder_pan 等语义名)。

9.3 推荐工作流

1. calibrate_arm            生成基础标定
2. arm_calibration_transfer 复用旧标定逻辑时迁移数据
3. arm_calibration_checker  真机人工核验(30°/60°/90° 三角尺、腕部翻转、夹爪开合)

核验时重点关注:关节 1-4 的角度到达是否与三角尺一致、是否存在系统性偏差;关节 5 翻转正上/正下方时相机是否倾斜、视觉中心是否"画圈";关节 6 完全打开时是否达到最大开口、舵机是否堵转异响。

十、C++ 插件 vs Python 工具:选型对比

特性 C++ 插件(生产) Python 工具(开发/标定)
延迟 极低(FTServo SDK 直连) 较高(Python 开销)
性能 高(实时性好) 中等
模式 ros2_control 硬件接口 话题桥接/脚本
用途 强化学习 / 轨迹执行 标定 / 示教记录 / 诊断

从源码结构看,两者的分工非常清晰:插件走 SO101SystemHardware → so101_sdk/feetech_sdk → Feetech 舵机 的实时链路;Python 侧则包括 calibrate_arm(标定)、leader_arm_pub(示教采集)、motor_bridge(话题式 ros2_control 桥接)、motor_test(舵机诊断)与 read_motor_steps(步数读取)等工具,共同构成开发调试闭环。

十一、故障排除

现象 处理
串口无权限 sudo chmod 666 /dev/ttyACM0,或把当前用户加入 dialout 组:sudo usermod -a -G dialout $USER
报 Calibration file not found 先运行 ros2 run so101_hardware calibrate_arm 生成标定文件
子模块为空 确认已执行 git submodule update --init --recursive
迁移提示找不到模板/旧文件 检查 new_dir_path/<new_arm_id>.json 与 old_dir_path/<old_arm_id>.json 是否存在
迁移结果非法 检查旧文件完整性、新模板字段是否齐全、迁移后是否超出 0..4095 合法范围,回头修正源文件而非强行使用输出
迁移完成但动作仍不对 确认运行时实际加载的是新标定文件;新旧臂装配差异过大时单纯范围重映射可能不足,建议用 arm_calibration_checker 做物理核验
真机动作错乱 优先检查:串口是否接对臂、加载的标定文件是否对应当前机械臂、关节装配是否发生机械偏移

十二、小结

so101_hardware 以"一条硬件、双路线"的设计覆盖了 SO-101 机械臂从标定、示教采集到实时控制的完整闭环:C++ 插件通过 ros2_control 与 FTServo SDK 直连舵机,以生命周期状态机、200 ms 容错、0.4 × 周期 写重试预算和 fail-closed 激活回滚保障生产控制安全;Python 工具链则提供了可交互的标定、旧数据迁移、真机核验与电流/状态采集能力。对希望在本仓库中复现 SO-101 控制链路或扩展机械臂驱动的开发者,建议从 README.en.md 入手,对照 插件实现 与 测试用例 逐步深入。

登录后查看全文
IB_Robot