【从零开始的ROS四轴机械臂控制】（三） – 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型_人工智能

【从零开始的ROS四轴机械臂控制（三）】

- 五、在gazebo中添加摄像头
- - 1.修改arm1.gazebo.xacro文件
  - 2.修改arm1.urdf.xacro文件
  - 3.查看摄像头图像
- 六、为模型添加夹爪（Gripper）
- - 1.通过solidworks建立模型
  - 2.将夹爪添加进gazebo
  - - （1）模型导入
    - （2）更改urdf文件夹
  - 3.gazebo模型抖动解决办法
  - - - ①pid参数设置
      - ②旋转惯量参数矩阵
  - 4.制作物块sdf模型

五、在gazebo中添加摄像头

在完成simple_mover之后，回顾arm0.1版本的目标，是对不同颜色的方块进行分类并在Gazebo中模拟出来。要实现不同颜色方块的分类，就需要用到摄像头了。所以接下来的任务是为模型添加一个摄像头，以供以后图像处理使用。若要实现摄像头功能，则需要添加相应的插件。可以参考：http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros。本部分完成效果如下：
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型我们需要更改urdf文件夹下arm1.urdf.xacro、arm1.gazebo.xacro两个文件。这个作为Robot arm 0.1.2 版本已经上传github。

1.修改arm1.gazebo.xacro文件

首先我们需要向arm1.gazebo.xacro文件中添加camera插件：

<!--link_4-->
<?xml version="1.0"?>

<robot>
  
  ...(部分省略)
  <gazebo reference="link4">

    <mu1>0.2</mu1>

    <mu2>0.2</mu2>

    <material>Gazebo/White</material>

  </gazebo>


  <!--camera_link-->
  <gazebo reference="camera_link">
    <material>Gazebo/Grey</material>
  </gazebo>

  <!-- camera-->
  <gazebo reference="camera_link">
    <sensor type="camera" name="rgb_camera">
      <update_rate>30.0</update_rate>
      <camera name="rgb_camera">
        <horizontal_fov>1.4</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>rgb_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_link</frameName>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
</robot>

让我们讨论一下此插件的一些属性…

  <gazebo reference="camera_link">

链接名称“ camera_link”必须与我们添加到Xacro URDF的链接的名称匹配。

<code class="prism language-xml has-numbering">    <span class="token tag"><span class="token punctuation"><</span>sensor <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>camera<span class="token punctuation">"</span></span> <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb_camera<span class="token punctuation">"</span></span><span class="token punctuation">></span></span>
</code>

传感器名称“ rgb_camera”必须保证相对于其他传感器名称唯一。除了可以访问的Gazebo插件内，此名称在很多地方都没有使用

<code class="prism language-xml has-numbering">      <span class="token tag"><span class="token punctuation"><</span>update_rate<span class="token punctuation">></span></span>30.0<span class="token tag"><span class="token punctuation"></</span>update_rate<span class="token punctuation">></span></span>
</code>

gazebo内每秒拍摄一次新相机图像的次数。这是传感器在仿真过程中将尝试的最大更新速率，但是如果物理仿真的运行速度快于传感器生成的速度，则它可能会落后于该目标速率。

        <horizontal_fov>1.4</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>

填写这些值以匹配物理相机硬件上制造商的规格。要注意的一件事是，假定像素为正方形。此外，远近剪辑是特定于模拟的参数，它给出摄像机在模拟中可以看到物体的距离的上限和下限。这是在相机的视光框中指定的。

      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">

这是实际文件作为共享对象链接到的地方。gazebo_ros/gazebo_ros_camera.cpp

        <cameraName>rgb_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>

在这里，我们为图像主题和照相机信息主题定义了照相机将发布到的rostopic。对于arm1，我们用以下命令来订阅：

/arm1/rgb_camera/image_raw
/arm1/rgb_camera/camera_info

        <frameName>camera_link</frameName>

图像的坐标系被发布在 tf树下。

2.修改arm1.urdf.xacro文件

若将摄像机添加在urdf文件中，则需在arm1.urdf.xacro文件中添加以下内容：

  <!--camera_link-->
  <link name="camera_link">
    <collision>
      <origin xyz="1 0 0" rpy="0 0 0"/>
      <geometry>
        <!--mesh filename="package://arm1/meshes/camera.dae"/-->
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <!--mesh filename="package://arm1/meshes/camera.dae"/-->
        <sphere radius="0.005"/>
      </geometry>
      <material name="red">
        <color rgba="1.0 0 0 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0.05 0" rpy="0 1.57 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <!--camera_joint-->
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 0 0" />
    <origin xyz="-0.042 -0.035 -0.01" rpy="-1.57 -3.1415 0"/>
    <parent link="link4"/>
    <child link="camera_link"/>
  </joint>

其中，

      <geometry>
        <!--mesh filename="package://arm1/meshes/camera.dae"/-->
        <sphere radius="0.005"/>
      </geometry>

我们定义了一个半径为0.005m的球体当作摄像头,所以在标签中写作<sphere radius="0.005"/>。除此之外，也可以使用模型<mesh filename="package://arm1/meshes/camera.dae"/>作为摄像头，这里因为摄像头尺寸问题，所以选择使用球体作为摄像头的模型。

将更改的文件保存后，我们就可以运行gazebo来仿真了。新建一个终端，输入

$ killall gzserver
$ source ~/catkin_ws/devel/setup.bash 
$ roslaunch arm1 gazebo.launch

其中killall gzserver作用为关闭所有gazebo服务，可能是因为虚拟机的问题还是别的，gazebo总是出问题，所以最好每次运行新的终端的时候输入此命令来防止gazebo加载出错。若无意外，gazebo应该打开如下图，可以看到camera已经成功加载出来了。
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型

3.查看摄像头图像

要查看摄像机图像流，可以使用命令rqt_image_view（在此处了解有关rqt和相关工具的更多信息）

<code class="prism language-cmd has-numbering">$ rqt_image_view /rgb_camera/image_raw
</code>

打开如下视图
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型为了方便测试摄像头，可以选择gazebo左上角的insert标签，插入某些模型，其效果如下

六、为模型添加夹爪（Gripper）

为了实现夹取操作，需要为我们的模型添加夹爪。我还是选择在solidworks 中来绘制模型。导出stl文件和更改urdf文件。

1.通过solidworks建立模型

首先在solidworks中建立模型，因为只是在gazebo中模拟，我就画的比较简陋了一点…
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型具体方法还是可以参照【从零开始的ROS四轴机械臂控制】(三)，还是可以选择导出urdf文件和具体模型。夹爪由三个link组成，分别是gripper_base_link(后面称之为gripper_link)，right_finger_link(后面称之为right_link)和left_finger_link(后面称之为left_link)，还有两个joint，称之为right_joint 和 left_joint。在之后导出urdf文件后，会导出三个link的stl文件。因为我们在~/catkin_ws/src目录下已经有arm1的包了，所以现在需要做的是把夹爪添加到我们的模型中。个人感觉，其实不如一开始就把所有模型都建好，因为后期导入新的模型会出现很多的bug。

2.将夹爪添加进gazebo

（1）模型导入

最简单的一步，就是把新导出的三个stl文件拷贝进meshes文件夹
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型

（2）更改urdf文件夹

拷贝完模型，接下来就是更改urdf文件，和添加摄像头类似，添加了各个joints，links还有controllers，并对相应文件进行了添加修改，更多的这里不再赘述。完全可以参考上面如何添加摄像头。部分添加的urdf文件如下所示：

...
  <!--gripper links-->
  <!--gripper base-->
  <link

    name="gripper_link">

    <inertial>

      <origin

        xyz="-0.0066122 0.0073345 0.035"

        rpy="0 0 0" />

      <mass

        value="0.0035" />

      <inertia

        ixx="1.2396E-07"

        ixy="-1.3634E-23"

        ixz="1.9573E-24"

        iyy="3.6458E-07"

        iyz="5.8326E-24"

        izz="4.7396E-07" />

    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm1/meshes/gripper_Link.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="1 1 1 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm1/meshes/gripper_Link.STL" />

      </geometry>

    </collision>

  </link>

  <joint name="joint_gripper"

    type="fixed">

    <origin

      xyz="-0.045 -0.035 -0.01"

      rpy="0 1.57 -2.45" />

    <parent

      link="link4" />

    <child

      link="gripper_link" />

  </joint>

  <!--gripper_right link and joint-->

  <link

    name="right_link">

    <inertial>

      <origin
        xyz="-0.0085551 0.004 0.024348"
        rpy="0 0 0" />
      <mass
        value="0.0013847" />
      <inertia

        ixx="1.28810723367291E-06"

        ixy="-4.80571958593516E-08"

        ixz="-1.80536823271041E-09"

        iyy="1.3067705064679E-06"

        iyz="-1.51983137928678E-09"

        izz="5.58044990324735E-07" />
    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm1/meshes/right_Link.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="1 1 1 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm1/meshes/right_Link.STL" />

      </geometry>

    </collision>

  </link>


  <joint
    name="right_joint"
    type="revolute">
    <origin
      xyz="0.0125 -0.004 0"
      rpy="-1.5708 0 0" />
    <parent
      link="gripper_link" />
    <child
      link="right_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-0.4"
      upper="0.4"
      effort="1.0"
      velocity="0.5" />
  </joint>

  <!--gripper_left link and joint-->

  <link

    name="left_link">

    <inertial>

      <origin
        xyz="-0.0085551 -0.004 0.024348"
        rpy="0 0 0" />
      <mass
        value="0.0013847" />
      <inertia

        ixx="1.28810723367291E-06"

        ixy="-4.80571958593516E-08"

        ixz="-1.80536823271041E-09"

        iyy="1.3067705064679E-06"

        iyz="-1.51983137928678E-09"

        izz="5.58044990324735E-07" />
    </inertial>

    <visual>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm1/meshes/left_Link.STL" />

      </geometry>

      <material

        name="">

        <color

          rgba="1 1 1 1" />

      </material>

    </visual>

    <collision>

      <origin

        xyz="0 0 0"

        rpy="0 0 0" />

      <geometry>

        <mesh

          filename="package://arm1/meshes/left_Link.STL" />

      </geometry>

    </collision>

  </link>


  <joint
    name="left_joint"
    type="revolute">
    <origin
      xyz="-0.0125 -0.004 0"
      rpy="-1.5708 0 0" />
    <parent
      link="gripper_link" />
    <child
      link="left_link" />
    <axis
      xyz="0 0 -1" />
    <limit
      lower="-0.4"
      upper="0.4"
      effort="1.0"
      velocity="0.5" />
  </joint>


  <!--camera_link-->
  <link name="camera_link">
  ...

之所以不建议后来添加模型是因为joint位置问题，如果后来添加模型，需要对<joint>的<origin>标签经行仔细修改。而如果直接使用solidworks就不用这么麻烦，而且还可以避免一定的模型抖动问题。若添加无问题则如下图所示，
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型添加进几个模型，执行命令：

$ rqt_image_view /rgb_camera/image_raw

<code class="has-numbering"></code>

可以达到如下图所示的效果
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型

3.gazebo模型抖动解决办法

我总共遇到了两种情况的模型抖动，一是为pid参数设置，二是为旋转惯量参数矩阵设置。这两种情况都会造成模型的抖动，其它的情况没有遇到过，等以后有机会遇到会再总结一次的。

①pid参数设置

在controllers.yaml文件中，记录着各个控制器的pid参数

arm1:
    #list of controllers
    joint_state_controller:
      type: joint_state_controller/JointStateController
      publish_rate: 50

    joint1_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint1
      pid: {p: 1, i: 0.001, d: 0.002}
    joint2_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint2
      pid: {p: 1, i: 0.001, d: 0.002}
    ...

pid参数的值过大会造成震荡等问题，过小就会控制效果差。所以要选择适合自己模型的pid参数，才能达到最好的效果。那什么是最好的参数选择呢?..这就是门很深的学问了，可以自行百度，或者自己多次尝试更改…

②旋转惯量参数矩阵

在arm1.urdf.xacro文件中，在link的<inertial>标签下，都有类似于如下的内容

      <inertia
        ixx="7.57454654701591E-08"
        ixy="2.28779573583169E-08"
        ixz="8.70361108935048E-14"
        iyy="1.66799537620888E-08"
        iyz="3.19929708005816E-14"
        izz="8.54396424256289E-08" />

3×3转动惯量矩阵由惯量元素指定。因为它是对称的，所以它只能由6个元素来表示
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型还是那句话，选择合适的转动惯量矩阵的参数会减少模型的震动。最终效果如下所示：
本部分代码已上传github，接下来的博客便是关于添加相关节点的了。

4.制作物块sdf模型

我们需要几个物块作为夹取目标，所以一个比较好的方法就是直接使用gazebo建立sdf模型。我准备制作两种颜色 $20mm^3$ 的立方体作为抓取目标。在打开的gazebo中选择Edit–>Model Editor
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型插入一个立方体
更改相应link，Visual，Collision

选择ok进行确认。保存模型，将其插入到我们的机械臂世界中。效果如下图所示。
【从零开始的ROS四轴机械臂控制】（三） - 为机械臂添加摄像头和夹爪、解决gazebo模型抖动、使用gazebo建立sdf模型