diff --git a/src/lerobot/robots/so_follower/so_follower.py b/src/lerobot/robots/so_follower/so_follower.py index f29ab2f06..2d4c7af60 100644 --- a/src/lerobot/robots/so_follower/so_follower.py +++ b/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(): diff --git a/src/lerobot/teleoperators/so_leader/so_leader.py b/src/lerobot/teleoperators/so_leader/so_leader.py index 714077405..d9c080ef8 100644 --- a/src/lerobot/teleoperators/so_leader/so_leader.py +++ b/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(): diff --git a/tests/robots/test_so100_follower.py b/tests/robots/test_so100_follower.py index fe2fab2c7..38bc5e164 100644 --- a/tests/robots/test_so100_follower.py +++ b/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":