UR机器人双臂开发实例

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 下

未完待续……