diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index 52a84e5e..5244705e 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -145,6 +145,32 @@ class DynamixelMotorsBus(MotorsBus): for motor in self.motors: self.write("Return_Delay_Time", motor, 0) + def read_calibration(self) -> dict[str, MotorCalibration]: + offsets = self.sync_read("Homing_Offset", normalize=False) + mins = self.sync_read("Min_Position_Limit", normalize=False) + maxes = self.sync_read("Max_Position_Limit", normalize=False) + drive_modes = self.sync_read("Drive_Mode", normalize=False) + + calibration = {} + for name, motor in self.motors.items(): + calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=drive_modes[name], + homing_offset=offsets[name], + range_min=mins[name], + range_max=maxes[name], + ) + + return calibration + + def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None: + for motor, calibration in calibration_dict.items(): + self.write("Homing_Offset", motor, calibration.homing_offset) + self.write("Min_Position_Limit", motor, calibration.range_min) + self.write("Max_Position_Limit", motor, calibration.range_max) + + self.calibration = calibration_dict + def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None: for name in self._get_motors_list(motors): self.write("Torque_Enable", name, TorqueMode.DISABLED.value, num_retry=num_retry) diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index bcf54972..5d1739a2 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -173,6 +173,43 @@ class FeetechMotorsBus(MotorsBus): self.write("Maximum_Acceleration", motor, 254) self.write("Acceleration", motor, 254) + def read_calibration(self) -> dict[str, MotorCalibration]: + if self.protocol_version == 0: + offsets = self.sync_read("Homing_Offset", normalize=False) + mins = self.sync_read("Min_Position_Limit", normalize=False) + maxes = self.sync_read("Max_Position_Limit", normalize=False) + drive_modes = dict.fromkeys(self.motors, 0) + else: + offsets, mins, maxes, drive_modes = {}, {}, {}, {} + for motor in self.motors: + offsets[motor] = 0 + mins[motor] = self.read("Min_Position_Limit", motor, normalize=False) + maxes[motor] = self.read("Max_Position_Limit", motor, normalize=False) + drive_modes[motor] = 0 + + # TODO(aliberts): add set/get_drive_mode? + + calibration = {} + for name, motor in self.motors.items(): + calibration[name] = MotorCalibration( + id=motor.id, + drive_mode=drive_modes[name], + homing_offset=offsets[name], + range_min=mins[name], + range_max=maxes[name], + ) + + return calibration + + def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None: + for motor, calibration in calibration_dict.items(): + if self.protocol_version == 0: + self.write("Homing_Offset", motor, calibration.homing_offset) + self.write("Min_Position_Limit", motor, calibration.range_min) + self.write("Max_Position_Limit", motor, calibration.range_max) + + self.calibration = calibration_dict + def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]: """ On Feetech Motors: diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index b70a728c..ef2703b5 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -492,35 +492,13 @@ class MotorsBus(abc.ABC): def is_calibrated(self) -> bool: return self.calibration == self.read_calibration() + @abc.abstractmethod def read_calibration(self) -> dict[str, MotorCalibration]: - offsets = self.sync_read("Homing_Offset", normalize=False) - mins = self.sync_read("Min_Position_Limit", normalize=False) - maxes = self.sync_read("Max_Position_Limit", normalize=False) - - try: - drive_modes = self.sync_read("Drive_Mode", normalize=False) - except KeyError: - drive_modes = dict.fromkeys(self.names, 0) - - calibration = {} - for name, motor in self.motors.items(): - calibration[name] = MotorCalibration( - id=motor.id, - drive_mode=drive_modes[name], - homing_offset=offsets[name], - range_min=mins[name], - range_max=maxes[name], - ) - - return calibration + pass + @abc.abstractmethod def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None: - for motor, calibration in calibration_dict.items(): - self.write("Homing_Offset", motor, calibration.homing_offset) - self.write("Min_Position_Limit", motor, calibration.range_min) - self.write("Max_Position_Limit", motor, calibration.range_max) - - self.calibration = calibration_dict + pass def reset_calibration(self, motors: NameOrID | list[NameOrID] | None = None) -> None: if motors is None: diff --git a/tests/motors/test_motors_bus.py b/tests/motors/test_motors_bus.py index 879a8c81..9b2a012e 100644 --- a/tests/motors/test_motors_bus.py +++ b/tests/motors/test_motors_bus.py @@ -133,6 +133,8 @@ class MockMotorsBus(MotorsBus): def _assert_protocol_is_compatible(self, instruction_name): ... def _handshake(self): ... def configure_motors(self): ... + def read_calibration(self): ... + def write_calibration(self, calibration_dict): ... def disable_torque(self, motors): ... def enable_torque(self, motors): ... def _get_half_turn_homings(self, positions): ...