Reads positions and velocities.
| 463 | |
| 464 | |
| 465 | class DynamixelPosReader(DynamixelReader): |
| 466 | """Reads positions and velocities.""" |
| 467 | |
| 468 | def __init__(self, |
| 469 | client: DynamixelClient, |
| 470 | motor_ids: Sequence[int], |
| 471 | pos_scale: float = 1.0, |
| 472 | vel_scale: float = 1.0, |
| 473 | cur_scale: float = 1.0): |
| 474 | super().__init__( |
| 475 | client, |
| 476 | motor_ids, |
| 477 | address=ADDR_PRESENT_POS_VEL_CUR, |
| 478 | size=LEN_PRESENT_POS_VEL_CUR, |
| 479 | ) |
| 480 | self.pos_scale = pos_scale |
| 481 | |
| 482 | def _initialize_data(self): |
| 483 | """Initializes the cached data.""" |
| 484 | self._pos_data = np.zeros(len(self.motor_ids), dtype=np.float32) |
| 485 | |
| 486 | def _update_data(self, index: int, motor_id: int): |
| 487 | """Updates the data index for the given motor ID.""" |
| 488 | pos = self.operation.getData(motor_id, ADDR_PRESENT_POSITION, |
| 489 | LEN_PRESENT_POSITION) |
| 490 | pos = unsigned_to_signed(pos, size=4) |
| 491 | self._pos_data[index] = float(pos) * self.pos_scale |
| 492 | |
| 493 | def _get_data(self): |
| 494 | """Returns a copy of the data.""" |
| 495 | return self._pos_data.copy() |
| 496 | |
| 497 | class DynamixelVelReader(DynamixelReader): |
| 498 | """Reads positions and velocities.""" |