Reads positions and velocities.
| 495 | return self._pos_data.copy() |
| 496 | |
| 497 | class DynamixelVelReader(DynamixelReader): |
| 498 | """Reads positions and velocities.""" |
| 499 | |
| 500 | def __init__(self, |
| 501 | client: DynamixelClient, |
| 502 | motor_ids: Sequence[int], |
| 503 | pos_scale: float = 1.0, |
| 504 | vel_scale: float = 1.0, |
| 505 | cur_scale: float = 1.0): |
| 506 | super().__init__( |
| 507 | client, |
| 508 | motor_ids, |
| 509 | address=ADDR_PRESENT_POS_VEL_CUR, |
| 510 | size=LEN_PRESENT_POS_VEL_CUR, |
| 511 | ) |
| 512 | self.pos_scale = pos_scale |
| 513 | self.vel_scale = vel_scale |
| 514 | self.cur_scale = cur_scale |
| 515 | |
| 516 | def _initialize_data(self): |
| 517 | """Initializes the cached data.""" |
| 518 | self._vel_data = np.zeros(len(self.motor_ids), dtype=np.float32) |
| 519 | |
| 520 | def _update_data(self, index: int, motor_id: int): |
| 521 | """Updates the data index for the given motor ID.""" |
| 522 | vel = self.operation.getData(motor_id, ADDR_PRESENT_VELOCITY, |
| 523 | LEN_PRESENT_VELOCITY) |
| 524 | vel = unsigned_to_signed(vel, size=4) |
| 525 | self._vel_data[index] = float(vel) * self.vel_scale |
| 526 | |
| 527 | def _get_data(self): |
| 528 | """Returns a copy of the data.""" |
| 529 | return self._vel_data.copy() |
| 530 | |
| 531 | class DynamixelCurReader(DynamixelReader): |
| 532 | """Reads positions and velocities.""" |