-
Notifications
You must be signed in to change notification settings - Fork 0
/
test_HdownL.py
executable file
·174 lines (162 loc) · 5.55 KB
/
test_HdownL.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
from interbotix_xs_modules.arm import InterbotixManipulatorXS
import time
def get_chess_square_info(move, number):
characters = "ABCDEFGH"
character = characters[move]
return f'{character}{number}'
chess_info_H = {
"A1": (13.5, 10.5, 0.15, 1.56),
"B1": (13.5, 7.5, 0.15, 1.56),
"C1": (13.5, 4.5, 0.15, 1.56),
"D1": (13.5, 1.5, 0.15, 1.56),
"E1": (13.5, -1.5, 0.15, 1.56),
"F1": (13.5, -4.5, 0.15, 1.56),
"G1": (13.5, -7.5, 0.15, 1.56),
"H1": (13.5, -10.5, 0.15, 1.56),
"A2": (16.5, 10.5, 0.15, 1.56),
"B2": (16.5, 7.5, 0.15, 1.56),
"C2": (16.5, 4.5, 0.15, 1.56),
"D2": (16.5, 1.5, 0.15, 1.56),
"E2": (16.5, -1.5, 0.15, 1.56),
"F2": (16.5, -4.5, 0.15, 1.56),
"G2": (16.5, -7.5, 0.15, 1.56),
"H2": (16.5, -10.5, 0.15, 1.56),
"A3": (19.5, 10.5, 0.15, 1.56),
"B3": (19.5, 7.5, 0.15, 1.56),
"C3": (19.5, 4.5, 0.15, 1.56),
"D3": (19.5, 1.5, 0.15, 1.56),
"E3": (19.5, -1.5, 0.15, 1.56),
"F3": (19.5, -4.5, 0.15, 1.56),
"G3": (19.5, -7.5, 0.15, 1.56),
"H3": (19.5, -10.5, 0.15, 1.56),
"A4": (22.5, 10.5, 0.15, 1.46),
"B4": (22.5, 7.5, 0.15, 1.51),
"C4": (22.5, 4.5, 0.15, 1.54),
"D4": (22.5, 1.5, 0.15, 1.56),
"E4": (22.5, -1.5, 0.15, 1.56),
"F4": (22.5, -4.5, 0.15, 1.54),
"G4": (22.5, -7.5, 0.15, 1.51),
"H4": (22.5, -10.5, 0.15, 1.46),
"A5": (25.5, 10.5, 0.12, 1.43),
"B5": (25.5, 7.5, 0.12, 1.49),
"C5": (25.5, 4.5, 0.12, 1.53),
"D5": (25.5, 1.5, 0.12, 1.55),
"E5": (25.5, -1.5, 0.12, 1.55),
"F5": (25.5, -4.5, 0.12, 1.53),
"G5": (25.5, -7.5, 0.12, 1.49),
"H5": (25.5, -10.5, 0.12, 1.43),
"A6": (28.5, 10.5, 0.12, 1.27),
"B6": (28.5, 7.5, 0.12, 1.32),
"C6": (28.5, 4.5, 0.12, 1.36),
"D6": (28.5, 1.5, 0.12, 1.38),
"E6": (28.5, -1.5, 0.12, 1.38),
"F6": (28.5, -4.5, 0.12, 1.36),
"G6": (28.5, -7.5, 0.12, 1.32),
"H6": (28.5, -10.5, 0.12, 1.27),
"A7": (31.5, 10.5, 0.1, 1.18),
"B7": (31.5, 7.5, 0.1, 1.23),
"C7": (31.5, 4.5, 0.1, 1.26),
"D7": (31.5, 1.5, 0.1, 1.28),
"E7": (31.5, -1.5, 0.1, 1.28),
"F7": (31.5, -4.5, 0.1, 1.26),
"G7": (31.5, -7.5, 0.1, 1.23),
"H7": (31.5, -10.5, 0.1, 1.18),
"A8": (34.5, 10.5, 0.1, 1.02),
"B8": (34.5, 7.5, 0.1, 1.06),
"C8": (34.5, 4.5, 0.1, 1.09),
"D8": (34.5, 1.5, 0.1, 1.11),
"E8": (34.5, -1.5, 0.1, 1.11),
"F8": (34.5, -4.5, 0.1, 1.09),
"G8": (34.5, -7.5, 0.1, 1.06),
"H8": (34.5, -10.5, 0.1, 1.02),
}
chess_info_L = {
"A1": (13.5, 10.5, 0.03, 1.56),
"B1": (13.5, 7.5, 0.03, 1.56),
"C1": (13.5, 4.5, 0.03, 1.56),
"D1": (13.5, 1.5, 0.03, 1.56),
"E1": (13.5, -1.5, 0.03, 1.56),
"F1": (13.5, -4.5, 0.03, 1.56),
"G1": (13.5, -7.5, 0.03, 1.56),
"H1": (13.5, -10.5, 0.03, 1.56),
"A2": (16.5, 10.5, 0.03, 1.56),
"B2": (16.5, 7.5, 0.03, 1.56),
"C2": (16.5, 4.5, 0.03, 1.56),
"D2": (16.5, 1.5, 0.03, 1.56),
"E2": (16.5, -1.5, 0.03, 1.56),
"F2": (16.5, -4.5, 0.03, 1.56),
"G2": (16.5, -7.5, 0.03, 1.56),
"H2": (16.5, -10.5, 0.03, 1.56),
"A3": (19.5, 10.5, 0.03, 1.56),
"B3": (19.5, 7.5, 0.03, 1.56),
"C3": (19.5, 4.5, 0.03, 1.56),
"D3": (19.5, 1.5, 0.03, 1.56),
"E3": (19.5, -1.5, 0.03, 1.56),
"F3": (19.5, -4.5, 0.03, 1.56),
"G3": (19.5, -7.5, 0.03, 1.56),
"H3": (19.5, -10.5, 0.03, 1.56),
"A4": (22.5, 10.5, 0.03, 1.56),
"B4": (22.5, 7.5, 0.03, 1.56),
"C4": (22.5, 4.5, 0.03, 1.56),
"D4": (22.5, 1.5, 0.03, 1.56),
"E4": (22.5, -1.5, 0.03, 1.56),
"F4": (22.5, -4.5, 0.03, 1.56),
"G4": (22.5, -7.5, 0.03, 1.56),
"H4": (22.5, -10.5, 0.03, 1.56),
"A5": (25.5, 10.5, 0.03, 1.56),
"B5": (25.5, 7.5, 0.03, 1.56),
"C5": (25.5, 4.5, 0.03, 1.56),
"D5": (25.5, 1.5, 0.03, 1.56),
"E5": (25.5, -1.5, 0.03, 1.56),
"F5": (25.5, -4.5, 0.03, 1.56),
"G5": (25.5, -7.5, 0.03, 1.56),
"H5": (25.5, -10.5, 0.03, 1.56),
"A6": (28.5, 10.5, 0.03, 1.52),
"B6": (28.5, 7.5, 0.03, 1.56),
"C6": (28.5, 4.5, 0.03, 1.56),
"D6": (28.5, 1.5, 0.03, 1.52),
"E6": (28.5, -1.5, 0.03, 1.52),
"F6": (28.5, -4.5, 0.03, 1.56),
"G6": (28.5, -7.5, 0.03, 1.56),
"H6": (28.5, -10.5, 0.03, 1.52),
"A7": (31.5, 10.5, 0.03, 1.34),
"B7": (31.5, 7.5, 0.03, 1.39),
"C7": (31.5, 4.5, 0.03, 1.43),
"D7": (31.5, 1.5, 0.03, 1.44),
"E7": (31.5, -1.5, 0.03, 1.44),
"F7": (31.5, -4.5, 0.03, 1.43),
"G7": (31.5, -7.5, 0.03, 1.39),
"H7": (31.5, -10.5, 0.03, 1.34),
"A8": (34.5, 10.5, 0.03, 1.17),
"B8": (34.5, 7.5, 0.03, 1.21),
"C8": (34.5, 4.5, 0.03, 1.25),
"D8": (34.5, 1.5, 0.03, 1.26),
"E8": (34.5, -1.5, 0.03, 1.26),
"F8": (34.5, -4.5, 0.03, 1.25),
"G8": (34.5, -7.5, 0.03, 1.21),
"H8": (34.5, -10.5, 0.03, 1.17),
}
bot = InterbotixManipulatorXS('rx150', 'arm', 'gripper')
bot.arm.moving_time = 0.5
chess_height = 3*0.01
file = open('/home/drx/Chess_robot/ytuongmoi', 'w')
for number in range(1, 9, 1):
for move in range(8):
name = get_chess_square_info(move, number)
xH, yH, zH, pitchH = chess_info_H[name]
xL, yL, zL, pitchL = chess_info_L[name]
# print(f'x = {x}')
# print(f'y = {y}')
# print(f'pitch')
print(name)
bot.arm.set_ee_pose_components(
x=xH*0.01, y=yH*0.01, z=zH, pitch=pitchH)
success = bot.arm.set_ee_cartesian_trajectory(
z=-(zH-chess_height), pitch=+(pitchL-pitchH))
if success:
file.write(f'{name}: success' + '\n')
else:
file.write(f'{name}: fail' + '\n')
# time.sleep(3)
print(f"End__{name}")
file.close()