diff --git a/examples/openarms_web_interface/App.css b/examples/openarms_web_interface/App.css index 364e496b5..c056622b5 100644 --- a/examples/openarms_web_interface/App.css +++ b/examples/openarms_web_interface/App.css @@ -433,13 +433,29 @@ button { .status.recording.recording-active .time-display { display: flex; - align-items: center; - gap: 0.75rem; + flex-direction: column; + gap: 0.5rem; font-size: 1.5rem; font-weight: 700; color: white; } +.fps-display { + font-size: 1rem; + font-weight: 500; + opacity: 0.95; +} + +.fps-warning { + color: #fef2f2; + animation: pulse-warning 1s ease-in-out infinite; +} + +@keyframes pulse-warning { + 0%, 100% { opacity: 1; } + 50% { opacity: 0.5; } +} + .status.recording.recording-active .btn-stop { align-self: stretch; } diff --git a/examples/openarms_web_interface/App.jsx b/examples/openarms_web_interface/App.jsx index c02f613e0..ea7b7b3ea 100644 --- a/examples/openarms_web_interface/App.jsx +++ b/examples/openarms_web_interface/App.jsx @@ -13,6 +13,7 @@ function App() { const [robotsReady, setRobotsReady] = useState(false); const [elapsedTime, setElapsedTime] = useState(0); const [currentFps, setCurrentFps] = useState(0); + const [loopFps, setLoopFps] = useState(0); const [episodeCount, setEpisodeCount] = useState(0); const [error, setError] = useState(null); const [statusMessage, setStatusMessage] = useState('Ready'); @@ -71,6 +72,7 @@ function App() { setRobotsReady(data.robots_ready); setElapsedTime(data.elapsed_time); setCurrentFps(data.current_fps || 0); + setLoopFps(data.loop_fps || 0); setEpisodeCount(data.episode_count); setError(data.error); setStatusMessage(data.status_message || 'Ready'); @@ -527,9 +529,14 @@ function App() { {isRecording && rampUpRemaining <= 0 && (
- - {formatTime(elapsedTime)} @ {currentFps.toFixed(1)} FPS - +
+ {formatTime(elapsedTime)} + + Loop: {loopFps.toFixed(1)} Hz + {loopFps > 0 && loopFps < 29 && ⚠️} + + Recording: {currentFps.toFixed(1)} FPS +
diff --git a/examples/openarms_web_interface/web_record_server.py b/examples/openarms_web_interface/web_record_server.py index 3a59927b8..c87ac649e 100644 --- a/examples/openarms_web_interface/web_record_server.py +++ b/examples/openarms_web_interface/web_record_server.py @@ -56,6 +56,7 @@ recording_state = { "status_message": "Ready", "upload_status": None, "current_fps": 0.0, + "loop_fps": 0.0, # Actual control loop FPS (critical - must be 30) "ramp_up_remaining": 0.0, # Remaining seconds for PID ramp-up "recording_started_time": None, # Time when actual recording starts (after ramp-up) "moving_to_zero": False, # Whether robot is moving to zero position @@ -378,6 +379,10 @@ def record_loop_with_compensation(): last_fps_update = loop_start_time fps_frame_count = 0 + # Separate tracking for actual loop FPS (critical for control) + loop_fps_frame_count = 0 + last_loop_fps_update = loop_start_time + # All joints (both arms) all_joints = [] for motor in leader.bus_right.motors: @@ -404,15 +409,28 @@ def record_loop_with_compensation(): # Update ramp-up status for UI recording_state["ramp_up_remaining"] = round(ramp_up_remaining, 2) - # Track if we're falling behind - if so, skip encoding to maintain FPS - # Check if previous loop took too long (indicating we're falling behind) + # CRITICAL: Track actual control loop FPS (must be 30 Hz) + # Calculate every second for accurate monitoring + loop_fps_frame_count += 1 + loop_elapsed = loop_start - last_loop_fps_update + if loop_elapsed >= 1.0: + actual_loop_fps = loop_fps_frame_count / loop_elapsed + recording_state["loop_fps"] = round(actual_loop_fps, 1) + loop_fps_frame_count = 0 + last_loop_fps_update = loop_start + + # Log if we're falling behind + if actual_loop_fps < 29.0: + print(f"[Recording] WARNING: Loop FPS low: {actual_loop_fps:.1f} Hz") + + # More aggressive skip logic: skip encoding if loop is taking >90% of frame time + # OR if previous loop was slow if hasattr(record_loop_with_compensation, '_last_loop_duration'): - # If previous loop took longer than frame time, we're falling behind - skip encoding - skip_encoding = record_loop_with_compensation._last_loop_duration > dt * 1.1 + skip_encoding = record_loop_with_compensation._last_loop_duration > dt * 0.9 else: skip_encoding = False - # Calculate actual FPS every second (only after ramp-up) + # Calculate frame recording FPS every second (only after ramp-up) if is_ramp_up_complete: elapsed = loop_start - episode_start_time fps_frame_count += 1 @@ -544,6 +562,7 @@ def record_loop_with_compensation(): encoded_frames = {} # Only encode if we're not falling behind + # PRIORITY: 30 Hz control loop > streaming quality if not skip_encoding: for cam_name in ["left_wrist", "right_wrist", "base"]: if cam_name in observation: @@ -551,13 +570,13 @@ def record_loop_with_compensation(): if frame is not None and len(frame.shape) == 3: # Frame is already RGB from OpenCVCamera (ColorMode.RGB by default) # Encode once, use many times - eliminates per-stream encoding overhead - # Quick encode - use lower quality if we're tight on time + # Use low quality (40) for fast streaming - control loop is priority encode_start = time.perf_counter() - success, jpeg_bytes = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 70]) + success, jpeg_bytes = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 40]) encode_duration = time.perf_counter() - encode_start - # Only use encoded frame if encoding was fast enough (< 10ms) - if success and encode_duration < 0.01: + # Only use encoded frame if encoding was very fast (< 5ms = 15% of frame time) + if success and encode_duration < 0.005: encoded_frames[cam_name] = jpeg_bytes.tobytes() # If encoding took too long, skip this camera for this frame @@ -579,7 +598,7 @@ def record_loop_with_compensation(): # Log progress every 5 seconds (150 frames @ 30 FPS) if frame_count % 150 == 0: - print(f"[Recording] {frame_count} frames @ {recording_state['current_fps']} FPS") + print(f"[Recording] {frame_count} frames | Loop: {recording_state['loop_fps']} Hz | Recording: {recording_state['current_fps']} FPS") except Exception as frame_error: print(f"[Recording] Frame error: {frame_error}") # Continue recording even if one frame fails @@ -854,7 +873,10 @@ async def disconnect_robots(): @app.post("/api/robots/move-to-zero") async def move_to_zero(): - """Move follower robot to zero position with reduced gains (for 2 seconds).""" + """Move follower robot to zero position with reduced gains (for 2 seconds). + + Also disables leader torque during the movement for safety and ease of repositioning. + """ global recording_state if recording_state["is_recording"] or recording_state["is_initializing"]: @@ -871,6 +893,14 @@ async def move_to_zero(): recording_state["status_message"] = "Moving to zero position..." follower = robot_instances["follower"] + leader = robot_instances.get("leader") + + # Disable leader torque for safety and ease of repositioning + if leader: + print(f"[MoveToZero] Disabling leader torque...") + leader.bus_right.disable_torque() + leader.bus_left.disable_torque() + time.sleep(0.1) # Motor name to index mapping motor_index = { @@ -925,14 +955,32 @@ async def move_to_zero(): if sleep_time > 0: time.sleep(sleep_time) + # Re-enable leader torque for gravity compensation + if leader: + print(f"[MoveToZero] Re-enabling leader torque...") + leader.bus_right.enable_torque() + leader.bus_left.enable_torque() + time.sleep(0.1) + recording_state["moving_to_zero"] = False recording_state["status_message"] = "Moved to zero position" if recording_state["robots_ready"] else "Ready" + print(f"[MoveToZero] Complete - leader torque re-enabled") return {"status": "success", "message": "Robot moved to zero position"} except Exception as e: recording_state["moving_to_zero"] = False recording_state["status_message"] = f"Error: {str(e)}" + + # Try to re-enable leader torque even on error + leader = robot_instances.get("leader") + if leader: + try: + leader.bus_right.enable_torque() + leader.bus_left.enable_torque() + except Exception as torque_error: + print(f"[MoveToZero] Failed to re-enable leader torque: {torque_error}") + raise HTTPException(status_code=500, detail=str(e)) @@ -952,6 +1000,7 @@ async def get_status(): "robots_ready": recording_state["robots_ready"], "elapsed_time": elapsed, "current_fps": recording_state["current_fps"], + "loop_fps": recording_state["loop_fps"], "task": recording_state["task"], "episode_count": recording_state["episode_count"], "error": recording_state["error"], diff --git a/src/lerobot/robots/openarms/openarms_follower.py b/src/lerobot/robots/openarms/openarms_follower.py index f4f33e87e..2fb4a83e0 100644 --- a/src/lerobot/robots/openarms/openarms_follower.py +++ b/src/lerobot/robots/openarms/openarms_follower.py @@ -114,16 +114,12 @@ class OpenArmsFollower(Robot): def _motors_ft(self) -> Dict[str, type]: """Motor features for observation and action spaces.""" features = {} - # Right arm motors + # Right arm motors - only positions stored in dataset for motor in self.bus_right.motors: features[f"right_{motor}.pos"] = float - features[f"right_{motor}.vel"] = float - features[f"right_{motor}.torque"] = float - # Left arm motors + # Left arm motors - only positions stored in dataset for motor in self.bus_left.motors: features[f"left_{motor}.pos"] = float - features[f"left_{motor}.vel"] = float - features[f"left_{motor}.torque"] = float return features @property @@ -301,6 +297,9 @@ class OpenArmsFollower(Robot): OPTIMIZED: Reads all motor states (pos/vel/torque) in one CAN refresh cycle instead of 3 separate reads. + + Note: Velocity and torque are read but not stored in dataset (only used for + internal calculations). Only positions and camera images are stored. """ if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.") diff --git a/src/lerobot/teleoperators/openarms/openarms_leader.py b/src/lerobot/teleoperators/openarms/openarms_leader.py index c75740ab4..f666c4ccc 100644 --- a/src/lerobot/teleoperators/openarms/openarms_leader.py +++ b/src/lerobot/teleoperators/openarms/openarms_leader.py @@ -110,16 +110,12 @@ class OpenArmsLeader(Teleoperator): def action_features(self) -> Dict[str, type]: """Features produced by this teleoperator.""" features = {} - # Right arm motors + # Right arm motors - only positions stored in dataset for motor in self.bus_right.motors: features[f"right_{motor}.pos"] = float - features[f"right_{motor}.vel"] = float - features[f"right_{motor}.torque"] = float - # Left arm motors + # Left arm motors - only positions stored in dataset for motor in self.bus_left.motors: features[f"left_{motor}.pos"] = float - features[f"left_{motor}.vel"] = float - features[f"left_{motor}.torque"] = float return features @property @@ -277,7 +273,9 @@ class OpenArmsLeader(Teleoperator): This is the main method for teleoperators - it reads the current state of the leader arm and returns it as an action that can be sent to a follower. - Reads all motor states (pos/vel/torque) in one CAN refresh cycle + Reads all motor states (pos/vel/torque) in one CAN refresh cycle. + Note: Velocity and torque are read but not stored in dataset (only used for + gravity/friction compensation during recording). """ if not self.is_connected: raise DeviceNotConnectedError(f"{self} is not connected.")