Gazebo仿真学习——3.使用ROS-control设置模拟控制器驱动机器人关节

1.Kinova_ROS软件包官网： https://github.com/Kinovarobotics/kinova-ros#moveit.
2.Gazebo for Kinova robots官网： https://github.com/Kinovarobotics/kinova-ros/wiki/Gazebo.

下载kinova-ros软件包：

git clone https://github.com/Kinovarobotics/kinova-ros

【说明】kinova-ros 机械臂使用ros_control 来控制 Gazebo 中的机器人模型。为三种机器人类型添加了使用 ros-control 控制关节的配置文件。提供三种不同类型的控制器，力，位置，速度。还添加了轨迹控制器以提供MoveIt！接口。

ros_control tutorial: http://gazebosim.org/tutorials/?tut=ros_control.
【预备知识ros-control】https://wiki.ros.org/ros_control.
【ros-control参考博客】https://www.cnblogs.com/lizhensheng/p/11253331.html.
【ros-control参考知乎】https://zhuanlan.zhihu.com/p/94500307.
【前言】ROS提供了丰富的机器人应用：SLAM、导航、Moveit!..…这些功能包就是通过ros_control应用到机器人上的。
ros_control充当了实际机器人和机器人仿真器之间的连接件。包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等，可以帮助机器人应用快速落地，提高开发效率。简单来说，就是将上层指令比如角速度、线速度等指令转换为电机能够执行的指令的中间件。

1.ROS-control 总体架构

针对不同类型的控制器（底盘、机械臂等），ros_control可以提供多种类型的控制器，但是这些控制器的接口各不相同，为了提高代码的复用率，ros_control还提供一个硬件的抽象层。

硬件抽象层负责机器人硬件资源的管理，而controller从抽象层请求资源即可，并不直接接触硬件。

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

2.Controllers

ros_control 功能包提供的控制器种类如下：
自己创建需要的controller参考: https://github.com/ros-controls/ros_control/wiki/controller_interface.

3.Hardware Interface

Hardware Interface是controller和RobotHw沟通的接口，ROS控制将硬件接口与上述ROS控制器之一结合使用，以向硬件发送和接收命令，可以自己创建需要的接口，可以参考：https://github.com/ros-controls/ros_control/wiki/hardware_interface.

4.Transmissions

Transmissions就是机器人的传动系统，机器人每个需要运动的关节都需要配置相应的Transmission，大部分情况下在URDF文件中直接添加该部分：https://wiki.ros.org/urdf/XML/Transmission.
传输元素的示例：

5.Joint Limits

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

5.1 从URDF中设置Joint Limits

URDF不支持加速和加速度限制。这些值可以由其他YAML规范提供。
YAML规范不仅可以用于指定加速度和加速度极限，而且可以覆盖URDF描述中包含的位置，速度和力值。

5.2 通过YAML配置文件先加载到参数服务器中，YAML文件格式如下

6.Controller manager

Controller_manager用于实现对控制器的加载、运行、停止等操作，根据你是从启动文件、命令行还是从ROS节点运行控制器，控制器管理器提供了以下三种方式来运行控制器: http://wiki.ros.org/controller_manager.

6.1 命令行工具：

$ rosrun controller_manager controller_manager <command> <controller_name>

command 包括：

• load: 加载控制器（构造和初始化）
• unload: 卸载控制器（销毁）
• start:启动控制器
• stop: 停止控制器
• spawn: 加载和启动控制器
• kill: 停止并卸载控制器

查看 controller 的状态：

$ rosrun controller_manager controller_manager <command>

command 包括：

• list ：按执行顺序列出所有控制器，并给出每个控制器的状态
• list-types ：列出控制器管理器知道的所有控制器类型。
• reload-libraries ：重新加载所有可作为插件使用的控制器库。
• reload-libraries --restore ：重新加载所有可作为插件使用的控制器库，并将所有控制器恢复为原始状态。

spawner工具：
很多时候我们需要控制的controller有很多，比如六轴机器人，至少有六个controller，这时也可以使用“spawner ”这个命令来一次控制多个controller：

$ rosrun controller_manager spawner [--stopped] name1 name2 name3

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

 $ rosrun controller_manager unspawner name1 name2 name3

6.2 Launch 工具：

在launch文件中，同样可以通过运行controller_manager包的命令，来加载和启动一系列controller：

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

上边的launch文件会加载并启动controllers，如果只需要加载：

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

6.3 可视化工具：

rqt_controller_manager是一个rqt插件，允许以图形方式加载，卸载，启动和停止控制器，以及显示有关已加载控制器的信息。
它可以从rqt的“插件”菜单启动，也可以作为具有以下内容的独立可执行文件启动：

rosrun rqt_controller_manager rqt_controller_manager

7. 实际案例分析与体会

RRBot in Gazebo:http://gazebosim.org/tutorials/?tut=ros_control.
使用ros_control和简单的Gazebo插件适配器在Gazebo中模拟机器人的控制器。
安装ros_control，ros_controllers及其依赖项。
http://gazebosim.org/tutorials?tut=ros_installing&cat=connect_ros.
查看安装Gazebo的位置以及位置是否正确：
可见我是从debian 安装的。

• ROS Kinetic

sudo apt-get install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-ros-control

在启动roscore之后，通过简单的rosrun命令运行Gazebo：

cd catkin_ws
source devel/setup.bash
rosrun gazebo_ros gazebo

出现 gazebo GUI界面：

7.1 Add transmission elements to a URDF

RRBot 有两个关节，每个关节都有一个传动装置，所以有两个Transmissions，transmission元素用于将执行器链接到关节：（代码见rrbot.xacro文件）

7.2 Add the gazebo_ros_control plugin

在rrbot.gazebo中有 gazebo_ros_control 插件，该插件读取所有transmission标签（解析传输标签并加载适当的硬件接口和控制器管理器）：

7.3 create a ros_controls package（controller和controller_manager）

7.3.1 在config 文件中创建 .yaml 配置文件

PID增益和控制器设置必须保存在yaml文件中，该文件通过roslaunch文件加载到参数服务器中。
其中还包含一个joint_state_controller，来控制发布每个关节的实时状态。

7.3.2 创建一个 roslaunch 文件

这个例程中没有涉及到 Joint Limits 。

7.3.3 Start the controllers using roslaunch

（1）启动 RRBot 仿真：

roslaunch rrbot_gazebo rrbot_world.launch

【报错】
【分析】
python在安装时，默认的编码是ascii，当程序中出现非ascii编码时，python的处理常常会报这样的错UnicodeDecodeError: ‘ascii’ codec can’t decode byte 0x?? in position 1: ordinal not in range(128)，python没办法处理非ascii编码的，此时需要自己设置将python的默认编码，一般设置为utf8的编码格式。就是因为python默认字符串编码惹得祸,由于python默认是ascii码,但是在linux下的字符串是utf8,所以有些字符串无法识别,从而导致设置param失效。
参考博客：https://blog.csdn.net/weixin_43922901/article/details/89244457.
【解决】
在运行命令前执行：

export PYTHONIOENCODING=utf-8 >> ~/.bashrc
source ~/.bashrc
source devel/setup.bash

就像这样：

（2）加载两个控制器：
新开一个终端

cd catkin_ws
source devel/setup.bash
roslaunch rrbot_control rrbot_control.launch

（3）手动发送示例命令:

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: 5.0"

（4）控制 joint1 和 joint2 的位置：