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

212
0
2020年12月24日 09时18分

【从零开始的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

 

本部分完成效果如下:

 

1

 

我们需要更改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的链接的名称匹配。

 


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

 

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

 


      <update_rate>30.0</update_rate>

 

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已经成功加载出来了。

 

2

 

3.查看摄像头图像

 

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

 

$ rqt_image_view /rgb_camera/image_raw

 

打开如下视图

 

3

 

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

 

4

 

六、为模型添加夹爪(Gripper)

 

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

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

 

1.通过solidworks建立模型

 

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

 

5

 

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

 

6

 

(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就不用这么麻烦,而且还可以避免一定的模型抖动问题。

 

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

 

7

 

添加进几个模型,执行命令:

 

$ rqt_image_view /rgb_camera/image_raw

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

 

8

 

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个元素来表示

 

9

 

还是那句话,选择合适的转动惯量矩阵的参数会减少模型的震动。

 

最终效果如下所示:

 

11

 

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

 

4.制作物块sdf模型

 

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

 

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

 

12

 

插入一个立方体

 

13

 

更改相应link,Visual,Collision

 

14

 

15

 

16

 

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

 

17

发表评论

后才能评论