Reads positions and velocities.
| 417 | |
| 418 | |
| 419 | class DynamixelPosVelCurReader(DynamixelReader): |
| 420 | """Reads positions and velocities.""" |
| 421 | |
| 422 | def __init__(self, |
| 423 | client: DynamixelClient, |
| 424 | motor_ids: Sequence[int], |
| 425 | pos_scale: float = 1.0, |
| 426 | vel_scale: float = 1.0, |
| 427 | cur_scale: float = 1.0): |
| 428 | super().__init__( |
| 429 | client, |
| 430 | motor_ids, |
| 431 | address=ADDR_PRESENT_POS_VEL_CUR, |
| 432 | size=LEN_PRESENT_POS_VEL_CUR, |
| 433 | ) |
| 434 | self.pos_scale = pos_scale |
| 435 | self.vel_scale = vel_scale |
| 436 | self.cur_scale = cur_scale |
| 437 | |
| 438 | def _initialize_data(self): |
| 439 | """Initializes the cached data.""" |
| 440 | self._pos_data = np.zeros(len(self.motor_ids), dtype=np.float32) |
| 441 | self._vel_data = np.zeros(len(self.motor_ids), dtype=np.float32) |
| 442 | self._cur_data = np.zeros(len(self.motor_ids), dtype=np.float32) |
| 443 | |
| 444 | def _update_data(self, index: int, motor_id: int): |
| 445 | """Updates the data index for the given motor ID.""" |
| 446 | cur = self.operation.getData(motor_id, ADDR_PRESENT_CURRENT, |
| 447 | LEN_PRESENT_CURRENT) |
| 448 | vel = self.operation.getData(motor_id, ADDR_PRESENT_VELOCITY, |
| 449 | LEN_PRESENT_VELOCITY) |
| 450 | pos = self.operation.getData(motor_id, ADDR_PRESENT_POSITION, |
| 451 | LEN_PRESENT_POSITION) |
| 452 | cur = unsigned_to_signed(cur, size=2) |
| 453 | vel = unsigned_to_signed(vel, size=4) |
| 454 | pos = unsigned_to_signed(pos, size=4) |
| 455 | self._pos_data[index] = float(pos) * self.pos_scale |
| 456 | self._vel_data[index] = float(vel) * self.vel_scale |
| 457 | self._cur_data[index] = float(cur) * self.cur_scale |
| 458 | |
| 459 | def _get_data(self): |
| 460 | """Returns a copy of the data.""" |
| 461 | return (self._pos_data.copy(), self._vel_data.copy(), |
| 462 | self._cur_data.copy()) |
| 463 | |
| 464 | |
| 465 | class DynamixelPosReader(DynamixelReader): |