-
Notifications
You must be signed in to change notification settings - Fork 0
/
key_bot_arm_right_notes.py
88 lines (71 loc) · 3.45 KB
/
key_bot_arm_right_notes.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
import key_bot_ik as ik
C4 = [-55.37109375, 87.01171875, 30.76171875, -27.83203125, -31.0546875]
C4S = [-59.47265625, 72.94921875, 31.640625, 6.15234375, -34.5703125]
D4 = [-52.734375, 75.5859375, 31.34765625, 5.2734375, -38.96484375]
E4F = [-56.25, 68.84765625, 32.2265625, 15.52734375, -24.90234375]
E4 = [-50.390625, 72.36328125, 31.0546875, 13.4765625, -31.0546875]
F4 = [-45.703125, 62.6953125, 38.37890625, 30.76171875, -48.6328125]
F4S = [-50.68359375, 53.61328125, 43.359375, 53.61328125, -67.3828125]
G4 = [-41.6015625, 57.421875, 42.48046875, 45.703125, -62.6953125]
G4S = [-46.2890625, 51.5625, 46.2890625, 64.16015625, -75.29296875]
A5 = [-37.20703125, 57.421875, 39.2578125, 54.4921875, -65.625]
B5F = [-39.55078125, 46.2890625, 53.61328125, 70.3125, -83.49609375]
B5 = [-31.640625, 54.78515625, 42.7734375, 62.40234375, -73.828125]
C5 = [-24.90234375, 53.02734375, 45.41015625, 70.01953125, -85.546875]
C5S = [-26.66015625, 46.2890625, 55.6640625, 83.203125, -99.31640625]
D5 = [-17.87109375, 52.1484375, 49.21875, 75.87890625, -98.73046875]
E5F = [-15.234375, 49.51171875, 54.78515625, 92.578125, -113.96484375]
E5 = [-8.49609375, 55.6640625, 43.65234375, 85.546875, -106.640625]
F5 = [0.5859375, 55.95703125, 43.65234375, 86.71875, -106.640625]
F5S = [5.56640625, 55.37109375, 50.9765625, 101.07421875, -127.1484375]
G5 = [9.375, 60.9375, 33.10546875, 90.52734375, -102.24609375]
G5S = [20.21484375, 59.47265625, 36.328125, 103.41796875, -112.5]
A6 = [20.21484375, 62.6953125, 28.41796875, 89.6484375, -96.97265625]
B6F = [29.8828125, 60.9375, 30.17578125, 97.55859375, -98.4375]
B6 = [26.66015625, 63.8671875, 26.953125, 89.0625, -98.4375]
C6 = [32.51953125, 65.625, 19.921875, 76.46484375, -77.34375]
xyz = [0.1131434, -0.16205538, 0.07500772]
dy = 0.01
C4_pos = [xyz[0], xyz[1], xyz[2]]
C4S_pos = [xyz[0], xyz[1], xyz[2]]
D4_pos = [xyz[0], xyz[1], xyz[2]]
E4F_pos = [xyz[0], xyz[1], xyz[2]]
E4_pos = [xyz[0], xyz[1], xyz[2]]
F4_pos = [xyz[0], xyz[1], xyz[2]]
F4S_pos = [xyz[0], xyz[1], xyz[2]]
G4_pos = [xyz[0], xyz[1], xyz[2]]
G4S_pos = [xyz[0], xyz[1], xyz[2]]
A5_pos = [xyz[0], xyz[1], xyz[2]]
B5F_pos = [xyz[0], xyz[1], xyz[2]]
B5_pos = [xyz[0], xyz[1], xyz[2]]
C5_pos = [xyz[0], xyz[1], xyz[2]]
C5S_pos = [xyz[0], xyz[1], xyz[2]]
D5_pos = [xyz[0], xyz[1], xyz[2]]
E5F_pos = [xyz[0], xyz[1], xyz[2]]
E5_pos = [xyz[0], xyz[1], xyz[2]]
F5_pos = [xyz[0], xyz[1], xyz[2]]
F5S_pos = [xyz[0], xyz[1], xyz[2]]
G5_pos = [xyz[0], xyz[1], xyz[2]]
G5S_pos = [xyz[0], xyz[1], xyz[2]]
A6_pos = [xyz[0], xyz[1], xyz[2]]
B6F_pos = [xyz[0], xyz[1], xyz[2]]
B6_pos = [xyz[0], xyz[1], xyz[2]]
C6_pos = [xyz[0], xyz[1], xyz[2]]
POS = [C4_pos, C4S_pos, D4_pos, E4F_pos, E4_pos, F4_pos, F4S_pos, G4_pos, G4S_pos, A5_pos, B5F_pos, B5_pos,
C5_pos, C5S_pos, D5_pos, E5F_pos, E5_pos, F5_pos, F5S_pos, G5_pos, G5S_pos, A6_pos, B6F_pos, B6_pos,
C6_pos]
for i in range(len(POS)):
POS[i][1] = POS[i][1] + dy*i
THETA = [C4, C4S, D4, E4F, E4, F4, F4S, G4, G4S, A5, B5F, B5,
for i in range(len(POS)):
order = ['p_x', 'p_y', 'p_z' ]
r = [0.0, 0.0, 0.0, 0.0, 0.0]
a = POS[i]
ik_theta, stat = ik.getIK(r,a, order)
THETA[i] = ik_theta
NAMES = ['C4', 'C4S', 'D4', 'E4F', 'E4', 'F4', 'F4S', 'G4', 'G4S', 'A5', 'B5F', 'B5',
'C5', 'C5S', 'D5', 'E5F', 'E5', 'F5', 'F5S', 'G5', 'G5S', 'A6', 'B6F', 'B6',
'C6']
NOTES = [C4, C4S, D4, E4F, E4, F4, F4S, G4, G4S, A5, B5F, B5,
C5, C5S, D5, E5F, E5, F5, F5S, G5, G5S, A6, B6F, B6,
C6]