UR10+gazebo+moveit吸盘抓取搬运demo

使用ur10+gazebo开发了一个简易吸盘抓取箱子码垛的仿真过程，机械臂控制使用的是moveit配置。 本博客对部分关键的代码进行解释。
代码运行环境：支持ubuntu16、 18、 20， ros版本是ros1（经过测试）。

1、搬运场景

场景的搭建过程大致就是先用sw建模，然后导出stl模型到gazebo世界中，另存为sdf文件，再把所有模型文件拖入gazebo世界中制作world文件。导入ur机械臂就不再赘述。

2、world文件 carry_box.world

<sdf version='1.6'>
  <world name='default'>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose frame=''>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>
    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <contact>
              <collide_bitmask>65535</collide_bitmask>
              <ode/>
            </contact>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
              <torsional>
                <ode/>
              </torsional>
            </friction>
            <bounce/>
          </surface>
          <max_contacts>10</max_contacts>
        </collision>
        <visual name='visual'>
          <cast_shadows>0</cast_shadows>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/LightGrey</name>
            </script>
          </material>
        </visual>
        <self_collide>0</self_collide>
        <enable_wind>0</enable_wind>
        <kinematic>0</kinematic>
      </link>
    </model>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <physics name='default_physics' default='0' type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>0</shadows>
    </scene>
    <audio>
      <device>default</device>
    </audio>
    <wind/>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <state world_name='default'>
      <sim_time>486 140000000</sim_time>
      <real_time>4 327300810</real_time>
      <wall_time>1696174243 353467065</wall_time>
      <iterations>4291</iterations>

      <model name='desk_plane'>
        <pose frame=''>-0.25 -0.25 0.35 0 -0 0</pose>
        <scale>1 1 1</scale>
        <link name='link_0'>
          <pose frame=''>-0.25 -0.25 0.35 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>-2.9e-05 -0 4.55499 -3.14158 -0.458701 -3.14159</acceleration>
          <wrench>-2.9e-05 -0 4.55499 0 -0 0</wrench>
        </link>
      </model>
      <model name='ground_plane'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose frame=''>0 0 0 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
      <model name='pidai'>
        <pose frame=''>0.3 0.45 0.3 0 -7.3e-05 1.57809</pose>
        <scale>1 1 1</scale>
        <link name='link_1'>
          <pose frame=''>0.3 0.45 0.3 0 -7.3e-05 1.57809</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>5.3e-05 0.004276 -0 -2.24965 -0.428859 0</acceleration>
          <wrench>5.3e-05 0.004276 -0 0 -0 0</wrench>
        </link>
      </model>
      <model name='tuopan'>
        <pose frame=''>0.2 0.5 0.07 -3.14159 0 0</pose>
        <scale>1 1 1</scale>
        <link name='link_3'>
          <pose frame=''>0.2 0.5 0.07 -3.14159 0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>-0.030733 -2.32282 -0.311017 1.76562 -0.44259 0.000324</acceleration>
          <wrench>-0.030733 -2.32282 -0.311017 0 -0 0</wrench>
        </link>
      </model>
      <light name='sun'>
        <pose frame=''>0 0 10 0 -0 0</pose>
      </light>
    </state>
    <model name='pidai'>
      <link name='link_1'>
        <inertial>
          <mass>1</mass>
          <pose fr