IB_Robot 中 SO-101 机械臂硬件驱动包深度指南:C++ ros2_control 插件与 Python 工具链实战
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
- ROS 2 Humble(
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 中,是一个需要人工介入的两步过程:
- 关闭全部舵机力矩(
disable_torque),并把每个电机写入位置模式(Operating_Mode = POSITION); - 提示操作者把机械臂移动到量程中位,回车后记录各关节的
homing_offset(零位偏移); - 提示操作者将除 5 号电机外的所有关节走满全程,回车停止记录,得到各关节的
range_min/range_max; - 5 号电机是
wrist_roll全回转关节,直接取满量程range_min = 0、range_max = 4095; - 组装每个关节的
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_armprovision; - 读取结果必须覆盖全部关节名、所有值有限(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 覆盖):
- 先卸力矩:对所有电机执行
EnableTorque(id, 0)(带重试)。这是安全默认动作,与 EPROM 状态无关;卸力矩必须优先于 relock,因为部分 Feetech 舵机只在力矩关闭时才接受 EPROM 锁定指令; - 再 relock EPROM:仅对回滚开始时仍处于解锁状态的电机(
unlocked_motors集合)尝试LockEprom(id)(带重试),并在关闭串口之前完成。正常流程中每完成一个电机的配置即从解锁集合移除,因此只有中途失败残留的电机才会进入 relock; - 最后关闭串口:
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),设有两道门禁:
- 发送/总线应答(
syncReadPacketTx):返回收到 SDK 缓冲区的字节数,<= 0表示发送失败或无应答(超时)。注意syncReadBegin只返回void(分配接收缓冲区、记录超时),不是门禁,真正的门禁是 Tx 返回值; - 每电机返回包(
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 臂到一组预设姿态,用于确认标定可信度与装配漂移:
- 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 入手,对照 插件实现 与 测试用例 逐步深入。