UR5+robotiq_85_gripper GAZEBO模拟|20.04 Noetic|

        在大一的时候尝试跟着古月老师的ROS21讲学习过Moveit，但是一直不理解所谓的使MoveIt控制器的命名空间与机械臂控制器插件的命名空间相同是什么意思。并且由于古月老师使用的Ubuntu版本是18.04，由于本人计算机的原因，使用18.04会产生卡顿，于是一直以来我坚持使用的是20.04。所以，势必最终会与古月老师的代码产生一定的出入，不能直接编译执行。

        现在大二暑假了，花了一个星期的事件整理了这部分的资料，并且修改了古月老师的marm机械臂适用于noetic，并依葫芦画瓢给ur机器人增加了robotiq_85_gripper，希望给跟我一样的ROS小白有一定的启发意义。

        本次实验最后的目的是实现一个自动化的物流系统，机械臂视觉抓取物料，放置在物料小车上，物料小车接收到物料后，再按原路返回。实验的内容会不断更新，喜欢的可以关注我的进度。

        本文默认已经完成了ros的安装和moveit的安装

一、解构marm文件了解moveit

启动文件：

roslaunch marm_gazebo arm_bringup.launch

启动文件中共四步：

<launch>
    <!-- Launch Gazebo  -->
    <include file="$(find marm_gazebo)/launch/marm_world.launch" />

    <!-- ros_control arm launch file -->
    <include file="$(find marm_gazebo)/launch/arm_gazebo_states.launch" />   

    <!-- ros_control trajectory control dof arm launch file -->
    <include file="$(find marm_gazebo)/launch/arm_trajectory_controller.launch" />

    <include file="$(find new_marm_moveit_config)/launch/moveit_planning_execution.launch" />
</launch>

        如图文件中描述的，第一步加载了机械臂世界marm_world.launch。其中加载empty_world,加载机械臂参数，并通过节点讲机械臂记载在Gazebo环境中，并设置沿z轴上升0.04，避免机械臂自发抖动。

<launch>
  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" command="$(find xacro)/xacro '$(find marm_description)/urdf/arm.xacro'" /> 

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
	args="-urdf -model dakebot -param robot_description -R 0.0 -P 0.0 -Y 1.57 -x 0.0 -y -0.0 -z 0.04" /> 
</launch>

        在文件中描述的arm.xacro除了需要正常的link和joint关系，还需要带有gazebo参数，分别包括link和joint的gazebo参数。另外需要加载ros_control plugin。ros_controller的命名空间实际也会影响到gazebo控制器与moveit控制器的连接，在本次实验中，我没有设置ros_controller的命名空间。下面的代码块仅供参考。

<!--例如link：-->
    <gazebo reference="base_link">
        <material>Gazebo/White</material>
    </gazebo>
<!--joint：-->
    <xacro:macro name="transmission_block" params="joint_name">
        <transmission name="tran1">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            </joint>
            <actuator name="motor1">
                <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>
    
    <xacro:transmission_block joint_name="joint1"/>
<!--ros_control plugin： -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/</robotNamespace>
        </plugin>
    </gazebo>

        第二步，加载arm_gazebo_states.launch,将关节控制器的配置参数加载到参数服务器中，并通过节点发布出来。另外通过robot_state_publisher发布TF。具体的控制器配置贴在marm配置的最下面，以便对比理解。下面同样出现controller的配置，不再提及为什么没有贴。

<launch>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    <rosparam file="$(find marm_gazebo)/config/arm_gazebo_joint_states.yaml" command="load"/>
    <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
        respawn="false" output="screen">
    </node>
</launch>

        第三步，加载arm_trajectory_controller.launch。实际就是加载了gazebo_controller，其中包括arm和gripper的controller，这部分的控制器与上面第二步的关节控制器同属于gazebo_controller，与moveit控制器相连接。同样的，通过节点将控制器发布出去。

<launch>
    <rosparam file="$(find marm_gazebo)/config/trajectory_control.yaml" command="load"/>

    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" args="arm_joint_controller gripper_controller"/>
        <!-- arm_joint_controller -->
</launch>

        第四步，加载moveit_planning_execution.launch。这一部分的文件是属于moveit配置完机械臂后自己创建的，内容包括启动move_group和启动rviz。在完成moveit配置时，我一般习惯将机器人的参数加载在move_group里（false——>true）。另外moveit控制器这一部分也需要在moveit_group.launch里进行选择配置，默认的是ros_controller。

<launch>
 <include file="$(find new_marm_moveit_config)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find new_marm_moveit_config)/config/marm.rviz" />
</launch>

以下是gazebo控制器和moveit控制器的配置文件，对比以便理解所谓的命名空间。我们可以看到机械臂的控制器命名空间都是arm_joint_controller,夹爪的空间都是gripper_controller，当命名空间一致后，moveit会自动与gazebo控制器进行匹配连接。

其中还会产生错误，大概是No gain p……

这个不会对你的控制产生任何影响，如果你需要消除这错误，只需要参考csdn以下，并且注意缩进

可以让红字消失，但会让机械臂抽搐和瘫软，所以最终我忽略了这个问题。

<!--arm_gazebo_joint_states.yaml-->
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  

<!--trajectory_control.yaml-->
arm_joint_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - joint1
    - joint2
    - joint3
    - joint4
    - joint5
    - joint6

  gains:
    joint1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint3:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint4:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint5:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint6:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}

gripper_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - finger_joint1
  gains:
    finger_joint1:  {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}

<!--moveit中的gazebo_controller.yaml-->
controller_manager_ns: controller_manager
controller_list:
  - name: arm_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

  - name: gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_joint1

        至此，我们已经通过marm分析完正常在gazebo中控制一个机械臂我们需要做哪些工作。并且，我们了解了如果理解在编写控制器时的命名空间一致的问题。

二、配置UR5和Robotiq_85_Gripper

安装支持noetic的ur5机器人，官方ur5并没有noetic的包

cd ~/catkin_ws/
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot
git clone https://github.com/ros-industrial/ur_msgs.git src/ur_msgs
rosdep update
rosdep install --from-paths src --ignore-src -y
catkin_make

完成以上操作后，可以测试一下。官方的ur5机械臂本身就是可以直接执行的，可以完成在gazebo的控制

roslaunch ur_gazebo ur5_bringup.launch
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true

在rviz中规划了机械臂的运动状态，plan and execute，可以发现机械臂在gazebo环境中已经开始运动了，并达到目标位置。

接下来就是gripper 官方也没有支持noetic的包

git clone https://github.com/jr-robotics/robotiq.git
catkin_make

至此位置，ur机器人和robotiq夹爪都已经下载完毕并通过编译。

        接下来还是最重要的配置urdf（即将夹爪和ur机器人合并在一起），urdf没问题了才可以实现moveit和正常的控制。其中urdf包括gazebo参数和ros_controller_plugin

        具体的urdf修改逻辑如一中所描述的marm一致，这边不再赘述，关于其moveit配置也与上面一致，如果你不理解，是不是没看一呢？

这边只展示我启动文件的配置,分别这两步：

roslaunch ur5_gazebo ur5_bringup.launch
roslaunch ur5_gripper_moveit_config ur5_gripper_moveit_planning_execution.launch sim:=true

其中ur5_bringup.launch：加载机器人参数、发布机器人状态、加载空白gazbeo世界、将model加载在gazbeo中、加载gazbeo中的控制器，并通过节点发布出来；ur5_gripper_moveit_planning_execution.launch：映射了控制器的命名空间，与gazebo_controller连接、启动move_group、启动rviz。

具体的：

ur5_bringup.launch：

<?xml version="1.0"?>
<launch>
  <!--Robot description and related parameter files -->
  <arg name="robot_description_file" default="$(dirname)/inc/load_ur5.launch.xml" doc="Launch file which populates the 'robot_description' parameter."/>
  <arg name="joint_limit_params" default="$(find ur5_description)/config/ur5/joint_limits.yaml"/>
  <arg name="kinematics_params" default="$(find ur5_description)/config/ur5/default_kinematics.yaml"/>
  <arg name="physical_params" default="$(find ur5_description)/config/ur5/physical_parameters.yaml"/>
  <arg name="visual_params" default="$(find ur5_description)/config/ur5/visual_parameters.yaml"/>

  <!-- Controller configuration -->
  <arg name="controller_config_file" default="$(find ur5_gazebo)/config/ur5_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller gripper_controller" doc="Controllers that are activated by default."/>
  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>

  <!-- robot_state_publisher configuration -->
  <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
  <arg name="tf_pub_rate" default="125" doc="Rate at which robot_state_publisher should publish transforms."/>

  <!-- Gazebo parameters -->
  <arg name="paused" default="false" doc="Starts Gazebo in paused mode" />
  <arg name="gui" default="true" doc="Starts Gazebo gui" />

  <!-- Load urdf on the parameter server -->
  <include file="$(arg robot_description_file)">
    <arg name="joint_limit_params" value="$(arg joint_limit_params)"/>
    <arg name="kinematics_params" value="$(arg kinematics_params)"/>
    <arg name="physical_params" value="$(arg physical_params)"/>
    <arg name="visual_params" value="$(arg visual_params)"/>
  </include>

  <!-- Robot state publisher -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="$(arg tf_pub_rate)" />
    <param name="tf_prefix" value="$(arg tf_prefix)" />
  </node>

  <!-- Start the 'driver' (ie: Gazebo in this case) -->
  <include file="$(dirname)/inc/ur_control.launch.xml">
    <arg name="controller_config_file" value="$(arg controller_config_file)"/>
    <arg name="controllers" value="$(arg controllers)"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
  </include>
</launch>

ur5_gripper_moveit_planning_execution.launch：

<launch>
  <arg name="sim" default="false" />
  <arg name="debug" default="false" />

  <!-- Remap follow_joint_trajectory -->
  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>

  <!-- Launch moveit -->
  <include file="$(find ur5_gripper_moveit_config)/launch/move_group.launch">
    <arg name="debug" default="$(arg debug)" />
  </include>
    
	<node pkg="rviz" type="rviz" name="rviz" args="-d $(find new_marm_moveit_config)/config/marm.rviz" />
</launch>

同样的，最终我们再一次提及控制器命名空间一致的问题。再次提及是为了加深对其的理解。

<!--gazebo中的控制器-->
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: &loop_hz 125

pos_joint_traj_controller:
  type: position_controllers/JointTrajectoryController
  joints: &robot_joints
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
    shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
    elbow_joint: {trajectory: 0.1, goal: 0.1}
    wrist_1_joint: {trajectory: 0.1, goal: 0.1}
    wrist_2_joint: {trajectory: 0.1, goal: 0.1}
    wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate: *loop_hz
  action_monitor_rate: 10

gripper_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - robotiq_85_left_knuckle_joint
  gains:
    robotiq_85_left_knuckle_joint:
      p: 40
      d: 0
      i: 0
      i_clamp: 1

joint_group_pos_controller:
  type: position_controllers/JointGroupPositionController
  joints: *robot_joints

<!--moveit中的控制器-->
controller_list:
  - name: scaled_pos_joint_traj_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_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - robotiq_85_left_knuckle_joint

        看以上的代码，你会发现夹爪的控制器确实如我所说，他们的命名空间都是gripper_controller，但是，机械臂本省的命名空间却有所不同：gazebo中控制器的命名空间是pos_joint_traj_controller:而在moveit中控制器的命名空间是scaled_pos_joint_traj_controller：。你会想这不是不一样了吗。确实，所以再启动中，会有一个“sim:=true”的过程，就是将"/scaled_pos_joint_traj_controller/follow_joint_trajectory"转到"/pos_joint_traj_controller/follow_joint_trajectory"/>，这样，控制器的命名空间又是一样的了。具体为什么要这么做，这与真实机械臂控制有关，本文只考虑仿真问题，就不提及。

三、参考文献

UR5机械臂+ROS noetic+Ubuntu20.04+moveit实物和仿真驱动_little蔡的博客-CSDN博客

ROS Tutorial: How to create a Moveit config for the UR5 and a gripper - Robotics Casual

UR5 Yolo+抓取GitHub项目复现(中) | 为UR5+robotiq85配置MoveIt包 | MoveItAssistant配置 | Ubuntu20.04+ Ros Noetic_乘风会落雨的博客-CSDN博客

ubuntu20.04安装ros-noetic,moveit,ur机器人驱动...指南！_Dawn_yc的博客-CSDN博客