本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

MoveIt2控制器模块：轨迹执行与ros2_control实时控制

导读：规划出的轨迹如何变成真实的机械臂运动？MoveIt2 通过 ros2_control 框架对接底层硬件，是连接规划与执行的关键桥梁。本文详解 FollowJointTrajectory 接口、控制器配置与轨迹执行流程。

原理简析

MoveIt2 通过 moveit_simple_controller_manager 管理 FollowJointTrajectoryAction 接口，规划层只负责生成轨迹，执行层由 ros2_control 的 JointTrajectoryController 完成实时插值与下发。

flowchart TB A(["规划轨迹"]) --> B["MoveGroupInterface execute()"] B --> C["FollowJointTrajectory Action Client"] C --> D["ros2_control JointTrajectoryController"] D --> E["Hardware Interface 硬件抽象层"] E --> F["实际机械臂"] F --> G["编码器反馈"] G -->|"位置/速度/电流"| D

ros2_control 三大核心：
- Controller Manager：加载、激活、停止控制器
- Hardware Interface：抽象电机驱动、编码器（支持 Position/Velocity/Effort 接口）
- Controller：joint_trajectory_controller 接收轨迹，joint_state_broadcaster 发布关节状态

控制器类型对照：

来自 linuxros.cn · linuxROS

类型	控制量	适用场景
位置控制器	关节位置	大多数工业机械臂
速度控制器	关节速度	连续轨迹、输送跟踪
力矩控制器	关节力矩	打磨、装配、柔顺控制

实操步骤

控制器配置架构

flowchart TB A["MoveIt2 轨迹规划"] --> B["moveit_simple_controller_manager"] B --> C["FollowJointTrajectory Action 接口"] C --> D["ros2_control ControllerManager"] D --> E["JointTrajectoryController 轨迹跟踪"] D --> F["JointStateBroadcaster 状态广播"] E --> G["HardwareInterface 硬件抽象"] F --> G G --> H["电机驱动器 CAN/EtherCAT"]

避坑点：joint_state_broadcaster 不是控制器而是广播器，仅负责把硬件状态发到 /joint_states 话题；它必须与 joint_trajectory_controller 成对激活，否则 RViz 看不到机器人状态。

代码实现

配置 ros2_control（controllers.yaml）

# ros2_controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    command_interfaces: [position]
    state_interfaces: [position, velocity]
    state_publish_rate: 50.0
    action_monitor_rate: 20.0
    constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05

配置 MoveIt 控制器（moveit_controllers.yaml）

# moveit_controllers.yaml
controller_names:
  - joint_trajectory_controller

joint_trajectory_controller:
  type: FollowJointTrajectory
  action_ns: /joint_trajectory_controller/follow_joint_trajectory
  default: true
  joints:
    - joint1
    - joint2
    - joint3
    - joint4
    - joint5
    - joint6

C++ 轨迹执行

// execute_trajectory.cpp
#include <moveit/move_group_interface/move_group_interface.h>

void ExecuteTrajectoryExample(
    moveit::planning_interface::MoveGroupInterface& move_group)
{
    // 设置目标（关节空间）
    std::vector<double> target = {0.0, -0.5, 0.3, 0.0, 0.5, 0.0};
    move_group.setJointValueTarget(target);

    // 规划
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS) {
        // execute() 内部通过 Action Client 下发给 ros2_control
        move_group.execute(plan);
    }
}

直接调用 Action Client（自定义场景）

// action_client.cpp
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <control_msgs/action/follow_joint_trajectory.hpp>

void ActionClientExample(rclcpp::Node::SharedPtr node)
{
    using FollowJointTrajectory =
        control_msgs::action::FollowJointTrajectory;

    auto action_client = rclcpp_action::create_client<FollowJointTrajectory>(
        node, "/joint_trajectory_controller/follow_joint_trajectory");

    if (!action_client->wait_for_action_server(std::chrono::seconds(2))) {
        RCLCPP_ERROR(node->get_logger(), "Action server not available");
        return;
    }

    auto goal = FollowJointTrajectory::Goal();
    goal.trajectory.joint_names = {"joint1", "joint2", "joint3"};
    goal.trajectory.points.resize(1);
    goal.trajectory.points[0].positions = {0.5, 0.5, 0.5};
    goal.trajectory.points[0].time_from_start =
        rclcpp::Duration::from_seconds(2.0);

    action_client->async_send_goal(goal);
}

常见问题解决

Q1：轨迹执行抖动怎么办？
A：检查 joint_trajectory_controller 的 PID 增益（若启用）；降低轨迹速度限制（max_velocity）；确认硬件接口的 command_interfaces 与控制器一致。

Q2：执行中途停止？
A：查看 /joint_trajectory_controller/follow_joint_trajectory/_action/status 的反馈；增大 constraints.goal_time 超时；确认 joint_state_broadcaster 已激活。

Q3：位置跳变？
A：轨迹点间插值不足，增加中间路径点；启用 velocity_ff 前馈；检查 stopped_velocity_tolerance 是否合理。

总结

MoveIt2 的控制器接口基于 ros2_control 框架，通过 FollowJointTrajectory Action 与 JointTrajectoryController 对接。实际使用时需配置两份 YAML（MoveIt 端的 moveit_controllers.yaml 和 ros2_control 端的 ros2_controllers.yaml），并保证关节名、接口类型一致。下期讲解可视化模块，搞懂 RViz2 调试与轨迹仿真技巧。