基于ROS的UR5机器人开发学习（三）| 安装力矩传感器插件和Onrobot RG2夹爪

操作系统：Ubuntu18.04
ROS版本：Melodic

1.使用Gazebo中的力矩传感器插件F/T sensor

1.1 添加插件

在ur_gazebo —>urdf—>ur_macro.xacro中添加如下代码：

 <!-- Gazebo FT sensor plugin -->

  <gazebo reference="wrist_3_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>

  <gazebo>
    <plugin name="ft_sensor_plugin" filename="libgazebo_ros_ft_sensor.so">
      <updateRate>200</updateRate>
      <topicName>ft_sensor/raw</topicName>
      <gaussianNoise>0.0</gaussianNoise>
      <jointName>wrist_3_joint</jointName>
    </plugin>
  </gazebo>

1.2 完整文件

最终完整的ur_macro.xacro如下所示：

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

  <!-- Definition of the main macro -->
  <xacro:macro name="ur_robot_gazebo" params="
   prefix
   joint_limits_parameters_file
   kinematics_parameters_file
   physical_parameters_file
   visual_parameters_file
   transmission_hw_interface:=hardware_interface/EffortJointInterface
   safety_limits:=false safety_pos_margin:=0.15 safety_k_position:=20"
  >
    <!--
      Import the xacro macro for the REAL robot (which we'll augment with Gazebo
      specific elements in the wrapper macro below).

      NOTE: this imports the '_macro.xacro' from myur5_description, as that contains
      the definitions for the real robot.
    -->
  <xacro:include filename="$(find myur5_description)/urdf/inc/ur_macro.xacro"/>


    <!-- Instantiate model for the REAL robot. -->
    <xacro:ur_robot
      prefix="${prefix}"
      joint_limits_parameters_file="${joint_limits_parameters_file}"
      kinematics_parameters_file="${kinematics_parameters_file}"
      physical_parameters_file="${physical_parameters_file}"
      visual_parameters_file="${visual_parameters_file}"
      transmission_hw_interface="${transmission_hw_interface}"
      safety_limits="${safety_limits}"
      safety_pos_margin="${safety_pos_margin}"
      safety_k_position="${safety_k_position}"
    />

    <!-- Configure self collision properties per link -->
    <gazebo reference="${prefix}shoulder_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}upper_arm_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}forearm_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}wrist_1_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}wrist_3_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}wrist_2_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}ee_link">
      <selfCollide>true</selfCollide>
    </gazebo>


 <!-- Gazebo FT sensor plugin -->

  <gazebo reference="wrist_3_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>

  <gazebo>
    <plugin name="ft_sensor_plugin" filename="libgazebo_ros_ft_sensor.so">
      <updateRate>1</updateRate>
      <topicName>ft_sensor/raw</topicName>
      <gaussianNoise>0.0</gaussianNoise>
      <jointName>wrist_3_joint</jointName>
    </plugin>
  </gazebo>
    <!--
      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">
        <!--robotNamespace>/</robotNamespace-->
        <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
      </plugin>
    </gazebo>
 
  </xacro:macro>
</robot>

1.3查看力矩传感器信息

先运行Gazebo

roslaunch myur5_gazebo ur5_bringup.launch

再订阅相关话题ft_sensor/raw：

 rostopic echo -c ft_sensor/raw

2. 安装Onrobot RG2夹爪

2.1 下载功能包Onrobot_ros

下载功能包到自定义的工作空间下并编译

2.2 在UR5上安装夹爪

在ur_description —>urdf—>ur_macro.xacro中添加如下代码：

前面加入声明：

    <xacro:include filename="$(find onrobot_description)/urdf/onrobot_rg.urdf.xacro" /> 

末端关节tool0加入：

    <!-- rg2 joint -->
    <xacro:onrobot_rg parent="tool0" model='rg2_v1' prefix='rg2'>
       <origin xyz="0.0 0.0 0.0" rpy="0.0 0 0.0" />
    </xacro:onrobot_rg>

2.2 完整文件

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

  <xacro:include filename="$(find myur5_description)/urdf/inc/ur_transmissions.xacro" />
  <xacro:include filename="$(find myur5_description)/urdf/inc/ur_common.xacro" />
  <xacro:include filename="$(find onrobot_description)/urdf/onrobot_rg.urdf.xacro" /> 

  <xacro:macro name="ur_robot" params="
    prefix
    joint_limits_parameters_file
    kinematics_parameters_file
    physical_parameters_file
    visual_parameters_file
    transmission_hw_interface:=hardware_interface/PositionJointInterface
    safety_limits:=false
    safety_pos_margin:=0.15
    safety_k_position:=20"
  >
    <!-- Load configuration data from the provided .yaml files -->
    <xacro:read_model_data
      joint_limits_parameters_file="${joint_limits_parameters_file}" 
      kinematics_parameters_file="${kinematics_parameters_file}"
      physical_parameters_file="${physical_parameters_file}"
      visual_parameters_file="${visual_parameters_file}"/>

    <!-- Add URDF transmission elements (for ros_control) -->
    <xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />

    <!-- links: main serial chain -->
    <link name="${prefix}base_link"/>
    <link name="${prefix}base_link_inertia">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="${base_visual_mesh}"/>
        </geometry>
        <material name="${base_visual_material_name}">
          <color rgba="${base_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="${base_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${base_inertia_radius}" length="${base_inertia_length}" mass="${base_mass}">
        <origin xyz="0 0 0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>
    <link name="${prefix}shoulder_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="${shoulder_visual_mesh}"/>
        </geometry>
        <material name="${shoulder_visual_material_name}">
          <color rgba="${shoulder_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="${shoulder_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${shoulder_inertia_radius}" length="${shoulder_inertia_length}" mass="${shoulder_mass}">
        <origin xyz="0 0 0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>
    <link name="${prefix}upper_arm_link">
      <visual>
        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="${upper_arm_visual_mesh}"/>
        </geometry>
        <material name="${upper_arm_visual_material_name}">
          <color rgba="${upper_arm_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="${upper_arm_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${upperarm_inertia_radius}" length="${upperarm_inertia_length}" mass="${upper_arm_mass}">
        <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
      </xacro:cylinder_inertial>
    </link>
    <link name="${prefix}forearm_link">
      <visual>
        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="${forearm_visual_mesh}"/>
        </geometry>
        <material name="${forearm_visual_material_name}">
          <color rgba="${forearm_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
        <geometry>
          <mesh filename="${forearm_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}"  mass="${forearm_mass}">
        <origin xyz="${-0.5 * forearm_inertia_length} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
      </xacro:cylinder_inertial>
    </link>
    <link name="${prefix}wrist_1_link">
      <visual>
        <!-- TODO: Move this to a parameter -->
        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="${wrist_1_visual_mesh}"/>
        </geometry>
        <material name="${wrist_1_visual_material_name}">
          <color rgba="${wrist_1_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="${wrist_1_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}"  mass="${wrist_1_mass}">
        <origin xyz="0 0 0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>
    <link name="${prefix}wrist_2_link">
      <visual>
        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
        <geometry>
          <mesh filename="${wrist_2_visual_mesh}"/>
        </geometry>
        <material name="${wrist_2_visual_material_name}">
          <color rgba="${wrist_2_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
        <geometry>
          <mesh filename="${wrist_2_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}"  mass="${wrist_2_mass}">
        <origin xyz="0 0 0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>
    <link name="${prefix}wrist_3_link">
      <visual>
        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="${wrist_3_visual_mesh}"/>
        </geometry>
        <material name="${wrist_3_visual_material_name}">
          <color rgba="${wrist_3_visual_material_color}"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
        <geometry>
          <mesh filename="${wrist_3_collision_mesh}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}"  mass="${wrist_3_mass}">
        <origin xyz="0.0 0.0 ${-0.5 * wrist_3_inertia_length}" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

    <!-- joints: main serial chain -->
    <joint name="${prefix}base_link-base_link_inertia" type="fixed">
      <parent link="${prefix}base_link" />
      <child link="${prefix}base_link_inertia" />
      <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
           frames of the robot/controller have X+ pointing backwards.
           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
           link/frame) to introduce the necessary rotation over Z (of pi rad).
      -->
      <origin xyz="0 0 0" rpy="0 0 ${pi}" />
    </joint>
    <joint name="${prefix}shoulder_pan_joint" type="revolute">
      <parent link="${prefix}base_link_inertia" />
      <child link="${prefix}shoulder_link" />
      <origin xyz="${shoulder_x} ${shoulder_y} ${shoulder_z}" rpy="${shoulder_roll} ${shoulder_pitch} ${shoulder_yaw}" />
      <axis xyz="0 0 1" />
      <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}"
        effort="${shoulder_pan_effort_limit}" velocity="${shoulder_pan_velocity_limit}"/>
      <xacro:if value="${safety_limits}">
         <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
      </xacro:if>
      <dynamics damping="0" friction="0"/>
    </joint>
    <joint name="${prefix}shoulder_lift_joint" type="revolute">
      <parent link="${prefix}shoulder_link" />
      <child link="${prefix}upper_arm_link" />
      <origin xyz="${upper_arm_x} ${upper_arm_y} ${upper_arm_z}" rpy="${upper_arm_roll} ${upper_arm_pitch} ${upper_arm_yaw}" />
      <axis xyz="0 0 1" />
      <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}"
        effort="${shoulder_lift_effort_limit}" velocity="${shoulder_lift_velocity_limit}"/>
      <xacro:if value="${safety_limits}">
         <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
      </xacro:if>
      <dynamics damping="0" friction="0"/>
    </joint>
    <joint name="${prefix}elbow_joint" type="revolute">
      <parent link="${prefix}upper_arm_link" />
      <child link="${prefix}forearm_link" />
      <origin xyz="${forearm_x} ${forearm_y} ${forearm_z}" rpy="${forearm_roll} ${forearm_pitch} ${forearm_yaw}" />
      <axis xyz="0 0 1" />
      <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}"
        effort="${elbow_joint_effort_limit}" velocity="${elbow_joint_velocity_limit}"/>
      <xacro:if value="${safety_limits}">
         <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
      </xacro:if>
      <dynamics damping="0" friction="0"/>
    </joint>
    <joint name="${prefix}wrist_1_joint" type="revolute">
      <parent link="${prefix}forearm_link" />
      <child link="${prefix}wrist_1_link" />
      <origin xyz="${wrist_1_x} ${wrist_1_y} ${wrist_1_z}" rpy="${wrist_1_roll} ${wrist_1_pitch} ${wrist_1_yaw}" />
      <axis xyz="0 0 1" />
      <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}"
        effort="${wrist_1_effort_limit}" velocity="${wrist_1_velocity_limit}"/>
      <xacro:if value="${safety_limits}">
         <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
      </xacro:if>
      <dynamics damping="0" friction="0"/>
    </joint>
    <joint name="${prefix}wrist_2_joint" type="revolute">
      <parent link="${prefix}wrist_1_link" />
      <child link="${prefix}wrist_2_link" />
      <origin xyz="${wrist_2_x} ${wrist_2_y} ${wrist_2_z}" rpy="${wrist_2_roll} ${wrist_2_pitch} ${wrist_2_yaw}" />
      <axis xyz="0 0 1" />
      <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}"
             effort="${wrist_2_effort_limit}" velocity="${wrist_2_velocity_limit}"/>
      <xacro:if value="${safety_limits}">
         <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
      </xacro:if>
      <dynamics damping="0" friction="0"/>
    </joint>
    <joint name="${prefix}wrist_3_joint" type="revolute">
      <parent link="${prefix}wrist_2_link" />
      <child link="${prefix}wrist_3_link" />
      <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
      <axis xyz="0 0 1" />
      <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
             effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
      <xacro:if value="${safety_limits}">
         <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
      </xacro:if>
      <dynamics damping="0" friction="0"/>
    </joint>

    <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
    <link name="${prefix}base"/>
    <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
      <!-- Note the rotation over Z of pi radians: as base_link is REP-103
           aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
           to correctly align 'base' with the 'Base' coordinate system of
           the UR controller.
      -->
      <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}base"/>
    </joint>

    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
    <link name="${prefix}flange" />
    <joint name="${prefix}wrist_3-flange" type="fixed">
      <parent link="${prefix}wrist_3_link" />
      <child link="${prefix}flange" />
      <origin xyz="0 0 0" rpy="0 ${-pi/2.0} ${-pi/2.0}" />
    </joint>

    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
    <link name="${prefix}tool0"/>
    <joint name="${prefix}flange-tool0" type="fixed">
      <!-- default toolframe: X+ left, Y+ up, Z+ front -->
      <origin xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}"/>
      <parent link="${prefix}flange"/>
      <child link="${prefix}tool0"/>
    </joint>

    <!-- rg2 joint -->
    <xacro:onrobot_rg parent="tool0" model='rg2_v1' prefix='rg2'>
       <origin xyz="0.0 0.0 0.0" rpy="0.0 0 0.0" />
    </xacro:onrobot_rg>

  </xacro:macro>
</robot>

3.控制Onrobot RG2夹爪

#!/usr/bin/env python

import actionlib
import rospy
from control_msgs.msg import *


def main():
    rospy.init_node('test_object_recognition')

    client = actionlib.SimpleActionClient(
        'rg2/gripper_controller/gripper_cmd', GripperCommandAction)
    client.wait_for_server()

    def gripper_cmd(position, effort):
        rospy.loginfo('Cmd:\nposition=%.2f, max_effort=%.2f', position, effort)

        goal = GripperCommandGoal()
        goal.command.position = position
        goal.command.max_effort = effort
        client.send_goal(goal)

        if client.wait_for_result():
            result = client.get_result()
            rospy.loginfo('Result:\n%s', result)

    gripper_cmd(position=1, effort=0)
    gripper_cmd(position=0, effort=0)
    gripper_cmd(position=1, effort=1)
    gripper_cmd(position=0, effort=1)


if __name__ == '__main__':
    main()

后续工作：力矩传感器消除夹爪重力并进行零点标定
未完待续……