UR机器人双臂开发实例

前言：UR系列机器人已经运用一段时间了，随着应用过程中发现并解决的问题越来越多，脑袋已经不够记录了，因此撰写系列博文进行记录，本篇博文记录核心内容，基础内容等我有时间再详细描述。

1 已完成的内容

目前已经完成了UR系列机器人的配置，工作空间名为：ur_ws。调用了 Universal_Robots_ROS_Driver；fmauch_universal_robot；puncture_demo 三个功能包。自行编写了sh脚本文件如下：以UR3为例。
文件名：ur3_setup.sh

gnome-terminal -t "roscore" -x bash -c "source /opt/ros/kinetic/setup.bash;roscore;"
sleep 1
gnome-terminal --geometry=60x5+1500+10 -x bash -c "source ~/ur_ws/devel/setup.bash; roslaunch ur_robot_driver ur3_bringup.launch limited:=true robot_ip:=192.168.1.102; exec bash;"
sleep 5
gnome-terminal --geometry=60x5+1500+150 -x bash -c "source ~/ur_ws/devel/setup.bash;roslaunch ur3_moveit_config ur3_moveit_planning_execution.launch limited:=true; exec bash;"
sleep 3
gnome-terminal --geometry=60x5+1500+300 -x bash -c "source ~/ur_ws/devel/setup.bash;roslaunch ur3_moveit_config moveit_rviz.launch config:=true;exec bash;"
sleep 3
gnome-terminal --geometry=60x5+1500+450 -x bash -c "source ~/ur3e_ws/devel/setup.bash;rosrun puncture_demo puncture_demo;exec bash;"

说白了，就是一下起5个终端，每个终端各司其职，分别是：
（1）启动 roscore
（2）启动 ur3_bringup.launch

<?xml version="1.0"?>
<launch><arg name="debug" default="false" doc="Debug flag that will get passed on to ur_common.launch"/><arg name="robot_ip" doc="IP address by which the robot can be reached."/><arg name="reverse_port" default="50001" doc="Port that will be opened by the driver to allow direct communication between the driver and the robot controller."/><arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/><arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/><arg name="controllers" default="joint_state_controller scaled_pos_joint_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/><arg name="stopped_controllers" default="pos_joint_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/><arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur3_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/><arg name="robot_description_file" default="$(find ur_description)/launch/ur3_upload.launch" doc="Robot description launch file."/><arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/><arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/><arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/><include file="$(find ur_robot_driver)/launch/ur_common.launch" pass_all_args="true"><arg name="use_tool_communication" value="false"/></include>
</launch>

（3）启动 ur3_moveit_planning_execution.launch
（4）启动 moveit_rviz.launch
（5）启动demo puncture_demo
这样，我们就实现了一键启动UR的走点demo。

2 把机器人安在架子上

已完成的工作中，在rviz界面中只有孤零零的一个机器人，而我们希望构建一个双臂机器人系统，这就至少需要两个机械臂和一个连接这两个机械臂的身体。

我们首先来解决这个“身体”的问题，就是把机器人安在架子上。

根据开发经验，ROS开发的机器人，都会把模型存在一个名为 xxx_description 的文件夹中。
我们在 ~\ur_ws\src\fmauch_universal_robot\ur_description\urdf 路径下发现与UR3机器人模型有关的urdf文件以xacro形式编写，具体有三个，如下：

逐一查看发现，发现第一个文件即为ur3机器人本体，是封装好的，一般不用修改；第二个和第三个文件是在包含机器人本体的基础上引入了“world ”这一link，而区别在于在启动 ur3_bringup.launch 文件中如果limited:=true则调用第二个文件，反之则调用第三个。

ur3_bringup.launch文件中有一行调用了 ur3_upload.launch文件，ur3_upload.launch 文件决定调用哪个xacro文件，具体如下：

ur3_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_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>

既然第二个和第三个文件是在包含机器人本体的基础上引入了“world ”这一link，我们的工作可以从这里入手，通过加入2个link和joint的方式，并改变原有base_link的连接，来生成一个“架子“。
ur3_joint_limited_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" /><!-- arm --><xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml"/><xacro:ur3_robot prefix="" joint_limited="true"shoulder_pan_lower_limit="${-pi}" shoulder_pan_upper_limit="${pi}"shoulder_lift_lower_limit="${-pi}" shoulder_lift_upper_limit="${pi}"elbow_joint_lower_limit="${-pi}" elbow_joint_upper_limit="${pi}"wrist_1_lower_limit="${-pi}" wrist_1_upper_limit="${pi}"wrist_2_lower_limit="${-pi}" wrist_2_upper_limit="${pi}"wrist_3_lower_limit="${-pi}" wrist_3_upper_limit="${pi}"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></robot>

我们尝试在ur3_joint_limited_robot.urdf.xacro与ur3_robot.urdf.xacro后半部分改写成如下代码：（link name="world"之前的内容未做任何改动。）

<?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" /><!-- arm --><xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml"/><xacro:ur3_robot prefix="" joint_limited="true"shoulder_pan_lower_limit="${-pi}" shoulder_pan_upper_limit="${pi}"shoulder_lift_lower_limit="${-pi}" shoulder_lift_upper_limit="${pi}"elbow_joint_lower_limit="${-pi}" elbow_joint_upper_limit="${pi}"wrist_1_lower_limit="${-pi}" wrist_1_upper_limit="${pi}"wrist_2_lower_limit="${-pi}" wrist_2_upper_limit="${pi}"wrist_3_lower_limit="${-pi}" wrist_3_upper_limit="${pi}"transmission_hw_interface="$(arg transmission_hw_interface)"kinematics_file="${load_yaml('$(arg kinematics_config)')}"/><link name="world" /><joint name="semaphore_robot_base_joint" type="fixed"><parent link ="world" /><child link = "semaphore_robot_base_link" /><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></joint><link name = "semaphore_robot_base_link" type="fixed"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="1" radius="0.05"/></geometry><material name="yellow"><color rgba="1 0.4 0 0.5"/></material></visual></link><joint name="semaphore_transverse_joint" type="fixed"><parent link ="semaphore_robot_base_link" /><child link = "semaphore_transverse_link" /><origin xyz="0.2 0.0 0.5" rpy="1.57 0.0 0.0" /></joint><link name = "semaphore_transverse_link" type="fixed"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="0.2" radius="0.06"/></geometry><material name="yellow"><color rgba="1 0.4 0 0.5"/></material></visual></link><joint name="left_world_joint" type="fixed"><parent link="semaphore_transverse_link" /><child link = "base_link" /><origin xyz="0 0 0.1" rpy="0 0.0 1.57" /></joint></robot>

这样，一个UR3机械臂就被固定在架子上了。

那么，另一条机械臂怎么办呢？

3 重新“造”一个UR3

在这里要先特殊注明一下：
由于我们实验室条件有限，只有一台UR3和一台UR3e，所以搭建的双臂机器人并不完全“对称”，但配置双臂的方法应该都是类似的。

也就是说，之前的工作完成了UR3与架子的组合，接下来我们要添加一个另一个机械臂（可以是UR3，但我们这里用UR3e了）。

在ur3_robot.urdf.xacro文件的基础上，我新建了ur3+ur3e_robot.urdf.xacro文件。

ur3+ur3e_robot.urdf.xacro：

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro"name="ur3_ur3e_robot" ><!--    <xacro:property name="pi" value="3.1415926535897931"/>    --><xacro:arg name="prefix_ur3" default="ur3_"/><xacro:arg name="prefix_ur3e" default="ur3e_"/><!-- do not know  --><xacro:arg name="transmission_hw_interface_ur3" default="hardware_interface/PositionJointInterface"/><xacro:arg name="transmission_hw_interface_ur3e" 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" /><!-- ur3e --><xacro:include filename="$(find ur_e_description)/urdf/ur3e.urdf.xacro" /><!-- arm --><xacro:arg name="kinematics_config_ur3" default="$(find ur_description)/config/ur3_default.yaml"/><xacro:arg name="kinematics_config_ur3e" default="$(find ur_e_description)/config/ur3e_default.yaml"/><!-- arm right--><xacro:ur3_robot prefix="$(arg prefix_ur3)" joint_limited="false"transmission_hw_interface="$(arg transmission_hw_interface_ur3)"kinematics_file="${load_yaml('$(arg kinematics_config_ur3)')}"/><!-- arm left--><xacro:ur3e_robot prefix="$(arg prefix_ur3e)" joint_limited="false"transmission_hw_interface="$(arg transmission_hw_interface_ur3e)"kinematics_file="${load_yaml('$(arg kinematics_config_ur3e)')}"/><link name="world" /><joint name="semaphore_robot_base_joint" type="fixed"><parent link ="world" /><child link = "semaphore_robot_base_link" /><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></joint><link name = "semaphore_robot_base_link" type="fixed"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="1" radius="0.075"/><!--    <mesh filename="package://ur_description/meshes/DH-UR-ASSY.stl"/> --></geometry><material name="yellow"><color rgba="1 0.4 0 0.5"/></material></visual></link><joint name="semaphore_transverse_joint" type="fixed"><parent link ="semaphore_robot_base_link" /><child link = "semaphore_transverse_link" /><origin xyz="0.22 0.0 0.5" rpy="1.57 0.0 0.0" /></joint><link name = "semaphore_transverse_link" type="fixed"><visual><origin xyz=" 0 0 0" rpy="0 0 0" /><geometry><cylinder length="0.2" radius="0.075"/></geometry><material name="yellow"><color rgba="1 0.4 0 0.5"/></material></visual></link><joint name="right_world_joint" type="fixed"><parent link="semaphore_transverse_link" /><child link = "$(arg prefix_ur3)base_link" /><origin xyz="0 0 0.1" rpy="0 0.0 -1.57" /></joint><joint name="left_world_joint" type="fixed"><parent link="semaphore_transverse_link" /><child link = "$(arg prefix_ur3e)base_link" /><origin xyz="0 0 -0.1" rpy="0 3.14 -1.57" /></joint></robot>

通过对比可以发现，这里引入两个机器人模型，分别是来自 ur_description/urdf 的 ur3.urdf.xacro，以及来自ur_e_description/urdf 的 ur3e.urdf.xacro，还有一些列对应的config文件。这些都是UR公司封装好的，我们直接使用就好。（如果你是两个UR3，直接调用两个ur3.urdf.xacro，而起不同的名字就好，这非常关键，因为机器人如果和模型不同步的话机器人是无法被控制的。）

同理，我们也可以新建ur3+ur3e_joint_limited_robot.urdf.xacro 文件，把约束加上，我这里就不赘述了。

对应的启动文件我也进行了新建：
ur3+ur3e_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_ur3" default="hardware_interface/PositionJointInterface" /><arg name="transmission_hw_interface_ur3e" default="hardware_interface/PositionJointInterface" /><arg name="kinematics_config_ur3" default="$(find ur_description)/config/ur3_default.yaml"/><arg name="kinematics_config_ur3e" default="$(find ur_e_description)/config/ur3e_default.yaml"/><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+ur3e_robot.urdf.xacro'transmission_hw_interface_ur3:=$(arg transmission_hw_interface_ur3)transmission_hw_interface_ur3e:=$(arg transmission_hw_interface_ur3e)kinematics_config_ur3:=$(arg kinematics_config_ur3)kinematics_config_ur3e:=$(arg kinematics_config_ur3e)" /><param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur3+ur3e_joint_limited_robot.urdf.xacro'transmission_hw_interface_ur3:=$(arg transmission_hw_interface_ur3)transmission_hw_interface_ur3e:=$(arg transmission_hw_interface_ur3e)kinematics_config_ur3:=$(arg kinematics_config_ur3)kinematics_config_ur3e:=$(arg kinematics_config_ur3e)" /></launch>

至此，模型部分的操作完成，我们共补充了3个全新文件在相应目录，具体如下：

（补充）将文件ur3+ur3e_upload.launch
放置在目录：~/ur_ws/src/fmauch_universal_robot/ur_description/launch 下
（补充）将文件ur3+ur3e_joint_limited_robot.urdf.xacro
放置在目录：~/ur_ws/src/fmauch_universal_robot/ur_description/urdf 下
（这个没来得及改，参考ur3+ur3e_robot.urdf.xacro）
（补充）将文件ur3+ur3e_robot.urdf.xacro
放置在目录：~/ur_ws/src/fmauch_universal_robot/ur_description/urdf 下

现在运行ur3+ur3e_upload.launch 文件的话，应该可以加载出双臂机器人的模型（图后期补充）

然而模型只是开始，如何驱动两个机械臂还有很长的路要走。

X 最终功能包

先把最后完整的功能包放在这，已经测试完毕，教程慢慢更新。

(1)ur_ws(初代配置版本)

这个就是自己配的最基本UR驱动包。

(2)ur_ws(UR3+UR3e双臂机器人)

本ws在ur_ws基础上做了如下修改，以完成双臂机器人系统搭建：

part1:模型部分
（补充）将文件ur3+ur3e_upload.launch 放置在目录：~/ur_ws/src/fmauch_universal_robot/ur_description/launch 下
（补充）将文件ur3+ur3e_joint_limited_robot.urdf.xacro 放置在目录：~/ur_ws/src/fmauch_universal_robot/ur_description/urdf 下（这个没来得及改，参考ur3+ur3e_robot.urdf.xacro）
（补充）将文件ur3+ur3e_robot.urdf.xacro 放置在目录：~/ur_ws/src/fmauch_universal_robot/ur_description/urdf 下

part2:驱动部分
（补充）将文件ur3+ur3e_bringup.launch 放置在目录：~/ur_ws/src/Universal_Robots_ROS_Driver/ur_robot_driver/launch 下
（改写）ur_common.launch 目录：~/ur_ws/src/Universal_Robots_ROS_Driver/ur_robot_driver/launch （修改内容：删掉了模型启用，因为在ur3+ur3e_bringup.launch启动了模型）
（改写）ur3_controllers.yaml 目录：~/ur_ws/src/Universal_Robots_ROS_Driver/ur_robot_driver/config （修改内容：加了前缀ur3_）
（改写）ur3e_controllers.yaml 目录：~/ur_ws/src/Universal_Robots_ROS_Driver/ur_robot_driver/config （修改内容：加了前缀ur3e_）

part3:控制器部分
替换了整个 ur3_moveit_config
具体修改了 ~/ur_ws/src/fmauch_universal_robot/ur3_moveit_config/config 中的六个文件：

controllers.yaml
fake_controllers.yaml
joint_limits.yaml
kinematics.yaml
ompl_planning.yaml
ur3.srdf

（修改内容：我懒得新建一个类似名为ur3+ur3e_moveit_config 的文件了，因为里边要改一堆launch文件的名字，所以就直接把 ur3_moveit_config 作为双臂机器的moveit_config 文件了）

part4:demo与启动部分
将功能包 ur3_flag_demo 放在 ~/ur_ws/src下
将启动脚本文件 ur3+ur3e_setup.sh 放在 ~/ur_ws 下

未完待续……