-
Notifications
You must be signed in to change notification settings - Fork 4
/
createPath.py
85 lines (70 loc) · 2.49 KB
/
createPath.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
from .TestController import RobotController
#import DepthCam
import json
from threading import Thread
import os
def create_path(name):
path = './robot_path'
if not os.path.exists(path):
os.makedirs(path)
print('creating Robot Controller')
controller = RobotController()
data = {
'joints': [],
'cart_pose': [],
'via_points': []
}
via = True
vias = 0
points = 0
print('starting loop')
while True:
p = input('enter for next position, v for via point, e for exit')
if p == 'e':
break
elif p == 'v':
via = True
else:
via = False
currentJoints = list(controller.get_joints())
currentPose = controller.get_pose()
if len(data['joints']) > 0:
same = True
for i in range(len(currentJoints)):
if currentJoints[i] != data['joints'][-1][i]:
same = False
break
if same:
print('same point')
else:
data['joints'].append(currentJoints)
data['cart_pose'].append(currentPose)
print()
if via:
data['via_points'].append('1')
vias += 1
print('Added via. vias = {}, points = {}, total points = {}, joints = {}'.format(vias, points, vias+points, currentJoints))
else:
points += 1
data['via_points'].append('0')
print('Added point. vias = {}, points = {}, total points = {}, joints = {}'.format(vias, points, vias + points, currentJoints))
else:
data['joints'].append(currentJoints)
data['cart_pose'].append(currentPose)
if via:
data['via_points'].append('1')
vias += 1
print('Added via. vias = {}, points = {}, total points = {}, joints = {}'.format(vias, points, vias + points, currentJoints))
else:
points += 1
data['via_points'].append('0')
print('Added point. vias = {}, points = {}, total points = {}, joints = {}'.format(vias, points, vias + points, currentJoints))
save_path = os.path.join(path, '{}.json'.format(name))
#print(save_path)
with open(save_path, 'w') as f:
json.dump(data, f)
def main():
name = 'handEyeCalibPath4'
create_path(name)
if __name__ == '__main__':
main()