给Gazebo中的机器人配上ROS的控制器

译者的话：

在Gazebo中搭建机器人，你可以用ROS控制这个机器人，前提是你需要先为机器人的关节配置控制器，借助ROS的ros_control和ros_controllers软件包，你可以将机器人关节与一个ROS提供的控制器连接。一旦配置好这个关节控制器，你就可以通过ROS提供的各种工具让你的机器人关节动起来，从而让机器人做各种动作。

ros_controllers软件包提供了一些已经做好的常用的控制器。ros_control包可以被看作是控制机器人的一套中间件，它包含了一系列控制器接口（controller interfaces）、传动装置接口（transmission interfaces）、硬件接口、控制器工具箱等等，这套中间件可以帮助机器人应用快速落地，提高开发效率。

ROS实现这套方法是试图将机器人的控制标准化。

ros_controllers软件包（代码地址）提供了各种控制器, 比如：

joint_state_controller

position_controllers

velocity_controllers

effort_controllers

joint_trajectory_controllers

diff_drive_controller

ackermann_steering_controller

等等

ros_control软件包（代码地址）提供了使用和管理上述这些控制器的各种功能，你可以把ros_control提供的这些功能叫“框架”，也可以叫“中间件”，比如:

controller interfaces（控制器接口）

controller managers（控制器管理器）

transmission

hardware_interfaces（硬件接口）

等等

控制器把机器人执行器（actuator）对应关节（joint）的当前状态数据和设定点数据(setpoint，设定的下一个关节的运动状态)作为输入，用一个通用的反馈控制回路（通常是一个 PID 控制器）来计算出对机器人执行器的输出（通常是力）并发送给执行器。如果机器人拥有多个关节和多个执行器，则需要建立某个执行器与某个关节之间的对应关系，ros_control用transmission元素来描述这些关系，一旦确立了对应关系，位置、速度和作用力将根据transmission_interface规定的方法从关节发送到执行器。

以下内容来自gazebo tutorial-ROS Control

ros_control和Gazebo之间的数据流

在Gazebo中模拟机器人的控制器可以使用ros_control和一个简单的Gazebo插件适配器来完成。下面是模拟、硬件、控制器和传输之间关系的概述。

先决条件

本教程基于以前教程中的许多概念。我们将使用在Gazebo教程中（在Gazebo中使用URDF）设置的RRBot作为这里涵盖的插件的示例。请确保您已按照安装说明中描述的方式安装了ros_control、ros_controllers及其依赖项。

使用方法

在URDF文件中添加transmission元素

用ros_control控制机器人需要向URDF添加一些附加元素。<transmission>元素用于把执行器（actuator）和关节（joint）连接在一起，参见<transmission>规范以获取确切的XML格式。

在当前实现的gazebo_ros_control中，transimission标签中最重要的信息是：

1. <joint name=“”> - 关节名称必须对应于URDF中的一个已经定义的关节。
2. <type> - 传输类型。目前仅实现了“transmission_interface/SimpleTransmission”。
3. <hardwareInterface> - 在<actuator>和<joint>标签中，指定gazebo_ros_control插件要加载哪种硬件接口（位置、速度或力接口）。目前仅实现了力接口（effort Interface）。

目前，其它名称和元素都将被忽略。

添加gazebo_ros_control插件

除了transmission标签之外，还需要向URDF添加一个Gazebo插件，该插件负责解析transmission标签并加载适当的硬件接口和控制器管理器。默认情况下，gazebo_ros_control插件是非常简单的，但也可以对插件进行扩展，让高级用户在ros_control和Gazebo之间创建自己的自定义的机器人硬件接口。

 默认的插件XML添加到URDF中：

<gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><robotNamespace>/MYROBOT</robotNamespace></plugin>
</gazebo>

gazebo_ros_control <plugin>标签还具有以下可选的子元素：

• <robotNamespace>：用于此插件实例的 ROS 命名空间，默认为 URDF/SDF 中的机器人名称
• <controlPeriod>：控制器更新的周期（以秒为单位），默认为 Gazebo 的周期
• <robotParam>：参数服务器上robot_description （URDF） 的位置，默认为“/robot_description”
• <robotSimType>：要使用的自定义机器人模拟界面的插件库pluginlib的名称（有关更多详细信息，请参见下文），默认为”DefaultRobotHWSim”

gazebo_ros_control的默认行为

默认情况下，如果没有<robotSimType>标签，gazebo_ros_control将尝试从URDF中获取所有与ros_control控制器接口所需的信息。这对于大多数情况来说已经足够了，至少能用。

默认行为提供以下ros_control接口：

1. o hardware_interface::JointStateInterface
2. o hardware_interface::EffortJointInterface
3. o hardware_interface::VelocityJointInterface – 还没有全部实现

高级: 客户化gazebo_ros_control插件

Gazebo插件gazebo_ros_control还提供了基于pluginlib的接口，以实现Gazebo和ros_control之间的自定义接口，用于模拟更复杂的机制（非线性弹簧，连杆等）。

这些插件必须继承gazebo_ros_control::RobotHWSim，后者实现了仿真的ros_control hardware_interface::RobotHW。RobotHWSim提供API级别的访问，以从Gazebo读取关节属性或向关节发送命令。

相应的RobotHWSim子类在URDF模型中指定，并在加载机器人模型时加载。例如，以下XML将加载默认插件（与不使用<robotSimType>标签时相同）

<gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><robotNamespace>/MYROBOT</robotNamespace><robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType></plugin>
</gazebo>

RRBot 例子

我们为每个在Gazebo中能动起来的关节添加类似以下内容的<transmission>块。请注意，<hardwareInterface>必须包含在<joint>和<actuator>标签中（请参见ros_control问题）。打开rrbot.xacro文件，应该在文件底部看到：

<transmission name="tran1"><type>transmission_interface/SimpleTransmission</type><joint name="joint1"><hardwareInterface>EffortJointInterface</hardwareInterface></joint><actuator name="motor1"><hardwareInterface>EffortJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission><transmission name="tran2"><type>transmission_interface/SimpleTransmission</type><joint name="joint2"><hardwareInterface>EffortJointInterface</hardwareInterface></joint><actuator name="motor2"><hardwareInterface>EffortJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission>

在rrbot.gazebo中能看到gazebo_ros_control插件，该插件将读取所有<transmission>标签中的内容：

<gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><robotNamespace>/rrbot</robotNamespace></plugin>
</gazebo>

建立一个ros_control包

接下来，我们为ros_control控制器创建配置文件和启动文件，利用他们与Gazebo接口。

建立一个新的包

mkdir ~/catkin_ws
cd ~/catkin_ws
catkin_create_pkg MYROBOT_control controller_manager joint_state_controller robot_state_publisher
cd MYROBOT_control
mkdir config
mkdir launch

建立一个.yaml 配置文件

PID增益和控制器设置必须保存在yaml文件中，该文件通过roslaunch文件加载到param参数服务器中。在MYROBOT_control包的config文件夹中，把RRBot给出的示例rrbot_control.yaml放到你自己的MYROBOT_control/config/目录下：

rrbot:# Publish all joint states -----------------------------------joint_state_controller:type: joint_state_controller/JointStateControllerpublish_rate: 50  # Position Controllers ---------------------------------------joint1_position_controller:type: effort_controllers/JointPositionControllerjoint: joint1pid: {p: 100.0, i: 0.01, d: 10.0}joint2_position_controller:type: effort_controllers/JointPositionControllerjoint: joint2pid: {p: 100.0, i: 0.01, d: 10.0}

建立一个roslaunch文件

为启动ros_control控制器创建一个roslaunch文件。在launch文件夹中建立一个MYROBOT_control/launch/MYROBOT_control.launch文件，并将以下RRBot示例调整为您的机器人：

<launch><!-- Load joint controller configurations from YAML file to parameter server --><rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/><!-- load the controllers --><node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_controller"/><!-- convert joint states to TF transforms for rviz, etc --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"respawn="false" output="screen"><remap from="/joint_states" to="/rrbot/joint_states" /></node></launch>

解释：

第一行“rosparam”通过加载yaml配置文件（在下一节中讨论）将控制器的设置参数加载到参数服务器上。

controller_spawner节点通过运行一个python脚本启动RRBot的2个关节位置控制器，joint1_position_controller和joint2_position_controller。有了这个脚本，就可以用rosservice call与ros_control控制器的管理器进行交互。通过rosservice call，你可以向ros_control控制器的管理器指定您要操作哪个控制器。这个节点还加载了第3个控制器joint_state_controller，该控制器发布通过hardware_interface能访问到的所有关节的关节状态，并在主题/joint_states上公布这些关节的状态数据。controller_spawner只是一个用于roslaunch的辅助脚本。

最后一行启动了robot_state_publisher节点，该节点监听joint_state_controller在/joint_states主题上发布的消息，然后将其做变换并发布到/tf。这就使您可以在Rviz中看到模拟的机器人以及机器人的动作。

用roslaunch启动控制器

通过运行以下命令来测试由ros_control控制的 RRBot：

启动 RRBot 仿真：

roslaunch rrbot_gazebo rrbot_world.launch

通过运行第二个启动文件加载两个关节的控制器：

roslaunch rrbot_control rrbot_control.launch

手动运行rosservice call

如果已经把rrbot_control.yaml文件加载到参数服务器中了，则可以通过服务请求（rosservice call）手动加载控制器，尽管我们通常更喜欢用roslaunch自动加载控制器。我们现在演示一下手动方法以供参考：

用rosservice call手动加载控制器：

rosservice call /rrbot/controller_manager/load_controller "name: 'joint1_position_controller'"
rosservice call /rrbot/controller_manager/load_controller "name: 'joint2_position_controller'"

用rosservice call手动启动控制器:

rosservice call /rrbot/controller_manager/switch_controller "{start_controllers: ['joint1_position_controller','joint2_position_controller'], stop_controllers: [], strictness: 2}"

用rosservice call手动停止控制器:

rosservice call /rrbot/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['joint1_position_controller','joint2_position_controller'], strictness: 2}"

手动向控制器发送命令

向关节控制器发送命令进行测试：

rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

使用RQT向控制器发送命令

在本节中，我们将介绍一些工具，通过这些工具可以让控制器的性能可视化，也可以调整控制器的增益以及其他参数，特别是PID增益。我们将用到RQT以及ROS的基于插件的用户界面，请确保您已经安装了这些工具。

启动RQT:

rosrun rqt_gui rqt_gui

增加一个Command Publisher

在RQT的“Plugins”菜单中选择“Topic->Message Publisher”，然后在Message Publisher对话框中的Topic下拉菜单中选择要发布到任何特定控制器的Topic。对于RRBot，

可以为如下控制器添加主题：

/rrbot/joint1_position_controller/command

然后按右上角的绿色加号按钮。

通过选中主题名称左侧的复选框启用主题发布者。将速率列设置为100（我们发送命令的频率-在这种情况下为100hz）。

接下来，展开主题，以便您看到“data”行。在数据行的表达式框中，尝试使用joint1的关节限制之间的不同弧度值-对于RRBot，因为关节是continuous的，所以没有限制，，因此任何值都可以。你可以让RRBot机器人的关节摇摆。

接下来，在同一表达式框中，我们可以使用正弦波自动更改控制量。输入以下内容：

sin(i/100)

对于更通用的控制，表达式可以设计为一种带变量的配置，可以用这个表达式向具备精确关节限制的机器人发送正弦波数据：

sin(i/rate*speed)*diff + offset

变量的解释：

1. i: 时间的RQT变量
2. rate：评估此表达式的频率。这应该与主题发布者的速率列中的数字相同。建议值为100。
3. speed：期望的执行器的执行速度。从1开始，速度较慢
4. upper_limit和lower_limit：由这个控制器控制的硬件的关节限制
5. diff =（upper_limit-lower_limit）/ 2
6. offset = upper_limit-diff

将控制器的性能可视化

在RQT的菜单中选择Plugins->Visualization->Plot，像上节一样，增加一个针对某个控制器的command publisher：

/rrbot/joint1_position_controller/command/data

在表达式框中输入：sin(i/100*5)*1.7+0

点击绿色按钮，你会看到正弦波显示出来了。

将另一个主题添加到 Plot 插件中，用于跟踪被控制的执行器的实际位置。需要通过调整PID增益来减小这两个值之间的误差，如下一步所示。 对于 RRBot：

/rrbot/joint1_position_controller/state/process_value

屏幕应如下所示：

调节pid增益

最后，我们将使用dynamic reconfigure来调整PID控制器的比例、微分和积分增益，假设这适用于您的机器人。

将“Dynamic Reconfigure”插件添加到 RQT，然后单击“Expand All”以查看子选项。 假设您的控制器使用 PID，您应该使用“pid”选项。 单击它应该会显示 5 个滑块，可用于调整控制器，如以下屏幕截图所示。 调整这些值，直到获得所需的控制器性能。

使用 roslaunch 保存您的 RQT 透视图

可以使用以下命令轻松启动 rrbot 的预配置 RQT 透视图：

roslaunch rrbot_control rrbot_rqt.launch

您可以将其用作模板，以使用自己的机器人执行此操作。

将 Rviz 连接到Gazebo仿真

现在，您在仿真中使用ros_control向机器人发送命令，您还可以使用 ros_control jointstatecontroller 从 Gazebo 读取机器人的状态。 一个好的仿真器背后的想法是，你应该能够在真实硬件上使用与仿真中相同的软件。 一个很好的起点是在 Rviz 中可视化您的仿真机器人，类似于使用真实硬件完成的方式。

假设您已经在启动joint_state_controller，如上文 rosparam 和 roslaunch 文件中所述，下一步是启动 Rviz：

rosrun rviz rviz

在“Global option”下，将您的“Fixed Frame”更改为“world”以解决它可能给您的任何错误。

接下来，将“RobotModel”显示类型添加到 Rviz，然后您应该会看到 Gazebo 中的仿真机器人在 Rviz 中可视化！

名词翻译：

Topic：主题

Pose:姿态

Twist：扭曲

Model：模型

Link：连杆

Joint：关节

Body：物体

Object：对象

Spawn：生成

Force：力

Wrenches ：扭矩