Skip to content

Commit 1fc2dc2

Browse files
TheFive666TheFive666
authored
add pro630_3D_Kit_Code (#172)
* Create .txt * add pro630_3D_Kit_Code add pro630_3D_Kit_Code
1 parent cc2d642 commit 1fc2dc2

File tree

6 files changed

+556
-0
lines changed

6 files changed

+556
-0
lines changed

‎demo/pro630_3D_Kit_Code/.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+

‎demo/pro630_3D_Kit_Code/HandInEyeCailb.py‎

Lines changed: 147 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,147 @@
1+
# -*- coding: utf-8 -*-
2+
import _thread
3+
import socket
4+
import json
5+
import time
6+
import sys
7+
import math
8+
import copy
9+
from pymycobot import ElephantRobot
10+
11+
12+
13+
14+
def connectRvsServer(ip, port=2013):
15+
sock=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
16+
try:
17+
sock.connect((ip,port))
18+
print("ip:%s connected!" %(ip))
19+
return (True,sock)
20+
except Exception as e:
21+
sock.close()
22+
print("ip:%s disconnected!" %(ip))
23+
return (False,0)
24+
25+
def disconnect (sock):
26+
if(sock):
27+
sock.close ()
28+
sock=None
29+
else:
30+
sock=None
31+
32+
33+
def get_send_TCP(sock_rvs):
34+
#TCP
35+
#get tcp
36+
time.sleep(2)
37+
result = erobot.get_coords()
38+
#result = result[0:6]
39+
robot_TCP = result.copy()
40+
print("***get tcp***%s"%time.asctime())
41+
print(result)
42+
43+
resCutStr = ' '.join(repr(e) for e in result)
44+
print(resCutStr)
45+
#send tcp
46+
sendStr = "ROBOT_TCP " + resCutStr + '#'
47+
print("***send tcp***%s"%time.asctime())
48+
print(sendStr)
49+
sock_rvs.send(bytes(sendStr,encoding="utf-8"))
50+
print("***tcp_receive data***%s"%time.asctime())
51+
data=sock_rvs.recv(socket_buf_len)
52+
print(data)
53+
if data == 'ROBOT_TCP'.encode('utf-8'):
54+
print("***receive ROBOT_TCP***\n")
55+
return True,robot_TCP;
56+
else:
57+
print(str(sys._getframe().f_lineno) + "error\n")
58+
print("tcp_receive FAIL\n")
59+
time.sleep(0)
60+
return False;
61+
62+
def wait_done():
63+
time.sleep(2)
64+
while erobot.command_wait_done()!="0":
65+
time.sleep(2)
66+
67+
if __name__ == "__main__":
68+
69+
robot_ip="192.168.6.79"
70+
erobot = ElephantRobot(robot_ip,5001)
71+
erobot.start_client()
72+
rvs_ip = "localhost"
73+
rvs_port = 2013
74+
conSuc_rvs, sock_rvs=connectRvsServer(rvs_ip, rvs_port)#连接RVS
75+
76+
cal_joint=[-10.01, -112.825, 104.148, -82.116, -89.213, -28.698]
77+
J1 = cal_joint[0]
78+
J2 = cal_joint[1]
79+
J3 = cal_joint[2]
80+
J4 = cal_joint[3]
81+
J5 = cal_joint[4]
82+
J6 = cal_joint[5]
83+
84+
point = []
85+
# home位 p0
86+
point.append ([J1, J2, J3, J4, J5, J6])
87+
88+
# p1
89+
point.append ([J1, J2, J3, J4+2, J5-6, J6+2])
90+
# p2
91+
point.append ([J1-2, J2+4, J3-2, J4+4, J5-4, J6+2])
92+
# p3
93+
point.append ([J1-4, J2+2, J3-4, J4+4, J5-4, J6+2])
94+
# p4
95+
point.append ([J1+4, J2+4, J3-4, J4-4, J5-4, J6+2])
96+
# p5
97+
point.append ([J1+4, J2+2, J3-4, J4+4, J5, J6+2])
98+
# p6
99+
point.append ([J1+4, J2, J3-2, J4-2, J5, J6+2])
100+
# p7
101+
point.append ([J1+2, J2+6, J3-4, J4-4, J5, J6+2])
102+
# p8
103+
point.append ([J1+2, J2, J3-2, J4+4, J5-2, J6+2])
104+
# p9
105+
point.append ([J1+6, J2+4, J3, J4-2, J5-6, J6+2])
106+
# p10
107+
point.append ([J1-4, J2+4, J3+2, J4-2, J5+4, J6-2])
108+
# p11
109+
point.append ([J1-2, J2+2, J3-2, J4+4, J5+4, J6-3])
110+
# p12
111+
point.append ([J1-4, J2, J3-2, J4+4, J5+2, J6+4])
112+
# p13
113+
point.append ([J1, J2+2, J3+4, J4-4, J5, J6+5])
114+
# p14
115+
point.append ([J1+4, J2, J3+2, J4-2, J5, J6-7])
116+
# p15
117+
point.append ([J1-2, J2-4, J3+2, J4-4, J5+4, J6])
118+
# p16
119+
point.append ([J1-2, J2-2, J3+4, J4-4, J5-2, J6-2])
120+
# p17
121+
point.append ([J1+2, J2+2, J3+2, J4+4, J5+2, J6+1])
122+
# p18
123+
point.append ([J1+1, J2+2, J3+1, J4+2, J5+2, J6-3])
124+
# p19
125+
point.append ([J1+3, J2-2, J3+1, J4+1, J5+2, J6+4])
126+
# p20
127+
point.append ([J1+3, J2+2, J3, J4-1, J5-3, J6+2])
128+
129+
130+
socket_buf_len = 256
131+
exec_index = 0#拍照次数
132+
robot_speed = 1000
133+
134+
if erobot.state_check()==False:
135+
erobot.start_robot()
136+
137+
while exec_index <len(point):
138+
exec_index = exec_index +1
139+
print("第 %d 次拍照" %exec_index)
140+
print("移动到拍照位")
141+
#移动
142+
erobot.write_angles(point[exec_index-1], robot_speed)
143+
time.sleep(3)
144+
wait_done()
145+
get_send_TCP(sock_rvs)
146+
print("\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n")
147+

‎demo/pro630_3D_Kit_Code/Plane_grabbing.py‎

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
# -*- coding: utf-8 -*-
2+
3+
import socket
4+
5+
import time
6+
import numpy as np
7+
from pymycobot import ElephantRobot
8+
import sys
9+
10+
11+
12+
13+
14+
15+
def connectRvsServer(ip, port=2013):
16+
sock=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
17+
try:
18+
sock.connect((ip,port))
19+
print("ip:%s connected!" %(ip))
20+
return (True,sock)
21+
except Exception as e:
22+
sock.close()
23+
print("ip:%s disconnected!" %(ip))
24+
return (False,0)
25+
26+
def disconnect (sock):
27+
if(sock):
28+
sock.close ()
29+
sock=None
30+
else:
31+
sock=None
32+
33+
def get_send_TCP(sock_rvs):
34+
result =erobot.get_coords()
35+
robot_TCP = result.copy()
36+
print("***get tcp***%s"%time.asctime())
37+
print(result)
38+
39+
resCutStr = ' '.join(repr(e) for e in result)
40+
print(resCutStr)
41+
#send tcp
42+
sendStr = "SET_POSE " + resCutStr + '#'
43+
print("***send tcp***%s"%time.asctime())
44+
print(sendStr)
45+
sock_rvs.send(bytes(sendStr,encoding="utf-8"))
46+
print("***tcp_receive data***%s"%time.asctime())
47+
data=sock_rvs.recv(socket_buf_len)
48+
print(data)
49+
data_arr = data.split()
50+
pose_real = [0, 0, 0, 0, 0, 0]
51+
for j in range(0,6):
52+
pose_real[j] = float(data_arr[j])
53+
return pose_real,data_arr[-1].decode()
54+
55+
56+
def wait_done():
57+
time.sleep(3)
58+
while erobot.command_wait_done()!="0":
59+
# print("fail")
60+
time.sleep(6)
61+
62+
63+
# photo_point = [-10.01, -112.825, 104.148, -82.116, -89.213, -28.698]
64+
65+
66+
photo_point = [-14.634, -113.399, 104.467, -81.796, -89.152, -40.853]
67+
a=[41.106, -82.693, 114.778, -123.192, -90.124, 22.411]
68+
b=[55.486, -55.102, 76.027, -111.967, -90.395, 36.775]
69+
c=[41.042, -37.662, 48.943, -96.857, -89.985, 22.758]
70+
d=[25.567, -57.895, 80.522, -113.726, -89.821, 6.878]
71+
72+
target_name=["tee","elbow","ball_valve","through"]
73+
if __name__ == "__main__":
74+
75+
rvs_ip = "localhost"
76+
rvs_port = 2013
77+
robot_ip="192.168.1.159"
78+
erobot = ElephantRobot(robot_ip,5001)
79+
erobot.start_client()
80+
conSuc_rvs, sock_rvs=connectRvsServer(rvs_ip, rvs_port)
81+
robot_speed = 1999
82+
socket_buf_len = 1024
83+
exec_index = 0
84+
erobot.set_digital_out(0,0)
85+
reference_pose=[322.511, 28.932, 172.421, 179.064, 0.526, 108.679]
86+
if erobot.state_check()==False:
87+
erobot.start_robot()
88+
try:
89+
while 1:
90+
exec_index = exec_index +1
91+
print("第 %d 次拍照" %exec_index)
92+
93+
94+
print("移动到1号拍照位")
95+
erobot.write_angles(photo_point, robot_speed)
96+
erobot.command_wait_done()
97+
time.sleep(1)
98+
pose,id=get_send_TCP(sock_rvs)
99+
if id=="false":
100+
break
101+
for i in range(3,6):
102+
pose[i]=reference_pose[i]
103+
104+
105+
pose[2]+=160
106+
erobot.write_coords(pose,1999)
107+
erobot.command_wait_done()
108+
time.sleep(1)
109+
110+
111+
pose[2]-=45
112+
erobot.write_coords(pose,1999)
113+
erobot.command_wait_done()
114+
time.sleep(1)
115+
erobot.set_digital_out(0,1)
116+
time.sleep(2)
117+
118+
119+
pose[2]+=45
120+
erobot.write_coords(pose,1999)
121+
erobot.command_wait_done()
122+
time.sleep(1)
123+
124+
erobot.write_angles(photo_point, robot_speed)
125+
erobot.command_wait_done()
126+
time.sleep(1)
127+
if id==target_name[0]:
128+
erobot.write_angles(a,robot_speed)
129+
erobot.command_wait_done()
130+
time.sleep(1)
131+
elif id==target_name[1]:
132+
erobot.write_angles(b,robot_speed)
133+
erobot.command_wait_done()
134+
time.sleep(1)
135+
elif id==target_name[2]:
136+
erobot.write_angles(c,robot_speed)
137+
erobot.command_wait_done()
138+
time.sleep(1)
139+
elif id==target_name[3]:
140+
erobot.write_angles(d,robot_speed)
141+
erobot.command_wait_done()
142+
time.sleep(1)
143+
144+
erobot.set_digital_out(0,0)
145+
time.sleep(2)
146+
except:
147+
erobot.set_digital_out(0,0)
148+
erobot.stop_client()
149+
sock_rvs.close()
150+
print("end")

0 commit comments

Comments
 (0)