From 845b359d390b4c4f53c0560a8009e6d3ce7b88e1 Mon Sep 17 00:00:00 2001
From: Ryan Pennings <ryanpennings@gmail.com>
Date: Thu, 16 Oct 2025 20:39:05 +1100
Subject: [PATCH] Fix homunculus teleoperator input lag (#2196)

Removes input lag by making changes to the serial
reading loop
- remove serial flush as this only clears
output buffer
- read all data in the input buffer in per loop
and use the latest line as the state to clear
the input buffer
previously was only reading one line per loop,
which in combination with teleoperator script loop
busy_wait function (which is slowing the
_read_loops down) was causing a backlog in input
buffer

Co-authored-by: Martino Russi <77496684+nepyope@users.noreply.github.com>
---
 .../teleoperators/homunculus/homunculus_arm.py        | 11 +++++++++--
 .../teleoperators/homunculus/homunculus_glove.py      | 11 +++++++++--
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/src/lerobot/teleoperators/homunculus/homunculus_arm.py b/src/lerobot/teleoperators/homunculus/homunculus_arm.py
index 21d73de2..43116f5c 100644
--- a/src/lerobot/teleoperators/homunculus/homunculus_arm.py
+++ b/src/lerobot/teleoperators/homunculus/homunculus_arm.py
@@ -270,8 +270,15 @@ class HomunculusArm(Teleoperator):
                 raw_values = None
                 with self.serial_lock:
                     if self.serial.in_waiting > 0:
-                        self.serial.flush()
-                        raw_values = self.serial.readline().decode("utf-8").strip().split(" ")
+                        lines = []
+                        while self.serial.in_waiting > 0:
+                            line = self.serial.read_until().decode("utf-8").strip()
+                            if line:
+                                lines.append(line.split(" "))
+
+                        if lines:
+                            raw_values = lines[-1]
+
                 if raw_values is None or len(raw_values) != 21:  # 16 raw + 5 angle values
                     continue
 
diff --git a/src/lerobot/teleoperators/homunculus/homunculus_glove.py b/src/lerobot/teleoperators/homunculus/homunculus_glove.py
index 251ecf56..fefeec1e 100644
--- a/src/lerobot/teleoperators/homunculus/homunculus_glove.py
+++ b/src/lerobot/teleoperators/homunculus/homunculus_glove.py
@@ -304,8 +304,15 @@ class HomunculusGlove(Teleoperator):
                 positions = None
                 with self.serial_lock:
                     if self.serial.in_waiting > 0:
-                        self.serial.flush()
-                        positions = self.serial.readline().decode("utf-8").strip().split(" ")
+                        lines = []
+                        while self.serial.in_waiting > 0:
+                            line = self.serial.read_until().decode("utf-8").strip()
+                            if line:
+                                lines.append(line.split(" "))
+
+                        if lines:
+                            positions = lines[-1]
+
                 if positions is None or len(positions) != len(self.joints):
                     continue