forked from hrc2/blossom-public
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathconfig.py
272 lines (249 loc) · 9.41 KB
/
config.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
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
"""
Module to help differentiate between robots with different motor configurations.
"""
from __future__ import print_function
import pypot.dynamixel as pd
from serial.serialutil import SerialException
import sys
class RobotConfig(object):
"""
Repo for PyPot configurations for robots
Configs defined as controllers, motorgroups, and motors
"""
def __init__(self):
self.ports = pd.get_available_ports()
# catch no available ports
if (len(self.ports) == 0):
self.ports = ['']
self.configs = {
'woody': {
'controllers': {
'my_dxl_controller': {
'sync_read': False,
'attached_motors': ['tower', 'bases', 'head'],
'port': 'auto',
'baudrate': 1000000,
'protocol': 2
}
},
'motorgroups': {
'tower': ['tower_1', 'tower_2', 'tower_3'],
'bases': ['base'],
'head': ['ears']
},
'motors': {
'tower_1': {
'orientation': 'direct',
'type': 'XL-320',
'id': 1,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_2': {
'orientation': 'direct',
'type': 'XL-320',
'id': 2,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_3': {
'orientation': 'direct',
'type': 'XL-320',
'id': 3,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'base': {
'orientation': 'direct',
'type': 'XL-320',
'id': 4,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'ears': {
'orientation': 'direct',
'type': 'XL-320',
'id': 5,
'angle_limit': [50, 130.0],
'offset': 0.0
},
}
},
'blossom': {
'controllers': {
'mx_controller': {
'sync_read': False,
'attached_motors': ['tower', 'bases'],
'port': 'auto',
'baudrate': 1000000,
'protocol': 1
}
},
'motorgroups': {
'tower': ['tower_1', 'tower_2', 'tower_3'],
'bases': ['base']
},
'motors': {
'tower_1': {
'orientation': 'direct',
'type': 'MX-28',
'id': 1,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_2': {
'orientation': 'direct',
'type': 'MX-28',
'id': 2,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_3': {
'orientation': 'direct',
'type': 'MX-28',
'id': 3,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'base': {
'orientation': 'direct',
'type': 'MX-28',
'id': 4,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
}
}
},
'blossom_ears': {
'controllers': {
'xl_controller': {
'sync_read': False,
'attached_motors': ['ears'],
'port': self.ports[-1],
'baudrate': 1000000,
'protocol': 2
}
},
'motorgroups': {
'head': ['ears']
},
'motors': {
'ears': {
'orientation': 'direct',
'type': 'XL-320',
'id': 6,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
}
}
},
'test': {
'controllers': {
},
'motorgroups': {
},
'motors': {
}
}
}
def get_names(self):
"""
return a list of robot names associated with configurations
"""
return self.configs.keys()
def get_configs(self, names):
"""
assign unique ports to a list of names for as many ports as we have available
"""
# test config for debugging without a robot
if (names[0] == 'test'):
return {names[0]: self.configs[names[0]]}
# get configs for all robots
configs = []
for name in names:
if name in self.configs:
configs.append((name, self.configs[name]))
# get IDs for connected motors
used_configs = []
for port in self.ports:
if port == "":
print("No ports available")
sys.exit(1)
try:
if (names[0]!='blossom'):
dxl_io = pd.Dxl320IO(port)
else:
dxl_io = pd.DxlIO(port)
scanned_ids = dxl_io.scan(range(8))
# handle unopenable serial port
except SerialException as e:
print(e)
print("Error opening port, try:")
print("sudo chmod 777 " + port)
sys.exit(1)
# general exception
except Exception as e:
print("general exception caught (please update the except statement with the exception)", e)
scanned_ids = []
# print found motors
if (len(scanned_ids)==0):
print("No motors found on %s" % port)
continue
else:
print("Motors for %s:" % port, scanned_ids)
# iterate through all robot configs
for i in range(len(configs)):
name, config = configs[i]
# MH: scanning doesn't seem to work for the usb hub. Doing this for now since we don't use multiple bots, but if we ever do...valid_port_for_robot probably won't work
valid_port = (len(names) == 1 or self.valid_port_for_robot(
scanned_ids, config))
# remove missing motors
config = self.return_valid_motors(scanned_ids, config)
# iterate through all configs
if configs[i] not in used_configs and valid_port:
controller = config['controllers'].keys()[0]
config['controllers'][controller]['port'] = port
used_configs.append(configs[i])
print("Assigning %s to port %s" % (name, port))
break
else:
print("No robot found for port", port)
return {n: c for n, c in used_configs}
def valid_port_for_robot(self, scanned_ids, config):
"""
returns true if the number of motors discovered on a port match the number of motors in the given robot config
Args:
scanned_ids: a list of motor ids discovered on a port
config: the robot config
"""
motor_ids = [v["id"] for k, v in config["motors"].items()]
return len(scanned_ids) == len(motor_ids)
def return_valid_motors(self, scanned_ids, config):
"""
modify the robot config to handle cases where motors can't be found.
Args:
scanned_ids: a list of motor ids discovered on a port
config: the robot config
"""
# get lists from
motor_list = config['motors']
motor_groups = config['motorgroups']
missing_motors = []
# iterate through all motors
for m in motor_list:
# if motor ID is not in list of scanned IDs, mark it for removal
if not (motor_list[m]['id'] in scanned_ids):
print("Couldn't find motor %s: " % motor_list[m]['id'], m)
missing_motors.append(m)
# remove missing motors
for m in missing_motors:
del(motor_list[m])
# must also remove motors from any motorgroups that they belong in
for mg in motor_groups:
# get motor names in this group
mg_values = motor_groups[mg]
# if motor is in motor group, delete
if m in mg_values:
mg_values.remove(m)
# return new config
return config