第二十六届中国机器人及人工智能大赛CRAIC决赛小型桌面级-立体仓储代码分享

国二奖，精度还有不少问题，没有视觉模块，单线程，比起其他大佬写的确实还有欠缺，继续加油吧

from wlkata_mirobot import WlkataMirobot
from wlkata_mirobot_o import WlkataMirobot_o
import time
import cv2   
import numpy as np  
import pyzbar.pyzbar as pyzbar  
import numpy as np  
##############################################################################################################################################
#货叉
s1= [86.5,-11.77,7,-86.52,0,0,10.1,165.08,248.99,0,0,-0.02,0]
s2=[80.35,61.08,94.12,-80.36,0,0,28.07,165.1,57.08,0,0,-0.01,-1.53]
s3=[81.88,66.32,96.7,11.59,0,0,23.59,165.21,46.17,0,0,93.47,-1.77]
s4=[84.72,73.38,61.33,8.75,0,0,23.57,255.24,46.22,0,0,93.47,-1.77]
s6=[85.65,53.28,49.6,7.82,0,0,19.57,257.21,106.1,0,0,93.47,-1.77]
s7=[-12.18,40.25,67.95,105.66,0,0,193.56,-41.76,106.09,0,0,93.48,-1.77]
s11=[-12.95,38.33,71.47,106.43,0,0,181.56,-41.75,106.08,0,0,93.48,-1.77]
s12=[-27.83,64.72,79.35,121.32,0,0,183.48,-96.84,51.19,0,0,93.49,-1.77]
s13=[-27.83,69.68,81.3,121.32,0,0,183.47,-96.84,39.24,0,0,93.49,-1.77]
s14=[-32.47,70.43,77.4,125.97,0,0,183.45,-116.75,39.28,0,0,93.5,-1.77]
s15=[-32.47,70.43,77.4,125.97,0,0,183.45,-116.75,39.28,0,0,93.5,-1.77]
##############################################################################################################################################
m1a=[21.88,35.8,1.77,0,-37.57,-21.89,221.78,89.07,103.13,0,0,0,0]#拿A
m2a=[20.56,23.08,-26.5,0,3.41,-20.57,224.27,84.11,232.11,0,0,0,0]#抬起来到a的上方（设次点位是防止撞到其他货物）： 这个可以快一点

m3a=[13.73,31.94,8.48,0,-40.42,-23.39,221.9,54.21,100.02,0,0,-9.66,0]#拿B
m4a=[13.73,15.82,-3.28,0,-12.55,-23.39,221.9,54.21,189.46,0,0,-9.66,0]#抬起来到B的上方

m5a=[26.57,23.07,19.12,0,-42.19,-36.25,188.38,94.23,103.4,0,0,-9.68,0]#拿c
m6a=[26.57,5.83,3.53,0,-9.35,-36.25,188.38,94.23,202.42,0,0,-9.68,0]#抬起来到C的上方

m7a=[16.06,16.3,27.41,0,-43.7,-25.73,188.32,54.23,104.09,0,0,-9.66,0]#拿D
m8a=[16.06,2.43,22.47,0,-24.9,-25.73,188.31,54.22,157.61,0,0,-9.66,0]#抬起来到D的上方

z1=[-68.09,0.11,32.48,1.89,-37.43,18.2,69.66,-171.48,136.64,0,-5.08,-50.97,0]#正对操作台的左第一个位置（左1）
z01=[-66.5,-5.88,3.7,0,2.17,66.5,74.45,-171.24,236.55,0,0,0,0]#正对操作台的左第一个位置（左1）的上面

z2=[-80.34,0.49,30.88,4.22,-35.37,-90.29,32.82,-184.14,139.94,3.92,3.62,-172.83,-2.56]#正对操作台的左第二个位置（左2）
z02=[-80.56,-7.18,7.42,3.34,-14.56,-58.41,32.72,-188.44,229.96,13.98,-4.57,-139.96,0]#正对操作台的左第二个位置（左2）的上面

z3=[-93.74,-3.1,35.27,3.86,-38.62,-18.89,-10.48,-179.99,137.71,5.18,-4.98,-115.09,0]#正对操作台的左第三个位置（左3）
z03=[-95.79,-9.95,9.67,7.08,-12.62,-42.91,-15.78,-184.67,230.72,14,-4.5,-140.01,0]#正对操作台的左第三个位置（左3）的上面

z4=[-106.61,-8.73,40.89,-0.89,-46.84,-74.77,-49.73,-165.85,137.26,13.99,-4.55,178.64,0]#正对操作台的左第四个位置（左4）
z04=[-106.61,-17.25,22.44,-0.64,-19.88,-74.34,-49.95,-166.59,211.29,13.99,-4.54,178.64,0]#正对操作台的左第四个位置（左4）的上面
##################矫正用点位########################################
g1=[-58.72,-33.9,46.47,0,-12.57,58.73,72.03,-118.57,175.12,0,0,0,0]#用于物体A-左1
g2=[-18.49,-30.8,39.97,0,-9.17,8.86,136.95,-45.79,188.3,0,0,-9.62,0]
g3=[-60.99,-34.85,42.81,0,-7.97,51.38,66.96,-120.78,187.75,0,0,-9.61,0]
g4=[-81.07,-32.19,41.84,0,-9.66,71.48,22.07,-140.38,185.49,0,0,-9.59,0]
g5=[-68.09,-3.17,30.42,1.77,-32.1,17.98,69.68,-171.54,151.05,0,-5.09,-50.99,0]
g6=[-80.34,-4.37,26.75,3.84,-26.41,-91.04,32.82,-184.16,164.66,3.92,3.63,-172.85,-2.56]
g7=[-18.4,-30.36,35.09,0,-4.75,8.7,137.74,-45.82,201.89,0,0,-9.69,0]
g8=[-95.02,-33.33,37.84,0,-4.51,85.27,-12.27,-139.84,199.59,0,0,-9.74,0]
g9=[-97.04,-25.62,47.64,8.73,-34.56,-38.2,-15.69,-151.72,156.07,13.99,-4.51,-139.97,0]
g10=[-101.76,-34.54,38.88,8.56,-16.39,-36.16,-25.86,-141.25,199.65,14,-4.5,-140.02,0]
g11=[-51.45,-32.92,47.05,-4.58,-28.26,-89.68,89.46,-115.01,172.33,14.06,-4.52,-140.02,0]
g12=[-26.46,-33.46,52.41,0,-18.95,16.8,122.17,-60.82,156.86,0,0,-9.67,0]
g13=[-91.27,-28.57,49.88,0,-21.31,81.59,-3.16,-142.71,154.8,0,0,-9.68,0]
g14=[-108.04,-23.39,46.48,0,-23.1,98.34,-46.48,-142.72,153.96,0,0,-9.7,0]
g15=[-107.4,-13.62,42.34,-1.36,-36.92,-65.5,-49.94,-157.81,144.27,6.97,-4.47,-172.43,0]
g16=[-103.54,-7.46,38.52,-9.38,-33.35,-133.54,-43.46,-166.31,140.01,6.95,-4.47,127.65,0]
#########################################################################

#B D
#A C
########################################################################
#A在托上的位置
t1=[-19.24,20.41,17.81,0,-38.23,19.23,200.62,-70.01,115.09,0,0,0,0]
t2=[-19.24,7.09,4.88,0,-11.95,19.23,200.61,-70,194.42,0,0,0,0]
#b在托上的位置
t3= [-27.65,26.71,10.94,0,-37.65,27.66,198.63,-104.08,111.8,0,0,0,0]
t4 = [-27.65,13.55,-12.69,0,-0.85,27.66,198.63,-104.08,225.17,0,0,0,0]
#c在托上的位置
t5 = [-32.3,12.37,27.84,0,-40.22,32.29,164.63,-104.09,114.38,0,0,0,0]
t6=[-34.64,-8.42,9.23,0,-0.82,34.64,150.67,-104.09,227.15,0,0,0,0]
#D在托上的位置
t7=[-23.05,4.83,36.5,0,-41.33,45.04,164.6,-70.04,113.76,0,0,22,0]
t8=[-30.14,-31.63,26.38,0,5.25,30.14,120.63,-70.05,230.07,0,0,0,0]

##################################################################################################################################################
a1=[-63.75,14.11,3.72,0,-17.82,63.75,98.64,-200,174.77,0,0,0,0]#吸左上1
a2=[-63.75,12.81,-11.61,0,-1.21,63.75,98.64,-200,224.48,0,0,0,0]#左上1的上方

a3 = [-75.21,18.92,-3.41,0,-15.51,75.2,59.46,-225.16,178.97,0,0,0,0]#吸左上2
a4 = [-75.21,18.71,-19.86,0,1.16,75.2,59.46,-225.15,228.4,0,0,0,0]

a5 = [-87.15,19.25,-2.94,0,-16.31,87.16,11.59,-232.8,176.4,0,0,0,0]#吸左3
a6= [-87.13,17.44,-17.58,0,0.15,87.13,11.58,-230.7,226.16,0,0,0,0]

a7= [-99.25,15.79,-1.2,0,-14.6,99.25,-36.58,-224.7,183.44,0,0,0,0]#吸左4
a8 = [-99.25,16.31,0.33,0,-16.64,99.25,-36.58,-224.7,177.15,0,0,0,0]

##############矫正用点位
q1 = [-19.59,19.08,20.06,0,-39.13,71.59,196.6,-69.96,113.63,0,0,52,0]
q2 = [-27.65,26.94,10.94,0,-37.89,7.66,198.66,-104.1,111.03,0,0,-20,0]

#####################################################取料开始###################################################################
arm = WlkataMirobot()
arm_ = WlkataMirobot_o(portname='/dev/ttyUSB0')
arm.home()
#a
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = a1)
arm.Master.set_DO(0,1)
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = t2)
arm.set_joint_angle(P = q1)
arm.Master.set_DO(0,0)
arm.set_joint_angle(P = t2)

#b
arm.set_joint_angle(P = a4)
arm.set_joint_angle(P = a3)
arm.Master.set_DO(0,1)
arm.set_joint_angle(P = a4)
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = t4)
arm.set_joint_angle(P = q2)
arm.Master.set_DO(0,0)
arm.set_joint_angle(P = t4)

#c
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = a4)
arm.set_joint_angle(P = a6)
arm.set_joint_angle(P = a5)
arm.Master.set_DO(0,1)
arm.set_joint_angle(P = a6)
arm.set_joint_angle(P = a4)
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = t6)
arm.set_joint_angle(P = t5)
arm.Master.set_DO(0,0)
arm.set_joint_angle(P = t6)

#d
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = a4)
arm.set_joint_angle(P = a6)
arm.set_joint_angle(P = a8)
arm.set_joint_angle(P = a7)
arm.Master.set_DO(0,1)
arm.set_joint_angle(P = a8)
arm.set_joint_angle(P = a6)
arm.set_joint_angle(P = a4)
arm.set_joint_angle(P = a2)
arm.set_joint_angle(P = t8)
arm.set_joint_angle(P = t7)
arm.Master.set_DO(0,0)
arm.set_joint_angle(P = t8)
###################################################取料结束，货物都在托盘上###########################################
#######################################货叉开始运动##################################################################

arm_.home()

arm_.set_joint_angle(P = s1)
time.sleep(1)
arm_.set_joint_angle(P = s2)
time.sleep(1)
arm_.set_joint_angle(P = s3)
time.sleep(1)
arm_.set_joint_angle(P = s4)
arm_.set_joint_angle(P = s6)
arm_.set_joint_angle(P = s7)
arm_.set_joint_angle(P = s11)
arm_.set_joint_angle(P = s12)
arm_.set_joint_angle(P = s13)
time.sleep(2)

arm_.set_joint_angle(P = s14)
arm_.set_joint_angle(P = s15)
##################################################货叉运动结束，货物和托盘一起在传送带上##################################
########################################传送带开始载着托盘和货物运动#####################################################
arm.Master.set_DO(1,1)
time.sleep(15)
arm.Master.set_DO(1,0)

arm_.home()
########################################传送带运动结束###################################################################
###################################################从传送带上取物到下层货架############################################
arm.set_joint_angle(P = m1a)
arm.Master.set_DO(0,1)
time.sleep(1)
arm.set_joint_angle(P = m2a)
arm.set_joint_angle(P = g1)#g1辅助点位
arm.set_joint_angle(P = z1)
arm.Master.set_DO(0,0)
arm.set_joint_angle(P = g5)

arm.set_joint_angle(P = m4a)#先要到B上面 防止碰撞
arm.set_joint_angle(P = m3a)
arm.Master.set_DO(0,1)
time.sleep(1)
arm.set_joint_angle(P = g2)#过度点位
arm.set_joint_angle(P = g3)
arm.set_joint_angle(P = g4)
arm.set_joint_angle(P = z2)
arm.Master.set_DO(0,0)

arm.set_joint_angle(P = g6)
arm.set_joint_angle(P = g4)
arm.set_joint_angle(P = m6a)
arm.set_joint_angle(P = m5a)
arm.Master.set_DO(0,1)
arm.set_joint_angle(P = m6a)
arm.set_joint_angle(P = g7)
arm.set_joint_angle(P = g8)
arm.set_joint_angle(P = g9)
arm.set_joint_angle(P = z3)
arm.Master.set_DO(0,0)

arm.set_joint_angle(P = g10)#回去过程可加速
arm.set_joint_angle(P = g11)#回去过程可加速
arm.set_joint_angle(P = m8a)
arm.set_joint_angle(P = m7a)
arm.Master.set_DO(0,1)
arm.set_joint_angle(P = m8a)
arm.set_joint_angle(P = g11)
arm.set_joint_angle(P = g12)
arm.set_joint_angle(P = g13)
arm.set_joint_angle(P = g14)
arm.set_joint_angle(P = g15)
arm.set_joint_angle(P = g16)
arm.Master.set_DO(0,0)