UR5机械臂仿真实例（三）2 末端添加robotiq夹爪[ubuntu20.04+ROSnoetic+gazebo11]

【ur5机械臂末端添加robotiq85夹爪】

分别给出以下场景的实现过程：

一、实现rviz中ur5与robotiq85的连接和控制

二、实现gazebo中ur5与robotiq85的连接和控制【本文】

三、Moveit配置实现rivz与gazebo的联合控制

二、实现gazebo中ur5与robotiq85的连接和控制

（一）功能包下载

Robotiq2f85 gripper有被动关节（Mimic Joint），标准Gazebo不支持Mimic关节仿真， 所以需要安装一个插件。在一、实现rviz中ur5与robotiq85的连接和控制的基础上，下载plugins，然后catkin_make编译。

cd ~/catkin_ws/src
git clone https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins
catkin_make

（二）修改launch文件

gazebo的启动文件为fmauch_universal_robot/ur_gazebo/launch/ur5_bringup.launch。如一、中提到的，launch文件主要实现：加载参数、启动节点，下面一步步修改。

1. <!--Robot description and related parameter files -->加载模型参数

我们需要修改的模型文件为：load_ur5.launch.xml—>load_ur.launch.xml—>ur_gazebo/urdf/ur.xacro。

添加以下两处代码。

<!--继承robotiq_arg2f_85宏-->
<xacro:include filename="$(find robotiq_2f_85_gripper_gazebo)/urdf/robotiq_arg2f_85_macro.xacro"/>

<!--定义fixed关节，将robotiq_arg2f_base_link连接到机械臂末端的tool0-->
<joint name="ur_robotiq_joint" type="fixed">
  <parent link="tool0"/>
  <child link="robotiq_arg2f_base_link"/>
  <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<!--调用robotiq_arg2f_85，加入夹爪的link和joint-->
<xacro:robotiq_arg2f_85_gazebo 
  prefix=""
  transmission_hw_interface="$(arg transmission_hw_interface)"
/>

打开robotiq_2f_85_gripper_gazebo/urdf/robotiq_arg2f_85_macro.xacro，修改以下两处。

将插件名字改为“libroboticsgroup_gazebo_mimic_joint_plugin.so”；

<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_1">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_2">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_3">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_4">
<plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="${prefix}mimic_robotiq_85_5">

将100行左右这段代码注释掉。因为在fmauch_universal_robot/ur_gazebo/urdf/ur_macro.xacro中已经添加了这个插件，不再重复添加。

    <!--
      Inject Gazebo ROS Control plugin, which allows us to use ros_control
      controllers to control the virtual robot hw.
    -->
    <!--<gazebo>-->
      <!--<plugin name="ros_control" filename="libgazebo_ros_control.so">-->
      <!--</plugin>-->
    <!--</gazebo>-->

2. <!-- Controller configuration -->加载控制器参数

修改ur_gazebo/config/ur5_controllers.yaml。打开robotiq_2f_85_gripper_gazebo/config/robotiq_2f_85_gripper_controllers.yaml，修改gripper_controller的类型和关节。

type: position_controllers/JointTrajectoryController

joints: 
    - finger_joint 

将gripper_controller添加到ur5_controllers.yaml。

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: position_controllers/JointTrajectoryController
  gains:
    finger_joint: {p: 10,  d: 0.1, i: 1, i_clamp: 1}
  joints: 
    - finger_joint 
  action_monitor_rate: 20
  goal_tolerance: 0.002
  max_effort: 100
  stall_velocity_threshold: 0.001
  stall_timeout: 1.0

joint_group_pos_controller:
  type: position_controllers/JointGroupPositionController
  joints: *robot_joints

ur5_bringup.launch中修改controllers参数值，启动三个控制器：关节状态发布、机械臂关节控制、夹爪关节控制。

<arg name="controllers" default="joint_state_controller pos_joint_traj_controller gripper_controller" doc="Controllers that are activated by default."/>

在robotiq_2f_85_gripper_visualization/config文件夹新建gazebo_controller.yaml。

# Note: You MUST load these PID parameters for all joints that are using the
# PositionJointInterface, otherwise the arm + gripper will act like a giant
# parachute, counteracting gravity, and causing some of the wheels to lose
# contact with the ground, so the robot won't be able to properly navigate. See
# https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612
ros_control:
  pid_gains:
    # these gains are used by the gazebo_ros_control plugin
    finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
    # the following gains are used by the gazebo_mimic_joint plugin
    left_inner_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

    left_inner_finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

    right_outer_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

    right_inner_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

    right_inner_finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

ur5_bringup.launch中加载夹爪的pid参数，如果不添加这一步，仿真环境中夹爪会一直抖动。

<rosparam file="$(find robotiq_2f_85_gripper_gazebo)/config/gazebo_controller.yaml" command="load" />

3. 启动节点

添加控制节点

<node name="gui_controller" pkg="rqt_joint_trajectory_controller" type="rqt_joint_trajectory_controller" />

使用rqt_joint_trajectory_controller自带的gui_controller控制机械臂关节。

（三）启动

cd ~/catkin_ws/
catkin_make
source devel/setup.bash
roslaunch ur_gazebo ur5_bringup.launch

『相关学习资源』

 ros_control和controllers学习：ROS学习笔记10 —— ros_control和controller的使用-CSDN博客