UR5+robotiq_85_gripper GAZEBO模拟视觉抓取平台仿真-1

由于疫情原因不能进入实验室，遂学习在仿真环境gazebo下利用UR5机械臂搭建模拟平台，此模拟平台可以用于UR5机械臂通用视觉抓取平台。以下是个人总结一些观点，仅供参考。

运行环境：ubuntu16.04 + ros-kinetic

1. 环境搭建

1. 世界环境
   参考网址：http://gazebosim.org/tutorials?cat=build_world
   作为视觉抓取环境，我选了一个桌子作为抓取平台和三个带颜色的方形物块作为抓取对象，同时我给UR5机械臂加了一个基座。其实我们可以根据根据自己需求搭建自己的环境，但是这些模型又是怎么产生的呢，可以参照上面参考网址进行了解。当然本身官网已经有了大量模型，我们直接拿来用。
   gazebo模型数据集网址：https://github.com/osrf/gazebo_models
   世界环境的搭建，主要是在ur_icam_gazebo的worlds文件下进行，它本身语言的含义可以通过简单字面意思去知道模型位置，大小，摩擦力特性等，这里说一下这些模型的默认搜索位置应该是在/home/×××/.gazebo/models下，默认是一个隐藏文件，需要在home目录下，同时按ctrl+h来显示隐藏文件.gazebo。
   由于程序太长，这里我不会贴出来，后面会有相关的链接。

2. 模型环境
   首先我们需要准备好的UR5本身的官方包。相机这里我选用了kinect_V2作为深度相机用于视觉采集，然后手抓方面我选择了robotiq_85_gripper二指手抓，这里的深度相机和手抓是可以去更换的，当然需要重新编写URDF或xacro文件来进行配置。
   这里我可能会疑问我怎么把UR5机械臂设置成如下默认位置，以及相机位置的选择，怎么把手抓和相机，机械臂三者结合到一块。
   1,首先设置UR5机械臂的位置，我是在gazebo启动的ur5.launch文件下，首先设置了暂停模式

  <arg name="paused" default="true" doc="Starts gazebo in paused mode" />

然后利用gazebo_ros的spawn_gazebo_model节点获取了关节位置，这样可以修改UR5的起始位置和姿态

  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
        args="-urdf -param robot_description -model robot -z 0.594
              -J shoulder_lift_joint -2.0
              -J elbow_joint 1.0"
        output="screen" />

最后启动暂停的gazebo文件，便可以获得如下的初始位姿。

2,我是先在ur5_single_arm.urdf.xacro文件下，确定了世界坐标与UR5基座标的关系后，然后在相机的urdf文件中，又确立了相机坐标与世界坐标的关系，引入到模型中。
ur5_single_arm.urdf.xacro

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>

ur5_single_arm.urdf.xacro中

  <xacro:include filename="$(find kinect_v2)/urdf/kinect_v2.urdf.xacro" />
    <xacro:kinect_v2  parent="world">
        <origin xyz="0.4 0 1.57"
                rpy="0 1.57 0" />
    </xacro:kinect_v2>

3,第三个问题主要是在robotiq_85_gripper.urdf.xacro文件下
我将三个urdf文件进行了声明，然后我找的包本身已经把手抓和机械臂在这个文件中结合到一块，相机的话我第二个问题有提到

  <!-- robotiq 85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />

  <!-- gripper -->
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_85_gripper> 

根据上面所说的我搭建了自己的模拟环境如下：

2,gazebo ros控制
ros-control包含两个控制，一个对UR5机械臂的控制，另一个是对二指手抓的控制。
分别是在加载gazebo的同时启动相应的控制。
在ur5.launch下，利用下面的语句查找了控制器

  <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
  
  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>

其中对ur5的控制器是查找了ur5官方提供包的arm_controller_ur5.yaml
利用了position_controllers/JointTrajectoryController，关节轨迹控制器

对手抓的控制是查找了gripper_controller_robotiq.yaml

  <rosparam file="$(find robotiq_85_gazebo)/controller/gripper_controller_robotiq.yaml" command="load"/>

  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>

控制器内容：

gripper:
  type: position_controllers/JointTrajectoryController
  joints:
     - gripper_finger1_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      gripper_finger1_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10

利用了 position_controllers/JointTrajectoryController，手抓action控制

上述两者的消息类型，可以在这里查找到
http://docs.ros.org/api/control_msgs/html/index-msg.html

只有这里对ros-control进行声明，才可以对手臂和手抓进行驱动，类似于驱动的协议。

3,moveit

MoveIt Setup Assistant

moveit配置：
大致配置思路如下：

Step 1: Start

roslaunch moveit_setup_assistant setup_assistant.launch

Click on the Create New MoveIt Configuration Package button to bring up the following screen:
选择/ur_icam_description/urdf/ur5_robot.urdf.xacro，进行加载

Step 2: Generate Self-Collision Matrix
点击产生碰撞矩阵

Step 3: Add Virtual Joints

Step 4: Add Planning Groups