From 7d615acf9aef770e66054ce1d38d256e2922448f Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Wed, 29 Jul 2026 15:20:14 +0200 Subject: [PATCH] fix(robots): retry SO follower/leader bus reads on transient Feetech errors (#4207) * fix(robots): retry SO follower/leader bus reads on transient Feetech errors SO-100/SO-101 teleoperation aborts when a sync_read of Present_Position returns a corrupted status packet ("Incorrect status packet!"), which the Feetech bus emits intermittently under load. The read path already supports a num_retry argument but the SO follower and leader never used it, so a single transient failure crashed the control loop. Add a max_read_retry config option (default 3) to SOFollowerConfig and SOLeaderConfig and forward it to every Present_Position sync_read. Retries are immediate and only happen on failure, so the steady-state read cost is unchanged; set max_read_retry=0 to restore the previous behavior. Fixes #3131 * chore(robots): change defaults --------- Co-authored-by: isaka1022 --- .../robots/so_follower/config_so_follower.py | 6 +++++ src/lerobot/robots/so_follower/so_follower.py | 4 +-- .../so_leader/config_so_leader.py | 6 +++++ .../teleoperators/so_leader/so_leader.py | 2 +- tests/motors/test_feetech.py | 13 ++++++++++ tests/robots/test_so100_follower.py | 25 +++++++++++++++++-- 6 files changed, 51 insertions(+), 5 deletions(-) diff --git a/src/lerobot/robots/so_follower/config_so_follower.py b/src/lerobot/robots/so_follower/config_so_follower.py index 45f972490..af4dda782 100644 --- a/src/lerobot/robots/so_follower/config_so_follower.py +++ b/src/lerobot/robots/so_follower/config_so_follower.py @@ -46,6 +46,12 @@ class SOFollowerConfig: position_i_coefficient: int = 0 position_d_coefficient: int = 32 + # Number of extra attempts when a `sync_read` of the motors fails. Feetech buses can occasionally + # return a corrupted status packet ("Incorrect status packet!"), especially when several joints move + # at once, which otherwise aborts the control loop. Retries are immediate (no sleep) and only happen on + # failure, so the steady-state read cost is unchanged. + num_read_retries: int = 2 + @RobotConfig.register_subclass("so101_follower") @RobotConfig.register_subclass("so100_follower") diff --git a/src/lerobot/robots/so_follower/so_follower.py b/src/lerobot/robots/so_follower/so_follower.py index 6d5ad79dc..4b477e75c 100644 --- a/src/lerobot/robots/so_follower/so_follower.py +++ b/src/lerobot/robots/so_follower/so_follower.py @@ -180,7 +180,7 @@ class SOFollower(Robot): def get_observation(self) -> RobotObservation: # Read arm position start = time.perf_counter() - obs_dict = self.bus.sync_read("Present_Position") + obs_dict = self.bus.sync_read("Present_Position", num_retry=self.config.num_read_retries) obs_dict = {f"{motor}.pos": val for motor, val in obs_dict.items()} dt_ms = (time.perf_counter() - start) * 1e3 logger.debug(f"{self} read state: {dt_ms:.1f}ms") @@ -221,7 +221,7 @@ class SOFollower(Robot): # Cap goal position when too far away from present position. # /!\ Slower fps expected due to reading from the follower. if self.config.max_relative_target is not None: - present_pos = self.bus.sync_read("Present_Position") + present_pos = self.bus.sync_read("Present_Position", num_retry=self.config.num_read_retries) goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()} goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) diff --git a/src/lerobot/teleoperators/so_leader/config_so_leader.py b/src/lerobot/teleoperators/so_leader/config_so_leader.py index 189303088..6f3d6cc94 100644 --- a/src/lerobot/teleoperators/so_leader/config_so_leader.py +++ b/src/lerobot/teleoperators/so_leader/config_so_leader.py @@ -29,6 +29,12 @@ class SOLeaderConfig: # Whether to use degrees for angles use_degrees: bool = True + # Number of extra attempts when a `sync_read` of the motors fails. Feetech buses can occasionally + # return a corrupted status packet ("Incorrect status packet!"), especially when several joints move + # at once, which otherwise aborts the teleoperation loop. Retries are immediate (no sleep) and only + # happen on failure, so the steady-state read cost is unchanged. + num_read_retries: int = 2 + @TeleoperatorConfig.register_subclass("so101_leader") @TeleoperatorConfig.register_subclass("so100_leader") diff --git a/src/lerobot/teleoperators/so_leader/so_leader.py b/src/lerobot/teleoperators/so_leader/so_leader.py index 7e731d5ed..99f0ee403 100644 --- a/src/lerobot/teleoperators/so_leader/so_leader.py +++ b/src/lerobot/teleoperators/so_leader/so_leader.py @@ -145,7 +145,7 @@ class SOLeader(Teleoperator): @check_if_not_connected def get_action(self) -> dict[str, float]: start = time.perf_counter() - action = self.bus.sync_read("Present_Position") + action = self.bus.sync_read("Present_Position", num_retry=self.config.num_read_retries) action = {f"{motor}.pos": val for motor, val in action.items()} dt_ms = (time.perf_counter() - start) * 1e3 logger.debug(f"{self} read action: {dt_ms:.1f}ms") diff --git a/tests/motors/test_feetech.py b/tests/motors/test_feetech.py index 6fc9d684e..20caf4080 100644 --- a/tests/motors/test_feetech.py +++ b/tests/motors/test_feetech.py @@ -294,6 +294,19 @@ def test__sync_read(addr, length, ids_values, mock_motors, dummy_motors): assert read_values == ids_values +def test__sync_read_retries_after_transient_failure(mock_motors, dummy_motors): + addr, length, ids_values = (10, 4, {1: 1337}) + stub = mock_motors.build_sync_read_stub(addr, length, ids_values, num_invalid_try=1) + bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) + bus.connect(handshake=False) + + read_values, read_comm = bus._sync_read(addr, length, list(ids_values), num_retry=1) + + assert read_comm == scs.COMM_SUCCESS + assert read_values == ids_values + assert mock_motors.stubs[stub].calls == 2 + + @pytest.mark.parametrize("raise_on_error", (True, False)) def test__sync_read_comm(raise_on_error, mock_motors, dummy_motors): addr, length, ids_values = (10, 4, {1: 1337}) diff --git a/tests/robots/test_so100_follower.py b/tests/robots/test_so100_follower.py index 694dd7da6..56706dc24 100644 --- a/tests/robots/test_so100_follower.py +++ b/tests/robots/test_so100_follower.py @@ -49,7 +49,7 @@ def _make_bus_mock() -> MagicMock: @pytest.fixture -def follower(): +def follower(tmp_path): bus_mock = _make_bus_mock() def _bus_side_effect(*_args, **kwargs): @@ -71,7 +71,7 @@ def follower(): ), patch.object(SO100Follower, "configure", lambda self: None), ): - cfg = SO100FollowerConfig(port="/dev/null") + cfg = SO100FollowerConfig(port="/dev/null", calibration_dir=tmp_path) robot = SO100Follower(cfg) yield robot if robot.is_connected: @@ -99,6 +99,27 @@ def test_get_observation(follower): assert obs[f"{motor}.pos"] == idx +def test_get_observation_uses_read_retries(follower): + # Feetech buses can intermittently fail a sync_read; the follower should forward the configured + # retry count so transient failures don't abort the control loop (see #3131). + follower.config.num_read_retries = 7 + follower.connect() + follower.get_observation() + + follower.bus.sync_read.assert_called_once_with("Present_Position", num_retry=7) + + +def test_send_action_uses_read_retries(follower): + follower.config.max_relative_target = 10.0 + follower.config.num_read_retries = 7 + follower.connect() + + action = {f"{motor}.pos": value * 10 for value, motor in enumerate(follower.bus.motors, 1)} + follower.send_action(action) + + follower.bus.sync_read.assert_called_once_with("Present_Position", num_retry=7) + + def test_send_action(follower): follower.connect()