一、前言
有很多小伙伴问我怎么样控制机械臂完成整个过程，其实经过上一篇博文的探究，这个问题其实很容易就可以解决。把他们每个位姿连起来执行不过就行了吗，因为我们做这个的场景就是抓取——>放置——>投递，整个场景和动作很固定。 所以我们完全用不上所谓的路径规划，说实话，路径规划这玩意弄起来也不简单。所以这里就不去讨论这块问题。 后面有机会，等我去更加深入的自学一下，有机会可以交流！
二、机械臂的运动逻辑（直接上代码讲解，具体请看注释）
import rospy
import time
from dobot.srv import SetHOMECmd #调用回零话题
from dobot.srv import SetEndEffectorSuctionCup #调用末端回零话题
from dobot.srv import SetPTPCmd #调用机械臂点到点运动话题
from visual_grab.msg import ocr_posemsg #视觉话题（后面单独讲）
from dobot.msg import navigational_state, dobot_state #我自定义的消息
#相关标志量和全局变量初始化
grap_state = -1
deliver_state = -1
grap_flag = 0
box_index = -1
capture_pose_x = 0
capture_pose_y = 0
start_ocr_time = 0
spin_grap_x = -50
spincnt = 0
'''
-------------------------------------------------------------------------------------------------
下面就是各个坐标点的初始化
这里是观察点的坐标值，使用获取实时位姿的话题可以获取相应的坐标，
我的逻辑是当调用机械臂归零话题以后，然后就会来到这个点停止，然后进行观察识别
---------------------------------
'''
grap_up_x = 7
grap_up_y = -257
grap_up_z = 95
grap_up_r = -170
'''
-------------------------------------------------------------------------------------------------
抓取点，这里的坐标值，除了R值(末端执行器)和Z值固定，其它X,Y值是需要改变的，
因为它要同时识别裁剪计算出和观察点的相对误差，然后进行换算调整
注意，因为这里的观察点固定，所以我们很容易算出邮件距离画面中心点的差值，然后进行换算可以找到邮件的中心点抓取。
没有使用到计算机械臂的逆解，当然，我会在垃圾分类那个专栏进行简单讲解，这里图省事就可以直接计算
-------------------------------------------------------------------------------------------------
'''
grap_down_x = 0
grap_down_y = 0
grap_down_z = -11
grap_down_r = -170
'''
-------------------------------------------------------------------------------------------------
这里是移动到小车放置台的上方
-------------------------------------------------------------------------------------------------
'''
put_up_x = 17
put_up_y = 185
put_up_z = 67
put_up_r = 2
'''
-------------------------------------------------------------------------------------------------
这里是将邮件放下来的坐标
-------------------------------------------------------------------------------------------------
'''
put_down_x = 20
put_down_y = 191
put_down_z = -30
put_down_r = 2
'''
-------------------------------------------------------------------------------------------------
#这里是投递点的坐标
-------------------------------------------------------------------------------------------------
'''
deliver_up_x = 7
deliver_up_y = -319
deliver_up_z = 25
deliver_up_r = -170
'''
-------------------------------------------------------------------------------------------------
功能：机械臂初始化
参数：空
讲解：机械臂初始化，初始化两个话题，再发布机洗鼻归零话题，然后发布运动到坐标点的话题。注意程序运行到这里，
他是在基本相同的时间，将两条命令发出去，但是你要知道机械臂的运动是需要时间的，所以要注意控制，由于我写的
程序时那边运动完，会返回一个消息，再开启识别，这个机械臂模块会进行等待识别那边发过来的消息。
其实他的API函数有返回值，执行完那个话题，我们可以增加一个返回消息的却，再发下一条，会更好，可以自行测试！
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def init_arm():
rospy.wait_for_service('DobotServer/SetHOMECmd')
rospy.wait_for_service('DobotServer/SetPTPCmd')
#t0 = rospy.Duration(5, 0)
try:
client = rospy.ServiceProxy('DobotServer/SetHOMECmd', SetHOMECmd)
PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd', SetPTPCmd)
response = client()
#rospy.sleep(t0)
response = PTP_client(1, grap_up_x, grap_up_y, grap_up_z, grap_up_r) # houmian,shangfang
except rospy.ServiceException, e:
print "Service call failed: %s" % e
'''
-------------------------------------------------------------------------------------------------
功能：机械臂抓取
参数：空
讲解：这个功能函数就是实现抓取功能，完成抓取整个过程。大概是收到了识别完成的消息，开始从
识别点——>抓取点——>启动夹爪气泵——>回到识别点——>运动到放置点上方——>放下——>关闭气泵——>回到放置点上方，
这里使用了MOVL模式点到点模式，使用JUMP（测试有点不可控，经常超范围，可能是我的方法不对）
和MOVJ关节运动模式（麻烦一点），所以一下全都采用MOVL模式
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def arm_grap():
global capture_pose_x, capture_pose_y, grap_down_x, grap_down_y
rospy.wait_for_service('DobotServer/SetEndEffectorSuctionCup')
rospy.wait_for_service('DobotServer/SetPTPCmd')
t1 = rospy.Duration(1,0)
try:
end_client = rospy.ServiceProxy('DobotServer/SetEndEffectorSuctionCup', SetEndEffectorSuctionCup)
PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd', SetPTPCmd)
response = PTP_client(1, grap_up_x, grap_up_y, grap_up_z, grap_up_r) # houmian,shangfang
response = PTP_client(1, grap_down_x, grap_down_y, grap_down_z, grap_down_r) # houmian,fangxia
response = end_client(1, 1, True)
rospy.sleep(t1)
response = PTP_client(1, grap_up_x, grap_up_y, grap_up_z, grap_up_r) # houmian,shangfang
response = PTP_client(1, put_up_x, put_up_y, put_up_z, put_up_r) # qianmian,shangfang
response = PTP_client(1, put_down_x, put_down_y, put_down_z, put_down_r) # qianmian,fangxia
response = end_client(0, 0, True)
rospy.sleep(t1)
response = PTP_client(1, put_up_x, put_up_y, put_up_z, put_up_r) # qianmian,shangfang
#print("grap_down_x", grap_down_x, "grap_down_y", grap_down_y)
except rospy.ServiceException, e:
print"Service call failed: %s" % e
'''
-------------------------------------------------------------------------------------------------
功能：机械臂投递
参数：空
讲解：这个功能函数就是实现投递功能，逻辑类似
放置点上——>下降抓取邮件——>启动气泵——>回到放置点上方——>移动到放置点——>关闭气泵——>回到观察点
这样就与抓取函数实现了闭环
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def arm_deliver():
rospy.wait_for_service('DobotServer/SetEndEffectorSuctionCup')
rospy.wait_for_service('DobotServer/SetPTPCmd')
t2 = rospy.Duration(1, 0)
try:
end_client = rospy.ServiceProxy('DobotServer/SetEndEffectorSuctionCup', SetEndEffectorSuctionCup)
PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd', SetPTPCmd)
response = PTP_client(1, put_down_x, put_down_y, put_down_z, put_down_r) # qianmian,fangxia
response = end_client(1, 1, True)
rospy.sleep(t2)
response = PTP_client(1, put_up_x, put_up_y, put_up_z, put_up_r) # qianmian,shangfang
response = PTP_client(1, deliver_up_x, deliver_up_y, deliver_up_z, deliver_up_r)
response = end_client(0, 0, True)
response = PTP_client(1, 7, grap_up_y, grap_up_z, grap_up_r) # houmian,shangfang
rospy.sleep(t2)
except rospy.ServiceException, e:
print"Service call failed: %s" % e
'''
-------------------------------------------------------------------------------------------------
功能：导航状态回调函数
参数：导航消息
讲解：导航成功后，到抓取点，启动是被、抓取，到投递点后，启动投递
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def navigational_statecallback(data):
global grap_state, deliver_state, start_ocr_time
try:
grap_state = data.nav_grap_flag
deliver_state = data.nav_deliver_flag
if deliver_state == 0:
start_ocr_time = rospy.get_time()
return grap_state, deliver_state, start_ocr_time
else:
return grap_state, deliver_state
except:
print("receive dobot_state error")
'''
-------------------------------------------------------------------------------------------------
功能：文字识别回调函数
参数：文字识别消息
讲解：
回调的消息包括（识别到的省份信息和物体的相对偏差值），身份会已知省份对比，确认投递点；
相对误差值，在观察点的坐标值上做修改，同时判断范围，避免超出机械臂运动范围
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def ocr_posCallback(data):
global text, grap_flag, box_index, capture_pose_x, capture_pose_y, \
grap_down_x, grap_down_y
deliver_point = ['shandongsheng', 'hunansheng', 'fujiansheng', 'sichuansheng', 'henansheng',
'yunnansheng','zhejiangsheng','guangxi', 'hubeisheng','guangdongsheng']
try:
capture_pose_x = data.x * -1000
capture_pose_y = data.y * -1000
text = data.text
box_index = deliver_point.index(text)
grap_flag = 1
grap_down_x = grap_up_x + capture_pose_x
grap_down_y = grap_up_y + capture_pose_y
rospy.loginfo("text: %s", text)
rospy.loginfo("grap_flag:%d", grap_flag)
print("capture_pose_x:", capture_pose_x, "capture_pose_y:", capture_pose_y)
if grap_down_y > -190:
grap_down_y = -192
rospy.loginfo("grap_down_y value too large!")
if grap_down_y < -310:
grap_down_y = -308
rospy.loginfo("grap_down_y value too small")
#print("grap_down_x", grap_down_x, "grap_down_y", grap_down_y)
return box_index, grap_flag, grap_down_x, grap_down_y
except:
print("receive ocr_pose error")
'''
-------------------------------------------------------------------------------------------------
功能：main函数
参数：空
讲解：
这里就不列代码了；
逻辑大概就行，机械臂最先归零移动到观察点；向文字识别话题发送消息，然后等待回信，那边识别完成向这边发送抓取消息；
抓取完成后向导航话题发送消息，等到那边导航完成，这边就运行投递程序，投递完成，再向导航发送消息，回到分拣台；
这样就是一项工作的闭环。
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
三、总结
其实这里的机械臂控制逻辑上是通俗易懂的，也不用去考虑机械臂的路径规划和机械臂本身的控制。 因为它都做好了函数封装，调用API就可以直接用。有兴趣的可以参考它的机械臂话题的源文件。 可以学习机械臂的控制！
后面的建图和文字识别，图像处理等工作才是更加繁琐有难度。我也会尽快继续整理与大家分享！
评论（0）
您还未登录，请登录后发表或查看评论