2.1.1 文档范围与背景知识#

本方法只关注控制器的串行级联，并尽量复用现有机制。它聚焦于控制器的输入与输出及其在控制器管理器中的管理。引入控制器组（controller groups）的概念仅仅是为了表述清晰，其唯一含义是：该组内的控制器可以按任意顺序更新。这并不意味着控制器级联文档中描述的控制器组将来不会被引入和使用。不过，作者认为在现阶段引入控制器组只会增加不必要的复杂度，尽管从长远来看它们 可能 能提供更清晰的结构和接口。

2.1.2 动机、目的与用途#

为描述本文档的意图，我们聚焦于 ‘controllers_chaining’ 设计文档中简单但足够的示例（Example 2）：

在该示例中，我们希望将 ‘position_tracking’ 控制器与 ‘diff_drive_controller’、两个 PID 控制器以及 ‘robot_localization’ 控制器级联。现在设想一个用例：我们不仅希望将这些控制器作为一个组运行，还希望灵活地添加前置步骤。这意味着以下几点：

1. 机器人启动时，我们希望检查电机速度控制是否正常工作，因此只激活 PID 控制器。在此阶段，我们还可以通过话题在外部控制 PID 控制器的输入。不过，这些控制器也提供虚拟接口，因此我们可以对它们进行级联。
2. 然后激活 “diff_drive_controller”，它会挂接到 PID 控制器的虚拟输入接口上。PID 控制器也会被告知它们正工作在级联模式，因此会通过订阅者禁用其外部接口。此时我们检查差速机器人的运动学是否正常运行。
3. 一旦 ‘diff_drive_controller’ 激活，它会暴露 ‘odom’ 状态接口，该接口被 ‘odom_publisher’ 和 ‘sensor_fusion’ 控制器使用。‘odom_publisher’ 控制器被激活并挂接到 ‘diff_drive_controller’ 导出的 ‘odom’ 状态接口上。‘sensor_fusion’ 控制器被激活并挂接到 ‘diff_drive_controller’ 导出的 ‘odom’ 状态接口以及 ‘imu’ 状态接口上。
4. 一旦 ‘sensor_fusion’ 控制器激活，它会暴露 ‘odom’ 状态接口，该接口被 ‘robot_localization’ 控制器使用。‘robot_localization’ 控制器被激活并挂接到 ‘sensor_fusion’ 控制器的 ‘odom’ 状态接口上。激活后，‘robot_localization’ 控制器暴露 ‘actual_pose’ 状态接口，该接口被 ‘position_tracking’ 控制器使用。
5. 之后，“position_tracking” 可以被激活并挂接到 “diff_drive_controller”（后者会禁用其外部接口）以及提供 ‘actual_pose’ 状态接口的 ‘robot_localization’ 控制器上。
6. 如果任何一个控制器被停用，其所有前置控制器也需要被停用。

注意

只有当暴露参考接口的控制器其参考接口被其他控制器使用时，这些控制器才切换到级联模式。当其参考接口未被其他控制器使用时，它们会切换回来从订阅者获取参考值。但是，暴露状态接口的控制器在其状态接口被其他控制器使用时，不会被触发进入级联模式。

注意

本文档使用了 前置（preceding）和 后置（following）控制器的术语。这两个术语指代如下顺序关系：如果控制器 A 认领（将其输出连接到）B 的参考接口（输入），则称 A 前置 于 B。在本节开头的示例图中，‘diff_drive_controller’ 前置 于 ‘pid left wheel’ 和 ‘pid right wheel’。相应地，‘pid left wheel’ 和 ‘pid right wheel’ 是 后置 于 ‘diff_drive_controller’ 的控制器。

2.1.3 实现#

2.1.4 调试输出#

• 当导出参考接口的控制器处于非活动（inactive）状态时，这些参考接口为 unavailable（不可用）且 unclaimed（未认领）。
• 当导出参考接口的控制器处于活动（active）状态但未与任何其他控制器处于级联模式时，这些参考接口为 available（可用）且 unclaimed（未认领）（此时控制器从订阅者获取其参考值）。
• 当导出参考接口的控制器处于活动状态且与其他控制器处于级联模式时，这些参考接口为 available（可用）且 claimed（已认领）（此时控制器从与其级联的控制器获取参考值）。

2.1.5 结语#

• 或许没有必要新增 ChainableController 控制器类型。也可以考虑在 ControllerInterface 类中直接实现 export_reference_interfaces() 和 export_state_interfaces() 方法，默认返回 interface_configuration_type::NONE。

2.2.1 术语#

自由度（Degrees of Freedom, DoF）

来自维基百科：

在物理学中，机械系统的自由度（DoF）是定义其构型或状态所需的独立参数数量。

关节（Joint）

关节是两个连杆之间的连接。在 ROS 生态中，三种类型较为典型：转动副（Revolute，带位置限位的铰链）、连续副（Continuous，没有任何位置限位的连续铰链）或移动副（Prismatic，沿轴线滑动的关节）。

一般来说，关节可以是驱动的（actuated）或非驱动的（non-actuated），后者也称为被动（passive）。被动关节没有自己的驱动机构，而是通过外力或被其他关节被动带动来实现运动。被动关节可以具有一个 DoF（如单摆），也可以是零 DoF 的并联机构的一部分。

串联运动学（Serial Kinematics）

串联运动学指的是机械臂中关节的排列方式：每个关节彼此独立，关节数量等于运动链的自由度。

一个典型例子是具有六个转动关节、6 自由度的工业机器人。每个关节可以独立驱动，末端执行器可以在工作空间内移动到任意位置和姿态。

运动学回路（Kinematic Loops）

另一方面，运动学回路（也称为闭链机构）涉及多个关节在一条运动链中连接并一起驱动。这意味着这些关节是耦合的，不能独立移动：一般来说，自由度数小于关节数。这种结构是并联机构的典型特征，其末端执行器通过多条运动链连接到基座。

一个例子是四连杆机构（four-bar linkage），由四个连杆和四个关节组成。尽管有四个关节，它可以有一个或两个驱动器，因而具有一个或两个自由度。此外，我们可以说它有一个（两个）驱动关节和三个（两个）被动关节，它们必须满足机构的运动学约束。

2.2.2 URDF#

URDF 是 ROS 中描述机器人运动学的默认格式。然而，除所谓的 mimic（模仿）关节外，URDF 只支持串联运动链。更多细节参见 URDF 规范。

Mimic 关节可以在 URDF 中以如下方式定义：

1
<joint name="right_finger_joint" type="prismatic">
2
  <axis xyz="0 1 0"/>
3
  <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
4
  <parent link="base"/>
5
  <child link="finger_right"/>
6
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
7
</joint>
8
<joint name="left_finger_joint" type="prismatic">
9
  <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
10
  <axis xyz="0 1 0"/>
11
  <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
12
  <parent link="base"/>
13
  <child link="finger_left"/>
14
  <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
15
</joint>

Mimic 关节是真实世界的抽象。例如，它们可用于描述：

• 关节位置与速度呈线性相关的简单闭链运动学
• 通过皮带连接的连杆，如皮带-带轮系统或伸缩臂
• 被动关节的简化模型，例如末端执行器上始终指向下方的单摆
• 抽象的运动关节复合组，其中多个关节由底层控制回路直接控制并同步运动。举个不涉及真实世界的例子：多个电机拥有各自的功率电子器件，但接收同一个设定值指令。

URDF 中定义的 mimic 关节由资源管理器（resource manager）解析，并存储在类型为 HardwareInfo 的类变量中，硬件组件可以访问该变量。Mimic 关节不能有命令接口，但可以有状态接口。

1
<ros2_control>
2
  <joint name="right_finger_joint">
3
    <command_interface name="effort"/>
4
    <state_interface name="position"/>
5
    <state_interface name="velocity"/>
6
    <state_interface name="effort"/>
7
  </joint>
8
  <joint name="left_finger_joint">
9
    <state_interface name="position"/>
10
    <state_interface name="velocity"/>
11
    <state_interface name="effort"/>
12
  </joint>
13
</ros2_control>

在官方发布的包中，以下包已经在使用这些信息：

• mock_components（generic system）
• gz_ros2_control

由于 URDF 只规定运动学，mimic 标签必须独立于 ros2_control 中使用的硬件接口类型。这意味着我们按如下方式解释这些信息：

• position = multiplier * other_joint_position + offset
• velocity = multiplier * other_joint_velocity

如果有人出于任何原因想在不修改 URDF 的情况下停用 mimic 关节行为，可以通过在 XML 的 <ros2_control> 部分设置关节标签的 mimic=false 属性来实现：

1
<joint name="left_finger_joint" mimic="false">
2
  <state_interface name="position"/>
3
  <state_interface name="velocity"/>
4
  <state_interface name="effort"/>
5
</joint>

2.2.3 传动接口（Transmission Interface）#

机械传动会对力/流（effort/flow）变量进行变换，使其乘积（功率）保持不变。线性和旋转域的力变量分别为力和力矩；流变量则分别为线速度和角速度。

在机器人学中，习惯将传动装置放置在驱动器和关节之间。该接口遵循这一命名习惯，以标识变换的输入和输出空间。所提供的接口支持在力、速度和位置的驱动器空间与关节空间之间进行双向映射。位置不是功率变量，但可以通过速度映射加上一个代表驱动器与关节零点之间偏移的积分常数来实现该映射。

transmission_interface 为插件提供基类和若干实现，插件可由自定义硬件组件集成并加载。它们不会由任何硬件组件或 gazebo 插件自动加载；每个硬件组件负责加载适当的传动接口，以将驱动器读数映射为关节读数。

目前可用的实现如下：

• SimpleTransmission：具有恒定减速比且无额外动力学的简单传动。
• DifferentialTransmission：具有两个驱动器和两个关节的差速传动。
• FourBarLinkageTransmission：具有两个驱动器和两个关节的四连杆传动。

更多信息，参见 example_8 或 transmission_interface 文档。

2.2.4 仿真闭链运动学#

根据不同的仿真插件，可以采用不同的方法来仿真闭链运动学。以下列表概述了可用的仿真插件及其能力：

gazebo_ros2_control：

• mimic 关节
• 通过在 URDF 中使用 <gazebo> 标签支持闭链运动学，参见相关示例。

gz_ros2_control：

• mimic 关节
• 尚不直接支持闭链运动学，但可以通过自定义插件使用 DetachableJoint 来实现。可关注该 issue 以获取此主题的更新。

2.3.1 启用关节限位#

在 ros2_control 中有几种控制关节限位处理的方式：

• 全局：通过 ros2_control_node 参数 enforce_command_limits 对所有硬件组件的所有接口全局启用，详见前文。
• 针对关节的所有接口：

1
<joint name="joint1">
2
  ...
3
  <limits enable="false"/>
4
  ...
5
</joint>

• 针对关节的单个接口：

1
<joint name="joint1">
2
  ...
3
  <command_interface name="position">
4
    <limits enable="false"/>
5
  </command_interface>
6
  ...
7
</joint>

如果某个特定接口启用了关节限位，controller_manager 会在启动时打印类似如下消息：

1
[controller_manager]: Using JointLimiter for joint 'joint1' in hardware 'RRBot' : '  has position limits: true [-1, 1]
2
[ros2_control_node-1]   has velocity limits: true [1]
3
[ros2_control_node-1]   has acceleration limits: false [nan]
4
[ros2_control_node-1]   has deceleration limits: false [nan]
5
[ros2_control_node-1]   has jerk limits: false [nan]
6
[ros2_control_node-1]   has effort limits: true [100]
7
[ros2_control_node-1]   angle wraparound: true'

2.3.2 限位的配置#

待补充（tba）

2.3.3 限位器算法说明#

待补充（tba）

2.4.1 异步控制器（Asynchronous Controllers）#

原文：https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/running_controllers_asynchronously.html

ros2_control 框架允许控制器异步运行。当控制器的 update() 方法包含阻塞调用或需要更长的执行时间而影响控制器管理器的控制循环周期时，异步运行非常有用；否则会影响控制循环的周期性并导致超时（overruns）。

例如，如果控制器管理器的 update_rate 为 100Hz，则所有控制器的 update() 调用与硬件组件的 read() 和 write() 调用的执行时间之和必须低于 10ms。如果某个控制器需要 15ms 的执行时间，那么它无法在同步模式下执行而不影响整体系统更新频率。异步运行这样的控制器，可以使其在专用线程上执行，而不阻塞主控制循环。

注意

异步更新支持对每个控制器实现是透明的。任何现有控制器只需将 is_async 参数设为 true 即可异步运行。

1
controller_manager:
2
  ros__parameters:
3
    update_rate: 100  # Hz
4

5
example_async_controller:
6
  ros__parameters:
7
    type: example_controller/ExampleController
8
    is_async: true
9
    update_rate: 20  # Hz

1
example_async_controller:
2
  ros__parameters:
3
    type: example_controller/ExampleController
4
    is_async: true
5
    update_rate: 20  # Hz
6
    async_parameters:
7
      thread_priority: 50
8
      cpu_affinity: [2, 4]

1
[ros2_control_node-1] [WARN] [1741626670.311533972] [example_async_controller]: The controller missed xx update cycles out of yy total triggers.

1
[ros2_control_node-1] [ERROR] [1741629098.352771957] [AsyncFunctionHandler]: AsyncFunctionHandler: Exception caught in the async callback thread!
2
...
3
[ros2_control_node-1] [ERROR] [1741629098.352874151] [controller_manager]: Caught exception of type : St13runtime_error while updating controller
4
[ros2_control_node-1] [ERROR] [1741629098.352940701] [controller_manager]: Deactivating controllers : [example_async_controller] as their update resulted in an error!

2.4.2 异步硬件组件（Asynchronous Hardware Components）#

原文：https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/asynchronous_components.html

ros2_control 框架允许异步运行硬件组件。当某些硬件组件需要在单独的线程或执行器中运行时非常有用。例如，某个传感器读取数据耗时较长，影响了 controller_manager 控制循环的周期性。这种情况下，可以让该传感器在单独的线程或执行器中运行，以避免阻塞控制循环。

1
<ros2_control name="RRBotSystemMutipleGPIOs" type="system">
2
  <hardware>
3
    <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
4
    <param name="example_param_hw_start_duration_sec">2.0</param>
5
    <param name="example_param_hw_stop_duration_sec">3.0</param>
6
    <param name="example_param_hw_slowdown">2.0</param>
7
  </hardware>
8
  <joint name="joint1">
9
    <command_interface name="position">
10
      <param name="min">-1</param>
11
      <param name="max">1</param>
12
    </command_interface>
13
    <state_interface name="position"/>
14
  </joint>
15
  <joint name="joint2">
16
    <command_interface name="position">
17
      <param name="min">-1</param>
18
      <param name="max">1</param>
19
    </command_interface>
20
    <state_interface name="position"/>
21
  </joint>
22
  <gpio name="flange_digital_IOs">
23
    <command_interface name="digital_output1"/>
24
    <state_interface name="digital_output1"/>    <!-- Needed to know current state of the output -->
25
    <command_interface name="digital_output2"/>
26
    <state_interface name="digital_output2"/>
27
    <state_interface name="digital_input1"/>
28
    <state_interface name="digital_input2"/>
29
  </gpio>
30
</ros2_control>
31
<ros2_control name="MultimodalGripper" type="actuator" is_async="true">
32
  <properties>
33
    <async affinity="[2,4]" scheduling_policy="synchronized" print_warnings="true" thread_priority="30"/>
34
  </properties>
35
  <hardware>
36
    <plugin>ros2_control_demo_hardware/MultimodalGripper</plugin>
37
  </hardware>
38
  <joint name="parallel_fingers">
39
    <command_interface name="position">
40
      <param name="min">0</param>
41
      <param name="max">100</param>
42
    </command_interface>
43
    <state_interface name="position"/>
44
  </joint>
45
</ros2_control>
46
<ros2_control name="RRBotForceTorqueSensor2D" type="sensor" is_async="true">
47
  <hardware>
48
    <plugin>ros2_control_demo_hardware/ForceTorqueSensor2DHardware</plugin>
49
    <param name="example_param_read_for_sec">0.43</param>
50
  </hardware>
51
  <sensor name="tcp_fts_sensor">
52
    <state_interface name="fx"/>
53
    <state_interface name="tz"/>
54
    <param name="frame_id">kuka_tcp</param>
55
    <param name="fx_range">100</param>
56
    <param name="tz_range">100</param>
57
  </sensor>
58
  <sensor name="temp_feedback">
59
    <state_interface name="temperature"/>
60
  </sensor>
61
  <gpio name="calibration">
62
    <command_interface name="calibration_matrix_nr"/>
63
    <state_interface name="calibration_matrix_nr"/>
64
  </gpio>
65
</ros2_control>