第五天：了解越疆dobot机械臂，使用python，编写通过ros服务控制机械臂运动程序

一、了解越疆dobot机械臂

详细信息可以查看用户手册和API开发手册，以及其它官方提供资料，这里我只列出我在开发过程中遇到的问题，以及需要了解的知识点。

（1）坐标系

我们可以看到这里涉及的坐标系主要是两种，一种是关节坐标系，一直是笛卡尔坐标系。我们可以通过命令获取到这两组坐标系的值，后面详细说明。

#这里是手册里面针对两种坐标系
 关节坐标系：以各运动关节为参照确定的坐标系。
	 若Dobot Magician未安装末端套件，则包含三个关节：J1、J2、J3，且均为旋转关节，逆时针为正。
	 若Dobot Magician安装带舵机的末端套件，如吸盘和夹爪套件，则包含四个关节：J1、J2、J3和J4，均为旋转关节，逆时针为正。
 笛卡尔坐标系：以机械臂底座为参照确定的坐标系。
	 坐标系原点为大臂、小臂以及底座三个电机三轴的交点。
	 X轴方向垂直于固定底座向前。
	 Y轴方向垂直于固定底座向左。
	 Z轴符合右手定则，垂直向上为正方向。
	 R轴为末端舵机中心相对于原点的姿态，逆时针为正。当安装了带舵机的末端套件时，才存在R轴。R轴坐标为J1轴和J4轴坐标之和。

（2）运动模式

本款机械臂主要有括点动模式、点位模式（PTP）、圆弧运动模式（ARC）。PTP和ARC可总称为存点再现运动模式。 三种运动模式，
（1）点动模式即示教时移动机械臂的坐标系，使机械臂移动至某一点。Dobot Magician的坐标系可分为笛卡尔坐标系和关节坐标系，用户可单击笛卡尔坐标系按钮或关节坐标系按钮移动机械臂。

 笛卡尔坐标系模式：
	 单击“X+”、“X-”，机械臂会沿X轴正负方向移动。
	 单击“Y+”、“Y-”，机械臂会沿Y轴正负方向移动。
	 单击“Z+”、“Z-”，机械臂会沿Z轴正负方向移动。
	 单击“R+”、“R-”，机械臂末端姿态会沿R轴正负方向旋转。
 关节坐标系模式：
	 单击“J1+”、“J1-”，可控制底座电机正负方向旋转。
	 单击“J2+”、“J2-”，可控制大臂电机正负方向旋转。
	 单击“J3+”、“J3-”，可控制小臂电机正负方向移动。
	 单击“J4+”、“J4-”，可控制末端舵机正负方向旋转。

（2）点位模式即实现点到点运动，Dobot Magician的点位模式包括MOVJ、MOVL以及JUMP三种运动模式。不同的运动模式，示教后存点回放的运动轨迹不同。

 MOVJ：关节运动，由A点运动到B点，各个关节从A点对应的关节角运行至B点对应的关节角。关节运动过程中，各个关节轴的运行时间需一致，且同时到达终点，如图 3.6所示。
 MOVL：直线运动，A点到B点的路径为直线，如图 3.6所示。
 JUMP：门型轨迹，A点到B点以MOVJ运动模式移动，如图 3.7所示。
		以MOVJ运动模式上升到一定高度（Height）。
		以MOVJ运动模式平移到B点上方的高度处。
		以MOVJ运动模式下降到B点所在位置。

（3）圆弧模式即示教后存点回放的运动轨迹为圆弧。圆弧轨迹是空间的圆弧，由当前点、圆弧上任一点和圆弧结束点三点共同确定。圆弧总是从起点经过圆弧上任一点再到结束点，如图 3.8所示。

几种方式的运用场景：

API接口

官方提供的接口函数有几百个，我们可以根据需要使用，其实我们这里也就常用的有几个接口而已，可以根据需要去使用，下面我们开始正式进入机械臂控制编程。

二、使用python，编写通过ros服务控制机械臂运动程序

注意后面运行python脚本的时候，要先开启机械臂的服务，直接使用一下指令即可

roslaunch dobot DobotServer.launch

（1）设置机械臂的归零状态：

SetHOMECmd.py（调用SetHOMECmd服务，编写client，这里函数接口已经通过服务方式进行封装好了我们直接可以使用）

#!/usr/bin/env python
#-*- conding:utf-8 -*-

import rospy
from dobot.srv import SetHOMECmd

if __name__ == "__main__":
    rospy.init_node('testDobot')
    rospy.wait_for_service('DobotServer/SetHOMECmd')
    try:
        client = rospy.ServiceProxy('DobotServer/SetHOMECmd',SetHOMECmd)
        response = client()
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

这是我在API里找到的函数原型，我们直接使用系统的回零接口即可。

（2）设置机械臂末端吸取器的状态

SetEndEffectorSuctionCup.py

#!/usr/bin/env python
#-*- conding:utf-8 -*-

import rospy
from dobot.srv import SetEndEffectorSuctionCup

if __name__ == "__main__":
    rospy.init_node('testDobot')
    rospy.wait_for_service('DobotServer/SetEndEffectorSuctionCup')
    try:
        client = rospy.ServiceProxy('DobotServer/SetEndEffectorSuctionCup',SetEndEffectorSuctionCup)
        response = client(1,1,True)
        #response = client(0,0,True)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

这里需要传递两个参数，分别表示气泵是否使能以及吹吸气状态。

（3）执行 PTP 指令

设置机械臂运动的目的坐标，这里一共需要传递五个参数，通过官方的API接口可以看到每一个参数的意思。

#!/usr/bin/env python
#-*- conding:utf-8 -*-

import rospy
from dobot.srv import SetPTPCmd

if __name__ == "__main__":
    rospy.init_node('testDobot')
    rospy.wait_for_service('DobotServer/SetPTPCmd')
    try:
        client = rospy.ServiceProxy('DobotServer/SetPTPCmd',SetPTPCmd)
        response = client(1,-2.2,258,121,-10)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

可以看到，第一个代表PTP模式的设置，后面四个参数主要是描述坐标系的相关量。我们这个工程主要使用模式1，MOVJ模式和笛卡尔坐标系了，这就用到了博文开头提到的坐标系和模式介绍。

（4）整合相关接口，实现完整的一个实现流程。

首先，我们先理一下我们的思路。
我们的流程就是：建图——>导航——>识别——>抓取——>移动到目标点——>投递——>（回到第三步，反复识别抓取投递）。
今天这里研究的是抓取。我们通过识别邮件的信息和位置，然后通过机械臂去抓取。我们车导航到分拣台，但是邮件的位置是一个不知道的位置，我们首先通过摄像头去进行一个图像分割，检测到邮件出现在我们摄像头里面，就可以算出邮件的中心点相对于摄像头图像中心点的差值，我们分别返回相差的X,Y值，然后控制机械臂运动去抓取。
我们首先可以知道现在的机械臂末端位置的X,Y,Z坐标（我们使用的就是笛卡尔坐标系，可以获得当前坐标点的一个坐标）然后我们可以通过X,Y分别加上我们放回的值然后移动机械臂去抓他是吧，听着确实也好懂。
当时，我们得考虑一下临界值，就是实际上Y,Z的值分别是有第二和第三连接角进行控制的，至于X只是底盘的一个值可以不用多讨论。而我们的Y变化是和Z有关的，所以我们需要测出这个范围值。

其实在思考这里的时候，我首先想到的不是上面这个样子直接对左边进行的一个加减。我之前做垃圾桶的时候进行的分拣，那里实际上用到了计算机械臂的逆解，当时是个六自由度的机械臂，这里是四自由度的一个机械臂，相对要简单一些，不过这里四自由度逆解算法手上没有现成的，也就没有使用算逆解的方式。后面有机会在补充试试。而且这里实际上也不要那么麻烦。
我们只要在固定的高度Z测试一下范围即可。因为我们首先是在一个固定的定去识别邮件，首先记录这个点的高度，然后我们需要降到桌面去吸取邮件，这是第二个高度，我们只需要测定这两个高度Z的前提下，Y的移动范围就可以了！
下面是我写的一个简单的测试程序：

#!/usr/bin/env python
#-*- conding:utf-8 -*-

import rospy
import time
from dobot.srv import SetPTPCmd

if __name__ == "__main__":
    new_y = -250
    x = 7
    y = -17
    r = -170 
    rospy.init_node('PTP_Dobot')
    rospy.wait_for_service('DobotServer/SetPTPCmd')
    try:
        PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd',SetPTPCmd)
	while True:
	    new_y += 2
	    response = PTP_client(1, x, new_y, z, r)
	    print("publish ok,new_y is:", new_y)
	    time.sleep(0.5)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

这里实在Z值固定的范围下对x,y进行测试，由于x的范围很大，不会超出其范围，所以只对y进行测试即可。
这里最终机械臂会进行报错，由于是软件报错，所以可以直接使用警报清除命令：rosservice call /DobotServer/ClearAllAlarmsState就可以恢复。

这是我们测试的几组值，其实可以看到X一定的情况下，Y的范围取决于Z值高的，也不难想，机械臂越高，那么Y轴可移动空间越小，所以建议以高的为限制。

当Y=-310时，Z的范围是   -90~100
当Y=-300时, Z的范围是  -100 ~120
当Y=-290时，Z的范围是   -110~135
注意，这里下方有气泵阻挡，所以Y不能大于-235
当Y=-235时，Z的范围是   -140~165


Z=85  Y的范围是    -310 ~  -190
Z=55  Y的范围是    -320 ~  -175
Z= 95  Y的范围是	  -310 ~-190
Z= -17 Y的范围是	  -328 ~ -174

今天大概就到这里！