From 7b78e751a6ad9a792f5e06a66e06a983fa90bcac Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Sun, 5 Jul 2026 20:49:02 +0200 Subject: [PATCH] fix(calibration): detect inverted gripper drive_mode on SO follower/leader calibration The SO follower/leader calibration hardcoded drive_mode=0 for all motors. When the gripper motor is mounted mirrored relative to the other arm's (raw position increases when closing instead of decreasing), the 0=closed/100=open normalization convention flips: a 'closed' (0%) command from the leader unnormalizes to the follower's fully-open hard stop, slamming the gripper open and triggering the Feetech overload protection (torque off until power cycle). FeetechMotorsBus already supports per-motor inversion via drive_mode (apply_drive_mode=True); calibration just never set it. This adds a step after range recording that asks the user to fully close the gripper, reads the raw position, and sets drive_mode=1 when the closed position sits at range_max. No calibration file format change. Verified against the exact calibration values reported in #3942 using the real FeetechMotorsBus normalization: with drive_mode=0 a 0% goal maps to raw 2035 (open, bug reproduced); with drive_mode=1 it maps to raw 3528 (closed, expected). Fixes #3942 --- src/lerobot/robots/so_follower/so_follower.py | 13 ++++++- .../teleoperators/so_leader/so_leader.py | 13 ++++++- tests/robots/test_so100_follower.py | 35 +++++++++++++++++++ 3 files changed, 59 insertions(+), 2 deletions(-) diff --git a/src/lerobot/robots/so_follower/so_follower.py b/src/lerobot/robots/so_follower/so_follower.py index 4b477e75c..f29ab2f06 100644 --- a/src/lerobot/robots/so_follower/so_follower.py +++ b/src/lerobot/robots/so_follower/so_follower.py @@ -142,11 +142,22 @@ class SOFollower(Robot): range_mins[full_turn_motor] = 0 range_maxes[full_turn_motor] = 4095 + 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. + 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(): self.calibration[motor] = MotorCalibration( id=m.id, - drive_mode=0, + drive_mode=drive_modes[motor], homing_offset=homing_offsets[motor], range_min=range_mins[motor], range_max=range_maxes[motor], diff --git a/src/lerobot/teleoperators/so_leader/so_leader.py b/src/lerobot/teleoperators/so_leader/so_leader.py index 99f0ee403..714077405 100644 --- a/src/lerobot/teleoperators/so_leader/so_leader.py +++ b/src/lerobot/teleoperators/so_leader/so_leader.py @@ -110,11 +110,22 @@ class SOLeader(Teleoperator): range_mins[full_turn_motor] = 0 range_maxes[full_turn_motor] = 4095 + 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. + 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(): self.calibration[motor] = MotorCalibration( id=m.id, - drive_mode=0, + drive_mode=drive_modes[motor], homing_offset=homing_offsets[motor], range_min=range_mins[motor], range_max=range_maxes[motor], diff --git a/tests/robots/test_so100_follower.py b/tests/robots/test_so100_follower.py index 56706dc24..fe2fab2c7 100644 --- a/tests/robots/test_so100_follower.py +++ b/tests/robots/test_so100_follower.py @@ -149,3 +149,38 @@ def test_configure_writes_position_pid_coefficients(): bus_mock.write.assert_any_call("P_Coefficient", "shoulder_pan", 32) bus_mock.write.assert_any_call("I_Coefficient", "shoulder_pan", 1) bus_mock.write.assert_any_call("D_Coefficient", "shoulder_pan", 16) + + +@pytest.mark.parametrize( + "gripper_closed_pos, expected_drive_mode", + [ + (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 + ], +) +def test_calibrate_detects_gripper_drive_mode(follower, gripper_closed_pos, expected_drive_mode): + """Regression test for #3942: the follower gripper can be mounted mirrored with respect to the + leader's, in which case its raw position increases when closing. Calibration must detect this + and set drive_mode=1 so that normalized values follow the 0=closed/100=open convention.""" + follower.connect() + + motors = list(follower.bus.motors) + follower.bus.set_half_turn_homings.return_value = dict.fromkeys(motors, 0) + follower.bus.record_ranges_of_motion.return_value = ( + dict.fromkeys(motors, 2035), + dict.fromkeys(motors, 3528), + ) + follower.bus.read.return_value = gripper_closed_pos + + with ( + patch("builtins.input", return_value=""), + patch.object(type(follower), "_save_calibration", lambda self: None), + ): + follower.calibration = {} + follower.calibrate() + + 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": + assert follower.calibration[motor].drive_mode == 0