UR3机械臂添加robotiq FT sensor和robotiq 140 2指夹爪
笔者环境：ros kinetic
IDE：roboware 1.2.0，roboware的官方最新版下载地址可查看
Ubuntu – 安装官方最新RoboWare
建议参考这篇
，
ur机械臂 + robotiq gripper + robotiq ft sensor + gazebo + 连接真实机械臂 + 网页控制
，
这是后来做的代码更加规范、内容更加丰富的一个，并且bug更少。
建议参考这篇
这是后来做的代码更加规范、内容更加丰富的一个，并且bug更少。
一、下载ur、robotiq的官方ros package
安装ur的ros驱动和ros package
，可点击查看详细信息，下面只把安装过程列出：
# source global ros
$ source /opt/ros/<your_ros_version>/setup.bash
# create a catkin workspace
$ mkdir -p catkin_ws/src && cd catkin_ws
# clone the driver
$ git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver
# clone fork of the description. This is currently necessary, until the changes are merged upstream.
$ git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot
# install dependencies
$ sudo apt update -qq
$ rosdep update
$ rosdep install --from-paths src --ignore-src -y
# build the workspace
$ catkin_make
# activate the workspace (ie: source it)
$ source devel/setup.bash
cd ~/catkin_ws/src
git clone https://github.com/ros-industrial/robotiq.git
rosdep install robotiq_modbus_tcp
sudo apt-get install ros-melodic-soem # 官方是ros-kinetic-soem，要换成对应的ros版本
二、新建一个工作空间，对用到的文件进行裁剪，并装配ur、ft sensor、gripper
mkdir -p ~/ml_ws/src
cd ~/ml_ws/src
catkin_init_workspace
然后使用roboware打开该工作空间。
现阶段，我们只使用到了三个设备的描述文件（description），所以在
ml_ws/src
下建立
ur3
和
robotiq
两个文件夹。将下载robotiq的ros package
robotiq
中的
robotiq_ft_sensor
，
robotiq_2f_140_gripper_visualization
文件夹和ur机械臂的ros package
famuch_universal_robot
中的
ur_description
复制一份到
~/ml_ws/src
文件夹下。最后目录结构如图：（ur_moveit_config是后面使用moveit_setup_assistant生成的）
打开roboware，执行ROS->Build。
① 添加ft sensor到ur末端
查看ft sensor的urdf，
可以看到整个ft sensor的描述文件在
robotiq_ft300.urdf.xacro
文件中被描述成了一个宏，并且第一个
joint
提供了
parent *origin
等参数，所以只需要给这个宏的参数赋值，就可以将ft sensor集成到新的描述文件中。
查看ur的描述文件
ur3.urdf.xacro
（这里使用ur3），
同样ur3也被声明为一个宏。在文件的最后，
可以看到ur3暴露给最外面的link是
${prefix}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
基础上做了修改。
<?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被成功添加。
完整的代码可参考
github上源码
② 添加robotiq 2 finger 140 gripper
查看gripper的描述文件，就发现复杂的多了，因为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>
为什么要无中生有一个joint出来的，查看ft sensor的宏定义（robotiq_ft300.urdf.xacro文件），可以看到最后暴露的link是
而gripper的宏定义（robotiq_arg2f_140_model_macro文件），暴露的第一个却是一个link。
link与link之间需要有一个joint来连接并确定位置。
同样，在
ur_description/launch
文件夹新建新的launch文件，
view_ur3_ft_gripper140.launch
，
ur3_ft_gripper140_upload.launch
。
完整的代码可参考
github上源码
最后，用
moveit_setup_assistant
生成新的配置文件，就可以进行moveit规划。