一砍艾、 ROS中的控制器插件
ros_control的功能：
- ROS為開發(fā)者提供的機器人控制中間件
- 包含一系列控制器接口雹洗、傳動裝置接口揖庄、硬件接口肮街、控制器工具箱等等
- 可以幫助機器人應用功能包快速落地，提高開發(fā)效率
下面介紹一下采用ros_control實現(xiàn)的控制系統(tǒng)整體架構：
- 控制器管理器
提供一種通用的接口來管理不同的控制器饮笛，比如小車的輪子需要速度指令咨察，機械臂需要位置、扭矩指令福青，不同的控制器就需要一個controller manager來管理摄狱，以同時包含多種controller - 控制器
讀取硬件狀態(tài)，發(fā)布控制命令无午，完成每個joint的控制 - 硬件資源
為上下兩層提供硬件資源的接口 - 機器人硬件抽象
機器人硬件抽象和硬件資源直接打交道媒役，通過write和read方法完成硬件操作 - 真實機器人
執(zhí)行接收到的命令
controller中主要實現(xiàn)的是PID控制，完成速度指厌、位置等閉環(huán)控制刊愚，并將指令發(fā)出去，發(fā)給誰呢踩验，當然是機器人，如果是仿真就發(fā)給gazebo商玫，如果是實際機器人箕憾，就發(fā)給真實機器人硬件。這里就有一個interface拳昌，通過串口袭异，網口等接口的形式把數據發(fā)出去hardware_interface是為了讓上層的接口屏蔽底層的硬件，作為一個硬件封裝層封裝了底層的電機炬藤、執(zhí)行器御铃。接收到上層的指令后，再分發(fā)到底層的硬件沈矿。比如發(fā)給小車車輪上真，上面通常會有一個嵌入式板卡來完成本地的閉環(huán)控制，比如發(fā)送電壓信號給電機羹膳，電機如果配備編碼器睡互，還可以通過硬件抽象層反饋上來，發(fā)回controller，再形成閉環(huán)就珠。
ros_control這個功能包包含了很多controllers寇壳，完成速度、位置妻怎、力控制壳炎。常用的有四個controller:
joint_state_controller
這個controller其實和控制沒關系，比較容易讓人誤解逼侦，監(jiān)控機器人狀態(tài)匿辩，比如編碼器的反饋回來的位置速度等數據，它將其封裝成topic發(fā)出來偿洁，功能和joint_state_publisher這個節(jié)點是一樣的撒汉，只是數據輸入不一樣，輸出都是話題形式涕滋。joint_effort_controller
joint_position_controller
機械臂經常會用這個controllerjoint_velocity_controller
此處會介紹 joint_state_controller和joint_position_controller睬辐，最后會再介紹一個更加上層的controller。因此ros_control在整個ROS框架中的作用就是作為一個中間件宾肺，銜接上層應用和真實機器人或仿真模型溯饵。
2.完善機器人模型
要放到gazebo中仿真，之前的urdf模型文件還需要再進行完善锨用。
2.1 模型文件
第一步：在可視化基礎上為link添加慣性參數和碰撞屬性
一個技巧是設置很小的質量和較大的慣性矩陣丰刊，這樣導入gazebo后模型運行會比較穩(wěn)定，當然前提是仿真應用對這些物理參數要求不高增拥。
第二步：為joint添加傳動裝置
代表減速器等傳動裝置啄巧，傳進來的是控制指令，比如下圖的位置指令
第三步：添加gazebo控制器插件
電機如何轉起來掌栅，需要添加一個控制器秩仆，對應真實機器人的伺服驅動器，就是前面講的controller猾封，完成電機的運動控制澄耍，輸入位置，輸出到transmission上面去晌缘。參數是比較多的齐莲，一般在模型中加載插件，再通過多個參數文件具體配置磷箕。
robotNamespace代表具體的機器人命名空間选酗，仿真類型robotSimType，使用默認的硬件抽象層接口搀捷，完成上面的指令到機器人的轉換星掰。legacyModeNS用于兼容之前ROS版本的配置多望。
2.2 在gazebo中加載機器人
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 啟動gazebo仿真環(huán)境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find probot_description)/urdf/probot_anno.xacro'" />
<!-- 在gazebo中加載模型-->
<!--Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model probot_anno -param robot_description"/>
</launch>
launch文件完成的功能：一是啟動gazebo仿真環(huán)境，二是將模型加載到仿真環(huán)境中去氢烘。
i5@i5-ThinkPad-T470:~/catkin_ws$ roslaunch probot_gazebo probot_anno_gazebo_world.launch
現(xiàn)在的機械臂還是不能運動的怀偷，可以看一下話題：
i5@i5-ThinkPad-T470:~/catkin_ws$ rostopic list
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/rosout
/rosout_agg
3. 構建MoveIt!+Gazebo仿真
首先介紹一下moveit機器人控制框架，moveit輸入用戶指令播玖，比如起始位置椎工，輸出軌跡數據，每個點通過位置速度加速度描述蜀踏，為了通過話題發(fā)出來维蒙，還需要一個Follow Joint Trajectory功能，通過通信接口發(fā)給機器人果覆，機器人還需要對軌跡進行精插補颅痊，再驅動電機同步運動。機器人控制器還需要將每個電機的狀態(tài)數據反饋給moveit來確定機器人是否到達指定位置局待，這樣完成一個閉環(huán)斑响。
主要是理解三個模塊的作用：Follow Joint Trajectory，Joint Trajectory Controller, Joint State Controller
先看gazebo這一端的钳榨，軌跡通過Action機制發(fā)出來舰罚，因此gazebo這邊是一個action server，moveit那邊是一個action client薛耻，sever這一端接收軌跡营罢，完成插補運算，再發(fā)到每一個電機位置控制接口上饼齿。
ROS提供的關節(jié)軌跡控制器插補運算（具體可以看ros_controllers的源碼）
- 線性樣條：位置連續(xù)饲漾，速度、加速度不連續(xù)
- 三次樣條：位置和速度連續(xù)缕溉，加速度不連續(xù)
- 五次樣條：位置能颁、速度、加速度都連續(xù)（ROS默認使用的）
image.png
電機轉起來涉及到兩個配置文件倒淫，一個是config/probot_anno_trajectory_control.yaml，用于明確控制器接口接收哪幾個關節(jié)信息败玉，要與urdf模型對應敌土，文件中還包括每個關節(jié)的PID參數
probot_anno:
arm_joint_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
gains:
joint_1: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_2: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_3: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_4: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_5: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
joint_6: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
另一個文件是控制器啟動文件：probot_anno_trajectory_controller.launch，首先將上面的文件加載到參數服務器运翼，再調用spawner根據參數文件內容啟動arm_joint_controller返干，這個控制器完成每個軸的插補運算，并讓每個軸運動血淌。
<launch>
<rosparam file="$(find probot_gazebo)/config/probot_anno_trajectory_control.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/probot_anno" args="arm_joint_controller"/>
</launch>
以上完成的就是框架中綠色部分的配置矩欠。
joint state controller配置是類似的财剖，先進行參數配置：probot_anno_gazebo_joint_states.yaml
probot_anno:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
用于監(jiān)控機器人實時狀態(tài)，joint_state_controller/JointStateController就是ros cotrollers四種當中的一種癌淮，參數就是反饋的頻率躺坟。配置好后，需要啟動它probot_anno_gazebo_states.launch：
<launch>
<!-- 將關節(jié)控制器的配置參數加載到參數服務器中 -->
<rosparam file="$(find probot_gazebo)/config/probot_anno_gazebo_joint_states.yaml" command="load"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/probot_anno" args="joint_state_controller" />
<!-- 運行robot_state_publisher節(jié)點乳蓄，發(fā)布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/probot_anno/joint_states" />
</node>
</launch>
先加載文件咪橙，再通過spawner啟動控制器，后面還啟動了robot_state_publisher虚倒，用于根據joint_states發(fā)布TF坐標系.
第三個配置是框圖中左邊的美侦，F(xiàn)ollow Joint Trajectory，也是配參數controllers_gazebo.yaml：
controller_manager_ns: controller_manager
controller_list:
- name: probot_anno/arm_joint_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
再啟動probot_anno_moveit_controller_manager.launch.xml:
<launch>
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- gazebo Controller -->
<rosparam file="$(find probot_anno_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>
下面梳理一下所有文件魂奥，首先是最頂層的probot_anno_bringup_moveit.launch:
<launch>
<!-- Launch Gazebo -->
<include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_gazebo_world.launch" />
<!-- ros_control arm launch file -->
<include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_gazebo_states.launch" />
<!-- ros_control trajectory control dof arm launch file -->
<include file="$(find probot_gazebo)/launch/probot_anno/probot_anno_trajectory_controller.launch" />
<!-- moveit launch file -->
<include file="$(find probot_anno_moveit_config)/launch/moveit_planning_execution.launch" />
</launch>
首先啟動仿真環(huán)境菠剩，接著啟動Joint state反饋控制器，接著啟動完成插補功能的控制器耻煤，最后啟動moveit具壮，里面會自動包含剛才的moveit一端封裝action數據接口的功能。
總結一下幾個核心的文件功能：
probot_anno_bringup_moveit.launch:最頂層的违霞，啟動下面三個文件和moveit里的文件
probot_anno_gazebo_world.launch: 啟動仿真環(huán)境
probot_anno_gazebo_states.launch: 啟動joint state controller
probot_anno_trajectory_controller.launch：啟動插補運算的controller
probot_anno_moveit_controller_manager.launch.xml: 啟動follow joint trajectory功能
i5@i5-ThinkPad-T470:~$ roslaunch probot_gazebo probot_anno_bringup_moveit.launch
最后梳理一下啟動仿真的各個文件包含關系：
