1.机械臂UR的控制包括两种，一种是通过终端控制 比较笨重不方便，用于测试使用，另一种是通过ros例程包，订阅位置信息控制机械臂的移动，moveit自动规划机械臂的路径，并躲过设置好的盒子（box）

手部RG2机械手的控制一种是通过终端的程序，在大终端中创建空程序 、结构、RG2、命令、带宽。

2.步骤

#roslaunch ur_control ur10_control.launch robot_ip:=192.168.2.24

将机械臂与本地路由器的地址相同连接起来

#roslaunch ur10_rg2_moveit_config demo.launch

启动rviz 在里面利用moveit进行仿真

#rosrun ur10_rg2_moveit_config moveit_controller.py

在这里添加限制box,此处是初始设置 手动测量位置后加了底座box（模仿麦轮），加了三面墙壁（模仿电梯）。

#rosrun ur10_rg2_moveit_config new_moveit_controller.py

在new_moveit_controller.py为总的控制程序，包含机械臂和机械手，其中在def go_to_pose_goal(self):函数中定义运动的位置。设计包含两种场景，一种是用于机械臂抓取物体的，另一种是在电梯中按按钮。

3.得到当前机械手位置的程序

current_pose = self.group.get_current_pose().pose
print self.group.get_current_pose()

4.电梯按钮的控制机械臂手程序，共设置button0.1.2三步 分别是到达电梯按钮正前方、合住机械手呈一条线、向正前方伸前点按钮、回来。

print "============ Press `Enter` to plan button0"
    raw_input()
    # rospy.sleep(0.5)
    q = tf.transformations.quaternion_from_euler(math.radians(-90.0), 0.0, 0.0)
    pose_goal.orientation.x = q[0]
    pose_goal.orientation.y = q[1]
    pose_goal.orientation.z = q[2]
    pose_goal.orientation.w = q[3]
    pose_goal.position.x = 0.6
    pose_goal.position.y = -0.65
    pose_goal.position.z = 1.40
    group.set_pose_target(pose_goal)
    plan00=group.plan()
    print "============ Press `Enter` to go button0"
    raw_input()
    # rospy.sleep(0.5)
    plan = group.go(wait=True)

合成一条线  
print "============ Press `Enter` to rg2"
    raw_input()
    # rospy.sleep(0.5)
    self.request.target_width.data=0.0
    resp1=self.val(self.request)

此处为实际已经测量好位置，之后根据深度相机的位置以及标定的柱状物体位置做抓取动作。

5.抓取百事可乐罐程序

 print "============ Press `Enter` to plan pepsi0"
    raw_input()
    pose_goal.orientation.x = -0.567
    pose_goal.orientation.y = -0.405
    pose_goal.orientation.z = 0.414
    pose_goal.orientation.w = 0.586
    pose_goal.position.x = 0.90
    pose_goal.position.y = -0.015
    pose_goal.position.z = 0.17
    group.set_pose_target(pose_goal)
    plan00=group.plan()
    print "============ Press `Enter` to go pepsi0"
    raw_input()
    # plan = group.go(wait=True)

抓取分为三个动作 到可乐罐侧边、前伸抓取、回到正中位置（稳定正中央） 之后就可以通过控制麦轮移动直接送到人的面前。

6.回到初始位置

在总控制端 为机器人设置。。、移动、回零、自动 即可完成手动归零动作（直立动作）。

点击左上角、文件、退出 可以退出这一步。

7.BUG部分

在将官方例程直接拷到本地src后会出现问题：

error:/home/yyang/catkin_ws/build/universal_robot/ur_driver/setup_custom_pythonpath.sh: 5: exec: /home/yyang/catkin_ws/src/universal_robot/ur_driver/cfg/URDriver.cfg: Permission denied 原因在于对于URDriver.cfg没有执行权限，而在跑例程其实是用到的，这就需要修改执行权限，当然用chmod 777这种给全部权限的尽量慎用。

#chmod +x /home/yyang/catkin_ws/src/universal_robot/ur_driver/cfg/URDriver.cfg

error:/home/yyang/catkin_ws/src/ur10_rg2_ros/ur_modern_driver/include/ur_modern_driver/ur_driver.h:37:28: fatal error: ur_control/RG2.h: No such file or directory 这个错误是因为这个文件是在跑通程序之后才能生成这个程序，而第一次跑缺少这个怎么可能会跑通，所以只能通过将他人已经跑通过类似程序后生成的文件ur_control里面的文件直接拷贝进来

solve:直接拷贝他人这个包文件到该目录文件夹下面,这几个头文件我已经放到了百度云网盘里面了(链接：https://pan.baidu.com/s/1i2N0lXDYN5T_ydcW1fojhA 密码：jcie)

8.Rviz里面的设置

在启用Moveit和Rviz后要更改一些参数

#roslaunch ur10_rg2_moveit_config demo.launch

MotionPanning-Planning Scene Topic 话题选择为为/move_group/monitored_planning_scene

在Context中的OMPL 选择RRTConnectkConfigDefault(RRT第一个)随后就可以自由拖动来进行实际测试了（作者此处已经加了限制故直接显示了出来）

9.接下来添加限制

#rosrun ur10_rg2_moveit_config moveit_controller.py

这其中包括的限制有：

9.1.电梯（即上面已经添加的三个顶部两侧面）

电梯的盒子（3维-哪一维是薄片就将值调为无限小）后续会调整，故加在单独的moveit_controller.py以便后续更改

box_pose = geometry_msgs.msg.PoseStamped()
 
box_pose.header.frame_id = "base_link"
 
#box_pose.pose.orientation.w = 1.0
 
box_pose.pose.position.x = 0.0
 
box_pose.pose.position.y = 0.75
 
box_pose.pose.position.z = 0.536943
 
q = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
 
box_pose.pose.orientation.x = q[0]
 
box_pose.pose.orientation.y = q[1]
 
box_pose.pose.orientation.z = q[2]
 
box_pose.pose.orientation.w = q[3]
 
box_name = "box1"
 
scene.add_box(box_name, box_pose, size=(2, 0.01, 2))

9.2.大地

基座的盒子因为基本固定不变，故写在urdf文件arm_extras_ur10.urdf.xacro中定义基座、地面以及手臂。其中

ground_base1即为设置的大地的模型（盒子 厚度为0.1）

<link name="${prefix}ground_base1">
 
<visual>
 
<geometry>
 
<box size="4 4 0.01"/>
 
</geometry>
 
<origin xyz="0 0 0.005" rpy="0 0 0"/>
 
<material name="white">
 
<color rgba="${150/255} ${150/255} ${150/255} 1.0"/>
 
</material>
 
</visual>
 
<collision>
 
<geometry>
 
<box size="4 4 0.01"/>
 
</geometry>
 
<origin xyz=" 0 0 0.005" rpy="0 0 0"/>
 
</collision>
 
<!--xacro:default_inertial mass="20"/-->
 
<inertial>
 
<origin xyz="0 0 0" rpy="0 0 0"/>
 
<mass value="100"/>
 
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
 
</inertial>
 
</link>

9.3.手臂：

第一关节

<!-- ROS base_link to UR 'Base' Coordinates transform -->
 
<joint name="${prefix}base_pillar_joint" type="fixed">
 
<origin xyz="0 0 0.01" rpy="0 0 0"/>
 
<parent link="${prefix}ground_base1"/>
 
<child link="${prefix}base_pillar"/>
 
</joint>

底座：

<link name="${prefix}base_pillar">
 
<visual>
 
<geometry>
 
<box size="1 0.7 0.473075191"/>
 
</geometry>
 
<origin xyz="0 0 0.236537596" rpy="0 0 0"/>
 
<material name="blue">
 
<color rgba="0 0 0.8 1.0"/>
 
</material>
 
</visual>
 
<collision>
 
<geometry>
 
<box size="1 0.7 0.473075191"/>
 
</geometry>
 
<origin xyz=" 0 0 0.236537596" rpy="0 0 0"/>
 
</collision>
 
 
<inertial>
 
<origin xyz="0 0 0" rpy="0 0 0"/>
 
<mass value="50"/>
 
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
 
</inertial>
 
</link>

在另一个urdf文件rg2.urdf.xacro则定义了机械手的部分。以上两个程序都是根据官方例程修改，根据实际测量的值添加 后期需要再继续调整。

<!-- xacro for rg2 mounted on the manipulator -->
<robot name="rg2">
<xacro:macro name="rg2" params="prefix">
<!-- rg2 joint -->
<joint name="rg2_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="${prefix}gripper_extender_link"/>
<child link="${prefix}rg2_body_link"/>
</joint>
<link name="${prefix}rg2_body_link">
<visual>
<origin rpy="0 ${M_PI/2} 0" xyz="0.051 0.0085 0.0015"/>
<geometry>
<mesh filename="package://robot_descriptions/meshes/rg2_gripper/dae_rg2_.dae" scale="0.001 0.001 0.001"/>
</geometry>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material></visual><collision>
<origin rpy="0 ${M_PI/2} 0" xyz="0.051 0.0085 0.0015"/>
<geometry><mesh filename="package://robot_descriptions/meshes/rg2_gripper/rg2_.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="1"/>
</inertial>
</link>
<joint name="rg2_eef_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.195 0.0 0.0"/>
<parent link="rg2_body_link"/>
<child link="rg2_eef_link"/>
</joint><link name="rg2_eef_link"/>
</xacro:macro>
</robot>

在MotionPlanning里面的SceneGeometry-Scene Alpha中可以调节限制盒子（box）的透明度。

这样在添加了限制以后运行程序#rosrun ur10_rg2_moveit_config moveit_controller.py就会添加周围的电梯限制（底座和大地一直不变故直接写在总程序中不修改），在超出限制后的机械臂部分会变红，并且moveit运动规划的时候会自动避开限制。在今后进行开发机器人自动按电梯按钮功能的时候继续调整。

10.其他

所以总的程序执行步骤为：

#roslaunch ur_control ur10_control.launch robot_ip:=192.168.2.24
//硬件连接
 
#roslaunch ur10_rg2_moveit_config demo.launch
//启动moveit和rviz
#rosrun ur10_rg2_moveit_config moveit_controller.py
//加限制 之后的电梯备用
 
#rosrun ur10_rg2_moveit_config new_moveit_controller.py
//控制程序 总的控制程序

完成机械臂的熟悉、控制，在今后将之前深度相机RealsenseZR300得到的位置信息与机械臂程序结合就可以控制机械臂抓取，并在以后逐步实现机械臂的自动按电梯按钮功能设计。

--------------------------------------------------更新2019.7.19--------------------------------------------------------------

因为最近半年都在准备发论文以及准备雅思，所以都没有登csdn回复。希望8.24雅思顺利，继续努力把。之后准备好雅思及申请好后，会总结一下今年前两个半月paper做的3D 物体识别、6D位姿估计以及重叠物体的检测等内容。

另外，有很多人通过邮箱联系到了我我也给出了我的解答。如有问题请发邮件到： 1701210376@pku.edu.cn 最近依然不会回复csdn。

-------------------------------------------------------------------------------2020.3.2更新-------------------------------------------------------------

有问题可以联系上面的邮箱。最终的文章代码链接以及原文、实现步骤已经在系列终结篇给出。