ros2_control 6 自由度机械臂（上）

系列文章目录

前言

       ros2_control 是一个实时控制框架，专为普通机器人应用而设计。标准的 c++ 接口用于与硬件交互和查询用户定义的控制器命令。这些接口增强了代码的模块化和与机器人无关的设计。具体的应用细节，例如使用什么控制器、机器人有多少个关节以及它们的运动学结构，则通过 YAML 参数配置文件和通用机器人描述文件（URDF）来指定。最后，通过 ROS 2 启动文件部署 ros2_control 框架。

       本教程将详细介绍 ros2_control 的各个组成部分，即

1. ros2_control 概述
2. 编写 URDF
3. 编写硬件接口
4. 编写控制器

一、ros2_control 概述

       ros2_control 引入了状态接口（state_interfaces）和命令接口（command_interfaces）来抽象硬件接口。state_interfaces 是只读数据句柄，通常代表传感器读数，如关节编码器。command_interfaces 是读写数据句柄，代表硬件命令，如设置关节速度参考。command_interfaces 是专用访问接口，也就是说，如果控制器 "认领 "了某个接口，那么在该接口被释放之前，任何其他控制器都不能使用它。这两种接口类型都有唯一的名称和类型。所有可用状态和命令接口的名称和类型都在 YAML 配置文件和 URDF 文件中指定。

       ros2_control 提供了控制器接口（ControllerInterface）和硬件接口（HardwareInterface）类，以实现与机器人无关的控制。在初始化过程中，控制器通过 ControllerInterface 申请运行所需的状态接口和命令接口。另一方面，硬件驱动程序通过硬件接口（HardwareInterface）提供状态接口和命令接口。ros2_control 确保在启动控制器之前，所有请求的接口都可用。接口模式允许供应商编写运行时加载的特定硬件驱动程序。

       主程序是一个实时读、更新、写循环。在读取调用期间，符合 HardwareInterface 的硬件驱动程序会用从硬件接收到的最新值更新其提供的状态_接口。在更新调用期间，控制器根据更新后的状态接口计算命令，并将其写入命令接口。最后，在写入调用期间，硬件驱动程序从其提供的 command_interfaces 中读取值，并将其发送给硬件。ros2_control 节点通过一个实时线程运行主循环。ros2_control 节点运行第二个非实时线程，与 ROS 发布者、订阅者和服务进行交互。

二、编写 URDF

       URDF 文件是一种基于 XML 的标准文件，用于描述机器人的特征。它可以表示任何具有树形结构的机器人，但具有循环结构的机器人除外。每个链接必须只有一个父链接。对于 ros2_control，有三个主要标签：link、joint 和 ros2_control。关节标签定义了机器人的运动学结构，而链接标签则定义了动态属性和 3D 几何结构。ros2_control 则定义硬件和控制器配置。

2.1 几何结构

       大多数商用机器人已经定义了机器人描述包（robot_description packages），请参阅通用机器人（Universal Robots）。不过，本教程将详细介绍如何从头开始创建一个机器人描述包。

       首先，我们需要一个机器人的 3D 模型。为了便于说明，我们将使用一个通用的 6 DOF 机器人操作器。

通用 6 DOF 机器人操作器

        机器人的 6 个刚体需要分别处理并导出为各自的 .stl 和 .dae 文件。一般来说，.stl 3D 模型文件是用于快速碰撞检查的粗网格，而 .dae 文件仅用于可视化目的。为简单起见，我们将使用相同的网格。

       按照惯例，每个 .stl 文件都在自己的坐标系中表达其顶点的位置。因此，我们需要指定每个刚体之间的线性变换（旋转和平移），以定义机器人的完整几何体。每个刚体的三维模型都应调整为近端关节轴（连接刚体与其父体的轴）位于 Z 轴方向。三维模型的原点也应调整为网格底面与 xy 平面共面。下面的网格说明了这种配置。

刚体 1

刚体 2 已对齐

       每个网格都应在处理后导出到各自的文件中。Blender 是一款开源三维建模软件，可以导入/导出 .stl 和 .dae 文件并操作其顶点。本教程使用 Blender 处理机器人模型。

       最后，我们可以计算机器人关节之间的变换，并开始编写 URDF。首先，对其坐标系中的刚体 2 应用负 90 度翻滚。

刚体 2 带 -90 度滚动

        为了使示例简单明了，我们现在不应用俯仰角。然后，我们施加 90 度的正偏航。

刚体 2，-90 度滚动和 90 度偏航

       最后，我们在刚体 2 和刚体 1 的坐标系帧之间进行 x 轴-0.1 米和 z 轴 0.18 米的平移。最终结果如下所示。

刚体 2 带有-90 度滚动、90 度偏航和平移功能

       然后对所有刚体重复上述过程。

2.2 URDF 文件

       URDF 文件一般按照以下模板格式化。

<robot name="robot_6_dof">
  <!-- create link fixed to the "world" -->
  <link name="base_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot_6_dof/meshes/visual/link_0.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot_6_dof/meshes/collision/link_0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <!-- additional links ... -->
  <link name="world"/>
  <link name="tool0"/>
  <joint name="base_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- joints - main serial chain -->
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin rpy="0 0 0" xyz="0 0 0.061584"/>
    <axis xyz="0 0 1"/>
    <limit effort="1000.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5"/>
  </joint>
  <!-- additional joints ... -->
  <!-- ros2 control tag -->
  <ros2_control name="robot_6_dof" type="system">
    <hardware>
      <plugin>
        <!-- {Name_Space}/{Class_Name}-->
      </plugin>
    </hardware>
    <joint name="joint_1">
      <command_interface name="position">
        <param name="min">{-2*pi}</param>
        <param name="max">{2*pi}</param>
      </command_interface>
      <!-- additional command interfaces ... -->
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <!-- additional state interfaces ... -->
    </joint>
    <!-- additional joints ...-->
    <!-- additional hardware/sensors ...-->
  </ros2_control>
</robot>

• robot 标签包含 URDF 文件的所有内容。它有一个必须指定的名称属性。
• link 标签定义了机器人的几何形状和惯性属性。它有一个名称属性，将被关节标签引用。
• visual 标记指定视觉网格的旋转和平移。如果网格是按照前面所述的方法处理的，那么原点标签可以全部为零。
• geometry 和 mesh 标签指定三维网格文件相对于指定 ROS 2 软件包的位置。
• collision 标签等同于视觉标签，只是指定的网格在某些应用中用于碰撞检测。
• inertial 标签指定 link 的质量和惯性。origin 标签指定刚体的质心。这些值用于计算正向和反向动力学。由于我们的应用程序不使用动力学，因此使用统一的任意值。
• 注释表示将定义多个连续的链接标记，每个链接一个。
• 和  元素不是必需的。不过，约定俗成的做法是将机器人顶端的 link 设置为 tool0，并定义机器人相对于世界坐标系的 base link。
• joint 标签指定机器人的运动结构。它有两个必备属性：名称和类型。类型指定两个相连刚体之间的可行运动。随后的父刚体 parent 和子刚体 child 则指定关节连接了哪两个链接。
• axis 标签规定了关节的自由度。如果网格是按前述方法处理的，则轴值始终为 "0 0 1"。
• limits 标签指定关节的运动学和动力学限制。
• ros2_control 标签指定机器人的硬件配置。更具体地说，就是可用的状态和命令接口。该标签有两个必填属性：名称和类型。该标签还包含传感器等附加元素。
• hardware 和 plugin 标签指示 ros2_control 框架将符合 HardwareInterface 标准的硬件驱动程序作为插件动态加载。插件被指定为 <{Name_Space}/{Class_Name} 。
• 最后，joint 标记指定了加载的插件将提供的状态和命令接口。关节点用 name 属性指定。command_interface 和 state_interface 标签指定接口类型，通常是位置、速度、加速度或力。

       为简化 URDF 文件，使用 xacro 来定义宏，请参阅本教程。本教程中机器人的完整 xacro 文件可在此处获取。在 xacro 生成 URDF 后，可以使用 urdf_to_graphviz 工具验证运动学链。运行

xacro description/urdf/r6bot.urdf.xacro > r6bot.urdf
urdf_to_graphviz r6bot.urdf r6bot

       生成 r6bot.pdf，显示机器人的运动链。

三、编写硬件接口

       在 ros2_control 中，硬件系统组件是通过符合 HardwareInterface 公共接口的用户定义驱动插件集成的。URDF 中指定的硬件插件在初始化过程中使用 pluginlib 接口动态加载。为了运行 ros2_control_node，必须设置名为 robot_description 的参数。这通常在 ros2_control 启动文件中完成。

       以下代码块将解释编写新硬件接口的要求。

       教程机器人的硬件插件是一个名为 RobotSystem 的类，它继承自 hardware_interface::SystemInterface。SystemInterface 是为完整机器人系统设计的硬件接口之一。例如，UR5 就使用了该接口。机器人系统必须实现五个公共方法。

1. 启动（on_init）
2. 输出状态接口（export_state_interfaces）
3. 导出命令接口（export_command_interfaces）
4. 读取（read）
5. 写（write）

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
#include "hardware_interface/types/hardware_interface_return_values.hpp"
class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface {
    public:
    CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
    return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
    return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
    // private members
    // ...
}

       如果在 URDF 中指定了机器人系统，on_init 方法会在 ros2_control 初始化过程中被调用一次。在该方法中，需要设置机器人硬件之间的通信，并分配动态内存。由于教程机器人是仿真的，因此不会建立明确的通信。相反，将初始化代表所有硬件状态的向量，例如描述关节角度的 double 向量等。

CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo &info) {
    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
        return CallbackReturn::ERROR;
    }
    // setup communication with robot hardware
    // ...
    return CallbackReturn::SUCCESS;
}

       值得注意的是，on_init 的行为预计会因 URDF 文件的不同而不同。SystemInterface::on_init(info) 调用会用 URDF 中的具体信息填充 info 对象。例如，info 对象有 joints、sensors、gpios 等字段。假设传感器字段的名称值为 tcp_force_torque_sensor。那么 on_init 必须尝试与该传感器建立通信。如果失败，则返回错误值。

       接下来，将依次调用 export_state_interfaces 和 export_command_interfaces 方法。export_state_interfaces 方法会返回一个 StateInterface 向量，描述每个关节的状态接口。StateInterface 对象是只读数据句柄。它们的构造函数需要一个接口名称、接口类型和一个指向 double 数据值的指针。对于机器人系统来说，数据指针引用的是类成员变量。这样，每个方法都可以访问数据。

std::vector<hardware_interface::StateInterface> RobotSystem::export_state_interfaces() {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    // add state interfaces to ``state_interfaces`` for each joint, e.g. `info_.joints[0].state_interfaces_`, `info_.joints[1].state_interfaces_`, `info_.joints[2].state_interfaces_` ...
    // ...
    return state_interfaces;
  }

       export_command_interfaces 方法与前一个方法几乎完全相同。不同之处在于返回的是一个 CommandInterface 向量。该向量包含描述每个关节的命令接口的对象。

std::vector<hardware_interface::CommandInterface> RobotSystem::export_command_interfaces() {
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    // add command interfaces to ``command_interfaces`` for each joint, e.g. `info_.joints[0].command_interfaces_`, `info_.joints[1].command_interfaces_`, `info_.joints[2].command_interfaces_` ...
    // ...
    return command_interfaces;
}

       read 方法是 ros2_control 循环的核心方法。在主循环中，ros2_control 会遍历所有硬件组件并调用 read 方法。该方法在实时线程上执行，因此必须遵守实时约束。read 方法负责更新状态接口的数据值。由于数据值指向类成员变量，因此可以用相应的传感器值填充这些值，进而更新每个导出状态接口对象的值。

return_type RobotSystem::read(const rclcpp::Time & time, const rclcpp::Duration &period) {
    // read hardware values for state interfaces, e.g joint encoders and sensor readings
    // ...
    return return_type::OK;
}

       write 方法是 ros2_control 循环中的另一个核心方法。它在实时循环中更新后被调用。因此，它也必须遵守实时约束。write 方法负责更新命令接口的数据值。与读取方法不同，写入方法访问由导出的命令接口对象指针指向的数据值，并将其发送给相应的硬件。例如，如果硬件支持通过 TCP 协议设置关节速度，那么该方法就会访问相应命令接口的数据并发送包含该值的数据包。

return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) {
    // send command interface values to hardware, e.g joint set joint velocity
    // ...
    return return_type::OK;
}

       最后，所有 ros2_control 插件都应在文件末尾添加以下两行代码。

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robot_6_dof_hardware::RobotSystem, hardware_interface::SystemInterface)

       PLUGINLIB_EXPORT_CLASS 是一个使用 pluginlib 创建插件库的 c++ 宏。

ros2_control 6 自由度机械臂（下）+https://developer.aliyun.com/article/1585409