Reads positions and velocities.
| 529 | return self._vel_data.copy() |
| 530 | |
| 531 | class DynamixelCurReader(DynamixelReader): |
| 532 | """Reads positions and velocities.""" |
| 533 | |
| 534 | def __init__(self, |
| 535 | client: DynamixelClient, |
| 536 | motor_ids: Sequence[int], |
| 537 | pos_scale: float = 1.0, |
| 538 | vel_scale: float = 1.0, |
| 539 | cur_scale: float = 1.0): |
| 540 | super().__init__( |
| 541 | client, |
| 542 | motor_ids, |
| 543 | address=ADDR_PRESENT_POS_VEL_CUR, |
| 544 | size=LEN_PRESENT_POS_VEL_CUR, |
| 545 | ) |
| 546 | self.cur_scale = cur_scale |
| 547 | |
| 548 | def _initialize_data(self): |
| 549 | """Initializes the cached data.""" |
| 550 | self._cur_data = np.zeros(len(self.motor_ids), dtype=np.float32) |
| 551 | |
| 552 | def _update_data(self, index: int, motor_id: int): |
| 553 | """Updates the data index for the given motor ID.""" |
| 554 | cur = self.operation.getData(motor_id, ADDR_PRESENT_CURRENT, |
| 555 | LEN_PRESENT_CURRENT) |
| 556 | cur = unsigned_to_signed(cur, size=2) |
| 557 | self._cur_data[index] = float(cur) * self.cur_scale |
| 558 | |
| 559 | def _get_data(self): |
| 560 | """Returns a copy of the data.""" |
| 561 | return self._cur_data.copy() |
| 562 | |
| 563 | |
| 564 | # Register global cleanup function. |