在ROS中配置使用Robotiq二指抓手（三）

前言

这部分内容主要讲如何实现Robotiq二指抓手在Gazebo中的仿真。
由于在二指抓手中使用了mimic标签，而Gazebo无法识别，导致直接读取二指手抓模型会在Gazebo中离散。
之前已经介绍过，可以使用mimic的插件实现（https://blog.csdn.net/weixin_42268975/article/details/106255685）。本文主要讲，如何在gazebo中为robotiq二指手抓配置该插件。

安装插件

https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins
下载该插件功能包并编译安装。然后，将该插件功能包添加到工作空间。
可以用echo $ROS_PACKAGE_PATH查看是否添加到工作空间。

需要注意：由于这是外部添加的插件，需要在使用时确认该插件功能包已经添加到工作空间，插件功能才会生效。

配置Robotiq二指手抓相关文件

这里使用robotiq的纯原生功能包https://github.com/ros-industrial/robotiq.git

1. 修改URDF

主要修改robotiq_2f_140_gripper_visualization/urdf/robotiq_arg2f_transmission.xacro，添加三块内容。需要注意，对于transmission标签只需要给主动关节配置即可，不需要给被动关节（mimic）配置，不然可能会用ros_control来驱动。

另外，如果报错：

Deprecated syntax, please prepend ‘hardware_interface/’ to ‘PositionJointInterface’ within the tag in joint ‘finger_joint’.

在robotiq_arg2f_transmission.xacro中更新标签如下：

<hardwareInterface>hardware_interface/PositionJointInterfacehardwareInterface>

1.1 添加mimic插件宏

  <xacro:macro name="mimic_joint_plugin_gazebo" params="name_prefix parent_joint mimic_joint has_pid:=false multiplier:=1.0 offset:=0 sensitiveness:=0.0 max_effort:=1.0 robot_namespace:=''">
    <gazebo>
      <plugin name="${name_prefix}mimic_joint_plugin" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
        <joint>${parent_joint}joint>
        <mimicJoint>${mimic_joint}mimicJoint>
        <xacro:if value="${has_pid}">                     
          <hasPID />
        xacro:if>
        <multiplier>${multiplier}multiplier>
        <offset>${offset}offset>
        <sensitiveness>${sensitiveness}sensitiveness>   
        <maxEffort>${max_effort}maxEffort>              
        <xacro:unless value="${robot_namespace == ''}">
          <robotNamespace>($robot_namespace)robotNamespace>
        xacro:unless>
      plugin>
    gazebo>
  xacro:macro>

1.2 被动关节配置

接着，对5个被动关节进行配置，注意parent joint 及multiplier，使其与相应joint的mimic标签对应。注意到这里has_pid="true"，因此需要为期配置PID参数，见2.1

  <xacro:mimic_joint_plugin_gazebo name_prefix="left_inner_knuckle_joint"
    parent_joint="finger_joint" mimic_joint="left_inner_knuckle_joint"
    has_pid="true" multiplier="-1.0" max_effort="10.0" />

  <xacro:mimic_joint_plugin_gazebo name_prefix="left_inner_finger_joint"
    parent_joint="finger_joint" mimic_joint="left_inner_finger_joint"
    has_pid="true" multiplier="1.0" max_effort="10.0" />

  <xacro:mimic_joint_plugin_gazebo name_prefix="right_outer_knuckle_joint"
    parent_joint="finger_joint" mimic_joint="right_outer_knuckle_joint"
    has_pid="true" multiplier="-1.0" max_effort="10.0" />

  <xacro:mimic_joint_plugin_gazebo name_prefix="right_inner_knuckle_joint"
    parent_joint="finger_joint" mimic_joint="right_inner_knuckle_joint"
    has_pid="true" multiplier="-1.0" max_effort="10.0" />

  <xacro:mimic_joint_plugin_gazebo name_prefix="right_inner_finger_joint"
    parent_joint="finger_joint" mimic_joint="right_inner_finger_joint"
    has_pid="true" multiplier="1.0" max_effort="10.0" />

1.3 添加ros_control插件。

为主动关节配置gazebo_ros_control插件。

  <gazebo>
      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
          <robotNamespace>/robotNamespace>
          <legacyModeNS>truelegacyModeNS>
      plugin>
  gazebo>

2. 修改控制器文件

新建robotiq_2f_140_gripper_gazebo功能包，用来配置相关控制器文件。我把它建在robotiq元功能包的src目录下。里面内容与mimic教程相同，放置了config文件夹、launch文件夹，对控制器文件做了相关修改。

2.1 gazebo_controller.yaml

不论是主动关节，还是被动关节，都需要使用pid参数。
在robotiq_2f_140_gripper_gazebo/config/gazebo下创建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
gazebo_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

2.2 joint_state_controller.yaml

在robotiq_2f_140_gripper_gazebo/config/ros_control下创建joint_state_controller.yaml

# Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

2.3 joint_trajectory_controller.yaml

在src/robotiq/robotiq_2f_140_gripper_gazebo/config/ros_control下创建joint_trajectory_controller.yaml，实现对主动关节的控制，注意joints修改为了主动关节的名称。

# Position Controllers ---------------------------------------
gripper_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - finger_joint
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    gripper_finger_joint: {trajectory: 0.2, goal: 0.2}
  stop_trajectory_duration: 0.5
  state_publish_rate:  125
  action_monitor_rate: 10

3. launch文件

在robotiq_2f_140_gripper_gazebo/launch下创建gazebo.launch，注意其中几个文件的读取路径。

<launch>
  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find robotiq_2f_140_gripper_visualization)/urdf/robotiq_arg2f_140_model.xacro" />

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

  
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="false" />
  include>

  
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model robotiq_2f_140_gripper_model" />

  
  <rosparam file="$(find robotiq_2f_140_gripper_gazebo)/config/ros_control/joint_state_controller.yaml" command="load" />
  <rosparam file="$(find robotiq_2f_140_gripper_gazebo)/config/ros_control/joint_trajectory_controller.yaml" command="load" />

  
  <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
    args="joint_state_controller gripper_controller"/>

  
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />

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

如果有报错：

ERROR: cannot launch node of type [rqt_joint_trajectory_controller/rqt_joint_trajectory_controller]: rqt_joint_trajectory_controller
ROS path [0]=/opt/ros/kinetic/share/ros
…………

解决方法查看：https://blog.csdn.net/weixin_42268975/article/details/106255685

4. 其他

需要指出的是，

1. 在使用mimic插件功能时，不需要特意配置CMakeListstxt及package.xml.在我robotiq_2f_140_gripper_gazebo下的这两个文件未有任何改动，纯原生。
2. 默认robotiq二指手抓底座有惯性标签，直接会有警告。可以在该杆件前添加一个空杆件
3. 我的手指一直在轻微抖动，是重力+pid的综合影响？可能调个好点的pid就好了？