From 90643c0d6490aaa8a998c4750c0171a9789f39e7 Mon Sep 17 00:00:00 2001 From: cwzhong627-source Date: Wed, 8 Apr 2026 21:03:36 +0800 Subject: [PATCH 1/2] fix: calibrate wrist_roll based on current position with symmetric range --- src/lerobot/robots/so_follower/so_follower.py | 8 ++++++-- src/lerobot/teleoperators/so_leader/so_leader.py | 8 ++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/lerobot/robots/so_follower/so_follower.py b/src/lerobot/robots/so_follower/so_follower.py index ca132d1022..e3454e2b1d 100644 --- a/src/lerobot/robots/so_follower/so_follower.py +++ b/src/lerobot/robots/so_follower/so_follower.py @@ -134,9 +134,13 @@ def calibrate(self) -> None: f"Move all joints except '{full_turn_motor}' sequentially through their " "entire ranges of motion.\nRecording positions. Press ENTER to stop..." ) + current_pos = self.bus.read("Present_Position", full_turn_motor, normalize=False) + if isinstance(current_pos, dict): + current_pos = current_pos[full_turn_motor] range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors) - range_mins[full_turn_motor] = 0 - range_maxes[full_turn_motor] = 4095 + safe_delta = min(current_pos - 0, 4095 - current_pos, 2048) + range_mins[full_turn_motor] = current_pos - safe_delta + range_maxes[full_turn_motor] = current_pos + safe_delta 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 04ce0f21f8..351da776d5 100644 --- a/src/lerobot/teleoperators/so_leader/so_leader.py +++ b/src/lerobot/teleoperators/so_leader/so_leader.py @@ -106,9 +106,13 @@ def calibrate(self) -> None: f"Move all joints except '{full_turn_motor}' sequentially through their " "entire ranges of motion.\nRecording positions. Press ENTER to stop..." ) + current_pos = self.bus.read("Present_Position", full_turn_motor, normalize=False) + if isinstance(current_pos, dict): + current_pos = current_pos[full_turn_motor] range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors) - range_mins[full_turn_motor] = 0 - range_maxes[full_turn_motor] = 4095 + safe_delta = min(current_pos - 0, 4095 - current_pos, 2048) + range_mins[full_turn_motor] = current_pos - safe_delta + range_maxes[full_turn_motor] = current_pos + safe_delta self.calibration = {} for motor, m in self.bus.motors.items(): From acc84afc3dd4aa456e9918aa5a62d5a40ae36bc6 Mon Sep 17 00:00:00 2001 From: cwzhong627-source Date: Thu, 9 Apr 2026 12:42:53 +0800 Subject: [PATCH 2/2] fix: exclude wrist_roll from homing to avoid offset overflow (>2047) --- src/lerobot/robots/so_follower/so_follower.py | 16 ++++++++++------ src/lerobot/teleoperators/so_leader/so_leader.py | 15 ++++++++++----- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/lerobot/robots/so_follower/so_follower.py b/src/lerobot/robots/so_follower/so_follower.py index e3454e2b1d..ada12d539a 100644 --- a/src/lerobot/robots/so_follower/so_follower.py +++ b/src/lerobot/robots/so_follower/so_follower.py @@ -125,18 +125,22 @@ def calibrate(self) -> None: self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) input(f"Move {self} to the middle of its range of motion and press ENTER....") - homing_offsets = self.bus.set_half_turn_homings() - # Attempt to call record_ranges_of_motion with a reduced motor set when appropriate. full_turn_motor = "wrist_roll" - unknown_range_motors = [motor for motor in self.bus.motors if motor != full_turn_motor] + + current_pos = self.bus.read("Present_Position", full_turn_motor, normalize=False) + if isinstance(current_pos, dict): + current_pos = current_pos[full_turn_motor] + + other_motors = [motor for motor in self.bus.motors if motor != full_turn_motor] + homing_offsets = self.bus.set_half_turn_homings(other_motors) + homing_offsets[full_turn_motor] = 0 + + unknown_range_motors = other_motors print( f"Move all joints except '{full_turn_motor}' sequentially through their " "entire ranges of motion.\nRecording positions. Press ENTER to stop..." ) - current_pos = self.bus.read("Present_Position", full_turn_motor, normalize=False) - if isinstance(current_pos, dict): - current_pos = current_pos[full_turn_motor] range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors) safe_delta = min(current_pos - 0, 4095 - current_pos, 2048) range_mins[full_turn_motor] = current_pos - safe_delta diff --git a/src/lerobot/teleoperators/so_leader/so_leader.py b/src/lerobot/teleoperators/so_leader/so_leader.py index 351da776d5..5078d3f3b4 100644 --- a/src/lerobot/teleoperators/so_leader/so_leader.py +++ b/src/lerobot/teleoperators/so_leader/so_leader.py @@ -98,17 +98,22 @@ def calibrate(self) -> None: self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) input(f"Move {self} to the middle of its range of motion and press ENTER....") - homing_offsets = self.bus.set_half_turn_homings() full_turn_motor = "wrist_roll" - unknown_range_motors = [motor for motor in self.bus.motors if motor != full_turn_motor] + + current_pos = self.bus.read("Present_Position", full_turn_motor, normalize=False) + if isinstance(current_pos, dict): + current_pos = current_pos[full_turn_motor] + + other_motors = [motor for motor in self.bus.motors if motor != full_turn_motor] + homing_offsets = self.bus.set_half_turn_homings(other_motors) + homing_offsets[full_turn_motor] = 0 + + unknown_range_motors = other_motors print( f"Move all joints except '{full_turn_motor}' sequentially through their " "entire ranges of motion.\nRecording positions. Press ENTER to stop..." ) - current_pos = self.bus.read("Present_Position", full_turn_motor, normalize=False) - if isinstance(current_pos, dict): - current_pos = current_pos[full_turn_motor] range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors) safe_delta = min(current_pos - 0, 4095 - current_pos, 2048) range_mins[full_turn_motor] = current_pos - safe_delta