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

五、在gazebo中添加摄像头

在完成simple_mover之后，回顾arm0.1版本的目标，是对不同颜色的方块进行分类并在Gazebo中模拟出来。

要实现不同颜色方块的分类，就需要用到摄像头了。所以接下来的任务是为模型添加一个摄像头，以供以后图像处理使用。若要实现摄像头功能，则需要添加相应的插件。可以参考：http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros。

本部分完成效果如下：

我们需要更改urdf文件夹下arm1.urdf.xacro、arm1.gazebo.xacro两个文件。这个作为Robot arm 0.1.2 版本已经上传github。

1.修改arm1.gazebo.xacro文件

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

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

    <mu1>0.2mu1>

    <mu2>0.2mu2>

    <material>Gazebo/Whitematerial>

  gazebo>


  
  <gazebo reference="camera_link">
    <material>Gazebo/Greymaterial>
  gazebo>

  
  <gazebo reference="camera_link">
    <sensor type="camera" name="rgb_camera">
      <update_rate>30.0update_rate>
      <camera name="rgb_camera">
        <horizontal_fov>1.4horizontal_fov>
        <image>
          <width>640width>
          <height>480height>
          <format>R8G8B8format>
        image>
        <clip>
          <near>0.02near>
          <far>300far>
        clip>
      camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>truealwaysOn>
        <updateRate>0.0updateRate>
        <cameraName>rgb_cameracameraName>
        <imageTopicName>image_rawimageTopicName>
        <cameraInfoTopicName>camera_infocameraInfoTopicName>
        <frameName>camera_linkframeName>
        <hackBaseline>0.0hackBaseline>
        <distortionK1>0.0distortionK1>
        <distortionK2>0.0distortionK2>
        <distortionK3>0.0distortionK3>
        <distortionT1>0.0distortionT1>
        <distortionT2>0.0distortionT2>
      plugin>
    sensor>
  gazebo>
robot>

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

  <gazebo reference="camera_link">

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

---

    <sensor type="camera" name="rgb_camera">

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

---

      <update_rate>30.0update_rate>

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

---

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

填写这些值以匹配物理相机硬件上制造商的规格。要注意的一件事是，假定像素为正方形。

此外，远近剪辑是特定于模拟的参数，它给出摄像机在模拟中可以看到物体的距离的上限和下限。这是在相机的视光框中指定的。

---

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

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

---

        <cameraName>rgb_cameracameraName>
        <imageTopicName>image_rawimageTopicName>
        <cameraInfoTopicName>camera_infocameraInfoTopicName>

在这里，我们为图像主题和照相机信息主题定义了照相机将发布到的rostopic。

对于arm1，我们用以下命令来订阅：

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

---

        <frameName>camera_linkframeName>

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

2.修改arm1.urdf.xacro文件

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

  
  <link name="camera_link">
    <collision>
      <origin xyz="1 0 0" rpy="0 0 0"/>
      <geometry>
        
        <sphere radius="0.005"/>
      geometry>
    collision>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        
        <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>

  
  <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>
        
        <sphere radius="0.005"/>
      geometry>

我们定义了一个半径为0.005m的球体当作摄像头,所以在标签中写作。除此之外，也可以使用模型作为摄像头，这里因为摄像头尺寸问题，所以选择使用球体作为摄像头的模型。

---

将更改的文件保存后，我们就可以运行gazebo来仿真了。

新建一个终端，输入

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

其中killall gzserver作用为关闭所有gazebo服务，可能是因为虚拟机的问题还是别的，gazebo总是出问题，所以最好每次运行新的终端的时候输入此命令来防止gazebo加载出错。

若无意外，gazebo应该打开如下图，可以看到camera已经成功加载出来了。

3.查看摄像头图像

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

$ rqt_image_view /rgb_camera/image_raw

打开如下视图

为了方便测试摄像头，可以选择gazebo左上角的insert标签，插入某些模型，其效果如下

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

为了实现夹取操作，需要为我们的模型添加夹爪。

我还是选择在solidworks 中来绘制模型。导出stl文件和更改urdf文件。

1.通过solidworks建立模型

首先在solidworks中建立模型，因为只是在gazebo中模拟，我就画的比较简陋了一点…

具体方法还是可以参照【从零开始的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文件夹

（2）更改urdf文件夹

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

部分添加的urdf文件如下所示：

...
  
  
  <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>

  

  <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>

  

  <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>


  
  <link name="camera_link">
  ...

之所以不建议后来添加模型是因为joint位置问题，如果后来添加模型，需要对的标签经行仔细修改。而如果直接使用solidworks就不用这么麻烦，而且还可以避免一定的模型抖动问题。

若添加无问题则如下图所示，

添加进几个模型，执行命令：

$ rqt_image_view /rgb_camera/image_raw

可以达到如下图所示的效果

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的标签下，都有类似于如下的内容

      <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" />

3x3转动惯量矩阵由惯量元素指定。因为它是对称的，所以它只能由6个元素来表示
$$\begin{array}{ccc}ixx& ixy& ixz\\ ixy& iyy& iyz\\ ixz& iyz& izz\end{array}$$
还是那句话，选择合适的转动惯量矩阵的参数会减少模型的震动。

最终效果如下所示：

本部分代码已上传github，接下来的博客便是关于添加相关节点的了。

4.制作物块sdf模型

我们需要几个物块作为夹取目标，所以一个比较好的方法就是直接使用gazebo建立sdf模型。我准备制作两种颜色$20m{m}^3$的立方体作为抓取目标。

在打开的gazebo中选择Edit–>Model Editor

插入一个立方体

更改相应link，Visual，Collision

选择ok进行确认。保存模型，将其插入到我们的机械臂世界中。效果如下图所示。