Move read/write_calibration implementations

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)

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:

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)
+        pass
-        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
 
+    @abc.abstractmethod
     def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
-        for motor, calibration in calibration_dict.items():
+        pass
-            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 reset_calibration(self, motors: NameOrID | list[NameOrID] | None = None) -> None:
         if motors is None:

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): ...