机器人建模与仿真（三）——ros_control

机器人建模与仿真（三）——ros_control

总述

ROS中提供了丰富的机器人应用：SLAM、导航、MoveIt…但是你可能一直有一个疑问，这些功能包到底应该怎么样用到我们的机器人上，也就是说在应用和实际机器人或者机器人仿真器之间，缺少一个连接两者的东西。
ros_control就是ROS为用户提供的应用与机器人之间的中间件，包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等，可以帮助机器人应用快速落地，提高开发效率。

一、ros_control框架

如上图所示为ros_control的总体框架，可以看到针对不同类型的机器人（移动机器人、机械臂等），ros_control可以提供多种类型的控制器(controller)，但是这些控制器的接口各不相同，为了提高代码的复用率，ros_control还提供一个硬件的抽象层。硬件抽象层负责机器人硬件资源的管理，而controller从抽象层请求资源即可，并不直接接触硬件。


上图是ros_control的数据流图，可以更加清晰的看到每个层次包含的功能：
（1）控制器管理器（Controller Manager）
每个机器人可能有多个controller，所以这里有一个控制器管理器的概念，提供一种通用的接口来管理不同的controller。控制器管理器的输入就是ROS上层应用功能包的输出。
（2）控制器（Controller）
控制器可以完成每个joint的控制，读取硬件资源接口中的状态，再发布控制命令。
（3）硬件资源（Hardware Rescource）
为上下两层提供硬件资源的接口。
（4）机器人硬件抽象（RobotHW）
机器人硬件抽象层和硬件资源直接打交道，通过write和read方法来完成硬件的操作，这一层也包含关节约束、力矩转换、状态转换等功能。
（5）真实机器人（Real Robot）
真实机器人上也需要有自己的嵌入式控制器，将接收到的命令反映到执行器上，比如接收到旋转90度的命令后，就需要让执行器快速、稳定地旋转90度。

二、控制器

ros_controllers这个功能包提供了以下控制器：


当然，我们也可以根据自己的需求，创建需要的控制器，然后通过控制器来管理自己创建的控制器，创建控制器的具体方法可以参考https://github.com/ros-controls/ros_control/wiki/controller_interface

三、硬件接口

硬件接口是控制器和RobotHw沟通的接口，基本与控制器的种类相互对应，同样可以自己创建需要的接口，具体实现方法可以参考：https://github.com/ros-controls/ros_control/wiki/hardware_interface

四、传动系统

传动系统（Transmissions）可以将机器人的关节指令转换成执行器的控制信号。机器人每个需要运动的关节都需要配置相应的传动系统，可以通过代码完成https://github.com/ros-controls/ros_control/wiki/transmission_interface，但大部分情况下，都会在URDF文件中直接添加（http://ros.org/wiki/urdf/XML/Transmission）：

<transmission name="simple_trans">
     <type>transmission_interface/SimpleTransmission</type>
     <joint name="foo_joint">
          <hardwareInterface>EffortJointInterface</hardwareInterface>
     </joint>
     <actuator name="foo_motor">
          <mechanicalReduction>50</mechanicalReduction>
          <hardwareInterface>EffortJointInterface</hardwareInterface>
     </actuator>
</transmission>

五、关节约束

关节约束（Joint Limits）是硬件抽象层中的一部分，维护一个关节约束的数据结构，这些约束数据可以从机器人的URDF文件中加载，也可以在ROS的参数服务器上加载（需要先用YAML配置文件导入ROS 参数服务器），不仅包含关节速度、位置、加速度、加加速度、力矩等方面的约束，还包含起安全作用的位置软限位、速度边界（k_v）和位置边界（k_p）等。
可以使用如下方式在URDF中设置Joint Limits参数：

<joint name="$foo_joint" type="revolute">
  <!-- other joint description elements -->
 
  <!-- Joint limits -->
  <limit lower="0.0"
         upper="1.0"
         effort="10.0"
         velocity="5.0" />
 
  <!-- Soft limits -->
  <safety_controller k_position="100"
                     k_velocity="10"
                     soft_lower_limit="0.1"
                     soft_upper_limit="0.9" /> 
</joint>

还有一些参数需要通过YAML配置文件事先加载到参数服务器中，YAML文件的格式如下：

joint_limits:
  foo_joint:
    has_position_limits: true
    min_position: 0.0
    max_position: 1.0
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 5.0
  bar_joint:
    has_position_limits: false # Continuous joint
    has_velocity_limits: true
    max_velocity: 4.0

六、控制器管理器

controller_manager提供了一种多控制器控制的机制，可以实现控制器的加载、开始运行、停止运行、卸载等多种操作。如下图所示的就是controller_manager控制控制器实现的状态跳转。

controller_manager还提供多种工具来辅助完成这些操作。
1、命令行工具
controller_manager命令的格式为：

rosrun controller_manager controller_manager <command> <controller_name>

支持的如下 ：
load: 加载一个控制器（load a controller (construct and initialize)）
unload: 卸载一个控制器（unload a controller (destruct)）
start: 启动控制器（start a controller）
stop: 停止控制器（stop a controller）
spawn: 加载并启动一个控制器（load and start a controller）
kill: 停止并卸载一个控制器（stop and unload a controller）
如果想要查看某个控制器的状态，可以使用如下命令：

rosrun controller_manager controller_manager <command>

支持的 ：
list: 根据执行顺序列出所有控制器，并显示每个控制器的状态。（list all the controllers in the order they are executed, and give the state of each controller）
list-types: 显示所有控制器类型。（list all the controller types the controller manager knows about. If your controller is not in this list, you won’t be able to spawn it.）
reload-libraries: 以插件的形式重载所有控制器的库，不需要重新启动，方便对控制器的开发和测试。（Reloads all the controller libraries that are available as plugins. This is convenient when you are developing a controller and you want to test your new controller code, without restarting the robot every time. This does not restart controllers which were running before.）
reload-libraries --restore: 以插件的形式重载所有控制器的库，并恢复到初始状态。（Reloads all the controller libraries that are available as plugins and restores all controllers to their original state.）
但是很多时候我们需要控制的控制器有很多，比如六轴机器人至少有六个控制器，这时也可以使用spawner 命令一次控制多个控制器：

rosrun controller_manager spawner [--stopped] name1 name2 name3

上边的命令可以自动加载、启动控制器，如果加上–stopped参数，那么控制器则只会被加载，但是并不会开始运行。如果想要停止一系列控制器，但是不需要卸载，可以使用如下命令：

rosrun controller_manager unspawner name1 name2 name3

2、launch工具
在launch文件中，同样可以通过运行controller_manager命令，加载和启动一系列控制器：

 <launch>
   <node pkg="controller_manager"
         type="spawner"
         args="controller_name1 controller_name2" /> 
 </launch>

以上launch文件会加载并启动controller，如果只需要加载不必启动，可以使用以下配置：

<launch>
  <node pkg="controller_manager"
    type="spawner"
    args="--stopped controller_name1 controller_name2" />
</launch>

3.可视化工具rqt_controller_manager
controller_manager还提供了可视化工具rqt_controller_manager，安装成功后，直接使用以下命令即可打开界面：

rosrun rqt_controller_manager rqt_controller_manager

七、Gazebo仿真

1、机器人模型添加Gazebo属性
（1）为link添加< gazebo >标签
（2）添加传动装置
（3）添加Gazebo控制器插件
2、在Gazebo中显示机器人模型
3、控制机器人在Gazebo中运动
4、摄像头仿真
（1）为摄像头模型添加Gazebo插件
（2）运行仿真环境
5、Kinect仿真
（1）为Kinect模型添加Gazebo插件
（2）运行仿真环境
6、激光雷达仿真
（1）为rplidar模型添加Gazebo插件
（2）运行仿真环境