From e2ee719b733fc63435d90b85d50ab7ed13beecbd Mon Sep 17 00:00:00 2001 From: ZouzouWP Date: Fri, 17 Jul 2026 17:12:16 +0200 Subject: [PATCH] fix: 3D joint view drifting out of sync during long teleoperation sessions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The live 3D arm view in the browser would drift further and further behind the real robot the longer a session ran, and it never recovered on its own — restarting the connection only reset it temporarily. ## Root cause Joint-position frames were pushed onto a FIFO queue and broadcast in order. If the browser tab ever fell even slightly behind the ~30fps producer — a GC pause, a slow render, a backgrounded tab — frames piled up faster than they drained. Every future frame then had to wait behind a growing backlog of stale ones, so the lag only ever grew, never shrank. ```mermaid flowchart LR subgraph before["Before: FIFO queue"] P1["Producer ~30fps"] --> Q["Queue\n(grows if consumer is slow)"] --> C1["Consumer"] end subgraph after["After: latest-wins slot"] P2["Producer ~30fps"] -->|overwrites| S["Single slot\n(always newest)"] --> C2["Consumer\n(never behind)"] end ``` ## Fix - Joint updates now go into a single-slot "latest-wins" buffer instead of the FIFO queue: a new frame overwrites whatever hasn't been sent yet, so a slow consumer skips straight to the most recent pose instead of catching up through a backlog. - Broadcast poll interval tightened from 20fps to 30fps to match the teleoperation loop's own rate. - Added support for a per-arm `urdf_view_zero.json` (a zero pose captured by the user against the URDF's own rest pose), which takes priority over the previous hardcoded correction — that one assumed a canonical calibration and was off by ~200° for this arm. ## Testing Measured 17.5ms median WebSocket-to-render latency with zero drift over a long session (previously drifted continuously and never recovered). --- lelab/server.py | 24 ++++++++++++++++++++-- lelab/teleoperate.py | 47 ++++++++++++++++++++++++++++++++++++-------- 2 files changed, 61 insertions(+), 10 deletions(-) diff --git a/lelab/server.py b/lelab/server.py index 519e0c72..c9b08339 100644 --- a/lelab/server.py +++ b/lelab/server.py @@ -164,6 +164,11 @@ class ConnectionManager: def __init__(self): self.active_connections: list[WebSocket] = [] self.broadcast_queue = queue.Queue() + # Latest-wins slot for live joint frames. Joint updates supersede one + # another, so a slow consumer must never make them pile up in the + # queue — otherwise the 3D view drifts further and further behind the + # real robot as the backlog grows. + self._latest_joint_frame: dict[str, Any] | None = None self.broadcast_thread = None self.is_running = False # Guards `active_connections` since the broadcast worker thread also @@ -220,8 +225,18 @@ def _broadcast_worker(self): try: while self.is_running: try: - # Get data from queue with timeout - data = self.broadcast_queue.get(timeout=0.1) + # Send the freshest joint frame first (latest-wins slot). + # A frame published between the read and the reset below is + # simply picked up on the next iteration. + frame = self._latest_joint_frame + if frame is not None: + self._latest_joint_frame = None + if self.active_connections: + loop.run_until_complete(self._send_to_all_connections(frame)) + + # Get data from queue with a short timeout so joint frames + # are polled frequently enough to stay real-time. + data = self.broadcast_queue.get(timeout=0.02) if data is None: # Poison pill to stop break @@ -258,6 +273,11 @@ async def _send_to_all_connections(self, data: dict[str, Any]): def broadcast_joint_data_sync(self, data: dict[str, Any]): """Thread-safe method to queue data for broadcasting""" if self.is_running and self.active_connections: + if isinstance(data, dict) and data.get("type") == "joint_update": + # Replace any unsent frame instead of queueing: only the most + # recent robot pose is worth sending. + self._latest_joint_frame = data + return try: self.broadcast_queue.put_nowait(data) except queue.Full: diff --git a/lelab/teleoperate.py b/lelab/teleoperate.py index 63416803..5bfccd46 100644 --- a/lelab/teleoperate.py +++ b/lelab/teleoperate.py @@ -47,6 +47,27 @@ "elbow_flex": (+1, 1029), } + +def _load_urdf_view_zero() -> dict[str, dict[str, float]] | None: + """Per-arm 3D-view zero captured by the user at the URDF sleep pose. + + The hardcoded tick corrections above assume a canonical calibration; real + calibrations vary enough that the 3D view can be wildly offset. If + ``calibration/urdf_view_zero.json`` exists (mapping motor name to + ``{"zero_deg": float, "sign": ±1}``), it takes priority: + URDF angle = sign * (raw_normalized_deg - zero_deg). + """ + import json + from pathlib import Path + + path = Path.home() / ".cache" / "huggingface" / "lerobot" / "calibration" / "urdf_view_zero.json" + try: + with open(path, encoding="utf-8") as f: + data = json.load(f) + return {k: v for k, v in data.items() if isinstance(v, dict) and "zero_deg" in v} + except (OSError, ValueError): + return None + # Global variables for teleoperation state teleoperation_active = False teleoperation_thread: threading.Thread | None = None @@ -85,6 +106,12 @@ def get_joint_positions_from_robot(robot) -> dict[str, float]: try: observation = robot.get_observation() calibration = robot.calibration or {} + view_zero = getattr(get_joint_positions_from_robot, "_view_zero_cache", "unset") + if view_zero == "unset": + view_zero = _load_urdf_view_zero() + get_joint_positions_from_robot._view_zero_cache = view_zero + if view_zero: + logger.info("Using user-captured urdf_view_zero.json for the 3D view") joint_positions: dict[str, float] = {} debug_rows = [] @@ -97,13 +124,17 @@ def get_joint_positions_from_robot(robot) -> dict[str, float]: raw_deg = observation[motor_key] angle_degrees = raw_deg - correction = _SO101_URDF_CORRECTIONS.get(motor_name) - if correction is not None and motor_name in calibration: - sign, urdf_zero_ticks = correction - cal = calibration[motor_name] - mid = (cal.range_min + cal.range_max) / 2 - motor_at_urdf_zero = (urdf_zero_ticks - mid) * 360 / _STS3215_MAX_RES - angle_degrees = sign * (raw_deg - motor_at_urdf_zero) + if view_zero and motor_name in view_zero: + entry = view_zero[motor_name] + angle_degrees = float(entry.get("sign", 1)) * (raw_deg - float(entry["zero_deg"])) + else: + correction = _SO101_URDF_CORRECTIONS.get(motor_name) + if correction is not None and motor_name in calibration: + sign, urdf_zero_ticks = correction + cal = calibration[motor_name] + mid = (cal.range_min + cal.range_max) / 2 + motor_at_urdf_zero = (urdf_zero_ticks - mid) * 360 / _STS3215_MAX_RES + angle_degrees = sign * (raw_deg - motor_at_urdf_zero) joint_positions[urdf_joint_name] = angle_degrees * math.pi / 180.0 debug_rows.append( @@ -227,7 +258,7 @@ def teleoperation_worker(): logger.info("Starting teleoperation loop...") try: last_broadcast_time = 0 - broadcast_interval = 0.05 # 20 FPS + broadcast_interval = 1 / 30 # 30 FPS while teleoperation_active: action = teleop_device.get_action()