ROS-control-gazebo-moveit-grasp（一、场景搭建）

1、简介

毕设要求搭建一个机械臂抓取系统，虽然重点在生成抓取的视觉部分，但目前还是要把系统跑通。又因为疫情不能去实验室，只能现在gazebo上仿真完成。
整篇文章为Ubuntu18.04 ros-melodic gazebo9下实验。主要为整个仿真的大致流程，不涉及某一部分具体细节。

2、场景搭建

场景搭建包含几部分：
1、机械臂，抓取夹，这里主要用了ur仓库下已经写好的文件
2、ros_control，连接应用与仿真或真实机械臂之间的接口
3、场景（.world），待抓取物体，机械臂底座等

机械臂+抓取夹.urdf

克隆ur仓库中的代码，按照其Readme安装相应依赖项
https://github.com/ros-industrial/universal_robot/tree/melodic-devel
补 : urdf原称为机器人统一描述格式，其比较冗杂，在此基础上发展了.xacro文件，其中可以定义变量，进行变量运算等。

克隆后得到的如下pkgs：

注 ： 其中config文件是通过moveit setup assistant配置的，可在命令行输入
roslaunch moveit_setup_assistant setup_assistant.launch，同时添加对应的机械臂模型配置（包括碰撞矩阵、添加规划组、预设机械臂pose等）。
① Generate self-collision matrix——采样一系列的检测点，检测机械臂的各个关节在运动时候是否会发生碰撞（默认一万采样点）
② Add virtual joints——虚拟关节主要用于将robot关联到world，这里我们只需要定义一个虚拟关节把base_link关联到world，从而定义机械臂底座和world的关系。通常定义名称为virtual_joint，子关节为base_link，父关节为world，type为fixed。
③ Planning groups——Moveit通过定义规划组来语义上定义机械臂的各个部分（如手臂、末端执行器等），简单来说就是定义某些关节为一个组合并起一个名字。首先Add arm group，选择运动求解器kdl_kinematics_plugin/KDLKinematicsPlugin （一般用KDL就可以）如果需要更强大的求解器，参考IKFast Kinematics/IKFast，然后默认设置，点击Add joints添加Joints，保存；然后是gripper（到底是Add joints还是Add links？）
④ Robot poses——预设一些机械臂姿态，之后方便通过名称直接设置该姿态。
⑤ End Effectors——配置末端执行器。之前我们已经为末端执行器添加了规划组，这里我们将其标记为End Effector，parent link设置为arm组最末端的Link，这里为ee_link。
⑥ Passive joints——添加被动关节，如果机器人中有被动的关节（不是主动控制的关节），需要将其添加为被动关节，这样可以告诉MoveIt在规划运动的时候这些关节是无法主动控制的。机械臂中没有被动关节，跳过这一步。
⑦ 3D sensors——这里可以为机械臂添加传感器，如kinetic。如没有传感器，设置为None

参考链接：https://blog.csdn.net/weixin_42503785/article/details/111940651
https://blog.csdn.net/weixin_43455581/article/details/106405167 （包含了配置结果的解析）

最后将生成的配置文件输出到新建文件（***_config），文件夹中可在ur5.urdf中查看配置结果。在生成该config文件夹后，还需要在该文件夹下新建一个controller.yaml，添加如下代码：（为啥要这个文件具体原因不清楚，可能是提供插件接口）

controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
  - name: "gripper"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - gripper_finger1_joint

配置完这些后，我们在ur5_moveit_config/launch下新建一个ur5_moveit_planning_execution.launch文件（同样在ur仓库代码中已经建好），代码内容如下：

<launch>
  <arg name="sim" default="false" />
  <arg name="debug" default="false" />
    
  <!-- Remap follow_joint_trajectory -->
  <remap if="$(arg sim)" from="/follow_joint_trajectory" to="/arm_controller/follow_joint_trajectory"/>
  
  <!-- Launch moveit -->
  <include file="$(find ur5_moveit_config)/launch/move_group.launch">
    <arg name="debug" default="$(arg debug)" />
  </include>
</launch>

其中move_group.launch为配置生成的用于启动规划组的文件。

有了ur5_moveit_planning_execution.launch文件后，在最终的生成仿真环境的launch文件（ur5_single_arm_gazebo.launch）中，可通过如下代码将moveit引入进仿真环境中：

  <include file="$(find ur5_single_arm_moveit_config)/launch/ur5_moveit_planning_execution.launch">
    <arg name="debug" default="$(arg debug)" />
    <arg name="sim" default="$(arg sim)" />
  </include>

  <include file="$(find ur5_single_arm_moveit_config)/launch/moveit_rviz.launch">
    <arg name="debug" default="$(arg debug)" />
    <arg name="config" default="true" />
  </include>

我们最终直接使用的机械臂描述文件为/ur_description/urdf/ur5.urdf.xacro，我们将其include进我们最终的urdf.xacro文件中（如下图所示，除了包含ur机械臂之外，还有夹子，Kinect等…还有世界坐标与基坐标之间的关系，相机的位置）

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur5" >

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="false"/>

  <!-- robotiq 85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />

  <!-- gripper -->
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_85_gripper>

    <!-- kinect -->
  <xacro:include filename="$(find kinect_v2)/urdf/kinect_v2.urdf.xacro" />
    <xacro:kinect_v2  parent="world">
        <origin xyz="0.4 0 1.57"
                rpy="0 1.57 0" />
  </xacro:kinect_v2>

抓取夹也同理，下载robotiq_85_gripper代码，这里不知道在哪里的description怎样将其与机械臂组合在一起的。

到这里我们已经有了机械臂、抓取夹的urdf.xacro文件，以及对应的配置文件。

为了能够在仿真环境gazebo中引入机械臂模型，在最终的launch文件中，需要通过gazobo_ros pkg中的spawn_models节点引入机械臂，代码如下：通过将作为param引入的“robot_description”作为arg传入，同时还可以设置机械臂初始位姿。

  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
        args="-urdf -param robot_description -model robot -z 0.594
              -J shoulder_lift_joint -2.0
              -J elbow_joint 1.0"
        output="screen" />

ros_control

ROS中提供了丰富的机器人应用：SLAM、导航、MoveIt……但是你可能一直有一个疑问，这些功能包到底应该怎么样用到我们的机器人上，也就是说在应用和实际机器人或者机器人仿真器之间，缺少一个连接两者的东西。这个东西便是ros_control。
注： 这里我使用的是ur仓库下的机械臂，其已经定义好control接口的文件，在ur_gazebo/config以及ur_description/urdf的ur.transmission（其在ur5.urdf.xacro中有引入）文件中，我们在使用时只需要调用即可，但如果要自己设计机器人，则需要自己定义。
（Transmissions就是机器人的传动系统，机器人每个需要运动的关节都需要配置相应的Transmission，其通常在urdf文件中直接添加）关于ros_control，参考阅读：
https://www.guyuehome.com/890
http://gazebosim.org/tutorials/?tut=ros_control
http://wiki.ros.org/ros_control
http://gazebosim.org/tutorials?cat=connect_ros

.world文件

首先，在最终的加载仿真环境的launch文件中这样引入了world文件：

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <!--arg name="world_name" default="worlds/empty.world"/-->
    <arg name="world_name" default="$(find ur5_single_arm_tufts)/worlds/ur5_cubes.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  </include>

与机械臂的.urdf或.xacro文件不同的是，描述环境.world文件是.sdf文件的集合，每个.sdf文件为一个模型，相比.urdf文件，.sdf可以设置摩擦系数等。可以直接在.world文件写model，也可以将model写成.sdf之后include进来。

最终的launch文件与机械臂（夹）urdf.xarco文件代码

1、ur5_single_arm_gazebo.launch

<?xml version="1.0"?>
<launch>
  <arg name="paused" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="sim" default="true" />
  <arg name="debug" default="false"/>

  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <!--arg name="world_name" default="worlds/empty.world"/-->
    <arg name="world_name" default="$(find ur5_single_arm_tufts)/worlds/ur5_cubes.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  </include>

  <!-- send robot urdf to param server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur5_single_arm_tufts)/urdf/ur5_single_arm.urdf.xacro'"/>
  
  <!-- push robot_description to factory and spawn robot in gazebo -->
  <!--node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0" respawn="false" output="screen"/-->

  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
        args="-urdf -param robot_description -model robot -z 0.594
              -J shoulder_lift_joint -2.0
              -J elbow_joint 1.0"
        output="screen" />

  <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
  
  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
  <rosparam file="$(find robotiq_85_gazebo)/controller/gripper_controller_robotiq.yaml" command="load"/>

  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>

  <include file="$(find ur5_single_arm_moveit_config)/launch/ur5_moveit_planning_execution.launch">
    <arg name="debug" default="$(arg debug)" />
    <arg name="sim" default="$(arg sim)" />
  </include>

  <include file="$(find ur5_single_arm_moveit_config)/launch/moveit_rviz.launch">
    <arg name="debug" default="$(arg debug)" />
    <arg name="config" default="true" />
  </include>

</launch>

2、ur5_single_arm.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur5" >

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="false"/>

  <!-- robotiq 85 -->
  <xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />

  <!-- gripper -->
  <xacro:robotiq_85_gripper prefix="" parent="ee_link" >
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_85_gripper>

    <!-- kinect -->
  <xacro:include filename="$(find kinect_v2)/urdf/kinect_v2.urdf.xacro" />
    <xacro:kinect_v2  parent="world">
        <origin xyz="0.4 0 1.57"
                rpy="0 1.57 0" />
  </xacro:kinect_v2>

  <link name="world"/> 

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>

  <xacro:include filename="$(find ur5_single_arm_tufts)/urdf/gzplugin_grasp_fix.urdf.xacro"/>
  <xacro:gzplugin_grasp_fix/>

</robot>

3、运行结果

Set <release_tolerance>0.0050</release_tolerance> in gzplugin_grasp_fix

Set <release_tolerance>0.0010</release_tolerance> in gzplugin_grasp_fix

4、最后

这是我的第一篇博客，代码参考https://github.com/harrycomeon/ur5-gazebo-grasping，原博主链接：https://github.com/harrycomeon/ur5-gazebo-grasping。逻辑可能不太清晰，以后还要多多加强~