chore(robots): drive mode calibration gripper

src/lerobot/robots/so_follower/so_follower.py

@@ -144,14 +144,17 @@ class SOFollower(Robot):
 
         drive_modes = dict.fromkeys(self.bus.motors, 0)
         input(f"Fully close the gripper of {self} and press ENTER....")
-        gripper_closed_pos = self.bus.read("Present_Position", "gripper", normalize=False)
-        if abs(gripper_closed_pos - range_maxes["gripper"]) < abs(gripper_closed_pos - range_mins["gripper"]):
-            # The closed position is at the top of the recorded range, meaning the raw position increases
-            # when the gripper closes. This is inverted with respect to the expected convention
-            # (0 = closed, 100 = open), which happens when the gripper motor is mounted mirrored.
-            # Invert it in software via drive_mode so both arms share the same convention.
+        gripper_closed_pos = self.bus.read(
+            "Present_Position", "gripper", normalize=False, num_retry=self.config.num_read_retries
+        )
+        distance_to_min = abs(gripper_closed_pos - range_mins["gripper"])
+        distance_to_max = abs(gripper_closed_pos - range_maxes["gripper"])
+        if min(distance_to_min, distance_to_max) > (range_maxes["gripper"] - range_mins["gripper"]) * 0.2:
+            raise ValueError("Gripper is not fully closed. Run calibration again.")
+
+        drive_modes["gripper"] = int(distance_to_max < distance_to_min)
+        if drive_modes["gripper"]:
             logger.info("Gripper motor is inverted, setting drive_mode=1 to compensate.")
-            drive_modes["gripper"] = 1
 
         self.calibration = {}
         for motor, m in self.bus.motors.items():

src/lerobot/teleoperators/so_leader/so_leader.py

@@ -112,14 +112,17 @@ class SOLeader(Teleoperator):
 
         drive_modes = dict.fromkeys(self.bus.motors, 0)
         input(f"Fully close the gripper of {self} and press ENTER....")
-        gripper_closed_pos = self.bus.read("Present_Position", "gripper", normalize=False)
-        if abs(gripper_closed_pos - range_maxes["gripper"]) < abs(gripper_closed_pos - range_mins["gripper"]):
-            # The closed position is at the top of the recorded range, meaning the raw position increases
-            # when the gripper closes. This is inverted with respect to the expected convention
-            # (0 = closed, 100 = open), which happens when the gripper motor is mounted mirrored.
-            # Invert it in software via drive_mode so both arms share the same convention.
+        gripper_closed_pos = self.bus.read(
+            "Present_Position", "gripper", normalize=False, num_retry=self.config.num_read_retries
+        )
+        distance_to_min = abs(gripper_closed_pos - range_mins["gripper"])
+        distance_to_max = abs(gripper_closed_pos - range_maxes["gripper"])
+        if min(distance_to_min, distance_to_max) > (range_maxes["gripper"] - range_mins["gripper"]) * 0.2:
+            raise ValueError("Gripper is not fully closed. Run calibration again.")
+
+        drive_modes["gripper"] = int(distance_to_max < distance_to_min)
+        if drive_modes["gripper"]:
             logger.info("Gripper motor is inverted, setting drive_mode=1 to compensate.")
-            drive_modes["gripper"] = 1
 
         self.calibration = {}
         for motor, m in self.bus.motors.items():

tests/robots/test_so100_follower.py

@@ -156,6 +156,7 @@ def test_configure_writes_position_pid_coefficients():
     [
         (2035, 0),  # closed position at range_min -> raw increases when opening -> not inverted
         (3528, 1),  # closed position at range_max -> raw increases when closing -> inverted
+        (2781, None),  # not near either end stop -> unsafe to infer
     ],
 )
 def test_calibrate_detects_gripper_drive_mode(follower, gripper_closed_pos, expected_drive_mode):
@@ -177,9 +178,21 @@ def test_calibrate_detects_gripper_drive_mode(follower, gripper_closed_pos, expe
         patch.object(type(follower), "_save_calibration", lambda self: None),
     ):
         follower.calibration = {}
-        follower.calibrate()
+        if expected_drive_mode is None:
+            with pytest.raises(ValueError, match="Gripper is not fully closed"):
+                follower.calibrate()
+        else:
+            follower.calibrate()
+
+    follower.bus.read.assert_called_with(
+        "Present_Position",
+        "gripper",
+        normalize=False,
+        num_retry=follower.config.num_read_retries,
+    )
+    if expected_drive_mode is None:
+        return
 
-    follower.bus.read.assert_called_with("Present_Position", "gripper", normalize=False)
     assert follower.calibration["gripper"].drive_mode == expected_drive_mode
     for motor in motors:
         if motor != "gripper":