通过ROS控制真实机械臂(3)---Gazebo配合Rviz 仿真机械臂_ros机械臂案例

###### gazebo配合rviz 仿真机械臂 #######

一旦机器人有超过6个关节，逆向运动学函数不唯一，可能存在多个解，逆向运动学很难，需要ROS中的逆向运动学包的使用，简化成矩阵的运算对于机械臂而言,需要的是能操作follow_joint_trajectory/joint_states接口的插件，需要用到两个插件, 分别用于从follow_joint_trajectory接受轨迹并产生控制的ros_control和对外发布joint_states数据的ros_joint_state_publisher。

仿真机械臂，首要的就是机器臂模型的URDF，不同于移动机器人的URDF，机械臂的仿真模型要重要的多，也需要更精确，这样才能便于控制的进行和效果的观察。在之前的redwall_arm包中继续添加文件，用于配置机械臂的模型参数，通过SolidWorks将三维模型的关节导出成STL格式，并保存到ros包的meshes目录,具体做法请参考https://blog.csdn.net/qq_34935373/article/details/88647161：

编辑config配置文件和urdf文件，创建了一个ros_control控制器,因为机械臂的控制方式一般是每个关节的位置，所以使用position_controller，joints表示的是每个关节，与urdf中定义的相符合。内容如下：

arm_controller.yaml

arm_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - Link1
    - Link2
    - Link3
    - Link4
    - Link5

要使用ros_control的话首先要对urdf中的每个关节定义相应的transmission(用于描述该关节的运动与电机转动输出的关系，transmission通常包括一个减速比，还包括其他如机械耦合等复杂的东西。可参考http://www.guyuehome.com/890

使用关节限定角度，可以给定continuous,然后启动joint control GUI观看一下极限位置和角度,记下数值（弧度）,然后修改continuous为revolute, 并设置lower和upper：<limit effort ="1000.0" lower="-1.3" upper="1.3" velocity="0.5"/>, 这里要注意一点,很多人在通过SolidWorks插件生成URDF的时候忘记配置了effor这些参数,给的数据默认为0,就会对后续的力控制相关的controller带来不确定的影响,或者在gazebo仿真的时候机器人完全不动.我之前就出现过这样的问题,经过长时间一步一步的排查才发现配置URDF和XACRO文件的时候effort给的数据为0.

由于arm_controller.yaml中定义的是"position_controllers/JointTrajectoryController"，则URDF中定义的transmission中如果使用的是<hardwareInterface>PositionJointInterface</hardwareInterface>必须要对应，否则会提示找不到controller。

最后一段的是在urdf中加入gazebo的ros_control插件，如果不加，运行gazebo会显示机械臂都耷拉在地上，仿佛电机没有使能一样。

    <gazebo>
        <plugin name="control" filename="libgazebo_ros_control.so" />
        <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
            <jointName>Link1,Link2,Link3,Link4,Link5</jointName>
        </plugin>
    </gazebo>

###### 安装gazebo_ros和ros_control(事先安装好了gazebo7) #######
    $ sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
    $ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
    $ sudo apt-get update
    $ sudo apt-get install gazebo7 sudo apt-get install libgazebo7-dev
    $ sudo apt-get install ros-indigo-gazebo-ros-pkgs ros-indigo-gazebo-ros-control提示依赖问题,因为我事先安装了gazebo7，所以如下
    $ sudo apt-get install ros-indigo-gazebo7-ros-pkgs ros-indigo-gazebo7-ros-control
 
用gazebo模拟的时候出现了下面这个问题
Could not load controller 'joint_state_controller' because controller type 'joint_state_controller/JointStateController' does not exist
原因是这几个包不是自带的，必须要手动装，解决方法在https://github.com/qboticslabs/mastering_ros/issues/7
    $ sudo apt-get install ros-indigo-joint-state-controller
    $ sudo apt-get install ros-indigo-effort-controllers
    $ sudo apt-get install ros-indigo-position-controllers
    $ sudo apt-get install ros-indigo-joint-trajectory-controller

继续添加launch文件：gazebo.launch，功能为启动gazebo仿真模型，其中使用了spawn_model节点自动管理ros_control

<launch>
  <rosparam file="$(find redwall_arm)/config/arm_controller.yaml" command="load" />
 
  <param name="robot_description" textfile="$(find redwall_arm)/robots/redwall_arm.urdf"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch" />
 
  <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model redwall_arm"/>
  <node name="controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller" />
 
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
</launch>

model.launch文件 ，功能为启动Rviz仿真

<launch>
  <arg name="model" />
  <arg name="gui" default="False" />
 
  <rosparam file="$(find redwall_arm)/config/arm_controller.yaml" command="load" />
 
  <param name="robot_description" textfile="$(find redwall_arm)/robots/redwall_arm.urdf" />
  <param name="use_gui" value="$(arg gui)" />
 
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz"  />
</launch>

配置完成，运行launch

$ roslaunch redwall_arm model.launch model:=arm.urdf \ gui:=True
$ roslaunch redwall_arm gazebo.launch

此时gazebo中的模型就不会耷拉下来了，使用rostopic list查看发现/arm_controller名称空间下包含了许多话题如下/follow_joint_trajectory包含的则是控制器的动作接口。/arm_controller/command让我们可以直接手动发送指令控制
    /arm_controller/command
    /arm_controller/follow_joint_trajectory/cancel
    /arm_controller/follow_joint_trajectory/feedback
    /arm_controller/follow_joint_trajectory/goal
    /arm_controller/follow_joint_trajectory/result
    /arm_controller/follow_joint_trajectory/status
    /arm_controller/state

    $ rostopic info /arm_controller/command 查看信息
    # 要求机械臂在1秒内完成指定的移动，运行下面的代码之后就可以看到gazebo和Rvbiz中的模型同时运动起来了
    $ rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory '{joint_names:['Link1', 'Link2', 'Link3', 'Link4', 'Link5'], points:[{positions:[0.1, -0.5, 0.5, 0.75, 0.5], time_from_start:[1.0, 0.0]}]}' -1

使用$rostopic echo /joint_states 则能看到各个关节的数据流在控制器的作用下，关节的位置会非常接近0，同事还会不停的在0附近波动，就像机械臂在不断的对抗重力，使自己的位置不变一样接下来需要添加的是robot_state_publisher,它会拿joint_states消息和机器人模型做前向运动学解算，产生tf消息并发布出去。