odrive 0.6.9.dev0__py37-none-win_amd64.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (49) hide show
  1. odrive/__init__.py +142 -0
  2. odrive/api_client.py +54 -0
  3. odrive/config.py +1296 -0
  4. odrive/crypto.py +369 -0
  5. odrive/data/brakeRs.json +18 -0
  6. odrive/data/drvs.json +82 -0
  7. odrive/data/encoders.json +86 -0
  8. odrive/data/motors.json +128 -0
  9. odrive/data/odrive-micro-x1.json +48 -0
  10. odrive/data/odrive-micro-x3.json +40 -0
  11. odrive/data/odrive-micro-x4.json +40 -0
  12. odrive/data/odrive-s1-x4.json +52 -0
  13. odrive/data/odrive-v4.4.json +58 -0
  14. odrive/data/schema-brakeR.json +6 -0
  15. odrive/data/schema-drv.json +6 -0
  16. odrive/data/schema-encoders.json +6 -0
  17. odrive/data/schema-motors.json +6 -0
  18. odrive/data/schema-odrive.json +5 -0
  19. odrive/data/schema.json +244 -0
  20. odrive/database.py +184 -0
  21. odrive/dfu.py +195 -0
  22. odrive/dfuse/DfuDevice.py +297 -0
  23. odrive/dfuse/__init__.py +1 -0
  24. odrive/enums.py +415 -0
  25. odrive/firmware.py +102 -0
  26. odrive/hw_version.py +67 -0
  27. odrive/legacy.py +73 -0
  28. odrive/legacy_config.py +123 -0
  29. odrive/legacy_dfu.py +442 -0
  30. odrive/lib/libodrive-windows-x64.dll +0 -0
  31. odrive/libodrive.py +325 -0
  32. odrive/pyfibre/fibre/__init__.py +4 -0
  33. odrive/pyfibre/fibre/libfibre-windows-amd64.dll +0 -0
  34. odrive/pyfibre/fibre/libfibre.py +1078 -0
  35. odrive/pyfibre/fibre/libwinpthread-1.dll +0 -0
  36. odrive/pyfibre/fibre/shell.py +162 -0
  37. odrive/pyfibre/fibre/utils.py +133 -0
  38. odrive/release_api.py +184 -0
  39. odrive/rich_text.py +134 -0
  40. odrive/shell.py +144 -0
  41. odrive/utils.py +881 -0
  42. odrive/version.py +1 -0
  43. odrive-0.6.9.dev0.data/scripts/odrive_demo.py +50 -0
  44. odrive-0.6.9.dev0.data/scripts/odrivetool +263 -0
  45. odrive-0.6.9.dev0.data/scripts/odrivetool.bat +2 -0
  46. odrive-0.6.9.dev0.dist-info/METADATA +22 -0
  47. odrive-0.6.9.dev0.dist-info/RECORD +49 -0
  48. odrive-0.6.9.dev0.dist-info/WHEEL +5 -0
  49. odrive-0.6.9.dev0.dist-info/top_level.txt +1 -0
odrive/config.py ADDED
@@ -0,0 +1,1296 @@
1
+
2
+ import odrive.database
3
+ from odrive.enums import EncoderId, AxisState, ComponentStatus, ProcedureResult, ODriveError, Rs485EncoderMode, Protocol
4
+ from odrive.hw_version import HwVersion
5
+ from odrive.rich_text import RichText, Color, Style
6
+ from enum import Enum
7
+ import enum
8
+ import time
9
+ from typing import NamedTuple, List, Set, Iterable, Dict, Optional
10
+ import re
11
+ import struct
12
+ import functools
13
+
14
+ class NetType(Enum):
15
+ AB = enum.auto()
16
+ DIGITAL = enum.auto()
17
+ THREE_PHASE = enum.auto()
18
+ DC = enum.auto()
19
+ THERMISTOR = enum.auto()
20
+ RS485 = enum.auto()
21
+ CAN = enum.auto()
22
+ ENC = enum.auto()
23
+
24
+ class EntityType(Enum):
25
+ DEVICE = enum.auto()
26
+ DEVICE_INVERTER = enum.auto()
27
+ DEVICE_RS485_INTF = enum.auto()
28
+ DEVICE_INC_ENC_INTF = enum.auto()
29
+ DEVICE_ONBOARD_ENC = enum.auto()
30
+ DEVICE_GPIO = enum.auto()
31
+ AXIS = enum.auto()
32
+ AXIS_MOTOR = enum.auto()
33
+ AXIS_ENCODER = enum.auto()
34
+
35
+ class CalibrationStatus(Enum):
36
+ OK = enum.auto()
37
+ NEEDED = enum.auto()
38
+ RECOMMENDED = enum.auto()
39
+ UNKNOWN = enum.auto()
40
+
41
+ class IssueType(Enum):
42
+ ERROR = enum.auto() # error level issues prevent the configuration from being written onto the ODrive(s)
43
+ WARN = enum.auto() # warn level issues don't prevent the configuration from being applied but require user attention
44
+
45
+
46
+ class ConfigType():
47
+ """
48
+ Base class for config objects in the machine configuration. Implements
49
+ similar functionality to typing.NamedTuple but with less restrictions.
50
+ TODO: elaborate
51
+ """
52
+ def __init_subclass__(cls):
53
+ cls._fields = [(k, t, getattr(cls, k)) for k, t in cls.__annotations__.items()]
54
+ def __init__(self, **kwargs):
55
+ for k, t, d in self.__class__._fields:
56
+ if k in kwargs:
57
+ setattr(self, k, kwargs[k])
58
+ else:
59
+ if isinstance(d, list):
60
+ d = [*d]
61
+ setattr(self, k, d)
62
+ def __repr__(self):
63
+ return self.__class__.__name__ + "(" + ", ".join(k + "=" + repr(getattr(self, k)) for k, _, _ in self._fields) + ")"
64
+ def __eq__(self, other):
65
+ if self.__class__ != other.__class__:
66
+ return False
67
+ def eq(a, b):
68
+ if isinstance(a, list) and isinstance(b, list):
69
+ return len(a) == len(b) and all(eq(a[i], b[i]) for i in range(len(a)))
70
+ return a == b
71
+ return all(eq(getattr(self, k), getattr(other, k)) for k, _, _ in self.__class__._fields)
72
+ def __hash__(self):
73
+ def custom_hash(v):
74
+ if isinstance(v, list):
75
+ return custom_hash(tuple(elem for elem in v))
76
+ return hash(v)
77
+ return hash(tuple(custom_hash(getattr(self, k)) for k, _, _ in self.__class__._fields))
78
+
79
+ def to_json(self):
80
+ return _dump_named_tuple(self)
81
+
82
+ class EntityRef():
83
+ def __init__(self, parent: 'EntityRef', name: str, key, entity_type: EntityType, net_type: Optional[NetType]):
84
+ self.parent = parent
85
+ self.name = name
86
+ self.key = key
87
+ self.entity_type = entity_type
88
+ self.net_type = net_type
89
+
90
+ def __eq__(self, other):
91
+ return (isinstance(other, EntityRef) and
92
+ self.parent == other.parent and
93
+ self.entity_type == other.entity_type and
94
+ self.key == other.key)
95
+ def __hash__(self):
96
+ return hash((self.parent, self.entity_type, self.key))
97
+ def __repr__(self):
98
+ return f'Entity[{self.name}]'
99
+
100
+ def fullname(self):
101
+ if self.name is None:
102
+ return None
103
+ else:
104
+ parent_name = None if self.parent is None else self.parent.fullname()
105
+ return ('' if parent_name is None else parent_name + '.') + self.name
106
+
107
+ def get_children(self) -> Iterable['EntityRef']:
108
+ return []
109
+
110
+
111
+ class DeviceRef(EntityRef):
112
+ def __init__(self, machine_ref, dev_num, config: 'DeviceConfig'):
113
+ EntityRef.__init__(self, machine_ref, f'devices[{dev_num}]', dev_num, EntityType.DEVICE, None)
114
+ self.num = dev_num
115
+ self.config = config
116
+
117
+ def get_children(self):
118
+ product_info = _db.get_product(self.config.board)
119
+ for i in range(len(product_info['inverters'])):
120
+ yield EntityRef(self, f'inverters[{i}]', i, EntityType.DEVICE_INVERTER, NetType.THREE_PHASE)
121
+ for i in range(len(product_info['inc_enc'])):
122
+ yield EntityRef(self, f'inc_enc[{i}]', i, EntityType.DEVICE_INC_ENC_INTF, NetType.AB)
123
+ for i in range(product_info['onboard_encoders']):
124
+ yield EntityRef(self, f'onboard_enc[{i}]', i, EntityType.DEVICE_ONBOARD_ENC, NetType.ENC)
125
+ for i in range(len(product_info['rs485'])):
126
+ yield EntityRef(self, f'rs485[{i}]', i, EntityType.DEVICE_RS485_INTF, NetType.RS485)
127
+ for k, v in product_info['io'].items():
128
+ yield EntityRef(self, f'io[{k}]', k, EntityType.DEVICE_GPIO, NetType.DIGITAL)
129
+ if 't' in v:
130
+ yield EntityRef(self, f'thermistor_input[{k}]', k, EntityType.DEVICE_GPIO, NetType.THERMISTOR)
131
+
132
+ class EncoderRef(EntityRef):
133
+ def __init__(self, axis_ref, enc_num, config: 'EncoderConfig'):
134
+ EntityRef.__init__(self, axis_ref, f'encoders[{enc_num}]', enc_num, EntityType.AXIS_ENCODER, NetType.ENC)
135
+ self.num = enc_num
136
+ self.config = config
137
+
138
+ def get_children(self):
139
+ # TODO: only yield the correct entities for this encoder
140
+ yield EntityRef(self, f'ab', None, None, NetType.AB)
141
+ yield EntityRef(self, f'z', None, None, NetType.DIGITAL)
142
+ yield EntityRef(self, f'rs485', None, None, NetType.RS485)
143
+
144
+ class MotorRef(EntityRef):
145
+ def __init__(self, axis_ref, motor_num, config: 'MotorConfig'):
146
+ EntityRef.__init__(self, axis_ref, f'motors[{motor_num}]', motor_num, EntityType.AXIS_MOTOR, None)
147
+ self.num = motor_num
148
+ self.config = config
149
+
150
+ def phases(self):
151
+ return EntityRef(self, f'phases', None, None, NetType.THREE_PHASE)
152
+
153
+ def get_children(self):
154
+ yield self.phases()
155
+ yield EntityRef(self, f'thermistor', None, None, NetType.THERMISTOR)
156
+
157
+ class AxisRef(EntityRef):
158
+ def __init__(self, machine_ref, axis_num, config: 'AxisConfig'):
159
+ EntityRef.__init__(self, machine_ref, f'axes[{axis_num}]', axis_num, EntityType.AXIS, None)
160
+ self.num = axis_num
161
+ self.config = config
162
+
163
+ def motors(self):
164
+ for i, config in enumerate(self.config.motors):
165
+ yield MotorRef(self, i, config)
166
+
167
+ def encoders(self):
168
+ for i, config in enumerate(self.config.encoders):
169
+ yield EncoderRef(self, i, config)
170
+
171
+ def get_children(self):
172
+ for motor_ref in self.motors():
173
+ yield motor_ref
174
+ yield from motor_ref.get_children()
175
+ for encoder_ref in self.encoders():
176
+ yield encoder_ref
177
+ yield from encoder_ref.get_children()
178
+
179
+ class MachineRef(EntityRef):
180
+ def __init__(self, config: 'MachineConfig'):
181
+ EntityRef.__init__(self, None, None, None, None, None)
182
+ self.config = config
183
+
184
+ def devices(self):
185
+ for i, dev_config in enumerate(self.config.devices):
186
+ yield DeviceRef(self, i, dev_config)
187
+
188
+ def axes(self):
189
+ for i, axis_config in enumerate(self.config.axes):
190
+ yield AxisRef(None, i, axis_config)
191
+
192
+ def get_children(self):
193
+ for dev_ref in self.devices():
194
+ yield dev_ref
195
+ yield from dev_ref.get_children()
196
+ for axis_ref in self.axes():
197
+ yield axis_ref
198
+ yield from axis_ref.get_children()
199
+
200
+
201
+ _db = odrive.database.load()
202
+
203
+ _reboot_vars = [
204
+ r'^inc_encoder[0-9]+.config.enabled$',
205
+ r'^rs485_encoder_group0\.config\.mode$',
206
+ r'^config\.enable_can_[a-z]$',
207
+ r'^can\.condig\.baud_rate$',
208
+ r'^axis[0-9]\.config\.load_encoder$',
209
+ r'^axis[0-9]\.config\.commutation_encoder$',
210
+ r'^axis[0-9]\.config\.can\.node_id$',
211
+ r'^axis[0-9]\.config\.can\.is_extended$',
212
+ ]
213
+
214
+ class ExpressionParseError(Exception):
215
+ pass
216
+
217
+
218
+ def _parse_expr(val, unit, default):
219
+ val = val.strip()
220
+ if val == '':
221
+ if default is None:
222
+ return None
223
+ val = default
224
+ if unit != '':
225
+ if not val.endswith(' ' + unit):
226
+ raise ExpressionParseError(f"{val} is not a {unit}")
227
+ val = val[:-len(unit)-1]
228
+ try:
229
+ return float(val)
230
+ except ValueError:
231
+ return ExpressionParseError(f"{val} is not a float")
232
+
233
+
234
+
235
+ class CalibrationTask():
236
+ def __init__(self, name, func, status, issues):
237
+ self.name = name
238
+ self.func = func
239
+ self.status = status
240
+ self.issues = issues
241
+
242
+ def run(self):
243
+ self.func()
244
+
245
+ class IssueCollection():
246
+ def __init__(self):
247
+ self.issues = []
248
+
249
+ def append(self, ref, message, level = IssueType.ERROR):
250
+ """
251
+ Appends an issue to the issue collection.
252
+
253
+ `ref` defines which object in the user configuration the issue pertains
254
+ to.
255
+ """
256
+ if isinstance(ref, EntityRef):
257
+ self.issues.append((ref, message, level))
258
+ elif isinstance(ref, list):
259
+ for r in ref:
260
+ self.issues.append((r, message, level))
261
+ else:
262
+ assert False, "bad issue reference: " + str(ref) + " - " + str(message)
263
+
264
+ def get(self, ref: EntityRef):
265
+ """
266
+ Returns all issues pertaining to the specified configuration object.
267
+ """
268
+ for _ref, message, level in self.issues:
269
+ if _ref == ref:
270
+ yield message, level
271
+
272
+ def get_for_type(self, ref_type: type):
273
+ """
274
+ Returns all issues pertaining to any object of the specified type.
275
+ """
276
+ for _ref, message, level in self.issues:
277
+ if isinstance(_ref, ref_type):
278
+ yield _ref, message, level
279
+
280
+ class AuxConfig(ConfigType):
281
+ """
282
+ Holds the configuration of a brake resistor in a user defined machine.
283
+ """
284
+ db_ref: str = ""
285
+ resistance: str = ""
286
+
287
+ @staticmethod
288
+ def from_db(db_ref: str):
289
+ return AuxConfig.from_json({'db_ref': db_ref})
290
+
291
+ @staticmethod
292
+ def from_json(json: dict) -> 'AuxConfig':
293
+ """
294
+ Loads a :class:`AuxConfig` object from a dictionary (usually loaded from JSON).
295
+ """
296
+ return _load_named_tuple(json, AuxConfig)
297
+
298
+ def parse_resistance(self, unit=''): return _parse_expr(self.resistance, unit, None)
299
+
300
+ class EncoderConfig(ConfigType):
301
+ """
302
+ Holds the configuration of an encoder on an axis in a user defined machine.
303
+ """
304
+ db_ref: str = ""
305
+ scale: str = "" # [scalar], default: 1.0
306
+
307
+ @staticmethod
308
+ def from_db(db_ref: str):
309
+ return EncoderConfig.from_json({'db_ref': db_ref})
310
+
311
+ @staticmethod
312
+ def from_json(json: dict) -> 'EncoderConfig':
313
+ """
314
+ Loads a :class:`EncoderConfig` object from a dictionary (usually loaded from JSON).
315
+ """
316
+ return _load_named_tuple(json, EncoderConfig)
317
+
318
+ def protocol(self):
319
+ if self.db_ref == "":
320
+ raise Exception("this encoder has no protocol property")
321
+ encoder_info = _db.get_encoder(self.db_ref)
322
+ return encoder_info['protocol']
323
+
324
+ class MotorConfig(ConfigType):
325
+ """
326
+ Holds the configuration of a motor on an axis in a user defined machine.
327
+ """
328
+ db_ref: str = ""
329
+ scale: str = "" # [scalar], default: 1.0
330
+ phase_resistance: str = "" # [Ohm]
331
+ phase_inductance: str = "" # [H]
332
+ use_thermistor: bool = False # default: False
333
+ override: Dict = {}
334
+
335
+ @staticmethod
336
+ def from_db(db_ref: str, use_thermistor: bool = None, **kwargs):
337
+ return MotorConfig.from_json({'db_ref': db_ref, 'use_thermistor': use_thermistor, **kwargs})
338
+
339
+ @staticmethod
340
+ def from_json(json: dict) -> 'MotorConfig':
341
+ """
342
+ Loads a :class:`MotorConfig` object from a dictionary (usually loaded from JSON).
343
+ """
344
+ return _load_named_tuple(json, MotorConfig)
345
+
346
+ def parse_scale(self, unit=''): return _parse_expr(self.scale, unit, 1.0)
347
+ def parse_phase_resistance(self, unit=''): return _parse_expr(self.phase_resistance, unit, None)
348
+ def parse_phase_inductance(self, unit=''): return _parse_expr(self.phase_inductance, unit, None)
349
+
350
+ class AxisConfig(ConfigType):
351
+ """
352
+ Holds the configuration of an axis in a user defined machine.
353
+ """
354
+ motors: List[MotorConfig] = []
355
+ encoders: List[EncoderConfig] = []
356
+ calib_scan_vel: str = ""
357
+ calib_scan_distance: str = ""
358
+ calib_scan_range: str = ""
359
+ calib_torque: str = ""
360
+ pos_encoder: int = None
361
+ vel_encoder: int = None
362
+ commutation_encoder: int = None
363
+ pos_index: str = ""
364
+ pos_index_offset: str = ""
365
+ can_protocol: str = ""
366
+ can_node_id: str = ""
367
+
368
+ @staticmethod
369
+ def from_json(json: dict) -> 'AxisConfig':
370
+ """
371
+ Loads an :class:`AxisConfig` object from a dictionary (usually loaded from JSON).
372
+ """
373
+ return _load_named_tuple(json, AxisConfig)
374
+
375
+ def parse_calib_scan_vel(self, unit=''): return _parse_expr(self.calib_scan_vel, unit, '0.275 rps')
376
+ def parse_calib_scan_distance(self, unit=''): return _parse_expr(self.calib_scan_distance, unit, '1.1 turns')
377
+ def parse_calib_scan_range(self, unit=''): return _parse_expr(self.calib_scan_range, unit, '0.02')
378
+ def parse_calib_torque(self, unit=''): return _parse_expr(self.calib_torque, unit, None) # TODO: define default
379
+
380
+ def add_motor(self, motor_config: MotorConfig):
381
+ self.motors.append(motor_config)
382
+
383
+ def add_encoder(self, encoder_type: str, cpr: int, use_for_pos: bool, use_for_vel: bool, use_for_commutation: bool):
384
+ self.encoders.append(EncoderConfig({'type': encoder_type}))
385
+ if use_for_pos:
386
+ self.pos_encoder = len(self.encoders) - 1
387
+ if use_for_vel:
388
+ self.vel_encoder = len(self.encoders) - 1
389
+ if use_for_commutation:
390
+ self.commutation_encoder = len(self.encoders) - 1
391
+
392
+ class DeviceConfig(ConfigType):
393
+ """
394
+ Holds the configuration of an ODrive in a user defined machine.
395
+ """
396
+ board: HwVersion = None
397
+ serial_number: str = ""
398
+ shunt_conductances: List[float] = []
399
+ aux: List[AuxConfig] = []
400
+ fans: Dict[str, List[int]] = {}
401
+
402
+ @staticmethod
403
+ def from_json(json):
404
+ return _load_named_tuple(json, DeviceConfig)
405
+
406
+ def __init__(self, board: HwVersion, **kwargs):
407
+ product_info = _db.get_product(board) # verify that this is a known product
408
+ ConfigType.__init__(self, board=board, **kwargs)
409
+
410
+ JSON_FORMAT_VERSION = '0.1'
411
+
412
+ class FoundNoneError(Exception):
413
+ pass
414
+
415
+ class FoundMultipleError(Exception):
416
+ pass
417
+
418
+ class Net():
419
+ @staticmethod
420
+ def from_json(json: List[str], all_refs: Dict[str, EntityRef]):
421
+ ports = set()
422
+ types = set()
423
+ for port_name in json:
424
+ port = all_refs.get(port_name, None)
425
+ if port is None:
426
+ raise Exception("Unknown port: \"" + port_name + "\". Known ports are: " + str(list(all_refs.keys())))
427
+ ports.add(port)
428
+ types.add(port.net_type)
429
+
430
+ assert len(types) == 1, types
431
+ return Net(list(types)[0], ports)
432
+
433
+ @staticmethod
434
+ def union(net_type: NetType, *nets: Iterable['Net']):
435
+ assert all((net.net_type == net_type for net in nets))
436
+ if len(nets) == 0:
437
+ return Net(net_type, set())
438
+ return Net(net_type, set.union(*(net.ports for net in nets)))
439
+
440
+ def __init__(self, net_type: NetType, ports: Set[EntityRef]):
441
+ self.net_type = net_type
442
+ self.ports = ports
443
+
444
+ def __eq__(self, other):
445
+ return isinstance(other, Net) and self.ports == other.ports
446
+ def __hash__(self):
447
+ return hash(self.ports)
448
+
449
+ def __repr__(self):
450
+ return 'Net(' + ', '.join(p.fullname() for p in self.ports) + ')'
451
+
452
+ def to_json(self):
453
+ return sorted(p.fullname() for p in self.ports)
454
+
455
+ def get_all(self, entity_type: EntityType):
456
+ """
457
+ Returns all :class:`EntityRef`s that are connected to this net and have
458
+ the specified type `entity_type`.
459
+ """
460
+ assert isinstance(entity_type, EntityType)
461
+ def match(p):
462
+ if p.entity_type == entity_type:
463
+ return p
464
+ elif p.parent is None:
465
+ return None
466
+ else:
467
+ return match(p.parent)
468
+ matches = [match(p) for p in self.ports]
469
+ return [m for m in matches if not m is None]
470
+
471
+ def get_single(self, entity_type: EntityType):
472
+ """
473
+ Like `get_all()`, but raises an exception if multiple or no matching
474
+ entities were found.
475
+ """
476
+ ports = self.get_all(entity_type)
477
+ if len(ports) == 0:
478
+ raise FoundNoneError()
479
+ elif len(ports) > 1:
480
+ raise FoundMultipleError()
481
+ return ports[0]
482
+
483
+ class Action():
484
+ pass
485
+
486
+ class MotorCalibration(Action):
487
+ def __init__(self, axis, motor_config):
488
+ self.axis = axis
489
+ self.motor_config = motor_config
490
+
491
+ @property
492
+ def name(self): return "Motor Calibration"
493
+
494
+ @property
495
+ def issues(self):
496
+ if self.axis is None:
497
+ return ["disconnected"]
498
+ return []
499
+
500
+ def run(self):
501
+ assert len(self.issues) == 0
502
+ time.sleep(1) # TODO: fix this
503
+ # highly frequent (<100%) fail with current limit violation when run from test script, fixed with 1sec delay
504
+ # ^ does not seem to be an issue from `odrivetool`
505
+ _run_state(self.axis, AxisState.MOTOR_CALIBRATION)
506
+ print(f'measured phase_resistance: {self.axis.config.motor.phase_resistance}')
507
+ print(f'measured phase_inductance: {self.axis.config.motor.phase_inductance}')
508
+
509
+ class EncoderCalibration(Action):
510
+ def __init__(self, axis):
511
+ self.axis = axis
512
+
513
+ @property
514
+ def name(self): return "Encoder Offset Calibration"
515
+
516
+ @property
517
+ def issues(self):
518
+ if self.axis is None:
519
+ return ["disconnected"]
520
+ elif ComponentStatus(self.axis.commutation_mapper.status) == ComponentStatus.MISSING_INPUT:
521
+ return ["Encoder disconnected or misconfigured."]
522
+ return []
523
+
524
+ def run(self):
525
+ assert len(self.issues) == 0
526
+ _run_state(self.axis, AxisState.ENCODER_OFFSET_CALIBRATION)
527
+
528
+ class ClosedLoopControl(Action):
529
+ def __init__(self, axis, motor_config):
530
+ self.axis = axis
531
+ self.motor_config = motor_config
532
+
533
+ @property
534
+ def prerequisites(self):
535
+ motor_calibrated = (self.motor_config.phase_resistance != "") and (self.motor_config.phase_inductance != "")
536
+ yield MotorCalibration(self.axis, self.motor_config), CalibrationStatus.OK if motor_calibrated else CalibrationStatus.RECOMMENDED
537
+ if ComponentStatus(self.axis.commutation_mapper.status) != ComponentStatus.NOMINAL:
538
+ yield EncoderCalibration(self.axis), CalibrationStatus.NEEDED
539
+
540
+ class Axis():
541
+ def __init__(self, handle, config):
542
+ self.handle = handle
543
+ self.config = config
544
+
545
+ @property
546
+ def closed_loop_control(self) -> Action:
547
+ return ClosedLoopControl(self.handle, self.config.motors[0])
548
+
549
+
550
+ class MachineConfig(ConfigType):
551
+ """
552
+ Represents the configuration of a machine.
553
+
554
+ This includes a list of axes, a list of ODrive devices and information about
555
+ how the axes and devices are connected.
556
+ """
557
+ devices: List[DeviceConfig] = []
558
+ axes: List[AxisConfig] = []
559
+ nets: List[Net] = []
560
+
561
+ @staticmethod
562
+ def from_json(json: dict) -> 'MachineConfig':
563
+ """
564
+ Loads a :class:`MachineConfig` object from a dictionary (usually loaded from JSON).
565
+ """
566
+ if json.get('version', None) != JSON_FORMAT_VERSION:
567
+ raise LoadConfigException("Unsupported config version " + str(json.get('version', None)))
568
+
569
+ json = {**json}
570
+ json.pop('version')
571
+ nets = json.pop('nets') if 'nets' in json else []
572
+ cfg = _load_named_tuple(json, MachineConfig)
573
+ all_refs = cfg.get_entities_by_name()
574
+ cfg.nets.extend([Net.from_json(net, all_refs) for net in nets])
575
+ return cfg
576
+
577
+ def __init__(self, **kwargs):
578
+ nets = kwargs.pop('nets') if 'nets' in kwargs else []
579
+ ConfigType.__init__(self, **kwargs)
580
+ all_refs = self.get_entities_by_name()
581
+ self.nets.extend([Net.from_json(net, all_refs) for net in nets])
582
+
583
+ def to_json(self):
584
+ json = _dump_named_tuple(self)
585
+ json['version'] = JSON_FORMAT_VERSION
586
+ return json
587
+
588
+ def add_axis(self, axis_config: AxisConfig):
589
+ """
590
+ Adds an axis to the machine config.
591
+ """
592
+ self.axes.append(axis_config)
593
+ return len(self.axes) - 1
594
+
595
+ def add_device(self, dev_config: DeviceConfig):
596
+ """
597
+ Adds an ODrive device to the machine config.
598
+ """
599
+ self.devices.append(dev_config)
600
+ return len(self.devices) - 1
601
+
602
+ def merge(self, other: 'MachineConfig'):
603
+ n_devices = len(self.devices)
604
+ n_axes = len(self.axes)
605
+
606
+ self.axes.extend(other.axes)
607
+ self.devices.extend(other.devices)
608
+
609
+ def shifted_name(ref: EncoderRef):
610
+ if ref.entity_type == EntityType.DEVICE:
611
+ my_name = f'devices[{ref.num + n_devices}]'
612
+ elif ref.entity_type == EntityType.AXIS:
613
+ my_name = f'axes[{ref.num + n_axes}]'
614
+ else:
615
+ my_name = ref.name
616
+ parent_name = None if ref.parent is None else shifted_name(ref.parent)
617
+ return my_name if (parent_name is None) else (parent_name + '.' + my_name)
618
+
619
+ for net in other.nets:
620
+ self.connect(*(shifted_name(ref) for ref in net.ports))
621
+
622
+ def get_entities_by_name(self):
623
+ machine_ref = MachineRef(self)
624
+ return {ref.fullname(): ref for ref in machine_ref.get_children()}
625
+
626
+ def connect(self, *refs):
627
+ refs = set(refs)
628
+ if len(refs) == 0:
629
+ return
630
+
631
+ all_ports = self.get_entities_by_name()
632
+ ports = {all_ports[ref] for ref in refs}
633
+
634
+ types = set(p.net_type for p in ports)
635
+ if len(types) != 1:
636
+ raise Exception(f"Cannot connect nets of different types: {types}")
637
+ net_type = list(types)[0]
638
+
639
+ for net in self.get_nets(net_type):
640
+ intersection = net.ports & ports
641
+ if len(intersection) > 0:
642
+ net.ports.update(ports)
643
+
644
+ self.nets.append(Net(net_type, ports))
645
+
646
+ # def connect_phases(self, *refs):
647
+ # assert all([isinstance(r, InverterRef) or isinstance(r, MotorRef) for r in refs])
648
+ # self._three_phase_connections.append(refs)
649
+ #
650
+ # def connect_abz(self, enc_ref, inc_enc_intf_ref, z_gpio_ref = None):
651
+ # assert isinstance(enc_ref, EncoderRef)
652
+ # assert isinstance(inc_enc_intf_ref, IncEncIntfRef)
653
+ # assert z_gpio_ref is None or isinstance(z_gpio_ref, GpioRef)
654
+ # # TODO: register connection for index signal
655
+ # print("CONNECTING ", enc_ref.to_json(), inc_enc_intf_ref.to_json())
656
+ # self._ab_connections.append([enc_ref, inc_enc_intf_ref])
657
+ #
658
+ # def connect_rs485(self, enc_ref, rs485_intf_ref):
659
+ # assert isinstance(enc_ref, EncoderRef)
660
+ # assert isinstance(rs485_intf_ref, Rs485IntfRef)
661
+ # self._rs485_connections.append([enc_ref, rs485_intf_ref])
662
+
663
+ def get_nets(self, net_type: NetType):
664
+ return [net for net in self.nets if net.net_type == net_type]
665
+
666
+ def get_net(self, port: EntityRef):
667
+ for net in self.nets:
668
+ if any(p == port for p in net.ports):
669
+ return net
670
+ return Net(port.net_type, set())
671
+
672
+ def get_status(self, odrives):
673
+ """
674
+ Returns various information about the machine configuration taking into
675
+ account the list of currently connected ODrives and their state.
676
+
677
+ Returns a tuple (odrv_list, output_configs, issues, axis_obj)
678
+ where:
679
+
680
+ odrv_list: A list of ODrive objects that need to be configured. Each
681
+ entry corresponds to one device in this configuration object. Some
682
+ entries can be None.
683
+ output_configs: A list of multi-level dictionaries that hold all
684
+ configuration settings for all ODrives, whether they are connected or
685
+ not. The order an length of this list corresponds to `odrv_list`.
686
+ issues: An `IssueCollection` containing all errors and warnings that
687
+ were found.
688
+ needs_reboot: A list of booleans indicating for each device if a reboot
689
+ is required after applying the new configuration.
690
+ axis_calib: A list of lists of CalibrationTask objects representing the
691
+ available calibration tasks for this axis.
692
+ Each list in axis_calib corresponds to an axis in this configuration.
693
+ """
694
+
695
+ output_configs = [{'config': {
696
+ **{f'inverter{i}': {}
697
+ for i in range(len(_db.get_product(d.board)['inverters']))},
698
+ **{f'brake_resistor{i}': {}
699
+ for i in range(len(_db.get_product(d.board)['aux_inverters']))}
700
+ },
701
+ **{
702
+ f'axis{i}': {
703
+ 'controller': {'config': {}}, 'config': {'motor': {}, 'can': {}},
704
+ 'motor': {}
705
+ }
706
+ for i in range(len(_db.get_product(d.board)['inverters']))
707
+ }}
708
+ for d in self.devices]
709
+ issues = IssueCollection()
710
+ axis_obj = [[] for _ in range(len(self.axes))]
711
+ encoder_ids = [{} for _ in range(len(self.devices))]
712
+
713
+ ref = MachineRef(self)
714
+
715
+ # Associate devices in the configuration with connected devices
716
+ odrv_list = [None for _ in range(len(self.devices))]
717
+ odrives_by_serial_number = {odrv._serial_number: odrv for odrv in odrives}
718
+ for dev_ref in ref.devices():
719
+ dev_config = dev_ref.config
720
+ if dev_config.serial_number is None:
721
+ issues.append(dev_ref, 'Not associated with any serial number.')
722
+ elif not dev_config.serial_number in odrives_by_serial_number:
723
+ issues.append(dev_ref, 'Not connected.')
724
+ else:
725
+ dev = odrives_by_serial_number[dev_config.serial_number]
726
+ board = dev._board
727
+ if dev_config.board != board:
728
+ issues.append(dev_ref, 'Expected {} but found {}.'.format(dev_config.board, board))
729
+ else:
730
+ odrv_list[dev_ref.num] = dev
731
+
732
+ for odrv in odrives:
733
+ if not odrv._serial_number in [c.serial_number for c in self.devices]:
734
+ issues.append(ref, "Unused ODrive: " + odrv._serial_number, IssueType.WARN)
735
+
736
+ # Configure RS485 encoders
737
+ for net in self.get_nets(NetType.RS485):
738
+ try:
739
+ rs485_intf_ref = net.get_single(EntityType.DEVICE_RS485_INTF)
740
+ except FoundNoneError:
741
+ continue # not an error
742
+ except FoundMultipleError as ex:
743
+ issues.append(ex.refs, "Multiple ODrives are not allowed on the same RS485 bus.")
744
+ continue
745
+
746
+ enc_refs = net.get_all(EntityType.AXIS_ENCODER)
747
+ odrv_output_config = output_configs[rs485_intf_ref.parent.num]
748
+
749
+ for enc_ref in enc_refs:
750
+ encoder_config = enc_ref.config
751
+ enc_data = _db.get_encoder(encoder_config.db_ref)
752
+
753
+ rs485_protocol = encoder_config.protocol()
754
+ if not hasattr(Rs485EncoderMode, rs485_protocol):
755
+ issues.append(enc_ref, f"RS485 protocol {rs485_protocol} not supported.")
756
+ continue # ignore encoder
757
+
758
+ # Each rs485_encoder_group is dedictated to one RS485 port on the ODrive.
759
+ # One port can talk to multiple encoders (needs firmware change!).
760
+ if f'rs485_encoder_group{rs485_intf_ref.key}' in odrv_output_config:
761
+ rs485_encoder_group_config = odrv_output_config[f'rs485_encoder_group{rs485_intf_ref.key}']
762
+ else:
763
+
764
+ rs485_encoder_group_config = {'config': {'mode': getattr(Rs485EncoderMode, rs485_protocol)}}
765
+ odrv_output_config[f'rs485_encoder_group{rs485_intf_ref.key}'] = rs485_encoder_group_config
766
+
767
+ encoder_ids[rs485_intf_ref.parent.num][enc_ref] = EncoderId.RS485_ENCODER0
768
+
769
+ # Configure incremental encoders
770
+ for net in self.get_nets(NetType.AB):
771
+ try:
772
+ enc_ref = net.get_single(EntityType.AXIS_ENCODER)
773
+ except FoundNoneError:
774
+ continue # not an error
775
+ except FoundMultipleError as ex:
776
+ issues.append(ex.refs, "Multiple incremental encoders cannot share the same A/B signals.")
777
+ continue
778
+
779
+ inc_enc_intf_refs = net.get_all(EntityType.DEVICE_INC_ENC_INTF)
780
+ enc_data = _db.get_encoder(enc_ref.config.db_ref)
781
+
782
+ for inc_enc_intf_ref in inc_enc_intf_refs:
783
+ output_configs[inc_enc_intf_ref.parent.num]["inc_encoder{}".format(inc_enc_intf_ref.key)] = {
784
+ 'config': {'enabled': True, 'cpr': enc_data['cpr']}
785
+ }
786
+ encoder_ids[inc_enc_intf_ref.parent.num][enc_ref] = [EncoderId.INC_ENCODER0, EncoderId.INC_ENCODER1][inc_enc_intf_ref.key]
787
+
788
+ # Configure onboard encoders
789
+ for net in self.get_nets(NetType.ENC):
790
+ try:
791
+ axis_enc_ref = net.get_single(EntityType.AXIS_ENCODER)
792
+ onboard_enc_ref = net.get_single(EntityType.DEVICE_ONBOARD_ENC)
793
+ except FoundNoneError:
794
+ continue # not an error
795
+ except FoundMultipleError as ex:
796
+ issues.append(ex.refs, "Only 1:1 connections supported for onboard encoders.")
797
+ continue
798
+ encoder_ids[onboard_enc_ref.parent.num][axis_enc_ref] = [EncoderId.ONBOARD_ENCODER0, EncoderId.ONBOARD_ENCODER1][onboard_enc_ref.key]
799
+
800
+ # Configure motors
801
+ for net in self.get_nets(NetType.THREE_PHASE):
802
+ try:
803
+ inv_ref = net.get_single(EntityType.DEVICE_INVERTER)
804
+ except FoundNoneError:
805
+ continue # not connected to any inverter
806
+ except FoundMultipleError:
807
+ issues.append(axis_ref, "Phase bundling not implemented. Each motor must be connected to at most one inverter.")
808
+ continue
809
+
810
+ try:
811
+ motor_ref = net.get_single(EntityType.AXIS_MOTOR)
812
+ except FoundNoneError:
813
+ continue # not connected to any inverter
814
+ except FoundMultipleError:
815
+ issues.append(axis_ref, "Each inverter must be connected to at most one motor.")
816
+ continue
817
+
818
+ motor_config = motor_ref.config
819
+ motor_data = _db.get_motor(motor_config.db_ref)
820
+
821
+ axis_output_config = output_configs[inv_ref.parent.key]['axis{}'.format(inv_ref.key)]
822
+
823
+ if motor_config.scale != '':
824
+ issues.append(motor_ref, "Support for motor scale other than 1.0 not implemented.")
825
+
826
+ if motor_config.phase_resistance != "":
827
+ axis_output_config['config']['motor']['phase_resistance'] = motor_config.parse_phase_resistance('Ohm')
828
+ else:
829
+ axis_output_config['config']['motor']['phase_resistance'] = motor_data['phase_resistance']
830
+
831
+ if motor_config.phase_inductance != "":
832
+ axis_output_config['config']['motor']['phase_inductance'] = motor_config.parse_phase_inductance('H')
833
+ else:
834
+ axis_output_config['config']['motor']['phase_inductance'] = motor_data['phase_inductance']
835
+
836
+ if 'calibration_current' in motor_data:
837
+ axis_output_config['config']['motor']['calibration_current'] = motor_data['calibration_current']
838
+
839
+ # TODO: take into account user max current
840
+ # Note: we multiply the motor current limit by two since it's given in "continuous max"
841
+ inv_data = _db.get_product(self.devices[inv_ref.parent.key].board)['inverters'][inv_ref.key]
842
+ axis_output_config['config']['motor']['current_soft_max'] = min(inv_data['max_current'], 2 * motor_data['max_current']) # TODO: inverter max current should be set as a separate config var
843
+ axis_output_config['config']['motor']['current_hard_max'] = 1.5 * min(inv_data['max_current'], 2 * motor_data['max_current'])
844
+
845
+ axis_output_config['config']['motor']['pole_pairs'] = motor_data['pole_pairs']
846
+ axis_output_config['config']['motor']['torque_constant'] = motor_data['torque_constant']
847
+ axis_output_config['config']['motor']['phase_resistance_valid'] = True
848
+ axis_output_config['config']['motor']['phase_inductance_valid'] = True
849
+
850
+ # Configure thermistors
851
+ for net in self.get_nets(NetType.THERMISTOR):
852
+ try:
853
+ motor_ref = net.get_single(EntityType.AXIS_MOTOR)
854
+ except FoundNoneError:
855
+ continue # not connected to any inverter
856
+ except FoundMultipleError:
857
+ issues.append(motor_ref, "Each thermistor must be connected to at most one motor.")
858
+ continue
859
+ try:
860
+ gpio_ref = net.get_single(EntityType.DEVICE_GPIO)
861
+ except FoundNoneError:
862
+ continue # not connected to any inverter
863
+ except FoundMultipleError:
864
+ issues.append(gpio_ref, "Each thermistor must be connected to at most one ODrive IO.")
865
+ continue
866
+
867
+ motor_config = motor_ref.config
868
+ motor_data = {**_db.get_motor(motor_config.db_ref), **motor_config.override}
869
+ # if motor_ref.num != gpio_ref.num:
870
+ # issues.append(motor_ref, f"The motor thermistor must either be disconnected or connected to the thermistor input that corresponds to the same ODrive and axis to which the motor is connected.")
871
+ # continue
872
+
873
+ axis_output_config = output_configs[gpio_ref.parent.key]['axis{}'.format(0)]
874
+
875
+ # axis_output_config = odrv_output_config['axis{}'.format(temp_in_num)]
876
+
877
+ odrv_data = _db.get_product(gpio_ref.parent.config.board)
878
+
879
+ temp_in_data = [temp_data for temp_data in odrv_data['temp_in'] if temp_data['io'] == gpio_ref.key][0]
880
+
881
+ axis_output_config['motor']['motor_thermistor'] = {
882
+ 'config': {
883
+ 'r_ref': motor_data['thermistor_r25'],
884
+ 'beta': motor_data['thermistor_beta'],
885
+ 'temp_limit_lower': motor_data['min_temp'] if 'min_temp' in motor_data else motor_data['max_temp'] - 20,
886
+ 'temp_limit_upper': motor_data['max_temp'],
887
+ 'enabled': True # requires reboot (?)
888
+ }
889
+ }
890
+ # TODO: set corresponding GPIO mode to 3 (or probably should be handled by firmware)
891
+
892
+ # Configure shunt conductance
893
+ for dev_num, dev_config in enumerate(self.devices):
894
+ for inv_num in range(len(_db.get_product(dev_config.board)['inverters'])):
895
+ if len(dev_config.shunt_conductances) > inv_num:
896
+ output_configs[dev_num]['config']['inverter{}'.format(inv_num)]['shunt_conductance'] = dev_config.shunt_conductances[inv_num]
897
+
898
+ # Other configuration
899
+ for dev_num, dev_config in enumerate(self.devices):
900
+ output_config = output_configs[dev_num]
901
+
902
+ # TODO: set vbus voltage trip level based on power supply setting
903
+ # TODO: set dc_max_negative_current based on power supply setting
904
+ for inv_num, aux in enumerate(dev_config.aux):
905
+ if inv_num > len(_db.get_product(dev_config.board)['aux_inverters']):
906
+ continue # TODO: log issue if trying to config invalid aux inv
907
+ if aux.resistance != "":
908
+ output_config['config'][f'brake_resistor{inv_num}']['resistance'] = aux.parse_resistance('Ohm')
909
+ else:
910
+ output_config['config'][f'brake_resistor{inv_num}']['resistance'] = _db.get_brakeR(aux.db_ref)['resistance']
911
+
912
+ # output_config['config'][f'brake_resistor{inv_num}']['enable'] = True # TODO: discuss desired default behavior
913
+
914
+ output_config['config']['dc_max_negative_current'] = -1
915
+
916
+ # fan config
917
+ for id, limits in dev_config.fans.items():
918
+ output_config['config'][id]= {
919
+ 'lower': limits[0],
920
+ 'upper': limits[1],
921
+ 'enabled': True
922
+ }
923
+
924
+ # Configure axes
925
+ for axis_ref in ref.axes():
926
+ axis_config = axis_ref.config
927
+ three_phase_net = Net.union(NetType.THREE_PHASE, *(self.get_net(motor_ref.phases()) for motor_ref in axis_ref.motors()))
928
+
929
+ try:
930
+ inv_ref = three_phase_net.get_single(EntityType.DEVICE_INVERTER)
931
+ except FoundNoneError:
932
+ issues.append(axis_ref, f"Not connected to any inverter.")
933
+ continue
934
+ except FoundMultipleError:
935
+ issues.append(axis_ref, f"Connected to more than one inverters.")
936
+ continue
937
+
938
+ axis_output_config = output_configs[inv_ref.parent.num]['axis{}'.format(inv_ref.key)]
939
+
940
+ if not axis_config.parse_calib_scan_vel('rps') is None:
941
+ axis_output_config['config']['calib_scan_vel'] = axis_config.parse_calib_scan_vel('rps') * motor_data['pole_pairs'] * motor_config.parse_scale()
942
+
943
+ if not axis_config.parse_calib_scan_distance('turns') is None:
944
+ axis_output_config['config']['calib_scan_distance'] = axis_config.parse_calib_scan_distance('turns') * motor_data['pole_pairs'] * motor_config.parse_scale()
945
+
946
+ if not axis_config.parse_calib_scan_range() is None:
947
+ axis_output_config['config']['calib_range'] = axis_config.parse_calib_scan_range()
948
+
949
+ # TODO: check if larger than current limit
950
+ if not axis_config.parse_calib_torque('Nm') is None:
951
+ axis_output_config['config']['calibration_lockin'] = {}
952
+ axis_output_config['config']['calibration_lockin']['current'] = axis_config.parse_calib_torque('Nm') / motor_data['torque_constant']
953
+
954
+ if axis_config.pos_encoder is None:
955
+ # TODO: use sensorless mode
956
+ issues.append(axis_ref, "No position encoder specified")
957
+ else:
958
+ enc_ref = list(axis_ref.encoders())[axis_config.pos_encoder]
959
+ enc_id = encoder_ids[inv_ref.parent.num].get(enc_ref, None)
960
+ if enc_id is None:
961
+ issues.append(axis_ref, f"Load encoder of this axis must be connected to the same odrive as the motor ({output_configs[inv_ref.parent.num]['serial_number']})")
962
+ else:
963
+ axis_output_config['config']['load_encoder'] = enc_id
964
+
965
+ if axis_config.commutation_encoder is None:
966
+ # TODO: use sensorless mode
967
+ issues.append(axis_ref, "No commutation encoder specified")
968
+ else:
969
+ enc_ref = list(axis_ref.encoders())[axis_config.commutation_encoder]
970
+ enc_id = encoder_ids[inv_ref.parent.num].get(enc_ref, None)
971
+ if enc_id is None:
972
+ issues.append(axis_ref, f"Commutation encoder of this axis must be connected to the same odrive as the motor ({output_configs[inv_rev.parent.num]['serial_number']})")
973
+ else:
974
+ axis_output_config['config']['commutation_encoder'] = enc_id
975
+
976
+ if axis_config.vel_encoder is None:
977
+ # TODO: use sensorless mode
978
+ issues.append(axis_ref, "No commutation encoder specified")
979
+ elif axis_config.vel_encoder == axis_config.commutation_encoder:
980
+ axis_output_config['controller']['config']['use_commutation_vel'] = False
981
+ elif axis_config.vel_encoder == axis_config.pos_encoder:
982
+ axis_output_config['controller']['config']['use_commutation_vel'] = True
983
+ else:
984
+ issues.append(axis_ref, "The velocity encoder must be the same as either the position encoder or the commutation encoder.")
985
+
986
+ if axis_config.can_protocol == "":
987
+ pass
988
+ elif axis_config.can_protocol == "simple":
989
+ output_configs[inv_ref.parent.num]['can'] = {"config": {"protocol": Protocol.SIMPLE}}
990
+ axis_output_config['config']['can']['node_id'] = int(axis_config.can_node_id)
991
+ else:
992
+ issues.append(axis_ref, f"Unknown CAN protocol {axis_config.can_protocol}.")
993
+
994
+ #if enc_id in [EncoderId.INC_ENCODER0, EncoderId.INC_ENCODER1]:
995
+ axis = None if odrv_list[inv_ref.parent.num] is None else getattr(odrv_list[inv_ref.parent.num], 'axis{}'.format(inv_ref.key))
996
+ axis_obj[axis_ref.num] = Axis(axis, axis_config)
997
+
998
+ def strip_empty_fields(d):
999
+ result = {}
1000
+ for k, v in d.items():
1001
+ if isinstance(v, dict):
1002
+ v = strip_empty_fields(v)
1003
+ if v == {}:
1004
+ continue
1005
+ result[k] = v
1006
+ return result
1007
+
1008
+ output_configs = [strip_empty_fields(cfg) for cfg in output_configs]
1009
+
1010
+ def compare(path, obj, config):
1011
+ all_equal = True
1012
+ reboot_required = False
1013
+
1014
+ for k, v in config.items():
1015
+ if isinstance(v, dict):
1016
+ equal, sub_reboot = compare(path + [k], getattr(obj, k), v)
1017
+ reboot_required = reboot_required or sub_reboot
1018
+ elif isinstance(v, float):
1019
+ # TODO: this comparison is a bit fragile (shouldn't compare floats like this)
1020
+ equal = getattr(obj, k) == struct.unpack("f", struct.pack("f", v))[0]
1021
+ elif isinstance(v, enum.Enum):
1022
+ equal = v.value == getattr(obj, k)
1023
+ else:
1024
+ equal = getattr(obj, k) == v
1025
+
1026
+ all_equal = all_equal and equal
1027
+ if not equal:
1028
+ name = '.'.join(path + [k])
1029
+ #print("changed: ", name)
1030
+ if any(re.match(r, name) for r in _reboot_vars):
1031
+ reboot_required = True
1032
+ return all_equal, reboot_required
1033
+
1034
+ needs_reboot = [False] * len(odrv_list)
1035
+ for dev_ref in ref.devices():
1036
+ odrv = odrv_list[dev_ref.num]
1037
+ if not odrv is None:
1038
+ all_equal, needs_reboot[dev_ref.num] = compare([], odrv, output_configs[dev_ref.num])
1039
+ if not all_equal:
1040
+ issues.append(dev_ref, "Configuration needs to be committed to ODrive", IssueType.WARN)
1041
+ needs_reboot[dev_ref.num] = needs_reboot[dev_ref.num] or odrv.reboot_required
1042
+ if needs_reboot[dev_ref.num]:
1043
+ issues.append(dev_ref, "Reboot required", IssueType.WARN)
1044
+
1045
+ return odrv_list, output_configs, issues, needs_reboot, axis_obj
1046
+ #
1047
+
1048
+ def format_status(self, odrives) -> RichText:
1049
+ """
1050
+ Returns a status summary of the configuration in a human readable format.
1051
+ This includes warnings about any issues with the configuration, calibration
1052
+ status and more.
1053
+ """
1054
+
1055
+ odrv_list, output_configs, issues, needs_reboot, axis_obj = self.get_status(odrives)
1056
+
1057
+ lines = []
1058
+
1059
+ check_sign = "\u2705"
1060
+ info_sign = "\U0001F4A1"
1061
+ warning_sign = "\u26A0\uFE0F "
1062
+ error_sign = "\u274C"
1063
+ question_sign = " " # TODO
1064
+
1065
+ sign = {
1066
+ IssueType.WARN: warning_sign,
1067
+ IssueType.ERROR: error_sign
1068
+ }
1069
+
1070
+ style = {
1071
+ IssueType.WARN: (Color.YELLOW, Color.DEFAULT, Style.BOLD),
1072
+ IssueType.ERROR: (Color.RED, Color.DEFAULT, Style.BOLD)
1073
+ }
1074
+
1075
+ ref = MachineRef(self)
1076
+
1077
+ for message, level in issues.get(ref):
1078
+ lines.append(sign[level] + " " + RichText(message, *style[level]))
1079
+
1080
+ for dev_ref in ref.devices():
1081
+ dev_config = dev_ref.config
1082
+ if not dev_config.serial_number is None:
1083
+ name = "ODrive with serial number " + str(dev_config.serial_number)
1084
+ else:
1085
+ name = "ODrive {} (unspecified serial number)".format(dev_ref.num)
1086
+ lines.append(name)
1087
+ for message, level in issues.get(dev_ref):
1088
+ lines.append(" " + sign[level] + " " + RichText(message, *style[level]))
1089
+
1090
+ for axis_ref in ref.axes():
1091
+ axis_config = axis_ref.config
1092
+ lines.append("Axis " + str(axis_ref.num))
1093
+ for message, level in issues.get(axis_ref):
1094
+ lines.append(" " + sign[level] + " " + RichText(message, *style[level]))
1095
+ for motor_ref in axis_ref.motors():
1096
+ for message, level in issues.get(motor_ref):
1097
+ lines.append(" " + sign[level] + " Motor " + str(motor_ref.num) + ": " + RichText(message, *style[level]))
1098
+ for enc_ref in axis_ref.encoders():
1099
+ for message, level in issues.get(enc_ref):
1100
+ lines.append(" " + sign[level] + " Encoder " + str(enc_ref.num) + ": " + RichText(message, *style[level]))
1101
+
1102
+
1103
+ # TODO: the calibration status is not really meaningful if there is
1104
+ # uncommitted configuration. needs_reboot is not the correct thing
1105
+ # to check but it's close enough for now
1106
+ if any(needs_reboot):
1107
+ continue
1108
+
1109
+ for calib, calib_status in axis_obj[axis_ref.num].closed_loop_control.prerequisites:
1110
+ if calib_status == CalibrationStatus.OK:
1111
+ lines.append(" " + check_sign + " " + RichText(str(calib.name) + " ok", Color.GREEN))
1112
+ elif calib_status == CalibrationStatus.RECOMMENDED:
1113
+ lines.append(" " + info_sign + " " + RichText(str(calib.name) + " recommended"))
1114
+ elif calib_status == CalibrationStatus.NEEDED:
1115
+ lines.append(" " + warning_sign + " " + RichText(str(calib.name) + " needed", style=Style.BOLD))
1116
+ elif calib_status == CalibrationStatus.UNKNOWN:
1117
+ lines.append(" " + question_sign + " " + RichText(str(calib.name) + ": unknown status", Color.GRAY))
1118
+ else:
1119
+ assert(False)
1120
+ for issue in calib.issues:
1121
+ lines.append(" " + RichText(str(calib.name) + ": " + issue, Color.RED))
1122
+
1123
+ return RichText("\n").join(lines)
1124
+
1125
+
1126
+ def apply(self, odrives):
1127
+ """
1128
+ Commits the configuration to the odrives. A reboot may be needed after
1129
+ this.
1130
+
1131
+ If there a are problems with the configuration this function throws an
1132
+ exception and does not change anything on any ODrive.
1133
+ In this case show_status() can be used to get more detailed error
1134
+ information.
1135
+
1136
+ Returns a list of devices that need a reboot before the configuration
1137
+ takes effect.
1138
+ """
1139
+
1140
+ odrv_list, output_configs, issues, needs_reboot, axis_obj = self.get_status(odrives)
1141
+
1142
+ if any([m for _, m, level in issues.issues if level == IssueType.ERROR]):
1143
+ print([m for _, m, level in issues.issues if level == IssueType.ERROR])
1144
+ raise Exception("There are problems with this configuration. No changes were applied to the ODrive(s).")
1145
+
1146
+ def _apply(obj, config):
1147
+ for k, v in config.items():
1148
+ if isinstance(v, dict):
1149
+ _apply(getattr(obj, k), v)
1150
+ else:
1151
+ setattr(obj, k, v)
1152
+
1153
+ for odrv_num, odrv in enumerate(odrv_list):
1154
+ if not odrv is None:
1155
+ if needs_reboot[odrv_num]:
1156
+ odrv.reboot_required = True
1157
+ _apply(odrv, output_configs[odrv_num])
1158
+
1159
+ return [odrv_list[num] for num, r in enumerate(needs_reboot) if r]
1160
+
1161
+ def calibrate(self, odrives, include_optional = True):
1162
+ """
1163
+ Runs the calibration tasks for this machine configuration based on the
1164
+ current state of the ODrives. This can include a reboot of one or more
1165
+ ODrives.
1166
+ """
1167
+
1168
+ odrv_list, output_configs, issues, needs_reboot, axis_obj = self.get_status(odrives)
1169
+
1170
+ if any(needs_reboot):
1171
+ raise Exception("Some devices need to be rebooted for the configuration to take effect.")
1172
+
1173
+ for axis_num, axis in enumerate(axis_obj):
1174
+ for calib, calib_status in axis.closed_loop_control.prerequisites:
1175
+ if (calib_status == CalibrationStatus.RECOMMENDED and include_optional) or calib_status == CalibrationStatus.NEEDED:
1176
+ issues = list(calib.issues)
1177
+ if len(issues):
1178
+ raise Exception(f"Can't run {calib.name}: {issues}")
1179
+ print(f"Running {calib.name} on axis {axis_num}...")
1180
+ calib.run()
1181
+ print("Done!")
1182
+
1183
+ class LoadConfigException(Exception):
1184
+ pass
1185
+
1186
+ def _load_py_obj(data, py_type):
1187
+ if py_type == str:
1188
+ if isinstance(data, str):
1189
+ return data
1190
+ else:
1191
+ raise LoadConfigException(f"expected str but got {data}")
1192
+
1193
+ elif py_type == bool:
1194
+ if isinstance(data, bool):
1195
+ return data
1196
+ else:
1197
+ raise LoadConfigException(f"expected bool but got {data}")
1198
+
1199
+ elif py_type == int:
1200
+ if isinstance(data, int):
1201
+ return data
1202
+ else:
1203
+ raise LoadConfigException(f"expected int but got {data}")
1204
+
1205
+ elif (hasattr(py_type, '_name') and py_type._name == 'List') or (hasattr(py_type, '__name__') and py_type.__name__ == 'List'): # __name__ check is for Python 3.6 compatibility
1206
+ elem_type = py_type.__args__[0]
1207
+ if isinstance(data, list):
1208
+ return [_load_py_obj(v, elem_type) for v in data if not v is None]
1209
+ else:
1210
+ raise LoadConfigException(f"expected list but got {data}")
1211
+
1212
+ elif hasattr(py_type, 'from_json'):
1213
+ return py_type.from_json(data)
1214
+
1215
+ elif (hasattr(py_type, '_name') and py_type._name == 'Dict') or (hasattr(py_type, '__name__') and py_type.__name__ == 'Dict'): # __name__ check is for Python 3.6 compatibility
1216
+ elem_type = py_type.__args__[1]
1217
+ if isinstance(data, dict):
1218
+ return {
1219
+ i: _load_py_obj(v, elem_type)
1220
+ for i,v in data.items() if not v is None}
1221
+ else:
1222
+ raise LoadConfigException(f"expected dict but got {data}")
1223
+
1224
+ elif hasattr(py_type, '__name__') and py_type.__name__ in ('T', 'KT', 'VT'):
1225
+ return data
1226
+
1227
+ else:
1228
+ # This is not the fault of the user config but of our own code
1229
+ raise Exception(f"Don't know how to decode JSON to {py_type.__name__}")
1230
+
1231
+ def _load_named_tuple(data, py_type, **kwargs):
1232
+ assert isinstance(data, dict)
1233
+
1234
+ unsupported_keys = set(data.keys()) - (set(py_type.__annotations__.keys()) - set(kwargs.keys()))
1235
+ if len(unsupported_keys) > 0:
1236
+ raise Exception(f"Unexpected keys for {py_type.__name__}: {unsupported_keys}")
1237
+
1238
+ init_dict = {}
1239
+ for field_name, field_type, field_default in py_type._fields:
1240
+ field_val = data.get(field_name, None)
1241
+ if field_val is None:
1242
+ continue
1243
+ init_dict[field_name] = _load_py_obj(field_val, field_type)
1244
+
1245
+ return py_type(**init_dict, **kwargs)
1246
+
1247
+
1248
+ def _is_default(val, default):
1249
+ if val == default:
1250
+ return True
1251
+ elif type(val) == str:
1252
+ return val.strip() == default
1253
+ return False
1254
+
1255
+ def _dump_py_obj(py_obj):
1256
+ if py_obj is None:
1257
+ return None
1258
+ elif type(py_obj) == str:
1259
+ return None if py_obj.strip() == "" else py_obj.strip()
1260
+ elif type(py_obj) == int:
1261
+ return py_obj
1262
+ elif type(py_obj) == float:
1263
+ return py_obj
1264
+ elif type(py_obj) == bool:
1265
+ return py_obj
1266
+ elif type(py_obj) == list:
1267
+ return None if (len(py_obj) == 0) else [_dump_py_obj(o) for o in py_obj]
1268
+ elif type(py_obj) == dict:
1269
+ return None if (len(py_obj) == 0) else {i:_dump_py_obj(o) for i,o in py_obj.items()}
1270
+ elif hasattr(py_obj, 'to_json'):
1271
+ return py_obj.to_json()
1272
+ else:
1273
+ raise Exception(f"Don't know how to dump object {py_obj}")
1274
+
1275
+ def _dump_named_tuple(py_obj):
1276
+ data = {}
1277
+ for k, _, d in py_obj.__class__._fields:
1278
+ subdata = getattr(py_obj, k)
1279
+ if _is_default(subdata, d):
1280
+ continue # skip
1281
+ data[k] = _dump_py_obj(subdata)
1282
+ return data
1283
+
1284
+
1285
+ def _run_state(axis, state):
1286
+ axis.requested_state = state
1287
+ while AxisState(axis.requested_state) == state:
1288
+ time.sleep(0.1)
1289
+ while ProcedureResult(axis.procedure_result) == ProcedureResult.BUSY:
1290
+ time.sleep(0.1)
1291
+
1292
+ result = ProcedureResult(axis.procedure_result)
1293
+ if result == ProcedureResult.DISARMED:
1294
+ raise Exception("Device failed with {}".format(repr(ODriveError(axis.disarm_reason))))
1295
+ elif result != ProcedureResult.SUCCESS:
1296
+ raise Exception("Device returned {}".format(repr(ProcedureResult(result))))