ur3+robotiq ft sensor+robotiq f140仿真

ur3+robotiq ft sensor+robotiq f140仿真

配置：
ubuntu16.04 kinetic
robotiq ft sensor
robotiq f140
ur3
参考链接：https://blog.csdn.net/bornfree5511/article/details/106454699

1.新建工作空间ml_ws,并且将robotiq 和ur3的功能包拷贝到src目录下

新建ml_ws的工作空间

mkdir -p ~/ml_ws/src

cd ~/ml_ws/src

catkin_init_workspace

注释：最后一句为初始化工作空间
编译工作空间

cd ~/ml_ws

catkin_make

配置环境变量
打开环境变量配置文件

gedit ~/.bashrc 

将下列添加到最后边

source /home/li/ml_ws/devel/setup.bash

注释：li为笔者电脑名称，ml_ws为笔者新建工作空间名字
将robotiq和ur3功能包拷贝到src文件夹下：

注释：ur功能包：

git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git

git clone https://github.com/ros-industrial/robotiq.git

2.在机械臂末端添加robotiq ft sensor

查看ft sensor的urdf，fr sensor被定义为宏：

查看ur的描述文件ur3.urdf.xacro，ur3也被定义了宏：

并且在ur3.urdf.xacro最后末端link为tool0：

将ft sensor的宏定义的parent赋值为tool0即可。
同时观察文件中还有ur3_robot.urdf.xacro文件，这是对调用ur3.urdf.xacro中的宏定义的一个使用示例代码，同样的也可以应用于所有的宏定义的调用方式。
新建文件ur3_ft_robot.urdf.xacro，将ur3_robot.urdf.xacro中的代码（这是对ur3机械臂宏定义的调用）复制过来，并添加robotiq_ft300.urdf.xacro的宏定义的调用。添加如下代码：

<xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro" />
 <!--robotiq FT_300 sernsor-->
 <xacro:robotiq_ft300 prefix="" parent="tool0" >
	<origin xyz="0 0 0" rpy="0 0 0"/> 
</xacro:robotiq_ft300>

最后整个文档：

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

  <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur3 -->
  <xacro:include filename="$(find ur_description)/urdf/ur3.urdf.xacro" />
  <xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro" />

  <!-- arm -->
  <xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml"/>

  <xacro:ur3_robot prefix="" joint_limited="false"
    transmission_hw_interface="$(arg transmission_hw_interface)"
    kinematics_file="${load_yaml('$(arg kinematics_config)')}"
  />

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>

  
  <!--robotiq FT_300 sernsor-->
  <xacro:robotiq_ft300 prefix="" parent="tool0" >
		<origin xyz="0 0 0" rpy="0 0 0"/> 
	</xacro:robotiq_ft300>

</robot>

然后，在ur3的ur_description文件夹的launch文件夹下新建view_ur3_ft.launch
此文件是在view_ur3.launch基础上做了修改。
注释：此为将机械臂在rviz显示出来，并不是与moveit的仿真。

<?xml version="1.0"?>
<launch>
  <include file="$(find ur_description)/launch/ur3_ft_upload.launch"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
    <param name="use_gui" value="true"/>
  </node>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_description)/cfg/view_robot_ft.rviz" required="true" />
</launch>

新建ur3_ft_upload.launch

<?xml version="1.0"?>
<launch>
  <arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" />
  <arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml"/>

  <param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur3_ft_robot.urdf.xacro'
    transmission_hw_interface:=$(arg transmission_hw_interface)
    kinematics_config:=$(arg kinematics_config)" />
  <param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur3_joint_limited_robot.urdf.xacro'
    transmission_hw_interface:=$(arg transmission_hw_interface)
    kinematics_config:=$(arg kinematics_config)" />
</launch>

运行view_ur3_ft.launch，可以看到ft sensor被成功添加。

3.在robotiq ft sensor传感器末端添加robotiq ft夹爪

查看gripper的描述文件。
robotiq_arg2f_140_model.xacro

是一个对gripper宏定义的调用，类似ur中的ur3_robot.urdf.xacro文件。
根据include，查看robotiq_arg2f_140_model_macro.xacro文件，这里的宏定义就非常非常多了。
如果不清楚这里的宏定义分别是对应哪个部位，可以先运行launch文件中的test_2f_140_model.launch文件，在rviz中取消每个link的选择来查看对应的组件。

在robotiq_arg2f_140_model_macro.xacro文件的最后，我们终于看到了整个夹爪的宏定义。

但是奇怪的是它引用了几个当前文件中没有的宏定义，还剩一个文件没看，robotiq_arg2f.xacro，查看这个文件的源码，发现是对robotiq_arg2f_140_model_macro.xacro文件中的所有宏定义的一个调用，相当于在函数的外面又封装了一层函数，用来简化代码。现在整个描述文件的源码架构已经被我们搞清楚了。现在来魔改。robotiq gripper的代码和ft sensor的还不太一样，需要修改的地方稍多一些。

新建ur3_ft_gripper140_robot.urdf.xacro

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

  <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

  <!-- ur3 -->
  <xacro:include filename="$(find ur_description)/urdf/ur3.urdf.xacro" />
  <xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro" />
  <xacro:include filename="$(find robotiq_2f_140_gripper_visualization)/urdf/robotiq_arg2f_140_model_macro.xacro" />
  <!-- arm -->
  <xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml"/>

  <xacro:ur3_robot prefix="" joint_limited="false"
    transmission_hw_interface="$(arg transmission_hw_interface)"
    kinematics_file="${load_yaml('$(arg kinematics_config)')}"
  />

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>

  
  <!--robotiq FT_300 sernsor-->
  <xacro:robotiq_ft300 prefix="" parent="tool0" >
		<origin xyz="0 0 0" rpy="0 0 0"/> 
	</xacro:robotiq_ft300>

  <xacro:robotiq_arg2f_140 prefix=""/>

  <joint name="ft_gripper_joint" type="fixed">
    <parent link="robotiq_ft_frame_id"/>
    <child link="robotiq_arg2f_base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>
  
</robot>

可以看出这个文件是在ur3_ft_robot.urdf.xacro的基础上，添加了

<xacro:include filename="$(find robotiq_2f_140_gripper_visualization)/urdf/robotiq_arg2f_140_model_macro.xacro" />

和

 <xacro:robotiq_arg2f_140 prefix=""/>

  <joint name="ft_gripper_joint" type="fixed">
    <parent link="robotiq_ft_frame_id"/>
    <child link="robotiq_arg2f_base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

查看ft sensor的宏定义（robotiq_ft300.urdf.xacro文件），可以看到最后暴露的link是

link与link之间需要有一个joint来连接并确定位置。
同样，在ur_description/launch文件夹新建新的launch文件，view_ur3_ft_gripper140.launch，

<?xml version="1.0"?>
<launch>
  <include file="$(find ur_description)/launch/ur3_ft_gripper140_upload.launch"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
    <param name="use_gui" value="true"/>
  </node>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_description)/cfg/view_robot_ft.rviz" required="true" />
</launch>

ur3_ft_gripper140_upload.launch。

<?xml version="1.0"?>
<launch>
  <arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" />
  <arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml"/>

  <param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur3_ft_gripper140_robot.urdf.xacro'
    transmission_hw_interface:=$(arg transmission_hw_interface)
    kinematics_config:=$(arg kinematics_config)" />
  <param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur3_joint_limited_robot.urdf.xacro'
    transmission_hw_interface:=$(arg transmission_hw_interface)
    kinematics_config:=$(arg kinematics_config)" />
</launch>

最后，用moveit_setup_assistant生成新的配置文件，就可以进行moveit规划。