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

###### 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目录下：

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

arm_controller.yaml

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

redwall_arm.urdf   

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

使用关节限定角度，可以给定continuous,然后启动joint control GUI观看一下极限位置和角度,记下数值（弧度）,然后修改continuous为revolute, 并设置lower和upper：, 极限effort力矩暂时不考虑

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

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

   
       
       
            Link1,Link2,Link3,Link4,Link5
       
   

  
    
      
      
      
    
    
      
      
        
      
      
        
      
    
    
      
      
        
      
    
  

  
    
      
      
      
    
    
      
      
        
      
      
        
      
    
    
      
      
        
      
    
  

  
    
    
    
    
  

  
    
      
      
      
    
    
      
      
        
      
      
        
      
    
    
      
      
        
      
    
  


  
    
    
    
    
    
  

  
    
      
      
      
    
    
      
      
        
      
      
        
      
    
    
      
      
        
      
    
  

  
    
    
    
    
    
  

  
      
      
      
    
    
      
      
        
      
      
        
      
    
    
      
      
        
      
    
  

  
    
    
    
    
  

  
    
      
      
      
    
    
      
      
        
      
      
        
      
    
    
      
      
        
      
    
  

  
    
    
    
    
    
  

  
    transmission_interface/SimpleTransmission
    
        PositionJointInterface
    
    
        1
        PositionJointInterface
    
  

  
    transmission_interface/SimpleTransmission
    
        PositionJointInterface
    
    
        1
        PositionJointInterface
    
  

  
    transmission_interface/SimpleTransmission
    
        PositionJointInterface
    
    
        1
        PositionJointInterface
    
  

  
    transmission_interface/SimpleTransmission
    
        PositionJointInterface
    
    
        1
        PositionJointInterface
    
  

  
    transmission_interface/SimpleTransmission
    
        PositionJointInterface
    
    
        1
        PositionJointInterface
    
  

    
        
        
            Link1,Link2,Link3,Link4,Link5
        
    

 

###### 安装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

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

配置完成，运行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消息并发布出去。