gazebo添加末端六维力传感器

常用的机器人只支持位置控制，但实际运用中，却对末端力控制有迫切的需求，其中一种常用的方法是基于位置的阻抗控制（也称为导纳控制）实现对末端的力控。该方法只需要增加一个末端六维力传感器，就能实现机械臂的力柔顺控制，在现实中很容易实现，但是在gazebo中没有提供专用的末端六维力传感器模块，这个问题困扰了我很久。
为了解决这个问题，我让实验室的师弟专门研究了一段时间，结果他直接采用关节传感器(ros_control中提供了关节传感的控制器模块），最后成功采集了末端六维力。
末端六维力传感器添加步骤：

• 1 建立传感器URDF模型
• 2 将模型拼接到机器人末端
• 3 在××.gazebo中配置传感器

建立六维力矩传感器模型

首先绘制六维力传感器三维模型，例如用SolidWorks绘制，让后采用插件导出URDF格式的模型，具体的导出方法参照Solidworks模型转换到URDF格式并配置Moveit的详细教程,本文导出如下

<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner ([email protected]) 
     Commit Version: 1.5.0-0-g9aa0fdb  Build Version: 1.5.7004.21443
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
  name="my_sensor">
  <link
    name="sensor_link">
    <inertial>
      <origin
        xyz="5.187E-06 8.9595E-06 0.011516"
        rpy="0 0 0" />
      <mass
        value="0.10567" />
      <inertia
        ixx="7.1441E-05"
        ixy="1.6499E-08"
        ixz="-4.9269E-09"
        iyy="7.146E-05"
        iyz="-8.5336E-09"
        izz="0.00013366" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://my_arm/meshes/sensor_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.79216 0.79216 0.79216 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://my_arm/meshes/sensor_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="sensor_joint"
    type="revolute">
    <origin
      xyz="0 0 0.192"
      rpy="0 0 0.12101" />
    <parent
      link="Link7" />
    <child
      link="sensor_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="0"
      upper="0"
      effort="0"
      velocity="0" />
    <safety_controller
      k_velocity="0" />
  </joint>
</robot>

如上所示，六维力传感器可以看做连杆和关节的组合体，关节的parent连杆采用需要连接传感器的机械臂对应连杆，child连杆采用自己绘制的传感器。需要注意的是，这中建模并不是准确的等效建模，若想要更高的建模精度，传感器需要建模为两个连杆和一个关节的组合体，尺寸和实际传感器完全一致。

<parent
    link="Link7" />
<child
   link="sensor_link" />

将传感器拼接到机器人模型

直接将外观文件sensor_link.STL粘贴到对应机器人模型文件的mesh文件中，将URDF粘贴到对应添加的位置即可，注意两个连杆之间需要用关节连接，如果不是实际关节，可以将关节设置为固定实现。如采用两个连杆和一个关节的组合体建模的传感器，传感器与机器人连杆连接处关节必须设置为固定。机器人的gazebo配置可参照ROS中gazebo配置教程详解

<link
    name="Link7">
    <inertial>
      <origin
        xyz="2.7283E-15 -5.3659E-09 0.18582"
        rpy="0 0 0" />
      <mass
        value="0.10414" />
      <inertia
        ixx="5.1163E-05"
        ixy="6.3527E-20"
        ixz="8.2388E-21"
        iyy="5.1163E-05"
        iyz="-3.3915E-21"
        izz="9.972E-05" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://armc_description/meshes/Link7.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.89804 0.91765 0.92941 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://armc_description/meshes/Link7.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="Joint7"
    type="revolute">
    <origin
      xyz="0 0 0"
      rpy="1.5708 0 0" />
    <parent
      link="Link6" />
    <child
      link="Link7" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="0"
      upper="0"
      effort="0"
      velocity="0" />
    <safety_controller
      k_velocity="0" />
  </joint>
  <link
    name="sensor_link">
    <inertial>
      <origin
        xyz="5.187E-06 8.9595E-06 0.011516"
        rpy="0 0 0" />
      <mass
        value="0.10567" />
      <inertia
        ixx="7.1441E-05"
        ixy="1.6499E-08"
        ixz="-4.9269E-09"
        iyy="7.146E-05"
        iyz="-8.5336E-09"
        izz="0.00013366" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://armc_description/meshes/sensor_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 0 0 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://armc_description/meshes/sensor_link.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="sensor_joint"
    type="revolute">
    <origin
      xyz="0 0 0.192"
      rpy="0 0 0.12101" />
    <parent
      link="Link7" />
    <child
      link="sensor_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="0"
      upper="0"
      effort="0"
      velocity="0" />
    <safety_controller
      k_velocity="0" />
  </joint>

配置传感器gazebo参数

在ros_control中，关节传感器在controller中有独立的功能包force_torque_sensor_controller，完全可以和关节控制器一样的方法启动，但是为了更简单的启动传感器，本文提供了一种更简单的方法，直接在××.gazebo中配置。

<?xml version="1.0"?>
<robot>
<!--设置传感器外观-->
  <gazebo reference="sensor_link">
  <material>Gazebo/Red</material>
  <mu1 value="10.0"/>
  <mu2 value="10.0"/>
  <kp value="0.3" />
  <kd value="1.0" />
  <fdir1 value="1 0 0"/>
  <gravity value="true"/>
</gazebo>
<!--机器人控制器-->
<gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>armc</robotNamespace>
     <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
   </plugin>
</gazebo>

<!--对关节设置反馈为真-->
<gazebo 
      reference="sensor_joint">
     <provideFeedback>true</provideFeedback>
</gazebo>
<!-- 添加ft_sensor插件 -->
<gazebo>
    <plugin name="ft_sensor" filename="libgazebo_ros_ft_sensor.so">
      <updateRate>100.0</updateRate>
      <topicName>ft_sensor_topic</topicName>
      <jointName>sensor_joint</jointName>
    </plugin>
</gazebo>
</robot>

如上所示：在传感器插件中设置了传感器的采集频率和反馈话题名称

<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic</topicName>

配置总结：

• 1 传感器是关节传感器，必须依附于关节
• 2 对应关节是否反馈设置为真
• 3 添加传感器插件，并设置话题名和反馈频率

实际效果图：红色部分为末端六维力传感器
采用rostopic list可以检测到已经获得力传感器的话题：/ft_sensor_topic

d@f:~$ rostopic list 
/armc/joint_positions_controller/command
/armc/joint_states
/clock
/ft_sensor_topic
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/joint_states
/rosout
/rosout_agg
/tf
/tf_static