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 <isaka1022@gmail.com>

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")

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)
 

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")

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")

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})

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()