mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-30 04:59:44 +00:00
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
This commit is contained in:
committed by
Steven Palma
parent
6e5f6df6e7
commit
7b78e751a6
@@ -142,11 +142,22 @@ class SOFollower(Robot):
|
|||||||
range_mins[full_turn_motor] = 0
|
range_mins[full_turn_motor] = 0
|
||||||
range_maxes[full_turn_motor] = 4095
|
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 = {}
|
self.calibration = {}
|
||||||
for motor, m in self.bus.motors.items():
|
for motor, m in self.bus.motors.items():
|
||||||
self.calibration[motor] = MotorCalibration(
|
self.calibration[motor] = MotorCalibration(
|
||||||
id=m.id,
|
id=m.id,
|
||||||
drive_mode=0,
|
drive_mode=drive_modes[motor],
|
||||||
homing_offset=homing_offsets[motor],
|
homing_offset=homing_offsets[motor],
|
||||||
range_min=range_mins[motor],
|
range_min=range_mins[motor],
|
||||||
range_max=range_maxes[motor],
|
range_max=range_maxes[motor],
|
||||||
|
|||||||
@@ -110,11 +110,22 @@ class SOLeader(Teleoperator):
|
|||||||
range_mins[full_turn_motor] = 0
|
range_mins[full_turn_motor] = 0
|
||||||
range_maxes[full_turn_motor] = 4095
|
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 = {}
|
self.calibration = {}
|
||||||
for motor, m in self.bus.motors.items():
|
for motor, m in self.bus.motors.items():
|
||||||
self.calibration[motor] = MotorCalibration(
|
self.calibration[motor] = MotorCalibration(
|
||||||
id=m.id,
|
id=m.id,
|
||||||
drive_mode=0,
|
drive_mode=drive_modes[motor],
|
||||||
homing_offset=homing_offsets[motor],
|
homing_offset=homing_offsets[motor],
|
||||||
range_min=range_mins[motor],
|
range_min=range_mins[motor],
|
||||||
range_max=range_maxes[motor],
|
range_max=range_maxes[motor],
|
||||||
|
|||||||
@@ -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("P_Coefficient", "shoulder_pan", 32)
|
||||||
bus_mock.write.assert_any_call("I_Coefficient", "shoulder_pan", 1)
|
bus_mock.write.assert_any_call("I_Coefficient", "shoulder_pan", 1)
|
||||||
bus_mock.write.assert_any_call("D_Coefficient", "shoulder_pan", 16)
|
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
|
||||||
|
|||||||
Reference in New Issue
Block a user