⚙️ ros2_control 实战深挖:控制器与硬件接口

上一页 MoveIt2 负责「算出轨迹」,本页回答「轨迹到底怎么驱动电机」—— 深入 ros2_control 的 controller_manager / hardware_interface / controller 三层架构,逐个吃透常用控制器配置,手写一个自定义硬件接口插件,并把 Unitree、CyberGear、自研 Hdrive 等真实关节模组接入,最后打通 MoveIt2 → FollowJointTrajectory → 轨迹控制器的完整执行链路。
ros2_control controller_manager hardware_interface 轨迹控制器 关节模组 实时控制 力控阻抗 CAN/EtherCAT
🎯 本页学习目标
1. 能画出 ros2_control 的三层架构图(controller_manager / controller / hardware_interface),说清各自职责与数据流向,并读懂 URDF 里的 ros2_control 描述块
2. 能用 ros2 control 命令行完成控制器的加载 / 配置 / 激活 / 切换 / 卸载,理解生命周期(lifecycle)节点与 switch_controller 的切换时序
3. 能独立写出 joint_state_broadcaster / joint_trajectory_controller / joint_group_effort_controller / forward_command_controller 的完整 yaml 配置,并用 list_controllers 验证状态
4. 能说明 SystemInterfaceActuatorInterface 的区别,并手写一个「读编码器、写 PWM/力矩」的硬件接口插件,掌握 pluginlib 注册与 CMakeLists/package.xml 要点
5. 能讲清真实关节模组(CAN/EtherCAT → 硬件接口)的适配思路,并会写模拟硬件(test 模式)用于离线联调
6. 能说清 MoveIt2 → FollowJointTrajectory action → 轨迹控制器的全链路,以及位置/速度/力矩模式切换与阻抗/力控的 ros2_control 实现思路
建议用时:90-120 分钟(含动手)。前置:建议先读本站 04-10 URDF 与 MoveIt2(已讲 ros2_control 骨架与 joint_state_broadcaster)与 04-13 MoveIt2 进阶(已讲规划与真机集成概览),本页是执行层的深入,默认你已会启动 controller_manager、会用 colcon 编译一个包。

1 ros2_control 架构全景

上一页我们反复提到「轨迹交给 ros2_control 执行」,这一页就把这件事彻底讲透。ros2_control 是 ROS2 官方维护的机器人控制框架,它的核心使命只有一句:把「控制逻辑」与「硬件驱动」彻底解耦。控制算法(轨迹插值、PID、状态广播)不需要知道底下是伺服电机、是 EtherCAT 从站、还是 Gazebo 仿真关节;硬件驱动也不需要知道上层跑的是轨迹控制器还是力控。

1.1 三层架构与数据流向

层次是什么职责典型实例
controller_manager控制框架的「管家」,一个生命周期节点加载/配置/激活/切换/卸载控制器,以固定周期调度所有激活的控制器,并管理硬件资源controller_manager 节点
controller(控制器)计算控制律的插件,也是生命周期节点读状态接口、算控制律、写命令接口;或广播状态joint_trajectory_controller、joint_state_broadcaster
hardware_interface(硬件接口)真实硬件/仿真的抽象插件把命令接口翻译成电机指令(写总线),把编码器/电流读数上报成状态接口(读总线)继承 SystemInterface 的自定义插件、gazebo_ros2_control

数据流是一个闭环:每个控制周期(update_rate,如 1000 Hz)内,controller_manager 先调硬件接口的 read() 把状态刷进来 → 再逐个调用激活控制器的 update(),让它们读状态、写命令 → 最后调硬件接口的 write() 把命令刷到电机。

          ┌────────────── controller_manager (RT 线程, update_rate Hz) ──────────────┐
          │   ① read(): 硬件 → 状态接口         ② update(): 控制器读状态/写命令      │
          │   ③ write(): 命令接口 → 硬件                                               │
          └───────────────┬───────────────────────────────────────┬────────────────────┘
              state 接口 ▲│(位置/速度/力矩)        command 接口 ▼│(位置/速度/力矩/effort)
          ┌───────────────┴───────────────────────────────────────┴────────────────────┐
          │              hardware_interface (SystemInterface / ActuatorInterface)       │
          │   CAN / EtherCAT / RS485 / 仿真插件(ign_ros2_control / gazebo_ros2_control) │
          └─────────────────────────────────────────────────────────────────────────────┘
                                    │ 写电机命令 / 读编码器反馈
                          ┌─────────▼─────────┐
                          │ 关节模组(伺服+减速器) │
                          └───────────────────┘

1.2 关键抽象:state / command 接口

控制器与硬件之间只通过两类命名接口交流,名字必须逐字匹配,这是全框架的「契约」:

接口类别方向常见名称含义
command interface(命令接口)控制器 → 硬件position / velocity / effort期望位置 / 期望速度 / 期望力矩(电流)
state interface(状态接口)硬件 → 控制器position / velocity / effort实测位置(编码器) / 实测速度 / 实测力矩
💡 为什么叫「接口」而不是「变量」?因为同一关节可以同时暴露多个接口:一个关节既声明 position 命令接口又声明 effort 命令接口,上层就能在「位置模式」与「力矩模式」之间切换(见第 7 节)。接口名是纯字符串,硬件插件导出什么、控制器申请什么,两边对不上就报「接口不存在」。

1.3 URDF 里的 ros2_control 描述块

URDF 描述机器人的几何与运动学,而 ros2_control 需要的「用什么硬件插件、每个关节暴露哪些接口」写在 URDF 的 <ros2_control> 标签里(通常单独放一个 xacro 文件再 <xacro:include> 进来)。一个最小示例:

<!-- 文件:ros2_control.xacro,被主 URDF xacro include -->
<ros2_control name="my_arm" type="system">
  <hardware>
    <plugin>hdrive_system/HDriveSystemHardware</plugin>
    <param name="can_interface">can0</param>
    <param name="motor_id">1</param>
    <param name="reduction_ratio">9.0</param>
  </hardware>
  <joint name="elbow_joint">
    <command_interface name="position"/>
    <command_interface name="effort"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
  </joint>
</ros2_control>

1.4 实时性:为什么要 RT 线程

控制回路对周期确定性有硬要求:如果本该 1 ms 执行一次的回路偶尔卡到 5 ms,关节就会一顿一顿,力矩控制甚至可能发散。因此 ros2_control 的 controller_manager 以实时(RT)线程跑控制循环,要求:

⚠️ 实时与 ROS2 默认 DDS 的边界:ROS2 的话题通信默认不是硬实时的,实时性只体现在 controller_manager 的控制循环内(read → update → write)。所以「读编码器 + 写力矩」要放在硬件接口的 read()/write() 里同步完成,而不是绕道去订阅话题。实时权限配置(优先级 + 锁页)见第 8 节。

2 controller_manager 详解

controller_manager 是所有控制器的生命周期管理器,它本身也是一个生命周期节点,配合命令行 ros2 control 完成「运行时动态增删控制器」而不重启整机——这是真机调试最重要的能力(改一个控制器不用重新拉起全部节点)。

2.1 生命周期(lifecycle)状态机

每个控制器都要走过一串状态:unconfigured → inactive → active,并可逆向 deactivate,最后 finalize(清理)。关键语义:

「加载 ≠ 激活」是新手的头号坑:load 之后控制器停在 inactive,不 activate 就永远不干活。

2.2 命令行:ros2 control

官方命令行工具 ros2 control 提供完整的运行时管理(下方命令均已核实于官方 ros2controlcli 文档):

ros2 control list_controllers            # 列出所有已加载控制器及其状态(active/inactive/unconfigured)
ros2 control list_controller_types       # 列出本机可用的控制器插件类型
ros2 control list_hardware_interfaces    # 列出硬件导出的全部 state/command 接口(诊断神器)

ros2 control load_controller   joint_state_broadcaster        # 加载(进入 unconfigured)
ros2 control configure_controller joint_state_broadcaster     # 配置(进入 inactive)
ros2 control set_controller_state joint_state_broadcaster active   # 激活
ros2 control set_controller_state joint_state_broadcaster inactive # 停用
ros2 control unload_controller joint_state_broadcaster        # 卸载
💡 一步到位的 spawner:手敲 4 条命令太啰嗦,官方提供 spawner 工具,一次完成「加载 + 配置 + 激活」:ros2 run controller_manager spawner joint_state_broadcaster。若控制器已存在,spawner 会报「已加载」并退出(可用 --inactive 只加载不激活、--activate-as-group 等参数控制行为)。

2.3 controller_manager 的 yaml 配置

controller_manager 本身的参数(更新频率、是否用仿真时间)与各控制器的参数写在同一份 yaml 里,一起传给节点:

controller_manager:
  ros__parameters:
    update_rate: 1000          # 控制循环 Hz(务必与硬件能力匹配)
    use_sim_time: false

# 下面每个顶级 key 是一个「待 spawner 启动的控制器」名
joint_state_broadcaster:
  ros__parameters:
    type: joint_state_broadcaster/JointStateBroadcaster
    publish_rate: 100

arm_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    # ... 具体参数见第 3 节

注意 update_rate 是整机的控制频率,所有 active 控制器共享同一节奏;每个控制器内部再靠各自参数(如 publish_ratestate_publish_rate)做降频。

2.4 控制器切换:switch_controller

真机上经常要在「力控」与「位置轨迹」之间来回切(抓取时用力控柔顺,搬运时用轨迹)。切换不是「停了 A 再起 B」那么随意,因为两个控制器可能争抢同一个关节的命令接口——同一时刻一个关节的 position 命令接口只能被一个 active 控制器占用。切换靠:

# 同时停用 joint_group_effort_controller、激活 arm_controller
ros2 control switch_controllers \
  --deactivate joint_group_effort_controller \
  --activate arm_controller

# 带严格度与时间窗(不同版本参数名略有差异,以 distro 文档为准)
ros2 control switch_controllers \
  --activate arm_controller \
  --deactivate joint_group_effort_controller \
  --strictness STRICT \
  --activate-asap --deactivate-asap
⚠️ 切换时序要点:switch_controller 会先让「要停用」的控制器安全停住(把当前命令写回硬件,避免跳变),再激活「要启用」的控制器。activate_asap/deactivate_asap 控制「是否等前一组完全就绪再切」。两个控制器若都申请同一关节的同一命令接口,必须先停一个再起另一个,直接双开会因接口冲突而拒绝激活。

3 常用控制器逐个配置

官方 ros2_controllers 仓库提供了一整套「开箱即用」的控制器,人形机器人关节模组最常用的四类如下(类型名均来自官方 controllers_index):

控制器插件类型作用申请的命令接口典型用途
joint_state_broadcasterjoint_state_broadcaster/JointStateBroadcaster把硬件状态广播到 /joint_states无(只读状态)给 TF、RViz、MoveIt2 供「当前关节角」
joint_trajectory_controllerjoint_trajectory_controller/JointTrajectoryController接收整条轨迹,插值后逐周期下发position(或 velocity/effort)执行 MoveIt2 规划出的轨迹
joint_group_effort_controllereffort_controllers/JointGroupEffortController直接下发一组关节的力矩/电流effort力控、重力补偿、拖动示教
forward_command_controllerforward_command_controller/ForwardCommandController把话题上的命令「透传」到命令接口interface_name 指定(1 个)手动调试、单关节点动、速度/力矩开环

3.1 joint_state_broadcaster

它是最基础的控制器,只「读」不「写」:把硬件上报的关节状态整理成 sensor_msgs/msg/JointState 发布到 /joint_states,供 robot_state_publisher 计算 TF、供 MoveIt2 获取当前状态。它不申请任何命令接口,所以可以和轨迹控制器同时 active、互不冲突。

joint_state_broadcaster:
  ros__parameters:
    type: joint_state_broadcaster/JointStateBroadcaster
    publish_rate: 100     # /joint_states 发布频率 Hz
    extra_joints: []      # 额外并入的非受控关节名(可选)
# 启动 + 验证
ros2 run controller_manager spawner joint_state_broadcaster
ros2 control list_controllers          # 应看到 joint_state_broadcaster : active
ros2 topic echo /joint_states          # 应能看到 position/velocity 数组在跳动

3.2 joint_trajectory_controller(轨迹控制器,核心)

它是 MoveIt2 真机执行的主力:通过 FollowJointTrajectory action 接收一条带时间戳的轨迹,在控制周期内做样条插值,把每个采样点的期望位置/速度下发到命令接口。参数(已核实官方 userdoc):

arm_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    joints:                       # 受控关节列表,顺序即命令数组顺序
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
    command_interfaces:           # 申请的命令接口(位置模式)
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 50.0      # 控制器状态回传频率
    action_monitor_rate: 20.0     # action 目标监控频率
    allow_partial_joints_goal: false   # 是否允许只给部分关节下目标
    constraints:
      stopped_velocity_tolerance: 0.01  # 判定「已停止」的速度容差
      goal_time: 0.0                    # 目标允许的到达时间余量
ros2 run controller_manager spawner arm_controller
ros2 control list_controllers        # arm_controller : active
# 发一条「各关节回到 0」的轨迹(action 方式,3 秒走完)
ros2 action send_goal /arm_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  "{trajectory: {joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint],
     points: [{positions: [0.0, 0.0, 0.0, 0.0], time_from_start: {sec: 3, nanosec: 0}}]}}"
💡 position / velocity / effort 三种模式怎么选?取决于电机底层的「内环」:伺服电机内部自带位置环时,ros2_control 只下发 position 即可;若要自己闭环速度就下发 velocity;直接给电流/力矩环则下发 effort(适合做力控/阻抗)。同一条轨迹,下发接口越「底层」,上层越需要把插值、闭环做扎实。

3.3 joint_group_effort_controller(力矩组控制器)

它把 std_msgs/msg/Float64MultiArray 话题上的一组力矩值,直接映射到一组关节的 effort 命令接口。常用于重力补偿、拖动示教、以及纯力矩的力控实验。注意它不做闭环,只是开环「你给多少我就下发多少」。

effort_controller:
  ros__parameters:
    type: effort_controllers/JointGroupEffortController
    joints:
      - elbow_joint
      - wrist_1_joint
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
ros2 run controller_manager spawner effort_controller
# 给两个关节各下发 0.5 / -0.3 的力矩(单位通常为 N·m)
ros2 topic pub /effort_controller/commands std_msgs/msg/Float64MultiArray \
  "{data: [0.5, -0.3]}"
⚠️ 版本提示:effort_controllers/JointGroupEffortController 在新版本 ros2_controllers 中被标记为将被弃用(官方建议逐步迁移到 forward_command_controller 等替代方案,约)。学习与旧项目兼容用它没问题,新项目建议先查当前 distro 的控制器索引确认替代品。

3.4 forward_command_controller(前馈/透传控制器)

它把 std_msgs/msg/Float64MultiArray 话题值原样写进指定的一种命令接口。比 joint_group_effort_controller 更通用:interface_name 参数决定透传的是 position、velocity 还是 effort。适合手动点动、单关节测试、或把自定义高层控制器的输出直接送入底层。

forward_position_controller:
  ros__parameters:
    type: forward_command_controller/ForwardCommandController
    joints:
      - elbow_joint
    interface_name: position     # 透传到 position 命令接口
    # interface_name: velocity / effort   # 也可透传速度或力矩
ros2 run controller_manager spawner forward_position_controller
ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray \
  "{data: [0.78]}"   # 让 elbow_joint 走到约 0.78 rad

4 自定义硬件接口实战

官方控制器覆盖了「控制律」,但「硬件」千差万别:你的关节模组可能是 CAN 总线的宇树电机、是 EtherCAT 伺服、也可能只是一块开发板发 PWM。这部分必须自己写 hardware_interface 插件。核心是继承 hardware_interface::SystemInterface(整机)或 ActuatorInterface(单个执行器),实现一组标准虚函数。

4.1 SystemInterface vs ActuatorInterface

基类适用场景生命周期归属
SystemInterface一个插件管多个关节(整条手臂/整机),共享一条总线对应 URDF 里 type="system"
ActuatorInterface一个插件管一个执行器,可独立开关(如每个模组自带 MCU)对应 URDF 里 type="actuator"
SensorInterface只读传感器(IMU、力传感器)对应 type="sensor"

人形机器人「每个关节模组一个独立电机」的形态,既可以用一个大 SystemInterface 统一收发,也可以用多个 ActuatorInterface 每个模组一个插件(更模块化,便于单个模组热插拔)。下面以 SystemInterface 为例,ActuatorInterface 只是把「多个关节」换成「单个关节」、虚函数签名略有差异。

4.2 必须实现的虚函数

4.3 C++ 骨架:读编码器、写力矩

#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <pluginlib/class_list_macros.hpp>
#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <string>

namespace hdrive_system
{
class HDriveSystemHardware : public hardware_interface::SystemInterface
{
public:
  hardware_interface::CallbackReturn on_init(
      const hardware_interface::HardwareInfo & info) override
  {
    if (hardware_interface::SystemInterface::on_init(info) !=
        hardware_interface::CallbackReturn::SUCCESS)
      return hardware_interface::CallbackReturn::ERROR;
    // 读 URDF 里的参数(总线名、电机 ID、减速比等)
    can_interface_ = info_.hardware_parameters.at("can_interface");
    motor_id_ = std::stoi(info_.hardware_parameters.at("motor_id"));
    joint_name_ = info_.joints[0].name;
    // 为每个状态/命令接口分配存储
    position_state_ = 0.0;
    velocity_state_ = 0.0;
    effort_state_ = 0.0;
    effort_command_ = 0.0;
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  std::vector<hardware_interface::StateInterface>
  export_state_interfaces() override
  {
    std::vector<hardware_interface::StateInterface> state;
    // 接口名必须与 URDF <state_interface> 一致
    state.emplace_back(joint_name_, "position", &position_state_);
    state.emplace_back(joint_name_, "velocity", &velocity_state_);
    state.emplace_back(joint_name_, "effort", &effort_state_);
    return state;
  }

  std::vector<hardware_interface::CommandInterface>
  export_command_interfaces() override
  {
    std::vector<hardware_interface::CommandInterface> cmd;
    // 只导出 effort 命令接口(力矩模式);要位置模式就加 "position"
    cmd.emplace_back(joint_name_, "effort", &effort_command_);
    return cmd;
  }

  hardware_interface::CallbackReturn on_activate(
      const rclcpp_lifecycle::State &) override
  {
    // 打开 CAN、使能电机(上电使能)
    open_can(can_interface_);
    enable_motor(motor_id_);
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  hardware_interface::return_type read(
      const rclcpp::Time &, const rclcpp::Duration &) override
  {
    // 从总线读编码器(角位移)→ 换算关节角(除以减速比)→ 写状态
    double raw = read_encoder(motor_id_);
    position_state_ = raw / reduction_ratio_;
    // 速度、力矩可在此处一并估算/读取
    return hardware_interface::return_type::OK;
  }

  hardware_interface::return_type write(
      const rclcpp::Time &, const rclcpp::Duration &) override
  {
    // 把期望力矩换算成电机电流/占空比后写入总线
    double current = effort_command_ * torque_to_current_;  // N·m → A
    write_current(motor_id_, current);
    return hardware_interface::return_type::OK;
  }

private:
  std::string joint_name_, can_interface_;
  int motor_id_ = 0;
  double reduction_ratio_ = 1.0, torque_to_current_ = 1.0;
  double position_state_ = 0.0, velocity_state_ = 0.0, effort_state_ = 0.0;
  double effort_command_ = 0.0;
};

}  // namespace hdrive_system

// pluginlib 注册:类名、命名空间类、基类
PLUGINLIB_EXPORT_CLASS(hdrive_system::HDriveSystemHardware,
                       hardware_interface::SystemInterface)
⚠️ read/write 的实时约束:这两个函数跑在 RT 线程里,禁止 RCLCPP_INFOstd::cout、动态内存分配、sleep 等阻塞/非确定操作。总线读写要选非阻塞、带超时的 API,否则一次 CAN 超时就会拖垮整个控制周期。

4.4 插件注册与构建(pluginlib / CMakeLists / package.xml)

硬件插件靠 pluginlib 机制被发现:编译产物是一个共享库,配一个 XML 清单声明「库里有哪个类、继承哪个基类」。三件套:

<!-- 文件:config/hdrive_system_plugin.xml -->
<library path="hdrive_system_hardware">
  <class name="hdrive_system/HDriveSystemHardware"
         type="hdrive_system::HDriveSystemHardware"
         base_class_type="hardware_interface::SystemInterface">
    <description>HDrive 关节模组硬件接口(CAN 收发)</description>
  </class>
</library>
# CMakeLists.txt 关键片段
find_package(ament_cmake REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)

add_library(hdrive_system_hardware src/hdrive_system_hardware.cpp)
target_include_directories(hdrive_system_hardware PRIVATE include)
ament_target_dependencies(hdrive_system_hardware
  hardware_interface pluginlib rclcpp rclcpp_lifecycle)

# 安装插件库与插件清单
pluginlib_export_plugin_description_file(hardware_interface config/hdrive_system_plugin.xml)
install(TARGETS hdrive_system_hardware
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
ament_export_dependencies(hardware_interface pluginlib rclcpp rclcpp_lifecycle)
ament_package()
<!-- package.xml 关键片段 -->
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<export>
  <build_type>ament_cmake</build_type>
</export>

编译后验证插件是否被找到:ros2 control list_hardware_components 里应能看到 hdrive_system/HDriveSystemHardware

5 与真实关节模组对接

关节模组的「硬件接口」本质就是一条通信总线的收发封装:CAN、EtherCAT、RS485、甚至 USB 串口。适配思路万变不离其宗——把「总线协议读写」隔离在 read()/write() 里,让上层控制器只看到标准的 position/velocity/effort 接口。

5.1 三类主流模组的适配思路

模组通信典型控制模式ros2_control 适配要点
宇树 Unitree 电机(A1/Go2/H1 系列关节)CAN(UART 亦可)位置 / 速度 / 力矩(PD 前馈)官方 unitree_ros 提供底层 SDK;在 read()/write() 里封装 CAN 帧收发,把 motor data 映射到关节接口
小米 CyberGearCAN(2.0)MIT 模式(位置/速度/力矩 + 增益)、位置/速度/电流模式社区已有 cybergear_ros2 等包;MIT 模式的 kp/kd 增益对应「阻抗/PD」参数,可映射到 effort/position 接口
自研 Hdrive(示例)CAN / EtherCAT位置 / 速度 / 电流(力矩)自己定义 CAN 报文(编码器回读 + 电流指令),封装成第 4 节的 SystemInterface

通用适配三步:① 写通信驱动(CAN 初始化、报文编解码、超时处理)→ ② 封装硬件接口(把「电机量」换算到「关节量」,乘/除减速比)→ ③ 在 URDF ros2_control 块声明接口与参数(总线名、电机 ID、减速比、力矩-电流系数)。

💡 减速比与量纲换算:电机侧是「电机轴角度/速度/力矩」,关节侧是「关节角度/速度/力矩」,中间隔着一个减速器。经验公式(忽略摩擦与效率):关节角 = 电机角 / 减速比;关节力矩 = 电机力矩 × 减速比。编码器回读与电流指令都别算错方向,否则一上电就会「反转 + 飞车」。

5.2 写 test 模式(模拟硬件)

真机到手前,先用模拟硬件把整套 ros2_control + MoveIt2 跑通,能提前暴露 90% 的配置错误。两种做法:

<!-- URDF 用 mock 组件做 test 模式(GenericSystem,type 为 system) -->
<ros2_control name="my_arm" type="system">
  <hardware>
    <plugin>mock_components/GenericSystem</plugin>
  </hardware>
  <joint name="elbow_joint">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>
⚠️ 从 mock 切到真机的改动点:只换 URDF 里 <plugin> 的类名与参数即可,控制器配置、MoveIt2 配置、上层代码一行不用改——这正是「控制与硬件解耦」的红利。但 mock 假设「命令即状态」,不会暴露实时性、总线丢包、限位、摩擦等真机才有的问题,别把 mock 通过当成真机能跑。

6 与 MoveIt2 串联:从规划到执行

MoveIt2 只做「规划」,不直接发电机指令。全链路:MoveIt2 规划 → 发布 control_msgs/action/FollowJointTrajectory 目标 → joint_trajectory_controller 逐周期插值写 position/velocity → hardware_interface 写电机/读编码器 → 关节模组,编码器反馈再沿 state 接口回到控制器形成闭环。

6.1 move_group 侧的 ros2_control 配置

MoveIt2 需要知道「规划组 arm 该把轨迹发给哪个 action、用哪个控制器」。在 moveit_config 包里配 ros2_controllers.yaml(控制器名、类型、关节、action 命名空间),并让 moveit_controller_manager 加载它:

# moveit_config/config/ros2_controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 1000

arm_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

# MoveIt 侧:moveit_controller_manager 的控制器声明
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
  controller_names:
    - arm_controller
  arm_controller:
    action_ns: follow_joint_trajectory   # action 命名空间
    type: FollowJointTrajectory
    default: true
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
⚠️ 两处关节名要「三对齐」:URDF 的关节、ros2_controllers.yamljointsmoveit_simple_controller_manager 里的 joints 必须逐字一致(含大小写、下划线)。任何一处不一致,要么 controller_manager 报「无此关节」,要么 MoveIt2 下发后 action 被拒绝。

6.2 真机调试流程(减速起步)

1起硬件 + 起控制器
先启动硬件接口(使能电机),再 spawner joint_state_broadcaster + arm_controller 并确认 active
2验证状态流
ros2 topic echo /joint_states 看编码器值是否随手动转动而变化
3低速点动
先用 forward_command_controller 小步点动单关节,确认方向/量纲正确
4起 MoveIt
demo.launch.py 连上真机控制器,确认 FollowJointTrajectory action 可达
5空载试跑
setMaxVelocityScalingFactor(0.1~0.2) 空载走一条 home 轨迹,手按急停
🔴 真机铁律:首次执行务必速度缩放 + 空载 + 手按急停;先验证「编码器方向与命令方向一致」,否则第一发轨迹就可能反向冲限位。力矩模式更是要把力矩上限压到最低再放开。

7 关节模式切换与力控/阻抗

人形机器人关节模组很少「一种模式跑到底」:定位用位置模式,柔顺拖动用速度/力矩模式,接触作业要力控/阻抗控制。ros2_control 支持同一关节暴露多个命令接口,由「当前激活哪个控制器」决定走哪条命令通道。

7.1 位置/速度/力矩模式如何切换

在 URDF 里给关节同时声明三种命令接口,然后配三个控制器各占一种:

<joint name="elbow_joint">
  <command_interface name="position"/>
  <command_interface name="velocity"/>
  <command_interface name="effort"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>

切换就是「换一个 active 控制器」:ros2 control switch_controllers --deactivate arm_position_controller --activate arm_effort_controller。因为三个控制器各申请不同的命令接口(position / velocity / effort),理论上可同时 active;但同一类接口(如两个都申请 position)则互斥,必须切。

7.2 笛卡尔力控与阻抗控制的实现思路

💡 阻抗参数放哪里?「上层算力矩」(自己写 controller)灵活、可加完整动力学前馈,但受控制周期与通信延迟限制;「下沉到电机 MIT 模式」实时性最好、响应最快,但 K/D 只能在线调、模型简单。工程上常是混合:电机做内环高带宽力矩环,ros2_control 做外环位置/阻抗环。

8 常见坑与调试

⚠️ 高频坑汇总:
1. 控制器未激活:加载后停在 inactive,不 activate 就没有任何输出 → 先 ros2 control list_controllers 看状态列;
2. 接口名称不匹配:URDF 的 <state_interface name="position"/> 与硬件插件 export_state_interfaces()"position" 字符串不一致 → 用 ros2 control list_hardware_interfaces 逐一比对;
3. 控制周期不匹配:update_rate 设 1000 Hz 但总线/硬件只支持 200 Hz,导致 read/write 超时、周期抖动 → 把 update_rate 降到硬件能力以内;
4. 关节名三处不一致:URDF / 控制器 yaml / MoveIt 配置的关节名必须逐字一致;
5. 实时权限不足:RT 线程拿不到 SCHED_FIFO 优先级或锁页内存,表现为周期抖动、偶发卡顿 → 配 rtpriomemlock(见下);
6. read/write 里打日志:RT 线程内 RCLCPP_INFO/std::cout 拖垮实时性,甚至偶发「丢周期」。

8.1 实时权限配置(优先级 + 锁页内存)

# ① 给实时用户组配置 rtprio 与 memlock(/etc/security/limits.d/ros2_rt.conf)
@realtime  -  rtprio  98
@realtime  -  memlock unlimited

# ② 把运行用户加入 realtime 组,重新登录生效
sudo usermod -a -G realtime $USER

# ③(可选)给 controller_manager 可执行文件授予调度能力,免 root
sudo setcap cap_sys_nice+ep /opt/ros/humble/lib/controller_manager/controller_manager

# ④ 验证:应分别显示 98 与 unlimited
ulimit -r; ulimit -l
💡 判断「是否真实时」:看 controller_manager 启动日志里的周期诊断信息(新版本提供执行时间与周期性的 diagnostics),或对比 update_rate 设定值与实际执行间隔的抖动。抖动大、偶发超时,优先查权限与 read/write 里的阻塞调用。

8.2 诊断命令速查

ros2 control list_controllers          # 谁 active / 谁 inactive / 谁配置失败
ros2 control list_hardware_interfaces  # 硬件到底导出了哪些 state/command 接口
ros2 control list_controller_types     # 有哪些控制器插件可加载
ros2 topic echo /joint_states          # 状态流是否在动

9 本节自测

1. 关于 ros2_control 的三层架构职责,下列说法正确的是?
💡 核心是「控制逻辑与硬件驱动解耦」:controller 计算控制律,hardware_interface 收发总线,controller_manager 以固定周期调度两者(读→算→写)。运动规划是 MoveIt2 的事。
2. 控制器是生命周期节点,load_controller 之后它的状态是?
💡 加载≠激活:load 进入 unconfigured,configure 进入 inactive,只有 activate 后 update() 才会被调度。spawner 工具帮你一次完成加载+配置+激活。
3. joint_state_broadcaster 与 joint_trajectory_controller 能同时 active 而不冲突,根本原因是?
💡 joint_state_broadcaster 只订阅状态并广播 /joint_states,不占用任何 command 接口;而「同一关节同一命令接口只能被一个 active 控制器占用」的互斥只发生在命令接口上。
4. 关于 forward_command_controller 与 joint_group_effort_controller,正确的是?
💡 forward_command_controller 是通用「透传」,interface_name 决定 position/velocity/effort;joint_group_effort_controller 只发 effort、开环,常用于力矩/拖动示教。
5. 在硬件接口插件里,把电机轴角度换算成关节角度的正确关系是(设减速比 R>1)?
💡 减速器让电机转 R 圈关节才转 1 圈:关节角=电机角/R,关节力矩=电机力矩×R(忽略效率)。方向与量纲算错会「反转/飞车」。
6. 关于硬件接口的 read() / write() 实时约束,错误的是?
💡 sleep、网络 I/O、动态内存、打日志都是 RT 循环里的「毒药」,会引入非确定延迟,拖垮控制周期,严重时导致关节抖动甚至发散。
7. MoveIt2 规划成功后把轨迹交给真机的完整链路是?
💡 MoveIt2 只规划,不直接驱动硬件;它把轨迹作为 FollowJointTrajectory action 目标发给轨迹控制器,由控制器逐周期插值写命令接口,再经硬件接口下发到电机。
8. 真机上要在「力矩控制」与「位置轨迹控制」之间来回切换,关于 switch_controller,下列说法正确的是?
💡 见第 2.4 与 7.1 节:切换不是瞬间的,框架先让停用方安全停住再激活启用方,避免命令跳变;activate_asap/deactivate_asap 控制「是否等前一组完全就绪再切」。而同一关节的同一命令接口同时只能被一个 active 控制器占用,直接双开会因接口冲突被拒绝激活——把切换想成「瞬时、可随便双开」是最常见的错误认知。
9. 关于硬件接口基类 SystemInterface 与 ActuatorInterface,下列说法正确的是?
💡 见第 4.1 节:SystemInterface 是「一个插件管多个关节、共享一条总线」,对应 type="system";ActuatorInterface 是「一个插件管一个执行器」,对应 type="actuator"(每个模组自带 MCU 时可独立热插拔);只读传感器才对应 SensorInterface/type="sensor"。二者适用场景不同,URDF 的 type 必须与所继承基类对应。
10. 真机到手前,用官方 mock_components/GenericSystem 做离线联调,下列说法正确的是?
💡 见第 5.2 节:mock_components 假设「命令即状态」,只用来提前暴露约 90% 的配置错误;它不接真实硬件、没有动力学,覆盖不了实时性/丢包/限位/摩擦。从 mock 切真机只需换 URDF 里 <plugin> 的类名与参数,控制器、MoveIt2、上层代码一行不用改——把「mock 通过」当成「真机能跑」是典型误区。
📌 本节要点速查:①三层架构:controller_manager(生命周期+调度)/ controller(控制律)/ hardware_interface(总线读写),URDF 的 ros2_control 块声明插件与每关节的 state/command 接口;②生命周期:load→unconfigured、configure→inactive、activate 才运行,spawner 一键「加载+配置+激活」,同一命令接口同时只许一个 active 控制器占用;③常用控制器:joint_state_broadcaster(只读状态)、joint_trajectory_controller(MoveIt2 执行主力,position/velocity/effort 三模式)、joint_group_effort_controller(开环 effort)、forward_command_controller(interface_name 透传);④自定义硬件:继承 SystemInterface(整机)/ActuatorInterface(单执行器),实现 read/write 并 pluginlib 注册,量纲换算 关节角=电机角÷R、关节力矩=电机力矩×R;⑤联调与安全:mock_components 做离线 test 模式、切真机只换 plugin 类名,真机铁律「速度缩放+空载+手按急停」。
📌 本节小结:ros2_control 用 controller_manager(调度 + 生命周期)/ controller(控制律)/ hardware_interface(总线读写) 三层把「控制」与「硬件」解耦,URDF 的 ros2_control 块声明插件与每个关节的 state/command 接口,实时性靠 RT 线程(固定 update_rate + 优先级 + 锁页)。controller_managerros2 control 完成加载/激活/切换(spawner 一键到位),joint_trajectory_controller 是 MoveIt2 执行主力,joint_state_broadcaster / joint_group_effort_controller / forward_command_controller 各司其职。自定义硬件只需继承 SystemInterface 实现 read/write 并用 pluginlib 注册;真实模组(Unitree/CyberGear/Hdrive)的适配就是把 CAN/EtherCAT 收发封装进 read/write,再用 mock 组件做离线 test 模式。全链路是 MoveIt2 → FollowJointTrajectory → 轨迹控制器 → 硬件接口 → 电机,模式切换靠「多命令接口 + 切换 active 控制器」,力控/阻抗则落到 effort 接口与电机 MIT 模式。
🤔 思考题: 1. 若 ros2 control list_controllers 显示 arm_controller 是 active,但电机完全不动,你会按什么顺序排查(至少列出 4 步)?
2. 同一关节同时声明 position 和 effort 两个命令接口,为什么「位置轨迹控制器」和「力矩组控制器」不能同时 active?写出你的切换命令。
3. CyberGear 的 MIT 模式要求下发 kp/kd 增益,你会把它们映射到 ros2_control 的哪个层次?为什么?
4. 自己动手:用官方 mock_components 起一个 2 关节臂,spawner joint_state_broadcaster + joint_trajectory_controller,发一条 FollowJointTrajectory 目标,观察 list_hardware_interfaceslist_controllers 的变化。

10 参考来源

资料整理更新至 2026-08;包名、命令与配置项已用 web_search 对照官方文档核实,不同 distro 的参数名与弃用状态可能略有差异,文中未能逐一核实之处均标注「约」,请以当前版本官方文档为准。