ROS Kinetic & Gazebo7下的ABB-Yumi双臂协作机器人的食品分类抓取仿真（一）联合仿真与环境搭建

项目简介

本项目原计划研究基于双臂机器人的物体插孔装配操作，这个问题是双臂机器人协作规划与双臂协调运动控制的关键问题。研究基于项目现有的物理仿真环境，在双臂机器人抓取到物体之后，规划双臂的运动轨迹，实现两个可相互装配物体的插孔操作。

但是经过仿真测试发现Gazebo对物理效果的仿真有缺陷，两个工件是否能成功装配很大程度上收工件尺寸，质量，惯量，材质，碰撞等参数以及逆运动学求解精度的影响，因此任务调整为结合Yolov3目标检测算法完成双臂的协作控制以及水果以及各种饮料的分类抓取

环境

Linux: ubuntu 16.04+Ros Kinetic+Gazebo7+Rviz+MoveitSetupAssistants+Blender2.9.3+Yolov3

Gazebo和Moveit的联合仿真

ABB-Yumi是一款比较新型的双臂协助机器人，开源资源相当有限且很不完善，因此从零搭建仿真环境十分困难，同时联合仿真的相关资料基本上都是基于自己搭建的模型以及UR系列的单臂机器人，可以借鉴但是实现起来并不简单。

此项目的前期工作就是通过一个简单的urdf模型一步步实现Moveit到Gazebo的联合仿真，中间涉及到各种控制器的配置，篇幅有限，所以暂时不在这里介绍，中间遇到了各种各样的bug,到最后虽然实现了联合仿真但是发现机器人总是和预定的位姿有一个小的偏差，推测可能和模型文件本身或者Moveit里面对position的tolerence有关系，经过一番寻找，在Github上面找到了一个有用的功能配置包，包括它的urdf model以及moveit config package，可以通过百度网盘下载，提取码为1234,使用方法具体可以看包里面的md说明文档。

- After pulling and building the project, use the following command to start YuMi control and simulation: 

$ roslaunch yumi_launch yumi_gazebo_traj_pos_control.launch

- To start planning excution with moveit, use:

$ roslaunch yumi_moveit_config demo_planning_excution.launch

- An example of controlling the robot to reach a desired position is included in python script in yumi_test_controllers/scripts/:

$ rosrun yumi_test_controllers test_moveit.py

1.

第一步启动Gazebo加载worlds里面的my_world.world模型文件，打开可以看见里面对kinect摄像头的模型渲染采用了dae网格文件，但是网格文件的路径是有问题的，可以在下载Gazebo的官方模型库后把….内容用

1
2
3

<mesh>
    <uri>model://kinect/meshes/kinect.dae</uri>
</mesh>

代替即可加载出kinect相机的渲染

2.

第二步启动了Moveit的相关节点和服务，其中这里如果需要Rviz可以自己手动加载

3.

第三步是抓取的例程，手动设定目标位姿可以前往目标点以及实现夹爪的开闭，但是里面的抓取函数没有用，表现为即使夹爪闭合也无法夹住物体，推测与夹爪材料的阻尼系数以及木块的阻尼系数有关系，因此改用夹爪闭合后直接连接两个link的方式来夹取物体。

于是这里又要用到Github上面的一个项目，叫做gazebo_ros_link_attacher-master，百度网盘链接，提取码：1234，系统是Kinetic。Melodic版本百度网盘链接，提取码同上。

使用方法官方的说明文档很详细，要注意的就是需要编译然后把插件<plugin name="ros_link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>

加到自己的那个world里面而前面那个ABB-Yumi的配置包不需要编译，并且在后面ABB-Yumi用到这里面的服务时需要

1 2	cd gazebo_link_attacher_ws source devel/setup.bash

attach.py

#!/usr/bin/env python

import rospy
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse


if __name__ == '__main__':
    rospy.init_node('demo_attach_links')
    rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach")
    attach_srv = rospy.ServiceProxy('/link_attacher_node/attach',
                                    Attach)
    attach_srv.wait_for_service()
    rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach")

    # Link them
    rospy.loginfo("Attaching cube1 and cube2")
    req = AttachRequest()
    req.model_name_1 = "cube1"
    req.link_name_1 = "link"
    req.model_name_2 = "cube2"
    req.link_name_2 = "link"

    attach_srv.call(req)
    # From the shell:
    """
rosservice call /link_attacher_node/attach "model_name_1: 'cube1'
link_name_1: 'link'
model_name_2: 'cube2'
link_name_2: 'link'"
    """

    rospy.loginfo("Attaching cube2 and cube3")
    req = AttachRequest()
    req.model_name_1 = "cube2"
    req.link_name_1 = "link"
    req.model_name_2 = "cube3"
    req.link_name_2 = "link"

    attach_srv.call(req)

    rospy.loginfo("Attaching cube3 and cube1")
    req = AttachRequest()
    req.model_name_1 = "cube3"
    req.link_name_1 = "link"
    req.model_name_2 = "cube1"
    req.link_name_2 = "link"

    attach_srv.call(req)

请求了服务把两个杆件连在一块

经过修改test_moveit.py调整目标点位姿，完成了对抓取的测试

查看Rviz,可以看到已经显示深度相机采集的图像

发现点云数据飘在天上，经过分析应该是kinect模型参数里面的cameraFrame不对

应该在yumi.urdf.xacro中添加

<link name="camera_link">
      <visual>
          <origin xyz=" 0 0 0 " rpy="0 0 0" />
          <geometry>
              <sphere radius="0.01" />
          </geometry>
          <material name="black">
              <color rgba="0 0 0 0.95"/>
          </material>
      </visual>
    </link>
<joint name="camera_joint" type="fixed">
        <origin xyz="0.7 0 1.0" rpy="0 ${PI} ${PI / 2}"/>
        <parent link="world"/>
        <child link="camera_link"/>
 </joint>

同时修改相机模型文件中的参考坐标系为camera link

到这里Kinect就已经成功加入到Gazebo里面了

搭建机器人的工作环境

由于Gazebo模型库的模型并不很符合此次任务的需求并且渲染效果不好不适合后面的目标检测，因此这里待抓取的香蕉、雪碧、芬达，可口可乐以及收纳盒的模型均来自网上下载之后再通过Blender2.93.0编辑

如果有需要，这里有百度网盘的下载链接：

香蕉：https://pan.baidu.com/s/1VjebDv5ycb6tPdLPDVbMnw

饮料：https://pan.baidu.com/s/1UUygB3csYz5b9SJJv6S9UQ

收纳盒：https://pan.baidu.com/s/1N93MCk2b2KTLfYSIjovaQw

后面两个均为blend工程文件修改尺寸可以直接导出贴图以及dae网格文件，前面的香蕉暂时还无法给它增加贴图。

导出的文件：

使用时只加载dae就可以

使用方法：

加入香蕉

在my_world.world文件中加入以下内容：

<model name="banana">
       <pose>0.5 0 0 0 0 0</pose>
       <link name="banana_link">
         <inertial>
            <mass>0.001</mass>
            <inertia>
              <ixx>0.166667</ixx>
              <ixy>0</ixy>
              <ixz>0</ixz>
              <iyy>0.166667</iyy>
              <iyz>0</iyz>
              <izz>0.166667</izz>
            </inertia>
          </inertial>
          <collision name='collision2'>
            <geometry>
              <mesh><uri>/home/li/catkin_ws/src/yumi_launch/worlds/Banana/file.dae</uri></mesh>
            </geometry>
            <max_contacts>10</max_contacts>
            <surface>
              <contact>
                <ode/>
              </contact>
              <bounce/>
              <friction>
                <torsional>
                  <ode/>
                </torsional>
                <ode/>
              </friction>
            </surface>
          </collision>
        
          <visual name="visual2">
            <geometry>
              <mesh><uri>/home/li/catkin_ws/src/yumi_launch/worlds/Banana/file.dae</uri></mesh>
            </geometry>
          </visual>
          <self_collide>0</self_collide>
          <kinematic>0</kinematic>
        </link>
    </model>

poseframe调整位置；

如果物体轻飘飘的可以减小mass；

collision是碰撞区域，visual是可视化区域，两者都可以用dae渲染；

这里因为相机太大影响抓取索性把kinect模型的collision调整为一个边长0.001的小正方体；

惯性,质量等参数也可以考虑用meshlab来配置。

经过不断调试：

发现物品视野很暗影响检测，因此在Gazebo中加了一个点光源，保存为新的my_lighted_world.world,并在将 yumi_gazebo_traj_pos_control.launch中加载的world文件改为my_lighted_world.world

最终工作环境如下：

再次运行roslaunch yumi_launch yumi_gazebo_traj_pos_control.launch时发现gazebo_link_attacher的服务没有启动，原因在于my_lighted_world.world和原来的my_world.world sdf版本不相同，前者为sdf1.4,后者为sdf1.6,需要修改的地方就是把 <plugin name='ros_link_attacher_plugin' filename='libgazebo_ros_link_attacher.so'/>改为 <plugin name="ros_link_attacher_plugin" filename="libgazebo_ros_link_attacher.so"/>即可加载出link-attacher的相关服务。