From 895900c75f135c71377d17d2f5fe3b7d9cee9bbd Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 20 May 2026 15:46:55 +0100 Subject: [PATCH 01/22] fix: make the policy inference work sequentially from remote inference. --- README.md | 4 +- examples/4_rollout_neuracore_policy.py | 291 +++++++++++--------- examples/common/configs.py | 21 +- examples/common/data_manager.py | 255 ++++------------- examples/common/policy_state.py | 18 +- examples/common/robot_visualizer.py | 44 +-- examples/common/threads/realsense_camera.py | 83 ++++-- halid_notes.txt | 0 piper_controller.py | 30 +- 9 files changed, 341 insertions(+), 405 deletions(-) create mode 100644 halid_notes.txt diff --git a/README.md b/README.md index 805298f..7e2beb6 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ Clone the Meta Quest teleoperation repository and install it: **NOTE**: Make sure you're installing it inside the `piper-teleop` conda environment. ```bash -git clone https://github.com/NeuracoreAI/meta_quest_teleop.git +git clone https://github.com/NeuracoreAI/meta_quest_teleop.git ## Halid Note: it is not clear where it should be cloned exactly. cd meta_quest_teleop pip install -e . cd .. @@ -124,6 +124,8 @@ python examples/2_collect_teleop_data_with_neuracore.py [--ip-address **Script**: `examples/3_replay_neuracore_episodes.py` +**Halid Note**: This step can be dangerous. always ready to press the emergency. The index start from 0 not 1, hence if you see number as x in the frontend, you need to run x-1. I cannot rerun this command again successfully after the first time running. The frequency should be default as 20. + Replay recorded episodes from a Neuracore dataset on the physical robot. ```bash diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 04bbcdb..023e214 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -218,7 +218,13 @@ def run_policy( predictions = policy.predict(timeout=60) prediction_horizon = convert_predictions_to_horizon(predictions) end_time = time.time() - horizon_length = policy_state.get_prediction_horizon_length() + + # Calculate length directly from the new prediction dictionary + horizon_length = 0 + if prediction_horizon: + first_key = next(iter(prediction_horizon.keys())) + horizon_length = len(prediction_horizon[first_key]) + print( f" ✓ Got {horizon_length} actions in {end_time - start_time:.3f} seconds" ) @@ -282,17 +288,23 @@ def start_policy_execution( if current_joint_angles is None: print("⚠️ Cannot execute policy: No current joint angles available") return False + # Get first action from horizon (index 0 for each joint) current_joint_target_positions_rad = np.array( [prediction_horizon[joint_name][0] for joint_name in JOINT_NAMES] ) - joint_differences = np.abs( - current_joint_angles - np.degrees(current_joint_target_positions_rad) - ) + current_target_deg = np.degrees(current_joint_target_positions_rad) + joint_differences = np.abs(current_joint_angles - current_target_deg) + if np.any(joint_differences > MAX_SAFETY_THRESHOLD): - print("⚠️ Cannot execute policy: Robot too far from first action") - print(f" Differences: {[f'{d:.3f}' for d in joint_differences]}") - print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print("⚠️ Cannot execute policy: Robot too far from first predicted action") + print(" --- DIAGNOSTICS ---") + print(f" Current Angles: {[f'{d:.2f}' for d in current_joint_angles]}") + print(f" AI Predicted: {[f'{d:.2f}' for d in current_target_deg]}") + print(f" Differences: {[f'{d:.2f}' for d in joint_differences]}") + print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print(" 💡 TIP 1: Did the arm sag? Check if 'Current Angles' are drooping.") + print(" 💡 TIP 2: If the AI naturally predicts large first steps, increase MAX_SAFETY_THRESHOLD in common/configs.py") return False # All checks passed - start execution @@ -321,25 +333,6 @@ def start_policy_execution( return True -def run_and_start_policy_execution( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> None: - """Handle Run and Execute Policy button press to capture state, get policy prediction, and immediately execute it.""" - print("Run and Execute Policy for one prediction horizon") - run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - start_policy_execution(data_manager, policy_state) - - def end_policy_play( data_manager: DataManager, policy_state: PolicyState, @@ -349,12 +342,100 @@ def end_policy_play( """End continuous play and set robot activity state to ENABLED and update policy status.""" if policy_state.get_continuous_play_active(): policy_state.set_continuous_play_active(False) + + # Reset ghost robot color to default orange + visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) + + visualizer.update_play_policy_button_status(False) + visualizer.update_play_policy_button_status(False) policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) data_manager.set_teleop_state(False, None, None) visualizer.update_policy_status(policy_status_message) +def continuous_prediction_worker( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", +) -> None: + """Background thread for continuous execution supporting pipelined and sequential modes.""" + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + + # 1. Bootstrap the very first prediction to get the robot moving + print(f"\n🚀 [Worker] Bootstrapping initial trajectory in '{continuous_mode}' mode...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + if success: + start_policy_execution(data_manager, policy_state) + + while policy_state.get_continuous_play_active(): + # Failsafe: if there's no active trajectory running yet, wait briefly + if policy_state.get_locked_prediction_horizon_length() == 0: + time.sleep(0.01) + continue + + if continuous_mode == "pipeline": + print("\n📸 [Pipeline Worker] Robot is moving! Prefetching next prediction in background...") + # Query the network in parallel while the execution thread is driving the motors + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + # Wait until the current trajectory buffer is running low before swapping + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + remaining = total_len - exec_idx + + # Hot-swap when 5 or fewer steps are left in the active trajectory + if remaining <= 5 or total_len == 0: + break + time.sleep(0.01) + + elif continuous_mode == "sequential": + # Wait until the current trajectory buffer is completely exhausted + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + if exec_idx >= total_len or total_len == 0: + break + time.sleep(0.01) + + if not policy_state.get_continuous_play_active(): + break + + print("\n📸 [Sequential Worker] Trajectory finished! Holding position and requesting next prediction...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + if not policy_state.get_continuous_play_active(): + break + + print("🔄 [Worker] Swapping to new trajectory buffer!") + # Seamlessly clear the lock and flash the new horizon into play + policy_state.end_policy_execution() + success = start_policy_execution(data_manager, policy_state) + + if success: + color_index = (color_index + 1) % len(VISUALIZATION_COLORS) + visualizer.set_ghost_robot_color(VISUALIZATION_COLORS[color_index]) + else: + print("❌ [Worker] Swap rejected by safety threshold. Retrying immediately...") + time.sleep(0.01) def play_policy( data_manager: DataManager, @@ -362,56 +443,30 @@ def play_policy( policy_state: PolicyState, visualizer: RobotVisualizer, input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", ) -> None: """Handle Play Policy button press to start/stop continuous policy execution.""" if not policy_state.get_continuous_play_active(): # Start continuous play - print("▶️ Play Policy button pressed - Starting continuous policy execution...") - - # Run policy to get prediction horizon - success = run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - if not success: - print("⚠️ Failed to run policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - return - - # Execute policy - success = start_policy_execution(data_manager, policy_state) - if not success: - print("⚠️ Failed to execute policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - execution failed", - ) - return - + print(f"▶️ Play Policy button pressed - Starting {continuous_mode.capitalize()} Mode...") policy_state.set_continuous_play_active(True) visualizer.update_play_policy_button_status(True) - + + # Spawn the background worker + threading.Thread( + target=continuous_prediction_worker, + args=(data_manager, policy, policy_state, visualizer, input_embodiment_description, continuous_mode), + daemon=True + ).start() else: # Stop continuous play print("⏹️ Stop Policy button pressed - Stopping continuous policy execution...") policy_state.set_continuous_play_active(False) end_policy_play( - data_manager, policy_state, visualizer, "Policy execution stopped " + data_manager, policy_state, visualizer, "Policy execution stopped" ) - print("✓ Policy execution stopped and robot enabled") - def policy_execution_thread( policy: nc.policy, data_manager: DataManager, @@ -422,6 +477,16 @@ def policy_execution_thread( ) -> None: """Policy execution thread.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE + + # Define colors for continuous horizon visualization + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + # Throttle visualization updates to ~30Hz to avoid overwhelming Viser last_visualization_update = 0.0 visualization_update_interval = 1.0 / 30.0 # 30 Hz @@ -443,6 +508,7 @@ def policy_execution_thread( f"robot enabled: {robot_controller.is_robot_enabled()}" ) + # If continuous play is active, only execute up to the chunk limit if execution_index < locked_horizon_length: # Check if previous goal was achieved, if any current_joint_angles = data_manager.get_current_joint_angles() @@ -520,57 +586,23 @@ def policy_execution_thread( visualizer.update_policy_status( f"Executing policy: {execution_index + 1}/{locked_horizon_length}" ) - # Check if continuous play is active - elif policy_state.get_continuous_play_active(): - # Automatically get new prediction and execute - try: - # End policy execution to clear input lock - policy_state.end_policy_execution() - # Run policy to get prediction horizon - success = run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - if not success: - print("⚠️ Failed to run policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - continue - - # Execute policy - success = start_policy_execution(data_manager, policy_state) - if not success: - print("⚠️ Failed to execute policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - execution failed", - ) - continue - - except Exception as e: - print(f"✗ Failed to get next policy prediction: {e}") - traceback.print_exc() + else: + # Horizon buffer exhausted + if not policy_state.get_continuous_play_active(): + print("✓ Policy execution completed") end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", + data_manager, policy_state, visualizer, "Policy execution completed" ) - else: - # Execution complete - print("✓ Policy execution completed") - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution completed" - ) + else: + # Failsafe: If the background thread is running slightly late, + # hold the very last predicted position to maintain motor torque. + if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): + last_index = locked_horizon_length - 1 + hold_positions_rad = np.array([ + locked_horizon[jn][last_index] for jn in JOINT_NAMES + ]) + if robot_controller.is_robot_enabled(): + robot_controller.set_target_joint_angles(np.degrees(hold_positions_rad)) # NOTE: Update visualization less frequently to avoid blocking # Throttle visualization updates to ~30Hz to prevent overwhelming Viser server @@ -618,9 +650,9 @@ def update_visualization( joint_config_rad = np.radians(target_joint_angles) visualizer.update_ghost_robot_pose(joint_config_rad) # Disable buttons during execution - visualizer.set_start_policy_execution_button_disabled(True) + # visualizer.set_start_policy_execution_button_disabled(True) visualizer.set_run_policy_button_disabled(True) - visualizer.set_run_and_start_policy_execution_button_disabled(True) + # visualizer.set_run_and_start_policy_execution_button_disabled(True) # Play/Stop button is enabled during execution so we can stop if needed visualizer.set_play_policy_button_disabled(False) @@ -669,7 +701,7 @@ def update_visualization( not (robot_enabled and has_horizon) ) visualizer.set_run_policy_button_disabled(not robot_enabled) - visualizer.set_run_and_start_policy_execution_button_disabled(not robot_enabled) + #visualizer.set_run_and_start_policy_execution_button_disabled(not robot_enabled) visualizer.set_play_policy_button_disabled(not robot_enabled) # Update policy status @@ -702,6 +734,15 @@ def update_visualization( default=None, help="Name of the training run to load policy from (for cloud training).", ) + + parser.add_argument( + "--continuous-mode", + type=str, + choices=["pipeline", "sequential"], + default="pipeline", + help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", + ) + policy_group.add_argument( "--model-path", type=str, @@ -796,7 +837,7 @@ def update_visualization( device="cuda", input_embodiment_description=input_embodiment_description, output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, + #input_preprocessing_config=input_preprocessing_config, robot_name=args.robot_name, ) else: @@ -923,15 +964,7 @@ def update_visualization( visualizer.set_start_policy_execution_callback( lambda: start_policy_execution(data_manager, policy_state) ) - visualizer.set_run_and_start_policy_execution_callback( - lambda: run_and_start_policy_execution( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - ) + visualizer.set_play_policy_callback( lambda: play_policy( data_manager, @@ -939,8 +972,10 @@ def update_visualization( policy_state, visualizer, input_embodiment_description, + args.continuous_mode, ) ) + # Set up execution mode dropdown callback to sync with PolicyState visualizer.set_execution_mode_callback( lambda: policy_state.set_execution_mode( @@ -983,10 +1018,10 @@ def update_visualization( print(" - Hold RIGHT TRIGGER to close gripper") print(" - Press BUTTON A or Enable Robot button to enable/disable robot") print(" - Press BUTTON B or Home Robot button to send robot home") - print(" 3. Click 'Run Policy' button to run policy (without executing)") - print(" 4. Click 'Execute Policy' button to execute prediction horizon") - print(" 5. Click 'Run and Execute Policy' button to run and execute policy") - print(" 6. Click 'Play Policy' button to play policy") + print(" 3. Click 'Run Policy' (Preview) to generate and visualize a prediction horizon") + print(" 4. Click 'Execute Policy' to run the currently previewed horizon") + print(" 5. Click 'Play Policy' (Receding Horizon) to constantly predict and execute the first action") + # print(" 6. Click 'Play Policy' button to play policy") print("⚠️ Press Ctrl+C to exit") print() print("🌐 Open browser: http://localhost:8080") diff --git a/examples/common/configs.py b/examples/common/configs.py index e8142b0..6ed3569 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -17,34 +17,34 @@ POSITION_COST = 1.0 ORIENTATION_COST = 0.75 FRAME_TASK_GAIN = 0.4 -LM_DAMPING = 0.0 +LM_DAMPING = 0.01 #0.0 DAMPING_COST = 0.25 -SOLVER_DAMPING_VALUE = 1e-12 +SOLVER_DAMPING_VALUE = 1e-4 #1e-12 # Controller 1€ Filter parameters CONTROLLER_MIN_CUTOFF = 0.8 # Minimum cutoff frequency (stabilizes when holding still) -CONTROLLER_BETA = 5.0 # Speed coefficient (reduces lag when moving) +CONTROLLER_BETA = 0.05 #5.0 # Speed coefficient (reduces lag when moving) CONTROLLER_D_CUTOFF = 0.9 # Derivative cutoff frequency # Controller parameters GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control # Scaling factors for translation and rotation -TRANSLATION_SCALE = 1.0 -ROTATION_SCALE = 1.0 +TRANSLATION_SCALE = 1.5 +ROTATION_SCALE = 1.2 SLOW_TRANSLATION_SCALE = 0.6 SLOW_ROTATION_SCALE = 0.6 WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 # Thread rates (Hz) CONTROLLER_DATA_RATE = 50.0 # Controller input reading -IK_SOLVER_RATE = 250.0 # IK solving and robot commands +IK_SOLVER_RATE = 100 #250.0 # IK solving and robot commands VISUALIZATION_RATE = 60.0 # GUI updates ROBOT_RATE = 100.0 # Neuracore data collection rates JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore -CAMERA_FRAME_STREAMING_RATE = 60.0 # Data collection rate for camera frame +CAMERA_FRAME_STREAMING_RATE = 30.0 # Data collection rate for camera frame # Meta quest axis mask META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] # [x, y, z, roll, pitch, yaw] @@ -56,7 +56,10 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] -NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +# NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # in degrees +# Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) + NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] # Posture task cost vector (one weight per joint) @@ -67,7 +70,7 @@ PREDICTION_HORIZON_EXECUTION_RATIO = ( 1.0 # percentage of the prediction horizon that is executed ) -MAX_SAFETY_THRESHOLD = 20.0 # degrees +MAX_SAFETY_THRESHOLD = 50.0 # degrees MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index fec186d..bf3cc3e 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -4,9 +4,9 @@ This module provides shared state classes for teleoperation systems that need to coordinate between multiple threads (data collection, IK solving, visualization). """ - import threading import time +import queue # Added for async queueing from enum import Enum from typing import Any, Callable @@ -18,7 +18,6 @@ class RobotActivityState(Enum): """Robot activity state enumeration.""" - ENABLED = "ENABLED" HOMING = "HOMING" DISABLED = "DISABLED" @@ -99,7 +98,6 @@ def __init__(self) -> None: # Map from camera name -> latest RGB image self.rgb_images: dict[str, np.ndarray] = {} - class DataManager: """Main state container coordinating all state groups. @@ -110,10 +108,8 @@ class DataManager: Uses separate locks for each state group to reduce contention. """ - def __init__(self) -> None: - """Initialize DataManager with default values.""" - # State groups with individual locks + """Initialize DataManager with background callback processing.""" self._controller_state = ControllerState() self._teleop_state = TeleopState() self._robot_state = RobotState() @@ -123,12 +119,20 @@ def __init__(self) -> None: # System state self._shutdown_event = threading.Event() - # Callback for state changes (RGB, target joints, current joints). - # The callable takes (name: str, payload: dict[str, Any], timestamp: float). - # payload is the data dict e.g. {joint1: v, joint2: v}, {gripper: v}, {rgb_scene: array}. + # Asynchronous processing elements self._on_change_callback: ( Callable[[str, dict[str, Any], float], None] | None ) = None + + # Maxsize 60 matches ~1 second of video frames buffer if disk spikes + self._callback_queue: queue.Queue = queue.Queue(maxsize=60) + + self._worker_thread = threading.Thread( + target=self._callback_worker_loop, + name="NeuracoreCallbackWorker", + daemon=True + ) + self._worker_thread.start() def set_on_change_callback( self, on_change_callback: Callable[[str, dict[str, Any], float], None] @@ -136,6 +140,35 @@ def set_on_change_callback( """Set on change callback (thread-safe).""" self._on_change_callback = on_change_callback + def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None: + """Helper to push payloads into the execution queue without blocking.""" + if self._on_change_callback is None: + return + + try: + # put_nowait drops data into the memory queue instantly (0.0ms blocking) + self._callback_queue.put_nowait((name, payload, timestamp)) + except queue.Full: + # Prevents out-of-memory if disk halts completely, without freezing telemetry loops + print(f"⚠️ Neuracore background queue full! Dropping log packet: {name}") + + def _callback_worker_loop(self) -> None: + """Background thread worker loop dedicated solely to performing slow disk IO updates.""" + while not self._shutdown_event.is_set() or not self._callback_queue.empty(): + try: + # Wait up to 100ms for a logging event + name, payload, timestamp = self._callback_queue.get(timeout=0.1) + + if self._on_change_callback is not None: + # Execute Neuracore disk operation safely here on a separate core + self._on_change_callback(name, payload, timestamp) + + self._callback_queue.task_done() + except queue.Empty: + continue + except Exception as e: + print(f"❌ Error in background logging callback: {e}") + # ============================================================================ # Camera State Methods # ============================================================================ @@ -145,28 +178,24 @@ def get_rgb_image(self, camera_name: str) -> np.ndarray | None: with self._camera_state._lock: if not self._camera_state.rgb_images: return None - img = self._camera_state.rgb_images.get(camera_name) return img.copy() if img is not None else None def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: - """Set RGB image for a specific camera (thread-safe).""" + """Set RGB image for a specific camera (thread-safe and non-blocking).""" with self._camera_state._lock: self._camera_state.rgb_images[camera_name] = image.copy() + if self._on_change_callback: img_copy = self._camera_state.rgb_images[camera_name].copy() - self._on_change_callback("log_rgb", {camera_name: img_copy}, time.time()) + # Queue it instead of executing directly! Camera loop returns immediately. + self._queue_callback("log_rgb", {camera_name: img_copy}, time.time()) # ============================================================================ # Controller State Methods # ============================================================================ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: - """Get current controller data (thread-safe). - - Returns: - Tuple of (controller_transform, grip_value, trigger_value) - """ with self._controller_state._lock: return ( ( @@ -181,18 +210,6 @@ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: def set_controller_data( self, transform: np.ndarray | None, grip: float, trigger: float ) -> None: - """Set controller data (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None - grip: float - grip value - trigger: float - trigger value - - Raises: - ValueError: If the transform is not a 4x4 matrix - ValueError: If the grip value is not between 0.0 and 1.0 - ValueError: If the trigger value is not between 0.0 and 1.0 - """ if transform is not None and transform.shape != (4, 4): raise ValueError("Transform must be a 4x4 matrix") if grip < 0.0 or grip > 1.0: @@ -206,11 +223,8 @@ def set_controller_data( if transform is not None: current_time = time.time() - - # Store raw transform self._controller_state.transform_raw = transform.copy() - # Initialize filter if needed if self._controller_state._filter is None: self._controller_state._filter = OneEuroFilterTransform( current_time, @@ -221,45 +235,28 @@ def set_controller_data( ) self._controller_state.transform = transform.copy() else: - # Update filter parameters if they changed self._controller_state._filter.update_params( self._controller_state.min_cutoff, self._controller_state.beta, self._controller_state.d_cutoff, ) - - # Apply filter self._controller_state.transform = self._controller_state._filter( current_time, transform ) else: self._controller_state.transform = None self._controller_state.transform_raw = None - self._controller_state._filter = ( - None # Reset filter when transform is None - ) + self._controller_state._filter = None def set_controller_filter_params( self, min_cutoff: float, beta: float, d_cutoff: float ) -> None: - """Update 1€ Filter parameters for controller transform (thread-safe). - - Args: - min_cutoff: Minimum cutoff frequency (stabilizes when holding still) - beta: Speed coefficient (reduces lag when moving) - d_cutoff: Cutoff frequency for derivative filtering - """ with self._controller_state._lock: self._controller_state.min_cutoff = min_cutoff self._controller_state.beta = beta self._controller_state.d_cutoff = d_cutoff def get_controller_filter_params(self) -> tuple[float, float, float]: - """Get 1€ Filter parameters for controller transform (thread-safe). - - Returns: - Tuple of (min_cutoff, beta, d_cutoff) - """ with self._controller_state._lock: return ( self._controller_state.min_cutoff, @@ -277,13 +274,6 @@ def set_teleop_state( controller_initial: np.ndarray | None, robot_initial: np.ndarray | None, ) -> None: - """Set teleoperation state (thread-safe). - - Args: - active: bool - whether teleop is active - controller_initial: np.ndarray | None - 4x4 transformation matrix for initial controller transform or None to clear - robot_initial: np.ndarray | None - 4x4 transformation matrix for initial robot transform or None to clear - """ with self._teleop_state._lock: self._teleop_state.active = active self._teleop_state.controller_initial_transform = ( @@ -296,26 +286,13 @@ def set_teleop_state( def set_teleop_scaling( self, translation_scale: float, rotation_scale: float ) -> None: - """Set teleoperation scaling factors (thread-safe). - - Args: - translation_scale: Scale applied to controller translation deltas - rotation_scale: Scale applied to controller rotation deltas - """ - # Ignore invalid values (e.g. transient 0 while typing) and keep old scaling. if translation_scale <= 0.0 or rotation_scale <= 0.0: return - with self._teleop_state._lock: self._teleop_state.translation_scale = translation_scale self._teleop_state.rotation_scale = rotation_scale def get_teleop_scaling(self) -> tuple[float, float]: - """Get teleoperation scaling factors (thread-safe). - - Returns: - Tuple of (translation_scale, rotation_scale) - """ with self._teleop_state._lock: return ( self._teleop_state.translation_scale, @@ -323,17 +300,14 @@ def get_teleop_scaling(self) -> tuple[float, float]: ) def get_teleop_active(self) -> bool: - """Get teleoperation active state (thread-safe).""" with self._teleop_state._lock: return self._teleop_state.active def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: - """Set slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = enabled def toggle_slow_scaling_mode_enabled(self) -> bool: - """Toggle and return slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = ( not self._teleop_state.slow_scaling_mode_enabled @@ -341,21 +315,12 @@ def toggle_slow_scaling_mode_enabled(self) -> bool: return self._teleop_state.slow_scaling_mode_enabled def get_slow_scaling_mode_enabled(self) -> bool: - """Get slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: return self._teleop_state.slow_scaling_mode_enabled def get_initial_robot_controller_transforms( self, ) -> tuple[np.ndarray | None, np.ndarray | None]: - """Get initial robot and controller transforms. - - These two transforms are captured on rising edge of grip button - and reset on falling edge of grip button. (thread-safe) - - Returns: - Tuple of (controller_initial_transform, robot_initial_transform) - """ with self._teleop_state._lock: return ( ( @@ -375,29 +340,14 @@ def get_initial_robot_controller_transforms( # ============================================================================ def get_robot_activity_state(self) -> RobotActivityState: - """Get robot activity state (thread-safe). - - Returns: - RobotActivityState - current robot activity state - """ with self._robot_state._lock: return self._robot_state.activity_state def set_robot_activity_state(self, state: RobotActivityState) -> None: - """Set robot activity state (thread-safe). - - Args: - state: RobotActivityState - new robot activity state - """ with self._robot_state._lock: self._robot_state.activity_state = state def get_current_joint_angles(self) -> np.ndarray | None: - """Get current joint angles (thread-safe). - - Returns: - Current joint angles or None if not available - """ with self._robot_state._lock: return ( self._robot_state.joint_angles.copy() @@ -406,11 +356,6 @@ def get_current_joint_angles(self) -> np.ndarray | None: ) def set_current_joint_angles(self, angles: np.ndarray) -> None: - """Set current joint angles (thread-safe). - - Args: - angles: np.ndarray - current joint angles - """ with self._robot_state._lock: self._robot_state.joint_angles = angles.copy() if self._on_change_callback: @@ -419,14 +364,9 @@ def set_current_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._on_change_callback("log_joint_positions", payload, time.time()) + self._queue_callback("log_joint_positions", payload, time.time()) def get_current_joint_torques(self) -> np.ndarray | None: - """Get current joint torques/currents proxy (thread-safe). - - Returns: - Current joint torques/currents vector or None if not available - """ with self._robot_state._lock: return ( self._robot_state.joint_torques.copy() @@ -435,25 +375,15 @@ def get_current_joint_torques(self) -> np.ndarray | None: ) def set_current_joint_torques(self, torques: np.ndarray) -> None: - """Set current joint torques/currents proxy and log to NeuraCore. - - Args: - torques: np.ndarray - current joint torques/currents vector - """ with self._robot_state._lock: self._robot_state.joint_torques = torques.copy() if self._on_change_callback: torques = self._robot_state.joint_torques if torques is not None: payload = {jn: float(torques[i]) for i, jn in enumerate(JOINT_NAMES)} - self._on_change_callback("log_joint_torques", payload, time.time()) + self._queue_callback("log_joint_torques", payload, time.time()) def get_current_end_effector_pose(self) -> np.ndarray | None: - """Get current end effector pose (thread-safe). - - Returns: - Current end effector pose or None if not available - """ with self._robot_state._lock: return ( self._robot_state.end_effector_pose.copy() @@ -462,61 +392,32 @@ def get_current_end_effector_pose(self) -> np.ndarray | None: ) def set_current_end_effector_pose(self, pose: np.ndarray) -> None: - """Set current end effector pose (thread-safe). - - Args: - pose: np.ndarray - current end effector pose - """ with self._robot_state._lock: self._robot_state.end_effector_pose = pose.copy() def get_current_gripper_open_value(self) -> float | None: - """Get current gripper open value (thread-safe). - - Returns: - Current gripper open value or None if not available - """ with self._robot_state._lock: - return ( - self._robot_state.current_gripper_open_value - if self._robot_state.current_gripper_open_value is not None - else None - ) + return self._robot_state.current_gripper_open_value def set_current_gripper_open_value(self, value: float) -> None: - """Set current gripper open value (thread-safe). - - Args: - value: float - current gripper open value - """ with self._robot_state._lock: self._robot_state.current_gripper_open_value = value if self._on_change_callback: - self._on_change_callback( + self._queue_callback( "log_parallel_gripper_open_amounts", {GRIPPER_NAME: value}, time.time(), ) def get_target_gripper_open_value(self) -> float | None: - """Get target gripper open value (thread-safe). - - Returns: - Target gripper open value or None if not available - """ with self._robot_state._lock: return self._robot_state.target_gripper_open_value def set_target_gripper_open_value(self, value: float) -> None: - """Set target gripper open value (thread-safe). - - Args: - value: float - target gripper open value - """ with self._robot_state._lock: self._robot_state.target_gripper_open_value = value if self._on_change_callback: - self._on_change_callback( + self._queue_callback( "log_parallel_gripper_target_open_amounts", {GRIPPER_NAME: self._robot_state.target_gripper_open_value}, time.time(), @@ -527,11 +428,6 @@ def set_target_gripper_open_value(self, value: float) -> None: # ============================================================================ def get_target_joint_angles(self) -> np.ndarray | None: - """Get current joint configuration (thread-safe). - - Returns: - Current target joint angles or None if not available - """ with self._ik_state._lock: return ( self._ik_state.target_joint_angles.copy() @@ -540,11 +436,6 @@ def get_target_joint_angles(self) -> np.ndarray | None: ) def set_target_joint_angles(self, angles: np.ndarray) -> None: - """Set target joint angles (thread-safe). - - Args: - angles: np.ndarray - target joint angles - """ with self._ik_state._lock: self._ik_state.target_joint_angles = angles.copy() if self._on_change_callback: @@ -553,27 +444,17 @@ def set_target_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._on_change_callback( + self._queue_callback( "log_joint_target_positions", payload, time.time() ) def set_target_pose(self, transform: np.ndarray | None) -> None: - """Set target transform for visualization (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None to clear target transform - """ with self._ik_state._lock: self._ik_state.target_pose = ( transform.copy() if transform is not None else None ) def get_target_pose(self) -> np.ndarray | None: - """Get target transform for visualization (thread-safe). - - Returns: - Target transform or None if target transform is not set - """ with self._ik_state._lock: return ( self._ik_state.target_pose.copy() @@ -582,38 +463,18 @@ def get_target_pose(self) -> np.ndarray | None: ) def set_ik_solve_time_ms(self, time_ms: float) -> None: - """Set IK solve time (thread-safe). - - Args: - time_ms: float - IK solve time in milliseconds - """ with self._ik_state._lock: self._ik_state.solve_time_ms = time_ms def set_ik_success(self, success: bool) -> None: - """Set IK success (thread-safe). - - Args: - success: bool - True if IK was successful, False otherwise - """ with self._ik_state._lock: self._ik_state.success = success def get_ik_solve_time_ms(self) -> float: - """Get IK solve time (thread-safe). - - Returns: - IK solve time in milliseconds - """ with self._ik_state._lock: return self._ik_state.solve_time_ms def get_ik_success(self) -> bool: - """Get IK success (thread-safe). - - Returns: - True if IK was successful, False otherwise - """ with self._ik_state._lock: return self._ik_state.success @@ -622,13 +483,9 @@ def get_ik_success(self) -> bool: # ============================================================================ def request_shutdown(self) -> None: - """Request shutdown of all threads (lock-free using Event).""" + """Request shutdown of all threads.""" self._shutdown_event.set() def is_shutdown_requested(self) -> bool: - """Check if shutdown is requested (lock-free using Event). - - Returns: - True if shutdown is requested, False otherwise - """ - return self._shutdown_event.is_set() + """Check if shutdown is requested.""" + return self._shutdown_event.is_set() \ No newline at end of file diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 42e90b6..37dda03 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -90,14 +90,7 @@ def get_policy_rgb_image_input(self) -> np.ndarray | None: ) def set_policy_rgb_image_input(self, image: np.ndarray) -> None: - """Set policy RGB image (thread-safe). - - Raises: - RuntimeError: If policy inputs are locked (during execution). - """ - with self._execution_lock: - if self._policy_inputs_locked: - raise RuntimeError("Policy inputs are locked during execution") + """Set policy RGB image (thread-safe).""" with self._policy_rgb_image_input_lock: self._policy_rgb_image_input = image.copy() if image is not None else None @@ -111,14 +104,7 @@ def get_policy_state_input(self) -> np.ndarray | None: ) def set_policy_state_input(self, input: np.ndarray) -> None: - """Set policy state input (thread-safe). - - Raises: - RuntimeError: If policy inputs are locked (during execution). - """ - with self._execution_lock: - if self._policy_inputs_locked: - raise RuntimeError("Policy inputs are locked during execution") + """Set policy state input (thread-safe).""" with self._policy_state_input_lock: self._policy_state_input = input.copy() if input is not None else None diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index 0547095..e8a3602 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -40,7 +40,7 @@ def __init__(self, urdf_path: str) -> None: self._server, ghost_urdf, root_node_name="/robot_ghost", - mesh_color_override=(0.2, 0.4, 1.0, 0.25), # Blue with 60% opacity + mesh_color_override=(1.0, 0.65, 0.0, 0.25), # Orange with 25% opacity ) # GUI handles (initialized as None, created on demand) - all private @@ -92,7 +92,6 @@ def __init__(self, urdf_path: str) -> None: self._execution_mode_dropdown = None self._run_policy_button = None self._start_policy_execution_button = None - self._run_and_start_policy_execution_button = None self._play_policy_button = None # Internal state @@ -675,14 +674,11 @@ def add_policy_controls( def add_policy_buttons(self) -> None: """Add policy control buttons.""" - self._run_policy_button = self._server.gui.add_button("Run Policy") + self._run_policy_button = self._server.gui.add_button("Run Policy (Preview)") self._start_policy_execution_button = self._server.gui.add_button( - "Start Policy Execution" + "Execute Policy (Run Preview)" ) - self._run_and_start_policy_execution_button = self._server.gui.add_button( - "Run and Execute Policy" - ) - self._play_policy_button = self._server.gui.add_button("Play Policy") + self._play_policy_button = self._server.gui.add_button("Continuous Receding Horizon") def update_policy_status(self, status: str) -> None: """Update policy status display. @@ -773,16 +769,6 @@ def set_start_policy_execution_callback(self, callback: Callable[[], Any]) -> No if self._start_policy_execution_button is not None: self._start_policy_execution_button.on_click(lambda _: callback()) - def set_run_and_start_policy_execution_callback( - self, callback: Callable[[], Any] - ) -> None: - """Set callback for Run and Execute Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._run_and_start_policy_execution_button is not None: - self._run_and_start_policy_execution_button.on_click(lambda _: callback()) def set_play_policy_callback(self, callback: Callable[[], Any]) -> None: """Set callback for Play Policy button. @@ -856,16 +842,6 @@ def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: if self._start_policy_execution_button is not None: self._start_policy_execution_button.disabled = disabled - def set_run_and_start_policy_execution_button_disabled( - self, disabled: bool - ) -> None: - """Set Run and Execute Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._run_and_start_policy_execution_button is not None: - self._run_and_start_policy_execution_button.disabled = disabled def set_play_policy_button_disabled(self, disabled: bool) -> None: """Set Play Policy button disabled state. @@ -883,8 +859,18 @@ def update_play_policy_button_status(self, active: bool) -> None: active: Whether continuous play is currently active """ if self._play_policy_button is not None: - self._play_policy_button.label = "Stop Policy" if active else "Play Policy" + self._play_policy_button.label = ( + "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + ) + def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + """Set the color of the ghost robot. + Args: + color: RGBA tuple (0.0 to 1.0) + """ + if self._ghost_robot_urdf is not None: + self._ghost_robot_urdf.mesh_color_override = color + def stop(self) -> None: """Stop the visualizer server.""" self._server.stop() diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py index 24e6dfe..8573eae 100644 --- a/examples/common/threads/realsense_camera.py +++ b/examples/common/threads/realsense_camera.py @@ -1,7 +1,9 @@ -"""Camera thread - captures RGB images from RealSense.""" +"""Camera thread - captures RGB images from RealSense with drop detection.""" import time import traceback +from collections import deque +import cv2 import numpy as np import pyrealsense2 as rs @@ -10,13 +12,21 @@ def camera_thread(data_manager: DataManager) -> None: - """Camera thread - captures RGB images from RealSense.""" + """Camera thread - captures RGB images from RealSense and monitors health.""" print("📷 Camera thread started") camera_name = CAMERA_NAMES[0] - dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE pipeline: rs.pipeline | None = None + # Diagnostic variables + last_frame_number = None + total_dropped_frames = 0 + fps_timer = time.time() + frame_count = 0 + # Store the last 100 frame intervals to check for extreme jitter + intervals = deque(maxlen=100) + last_frame_time = time.time() + try: # Configure RealSense pipeline pipeline = rs.pipeline() @@ -34,26 +44,67 @@ def camera_thread(data_manager: DataManager) -> None: pipeline.start(config) while not data_manager.is_shutdown_requested(): - iteration_start = time.time() - try: + # wait_for_frames naturally blocks at the target framerate (e.g., 60Hz) frames = pipeline.wait_for_frames(timeout_ms=500) except Exception as e: - print(f"⚠️ RealSense wait for frames error: {e}") + print(f"⚠️ RealSense wait for frames error (Timeout?): {e}") continue color_frame = frames.get_color_frame() + if not color_frame: + continue + + current_time = time.time() + intervals.append(current_time - last_frame_time) + last_frame_time = current_time + + # --------------------------------------------------------- + # DIAGNOSTICS: Check for dropped frames via hardware ID + # --------------------------------------------------------- + current_frame_number = color_frame.get_frame_number() + + if last_frame_number is not None: + # If frame numbers are not sequential, we missed something in software + drops = (current_frame_number - last_frame_number) - 1 + if drops > 0: + total_dropped_frames += drops + print(f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}") + + last_frame_number = current_frame_number + + # --------------------------------------------------------- + # DIAGNOSTICS: Calculate software-side FPS and Jitter + # --------------------------------------------------------- + frame_count += 1 + if current_time - fps_timer >= 5.0: # Report every 5 seconds + effective_fps = frame_count / (current_time - fps_timer) + + # Calculate jitter (max variance between frame arrivals) + max_interval = max(intervals) * 1000 # in ms + min_interval = min(intervals) * 1000 # in ms + + print(f"📊 Camera Health: {effective_fps:.1f} FPS | " + f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " + f"Total Drops: {total_dropped_frames}") + + # Reset counters for the next 5-second window + fps_timer = current_time + frame_count = 0 + + # --------------------------------------------------------- + # IMAGE PROCESSING + # --------------------------------------------------------- + # color_image = np.asanyarray(color_frame.get_data()) + # color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + # data_manager.set_rgb_image(color_image, camera_name) - if color_frame: - color_image = np.asanyarray(color_frame.get_data()) - color_image = np.rot90(color_image, k=3) # Rotate 270 degrees - data_manager.set_rgb_image(color_image, camera_name) + color_image = np.asanyarray(color_frame.get_data()) + color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + data_manager.set_rgb_image(color_image, camera_name) - # Sleep to maintain loop rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) + # Notice: The time.sleep() has been completely removed. + # wait_for_frames() manages the loop pace perfectly. except Exception as e: print(f"❌ Camera thread error: {e}") @@ -66,4 +117,4 @@ def camera_thread(data_manager: DataManager) -> None: print(" ✓ RealSense pipeline stopped") except Exception as e: print(f"⚠️ Error stopping pipeline: {e}") - print("📷 Camera thread stopped") + print("📷 Camera thread stopped") \ No newline at end of file diff --git a/halid_notes.txt b/halid_notes.txt new file mode 100644 index 0000000..e69de29 diff --git a/piper_controller.py b/piper_controller.py index e979bcc..63b96a4 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -91,7 +91,7 @@ def __init__( # Gripper range in degrees (for internal SDK communication) self.GRIPPER_DEGREES_MIN = -10.00 - self.GRIPPER_DEGREES_MAX = 30.00 + self.GRIPPER_DEGREES_MAX = 100.00 self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees @@ -223,18 +223,34 @@ def get_control_mode(self) -> "PiperController.ControlMode": with self.state_lock: return self._control_mode - def set_control_mode(self, mode: "PiperController.ControlMode") -> None: - """Set the control mode. + # def set_control_mode(self, mode: "PiperController.ControlMode") -> None: + # """Set the control mode. - Args: - mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE) - """ + # Args: + # mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE) + # """ + # with self.state_lock: + # old_mode = self._control_mode + # self._control_mode = mode + # if self.debug_mode: + # print(f"Control mode changed: {old_mode.value} -> {mode.value}") + + + def set_control_mode(self, mode: "PiperController.ControlMode") -> None: with self.state_lock: old_mode = self._control_mode self._control_mode = mode + + # Send the mode configuration ONCE here, not in the 100Hz loop + if hasattr(self, "piper") and self.piper.get_connect_status(): + if mode == PiperController.ControlMode.JOINT_SPACE: + self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) + elif mode == PiperController.ControlMode.END_EFFECTOR: + self.piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) + if self.debug_mode: print(f"Control mode changed: {old_mode.value} -> {mode.value}") - + @staticmethod def _pose_6d_to_4x4(pose_6d: np.ndarray) -> np.ndarray: """Convert 6D pose [x, y, z, rx, ry, rz] to 4x4 transformation matrix. From 6ba55ef971b8da0e2f33d1b530203461c2922212 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 20 May 2026 15:46:55 +0100 Subject: [PATCH 02/22] fix: make the policy inference work sequentially from remote inference. --- README.md | 4 +- examples/4_rollout_neuracore_policy.py | 295 +++++++++++--------- examples/common/configs.py | 21 +- examples/common/data_manager.py | 255 ++++------------- examples/common/policy_state.py | 18 +- examples/common/robot_visualizer.py | 44 +-- examples/common/threads/realsense_camera.py | 83 ++++-- halid_notes.txt | 0 piper_controller.py | 30 +- 9 files changed, 346 insertions(+), 404 deletions(-) create mode 100644 halid_notes.txt diff --git a/README.md b/README.md index 805298f..7e2beb6 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ Clone the Meta Quest teleoperation repository and install it: **NOTE**: Make sure you're installing it inside the `piper-teleop` conda environment. ```bash -git clone https://github.com/NeuracoreAI/meta_quest_teleop.git +git clone https://github.com/NeuracoreAI/meta_quest_teleop.git ## Halid Note: it is not clear where it should be cloned exactly. cd meta_quest_teleop pip install -e . cd .. @@ -124,6 +124,8 @@ python examples/2_collect_teleop_data_with_neuracore.py [--ip-address **Script**: `examples/3_replay_neuracore_episodes.py` +**Halid Note**: This step can be dangerous. always ready to press the emergency. The index start from 0 not 1, hence if you see number as x in the frontend, you need to run x-1. I cannot rerun this command again successfully after the first time running. The frequency should be default as 20. + Replay recorded episodes from a Neuracore dataset on the physical robot. ```bash diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 6ceb550..d613ae5 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -142,7 +142,13 @@ def run_policy( predictions = policy.predict(timeout=60) prediction_horizon = convert_predictions_to_horizon(predictions) end_time = time.time() - horizon_length = policy_state.get_prediction_horizon_length() + + # Calculate length directly from the new prediction dictionary + horizon_length = 0 + if prediction_horizon: + first_key = next(iter(prediction_horizon.keys())) + horizon_length = len(prediction_horizon[first_key]) + print( f" ✓ Got {horizon_length} actions in {end_time - start_time:.3f} seconds" ) @@ -206,17 +212,23 @@ def start_policy_execution( if current_joint_angles is None: print("⚠️ Cannot execute policy: No current joint angles available") return False + # Get first action from horizon (index 0 for each joint) current_joint_target_positions_rad = np.array( [prediction_horizon[joint_name][0] for joint_name in JOINT_NAMES] ) - joint_differences = np.abs( - current_joint_angles - np.degrees(current_joint_target_positions_rad) - ) + current_target_deg = np.degrees(current_joint_target_positions_rad) + joint_differences = np.abs(current_joint_angles - current_target_deg) + if np.any(joint_differences > MAX_SAFETY_THRESHOLD): - print("⚠️ Cannot execute policy: Robot too far from first action") - print(f" Differences: {[f'{d:.3f}' for d in joint_differences]}") - print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print("⚠️ Cannot execute policy: Robot too far from first predicted action") + print(" --- DIAGNOSTICS ---") + print(f" Current Angles: {[f'{d:.2f}' for d in current_joint_angles]}") + print(f" AI Predicted: {[f'{d:.2f}' for d in current_target_deg]}") + print(f" Differences: {[f'{d:.2f}' for d in joint_differences]}") + print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print(" 💡 TIP 1: Did the arm sag? Check if 'Current Angles' are drooping.") + print(" 💡 TIP 2: If the AI naturally predicts large first steps, increase MAX_SAFETY_THRESHOLD in common/configs.py") return False # All checks passed - start execution @@ -245,25 +257,6 @@ def start_policy_execution( return True -def run_and_start_policy_execution( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> None: - """Handle Run and Execute Policy button press to capture state, get policy prediction, and immediately execute it.""" - print("Run and Execute Policy for one prediction horizon") - run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - start_policy_execution(data_manager, policy_state) - - def end_policy_play( data_manager: DataManager, policy_state: PolicyState, @@ -273,12 +266,100 @@ def end_policy_play( """End continuous play and set robot activity state to ENABLED and update policy status.""" if policy_state.get_continuous_play_active(): policy_state.set_continuous_play_active(False) + + # Reset ghost robot color to default orange + visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) + + visualizer.update_play_policy_button_status(False) + visualizer.update_play_policy_button_status(False) policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) data_manager.set_teleop_state(False, None, None) visualizer.update_policy_status(policy_status_message) +def continuous_prediction_worker( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", +) -> None: + """Background thread for continuous execution supporting pipelined and sequential modes.""" + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + + # 1. Bootstrap the very first prediction to get the robot moving + print(f"\n🚀 [Worker] Bootstrapping initial trajectory in '{continuous_mode}' mode...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + if success: + start_policy_execution(data_manager, policy_state) + + while policy_state.get_continuous_play_active(): + # Failsafe: if there's no active trajectory running yet, wait briefly + if policy_state.get_locked_prediction_horizon_length() == 0: + time.sleep(0.01) + continue + + if continuous_mode == "pipeline": + print("\n📸 [Pipeline Worker] Robot is moving! Prefetching next prediction in background...") + # Query the network in parallel while the execution thread is driving the motors + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + # Wait until the current trajectory buffer is running low before swapping + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + remaining = total_len - exec_idx + + # Hot-swap when 5 or fewer steps are left in the active trajectory + if remaining <= 5 or total_len == 0: + break + time.sleep(0.01) + + elif continuous_mode == "sequential": + # Wait until the current trajectory buffer is completely exhausted + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + if exec_idx >= total_len or total_len == 0: + break + time.sleep(0.01) + + if not policy_state.get_continuous_play_active(): + break + + print("\n📸 [Sequential Worker] Trajectory finished! Holding position and requesting next prediction...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + if not policy_state.get_continuous_play_active(): + break + + print("🔄 [Worker] Swapping to new trajectory buffer!") + # Seamlessly clear the lock and flash the new horizon into play + policy_state.end_policy_execution() + success = start_policy_execution(data_manager, policy_state) + + if success: + color_index = (color_index + 1) % len(VISUALIZATION_COLORS) + visualizer.set_ghost_robot_color(VISUALIZATION_COLORS[color_index]) + else: + print("❌ [Worker] Swap rejected by safety threshold. Retrying immediately...") + time.sleep(0.01) def play_policy( data_manager: DataManager, @@ -286,56 +367,30 @@ def play_policy( policy_state: PolicyState, visualizer: RobotVisualizer, input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", ) -> None: """Handle Play Policy button press to start/stop continuous policy execution.""" if not policy_state.get_continuous_play_active(): # Start continuous play - print("▶️ Play Policy button pressed - Starting continuous policy execution...") - - # Run policy to get prediction horizon - success = run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - if not success: - print("⚠️ Failed to run policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - return - - # Execute policy - success = start_policy_execution(data_manager, policy_state) - if not success: - print("⚠️ Failed to execute policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - execution failed", - ) - return - + print(f"▶️ Play Policy button pressed - Starting {continuous_mode.capitalize()} Mode...") policy_state.set_continuous_play_active(True) visualizer.update_play_policy_button_status(True) - + + # Spawn the background worker + threading.Thread( + target=continuous_prediction_worker, + args=(data_manager, policy, policy_state, visualizer, input_embodiment_description, continuous_mode), + daemon=True + ).start() else: # Stop continuous play print("⏹️ Stop Policy button pressed - Stopping continuous policy execution...") policy_state.set_continuous_play_active(False) end_policy_play( - data_manager, policy_state, visualizer, "Policy execution stopped " + data_manager, policy_state, visualizer, "Policy execution stopped" ) - print("✓ Policy execution stopped and robot enabled") - def policy_execution_thread( policy: nc.policy, data_manager: DataManager, @@ -347,6 +402,16 @@ def policy_execution_thread( ) -> None: """Policy execution thread.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE + + # Define colors for continuous horizon visualization + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + # Throttle visualization updates to ~30Hz to avoid overwhelming Viser last_visualization_update = 0.0 visualization_update_interval = 1.0 / 30.0 # 30 Hz @@ -368,6 +433,7 @@ def policy_execution_thread( f"robot enabled: {robot_controller.is_robot_enabled()}" ) + # If continuous play is active, only execute up to the chunk limit if execution_index < locked_horizon_length: # Check if previous goal was achieved, if any current_joint_angles = data_manager.get_current_joint_angles() @@ -427,57 +493,23 @@ def policy_execution_thread( visualizer.update_policy_status( f"Executing policy: {execution_index + 1}/{locked_horizon_length}" ) - # Check if continuous play is active - elif policy_state.get_continuous_play_active(): - # Automatically get new prediction and execute - try: - # End policy execution to clear input lock - policy_state.end_policy_execution() - # Run policy to get prediction horizon - success = run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - if not success: - print("⚠️ Failed to run policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - continue - - # Execute policy - success = start_policy_execution(data_manager, policy_state) - if not success: - print("⚠️ Failed to execute policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - execution failed", - ) - continue - - except Exception as e: - print(f"✗ Failed to get next policy prediction: {e}") - traceback.print_exc() + else: + # Horizon buffer exhausted + if not policy_state.get_continuous_play_active(): + print("✓ Policy execution completed") end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", + data_manager, policy_state, visualizer, "Policy execution completed" ) - else: - # Execution complete - print("✓ Policy execution completed") - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution completed" - ) + else: + # Failsafe: If the background thread is running slightly late, + # hold the very last predicted position to maintain motor torque. + if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): + last_index = locked_horizon_length - 1 + hold_positions_rad = np.array([ + locked_horizon[jn][last_index] for jn in JOINT_NAMES + ]) + if robot_controller.is_robot_enabled(): + robot_controller.set_target_joint_angles(np.degrees(hold_positions_rad)) # NOTE: Update visualization less frequently to avoid blocking # Throttle visualization updates to ~30Hz to prevent overwhelming Viser server @@ -525,9 +557,9 @@ def update_visualization( joint_config_rad = np.radians(target_joint_angles) visualizer.update_ghost_robot_pose(joint_config_rad) # Disable buttons during execution - visualizer.set_start_policy_execution_button_disabled(True) + # visualizer.set_start_policy_execution_button_disabled(True) visualizer.set_run_policy_button_disabled(True) - visualizer.set_run_and_start_policy_execution_button_disabled(True) + # visualizer.set_run_and_start_policy_execution_button_disabled(True) # Play/Stop button is enabled during execution so we can stop if needed visualizer.set_play_policy_button_disabled(False) @@ -576,7 +608,7 @@ def update_visualization( not (robot_enabled and has_horizon) ) visualizer.set_run_policy_button_disabled(not robot_enabled) - visualizer.set_run_and_start_policy_execution_button_disabled(not robot_enabled) + #visualizer.set_run_and_start_policy_execution_button_disabled(not robot_enabled) visualizer.set_play_policy_button_disabled(not robot_enabled) # Update policy status @@ -609,6 +641,15 @@ def update_visualization( default=None, help="Name of the training run to load policy from (for cloud training).", ) + + parser.add_argument( + "--continuous-mode", + type=str, + choices=["pipeline", "sequential"], + default="pipeline", + help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", + ) + policy_group.add_argument( "--model-path", type=str, @@ -666,6 +707,12 @@ def update_visualization( policy = nc.policy( train_run_name=args.train_run_name, device="cuda", +<<<<<<< HEAD +======= + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + #input_preprocessing_config=input_preprocessing_config, +>>>>>>> 895900c (fix: make the policy inference work sequentially from remote inference.) robot_name=args.robot_name, ) else: @@ -802,15 +849,7 @@ def update_visualization( visualizer.set_start_policy_execution_callback( lambda: start_policy_execution(data_manager, policy_state) ) - visualizer.set_run_and_start_policy_execution_callback( - lambda: run_and_start_policy_execution( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - ) + visualizer.set_play_policy_callback( lambda: play_policy( data_manager, @@ -818,8 +857,10 @@ def update_visualization( policy_state, visualizer, input_embodiment_description, + args.continuous_mode, ) ) + # Set up execution mode dropdown callback to sync with PolicyState visualizer.set_execution_mode_callback( lambda: policy_state.set_execution_mode( @@ -863,10 +904,10 @@ def update_visualization( print(" - Hold RIGHT TRIGGER to close gripper") print(" - Press BUTTON A or Enable Robot button to enable/disable robot") print(" - Press BUTTON B or Home Robot button to send robot home") - print(" 3. Click 'Run Policy' button to run policy (without executing)") - print(" 4. Click 'Execute Policy' button to execute prediction horizon") - print(" 5. Click 'Run and Execute Policy' button to run and execute policy") - print(" 6. Click 'Play Policy' button to play policy") + print(" 3. Click 'Run Policy' (Preview) to generate and visualize a prediction horizon") + print(" 4. Click 'Execute Policy' to run the currently previewed horizon") + print(" 5. Click 'Play Policy' (Receding Horizon) to constantly predict and execute the first action") + # print(" 6. Click 'Play Policy' button to play policy") print("⚠️ Press Ctrl+C to exit") print() print("🌐 Open browser: http://localhost:8080") diff --git a/examples/common/configs.py b/examples/common/configs.py index e8142b0..6ed3569 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -17,34 +17,34 @@ POSITION_COST = 1.0 ORIENTATION_COST = 0.75 FRAME_TASK_GAIN = 0.4 -LM_DAMPING = 0.0 +LM_DAMPING = 0.01 #0.0 DAMPING_COST = 0.25 -SOLVER_DAMPING_VALUE = 1e-12 +SOLVER_DAMPING_VALUE = 1e-4 #1e-12 # Controller 1€ Filter parameters CONTROLLER_MIN_CUTOFF = 0.8 # Minimum cutoff frequency (stabilizes when holding still) -CONTROLLER_BETA = 5.0 # Speed coefficient (reduces lag when moving) +CONTROLLER_BETA = 0.05 #5.0 # Speed coefficient (reduces lag when moving) CONTROLLER_D_CUTOFF = 0.9 # Derivative cutoff frequency # Controller parameters GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control # Scaling factors for translation and rotation -TRANSLATION_SCALE = 1.0 -ROTATION_SCALE = 1.0 +TRANSLATION_SCALE = 1.5 +ROTATION_SCALE = 1.2 SLOW_TRANSLATION_SCALE = 0.6 SLOW_ROTATION_SCALE = 0.6 WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 # Thread rates (Hz) CONTROLLER_DATA_RATE = 50.0 # Controller input reading -IK_SOLVER_RATE = 250.0 # IK solving and robot commands +IK_SOLVER_RATE = 100 #250.0 # IK solving and robot commands VISUALIZATION_RATE = 60.0 # GUI updates ROBOT_RATE = 100.0 # Neuracore data collection rates JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore -CAMERA_FRAME_STREAMING_RATE = 60.0 # Data collection rate for camera frame +CAMERA_FRAME_STREAMING_RATE = 30.0 # Data collection rate for camera frame # Meta quest axis mask META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] # [x, y, z, roll, pitch, yaw] @@ -56,7 +56,10 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] -NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +# NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # in degrees +# Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) + NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] # Posture task cost vector (one weight per joint) @@ -67,7 +70,7 @@ PREDICTION_HORIZON_EXECUTION_RATIO = ( 1.0 # percentage of the prediction horizon that is executed ) -MAX_SAFETY_THRESHOLD = 20.0 # degrees +MAX_SAFETY_THRESHOLD = 50.0 # degrees MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index fec186d..bf3cc3e 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -4,9 +4,9 @@ This module provides shared state classes for teleoperation systems that need to coordinate between multiple threads (data collection, IK solving, visualization). """ - import threading import time +import queue # Added for async queueing from enum import Enum from typing import Any, Callable @@ -18,7 +18,6 @@ class RobotActivityState(Enum): """Robot activity state enumeration.""" - ENABLED = "ENABLED" HOMING = "HOMING" DISABLED = "DISABLED" @@ -99,7 +98,6 @@ def __init__(self) -> None: # Map from camera name -> latest RGB image self.rgb_images: dict[str, np.ndarray] = {} - class DataManager: """Main state container coordinating all state groups. @@ -110,10 +108,8 @@ class DataManager: Uses separate locks for each state group to reduce contention. """ - def __init__(self) -> None: - """Initialize DataManager with default values.""" - # State groups with individual locks + """Initialize DataManager with background callback processing.""" self._controller_state = ControllerState() self._teleop_state = TeleopState() self._robot_state = RobotState() @@ -123,12 +119,20 @@ def __init__(self) -> None: # System state self._shutdown_event = threading.Event() - # Callback for state changes (RGB, target joints, current joints). - # The callable takes (name: str, payload: dict[str, Any], timestamp: float). - # payload is the data dict e.g. {joint1: v, joint2: v}, {gripper: v}, {rgb_scene: array}. + # Asynchronous processing elements self._on_change_callback: ( Callable[[str, dict[str, Any], float], None] | None ) = None + + # Maxsize 60 matches ~1 second of video frames buffer if disk spikes + self._callback_queue: queue.Queue = queue.Queue(maxsize=60) + + self._worker_thread = threading.Thread( + target=self._callback_worker_loop, + name="NeuracoreCallbackWorker", + daemon=True + ) + self._worker_thread.start() def set_on_change_callback( self, on_change_callback: Callable[[str, dict[str, Any], float], None] @@ -136,6 +140,35 @@ def set_on_change_callback( """Set on change callback (thread-safe).""" self._on_change_callback = on_change_callback + def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None: + """Helper to push payloads into the execution queue without blocking.""" + if self._on_change_callback is None: + return + + try: + # put_nowait drops data into the memory queue instantly (0.0ms blocking) + self._callback_queue.put_nowait((name, payload, timestamp)) + except queue.Full: + # Prevents out-of-memory if disk halts completely, without freezing telemetry loops + print(f"⚠️ Neuracore background queue full! Dropping log packet: {name}") + + def _callback_worker_loop(self) -> None: + """Background thread worker loop dedicated solely to performing slow disk IO updates.""" + while not self._shutdown_event.is_set() or not self._callback_queue.empty(): + try: + # Wait up to 100ms for a logging event + name, payload, timestamp = self._callback_queue.get(timeout=0.1) + + if self._on_change_callback is not None: + # Execute Neuracore disk operation safely here on a separate core + self._on_change_callback(name, payload, timestamp) + + self._callback_queue.task_done() + except queue.Empty: + continue + except Exception as e: + print(f"❌ Error in background logging callback: {e}") + # ============================================================================ # Camera State Methods # ============================================================================ @@ -145,28 +178,24 @@ def get_rgb_image(self, camera_name: str) -> np.ndarray | None: with self._camera_state._lock: if not self._camera_state.rgb_images: return None - img = self._camera_state.rgb_images.get(camera_name) return img.copy() if img is not None else None def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: - """Set RGB image for a specific camera (thread-safe).""" + """Set RGB image for a specific camera (thread-safe and non-blocking).""" with self._camera_state._lock: self._camera_state.rgb_images[camera_name] = image.copy() + if self._on_change_callback: img_copy = self._camera_state.rgb_images[camera_name].copy() - self._on_change_callback("log_rgb", {camera_name: img_copy}, time.time()) + # Queue it instead of executing directly! Camera loop returns immediately. + self._queue_callback("log_rgb", {camera_name: img_copy}, time.time()) # ============================================================================ # Controller State Methods # ============================================================================ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: - """Get current controller data (thread-safe). - - Returns: - Tuple of (controller_transform, grip_value, trigger_value) - """ with self._controller_state._lock: return ( ( @@ -181,18 +210,6 @@ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: def set_controller_data( self, transform: np.ndarray | None, grip: float, trigger: float ) -> None: - """Set controller data (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None - grip: float - grip value - trigger: float - trigger value - - Raises: - ValueError: If the transform is not a 4x4 matrix - ValueError: If the grip value is not between 0.0 and 1.0 - ValueError: If the trigger value is not between 0.0 and 1.0 - """ if transform is not None and transform.shape != (4, 4): raise ValueError("Transform must be a 4x4 matrix") if grip < 0.0 or grip > 1.0: @@ -206,11 +223,8 @@ def set_controller_data( if transform is not None: current_time = time.time() - - # Store raw transform self._controller_state.transform_raw = transform.copy() - # Initialize filter if needed if self._controller_state._filter is None: self._controller_state._filter = OneEuroFilterTransform( current_time, @@ -221,45 +235,28 @@ def set_controller_data( ) self._controller_state.transform = transform.copy() else: - # Update filter parameters if they changed self._controller_state._filter.update_params( self._controller_state.min_cutoff, self._controller_state.beta, self._controller_state.d_cutoff, ) - - # Apply filter self._controller_state.transform = self._controller_state._filter( current_time, transform ) else: self._controller_state.transform = None self._controller_state.transform_raw = None - self._controller_state._filter = ( - None # Reset filter when transform is None - ) + self._controller_state._filter = None def set_controller_filter_params( self, min_cutoff: float, beta: float, d_cutoff: float ) -> None: - """Update 1€ Filter parameters for controller transform (thread-safe). - - Args: - min_cutoff: Minimum cutoff frequency (stabilizes when holding still) - beta: Speed coefficient (reduces lag when moving) - d_cutoff: Cutoff frequency for derivative filtering - """ with self._controller_state._lock: self._controller_state.min_cutoff = min_cutoff self._controller_state.beta = beta self._controller_state.d_cutoff = d_cutoff def get_controller_filter_params(self) -> tuple[float, float, float]: - """Get 1€ Filter parameters for controller transform (thread-safe). - - Returns: - Tuple of (min_cutoff, beta, d_cutoff) - """ with self._controller_state._lock: return ( self._controller_state.min_cutoff, @@ -277,13 +274,6 @@ def set_teleop_state( controller_initial: np.ndarray | None, robot_initial: np.ndarray | None, ) -> None: - """Set teleoperation state (thread-safe). - - Args: - active: bool - whether teleop is active - controller_initial: np.ndarray | None - 4x4 transformation matrix for initial controller transform or None to clear - robot_initial: np.ndarray | None - 4x4 transformation matrix for initial robot transform or None to clear - """ with self._teleop_state._lock: self._teleop_state.active = active self._teleop_state.controller_initial_transform = ( @@ -296,26 +286,13 @@ def set_teleop_state( def set_teleop_scaling( self, translation_scale: float, rotation_scale: float ) -> None: - """Set teleoperation scaling factors (thread-safe). - - Args: - translation_scale: Scale applied to controller translation deltas - rotation_scale: Scale applied to controller rotation deltas - """ - # Ignore invalid values (e.g. transient 0 while typing) and keep old scaling. if translation_scale <= 0.0 or rotation_scale <= 0.0: return - with self._teleop_state._lock: self._teleop_state.translation_scale = translation_scale self._teleop_state.rotation_scale = rotation_scale def get_teleop_scaling(self) -> tuple[float, float]: - """Get teleoperation scaling factors (thread-safe). - - Returns: - Tuple of (translation_scale, rotation_scale) - """ with self._teleop_state._lock: return ( self._teleop_state.translation_scale, @@ -323,17 +300,14 @@ def get_teleop_scaling(self) -> tuple[float, float]: ) def get_teleop_active(self) -> bool: - """Get teleoperation active state (thread-safe).""" with self._teleop_state._lock: return self._teleop_state.active def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: - """Set slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = enabled def toggle_slow_scaling_mode_enabled(self) -> bool: - """Toggle and return slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = ( not self._teleop_state.slow_scaling_mode_enabled @@ -341,21 +315,12 @@ def toggle_slow_scaling_mode_enabled(self) -> bool: return self._teleop_state.slow_scaling_mode_enabled def get_slow_scaling_mode_enabled(self) -> bool: - """Get slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: return self._teleop_state.slow_scaling_mode_enabled def get_initial_robot_controller_transforms( self, ) -> tuple[np.ndarray | None, np.ndarray | None]: - """Get initial robot and controller transforms. - - These two transforms are captured on rising edge of grip button - and reset on falling edge of grip button. (thread-safe) - - Returns: - Tuple of (controller_initial_transform, robot_initial_transform) - """ with self._teleop_state._lock: return ( ( @@ -375,29 +340,14 @@ def get_initial_robot_controller_transforms( # ============================================================================ def get_robot_activity_state(self) -> RobotActivityState: - """Get robot activity state (thread-safe). - - Returns: - RobotActivityState - current robot activity state - """ with self._robot_state._lock: return self._robot_state.activity_state def set_robot_activity_state(self, state: RobotActivityState) -> None: - """Set robot activity state (thread-safe). - - Args: - state: RobotActivityState - new robot activity state - """ with self._robot_state._lock: self._robot_state.activity_state = state def get_current_joint_angles(self) -> np.ndarray | None: - """Get current joint angles (thread-safe). - - Returns: - Current joint angles or None if not available - """ with self._robot_state._lock: return ( self._robot_state.joint_angles.copy() @@ -406,11 +356,6 @@ def get_current_joint_angles(self) -> np.ndarray | None: ) def set_current_joint_angles(self, angles: np.ndarray) -> None: - """Set current joint angles (thread-safe). - - Args: - angles: np.ndarray - current joint angles - """ with self._robot_state._lock: self._robot_state.joint_angles = angles.copy() if self._on_change_callback: @@ -419,14 +364,9 @@ def set_current_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._on_change_callback("log_joint_positions", payload, time.time()) + self._queue_callback("log_joint_positions", payload, time.time()) def get_current_joint_torques(self) -> np.ndarray | None: - """Get current joint torques/currents proxy (thread-safe). - - Returns: - Current joint torques/currents vector or None if not available - """ with self._robot_state._lock: return ( self._robot_state.joint_torques.copy() @@ -435,25 +375,15 @@ def get_current_joint_torques(self) -> np.ndarray | None: ) def set_current_joint_torques(self, torques: np.ndarray) -> None: - """Set current joint torques/currents proxy and log to NeuraCore. - - Args: - torques: np.ndarray - current joint torques/currents vector - """ with self._robot_state._lock: self._robot_state.joint_torques = torques.copy() if self._on_change_callback: torques = self._robot_state.joint_torques if torques is not None: payload = {jn: float(torques[i]) for i, jn in enumerate(JOINT_NAMES)} - self._on_change_callback("log_joint_torques", payload, time.time()) + self._queue_callback("log_joint_torques", payload, time.time()) def get_current_end_effector_pose(self) -> np.ndarray | None: - """Get current end effector pose (thread-safe). - - Returns: - Current end effector pose or None if not available - """ with self._robot_state._lock: return ( self._robot_state.end_effector_pose.copy() @@ -462,61 +392,32 @@ def get_current_end_effector_pose(self) -> np.ndarray | None: ) def set_current_end_effector_pose(self, pose: np.ndarray) -> None: - """Set current end effector pose (thread-safe). - - Args: - pose: np.ndarray - current end effector pose - """ with self._robot_state._lock: self._robot_state.end_effector_pose = pose.copy() def get_current_gripper_open_value(self) -> float | None: - """Get current gripper open value (thread-safe). - - Returns: - Current gripper open value or None if not available - """ with self._robot_state._lock: - return ( - self._robot_state.current_gripper_open_value - if self._robot_state.current_gripper_open_value is not None - else None - ) + return self._robot_state.current_gripper_open_value def set_current_gripper_open_value(self, value: float) -> None: - """Set current gripper open value (thread-safe). - - Args: - value: float - current gripper open value - """ with self._robot_state._lock: self._robot_state.current_gripper_open_value = value if self._on_change_callback: - self._on_change_callback( + self._queue_callback( "log_parallel_gripper_open_amounts", {GRIPPER_NAME: value}, time.time(), ) def get_target_gripper_open_value(self) -> float | None: - """Get target gripper open value (thread-safe). - - Returns: - Target gripper open value or None if not available - """ with self._robot_state._lock: return self._robot_state.target_gripper_open_value def set_target_gripper_open_value(self, value: float) -> None: - """Set target gripper open value (thread-safe). - - Args: - value: float - target gripper open value - """ with self._robot_state._lock: self._robot_state.target_gripper_open_value = value if self._on_change_callback: - self._on_change_callback( + self._queue_callback( "log_parallel_gripper_target_open_amounts", {GRIPPER_NAME: self._robot_state.target_gripper_open_value}, time.time(), @@ -527,11 +428,6 @@ def set_target_gripper_open_value(self, value: float) -> None: # ============================================================================ def get_target_joint_angles(self) -> np.ndarray | None: - """Get current joint configuration (thread-safe). - - Returns: - Current target joint angles or None if not available - """ with self._ik_state._lock: return ( self._ik_state.target_joint_angles.copy() @@ -540,11 +436,6 @@ def get_target_joint_angles(self) -> np.ndarray | None: ) def set_target_joint_angles(self, angles: np.ndarray) -> None: - """Set target joint angles (thread-safe). - - Args: - angles: np.ndarray - target joint angles - """ with self._ik_state._lock: self._ik_state.target_joint_angles = angles.copy() if self._on_change_callback: @@ -553,27 +444,17 @@ def set_target_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._on_change_callback( + self._queue_callback( "log_joint_target_positions", payload, time.time() ) def set_target_pose(self, transform: np.ndarray | None) -> None: - """Set target transform for visualization (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None to clear target transform - """ with self._ik_state._lock: self._ik_state.target_pose = ( transform.copy() if transform is not None else None ) def get_target_pose(self) -> np.ndarray | None: - """Get target transform for visualization (thread-safe). - - Returns: - Target transform or None if target transform is not set - """ with self._ik_state._lock: return ( self._ik_state.target_pose.copy() @@ -582,38 +463,18 @@ def get_target_pose(self) -> np.ndarray | None: ) def set_ik_solve_time_ms(self, time_ms: float) -> None: - """Set IK solve time (thread-safe). - - Args: - time_ms: float - IK solve time in milliseconds - """ with self._ik_state._lock: self._ik_state.solve_time_ms = time_ms def set_ik_success(self, success: bool) -> None: - """Set IK success (thread-safe). - - Args: - success: bool - True if IK was successful, False otherwise - """ with self._ik_state._lock: self._ik_state.success = success def get_ik_solve_time_ms(self) -> float: - """Get IK solve time (thread-safe). - - Returns: - IK solve time in milliseconds - """ with self._ik_state._lock: return self._ik_state.solve_time_ms def get_ik_success(self) -> bool: - """Get IK success (thread-safe). - - Returns: - True if IK was successful, False otherwise - """ with self._ik_state._lock: return self._ik_state.success @@ -622,13 +483,9 @@ def get_ik_success(self) -> bool: # ============================================================================ def request_shutdown(self) -> None: - """Request shutdown of all threads (lock-free using Event).""" + """Request shutdown of all threads.""" self._shutdown_event.set() def is_shutdown_requested(self) -> bool: - """Check if shutdown is requested (lock-free using Event). - - Returns: - True if shutdown is requested, False otherwise - """ - return self._shutdown_event.is_set() + """Check if shutdown is requested.""" + return self._shutdown_event.is_set() \ No newline at end of file diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 42e90b6..37dda03 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -90,14 +90,7 @@ def get_policy_rgb_image_input(self) -> np.ndarray | None: ) def set_policy_rgb_image_input(self, image: np.ndarray) -> None: - """Set policy RGB image (thread-safe). - - Raises: - RuntimeError: If policy inputs are locked (during execution). - """ - with self._execution_lock: - if self._policy_inputs_locked: - raise RuntimeError("Policy inputs are locked during execution") + """Set policy RGB image (thread-safe).""" with self._policy_rgb_image_input_lock: self._policy_rgb_image_input = image.copy() if image is not None else None @@ -111,14 +104,7 @@ def get_policy_state_input(self) -> np.ndarray | None: ) def set_policy_state_input(self, input: np.ndarray) -> None: - """Set policy state input (thread-safe). - - Raises: - RuntimeError: If policy inputs are locked (during execution). - """ - with self._execution_lock: - if self._policy_inputs_locked: - raise RuntimeError("Policy inputs are locked during execution") + """Set policy state input (thread-safe).""" with self._policy_state_input_lock: self._policy_state_input = input.copy() if input is not None else None diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index 0547095..e8a3602 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -40,7 +40,7 @@ def __init__(self, urdf_path: str) -> None: self._server, ghost_urdf, root_node_name="/robot_ghost", - mesh_color_override=(0.2, 0.4, 1.0, 0.25), # Blue with 60% opacity + mesh_color_override=(1.0, 0.65, 0.0, 0.25), # Orange with 25% opacity ) # GUI handles (initialized as None, created on demand) - all private @@ -92,7 +92,6 @@ def __init__(self, urdf_path: str) -> None: self._execution_mode_dropdown = None self._run_policy_button = None self._start_policy_execution_button = None - self._run_and_start_policy_execution_button = None self._play_policy_button = None # Internal state @@ -675,14 +674,11 @@ def add_policy_controls( def add_policy_buttons(self) -> None: """Add policy control buttons.""" - self._run_policy_button = self._server.gui.add_button("Run Policy") + self._run_policy_button = self._server.gui.add_button("Run Policy (Preview)") self._start_policy_execution_button = self._server.gui.add_button( - "Start Policy Execution" + "Execute Policy (Run Preview)" ) - self._run_and_start_policy_execution_button = self._server.gui.add_button( - "Run and Execute Policy" - ) - self._play_policy_button = self._server.gui.add_button("Play Policy") + self._play_policy_button = self._server.gui.add_button("Continuous Receding Horizon") def update_policy_status(self, status: str) -> None: """Update policy status display. @@ -773,16 +769,6 @@ def set_start_policy_execution_callback(self, callback: Callable[[], Any]) -> No if self._start_policy_execution_button is not None: self._start_policy_execution_button.on_click(lambda _: callback()) - def set_run_and_start_policy_execution_callback( - self, callback: Callable[[], Any] - ) -> None: - """Set callback for Run and Execute Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._run_and_start_policy_execution_button is not None: - self._run_and_start_policy_execution_button.on_click(lambda _: callback()) def set_play_policy_callback(self, callback: Callable[[], Any]) -> None: """Set callback for Play Policy button. @@ -856,16 +842,6 @@ def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: if self._start_policy_execution_button is not None: self._start_policy_execution_button.disabled = disabled - def set_run_and_start_policy_execution_button_disabled( - self, disabled: bool - ) -> None: - """Set Run and Execute Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._run_and_start_policy_execution_button is not None: - self._run_and_start_policy_execution_button.disabled = disabled def set_play_policy_button_disabled(self, disabled: bool) -> None: """Set Play Policy button disabled state. @@ -883,8 +859,18 @@ def update_play_policy_button_status(self, active: bool) -> None: active: Whether continuous play is currently active """ if self._play_policy_button is not None: - self._play_policy_button.label = "Stop Policy" if active else "Play Policy" + self._play_policy_button.label = ( + "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + ) + def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + """Set the color of the ghost robot. + Args: + color: RGBA tuple (0.0 to 1.0) + """ + if self._ghost_robot_urdf is not None: + self._ghost_robot_urdf.mesh_color_override = color + def stop(self) -> None: """Stop the visualizer server.""" self._server.stop() diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py index 24e6dfe..8573eae 100644 --- a/examples/common/threads/realsense_camera.py +++ b/examples/common/threads/realsense_camera.py @@ -1,7 +1,9 @@ -"""Camera thread - captures RGB images from RealSense.""" +"""Camera thread - captures RGB images from RealSense with drop detection.""" import time import traceback +from collections import deque +import cv2 import numpy as np import pyrealsense2 as rs @@ -10,13 +12,21 @@ def camera_thread(data_manager: DataManager) -> None: - """Camera thread - captures RGB images from RealSense.""" + """Camera thread - captures RGB images from RealSense and monitors health.""" print("📷 Camera thread started") camera_name = CAMERA_NAMES[0] - dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE pipeline: rs.pipeline | None = None + # Diagnostic variables + last_frame_number = None + total_dropped_frames = 0 + fps_timer = time.time() + frame_count = 0 + # Store the last 100 frame intervals to check for extreme jitter + intervals = deque(maxlen=100) + last_frame_time = time.time() + try: # Configure RealSense pipeline pipeline = rs.pipeline() @@ -34,26 +44,67 @@ def camera_thread(data_manager: DataManager) -> None: pipeline.start(config) while not data_manager.is_shutdown_requested(): - iteration_start = time.time() - try: + # wait_for_frames naturally blocks at the target framerate (e.g., 60Hz) frames = pipeline.wait_for_frames(timeout_ms=500) except Exception as e: - print(f"⚠️ RealSense wait for frames error: {e}") + print(f"⚠️ RealSense wait for frames error (Timeout?): {e}") continue color_frame = frames.get_color_frame() + if not color_frame: + continue + + current_time = time.time() + intervals.append(current_time - last_frame_time) + last_frame_time = current_time + + # --------------------------------------------------------- + # DIAGNOSTICS: Check for dropped frames via hardware ID + # --------------------------------------------------------- + current_frame_number = color_frame.get_frame_number() + + if last_frame_number is not None: + # If frame numbers are not sequential, we missed something in software + drops = (current_frame_number - last_frame_number) - 1 + if drops > 0: + total_dropped_frames += drops + print(f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}") + + last_frame_number = current_frame_number + + # --------------------------------------------------------- + # DIAGNOSTICS: Calculate software-side FPS and Jitter + # --------------------------------------------------------- + frame_count += 1 + if current_time - fps_timer >= 5.0: # Report every 5 seconds + effective_fps = frame_count / (current_time - fps_timer) + + # Calculate jitter (max variance between frame arrivals) + max_interval = max(intervals) * 1000 # in ms + min_interval = min(intervals) * 1000 # in ms + + print(f"📊 Camera Health: {effective_fps:.1f} FPS | " + f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " + f"Total Drops: {total_dropped_frames}") + + # Reset counters for the next 5-second window + fps_timer = current_time + frame_count = 0 + + # --------------------------------------------------------- + # IMAGE PROCESSING + # --------------------------------------------------------- + # color_image = np.asanyarray(color_frame.get_data()) + # color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + # data_manager.set_rgb_image(color_image, camera_name) - if color_frame: - color_image = np.asanyarray(color_frame.get_data()) - color_image = np.rot90(color_image, k=3) # Rotate 270 degrees - data_manager.set_rgb_image(color_image, camera_name) + color_image = np.asanyarray(color_frame.get_data()) + color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + data_manager.set_rgb_image(color_image, camera_name) - # Sleep to maintain loop rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) + # Notice: The time.sleep() has been completely removed. + # wait_for_frames() manages the loop pace perfectly. except Exception as e: print(f"❌ Camera thread error: {e}") @@ -66,4 +117,4 @@ def camera_thread(data_manager: DataManager) -> None: print(" ✓ RealSense pipeline stopped") except Exception as e: print(f"⚠️ Error stopping pipeline: {e}") - print("📷 Camera thread stopped") + print("📷 Camera thread stopped") \ No newline at end of file diff --git a/halid_notes.txt b/halid_notes.txt new file mode 100644 index 0000000..e69de29 diff --git a/piper_controller.py b/piper_controller.py index e979bcc..63b96a4 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -91,7 +91,7 @@ def __init__( # Gripper range in degrees (for internal SDK communication) self.GRIPPER_DEGREES_MIN = -10.00 - self.GRIPPER_DEGREES_MAX = 30.00 + self.GRIPPER_DEGREES_MAX = 100.00 self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees @@ -223,18 +223,34 @@ def get_control_mode(self) -> "PiperController.ControlMode": with self.state_lock: return self._control_mode - def set_control_mode(self, mode: "PiperController.ControlMode") -> None: - """Set the control mode. + # def set_control_mode(self, mode: "PiperController.ControlMode") -> None: + # """Set the control mode. - Args: - mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE) - """ + # Args: + # mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE) + # """ + # with self.state_lock: + # old_mode = self._control_mode + # self._control_mode = mode + # if self.debug_mode: + # print(f"Control mode changed: {old_mode.value} -> {mode.value}") + + + def set_control_mode(self, mode: "PiperController.ControlMode") -> None: with self.state_lock: old_mode = self._control_mode self._control_mode = mode + + # Send the mode configuration ONCE here, not in the 100Hz loop + if hasattr(self, "piper") and self.piper.get_connect_status(): + if mode == PiperController.ControlMode.JOINT_SPACE: + self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) + elif mode == PiperController.ControlMode.END_EFFECTOR: + self.piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) + if self.debug_mode: print(f"Control mode changed: {old_mode.value} -> {mode.value}") - + @staticmethod def _pose_6d_to_4x4(pose_6d: np.ndarray) -> np.ndarray: """Convert 6D pose [x, y, z, rx, ry, rz] to 4x4 transformation matrix. From 2f669cab61abe304d1f991f4e211f154e650df5a Mon Sep 17 00:00:00 2001 From: Halid Date: Thu, 21 May 2026 11:56:57 +0100 Subject: [PATCH 03/22] fix the seuqntial inference --- examples/4_rollout_neuracore_policy.py | 2 +- examples/common/configs.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 5f490bf..459c598 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -745,7 +745,7 @@ def update_visualization( "--continuous-mode", type=str, choices=["pipeline", "sequential"], - default="pipeline", + default="sequential", help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", ) diff --git a/examples/common/configs.py b/examples/common/configs.py index e2d04a3..eff056d 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -57,8 +57,8 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] # NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] -# NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) -NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid +NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) +# NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid # Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] From d8e427f9e55d2ca8253cc5099bc81bc5d789087c Mon Sep 17 00:00:00 2001 From: Halid Date: Tue, 26 May 2026 11:57:56 +0100 Subject: [PATCH 04/22] fix:agile_x is working with teleoperation smoothly --- .../2_collect_teleop_data_with_neuracore.py | 17 ++++++ examples/combine_code.py | 54 +++++++++++++++++++ examples/common/configs.py | 4 +- examples/common/policy_state.py | 2 +- 4 files changed, 74 insertions(+), 3 deletions(-) create mode 100644 examples/combine_code.py diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 2d04bd5..434a9b5 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -8,6 +8,7 @@ import argparse import multiprocessing +import subprocess import sys import threading import time @@ -60,12 +61,26 @@ from piper_controller import PiperController +def play_audio_feedback(action: str) -> None: + """Play distinct synthesized tones asynchronously using SoX.""" + # Start = High pitch (880 Hz), Stop = Low pitch (440 Hz) + freq = "880" if action == "start" else "440" + try: + # -q: quiet, -n: no input file, synth: generate audio, 0.3: duration, sine: wave type + subprocess.Popen( + ["play", "-q", "-n", "synth", "0.3", "sine", freq], + stderr=subprocess.DEVNULL + ) + except Exception as e: + print(f"⚠️ Failed to play tone (is SoX installed?): {e}") + def log_to_neuracore_on_change_callback( name: str, payload: dict[str, Any], timestamp: float ) -> None: """Log data to queue on change callback.""" # Call appropriate Neuracore logging function try: + #print(f"\n📤 Logging {name} to Neuracore with timestamp {timestamp:.3f}...") if name == "log_joint_positions": nc.log_joint_positions(payload, timestamp=timestamp) elif name == "log_joint_torques": @@ -131,6 +146,7 @@ def on_button_rj_pressed() -> None: try: nc.start_recording() print("✓ 🔴 Recording started (Button RJ)") + play_audio_feedback("start") # <-- Trigger distinct START sound except Exception as e: print(f"✗ Failed to start recording. Exception: {e}") print("Traceback:") @@ -140,6 +156,7 @@ def on_button_rj_pressed() -> None: try: nc.stop_recording() print("✓ ⏹️ Recording stopped (Button RJ)") + play_audio_feedback("stop") # <-- Trigger distinct STOP sound except Exception as e: print(f"✗ Failed to stop recording. Exception: {e}") print("Traceback:") diff --git a/examples/combine_code.py b/examples/combine_code.py new file mode 100644 index 0000000..043cf46 --- /dev/null +++ b/examples/combine_code.py @@ -0,0 +1,54 @@ +import pathlib + +def combine_python_files(directory_path, output_filename): + """ + Recursively finds all .py files in a directory and combines them into a single text file. + + Args: + directory_path (str): The path to the root directory to search. + output_filename (str): The name/path of the output text file. + """ + # Create a Path object for the target directory + source_dir = pathlib.Path(directory_path) + + # Ensure the directory exists + if not source_dir.is_dir(): + print(f"Error: The directory '{directory_path}' does not exist.") + return + + # Open the output file in write mode + with open(output_filename, 'w', encoding='utf-8') as outfile: + # rglob('*.py') recursively searches for all files ending in .py + py_files = list(source_dir.rglob('*.py')) + + if not py_files: + print(f"No Python files found in '{directory_path}'.") + return + + print(f"Found {len(py_files)} Python files. Combining...") + + for file_path in py_files: + # Write a clear separator and the file path as a header + outfile.write(f"\n{'='*60}\n") + outfile.write(f"FILE: {file_path}\n") + outfile.write(f"{'='*60}\n\n") + + # Read the python file and append its contents + try: + with open(file_path, 'r', encoding='utf-8') as infile: + outfile.write(infile.read()) + outfile.write("\n") + except Exception as e: + error_msg = f"Error reading file {file_path}: {e}\n" + print(error_msg) + outfile.write(error_msg) + + print(f"Success! All files have been combined into '{output_filename}'.") + +# --- Example Usage --- +if __name__ == "__main__": + # Replace these variables with your actual paths + TARGET_DIRECTORY = "./" # "./" means current directory + OUTPUT_FILE = "combined_code.txt" + + combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) \ No newline at end of file diff --git a/examples/common/configs.py b/examples/common/configs.py index eff056d..e65110c 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -58,7 +58,7 @@ # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] # NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) -# NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid # Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] @@ -71,7 +71,7 @@ PREDICTION_HORIZON_EXECUTION_RATIO = ( 1.0 # percentage of the prediction horizon that is executed ) -MAX_SAFETY_THRESHOLD = 50.0 # degrees +MAX_SAFETY_THRESHOLD = 200.0 # degrees MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 37dda03..8695aa1 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -20,7 +20,7 @@ def __init__(self) -> None: # Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names self._prediction_horizon: dict[str, list[float]] = {} self._prediction_horizon_lock = threading.Lock() - self._execution_ratio: float = 1.0 + self._execution_ratio: float = 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in self._policy_rgb_image_input: np.ndarray | None = None self._policy_rgb_image_input_lock = threading.Lock() From 12c69a30d6679336cf2765507ec28440b391db48 Mon Sep 17 00:00:00 2001 From: Halid Date: Tue, 26 May 2026 14:19:14 +0100 Subject: [PATCH 05/22] refector:make the examples smaller --- examples/1_tune_teleop_params.py | 581 +- .../2_collect_teleop_data_with_neuracore.py | 414 +- examples/3_replay_neuracore_episodes.py | 213 +- examples/4_rollout_neuracore_policy.py | 1073 +-- .../5_rollout_neuracore_policy_minimal.py | 311 +- examples/6_visualize_policy_from_dataset.py | 324 +- examples/7_teleop_with_pedal.py | 268 +- examples/combined_code_old.txt | 6025 +++++++++++++++++ examples/common/configs.py | 4 +- examples/common/data_manager.py | 33 +- examples/common/dataset_helpers.py | 40 + examples/common/policy_actions.py | 269 + examples/common/robot_visualizer.py | 889 +-- examples/common/shared_actions.py | 96 + examples/common/states.py | 61 + examples/common/system_bootstrap.py | 87 + examples/common/visualizer_core.py | 92 + examples/common/visualizer_gui.py | 201 + 18 files changed, 7410 insertions(+), 3571 deletions(-) create mode 100644 examples/combined_code_old.txt create mode 100644 examples/common/dataset_helpers.py create mode 100644 examples/common/policy_actions.py create mode 100644 examples/common/shared_actions.py create mode 100644 examples/common/states.py create mode 100644 examples/common/system_bootstrap.py create mode 100644 examples/common/visualizer_core.py create mode 100644 examples/common/visualizer_gui.py diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index 3a8ea8e..49e7d1e 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -1,12 +1,5 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller - REAL ROBOT CONTROL. - -This demo uses Pink IK control with Meta Quest controller input to control the REAL robot. -- REAL ROBOT CONTROL - sends commands to physical robot! -- Uses right hand controller grip as dead man's button -- Uses ROS pointer frame for natural pointing control -- Applies relative transformations accounting for different coordinate frames -""" +"""Piper Robot Teleoperation with Meta Quest Controller - REAL ROBOT CONTROL.""" import argparse import sys @@ -14,464 +7,168 @@ import time import traceback from pathlib import Path - import numpy as np -# Add parent directory to path to import pink_ik_solver and piper_controller +# Add parent directory to path to import local modules sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, - CAMERA_NAMES, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - IK_SOLVER_RATE, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - META_QUEST_AXIS_MASK, - NEUTRAL_END_EFFECTOR_POSE, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POSITION_COST, - POSTURE_COST_VECTOR, - ROBOT_RATE, - ROTATION_SCALE, - SLOW_ROTATION_SCALE, - SLOW_TRANSLATION_SCALE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, - TRANSLATION_SCALE, - URDF_PATH, - VISUALIZATION_RATE, - WRIST_JOINT_BUTTON_STEP_DEGREES, + CAMERA_NAMES, META_QUEST_AXIS_MASK, ROTATION_SCALE, + SLOW_ROTATION_SCALE, SLOW_TRANSLATION_SCALE, TRANSLATION_SCALE, + URDF_PATH, VISUALIZATION_RATE, WRIST_JOINT_BUTTON_STEP_DEGREES, + CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF, + POSITION_COST, ORIENTATION_COST, FRAME_TASK_GAIN, LM_DAMPING, + DAMPING_COST, SOLVER_DAMPING_VALUE, POSTURE_COST_VECTOR ) -from common.data_manager import DataManager, RobotActivityState +from common.data_manager import RobotActivityState +from common.system_bootstrap import bootstrap_robot_system +from common.shared_actions import toggle_robot_enabled, move_robot_home from common.robot_visualizer import RobotVisualizer -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread from common.threads.quest_reader import quest_reader_thread -from common.threads.realsense_camera import camera_thread from meta_quest_teleop.reader import MetaQuestReader - -from pink_ik_solver import PinkIKSolver from piper_controller import PiperController -def on_button_a_pressed() -> None: - """Handle Button A press to toggle robot enable/disable state.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - print("✓ 🔴 Robot disabled (Button A)") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Button A)") - else: - print("✗ Failed to enable robot") - - -def on_button_b_pressed() -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 Button B pressed - Moving to home position...") - # Set state to HOMING to prevent IK thread from sending robot commands - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - # Disable teleop during homing - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Button B pressed but robot is not enabled") - - -def _step_wrist_joint(delta_degrees: float) -> None: +def _step_wrist_joint(data_manager, robot_controller, delta_degrees: float) -> None: """Apply a relative step to the wrist joint target angle.""" - # Prevent IK teleop loop from overwriting this manual joint adjustment. data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) target_joint_angles = robot_controller.get_target_joint_angles() current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - base_wrist_joint_angle = float(current_joint_angles[-1]) - else: - base_wrist_joint_angle = float(target_joint_angles[-1]) + base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1]) - target_joint_angles[-1] = base_wrist_joint_angle + delta_degrees + target_joint_angles[-1] = base_wrist + delta_degrees robot_controller.set_target_joint_angles(target_joint_angles) data_manager.set_target_joint_angles(target_joint_angles) -def on_button_y_pressed() -> None: - """Handle Button Y press to add +5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button Y pressed but robot is not enabled") - return - _step_wrist_joint(WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_x_pressed() -> None: - """Handle Button X press to subtract -5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button X pressed but robot is not enabled") - return - _step_wrist_joint(-WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_lj_pressed() -> None: - """Toggle slow translation/rotation scaling mode from config values.""" - slow_scaling_mode_enabled = data_manager.toggle_slow_scaling_mode_enabled() - if slow_scaling_mode_enabled: - print( - "🐢 Button LJ pressed - Slow scaling enabled " - f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " - f"rotation={SLOW_ROTATION_SCALE:.3f})" - ) +def toggle_slow_scaling(data_manager): + """Toggle the movement scaling mode.""" + enabled = data_manager.toggle_slow_scaling_mode_enabled() + if enabled: + print(f"🐢 Slow scaling enabled (trans={SLOW_TRANSLATION_SCALE:.3f}, rot={SLOW_ROTATION_SCALE:.3f})") else: - print( - "🐇 Button LJ pressed - Slow scaling disabled " - f"(using GUI/default scales, currently translation={TRANSLATION_SCALE:.3f}, " - f"rotation={ROTATION_SCALE:.3f})" - ) - - -parser = argparse.ArgumentParser( - description="Piper Robot Teleoperation - REAL ROBOT CONTROL" -) -parser.add_argument( - "--ip-address", - type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", -) -args = parser.parse_args() - -print("=" * 60) -print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL") -print("=" * 60) -print("Thread frequencies:") -print(f" 🎮 Quest Reader: {CONTROLLER_DATA_RATE} Hz") -print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") -print(f" 🖥️ Visualization: {VISUALIZATION_RATE} Hz (running in the main thread)") -print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") -print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") -print(f" 📷 Camera: {CAMERA_FRAME_STREAMING_RATE} Hz") - - -# Initialize shared state -data_manager = DataManager() -data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, -) - -# Initialize robot controller -print("\n🤖 Initializing Piper robot controller...") -robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, - enable_joint_angle_limits=False, - debug_mode=False, -) - -# Start robot control loop -print("\n🚀 Starting robot control loop...") -robot_controller.start_control_loop() - -# Start joint state thread -print("\n📊 Starting joint state thread...") -joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True -) -joint_state_thread_obj.start() - -# Initialize Meta Quest reader -print("\n🎮 Initializing Meta Quest reader...") -quest_reader = MetaQuestReader( - ip_address=args.ip_address, - port=5555, - run=True, - axis_mask=META_QUEST_AXIS_MASK, -) - -# Register button callbacks (after state and robot_controller are initialized) -quest_reader.on("button_a_pressed", on_button_a_pressed) -quest_reader.on("button_b_pressed", on_button_b_pressed) -quest_reader.on("button_y_pressed", on_button_y_pressed) -quest_reader.on("button_x_pressed", on_button_x_pressed) -quest_reader.on("button_lj_pressed", on_button_lj_pressed) - -# Start quest reader thread -print("\n🎮 Starting quest reader thread...") -quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True -) -quest_thread.start() - -# set initial configuration to current joint angles -current_joint_angles = data_manager.get_current_joint_angles() -if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) -else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - -# Create Pink IK solver -print("\n🔧 Creating Pink IK solver...") -ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), -) - -# Start IK solver thread -print("\n🧮 Starting IK solver thread...") -ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True -) -ik_thread.start() - -# Start camera thread (if RealSense is available) -print("\n📷 Starting camera thread...") -camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True -) -camera_thread_obj.start() - - -# Set up visualizer -print("\n🖥️ Starting visualization...") -visualizer = RobotVisualizer(urdf_path=URDF_PATH) -visualizer.add_basic_controls() -visualizer.add_teleop_controls() -visualizer.add_gripper_status_controls() -visualizer.add_homing_controls() -visualizer.add_toggle_robot_enabled_status_button() -visualizer.add_rgb_image_placeholder() -visualizer.add_target_frame_visualization() -visualizer.add_controller_filter_controls( - initial_min_cutoff=CONTROLLER_MIN_CUTOFF, - initial_beta=CONTROLLER_BETA, - initial_d_cutoff=CONTROLLER_D_CUTOFF, -) -visualizer.add_scaling_controls( - initial_translation_scale=TRANSLATION_SCALE, - initial_rotation_scale=ROTATION_SCALE, -) -visualizer.add_pink_parameter_controls( - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - posture_cost_vector=POSTURE_COST_VECTOR, -) -visualizer.add_controller_visualization() - - -# Set up button callbacks -def toggle_robot_enabled_status() -> None: - """Toggle robot enabled/disabled state and update GUI button label.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - # Disable robot - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - visualizer.update_toggle_robot_enabled_status(False) - print("✓ 🔴 Robot disabled") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - visualizer.update_toggle_robot_enabled_status(True) - print("✓ 🟢 Robot enabled") - else: - print("✗ Failed to enable robot") - - -def on_go_home() -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 GUI: Moving to home position...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Cannot home: robot not enabled") - - -visualizer.set_toggle_robot_enabled_status_callback(toggle_robot_enabled_status) -visualizer.set_go_home_callback(on_go_home) - -print() -print("🚀 Starting teleoperation with REAL ROBOT CONTROL...") -print("🎮 CONTROLS:") -print(" 1. Press BUTTON A to enable/disable robot (or use GUI)") -print(" 2. Hold RIGHT GRIP to activate teleoperation") -print(" 3. Move controller - robot follows!") -print(" 4. Hold RIGHT TRIGGER to close gripper") -print(" 5. Press BUTTON B to send robot home (or use GUI)") -print(" 6. Press BUTTON Y to add +5° on the wrist joint") -print(" 7. Press BUTTON X to subtract 5° on the wrist joint") -print(" 8. Press LEFT JOYSTICK BUTTON to toggle slow scaling mode") -print(" 9. Release grip to stop") -print(" 10. Use 'Emergency Stop' in GUI if needed") -print("⚠️ Press Ctrl+C to exit") -print() - -# Visualization loop variables -dt: float = 1.0 / VISUALIZATION_RATE - -try: - while True: - iteration_start: float = time.time() - - # Update filter parameters - min_cutoff, beta, d_cutoff = visualizer.get_controller_filter_params() - data_manager.set_controller_filter_params(min_cutoff, beta, d_cutoff) - - # Update scaling factors (shared with IK thread via DataManager) - translation_scale = visualizer.get_translation_scale() - rotation_scale = visualizer.get_rotation_scale() - if data_manager.get_slow_scaling_mode_enabled(): - translation_scale = SLOW_TRANSLATION_SCALE - rotation_scale = SLOW_ROTATION_SCALE - data_manager.set_teleop_scaling(translation_scale, rotation_scale) - - # Update Pink parameters (GUI controls) - pink_params = visualizer.get_pink_parameters() - ik_solver.update_task_parameters(**pink_params) - - # Get data from shared state - controller_transform, grip_value, trigger_value = ( - data_manager.get_controller_data() - ) - teleop_active = data_manager.get_teleop_active() - # Get updated robot_activity_state and joint states (may have changed if homing completed) - robot_activity_state = data_manager.get_robot_activity_state() - current_joint_angles = data_manager.get_current_joint_angles() - target_joint_angles = data_manager.get_target_joint_angles() - solve_time_ms = data_manager.get_ik_solve_time_ms() - ik_success = data_manager.get_ik_success() - target_pose = data_manager.get_target_pose() - rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) - - # Update GUI displays - visualizer.set_grip_value(grip_value) - visualizer.set_trigger_value(trigger_value) - - # Update timing display - visualizer.update_timing(solve_time_ms) - - # Update controller visualization - visualizer.update_controller_visualization(controller_transform) - if controller_transform is not None: + print(f"🐇 Slow scaling disabled (trans={TRANSLATION_SCALE:.3f}, rot={ROTATION_SCALE:.3f})") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Piper Robot Teleoperation") + parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP") + args = parser.parse_args() + + print("=" * 60 + "\nPIPER ROBOT TELEOPERATION\n" + "=" * 60) + + # 1. Bootstrap Core System (Data Manager, Robot Controller, IK Solver, Base Threads) + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + start_ik=True, start_camera=True + ) + + # 2. Initialize Quest Reader + print("\n🎮 Initializing Meta Quest reader...") + quest_reader = MetaQuestReader( + ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ) + + # 3. Initialize Visualizer + print("\n🖥️ Starting visualization...") + visualizer = RobotVisualizer(urdf_path=URDF_PATH) + visualizer.add_basic_controls() + visualizer.add_teleop_controls() + visualizer.add_gripper_status_controls() + visualizer.add_homing_controls() + visualizer.add_toggle_robot_enabled_status_button() + visualizer.add_rgb_image_placeholder() + visualizer.add_target_frame_visualization() + visualizer.add_controller_visualization() + visualizer.add_controller_filter_controls(CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF) + visualizer.add_scaling_controls(TRANSLATION_SCALE, ROTATION_SCALE) + visualizer.add_pink_parameter_controls( + POSITION_COST, ORIENTATION_COST, FRAME_TASK_GAIN, LM_DAMPING, + DAMPING_COST, SOLVER_DAMPING_VALUE, POSTURE_COST_VECTOR + ) + + # 4. Bind Shared Callbacks (Quest Buttons & GUI Buttons) + quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) + quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) + quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, WRIST_JOINT_BUTTON_STEP_DEGREES)) + quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -WRIST_JOINT_BUTTON_STEP_DEGREES)) + quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager)) + + visualizer.set_toggle_robot_enabled_status_callback(lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) + visualizer.set_go_home_callback(lambda: move_robot_home(data_manager, robot_controller)) + + # 5. Start Quest Thread + quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) + quest_thread.start() + active_threads.append(quest_thread) + + print("\n🚀 Ready! Use Meta Quest controllers to move the robot. Press Ctrl+C to exit.\n") + + # 6. Main Visualization Loop + dt = 1.0 / VISUALIZATION_RATE + try: + while True: + iteration_start = time.time() + + # Pass GUI scaling updates to Data Manager + t_scale = SLOW_TRANSLATION_SCALE if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_translation_scale() + r_scale = SLOW_ROTATION_SCALE if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_rotation_scale() + data_manager.set_teleop_scaling(t_scale, r_scale) + + # Retrieve State Data + controller_transform, grip_value, trigger_value = data_manager.get_controller_data() + teleop_active = data_manager.get_teleop_active() + state = data_manager.get_robot_activity_state() + current_joints = data_manager.get_current_joint_angles() + target_joints = data_manager.get_target_joint_angles() + rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) + + # Update GUI Displays + visualizer.set_grip_value(grip_value) + visualizer.set_trigger_value(trigger_value) + visualizer.update_timing(data_manager.get_ik_solve_time_ms()) + visualizer.update_controller_visualization(controller_transform) visualizer.update_controller_status_display( - controller_transform[:3, 3], connected=True + controller_transform[:3, 3] if controller_transform is not None else None, + connected=controller_transform is not None ) - else: - visualizer.update_controller_status_display(None, connected=False) - - # Update teleop status display - visualizer.update_teleop_status(teleop_active) - - # Update RGB camera image in Viser GUI (if available) - if rgb_image is not None: + visualizer.update_teleop_status(teleop_active) + visualizer.update_target_visualization(data_manager.get_target_pose()) visualizer.update_rgb_image(rgb_image) - - # Update target/goal visualization - visualizer.update_target_visualization(target_pose) - - # Update main robot visualization from CURRENT joint angles (DataManager uses degrees, Viser expects radians) - if current_joint_angles is not None: - current_joint_rad = np.radians(current_joint_angles) - visualizer.update_robot_pose(current_joint_rad) - visualizer.update_joint_angles_display(current_joint_rad) - - # Update ghost robot to show TARGET joint angles when available (also in radians) - if ( - target_joint_angles is not None - and robot_activity_state == RobotActivityState.ENABLED - ): - target_joint_rad = np.radians(target_joint_angles) - visualizer.update_ghost_robot_visibility(True) - visualizer.update_ghost_robot_pose(target_joint_rad) - else: - # Hide ghost robot when no valid target is available or robot not enabled - visualizer.update_ghost_robot_visibility(False) - - # Update robot status - simple Enabled/Homing/Disabled - if robot_activity_state == RobotActivityState.ENABLED: - visualizer.update_robot_status("Robot Status: Enabled") - elif robot_activity_state == RobotActivityState.HOMING: - visualizer.update_robot_status("Robot Status: Homing") - else: # DISABLED - visualizer.update_robot_status("Robot Status: Disabled") - - # Update gripper status - visualizer.update_gripper_status( - trigger_value, - robot_enabled=(robot_activity_state == RobotActivityState.ENABLED), - ) - - # Sleep to maintain visualization rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - -except KeyboardInterrupt: - print("\n\n👋 Interrupt received - shutting down gracefully...") -except Exception as e: - print(f"\n❌ Demo error: {e}") - import traceback - - traceback.print_exc() - -# Cleanup (outside try/except so it always runs) -print("\n🧹 Cleaning up...") - -data_manager.request_shutdown() -data_manager.set_robot_activity_state(RobotActivityState.DISABLED) -quest_thread.join() -quest_reader.stop() -ik_thread.join() -joint_state_thread_obj.join() -robot_controller.cleanup() -visualizer.stop() - -print("\n👋 Demo stopped.") + + # Render Actual Robot + if current_joints is not None: + rad_joints = np.radians(current_joints) + visualizer.update_robot_pose(rad_joints) + visualizer.update_joint_angles_display(rad_joints) + + # Render Ghost Robot Target + if target_joints is not None and state == RobotActivityState.ENABLED: + visualizer.update_ghost_robot_visibility(True) + visualizer.update_ghost_robot_pose(np.radians(target_joints)) + else: + visualizer.update_ghost_robot_visibility(False) + + visualizer.update_robot_status(f"Robot Status: {state.value.capitalize()}") + visualizer.update_gripper_status(trigger_value, robot_enabled=(state == RobotActivityState.ENABLED)) + + # Maintain Loop Rate + time.sleep(max(0, dt - (time.time() - iteration_start))) + + except KeyboardInterrupt: + print("\n👋 Shutting down gracefully...") + except Exception as e: + print(f"\n❌ Error: {e}") + traceback.print_exc() + + # 7. Cleanup + print("\n🧹 Cleaning up...") + data_manager.request_shutdown() + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + quest_reader.stop() + for t in active_threads: + t.join() + robot_controller.cleanup() + visualizer.stop() + print("👋 Demo stopped.") \ No newline at end of file diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 434a9b5..8f27be4 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -1,388 +1,104 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller and Neuracore data collection. - -This demo uses Pink IK control with Meta Quest controller input to control the Piper robot and -logs data to Neuracore. -""" - +"""Piper Robot Teleoperation with Meta Quest Controller and Neuracore data collection.""" import argparse import multiprocessing -import subprocess import sys import threading import time import traceback from pathlib import Path -from typing import Any - -import neuracore as nc import numpy as np +import neuracore as nc -# Add parent directory to path to import pink_ik_solver and piper_controller +# Add parent directory to path sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - IK_SOLVER_RATE, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - META_QUEST_AXIS_MASK, - NEUTRAL_END_EFFECTOR_POSE, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POSITION_COST, - POSTURE_COST_VECTOR, - ROBOT_RATE, - ROTATION_SCALE, - SLOW_ROTATION_SCALE, - SLOW_TRANSLATION_SCALE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, - TRANSLATION_SCALE, - URDF_PATH, - WRIST_JOINT_BUTTON_STEP_DEGREES, + META_QUEST_AXIS_MASK, URDF_PATH, WRIST_JOINT_BUTTON_STEP_DEGREES, + SLOW_TRANSLATION_SCALE, SLOW_ROTATION_SCALE, TRANSLATION_SCALE, ROTATION_SCALE +) +from common.data_manager import RobotActivityState +from common.system_bootstrap import bootstrap_robot_system +from common.shared_actions import ( + toggle_robot_enabled, move_robot_home, + toggle_recording, neuracore_logging_callback ) -from common.data_manager import DataManager, RobotActivityState -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread from common.threads.quest_reader import quest_reader_thread -from common.threads.realsense_camera import camera_thread as realsense_camera_thread from meta_quest_teleop.reader import MetaQuestReader - -from pink_ik_solver import PinkIKSolver from piper_controller import PiperController -def play_audio_feedback(action: str) -> None: - """Play distinct synthesized tones asynchronously using SoX.""" - # Start = High pitch (880 Hz), Stop = Low pitch (440 Hz) - freq = "880" if action == "start" else "440" - try: - # -q: quiet, -n: no input file, synth: generate audio, 0.3: duration, sine: wave type - subprocess.Popen( - ["play", "-q", "-n", "synth", "0.3", "sine", freq], - stderr=subprocess.DEVNULL - ) - except Exception as e: - print(f"⚠️ Failed to play tone (is SoX installed?): {e}") - -def log_to_neuracore_on_change_callback( - name: str, payload: dict[str, Any], timestamp: float -) -> None: - """Log data to queue on change callback.""" - # Call appropriate Neuracore logging function - try: - #print(f"\n📤 Logging {name} to Neuracore with timestamp {timestamp:.3f}...") - if name == "log_joint_positions": - nc.log_joint_positions(payload, timestamp=timestamp) - elif name == "log_joint_torques": - nc.log_joint_torques(payload, timestamp=timestamp) - elif name == "log_joint_target_positions": - nc.log_joint_target_positions(payload, timestamp=timestamp) - elif name == "log_parallel_gripper_open_amounts": - nc.log_parallel_gripper_open_amounts(payload, timestamp=timestamp) - elif name == "log_parallel_gripper_target_open_amounts": - nc.log_parallel_gripper_target_open_amounts(payload, timestamp=timestamp) - elif name == "log_rgb": - camera_name = next(iter(payload)) - image_array = payload[camera_name] - nc.log_rgb(camera_name, image_array, timestamp=timestamp) - else: - print(f"\n⚠️ Unknown logging function: {name}") - except Exception as e: - print(f"\n⚠️ Failed to call {name} to Neuracore. Exception: {e}") - print("Traceback:") - traceback.print_exc() - - -def on_button_a_pressed() -> None: - """Handle Button A press to toggle robot enable/disable state.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - # Disable robot - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - print("✓ 🔴 Robot disabled (Button A)") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Button A)") - else: - print("✗ Failed to enable robot") - - -def on_button_b_pressed() -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 Button B pressed - Moving to home position...") - # Set state to HOMING to prevent IK thread from sending robot commands - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - # Disable teleop during homing - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Button B pressed but robot is not enabled") - - -def on_button_rj_pressed() -> None: - """Handle Button Right Joystick press to toggle data recording state.""" - if not nc.is_recording(): - # Start recording - try: - nc.start_recording() - print("✓ 🔴 Recording started (Button RJ)") - play_audio_feedback("start") # <-- Trigger distinct START sound - except Exception as e: - print(f"✗ Failed to start recording. Exception: {e}") - print("Traceback:") - traceback.print_exc() - else: - # Stop recording - try: - nc.stop_recording() - print("✓ ⏹️ Recording stopped (Button RJ)") - play_audio_feedback("stop") # <-- Trigger distinct STOP sound - except Exception as e: - print(f"✗ Failed to stop recording. Exception: {e}") - print("Traceback:") - traceback.print_exc() - - -def _step_wrist_joint(delta_degrees: float) -> None: +def _step_wrist_joint(data_manager, robot_controller, delta_degrees: float) -> None: """Apply a relative step to the wrist joint target angle.""" - # Prevent IK teleop loop from overwriting this manual joint adjustment. data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) target_joint_angles = robot_controller.get_target_joint_angles() current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - base_wrist_joint_angle = float(current_joint_angles[-1]) - else: - base_wrist_joint_angle = float(target_joint_angles[-1]) + base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1]) - target_joint_angles[-1] = base_wrist_joint_angle + delta_degrees + target_joint_angles[-1] = base_wrist + delta_degrees robot_controller.set_target_joint_angles(target_joint_angles) data_manager.set_target_joint_angles(target_joint_angles) -def on_button_y_pressed() -> None: - """Handle Button Y press to add +5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button Y pressed but robot is not enabled") - return - _step_wrist_joint(WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_x_pressed() -> None: - """Handle Button X press to subtract -5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button X pressed but robot is not enabled") - return - _step_wrist_joint(-WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_lj_pressed() -> None: - """Toggle slow translation/rotation scaling mode from config values.""" - slow_scaling_mode_enabled = data_manager.toggle_slow_scaling_mode_enabled() - if slow_scaling_mode_enabled: +def toggle_slow_scaling(data_manager): + enabled = data_manager.toggle_slow_scaling_mode_enabled() + if enabled: data_manager.set_teleop_scaling(SLOW_TRANSLATION_SCALE, SLOW_ROTATION_SCALE) - print( - "🐢 Button LJ pressed - Slow scaling enabled " - f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " - f"rotation={SLOW_ROTATION_SCALE:.3f})" - ) + print("🐢 Slow scaling enabled") else: data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) - print( - "🐇 Button LJ pressed - Slow scaling disabled " - f"(translation={TRANSLATION_SCALE:.3f}, " - f"rotation={ROTATION_SCALE:.3f})" - ) + print("🐇 Slow scaling disabled") if __name__ == "__main__": multiprocessing.set_start_method("spawn") - - parser = argparse.ArgumentParser( - description="Piper Robot Teleoperation with Neuracore Data Collection - REAL ROBOT CONTROL" - ) - parser.add_argument( - "--ip-address", - type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", - ) - parser.add_argument( - "--dataset-name", - type=str, - default=None, - help="Name for the dataset (optional, defaults to auto-generated timestamp-based name)", - ) + parser = argparse.ArgumentParser(description="Teleop with Neuracore Data Collection") + parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP") + parser.add_argument("--dataset-name", type=str, default=None) args = parser.parse_args() - print("=" * 60) - print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL") - print("=" * 60) - print("Thread frequencies:") - print(f" 🎮 Quest Controller: {CONTROLLER_DATA_RATE} Hz") - print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") - print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") - print(f" 📸 Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz") - print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") + print("=" * 60 + "\nPIPER TELEOP DATA COLLECTION\n" + "=" * 60) - # Connect to Neuracore + # 1. Connect to Neuracore & Create Dataset print("\n🔧 Initializing Neuracore...") nc.login() - nc.connect_robot( - robot_name="AgileX PiPER", - urdf_path=str(URDF_PATH), - overwrite=False, - ) - - # Create dataset - dataset_name = ( - args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" - ) - print(f"\n🔧 Creating dataset {dataset_name}...") - nc.create_dataset( - name=dataset_name, - description="Teleop data collection for Piper robot", - ) - - # Initialize shared state - data_manager = DataManager() - data_manager.set_on_change_callback(log_to_neuracore_on_change_callback) - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, + nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) + + dataset_name = args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" + nc.create_dataset(name=dataset_name, description="Teleop data collection for Piper robot") + + # 2. Bootstrap Core System + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + start_ik=True, start_camera=True ) - data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) + + # Wire Neuracore logging to DataManager + data_manager.set_on_change_callback(neuracore_logging_callback) - # Initialize robot controller - print("\n🤖 Initializing Piper robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, - enable_joint_angle_limits=False, - debug_mode=False, - ) - - # Start robot control loop - print("\n🚀 Starting robot control loop...") - robot_controller.start_control_loop() - - # Start joint state thread - print("\n📊 Starting joint state thread...") - joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) - joint_state_thread_obj.start() - - # Initialize Meta Quest reader + # 3. Initialize Quest Reader & Bind Controls print("\n🎮 Initializing Meta Quest reader...") quest_reader = MetaQuestReader( - ip_address=args.ip_address, - port=5555, - run=True, - axis_mask=META_QUEST_AXIS_MASK, + ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK ) - # Register button callbacks (after state and robot_controller are initialized) - quest_reader.on("button_a_pressed", on_button_a_pressed) - quest_reader.on("button_b_pressed", on_button_b_pressed) - quest_reader.on("button_rj_pressed", on_button_rj_pressed) - quest_reader.on("button_y_pressed", on_button_y_pressed) - quest_reader.on("button_x_pressed", on_button_x_pressed) - quest_reader.on("button_lj_pressed", on_button_lj_pressed) + quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller)) + quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) + quest_reader.on("button_rj_pressed", lambda: toggle_recording(play_audio=True)) + quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, WRIST_JOINT_BUTTON_STEP_DEGREES)) + quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -WRIST_JOINT_BUTTON_STEP_DEGREES)) + quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager)) - # Start data collection thread - print("\n🎮 Starting quest reader thread...") - quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ) + # 4. Start Quest Thread + quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) quest_thread.start() + active_threads.append(quest_thread) - # set initial configuration to current joint angles - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) - else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - - # Create Pink IK solver - print("\n🔧 Creating Pink IK solver...") - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - - # Start IK solver thread - print("\n🧮 Starting IK solver thread...") - ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ) - ik_thread.start() - - # Start cameras threads - print("\n📷 Starting cameras threads...") - realsense_camera_thread_obj = threading.Thread( - target=realsense_camera_thread, args=(data_manager,), daemon=True - ) - realsense_camera_thread_obj.start() - - # usb_camera_thread_obj = threading.Thread( - # target=usb_camera_thread, args=(data_manager,), daemon=True - # ) - # usb_camera_thread_obj.start() - - print() - print("🚀 Starting teleoperation with REAL ROBOT CONTROL...") - print("🎮 CONTROLS:") - print(" 1. Press BUTTON A to enable/disable robot") - print(" 2. Hold RIGHT GRIP to activate teleoperation") - print(" 3. Move controller - robot follows!") - print(" 4. Hold RIGHT TRIGGER to close gripper") - print(" 5. Press BUTTON B to send robot home") - print(" 6. Press RIGHT JOYSTICK to start/stop data recording") - print(" 7. Press BUTTON Y to add +5° on the wrist joint") - print(" 8. Press BUTTON X to subtract 5° on the wrist joint") - print(" 9. Press LEFT JOYSTICK BUTTON to toggle slow scaling mode") - print(" 10. Release grip to stop") - print("⚠️ Press Ctrl+C to exit") - print() + print("\n🚀 Ready! Use Meta Quest controllers. Press Right Joystick to Record. Press Ctrl+C to exit.\n") + # 5. Wait Loop (Work happens in background threads) try: while not data_manager.is_shutdown_requested(): time.sleep(1) @@ -390,36 +106,18 @@ def on_button_lj_pressed() -> None: print("\n👋 Interrupt received - shutting down gracefully...") data_manager.request_shutdown() except Exception as e: - print(f"\n❌ Demo error. Exception: {e}") - print("Traceback:") - traceback.print_exc() + print(f"\n❌ Demo error: {e}") data_manager.request_shutdown() - else: - print("\n⚠️ Worker requested shutdown - cleaning up...") - # Cleanup - print("\n🧹 Cleaning up...") - # Cancel recording if active + # 6. Cleanup + print("\n🧹 Cleaning up...") if nc.is_recording(): - try: - print("⚠️ Cancelling active recording...") - nc.cancel_recording() - print("✓ Recording cancelled") - except Exception as e: - print(f"⚠️ Error cancelling recording. Exception: {e}") - print("Traceback:") - traceback.print_exc() - - # shutdown threads and data producers before logging out - data_manager.request_shutdown() + nc.cancel_recording() + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) quest_reader.stop() - quest_thread.join() - joint_state_thread_obj.join() - ik_thread.join() - realsense_camera_thread_obj.join() - # usb_camera_thread_obj.join() + for t in active_threads: + t.join() nc.logout() robot_controller.cleanup() - - print("\n👋 Demo stopped.") + print("👋 Demo stopped.") \ No newline at end of file diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index 2153bcb..391db93 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 """Replay a recorded Neuracore dataset on the Piper robot.""" import argparse @@ -5,207 +6,99 @@ import time from pathlib import Path from typing import cast - import cv2 import neuracore as nc import numpy as np -from common.configs import ( - GRIPPER_NAME, - JOINT_NAMES, - NEUTRAL_JOINT_ANGLES, - ROBOT_RATE, -) -from neuracore.core.utils.robot_data_spec_utils import ( - merge_cross_embodiment_description, -) -from neuracore_types import ( - CrossEmbodimentDescription, - DataType, - SynchronizedPoint, -) from tqdm import tqdm -# Add parent directory to path to piper_controller sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.configs import GRIPPER_NAME, JOINT_NAMES, NEUTRAL_JOINT_ANGLES, ROBOT_RATE +from common.dataset_helpers import load_and_sync_dataset +from neuracore_types import DataType, SynchronizedPoint from piper_controller import PiperController +def wait_for_home(robot_controller, timeout: int = 10): + robot_controller.move_to_home() + for _ in range(timeout): + if robot_controller.is_robot_homed(): + print("✓ Robot is at home position.") + return True + time.sleep(1) + print("❌ Robot did not reach home position.") + return False -def main() -> None: - """Main function for replaying a Neuracore dataset on the Piper robot.""" +if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--dataset-name", type=str, required=True) parser.add_argument("--frequency", type=int, required=True) - parser.add_argument("--episode-index", type=int, required=False, default=0) + parser.add_argument("--episode-index", type=int, default=0) args = parser.parse_args() - # Initialize robot controller print("\n🤖 Initializing Piper robot controller...") robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, + can_interface="can0", robot_rate=ROBOT_RATE, control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - debug_mode=False, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, debug_mode=False, ) - # Start robot control loop - print("\n🚀 Starting robot control loop...") robot_controller.start_control_loop() - # Login to Neuracore print("\n🔑 Logging in to Neuracore...") nc.login() - # Get dataset from Neuracore - print("\n🔍 Getting dataset from Neuracore...") - dataset = nc.get_dataset(args.dataset_name) - - # Cross-embodiment sync (same pattern as examples/6_visualize_policy_from_dataset.py) - print("\n🔁 Building cross_embodiment_union for synchronization...") - input_modalities: list[DataType] = [ - DataType.JOINT_POSITIONS, - DataType.RGB_IMAGES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, - ] - output_modalities: list[DataType] = [ - DataType.JOINT_TARGET_POSITIONS, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, - ] - input_cross_embodiment_description: CrossEmbodimentDescription = {} - output_cross_embodiment_description: CrossEmbodimentDescription = {} - for robot_id in dataset.robot_ids: - full = dataset.get_full_embodiment_description(robot_id) - input_cross_embodiment_description[robot_id] = { - dt: full[dt] for dt in input_modalities if dt in full - } - output_cross_embodiment_description[robot_id] = { - dt: full[dt] for dt in output_modalities if dt in full - } - cross_embodiment_union = merge_cross_embodiment_description( - input_cross_embodiment_description, - output_cross_embodiment_description, - ) + input_mods = [DataType.JOINT_POSITIONS, DataType.RGB_IMAGES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] + output_mods = [DataType.JOINT_TARGET_POSITIONS, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] + + synced_dataset = load_and_sync_dataset(args.dataset_name, args.frequency, input_mods, output_mods) - # Synchronize dataset - print("\n🔁 Synchronizing dataset...") - synced_dataset = dataset.synchronize( - frequency=args.frequency, - cross_embodiment_union=cross_embodiment_union, - ) + episode_indices = list(range(len(synced_dataset))) if args.episode_index == -1 else [args.episode_index] + print(f"\n📊 Playing {len(episode_indices)} episode(s).") - # Determine which episodes to play - episode_indices: list[int] = [] - if args.episode_index == -1: - episode_indices = list(range(len(synced_dataset))) - print(f"\n📊 Found {len(synced_dataset)} episodes. Will play all episodes.") - else: - episode_indices = [args.episode_index] - print(f"\n📊 Playing episode {args.episode_index} only.") - - # Play episodes try: for episode_idx in episode_indices: + if not wait_for_home(robot_controller): continue - robot_controller.move_to_home() - seconds_to_wait = 10 - while not robot_controller.is_robot_homed(): - time.sleep(1) - seconds_to_wait -= 1 - if seconds_to_wait <= 0: - break - print( - f"🔍 Waiting for robot to reach home position... {seconds_to_wait} seconds remaining." - ) - if robot_controller.is_robot_homed(): - print("✓ Robot is at home position.") - else: - print("❌ Robot did not reach home position within 10 seconds.") - print( - f"🔍 Current joint angles: {robot_controller.get_current_joint_angles()}" - ) - print(f"🔍 Home joint angles: {robot_controller.HOME_JOINT_ANGLES}") - - print(f"\n{'='*60}") - print(f"🎬 Playing Episode {episode_idx} / {len(synced_dataset) - 1}") - print(f"{'='*60}") - + print(f"\n{'='*60}\n🎬 Playing Episode {episode_idx} / {len(synced_dataset) - 1}\n{'='*60}") episode = synced_dataset[episode_idx] - print(f"\n🚀 Collecting episode {episode_idx} data...") - rgb_frames_per_step: list[dict[str, np.ndarray]] = [] - parallel_gripper_open_amounts = [] - joint_positions = [] - for step in tqdm(episode, desc=f"Collecting episode {episode_idx}"): + rgb_frames_per_step, parallel_gripper_open_amounts, joint_positions = [], [], [] + + for step in tqdm(episode, desc=f"Collecting data"): step = cast(SynchronizedPoint, step) + + # Extract Joints + j_data = step.data.get(DataType.JOINT_TARGET_POSITIONS, {}) + joint_positions.append([j_data[jn].value for jn in JOINT_NAMES if jn in j_data]) - # Extract joint positions - joint_positions_dict = {} - if DataType.JOINT_TARGET_POSITIONS in step.data: - joint_data = step.data[DataType.JOINT_TARGET_POSITIONS] - for joint_name in JOINT_NAMES: - if joint_name in joint_data: - joint_positions_dict[joint_name] = joint_data[ - joint_name - ].value - joint_positions.append([joint_positions_dict[jn] for jn in JOINT_NAMES]) - - # Extract gripper - gripper_value = 0.0 - if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in step.data: - gripper_data = step.data[ - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ] - if GRIPPER_NAME in gripper_data: - gripper_value = gripper_data[GRIPPER_NAME].open_amount - parallel_gripper_open_amounts.append(gripper_value) - - # Extract RGB for all cameras - step_frames: dict[str, np.ndarray] = {} - if DataType.RGB_IMAGES in step.data: - rgb_data = step.data[DataType.RGB_IMAGES] - for camera_name, img_value in rgb_data.items(): - step_frames[camera_name] = img_value.frame - rgb_frames_per_step.append(step_frames) + # Extract Gripper + g_data = step.data.get(DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, {}) + parallel_gripper_open_amounts.append(g_data[GRIPPER_NAME].open_amount if GRIPPER_NAME in g_data else 0.0) + + # Extract Camera + rgb_data = step.data.get(DataType.RGB_IMAGES, {}) + rgb_frames_per_step.append({c: img.frame for c, img in rgb_data.items()}) joint_positions = np.degrees(np.array(joint_positions)) - parallel_gripper_open_amounts = np.array(parallel_gripper_open_amounts) - print(f"\n🚀 Replaying episode {episode_idx} data...") - for index in tqdm( - range(len(joint_positions)), desc=f"Replaying episode {episode_idx}" - ): + print(f"\n🚀 Replaying episode {episode_idx}...") + for idx in tqdm(range(len(joint_positions)), desc="Replaying"): start_time = time.time() - robot_controller.set_target_joint_angles(joint_positions[index]) - robot_controller.set_gripper_open_value( - parallel_gripper_open_amounts[index] - ) - - # Display camera frames (dataset stores RGB; OpenCV expects BGR) - if index < len(rgb_frames_per_step): - for camera_name, frame_rgb in rgb_frames_per_step[index].items(): - arr = np.asarray(frame_rgb, dtype=np.uint8) - frame_bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) - cv2.imshow(f"Replay: {camera_name}", frame_bgr) + robot_controller.set_target_joint_angles(joint_positions[idx]) + robot_controller.set_gripper_open_value(parallel_gripper_open_amounts[idx]) + + if idx < len(rgb_frames_per_step): + for cam_name, frame_rgb in rgb_frames_per_step[idx].items(): + cv2.imshow(f"Replay: {cam_name}", cv2.cvtColor(np.asarray(frame_rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR)) if cv2.waitKey(1) & 0xFF == ord("q"): - print("\n🛑 'q' pressed, stopping replay...") - break + print("\n🛑 'q' pressed, stopping replay..."); break - end_time = time.time() - time.sleep(max(0, 1 / args.frequency - (end_time - start_time))) + time.sleep(max(0, 1 / args.frequency - (time.time() - start_time))) + cv2.destroyAllWindows() - print(f"🎉 Episode {episode_idx} replay completed.") - - if args.episode_index == -1: - print(f"\n{'='*60}") - print(f"🎉 All {len(synced_dataset)} episodes replay completed!") - print(f"{'='*60}") + except KeyboardInterrupt: - print("\n🛑 Keyboard interrupt detected, stopping robot control loop...") + print("\n🛑 Keyboard interrupt detected.") cv2.destroyAllWindows() robot_controller.stop_control_loop() - robot_controller.cleanup() - - -if __name__ == "__main__": - main() + robot_controller.cleanup() \ No newline at end of file diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 459c598..350a8e4 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -1,10 +1,5 @@ #!/usr/bin/env python3 -"""Piper Robot Test with Neuracore policy. - -This script loads a trained Neuracore policy, reads status from the piper robot -controlled by the Meta Quest controller, and replays the prediction horizon virtually -on Viser to test the stability of the policy output. -""" +"""Piper Robot Test with Neuracore policy.""" import argparse import sys @@ -12,1060 +7,136 @@ import time import traceback from pathlib import Path - import neuracore as nc -import numpy as np -from neuracore.ml.preprocessing.methods.resize_pad import ResizePad -from neuracore.ml.utils.preprocessing_utils import PreprocessingConfiguration -from neuracore_types import ( - BatchedJointData, - BatchedNCData, - BatchedParallelGripperOpenAmountData, - DataType, - EmbodimentDescription, -) -# Add parent directory to path to import pink_ik_solver and piper_controller +# Add parent directory to path sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, - CAMERA_NAMES, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - GRIPPER_NAME, - IK_SOLVER_RATE, - JOINT_NAMES, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - MAX_ACTION_ERROR_THRESHOLD, - MAX_SAFETY_THRESHOLD, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POLICY_EXECUTION_RATE, - POSITION_COST, - POSTURE_COST_VECTOR, - PREDICTION_HORIZON_EXECUTION_RATIO, - ROBOT_RATE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, - TARGETING_POSE_TIME_THRESHOLD, - URDF_PATH, - VISUALIZATION_RATE, + CAMERA_NAMES, GRIPPER_NAME, URDF_PATH, + PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, ROBOT_RATE, ) -from common.data_manager import DataManager, RobotActivityState +from neuracore_types import DataType, EmbodimentDescription +from neuracore.ml.preprocessing.methods.resize_pad import ResizePad + +from common.data_manager import RobotActivityState from common.policy_state import PolicyState +from common.policy_helpers import embodiment_names_ordered +from common.system_bootstrap import bootstrap_robot_system +from common.shared_actions import toggle_robot_enabled, move_robot_home from common.robot_visualizer import RobotVisualizer -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread from common.threads.quest_reader import quest_reader_thread -from common.threads.realsense_camera import camera_thread from meta_quest_teleop.reader import MetaQuestReader -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - - -def _embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: - """Ordered channel names from an embodiment entry (list or index→name map).""" - if isinstance(spec, dict): - return [spec[i] for i in sorted(spec)] - return list(spec) - - -def convert_predictions_to_horizon( - predictions: dict[DataType, dict[str, BatchedNCData]], -) -> dict[str, list[float]]: - """Convert predictions to horizon dict.""" - print('[convert_predictions_to_horizon]') - horizon = {} - if DataType.JOINT_TARGET_POSITIONS in predictions: - joint_data = predictions[DataType.JOINT_TARGET_POSITIONS] - for joint_name in JOINT_NAMES: - if joint_name in joint_data: - batched = joint_data[joint_name] - if isinstance(batched, BatchedJointData): - values = batched.value[0, :, 0].cpu().numpy().tolist() - horizon[joint_name] = values - - # Extract gripper open amounts - if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: - print('[convert_predictions_to_horizon] Extracting gripper open amounts') - gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - if GRIPPER_NAME in gripper_data: - batched = gripper_data[GRIPPER_NAME] - if isinstance(batched, BatchedParallelGripperOpenAmountData): - values = batched.open_amount[0, :, 0].cpu().numpy().tolist() - horizon[GRIPPER_NAME] = values - return horizon - - -def toggle_robot_enabled_status( - data_manager: DataManager, - robot_controller: PiperController, - visualizer: RobotVisualizer, -) -> None: - """Handle Button A press to toggle robot enable/disable state.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - # Disable robot - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - visualizer.update_toggle_robot_enabled_status(False) - print("✓ 🔴 Robot disabled") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - visualizer.update_toggle_robot_enabled_status(True) - print("✓ 🟢 Robot enabled") - else: - print("✗ Failed to enable robot") - - -def home_robot(data_manager: DataManager, robot_controller: PiperController) -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 Button B pressed - Moving to home position...") - # Set state to HOMING to prevent IK thread from sending robot commands - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - # Disable teleop during homing - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Home command received but robot is not enabled") - - -def run_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> bool: - """Handle Run Policy button press to capture state and get policy prediction.""" - print("Running policy...") - - # Get available data from data_manager (only log what the model expects) - current_joint_angles = None - gripper_open_value = None - rgb_image = None - - if DataType.JOINT_POSITIONS in input_embodiment_description: - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - joint_angles_rad = np.radians(current_joint_angles) - positions_by_name = { - jn: float(ang) for jn, ang in zip(JOINT_NAMES, joint_angles_rad) - } - policy_joint_order = _embodiment_names_ordered( - input_embodiment_description[DataType.JOINT_POSITIONS] - ) - joint_positions_dict = { - jn: positions_by_name[jn] for jn in policy_joint_order - } - nc.log_joint_positions(joint_positions_dict) - print(" ✓ Logged joint positions") - else: - print(" ⚠️ No current joint angles available") - - if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: - gripper_open_value = data_manager.get_current_gripper_open_value() - if gripper_open_value is not None: - nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_open_value) - print(" ✓ Logged gripper open amount") - else: - print(" ⚠️ No gripper open value available") - - if DataType.RGB_IMAGES in input_embodiment_description: - rgb_names = _embodiment_names_ordered( - input_embodiment_description[DataType.RGB_IMAGES] - ) - logged_any_rgb = False - for camera_name in rgb_names: - img = data_manager.get_rgb_image(camera_name) - if img is not None: - nc.log_rgb(camera_name, img) - logged_any_rgb = True - if logged_any_rgb: - print(" ✓ Logged RGB image(s)") - else: - print(" ⚠️ No RGB image available") - if rgb_names: - rgb_image = data_manager.get_rgb_image(rgb_names[0]) - - # Check if we have at least some data to run the policy - if ( - current_joint_angles is None - and gripper_open_value is None - and rgb_image is None - ): - print("✗ No data available to run policy") - return False - - # Get policy prediction - try: - start_time = time.time() - predictions = policy.predict(timeout=60) - inference_time = time.time() - start_time - prediction_horizon = convert_predictions_to_horizon(predictions) - conversion_time = time.time() - start_time - inference_time - - # Calculate length directly from the new prediction dictionary - horizon_length = 0 - if prediction_horizon: - first_key = next(iter(prediction_horizon.keys())) - horizon_length = len(prediction_horizon[first_key]) - - print( - f" ✓ Got policy prediction in {inference_time:.2f} seconds with horizon length {horizon_length}" - ) - print(f" ✓ Conversion time: {conversion_time:.2f} seconds") - - prediction_ratio = visualizer.get_prediction_ratio() - policy_state.set_execution_ratio(prediction_ratio) - - # Set policy inputs (only if available) - if rgb_image is not None: - policy_state.set_policy_rgb_image_input(rgb_image) - if current_joint_angles is not None: - policy_state.set_policy_state_input(current_joint_angles) - - # Store prediction horizon actions in policy state - policy_state.set_prediction_horizon(prediction_horizon) - - visualizer.update_ghost_robot_visibility(True) - policy_state.set_ghost_robot_playing(True) - policy_state.reset_ghost_action_index() - - except Exception as e: - print(f"✗ Failed to get policy prediction: {e}") - traceback.print_exc() - return False - - return True - - -def start_policy_execution( - data_manager: DataManager, policy_state: PolicyState -) -> bool: - """Handle Execute Policy button press to start policy execution.""" - print("Starting policy execution...") - - # Check if policy execution is already active - if ( - data_manager.get_robot_activity_state() == RobotActivityState.POLICY_CONTROLLED - and not policy_state.get_continuous_play_active() - ): - print("⚠️ Policy execution already in progress") - return False - # Check if robot is enabled - elif data_manager.get_robot_activity_state() == RobotActivityState.DISABLED: - print("⚠️ Cannot execute policy: Robot is disabled") - return False - - # Get prediction horizon - prediction_horizon = policy_state.get_prediction_horizon() - prediction_horizon_length = policy_state.get_prediction_horizon_length() - if prediction_horizon_length == 0: - print("⚠️ No prediction horizon available. Make sure policy was run first.") - return False - - # Check that we have joint data for all joints - if not all(joint_name in prediction_horizon for joint_name in JOINT_NAMES): - print("⚠️ First prediction in horizon has no joint targets") - return False - - # Safety check: verify robot is close enough to first action - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is None: - print("⚠️ Cannot execute policy: No current joint angles available") - return False - - # Get first action from horizon (index 0 for each joint) - current_joint_target_positions_rad = np.array( - [prediction_horizon[joint_name][0] for joint_name in JOINT_NAMES] - ) - current_target_deg = np.degrees(current_joint_target_positions_rad) - joint_differences = np.abs(current_joint_angles - current_target_deg) - - if np.any(joint_differences > MAX_SAFETY_THRESHOLD): - print("⚠️ Cannot execute policy: Robot too far from first predicted action") - print(" --- DIAGNOSTICS ---") - print(f" Current Angles: {[f'{d:.2f}' for d in current_joint_angles]}") - print(f" AI Predicted: {[f'{d:.2f}' for d in current_target_deg]}") - print(f" Differences: {[f'{d:.2f}' for d in joint_differences]}") - print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") - print(" 💡 TIP 1: Did the arm sag? Check if 'Current Angles' are drooping.") - print(" 💡 TIP 2: If the AI naturally predicts large first steps, increase MAX_SAFETY_THRESHOLD in common/configs.py") - return False - - # All checks passed - start execution - - # Stop ghost visualization - policy_state.set_ghost_robot_playing(False) - - # Deactivate teleop - data_manager.set_teleop_state(False, None, None) - - # Lock policy inputs and start execution - policy_state.start_policy_execution() - - # Verify locked horizon was created successfully - locked_horizon_length = policy_state.get_locked_prediction_horizon_length() - if locked_horizon_length == 0: - print("⚠️ Failed to lock prediction horizon - horizon is empty") - policy_state.end_policy_execution() - return False - - print(f"✓ Starting policy execution with {locked_horizon_length} actions") - - # Change robot state to POLICY_CONTROLLED - data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) - - return True - - -def end_policy_play( - data_manager: DataManager, - policy_state: PolicyState, - visualizer: RobotVisualizer, - policy_status_message: str, -) -> None: - """End continuous play and set robot activity state to ENABLED and update policy status.""" - if policy_state.get_continuous_play_active(): - policy_state.set_continuous_play_active(False) - - # Reset ghost robot color to default orange - visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) - - visualizer.update_play_policy_button_status(False) - - visualizer.update_play_policy_button_status(False) - policy_state.end_policy_execution() - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - data_manager.set_teleop_state(False, None, None) - visualizer.update_policy_status(policy_status_message) - -def continuous_prediction_worker( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, - continuous_mode: str = "pipeline", -) -> None: - """Background thread for continuous execution supporting pipelined and sequential modes.""" - VISUALIZATION_COLORS = [ - (1.0, 0.65, 0.0, 0.25), # Orange (Default) - (0.0, 1.0, 0.0, 0.25), # Green - (1.0, 0.0, 0.0, 0.25), # Red - (0.0, 0.0, 1.0, 0.25), # Blue - ] - color_index = 0 - - # 1. Bootstrap the very first prediction to get the robot moving - print(f"\n🚀 [Worker] Bootstrapping initial trajectory in '{continuous_mode}' mode...") - success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) - if success: - start_policy_execution(data_manager, policy_state) - - while policy_state.get_continuous_play_active(): - # Failsafe: if there's no active trajectory running yet, wait briefly - if policy_state.get_locked_prediction_horizon_length() == 0: - time.sleep(0.01) - continue - - if continuous_mode == "pipeline": - print("\n📸 [Pipeline Worker] Robot is moving! Prefetching next prediction in background...") - # Query the network in parallel while the execution thread is driving the motors - success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) - - if not success or not policy_state.get_continuous_play_active(): - time.sleep(0.05) - continue - - # Wait until the current trajectory buffer is running low before swapping - while policy_state.get_continuous_play_active(): - exec_idx = policy_state.get_execution_action_index() - total_len = policy_state.get_locked_prediction_horizon_length() - remaining = total_len - exec_idx - - # Hot-swap when 5 or fewer steps are left in the active trajectory - if remaining <= 5 or total_len == 0: - break - time.sleep(0.01) - - elif continuous_mode == "sequential": - # Wait until the current trajectory buffer is completely exhausted - while policy_state.get_continuous_play_active(): - exec_idx = policy_state.get_execution_action_index() - total_len = policy_state.get_locked_prediction_horizon_length() - if exec_idx >= total_len or total_len == 0: - break - time.sleep(0.01) - - if not policy_state.get_continuous_play_active(): - break - - print("\n📸 [Sequential Worker] Trajectory finished! Holding position and requesting next prediction...") - success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) - - if not success or not policy_state.get_continuous_play_active(): - time.sleep(0.05) - continue - - if not policy_state.get_continuous_play_active(): - break - - print("🔄 [Worker] Swapping to new trajectory buffer!") - # Seamlessly clear the lock and flash the new horizon into play - policy_state.end_policy_execution() - success = start_policy_execution(data_manager, policy_state) - - if success: - color_index = (color_index + 1) % len(VISUALIZATION_COLORS) - visualizer.set_ghost_robot_color(VISUALIZATION_COLORS[color_index]) - else: - print("❌ [Worker] Swap rejected by safety threshold. Retrying immediately...") - time.sleep(0.01) - -def play_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, - continuous_mode: str = "pipeline", -) -> None: - """Handle Play Policy button press to start/stop continuous policy execution.""" - if not policy_state.get_continuous_play_active(): - # Start continuous play - print(f"▶️ Play Policy button pressed - Starting {continuous_mode.capitalize()} Mode...") - policy_state.set_continuous_play_active(True) - visualizer.update_play_policy_button_status(True) - - # Spawn the background worker - threading.Thread( - target=continuous_prediction_worker, - args=(data_manager, policy, policy_state, visualizer, input_embodiment_description, continuous_mode), - daemon=True - ).start() - else: - # Stop continuous play - print("⏹️ Stop Policy button pressed - Stopping continuous policy execution...") - policy_state.set_continuous_play_active(False) - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution stopped" - ) - print("✓ Policy execution stopped and robot enabled") - -def policy_execution_thread( - policy: nc.policy, - data_manager: DataManager, - policy_state: PolicyState, - robot_controller: PiperController, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> None: - """Policy execution thread.""" - dt_execution = 1.0 / POLICY_EXECUTION_RATE - - # Define colors for continuous horizon visualization - VISUALIZATION_COLORS = [ - (1.0, 0.65, 0.0, 0.25), # Orange (Default) - (0.0, 1.0, 0.0, 0.25), # Green - (1.0, 0.0, 0.0, 0.25), # Red - (0.0, 0.0, 1.0, 0.25), # Blue - ] - color_index = 0 - - # Throttle visualization updates to ~30Hz to avoid overwhelming Viser - last_visualization_update = 0.0 - visualization_update_interval = 1.0 / 30.0 # 30 Hz - while True: - start_time = time.time() - - if ( - data_manager.get_robot_activity_state() - == RobotActivityState.POLICY_CONTROLLED - ): - locked_horizon = policy_state.get_locked_prediction_horizon() - execution_index = policy_state.get_execution_action_index() - locked_horizon_length = policy_state.get_locked_prediction_horizon_length() - - # Debug output on first execution step - if execution_index == 0 and locked_horizon_length > 0: - print( - f"🔄 Policy execution started: {locked_horizon_length} actions, " - f"robot enabled: {robot_controller.is_robot_enabled()}" - ) - - # If continuous play is active, only execute up to the chunk limit - if execution_index < locked_horizon_length: - # Check if previous goal was achieved, if any - current_joint_angles = data_manager.get_current_joint_angles() - if ( - execution_index > 0 - and current_joint_angles is not None - and policy_state.get_execution_mode() - == PolicyState.ExecutionMode.TARGETING_POSE - ): - targeting_pose_start_time = time.time() - while ( - time.time() - targeting_pose_start_time - < TARGETING_POSE_TIME_THRESHOLD - ): - # Get previous action from horizon - if not all( - joint_name in locked_horizon for joint_name in JOINT_NAMES - ): - break - previous_joint_target_positions_rad = np.array( - [ - locked_horizon[joint_name][execution_index - 1] - for joint_name in JOINT_NAMES - ] - ) - previous_joint_target_positions_deg = np.degrees( - previous_joint_target_positions_rad - ) - joint_errors = np.abs( - current_joint_angles - previous_joint_target_positions_deg - ) - if np.any(joint_errors <= MAX_ACTION_ERROR_THRESHOLD): - break - time.sleep(0.001) - - # Send current action to robot (if available) - if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): - current_joint_target_positions_rad = np.array( - [ - locked_horizon[joint_name][execution_index] - for joint_name in JOINT_NAMES - ] - ) - current_joint_target_positions_deg = np.degrees( - current_joint_target_positions_rad - ) - # Update data_manager with target joint angles for visualization - data_manager.set_target_joint_angles( - current_joint_target_positions_deg - ) - - # Verify robot controller is enabled before sending commands - if robot_controller.is_robot_enabled(): - robot_controller.set_target_joint_angles( - current_joint_target_positions_deg - ) - else: - print( - f"⚠️ Robot controller not enabled, skipping command at index {execution_index}" - ) - - # Send current gripper open value to robot (if available) - if GRIPPER_NAME in locked_horizon: - current_gripper_target_open_value = locked_horizon[GRIPPER_NAME][ - execution_index - ] - robot_controller.set_gripper_open_value( - current_gripper_target_open_value - ) - - # Update execution index - policy_state.increment_execution_action_index() - - # Update status - visualizer.update_policy_status( - f"Executing policy: {execution_index + 1}/{locked_horizon_length}" - ) - else: - # Horizon buffer exhausted - if not policy_state.get_continuous_play_active(): - print("✓ Policy execution completed") - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution completed" - ) - else: - # Failsafe: If the background thread is running slightly late, - # hold the very last predicted position to maintain motor torque. - if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): - last_index = locked_horizon_length - 1 - hold_positions_rad = np.array([ - locked_horizon[jn][last_index] for jn in JOINT_NAMES - ]) - if robot_controller.is_robot_enabled(): - robot_controller.set_target_joint_angles(np.degrees(hold_positions_rad)) - - # NOTE: Update visualization less frequently to avoid blocking - # Throttle visualization updates to ~30Hz to prevent overwhelming Viser server - current_time = time.time() - if current_time - last_visualization_update >= visualization_update_interval: - update_visualization(data_manager, policy_state, visualizer) - last_visualization_update = current_time - - dt_execution = 1.0 / visualizer.get_policy_execution_rate() - elapsed = time.time() - start_time - if elapsed < dt_execution: - time.sleep(dt_execution - elapsed) - - -def update_visualization( - data_manager: DataManager, - policy_state: PolicyState, - visualizer: RobotVisualizer, -) -> None: - """Update visualization.""" - # Update actual robot visualization - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - joint_config_rad = np.radians(current_joint_angles) - visualizer.update_robot_pose(joint_config_rad) - - # Update RGB camera image in Viser GUI (if available) - rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) - if rgb_image is not None: - visualizer.update_rgb_image(rgb_image) - - # Get policy state for ghost robot - prediction_horizon = policy_state.get_prediction_horizon() - prediction_horizon_length = policy_state.get_prediction_horizon_length() - ghost_robot_playing = policy_state.get_ghost_robot_playing() - ghost_action_index = policy_state.get_ghost_action_index() - - # Update ghost robot based on current state - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.POLICY_CONTROLLED: - # During policy execution, make ghost robot show target joint angles - visualizer.update_ghost_robot_visibility(True) - target_joint_angles = data_manager.get_target_joint_angles() - if target_joint_angles is not None: - joint_config_rad = np.radians(target_joint_angles) - visualizer.update_ghost_robot_pose(joint_config_rad) - # Disable buttons during execution - visualizer.set_run_policy_button_disabled(True) - # Play/Stop button is enabled during execution so we can stop if needed - visualizer.set_play_policy_button_disabled(False) - - elif ( - robot_activity_state == RobotActivityState.ENABLED - and data_manager.get_teleop_active() - ): - # During teleoperation, make ghost robot show target joint angles - visualizer.update_ghost_robot_visibility(True) - target_joint_angles = data_manager.get_target_joint_angles() - if target_joint_angles is not None: - joint_config_rad = np.radians(target_joint_angles) - visualizer.update_ghost_robot_pose(joint_config_rad) - - elif ghost_robot_playing and prediction_horizon_length > 0: - # Enable execute policy button - visualizer.set_start_policy_execution_button_disabled(False) - # show ghost robot - visualizer.update_ghost_robot_visibility(True) - # Update ghost robot with prediction horizon actions (preview mode) - if ghost_action_index < prediction_horizon_length: - # Get ghost action from horizon - if all(joint_name in prediction_horizon for joint_name in JOINT_NAMES): - ghost_joint_config = np.array( - [ - prediction_horizon[joint_name][ghost_action_index] - for joint_name in JOINT_NAMES - ] - ) - visualizer.update_ghost_robot_pose(ghost_joint_config) - next_index = (ghost_action_index + 1) % prediction_horizon_length - policy_state.set_ghost_action_index(next_index) - else: - policy_state.reset_ghost_action_index() - - else: - # When not playing, hide the ghost robot - visualizer.update_ghost_robot_visibility(False) - - # Update button state and policy status when not policy controlled - robot_enabled = robot_activity_state == RobotActivityState.ENABLED - has_horizon = prediction_horizon_length > 0 - - # Update button enabled state - visualizer.set_start_policy_execution_button_disabled( - not (robot_enabled and has_horizon) - ) - visualizer.set_run_policy_button_disabled(not robot_enabled) - visualizer.set_play_policy_button_disabled(not robot_enabled) - - # Update policy status - if not has_horizon: - visualizer.update_policy_status( - "Ready - Press Right Joystick or 'Run Policy' button to get prediction" - ) - elif not robot_enabled: - visualizer.update_policy_status("Robot not enabled") - else: - visualizer.update_policy_status( - f"Ready - {prediction_horizon_length} actions in horizon" - ) - +# Import the newly extracted policy actions +from common.policy_actions import ( + run_policy, start_policy_execution, play_policy, policy_execution_thread +) if __name__ == "__main__": - parser = argparse.ArgumentParser( - description="Piper Robot Test with Neuracore Policy - REAL ROBOT CONTROL" - ) - parser.add_argument( - "--ip-address", - type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", - ) - parser.add_argument( - "--no-quest", - action="store_true", - help="Disable Meta Quest controller integration entirely.", - ) + parser = argparse.ArgumentParser(description="Piper Robot Policy Test") + parser.add_argument("--ip-address", type=str, default=None) + parser.add_argument("--no-quest", action="store_true") + parser.add_argument("--continuous-mode", choices=["pipeline", "sequential"], default="sequential") + policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument( - "--train-run-name", - type=str, - default=None, - help="Name of the training run to load policy from (for cloud training).", - ) - - parser.add_argument( - "--continuous-mode", - type=str, - choices=["pipeline", "sequential"], - default="sequential", - help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", - ) - - policy_group.add_argument( - "--model-path", - type=str, - default=None, - help="Path to local model file to load policy from.", - ) - policy_group.add_argument( - "--remote-endpoint-name", - type=str, - default=None, - help="Name of remote Neuracore policy endpoint.", - ) - parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Neuracore robot name (policy embodiment resolution).", - ) + policy_group.add_argument("--train-run-name", type=str, default=None) + policy_group.add_argument("--model-path", type=str, default=None) + policy_group.add_argument("--remote-endpoint-name", type=str, default=None) + parser.add_argument("--robot-name", type=str, default="AgileX PiPER") args = parser.parse_args() - print("=" * 60) - print("PIPER ROBOT TEST WITH NEURACORE POLICY") - print("=" * 60) - print("Thread frequencies:") - print(f" 🎮 Quest Controller: {CONTROLLER_DATA_RATE} Hz") - print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") - print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") - print(f" 📸 Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz") - print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") - print(f" 🖥️ Visualization: {VISUALIZATION_RATE} Hz") + print("=" * 60 + "\nPIPER ROBOT TEST WITH NEURACORE POLICY\n" + "=" * 60) - # Connect to Neuracore - print("\n🔧 Initializing Neuracore...") + # 1. Connect Neuracore & Define Embodiments nc.login() - nc.connect_robot( - robot_name=args.robot_name, - urdf_path=str(URDF_PATH), - overwrite=False, - ) + nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) - # Load policy (cross-embodiment + preprocessing; same pattern as example 6) - input_embodiment_description: EmbodimentDescription = { - DataType.JOINT_POSITIONS: { - 0: "joint1", - 1: "joint2", - 2: "joint3", - 3: "joint4", - 4: "joint5", - 5: "joint6", - }, + input_emb: EmbodimentDescription = { + DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]}, } - output_embodiment_description: EmbodimentDescription = { - DataType.JOINT_TARGET_POSITIONS: { - 0: "joint1", - 1: "joint2", - 2: "joint3", - 3: "joint4", - 4: "joint5", - 5: "joint6", - }, + output_emb: EmbodimentDescription = { + DataType.JOINT_TARGET_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME}, } - input_preprocessing_config: PreprocessingConfiguration = { - DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], - } - print("\n📋 Input embodiment description:") - for data_type, spec in input_embodiment_description.items(): - print(f" {data_type.name}: {_embodiment_names_ordered(spec)}") - print("\n📋 Output embodiment description:") - for data_type, spec in output_embodiment_description.items(): - print(f" {data_type.name}: {_embodiment_names_ordered(spec)}") - - if args.remote_endpoint_name is not None: - print( - f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." - ) - try: - policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: - print( - f"❌ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." - ) - sys.exit(1) - elif args.train_run_name is not None: - print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") - policy = nc.policy( - train_run_name=args.train_run_name, - device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - ) + # Load Policy + if args.remote_endpoint_name: + policy = nc.policy_remote_server(args.remote_endpoint_name) + elif args.train_run_name: + policy = nc.policy(train_run_name=args.train_run_name, device="cuda", input_embodiment_description=input_emb, output_embodiment_description=output_emb) else: - print(f"\n🤖 Loading policy from model file: {args.model_path}...") - policy = nc.policy( - model_file=args.model_path, - device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - ) - print(" ✓ Policy loaded successfully") + policy = nc.policy(model_file=args.model_path, device="cuda", input_embodiment_description=input_emb, output_embodiment_description=output_emb) - # Initialize policy state policy_state = PolicyState() policy_state.set_execution_mode(PolicyState.ExecutionMode.TARGETING_TIME) - # Initialize shared state - data_manager = DataManager() - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - ) - - # Initialize robot controller - print("\n🤖 Initializing Piper robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - debug_mode=False, - ) - - # Start robot control loop - print("\n🚀 Starting robot control loop...") - robot_controller.start_control_loop() + # 2. Bootstrap Core System + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system(start_ik=True, start_camera=True) - # Start joint state thread - print("\n📊 Starting joint state thread...") - joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) - joint_state_thread_obj.start() - - # Initialize and Start Meta Quest reader (Conditional) + # 3. Initialize Quest Reader (Conditional) quest_reader = None - quest_thread = None if not args.no_quest: print("\n🎮 Initializing Meta Quest reader...") quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) - - print("\n🎮 Starting quest reader thread...") - quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ) + quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) quest_thread.start() - else: - print("\n🎮 Meta Quest reader disabled via --no-quest flag.") + active_threads.append(quest_thread) - # set initial configuration to current joint angles - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) - else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - - # Create Pink IK solver - print("\n🔧 Creating Pink IK solver...") - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - - # Start IK solver thread - print("\n🧮 Starting IK solver thread...") - ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ) - ik_thread.start() - - # Start camera thread - print("\n📷 Starting camera thread...") - camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ) - camera_thread_obj.start() - - # Set up visualization + # 4. Initialize Visualizer & Bind Callbacks print("\n🖥️ Starting Viser visualization...") visualizer = RobotVisualizer(str(URDF_PATH)) - visualizer.add_policy_controls( - initial_prediction_ratio=PREDICTION_HORIZON_EXECUTION_RATIO, - initial_policy_rate=POLICY_EXECUTION_RATE, - initial_robot_rate=ROBOT_RATE, - initial_execution_mode=PolicyState.ExecutionMode.TARGETING_TIME.value, - ) + visualizer.add_policy_controls(PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, ROBOT_RATE, "targeting_time") visualizer.add_toggle_robot_enabled_status_button() visualizer.add_homing_controls() visualizer.add_policy_buttons() - # Set up button callbacks - visualizer.set_toggle_robot_enabled_status_callback( - lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer) - ) - visualizer.set_go_home_callback(lambda: home_robot(data_manager, robot_controller)) - visualizer.set_run_policy_callback( - lambda: run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - ) - visualizer.set_start_policy_execution_callback( - lambda: start_policy_execution(data_manager, policy_state) - ) - - visualizer.set_play_policy_callback( - lambda: play_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - args.continuous_mode, - ) - ) - - # Set up execution mode dropdown callback to sync with PolicyState - visualizer.set_execution_mode_callback( - lambda: policy_state.set_execution_mode( - PolicyState.ExecutionMode(visualizer.get_execution_mode()) - ) - ) - - # Register Quest reader button callbacks conditionally - if not args.no_quest and quest_reader is not None: - quest_reader.on( - "button_a_pressed", - lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer), - ) - quest_reader.on( - "button_b_pressed", lambda: home_robot(data_manager, robot_controller) - ) - - # Start policy execution thread - print("\n🤖 Starting policy execution thread...") - policy_execution_thread_obj = threading.Thread( + # Shared Button Binds + visualizer.set_toggle_robot_enabled_status_callback(lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) + visualizer.set_go_home_callback(lambda: move_robot_home(data_manager, robot_controller)) + if quest_reader: + quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) + quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) + + # Policy Button Binds + visualizer.set_run_policy_callback(lambda: run_policy(data_manager, policy, policy_state, visualizer, input_emb)) + visualizer.set_start_policy_execution_callback(lambda: start_policy_execution(data_manager, policy_state)) + visualizer.set_play_policy_callback(lambda: play_policy(data_manager, policy, policy_state, visualizer, input_emb, args.continuous_mode)) + visualizer.set_execution_mode_callback(lambda: policy_state.set_execution_mode(PolicyState.ExecutionMode(visualizer.get_execution_mode()))) + + # 5. Start Policy Execution Thread + policy_exec_thread = threading.Thread( target=policy_execution_thread, - args=( - policy, - data_manager, - policy_state, - robot_controller, - visualizer, - input_embodiment_description, - ), - daemon=True, + args=(policy, data_manager, policy_state, robot_controller, visualizer, input_emb), + daemon=True ) - policy_execution_thread_obj.start() + policy_exec_thread.start() + active_threads.append(policy_exec_thread) - print() - print("🚀 Starting teleoperation with policy testing...") - print("🎮 CONTROLS:") - if not args.no_quest: - print(" 1. Press BUTTON A or Enable Robot button to enable/disable robot") - print(" 2. You have same control over the robot as in teleoperation.") - print(" - Hold RIGHT GRIP to activate teleoperation") - print(" - Move controller - robot follows!") - print(" - Hold RIGHT TRIGGER to close gripper") - print(" - Press BUTTON A or Enable Robot button to enable/disable robot") - print(" - Press BUTTON B or Home Robot button to send robot home") - else: - print(" 1. Click Enable Robot button in Viser to enable/disable robot") - print(" 2. (Meta Quest controls disabled via --no-quest flag)") - print(" 3. Click 'Run Policy' (Preview) to generate and visualize a prediction horizon") - print(" 4. Click 'Execute Policy' to run the currently previewed horizon") - print(" 5. Click 'Play Policy' (Receding Horizon) to constantly predict and execute the first action") - print("⚠️ Press Ctrl+C to exit") - print() - print("🌐 Open browser: http://localhost:8080") + print("\n🚀 Ready! Check http://localhost:8080 to visualize and run policies.\n") + # 6. Main Loop try: while True: time.sleep(1) - except KeyboardInterrupt: - print("\n👋 Interrupt received - shutting down gracefully...") + print("\n👋 Shutting down gracefully...") except Exception as e: print(f"\n❌ Demo error: {e}") traceback.print_exc() - # Cleanup + # 7. Cleanup print("\n🧹 Cleaning up...") - - # Disconnect policy policy.disconnect() - - # shutdown threads data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - - # Conditional cleanup for Quest - if quest_thread is not None: - quest_thread.join() - if quest_reader is not None: + if quest_reader: quest_reader.stop() - - ik_thread.join() - camera_thread_obj.join() + for t in active_threads: + t.join() robot_controller.cleanup() - + visualizer.stop() nc.logout() - - print("\n👋 Demo stopped.") \ No newline at end of file + print("👋 Demo stopped.") \ No newline at end of file diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 132cde6..7815694 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -1,90 +1,25 @@ #!/usr/bin/env python3 -"""Minimal Piper Robot Policy Test - Terminal only, no GUI. - -Simple script that: -1. Enables robot -2. Sends robot home -3. Runs policy in continuous loop (get image, run policy, execute horizon, repeat) -4. On cancellation: sends robot home and exits -""" +"""Minimal Piper Robot Policy Test - Terminal only, no GUI.""" import argparse import sys -import threading import time import traceback from pathlib import Path - import neuracore as nc -# Add parent directory to path sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import ( - NEUTRAL_JOINT_ANGLES, - POLICY_EXECUTION_RATE, - PREDICTION_HORIZON_EXECUTION_RATIO, - ROBOT_RATE, - URDF_PATH, -) -from common.data_manager import DataManager, RobotActivityState -from common.policy_helpers import ( - convert_predictions_to_horizon, - embodiment_names_ordered, - get_policy_embodiments, - gripper_open_at_index, - joint_targets_deg_at_index, - log_robot_state_for_policy, - print_policy_embodiments, -) +from common.configs import URDF_PATH, POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO +from common.data_manager import RobotActivityState +from common.policy_helpers import get_policy_embodiments, print_policy_embodiments, embodiment_names_ordered, gripper_open_at_index, joint_targets_deg_at_index, log_robot_state_for_policy from common.policy_state import PolicyState -from common.threads.joint_state import joint_state_thread -from common.threads.realsense_camera import camera_thread -from neuracore_types import DataType, EmbodimentDescription - -from piper_controller import PiperController - - -def run_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - input_embodiment_description: EmbodimentDescription, -) -> bool: - """Run policy and get prediction horizon.""" - if not log_robot_state_for_policy(data_manager, input_embodiment_description): - print("⚠️ No policy input data available") - return False - - # Get policy prediction - try: - start_time = time.time() - predictions = policy.predict(timeout=5) - prediction_horizon = convert_predictions_to_horizon(predictions) - elapsed = time.time() - start_time - - # Get horizon length from the first joint (all should have same length) - horizon_length = policy_state.get_prediction_horizon_length() - print(f"✓ Got {horizon_length} actions in {elapsed:.3f}s") +from common.system_bootstrap import bootstrap_robot_system +from common.policy_actions import run_policy +from neuracore_types import DataType - policy_state.set_prediction_horizon(prediction_horizon) - return True - - except Exception as e: - print(f"✗ Policy prediction failed: {e}") - traceback.print_exc() - return False - - -def execute_horizon( - data_manager: DataManager, - policy_state: PolicyState, - robot_controller: PiperController, - frequency: int, - input_embodiment_description: EmbodimentDescription, - output_gripper_names: list[str] | None, -) -> None: - """Execute prediction horizon.""" +def execute_horizon(data_manager, policy_state, robot_controller, frequency, input_embodiment, output_grippers): + """Minimal terminal-only execution loop for the prediction horizon.""" policy_state.start_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) @@ -99,19 +34,13 @@ def execute_horizon( if joint_targets_deg is not None: robot_controller.set_target_joint_angles(joint_targets_deg) - gripper_target = gripper_open_at_index( - locked_horizon, i, gripper_names=output_gripper_names - ) + gripper_target = gripper_open_at_index(locked_horizon, i, gripper_names=output_grippers) if gripper_target is not None: robot_controller.set_gripper_open_value(gripper_target) - log_robot_state_for_policy(data_manager, input_embodiment_description) + log_robot_state_for_policy(data_manager, input_embodiment) + time.sleep(max(0, dt - (time.time() - start_time))) - # Sleep to maintain rate - elapsed = time.time() - start_time - time.sleep(max(0, dt - elapsed)) - - # End execution policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) @@ -119,213 +48,77 @@ def execute_horizon( if __name__ == "__main__": parser = argparse.ArgumentParser(description="Minimal Piper Policy Test") policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument( - "--train-run-name", - type=str, - default=None, - help="Name of the training run to load policy from (for cloud training).", - ) - policy_group.add_argument( - "--model-path", - type=str, - default=None, - help="Path to local model file to load policy from.", - ) - policy_group.add_argument( - "--remote-endpoint-name", - type=str, - default=None, - help="Name of remote Neuracore policy endpoint.", - ) - parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Neuracore robot name (policy embodiment resolution).", - ) - parser.add_argument( - "--frequency", - type=int, - default=POLICY_EXECUTION_RATE, - help="Frequency of policy execution", - ) - parser.add_argument( - "--execution-ratio", - type=float, - default=PREDICTION_HORIZON_EXECUTION_RATIO, - help="Execution ratio of the policy", - ) + policy_group.add_argument("--train-run-name", type=str, default=None) + policy_group.add_argument("--model-path", type=str, default=None) + policy_group.add_argument("--remote-endpoint-name", type=str, default=None) + parser.add_argument("--robot-name", type=str, default="AgileX PiPER") + parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE) + parser.add_argument("--execution-ratio", type=float, default=PREDICTION_HORIZON_EXECUTION_RATIO) args = parser.parse_args() - print("=" * 60) - print("PIPER POLICY ROLLOUT") - print("=" * 60) + print("=" * 60 + "\nPIPER POLICY ROLLOUT (MINIMAL)\n" + "=" * 60) - # Initialize Neuracore - print("\n🔧 Initializing Neuracore...") + # 1. Connect & Load Policy nc.login() - nc.connect_robot( - robot_name=args.robot_name, - urdf_path=str(URDF_PATH), - overwrite=False, - ) + nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) - if args.remote_endpoint_name is not None: - print( - f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." - ) - try: - policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: - print( - f"❌ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." - ) - sys.exit(1) - elif args.train_run_name is not None: - print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") - policy = nc.policy( - train_run_name=args.train_run_name, - device="cuda", - robot_name=args.robot_name, - ) + if args.remote_endpoint_name: + policy = nc.policy_remote_server(args.remote_endpoint_name) + elif args.train_run_name: + policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name) else: - print(f"\n🤖 Loading policy from model file: {args.model_path}...") - policy = nc.policy( - model_file=args.model_path, - device="cuda", - robot_name=args.robot_name, - ) - print(" ✓ Policy loaded successfully") - input_embodiment_description, output_embodiment_description = ( - get_policy_embodiments(policy) - ) - print_policy_embodiments( - input_embodiment_description, output_embodiment_description - ) + policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name) + + input_emb, output_emb = get_policy_embodiments(policy) + print_policy_embodiments(input_emb, output_emb) + output_gripper_names = None - if output_embodiment_description is not None: - gripper_spec = output_embodiment_description.get( - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ) - if gripper_spec is not None: - output_gripper_names = embodiment_names_ordered(gripper_spec) + if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb: + output_gripper_names = embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) - # Initialize state - data_manager = DataManager() + # 2. Bootstrap Core System (No Quest, No IK needed for minimal playback) + data_manager, robot_controller, _, active_threads = bootstrap_robot_system(start_ik=False, start_camera=True) policy_state = PolicyState() policy_state.set_execution_ratio(args.execution_ratio) - # Initialize robot controller - print("\n🤖 Initializing robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - debug_mode=False, - ) - robot_controller.start_control_loop() - - # Start joint state thread - print("\n📊 Starting joint state thread...") - joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) - joint_state_thread_obj.start() - - # Start camera thread - print("\n📷 Starting camera thread...") - camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ) - camera_thread_obj.start() - - # Wait for threads to initialize - print("\n⏳ Waiting for initialization...") - time.sleep(2.0) + time.sleep(2.0) # Wait for threads to init try: - # Enable robot - print("\n🟢 Enabling robot...") + print("\n🟢 Enabling and homing robot...") robot_controller.resume_robot() - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ Robot enabled") - - # Home robot - print("\n🏠 Moving to home position...") - robot_controller.move_to_home() data_manager.set_robot_activity_state(RobotActivityState.HOMING) - - # Wait for homing to complete - start_time = time.time() - while ( - data_manager.get_robot_activity_state() == RobotActivityState.HOMING - and not robot_controller.is_robot_homed() - and time.time() - start_time < 5.0 - ): + robot_controller.move_to_home() + + while data_manager.get_robot_activity_state() == RobotActivityState.HOMING and not robot_controller.is_robot_homed(): time.sleep(0.1) - print("✓ Robot homed") - - # Policy execution loop - print("\n🚀 Starting policy execution loop...") - print("Press Ctrl+C to stop\n") + + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ Robot ready. Starting policy execution loop... Press Ctrl+C to stop.\n") + # 3. Continuous Execution Loop while True: - # Run policy - if not run_policy( - data_manager, policy, policy_state, input_embodiment_description - ): + if not run_policy(data_manager, policy, policy_state, visualizer=None, input_embodiment_description=input_emb): print("⚠️ Policy run failed, retrying...") time.sleep(0.5) continue - - # Execute horizon - execute_horizon( - data_manager, - policy_state, - robot_controller, - args.frequency, - input_embodiment_description, - output_gripper_names, - ) + + execute_horizon(data_manager, policy_state, robot_controller, args.frequency, input_emb, output_gripper_names) except KeyboardInterrupt: - print("\n\n👋 Interrupt received - shutting down...") - + print("\n👋 Shutting down...") except Exception as e: print(f"\n❌ Error: {e}") traceback.print_exc() - finally: - # Cleanup print("\n🧹 Cleaning up...") - - # Home robot - print("\n🏠 Moving to home position...") data_manager.set_robot_activity_state(RobotActivityState.HOMING) robot_controller.move_to_home() - - # Wait for homing to complete - start_time = time.time() - while ( - data_manager.get_robot_activity_state() == RobotActivityState.HOMING - and not robot_controller.is_robot_homed() - and time.time() - start_time < 5.0 - ): - time.sleep(0.1) - print("✓ Robot homed") - - # Shutdown + time.sleep(1.0) + policy.disconnect() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) data_manager.request_shutdown() - joint_state_thread_obj.join() - camera_thread_obj.join() - time.sleep(0.5) # Give threads time to stop - + for t in active_threads: + t.join() robot_controller.cleanup() - nc.logout() - - print("✓ Cleanup complete") - print("\n👋 Done.") + nc.logout() \ No newline at end of file diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index 64d7f7e..7d5847e 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -1,9 +1,5 @@ #!/usr/bin/env python3 -"""Simple policy visualization from dataset. - -Loads a policy and a dataset, and randomly selects a state -from the dataset to run the policy with and visualize the results. -""" +"""Simple policy visualization from dataset.""" import argparse import random @@ -11,265 +7,115 @@ import time import traceback from pathlib import Path - import neuracore as nc import numpy as np import viser import yourdfpy -from neuracore.core.utils.robot_data_spec_utils import ( - merge_cross_embodiment_description, -) -from neuracore_types import DataType from PIL import Image from viser.extras import ViserUrdf -# Add parent directory to path sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH +from common.dataset_helpers import load_and_sync_dataset from common.policy_helpers import ( - DEFAULT_ROBOT_NAME, - convert_predictions_to_horizon, - embodiment_names_ordered, - get_policy_embodiments, - gripper_open_at_index, - horizon_length, - log_sync_step_for_policy, - print_policy_embodiments, - urdf_cfg_from_horizon, -) - -# Parse arguments -parser = argparse.ArgumentParser( - description="Visualize policy predictions from dataset" -) -parser.add_argument("--dataset-name", type=str, required=True, help="Dataset name") -policy_group = parser.add_mutually_exclusive_group(required=True) -policy_group.add_argument( - "--train-run-name", type=str, default=None, help="Training run name" -) -policy_group.add_argument( - "--model-path", type=str, default=None, help="Model file path" -) -policy_group.add_argument( - "--remote-endpoint-name", - type=str, - default=None, - help="Name of remote Neuracore policy endpoint to use instead of a local policy.", + DEFAULT_ROBOT_NAME, convert_predictions_to_horizon, embodiment_names_ordered, + get_policy_embodiments, gripper_open_at_index, horizon_length, + log_sync_step_for_policy, print_policy_embodiments, urdf_cfg_from_horizon, ) -parser.add_argument( - "--robot-name", - type=str, - default=DEFAULT_ROBOT_NAME, - help="Neuracore robot name (policy embodiment resolution).", -) -parser.add_argument( - "--frequency", - type=int, - default=POLICY_EXECUTION_RATE, - help="Frequency of visualization", -) -args = parser.parse_args() - -# Connect to Neuracore -print("🔧 Initializing Neuracore...") -nc.login() -nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) +from neuracore_types import DataType -if args.remote_endpoint_name: - print(f"🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") - try: +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Visualize policy predictions") + parser.add_argument("--dataset-name", type=str, required=True) + policy_group = parser.add_mutually_exclusive_group(required=True) + policy_group.add_argument("--train-run-name", type=str, default=None) + policy_group.add_argument("--model-path", type=str, default=None) + policy_group.add_argument("--remote-endpoint-name", type=str, default=None) + parser.add_argument("--robot-name", type=str, default=DEFAULT_ROBOT_NAME) + parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE) + args = parser.parse_args() + + nc.login() + nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + + if args.remote_endpoint_name: policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: - print( - f"❌ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." - ) - sys.exit(1) -elif args.train_run_name: - print(f"🤖 Loading policy from training run: {args.train_run_name}...") - policy = nc.policy( - train_run_name=args.train_run_name, - device="cuda", - robot_name=args.robot_name, - ) -else: - print(f"🤖 Loading policy from model file: {args.model_path}...") - policy = nc.policy( - model_file=args.model_path, - device="cuda", - robot_name=args.robot_name, - ) -print(" ✓ Policy loaded") -input_embodiment_description, output_embodiment_description = get_policy_embodiments( - policy -) -print_policy_embodiments(input_embodiment_description, output_embodiment_description) - -output_gripper_names = None -if output_embodiment_description is not None: - gripper_spec = output_embodiment_description.get( - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ) - if gripper_spec is not None: - output_gripper_names = embodiment_names_ordered(gripper_spec) - -# Load and synchronize dataset -print(f"🔍 Loading dataset: {args.dataset_name}...") -dataset = nc.get_dataset(args.dataset_name) -print(f" ✓ Dataset loaded: {len(dataset)} episodes") - -print("🔁 Building cross_embodiment_union for synchronization...") -input_cross_embodiment_description = { - robot_id: input_embodiment_description for robot_id in dataset.robot_ids -} -output_cross_embodiment_description = ( - {robot_id: output_embodiment_description for robot_id in dataset.robot_ids} - if output_embodiment_description is not None - else {} -) -cross_embodiment_union = merge_cross_embodiment_description( - input_cross_embodiment_description, - output_cross_embodiment_description, -) - -print("🔁 Synchronizing dataset...") -synced_dataset = dataset.synchronize( - frequency=args.frequency, - cross_embodiment_union=cross_embodiment_union, - prefetch_videos=True, - max_prefetch_workers=2, -) -print(f" ✓ Dataset synchronized: {len(synced_dataset)} episodes") - -# Setup Viser -print("🖥️ Starting Viser...") -server = viser.ViserServer() -server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) + elif args.train_run_name: + policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name) + else: + policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name) -# Load URDF -urdf = yourdfpy.URDF.load(str(URDF_PATH)) -urdf_vis = ViserUrdf(server, urdf, root_node_name="/robot") -urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) + input_emb, output_emb = get_policy_embodiments(policy) + print_policy_embodiments(input_emb, output_emb) -# State variables -current_horizon = None -current_action_idx = 0 -playing = False -rgb_gui_handle = None + out_grippers = embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb else None + input_mods = list(input_emb.keys()) + output_mods = list(output_emb.keys()) if output_emb else [] + synced_dataset = load_and_sync_dataset(args.dataset_name, args.frequency, input_mods, output_mods, prefetch_videos=True) -def select_random_state() -> None: - """Select random state and run policy.""" - global current_horizon, current_action_idx, playing, rgb_gui_handle + print("🖥️ Starting Viser...") + server = viser.ViserServer() + server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) + urdf_vis = ViserUrdf(server, yourdfpy.URDF.load(str(URDF_PATH)), root_node_name="/robot") + urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) - episode_idx = random.randint(0, len(synced_dataset) - 1) - episode = synced_dataset[episode_idx] - if len(episode) == 0: - print(f"⚠️ Episode {episode_idx} is empty") - return + state = {"horizon": None, "action_idx": 0, "playing": False, "rgb_handle": None} - step_idx = random.randint(0, len(episode) - 1) - step = episode[step_idx] - print(f"📊 Selected episode {episode_idx}, step {step_idx}") + def select_random_state() -> None: + episode = synced_dataset[random.randint(0, len(synced_dataset) - 1)] + if not len(episode): return + step = episode[random.randint(0, len(episode) - 1)] - if not log_sync_step_for_policy(step, input_embodiment_description): - print("⚠️ Step has no data for policy input channels") - return + if not log_sync_step_for_policy(step, input_emb): return - if hasattr(step, "data"): - rgb_data = step.data.get(DataType.RGB_IMAGES, {}) - for _camera_name, frame in rgb_data.items(): - rgb_image = np.array(frame.frame) - image_pil = Image.fromarray(rgb_image) - image_pil.save("current_image.png") - print("💾 Saved image to current_image.png") - if rgb_gui_handle is None: - rgb_gui_handle = server.gui.add_image( - rgb_image, - label="RGB (current step)", - format="jpeg", - jpeg_quality=85, - ) + rgb_data = getattr(step, "data", {}).get(DataType.RGB_IMAGES, {}) + for _, frame in rgb_data.items(): + rgb_arr = np.array(frame.frame) + Image.fromarray(rgb_arr).save("current_image.png") + if state["rgb_handle"] is None: + state["rgb_handle"] = server.gui.add_image(rgb_arr, label="RGB (current step)", format="jpeg") else: - rgb_gui_handle.image = rgb_image + state["rgb_handle"].image = rgb_arr break - print("🎯 Getting policy prediction...") - start_time = time.time() - try: - predictions = policy.predict(timeout=60) - except nc.EndpointError as e: - print(f"✗ Failed to get policy prediction: {e}") - traceback.print_exc() - return - duration = time.time() - start_time - current_horizon = convert_predictions_to_horizon(predictions) - current_action_idx = 0 - playing = True - print(f"FINISHED PREDICTION in {duration:.3f} s") - - joint_cfg = urdf_cfg_from_horizon(current_horizon or {}, 0) - if joint_cfg is not None: - urdf_vis.update_cfg(joint_cfg) - - print(f"✅ Prediction received: {horizon_length(current_horizon or {})} actions") - + try: + state["horizon"] = convert_predictions_to_horizon(policy.predict(timeout=60)) + state["action_idx"] = 0 + state["playing"] = True + + joint_cfg = urdf_cfg_from_horizon(state["horizon"], 0) + if joint_cfg is not None: urdf_vis.update_cfg(joint_cfg) + except Exception as e: + print(f"✗ Failed prediction: {e}") -# Add button -random_button = server.gui.add_button("Random Selection") -random_button.on_click(lambda _: select_random_state()) + server.gui.add_button("Random Selection").on_click(lambda _: select_random_state()) + gripper_handle = server.gui.add_slider("Gripper Open Amount", min=0.0, max=1.0, step=0.01, initial_value=0.0, disabled=True) + freq_handle = server.gui.add_number("Visualization Frequency (Hz)", initial_value=args.frequency, min=1.0, max=500.0, step=1.0) -# Add gripper value display -gripper_handle = server.gui.add_slider( - "Gripper Open Amount", - min=0.0, - max=1.0, - step=0.01, - initial_value=0.0, - disabled=True, -) - -# Add frequency control -frequency_handle = server.gui.add_number( - "Visualization Frequency (Hz)", - initial_value=args.frequency, - min=1.0, - max=500.0, - step=1.0, -) - -select_random_state() -try: - while True: - start_time = time.time() - - h_len = horizon_length(current_horizon or {}) - if playing and current_horizon and h_len > 0: - if current_action_idx < h_len: - joint_cfg = urdf_cfg_from_horizon(current_horizon, current_action_idx) - if joint_cfg is not None: - urdf_vis.update_cfg(joint_cfg) - nc.log_joint_positions( - {jn: float(joint_cfg[i]) for i, jn in enumerate(JOINT_NAMES)} - ) + select_random_state() - gripper_value = gripper_open_at_index( - current_horizon, - current_action_idx, - gripper_names=output_gripper_names, - ) - if gripper_value is not None: - gripper_handle.value = round(gripper_value, 2) - - current_action_idx = (current_action_idx + 1) % h_len - - elapsed = time.time() - start_time - frequency = max(frequency_handle.value, 0.1) - time.sleep(max(0, 1.0 / frequency - elapsed)) - -except KeyboardInterrupt: - print("\n👋 Shutting down...") -finally: - policy.disconnect() - nc.logout() + try: + while True: + start_time = time.time() + h_len = horizon_length(state["horizon"] or {}) + + if state["playing"] and state["horizon"] and h_len > 0: + if state["action_idx"] < h_len: + j_cfg = urdf_cfg_from_horizon(state["horizon"], state["action_idx"]) + if j_cfg is not None: + urdf_vis.update_cfg(j_cfg) + nc.log_joint_positions({jn: float(j_cfg[i]) for i, jn in enumerate(JOINT_NAMES)}) + + g_val = gripper_open_at_index(state["horizon"], state["action_idx"], out_grippers) + if g_val is not None: gripper_handle.value = round(g_val, 2) + state["action_idx"] = (state["action_idx"] + 1) % h_len + + time.sleep(max(0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time))) + + except KeyboardInterrupt: + print("\n👋 Shutting down...") + finally: + policy.disconnect() + nc.logout() \ No newline at end of file diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 4b00a1f..ead1a2e 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -1,9 +1,5 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest and Foot Pedal control. - -This script combines Meta Quest controller input for movement with Foot Pedal -control for session management (Activate, Home, Record). -""" +"""Piper Robot Teleoperation with Meta Quest and Foot Pedal control.""" import argparse import multiprocessing @@ -11,269 +7,91 @@ import threading import time from pathlib import Path -from typing import Any - import neuracore as nc -import numpy as np -# Add parent directory to path to import pink_ik_solver and piper_controller sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import ( - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_MIN_CUTOFF, - GRIPPER_FRAME_NAME, - GRIPPER_LOGGING_NAME, - JOINT_NAMES, - NEUTRAL_JOINT_ANGLES, - POSTURE_COST_VECTOR, - ROBOT_RATE, - URDF_PATH, +from common.configs import URDF_PATH +from common.system_bootstrap import bootstrap_robot_system +from common.shared_actions import ( + toggle_robot_enabled, move_robot_home, + toggle_recording, neuracore_logging_callback ) -from common.data_manager import DataManager, RobotActivityState from common.foot_pedal import FootPedal -from common.threads.camera import camera_thread -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread from common.threads.quest_reader import quest_reader_thread from meta_quest_teleop.reader import MetaQuestReader -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - ENABLE_DISABLE_PEDAL = "a" HOME_POSITION_PEDAL = "b" RECORD_TOGGLE_PEDAL = "c" - -def log_to_neuracore_on_change_callback( - name: str, value: Any, timestamp: float -) -> None: - """Callback triggered on state changes to log data to Neuracore. - - Args: - name: Name of the data stream. - value: Data value (float, array, or image). - timestamp: Time of the change. - """ - try: - if name == "log_joint_positions": - data_dict = {jn: np.radians(a) for jn, a in zip(JOINT_NAMES, value)} - nc.log_joint_positions(data_dict, timestamp=timestamp) - elif name == "log_joint_target_positions": - data_dict = {jn: np.radians(a) for jn, a in zip(JOINT_NAMES, value)} - nc.log_joint_target_positions(data_dict, timestamp=timestamp) - elif name == "log_parallel_gripper_open_amounts": - nc.log_parallel_gripper_open_amounts( - {GRIPPER_LOGGING_NAME: value}, timestamp=timestamp - ) - elif name == "log_parallel_gripper_target_open_amounts": - nc.log_parallel_gripper_target_open_amounts( - {GRIPPER_LOGGING_NAME: value}, timestamp=timestamp - ) - elif name == "log_rgb": - nc.log_rgb("rgb", value, timestamp=timestamp) - except Exception as e: - print(f"⚠️ Logging failed: {e}") - - -def toggle_robot_state() -> None: - """Toggle the robot's activity state between ENABLED and DISABLED.""" - print("🔘 Pedal toggled - Robot State") - state = data_manager.get_robot_activity_state() - - if state == RobotActivityState.ENABLED: - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - if robot_controller: - robot_controller.graceful_stop() - data_manager.set_teleop_state(False, None, None) - print("✓ 🔴 Robot disabled (Pedal)") - elif state == RobotActivityState.DISABLED or state == RobotActivityState.HOMING: - # If no robot, just toggle the state for dashboard visibility - if not robot_controller: - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Pedal - Headless)") - return - - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Pedal)") - else: - print("✗ [ACTION] Failed to enable robot") - - -def move_robot_home() -> None: - """Command the robot to move to its neutral/home position.""" - print("🏠 Pedal toggled - Move Home") - state = data_manager.get_robot_activity_state() - - if state == RobotActivityState.ENABLED: - print("🏠 Pedal pressed - Moving to home position...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - data_manager.set_teleop_state(False, None, None) - - if robot_controller: - if not robot_controller.move_to_home(): - print("✗ Failed to initiate home move") - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - # Headless simulation of homing - time.sleep(1) - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🏠 Robot homed (Headless)") - else: - print("⚠️ Pedal pressed but robot is not enabled") - - -def toggle_recording() -> None: - """Start or stop a data recording session in Neuracore.""" - print("⏺️ Pedal toggled - Recording") - if not nc.is_recording(): - try: - nc.start_recording() - print("✓ 🔴 Recording started (Pedal)") - except Exception as e: - print(f"✗ Failed to start recording: {e}") - else: - try: - nc.stop_recording() - print("✓ ⏹️ Recording stopped (Pedal)") - except Exception as e: - print(f"✗ Failed to stop recording: {e}") - - if __name__ == "__main__": multiprocessing.set_start_method("spawn") - - parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleoperation") + parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleop") parser.add_argument("--ip-address", type=str, help="Meta Quest IP") parser.add_argument("--dataset-name", type=str, help="Neuracore dataset name") args = parser.parse_args() - print("=" * 60) - print("PIPER TELEOP: META QUEST + FOOT PEDALS") - print("=" * 60) + print("=" * 60 + "\nPIPER TELEOP: META QUEST + FOOT PEDALS\n" + "=" * 60) - # Neuracore Init - print("\n🔧 Initializing Neuracore...") + # 1. Init Neuracore try: nc.login() - nc.connect_robot( - robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False - ) + nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%H-%M-%S')}" nc.create_dataset(name=ds_name, description="Quest + Pedal unified collection") except Exception as e: - print(f"⚠️ Neuracore initialization skipped/failed: {e}") + print(f"⚠️ Neuracore init skipped/failed: {e}") - # Shared State - data_manager = DataManager() - data_manager.set_on_change_callback(log_to_neuracore_on_change_callback) - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF + # 2. Bootstrap Core System + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + start_ik=True, start_camera=True ) + data_manager.set_on_change_callback(neuracore_logging_callback) - # Robot Initialization - print("\n🤖 Initializing Piper...") - robot_controller = None - try: - robot_controller = PiperController(can_interface="can0", robot_rate=ROBOT_RATE) - robot_controller.start_control_loop() - except Exception as e: - print(f"⚠️ Robot controller initialization skipped/failed: {e}") - - # Threads - print("\n📊 Starting Threads (JointState, QuestReader, IKSolver, Camera)...") - pedal_thread = None - if robot_controller: - threading.Thread( - target=joint_state_thread, - args=(data_manager, robot_controller), - daemon=True, - ).start() - + # 3. Quest Reader quest_reader = None try: print("🔍 Searching for Meta Quest...") - # Adb initialization in the reader might call sys.exit(1), so we catch BaseException quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) - threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ).start() + quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) + quest_thread.start() + active_threads.append(quest_thread) except (Exception, BaseException) as e: - print(f"⚠️ Quest reader initialization skipped/failed: {e}") + print(f"⚠️ Quest reader init skipped/failed: {e}") - # Sync IK solver to current position - try: - initial_joints = np.radians( - data_manager.get_current_joint_angles() or NEUTRAL_JOINT_ANGLES - ) - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - initial_configuration=initial_joints, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ).start() - except Exception as e: - print(f"⚠️ IK Solver initialization skipped/failed: {e}") - - try: - threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ).start() - except Exception as e: - print(f"⚠️ Camera thread failed to start: {e}") - - # Foot Pedal – started as a daemon thread, callbacks wired inline + # 4. Foot Pedal Init & Binding print("\n⌨️ Initializing Foot Pedals...") - pedal = FootPedal( - key_map={ - "button_a": ENABLE_DISABLE_PEDAL, - "button_b": HOME_POSITION_PEDAL, - "button_c": RECORD_TOGGLE_PEDAL, - }, - ) - pedal.bind("button_a", toggle_robot_state) - pedal.bind("button_b", move_robot_home) - pedal.bind("button_c", toggle_recording) - - pedal_thread = threading.Thread( - target=pedal.run_loop, args=(data_manager,), daemon=True - ) + pedal = FootPedal({ + "button_a": ENABLE_DISABLE_PEDAL, + "button_b": HOME_POSITION_PEDAL, + "button_c": RECORD_TOGGLE_PEDAL, + }) + + # 🔗 Bind the Shared Actions + pedal.bind("button_a", lambda: toggle_robot_enabled(data_manager, robot_controller)) + pedal.bind("button_b", lambda: move_robot_home(data_manager, robot_controller)) + pedal.bind("button_c", lambda: toggle_recording(play_audio=False)) + + pedal_thread = threading.Thread(target=pedal.run_loop, args=(data_manager,), daemon=True) pedal_thread.start() + active_threads.append(pedal_thread) print("\n✅ SYSTEM ONLINE") - print("------------------------------------------------------------") - print("🎮 QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") - print("⌨️ PEDAL CONTROLS: ENABLE/DISABLE (A), HOME (B), RECORD (C)") - print("------------------------------------------------------------") + print("🎮 QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") + print("⌨️ PEDAL CONTROLS: ENABLE/DISABLE (A), HOME (B), RECORD (C)") + # 5. Wait Loop try: while True: time.sleep(1) except KeyboardInterrupt: print("\n👋 Shutting down...") finally: - if pedal_thread: - pedal_thread.join(timeout=1.0) - try: - if nc.is_recording(): - nc.cancel_recording() - nc.logout() - except Exception: - pass + if nc.is_recording(): nc.cancel_recording() + try: nc.logout() + except: pass data_manager.request_shutdown() - if quest_reader: - try: - quest_reader.stop() - except Exception: - pass - if robot_controller: - try: - robot_controller.cleanup() - except Exception: - pass + if quest_reader: quest_reader.stop() + for t in active_threads: t.join(timeout=1.0) + if robot_controller: robot_controller.cleanup() \ No newline at end of file diff --git a/examples/combined_code_old.txt b/examples/combined_code_old.txt new file mode 100644 index 0000000..d002516 --- /dev/null +++ b/examples/combined_code_old.txt @@ -0,0 +1,6025 @@ + +============================================================ +FILE: 1_tune_teleop_params.py +============================================================ + +#!/usr/bin/env python3 +"""Piper Robot Teleoperation with Meta Quest Controller - REAL ROBOT CONTROL. + +This demo uses Pink IK control with Meta Quest controller input to control the REAL robot. +- REAL ROBOT CONTROL - sends commands to physical robot! +- Uses right hand controller grip as dead man's button +- Uses ROS pointer frame for natural pointing control +- Applies relative transformations accounting for different coordinate frames +""" + +import argparse +import sys +import threading +import time +import traceback +from pathlib import Path + +import numpy as np + +# Add parent directory to path to import pink_ik_solver and piper_controller +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from common.configs import ( + CAMERA_FRAME_STREAMING_RATE, + CAMERA_NAMES, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + CONTROLLER_DATA_RATE, + CONTROLLER_MIN_CUTOFF, + DAMPING_COST, + FRAME_TASK_GAIN, + GRIPPER_FRAME_NAME, + IK_SOLVER_RATE, + JOINT_STATE_STREAMING_RATE, + LM_DAMPING, + META_QUEST_AXIS_MASK, + NEUTRAL_END_EFFECTOR_POSE, + NEUTRAL_JOINT_ANGLES, + ORIENTATION_COST, + POSITION_COST, + POSTURE_COST_VECTOR, + ROBOT_RATE, + ROTATION_SCALE, + SLOW_ROTATION_SCALE, + SLOW_TRANSLATION_SCALE, + SOLVER_DAMPING_VALUE, + SOLVER_NAME, + TRANSLATION_SCALE, + URDF_PATH, + VISUALIZATION_RATE, + WRIST_JOINT_BUTTON_STEP_DEGREES, +) +from common.data_manager import DataManager, RobotActivityState +from common.robot_visualizer import RobotVisualizer +from common.threads.ik_solver import ik_solver_thread +from common.threads.joint_state import joint_state_thread +from common.threads.quest_reader import quest_reader_thread +from common.threads.realsense_camera import camera_thread +from meta_quest_teleop.reader import MetaQuestReader + +from pink_ik_solver import PinkIKSolver +from piper_controller import PiperController + + +def on_button_a_pressed() -> None: + """Handle Button A press to toggle robot enable/disable state.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + robot_controller.graceful_stop() + # Reset teleop state when disabling robot + data_manager.set_teleop_state(False, None, None) + print("✓ 🔴 Robot disabled (Button A)") + elif robot_activity_state == RobotActivityState.DISABLED: + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ 🟢 Robot enabled (Button A)") + else: + print("✗ Failed to enable robot") + + +def on_button_b_pressed() -> None: + """Handle Button B press to move robot to home position.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + print("🏠 Button B pressed - Moving to home position...") + # Set state to HOMING to prevent IK thread from sending robot commands + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + # Disable teleop during homing + data_manager.set_teleop_state(False, None, None) + ok = robot_controller.move_to_home() + if not ok: + print("✗ Failed to initiate home move") + # Revert to ENABLED on failure + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("⚠️ Button B pressed but robot is not enabled") + + +def _step_wrist_joint(delta_degrees: float) -> None: + """Apply a relative step to the wrist joint target angle.""" + # Prevent IK teleop loop from overwriting this manual joint adjustment. + data_manager.set_teleop_state(False, None, None) + robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) + + target_joint_angles = robot_controller.get_target_joint_angles() + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + base_wrist_joint_angle = float(current_joint_angles[-1]) + else: + base_wrist_joint_angle = float(target_joint_angles[-1]) + + target_joint_angles[-1] = base_wrist_joint_angle + delta_degrees + robot_controller.set_target_joint_angles(target_joint_angles) + data_manager.set_target_joint_angles(target_joint_angles) + + +def on_button_y_pressed() -> None: + """Handle Button Y press to add +5° on the wrist joint.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state != RobotActivityState.ENABLED: + print("⚠️ Button Y pressed but robot is not enabled") + return + _step_wrist_joint(WRIST_JOINT_BUTTON_STEP_DEGREES) + + +def on_button_x_pressed() -> None: + """Handle Button X press to subtract -5° on the wrist joint.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state != RobotActivityState.ENABLED: + print("⚠️ Button X pressed but robot is not enabled") + return + _step_wrist_joint(-WRIST_JOINT_BUTTON_STEP_DEGREES) + + +def on_button_lj_pressed() -> None: + """Toggle slow translation/rotation scaling mode from config values.""" + slow_scaling_mode_enabled = data_manager.toggle_slow_scaling_mode_enabled() + if slow_scaling_mode_enabled: + print( + "🐢 Button LJ pressed - Slow scaling enabled " + f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " + f"rotation={SLOW_ROTATION_SCALE:.3f})" + ) + else: + print( + "🐇 Button LJ pressed - Slow scaling disabled " + f"(using GUI/default scales, currently translation={TRANSLATION_SCALE:.3f}, " + f"rotation={ROTATION_SCALE:.3f})" + ) + + +parser = argparse.ArgumentParser( + description="Piper Robot Teleoperation - REAL ROBOT CONTROL" +) +parser.add_argument( + "--ip-address", + type=str, + default=None, + help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", +) +args = parser.parse_args() + +print("=" * 60) +print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL") +print("=" * 60) +print("Thread frequencies:") +print(f" 🎮 Quest Reader: {CONTROLLER_DATA_RATE} Hz") +print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") +print(f" 🖥️ Visualization: {VISUALIZATION_RATE} Hz (running in the main thread)") +print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") +print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") +print(f" 📷 Camera: {CAMERA_FRAME_STREAMING_RATE} Hz") + + +# Initialize shared state +data_manager = DataManager() +data_manager.set_controller_filter_params( + CONTROLLER_MIN_CUTOFF, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, +) + +# Initialize robot controller +print("\n🤖 Initializing Piper robot controller...") +robot_controller = PiperController( + can_interface="can0", + robot_rate=ROBOT_RATE, + control_mode=PiperController.ControlMode.JOINT_SPACE, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, + enable_joint_angle_limits=False, + debug_mode=False, +) + +# Start robot control loop +print("\n🚀 Starting robot control loop...") +robot_controller.start_control_loop() + +# Start joint state thread +print("\n📊 Starting joint state thread...") +joint_state_thread_obj = threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True +) +joint_state_thread_obj.start() + +# Initialize Meta Quest reader +print("\n🎮 Initializing Meta Quest reader...") +quest_reader = MetaQuestReader( + ip_address=args.ip_address, + port=5555, + run=True, + axis_mask=META_QUEST_AXIS_MASK, +) + +# Register button callbacks (after state and robot_controller are initialized) +quest_reader.on("button_a_pressed", on_button_a_pressed) +quest_reader.on("button_b_pressed", on_button_b_pressed) +quest_reader.on("button_y_pressed", on_button_y_pressed) +quest_reader.on("button_x_pressed", on_button_x_pressed) +quest_reader.on("button_lj_pressed", on_button_lj_pressed) + +# Start quest reader thread +print("\n🎮 Starting quest reader thread...") +quest_thread = threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True +) +quest_thread.start() + +# set initial configuration to current joint angles +current_joint_angles = data_manager.get_current_joint_angles() +if current_joint_angles is not None: + initial_joint_angles = np.radians(current_joint_angles) +else: + initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) + +# Create Pink IK solver +print("\n🔧 Creating Pink IK solver...") +ik_solver = PinkIKSolver( + urdf_path=URDF_PATH, + end_effector_frame=GRIPPER_FRAME_NAME, + solver_name=SOLVER_NAME, + position_cost=POSITION_COST, + orientation_cost=ORIENTATION_COST, + frame_task_gain=FRAME_TASK_GAIN, + lm_damping=LM_DAMPING, + damping_cost=DAMPING_COST, + solver_damping_value=SOLVER_DAMPING_VALUE, + integration_time_step=1 / IK_SOLVER_RATE, + initial_configuration=initial_joint_angles, + posture_cost_vector=np.array(POSTURE_COST_VECTOR), +) + +# Start IK solver thread +print("\n🧮 Starting IK solver thread...") +ik_thread = threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True +) +ik_thread.start() + +# Start camera thread (if RealSense is available) +print("\n📷 Starting camera thread...") +camera_thread_obj = threading.Thread( + target=camera_thread, args=(data_manager,), daemon=True +) +camera_thread_obj.start() + + +# Set up visualizer +print("\n🖥️ Starting visualization...") +visualizer = RobotVisualizer(urdf_path=URDF_PATH) +visualizer.add_basic_controls() +visualizer.add_teleop_controls() +visualizer.add_gripper_status_controls() +visualizer.add_homing_controls() +visualizer.add_toggle_robot_enabled_status_button() +visualizer.add_rgb_image_placeholder() +visualizer.add_target_frame_visualization() +visualizer.add_controller_filter_controls( + initial_min_cutoff=CONTROLLER_MIN_CUTOFF, + initial_beta=CONTROLLER_BETA, + initial_d_cutoff=CONTROLLER_D_CUTOFF, +) +visualizer.add_scaling_controls( + initial_translation_scale=TRANSLATION_SCALE, + initial_rotation_scale=ROTATION_SCALE, +) +visualizer.add_pink_parameter_controls( + position_cost=POSITION_COST, + orientation_cost=ORIENTATION_COST, + frame_task_gain=FRAME_TASK_GAIN, + lm_damping=LM_DAMPING, + damping_cost=DAMPING_COST, + solver_damping_value=SOLVER_DAMPING_VALUE, + posture_cost_vector=POSTURE_COST_VECTOR, +) +visualizer.add_controller_visualization() + + +# Set up button callbacks +def toggle_robot_enabled_status() -> None: + """Toggle robot enabled/disabled state and update GUI button label.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + # Disable robot + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + robot_controller.graceful_stop() + # Reset teleop state when disabling robot + data_manager.set_teleop_state(False, None, None) + visualizer.update_toggle_robot_enabled_status(False) + print("✓ 🔴 Robot disabled") + elif robot_activity_state == RobotActivityState.DISABLED: + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + visualizer.update_toggle_robot_enabled_status(True) + print("✓ 🟢 Robot enabled") + else: + print("✗ Failed to enable robot") + + +def on_go_home() -> None: + """Handle Button B press to move robot to home position.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + print("🏠 GUI: Moving to home position...") + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + data_manager.set_teleop_state(False, None, None) + ok = robot_controller.move_to_home() + if not ok: + print("✗ Failed to initiate home move") + # Revert to ENABLED on failure + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("⚠️ Cannot home: robot not enabled") + + +visualizer.set_toggle_robot_enabled_status_callback(toggle_robot_enabled_status) +visualizer.set_go_home_callback(on_go_home) + +print() +print("🚀 Starting teleoperation with REAL ROBOT CONTROL...") +print("🎮 CONTROLS:") +print(" 1. Press BUTTON A to enable/disable robot (or use GUI)") +print(" 2. Hold RIGHT GRIP to activate teleoperation") +print(" 3. Move controller - robot follows!") +print(" 4. Hold RIGHT TRIGGER to close gripper") +print(" 5. Press BUTTON B to send robot home (or use GUI)") +print(" 6. Press BUTTON Y to add +5° on the wrist joint") +print(" 7. Press BUTTON X to subtract 5° on the wrist joint") +print(" 8. Press LEFT JOYSTICK BUTTON to toggle slow scaling mode") +print(" 9. Release grip to stop") +print(" 10. Use 'Emergency Stop' in GUI if needed") +print("⚠️ Press Ctrl+C to exit") +print() + +# Visualization loop variables +dt: float = 1.0 / VISUALIZATION_RATE + +try: + while True: + iteration_start: float = time.time() + + # Update filter parameters + min_cutoff, beta, d_cutoff = visualizer.get_controller_filter_params() + data_manager.set_controller_filter_params(min_cutoff, beta, d_cutoff) + + # Update scaling factors (shared with IK thread via DataManager) + translation_scale = visualizer.get_translation_scale() + rotation_scale = visualizer.get_rotation_scale() + if data_manager.get_slow_scaling_mode_enabled(): + translation_scale = SLOW_TRANSLATION_SCALE + rotation_scale = SLOW_ROTATION_SCALE + data_manager.set_teleop_scaling(translation_scale, rotation_scale) + + # Update Pink parameters (GUI controls) + pink_params = visualizer.get_pink_parameters() + ik_solver.update_task_parameters(**pink_params) + + # Get data from shared state + controller_transform, grip_value, trigger_value = ( + data_manager.get_controller_data() + ) + teleop_active = data_manager.get_teleop_active() + # Get updated robot_activity_state and joint states (may have changed if homing completed) + robot_activity_state = data_manager.get_robot_activity_state() + current_joint_angles = data_manager.get_current_joint_angles() + target_joint_angles = data_manager.get_target_joint_angles() + solve_time_ms = data_manager.get_ik_solve_time_ms() + ik_success = data_manager.get_ik_success() + target_pose = data_manager.get_target_pose() + rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) + + # Update GUI displays + visualizer.set_grip_value(grip_value) + visualizer.set_trigger_value(trigger_value) + + # Update timing display + visualizer.update_timing(solve_time_ms) + + # Update controller visualization + visualizer.update_controller_visualization(controller_transform) + if controller_transform is not None: + visualizer.update_controller_status_display( + controller_transform[:3, 3], connected=True + ) + else: + visualizer.update_controller_status_display(None, connected=False) + + # Update teleop status display + visualizer.update_teleop_status(teleop_active) + + # Update RGB camera image in Viser GUI (if available) + if rgb_image is not None: + visualizer.update_rgb_image(rgb_image) + + # Update target/goal visualization + visualizer.update_target_visualization(target_pose) + + # Update main robot visualization from CURRENT joint angles (DataManager uses degrees, Viser expects radians) + if current_joint_angles is not None: + current_joint_rad = np.radians(current_joint_angles) + visualizer.update_robot_pose(current_joint_rad) + visualizer.update_joint_angles_display(current_joint_rad) + + # Update ghost robot to show TARGET joint angles when available (also in radians) + if ( + target_joint_angles is not None + and robot_activity_state == RobotActivityState.ENABLED + ): + target_joint_rad = np.radians(target_joint_angles) + visualizer.update_ghost_robot_visibility(True) + visualizer.update_ghost_robot_pose(target_joint_rad) + else: + # Hide ghost robot when no valid target is available or robot not enabled + visualizer.update_ghost_robot_visibility(False) + + # Update robot status - simple Enabled/Homing/Disabled + if robot_activity_state == RobotActivityState.ENABLED: + visualizer.update_robot_status("Robot Status: Enabled") + elif robot_activity_state == RobotActivityState.HOMING: + visualizer.update_robot_status("Robot Status: Homing") + else: # DISABLED + visualizer.update_robot_status("Robot Status: Disabled") + + # Update gripper status + visualizer.update_gripper_status( + trigger_value, + robot_enabled=(robot_activity_state == RobotActivityState.ENABLED), + ) + + # Sleep to maintain visualization rate + elapsed = time.time() - iteration_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + +except KeyboardInterrupt: + print("\n\n👋 Interrupt received - shutting down gracefully...") +except Exception as e: + print(f"\n❌ Demo error: {e}") + import traceback + + traceback.print_exc() + +# Cleanup (outside try/except so it always runs) +print("\n🧹 Cleaning up...") + +data_manager.request_shutdown() +data_manager.set_robot_activity_state(RobotActivityState.DISABLED) +quest_thread.join() +quest_reader.stop() +ik_thread.join() +joint_state_thread_obj.join() +robot_controller.cleanup() +visualizer.stop() + +print("\n👋 Demo stopped.") + + +============================================================ +FILE: 6_visualize_policy_from_dataset.py +============================================================ + +#!/usr/bin/env python3 +"""Simple policy visualization from dataset. + +Loads a policy and a dataset, and randomly selects a state +from the dataset to run the policy with and visualize the results. +""" + +import argparse +import random +import sys +import time +import traceback +from pathlib import Path + +import neuracore as nc +import numpy as np +import viser +import yourdfpy +from neuracore.core.utils.robot_data_spec_utils import ( + merge_cross_embodiment_description, +) +from neuracore_types import DataType +from PIL import Image +from viser.extras import ViserUrdf + +# Add parent directory to path +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from common.configs import JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH +from common.policy_helpers import ( + DEFAULT_ROBOT_NAME, + convert_predictions_to_horizon, + embodiment_names_ordered, + get_policy_embodiments, + gripper_open_at_index, + horizon_length, + log_sync_step_for_policy, + print_policy_embodiments, + urdf_cfg_from_horizon, +) + +# Parse arguments +parser = argparse.ArgumentParser( + description="Visualize policy predictions from dataset" +) +parser.add_argument("--dataset-name", type=str, required=True, help="Dataset name") +policy_group = parser.add_mutually_exclusive_group(required=True) +policy_group.add_argument( + "--train-run-name", type=str, default=None, help="Training run name" +) +policy_group.add_argument( + "--model-path", type=str, default=None, help="Model file path" +) +policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Name of remote Neuracore policy endpoint to use instead of a local policy.", +) +parser.add_argument( + "--robot-name", + type=str, + default=DEFAULT_ROBOT_NAME, + help="Neuracore robot name (policy embodiment resolution).", +) +parser.add_argument( + "--frequency", + type=int, + default=POLICY_EXECUTION_RATE, + help="Frequency of visualization", +) +args = parser.parse_args() + +# Connect to Neuracore +print("🔧 Initializing Neuracore...") +nc.login() +nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + +if args.remote_endpoint_name: + print(f"🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") + try: + policy = nc.policy_remote_server(args.remote_endpoint_name) + except nc.EndpointError: + print( + f"❌ Endpoint '{args.remote_endpoint_name}' not available. " + "Please start it from the Neuracore dashboard." + ) + sys.exit(1) +elif args.train_run_name: + print(f"🤖 Loading policy from training run: {args.train_run_name}...") + policy = nc.policy( + train_run_name=args.train_run_name, + device="cuda", + robot_name=args.robot_name, + ) +else: + print(f"🤖 Loading policy from model file: {args.model_path}...") + policy = nc.policy( + model_file=args.model_path, + device="cuda", + robot_name=args.robot_name, + ) +print(" ✓ Policy loaded") +input_embodiment_description, output_embodiment_description = get_policy_embodiments( + policy +) +print_policy_embodiments(input_embodiment_description, output_embodiment_description) + +output_gripper_names = None +if output_embodiment_description is not None: + gripper_spec = output_embodiment_description.get( + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS + ) + if gripper_spec is not None: + output_gripper_names = embodiment_names_ordered(gripper_spec) + +# Load and synchronize dataset +print(f"🔍 Loading dataset: {args.dataset_name}...") +dataset = nc.get_dataset(args.dataset_name) +print(f" ✓ Dataset loaded: {len(dataset)} episodes") + +print("🔁 Building cross_embodiment_union for synchronization...") +input_cross_embodiment_description = { + robot_id: input_embodiment_description for robot_id in dataset.robot_ids +} +output_cross_embodiment_description = ( + {robot_id: output_embodiment_description for robot_id in dataset.robot_ids} + if output_embodiment_description is not None + else {} +) +cross_embodiment_union = merge_cross_embodiment_description( + input_cross_embodiment_description, + output_cross_embodiment_description, +) + +print("🔁 Synchronizing dataset...") +synced_dataset = dataset.synchronize( + frequency=args.frequency, + cross_embodiment_union=cross_embodiment_union, + prefetch_videos=True, + max_prefetch_workers=2, +) +print(f" ✓ Dataset synchronized: {len(synced_dataset)} episodes") + +# Setup Viser +print("🖥️ Starting Viser...") +server = viser.ViserServer() +server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) + +# Load URDF +urdf = yourdfpy.URDF.load(str(URDF_PATH)) +urdf_vis = ViserUrdf(server, urdf, root_node_name="/robot") +urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) + +# State variables +current_horizon = None +current_action_idx = 0 +playing = False +rgb_gui_handle = None + + +def select_random_state() -> None: + """Select random state and run policy.""" + global current_horizon, current_action_idx, playing, rgb_gui_handle + + episode_idx = random.randint(0, len(synced_dataset) - 1) + episode = synced_dataset[episode_idx] + if len(episode) == 0: + print(f"⚠️ Episode {episode_idx} is empty") + return + + step_idx = random.randint(0, len(episode) - 1) + step = episode[step_idx] + print(f"📊 Selected episode {episode_idx}, step {step_idx}") + + if not log_sync_step_for_policy(step, input_embodiment_description): + print("⚠️ Step has no data for policy input channels") + return + + if hasattr(step, "data"): + rgb_data = step.data.get(DataType.RGB_IMAGES, {}) + for _camera_name, frame in rgb_data.items(): + rgb_image = np.array(frame.frame) + image_pil = Image.fromarray(rgb_image) + image_pil.save("current_image.png") + print("💾 Saved image to current_image.png") + if rgb_gui_handle is None: + rgb_gui_handle = server.gui.add_image( + rgb_image, + label="RGB (current step)", + format="jpeg", + jpeg_quality=85, + ) + else: + rgb_gui_handle.image = rgb_image + break + + print("🎯 Getting policy prediction...") + start_time = time.time() + try: + predictions = policy.predict(timeout=60) + except nc.EndpointError as e: + print(f"✗ Failed to get policy prediction: {e}") + traceback.print_exc() + return + duration = time.time() - start_time + current_horizon = convert_predictions_to_horizon(predictions) + current_action_idx = 0 + playing = True + print(f"FINISHED PREDICTION in {duration:.3f} s") + + joint_cfg = urdf_cfg_from_horizon(current_horizon or {}, 0) + if joint_cfg is not None: + urdf_vis.update_cfg(joint_cfg) + + print(f"✅ Prediction received: {horizon_length(current_horizon or {})} actions") + + +# Add button +random_button = server.gui.add_button("Random Selection") +random_button.on_click(lambda _: select_random_state()) + +# Add gripper value display +gripper_handle = server.gui.add_slider( + "Gripper Open Amount", + min=0.0, + max=1.0, + step=0.01, + initial_value=0.0, + disabled=True, +) + +# Add frequency control +frequency_handle = server.gui.add_number( + "Visualization Frequency (Hz)", + initial_value=args.frequency, + min=1.0, + max=500.0, + step=1.0, +) + +select_random_state() +try: + while True: + start_time = time.time() + + h_len = horizon_length(current_horizon or {}) + if playing and current_horizon and h_len > 0: + if current_action_idx < h_len: + joint_cfg = urdf_cfg_from_horizon(current_horizon, current_action_idx) + if joint_cfg is not None: + urdf_vis.update_cfg(joint_cfg) + nc.log_joint_positions( + {jn: float(joint_cfg[i]) for i, jn in enumerate(JOINT_NAMES)} + ) + + gripper_value = gripper_open_at_index( + current_horizon, + current_action_idx, + gripper_names=output_gripper_names, + ) + if gripper_value is not None: + gripper_handle.value = round(gripper_value, 2) + + current_action_idx = (current_action_idx + 1) % h_len + + elapsed = time.time() - start_time + frequency = max(frequency_handle.value, 0.1) + time.sleep(max(0, 1.0 / frequency - elapsed)) + +except KeyboardInterrupt: + print("\n👋 Shutting down...") +finally: + policy.disconnect() + nc.logout() + + +============================================================ +FILE: 5_rollout_neuracore_policy_minimal.py +============================================================ + +#!/usr/bin/env python3 +"""Minimal Piper Robot Policy Test - Terminal only, no GUI. + +Simple script that: +1. Enables robot +2. Sends robot home +3. Runs policy in continuous loop (get image, run policy, execute horizon, repeat) +4. On cancellation: sends robot home and exits +""" + +import argparse +import sys +import threading +import time +import traceback +from pathlib import Path + +import neuracore as nc + +# Add parent directory to path +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from common.configs import ( + NEUTRAL_JOINT_ANGLES, + POLICY_EXECUTION_RATE, + PREDICTION_HORIZON_EXECUTION_RATIO, + ROBOT_RATE, + URDF_PATH, +) +from common.data_manager import DataManager, RobotActivityState +from common.policy_helpers import ( + convert_predictions_to_horizon, + embodiment_names_ordered, + get_policy_embodiments, + gripper_open_at_index, + joint_targets_deg_at_index, + log_robot_state_for_policy, + print_policy_embodiments, +) +from common.policy_state import PolicyState +from common.threads.joint_state import joint_state_thread +from common.threads.realsense_camera import camera_thread +from neuracore_types import DataType, EmbodimentDescription + +from piper_controller import PiperController + + +def run_policy( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + input_embodiment_description: EmbodimentDescription, +) -> bool: + """Run policy and get prediction horizon.""" + if not log_robot_state_for_policy(data_manager, input_embodiment_description): + print("⚠️ No policy input data available") + return False + + # Get policy prediction + try: + start_time = time.time() + predictions = policy.predict(timeout=5) + prediction_horizon = convert_predictions_to_horizon(predictions) + elapsed = time.time() - start_time + + # Get horizon length from the first joint (all should have same length) + horizon_length = policy_state.get_prediction_horizon_length() + print(f"✓ Got {horizon_length} actions in {elapsed:.3f}s") + + policy_state.set_prediction_horizon(prediction_horizon) + return True + + except Exception as e: + print(f"✗ Policy prediction failed: {e}") + traceback.print_exc() + return False + + +def execute_horizon( + data_manager: DataManager, + policy_state: PolicyState, + robot_controller: PiperController, + frequency: int, + input_embodiment_description: EmbodimentDescription, + output_gripper_names: list[str] | None, +) -> None: + """Execute prediction horizon.""" + policy_state.start_policy_execution() + data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) + + locked_horizon = policy_state.get_locked_prediction_horizon() + horizon_length = policy_state.get_locked_prediction_horizon_length() + dt = 1.0 / frequency + + for i in range(horizon_length): + start_time = time.time() + + joint_targets_deg = joint_targets_deg_at_index(locked_horizon, i) + if joint_targets_deg is not None: + robot_controller.set_target_joint_angles(joint_targets_deg) + + gripper_target = gripper_open_at_index( + locked_horizon, i, gripper_names=output_gripper_names + ) + if gripper_target is not None: + robot_controller.set_gripper_open_value(gripper_target) + + log_robot_state_for_policy(data_manager, input_embodiment_description) + + # Sleep to maintain rate + elapsed = time.time() - start_time + time.sleep(max(0, dt - elapsed)) + + # End execution + policy_state.end_policy_execution() + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Minimal Piper Policy Test") + policy_group = parser.add_mutually_exclusive_group(required=True) + policy_group.add_argument( + "--train-run-name", + type=str, + default=None, + help="Name of the training run to load policy from (for cloud training).", + ) + policy_group.add_argument( + "--model-path", + type=str, + default=None, + help="Path to local model file to load policy from.", + ) + policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Name of remote Neuracore policy endpoint.", + ) + parser.add_argument( + "--robot-name", + type=str, + default="AgileX PiPER", + help="Neuracore robot name (policy embodiment resolution).", + ) + parser.add_argument( + "--frequency", + type=int, + default=POLICY_EXECUTION_RATE, + help="Frequency of policy execution", + ) + parser.add_argument( + "--execution-ratio", + type=float, + default=PREDICTION_HORIZON_EXECUTION_RATIO, + help="Execution ratio of the policy", + ) + args = parser.parse_args() + + print("=" * 60) + print("PIPER POLICY ROLLOUT") + print("=" * 60) + + # Initialize Neuracore + print("\n🔧 Initializing Neuracore...") + nc.login() + nc.connect_robot( + robot_name=args.robot_name, + urdf_path=str(URDF_PATH), + overwrite=False, + ) + + if args.remote_endpoint_name is not None: + print( + f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." + ) + try: + policy = nc.policy_remote_server(args.remote_endpoint_name) + except nc.EndpointError: + print( + f"❌ Endpoint '{args.remote_endpoint_name}' not available. " + "Please start it from the Neuracore dashboard." + ) + sys.exit(1) + elif args.train_run_name is not None: + print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") + policy = nc.policy( + train_run_name=args.train_run_name, + device="cuda", + robot_name=args.robot_name, + ) + else: + print(f"\n🤖 Loading policy from model file: {args.model_path}...") + policy = nc.policy( + model_file=args.model_path, + device="cuda", + robot_name=args.robot_name, + ) + print(" ✓ Policy loaded successfully") + input_embodiment_description, output_embodiment_description = ( + get_policy_embodiments(policy) + ) + print_policy_embodiments( + input_embodiment_description, output_embodiment_description + ) + output_gripper_names = None + if output_embodiment_description is not None: + gripper_spec = output_embodiment_description.get( + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS + ) + if gripper_spec is not None: + output_gripper_names = embodiment_names_ordered(gripper_spec) + + # Initialize state + data_manager = DataManager() + policy_state = PolicyState() + policy_state.set_execution_ratio(args.execution_ratio) + + # Initialize robot controller + print("\n🤖 Initializing robot controller...") + robot_controller = PiperController( + can_interface="can0", + robot_rate=ROBOT_RATE, + control_mode=PiperController.ControlMode.JOINT_SPACE, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + debug_mode=False, + ) + robot_controller.start_control_loop() + + # Start joint state thread + print("\n📊 Starting joint state thread...") + joint_state_thread_obj = threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True + ) + joint_state_thread_obj.start() + + # Start camera thread + print("\n📷 Starting camera thread...") + camera_thread_obj = threading.Thread( + target=camera_thread, args=(data_manager,), daemon=True + ) + camera_thread_obj.start() + + # Wait for threads to initialize + print("\n⏳ Waiting for initialization...") + time.sleep(2.0) + + try: + # Enable robot + print("\n🟢 Enabling robot...") + robot_controller.resume_robot() + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ Robot enabled") + + # Home robot + print("\n🏠 Moving to home position...") + robot_controller.move_to_home() + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + + # Wait for homing to complete + start_time = time.time() + while ( + data_manager.get_robot_activity_state() == RobotActivityState.HOMING + and not robot_controller.is_robot_homed() + and time.time() - start_time < 5.0 + ): + time.sleep(0.1) + print("✓ Robot homed") + + # Policy execution loop + print("\n🚀 Starting policy execution loop...") + print("Press Ctrl+C to stop\n") + + while True: + # Run policy + if not run_policy( + data_manager, policy, policy_state, input_embodiment_description + ): + print("⚠️ Policy run failed, retrying...") + time.sleep(0.5) + continue + + # Execute horizon + execute_horizon( + data_manager, + policy_state, + robot_controller, + args.frequency, + input_embodiment_description, + output_gripper_names, + ) + + except KeyboardInterrupt: + print("\n\n👋 Interrupt received - shutting down...") + + except Exception as e: + print(f"\n❌ Error: {e}") + traceback.print_exc() + + finally: + # Cleanup + print("\n🧹 Cleaning up...") + + # Home robot + print("\n🏠 Moving to home position...") + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + robot_controller.move_to_home() + + # Wait for homing to complete + start_time = time.time() + while ( + data_manager.get_robot_activity_state() == RobotActivityState.HOMING + and not robot_controller.is_robot_homed() + and time.time() - start_time < 5.0 + ): + time.sleep(0.1) + print("✓ Robot homed") + + # Shutdown + policy.disconnect() + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + data_manager.request_shutdown() + joint_state_thread_obj.join() + camera_thread_obj.join() + time.sleep(0.5) # Give threads time to stop + + robot_controller.cleanup() + nc.logout() + + print("✓ Cleanup complete") + print("\n👋 Done.") + + +============================================================ +FILE: 3_replay_neuracore_episodes.py +============================================================ + +"""Replay a recorded Neuracore dataset on the Piper robot.""" + +import argparse +import sys +import time +from pathlib import Path +from typing import cast + +import cv2 +import neuracore as nc +import numpy as np +from common.configs import ( + GRIPPER_NAME, + JOINT_NAMES, + NEUTRAL_JOINT_ANGLES, + ROBOT_RATE, +) +from neuracore.core.utils.robot_data_spec_utils import ( + merge_cross_embodiment_description, +) +from neuracore_types import ( + CrossEmbodimentDescription, + DataType, + SynchronizedPoint, +) +from tqdm import tqdm + +# Add parent directory to path to piper_controller +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from piper_controller import PiperController + + +def main() -> None: + """Main function for replaying a Neuracore dataset on the Piper robot.""" + parser = argparse.ArgumentParser() + parser.add_argument("--dataset-name", type=str, required=True) + parser.add_argument("--frequency", type=int, required=True) + parser.add_argument("--episode-index", type=int, required=False, default=0) + args = parser.parse_args() + + # Initialize robot controller + print("\n🤖 Initializing Piper robot controller...") + robot_controller = PiperController( + can_interface="can0", + robot_rate=ROBOT_RATE, + control_mode=PiperController.ControlMode.JOINT_SPACE, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + debug_mode=False, + ) + # Start robot control loop + print("\n🚀 Starting robot control loop...") + robot_controller.start_control_loop() + + # Login to Neuracore + print("\n🔑 Logging in to Neuracore...") + nc.login() + + # Get dataset from Neuracore + print("\n🔍 Getting dataset from Neuracore...") + dataset = nc.get_dataset(args.dataset_name) + + # Cross-embodiment sync (same pattern as examples/6_visualize_policy_from_dataset.py) + print("\n🔁 Building cross_embodiment_union for synchronization...") + input_modalities: list[DataType] = [ + DataType.JOINT_POSITIONS, + DataType.RGB_IMAGES, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, + ] + output_modalities: list[DataType] = [ + DataType.JOINT_TARGET_POSITIONS, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, + ] + input_cross_embodiment_description: CrossEmbodimentDescription = {} + output_cross_embodiment_description: CrossEmbodimentDescription = {} + for robot_id in dataset.robot_ids: + full = dataset.get_full_embodiment_description(robot_id) + input_cross_embodiment_description[robot_id] = { + dt: full[dt] for dt in input_modalities if dt in full + } + output_cross_embodiment_description[robot_id] = { + dt: full[dt] for dt in output_modalities if dt in full + } + cross_embodiment_union = merge_cross_embodiment_description( + input_cross_embodiment_description, + output_cross_embodiment_description, + ) + + # Synchronize dataset + print("\n🔁 Synchronizing dataset...") + synced_dataset = dataset.synchronize( + frequency=args.frequency, + cross_embodiment_union=cross_embodiment_union, + ) + + # Determine which episodes to play + episode_indices: list[int] = [] + if args.episode_index == -1: + episode_indices = list(range(len(synced_dataset))) + print(f"\n📊 Found {len(synced_dataset)} episodes. Will play all episodes.") + else: + episode_indices = [args.episode_index] + print(f"\n📊 Playing episode {args.episode_index} only.") + + # Play episodes + try: + for episode_idx in episode_indices: + + robot_controller.move_to_home() + seconds_to_wait = 10 + while not robot_controller.is_robot_homed(): + time.sleep(1) + seconds_to_wait -= 1 + if seconds_to_wait <= 0: + break + print( + f"🔍 Waiting for robot to reach home position... {seconds_to_wait} seconds remaining." + ) + if robot_controller.is_robot_homed(): + print("✓ Robot is at home position.") + else: + print("❌ Robot did not reach home position within 10 seconds.") + print( + f"🔍 Current joint angles: {robot_controller.get_current_joint_angles()}" + ) + print(f"🔍 Home joint angles: {robot_controller.HOME_JOINT_ANGLES}") + + print(f"\n{'='*60}") + print(f"🎬 Playing Episode {episode_idx} / {len(synced_dataset) - 1}") + print(f"{'='*60}") + + episode = synced_dataset[episode_idx] + + print(f"\n🚀 Collecting episode {episode_idx} data...") + rgb_frames_per_step: list[dict[str, np.ndarray]] = [] + parallel_gripper_open_amounts = [] + joint_positions = [] + for step in tqdm(episode, desc=f"Collecting episode {episode_idx}"): + step = cast(SynchronizedPoint, step) + + # Extract joint positions + joint_positions_dict = {} + if DataType.JOINT_TARGET_POSITIONS in step.data: + joint_data = step.data[DataType.JOINT_TARGET_POSITIONS] + for joint_name in JOINT_NAMES: + if joint_name in joint_data: + joint_positions_dict[joint_name] = joint_data[ + joint_name + ].value + joint_positions.append([joint_positions_dict[jn] for jn in JOINT_NAMES]) + + # Extract gripper + gripper_value = 0.0 + if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in step.data: + gripper_data = step.data[ + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS + ] + if GRIPPER_NAME in gripper_data: + gripper_value = gripper_data[GRIPPER_NAME].open_amount + parallel_gripper_open_amounts.append(gripper_value) + + # Extract RGB for all cameras + step_frames: dict[str, np.ndarray] = {} + if DataType.RGB_IMAGES in step.data: + rgb_data = step.data[DataType.RGB_IMAGES] + for camera_name, img_value in rgb_data.items(): + step_frames[camera_name] = img_value.frame + rgb_frames_per_step.append(step_frames) + + joint_positions = np.degrees(np.array(joint_positions)) + parallel_gripper_open_amounts = np.array(parallel_gripper_open_amounts) + + print(f"\n🚀 Replaying episode {episode_idx} data...") + for index in tqdm( + range(len(joint_positions)), desc=f"Replaying episode {episode_idx}" + ): + start_time = time.time() + robot_controller.set_target_joint_angles(joint_positions[index]) + robot_controller.set_gripper_open_value( + parallel_gripper_open_amounts[index] + ) + + # Display camera frames (dataset stores RGB; OpenCV expects BGR) + if index < len(rgb_frames_per_step): + for camera_name, frame_rgb in rgb_frames_per_step[index].items(): + arr = np.asarray(frame_rgb, dtype=np.uint8) + frame_bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) + cv2.imshow(f"Replay: {camera_name}", frame_bgr) + if cv2.waitKey(1) & 0xFF == ord("q"): + print("\n🛑 'q' pressed, stopping replay...") + break + + end_time = time.time() + time.sleep(max(0, 1 / args.frequency - (end_time - start_time))) + cv2.destroyAllWindows() + print(f"🎉 Episode {episode_idx} replay completed.") + + if args.episode_index == -1: + print(f"\n{'='*60}") + print(f"🎉 All {len(synced_dataset)} episodes replay completed!") + print(f"{'='*60}") + except KeyboardInterrupt: + print("\n🛑 Keyboard interrupt detected, stopping robot control loop...") + cv2.destroyAllWindows() + + robot_controller.stop_control_loop() + robot_controller.cleanup() + + +if __name__ == "__main__": + main() + + +============================================================ +FILE: combine_code.py +============================================================ + +import pathlib + +def combine_python_files(directory_path, output_filename): + """ + Recursively finds all .py files in a directory and combines them into a single text file. + + Args: + directory_path (str): The path to the root directory to search. + output_filename (str): The name/path of the output text file. + """ + # Create a Path object for the target directory + source_dir = pathlib.Path(directory_path) + + # Ensure the directory exists + if not source_dir.is_dir(): + print(f"Error: The directory '{directory_path}' does not exist.") + return + + # Open the output file in write mode + with open(output_filename, 'w', encoding='utf-8') as outfile: + # rglob('*.py') recursively searches for all files ending in .py + py_files = list(source_dir.rglob('*.py')) + + if not py_files: + print(f"No Python files found in '{directory_path}'.") + return + + print(f"Found {len(py_files)} Python files. Combining...") + + for file_path in py_files: + # Write a clear separator and the file path as a header + outfile.write(f"\n{'='*60}\n") + outfile.write(f"FILE: {file_path}\n") + outfile.write(f"{'='*60}\n\n") + + # Read the python file and append its contents + try: + with open(file_path, 'r', encoding='utf-8') as infile: + outfile.write(infile.read()) + outfile.write("\n") + except Exception as e: + error_msg = f"Error reading file {file_path}: {e}\n" + print(error_msg) + outfile.write(error_msg) + + print(f"Success! All files have been combined into '{output_filename}'.") + +# --- Example Usage --- +if __name__ == "__main__": + # Replace these variables with your actual paths + TARGET_DIRECTORY = "./" # "./" means current directory + OUTPUT_FILE = "combined_code.txt" + + combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) + +============================================================ +FILE: 2_collect_teleop_data_with_neuracore.py +============================================================ + +#!/usr/bin/env python3 +"""Piper Robot Teleoperation with Meta Quest Controller and Neuracore data collection. + +This demo uses Pink IK control with Meta Quest controller input to control the Piper robot and +logs data to Neuracore. +""" + + +import argparse +import multiprocessing +import subprocess +import sys +import threading +import time +import traceback +from pathlib import Path +from typing import Any + +import neuracore as nc +import numpy as np + +# Add parent directory to path to import pink_ik_solver and piper_controller +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from common.configs import ( + CAMERA_FRAME_STREAMING_RATE, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + CONTROLLER_DATA_RATE, + CONTROLLER_MIN_CUTOFF, + DAMPING_COST, + FRAME_TASK_GAIN, + GRIPPER_FRAME_NAME, + IK_SOLVER_RATE, + JOINT_STATE_STREAMING_RATE, + LM_DAMPING, + META_QUEST_AXIS_MASK, + NEUTRAL_END_EFFECTOR_POSE, + NEUTRAL_JOINT_ANGLES, + ORIENTATION_COST, + POSITION_COST, + POSTURE_COST_VECTOR, + ROBOT_RATE, + ROTATION_SCALE, + SLOW_ROTATION_SCALE, + SLOW_TRANSLATION_SCALE, + SOLVER_DAMPING_VALUE, + SOLVER_NAME, + TRANSLATION_SCALE, + URDF_PATH, + WRIST_JOINT_BUTTON_STEP_DEGREES, +) +from common.data_manager import DataManager, RobotActivityState +from common.threads.ik_solver import ik_solver_thread +from common.threads.joint_state import joint_state_thread +from common.threads.quest_reader import quest_reader_thread +from common.threads.realsense_camera import camera_thread as realsense_camera_thread +from meta_quest_teleop.reader import MetaQuestReader + +from pink_ik_solver import PinkIKSolver +from piper_controller import PiperController + + +def play_audio_feedback(action: str) -> None: + """Play distinct synthesized tones asynchronously using SoX.""" + # Start = High pitch (880 Hz), Stop = Low pitch (440 Hz) + freq = "880" if action == "start" else "440" + try: + # -q: quiet, -n: no input file, synth: generate audio, 0.3: duration, sine: wave type + subprocess.Popen( + ["play", "-q", "-n", "synth", "0.3", "sine", freq], + stderr=subprocess.DEVNULL + ) + except Exception as e: + print(f"⚠️ Failed to play tone (is SoX installed?): {e}") + +def log_to_neuracore_on_change_callback( + name: str, payload: dict[str, Any], timestamp: float +) -> None: + """Log data to queue on change callback.""" + # Call appropriate Neuracore logging function + try: + #print(f"\n📤 Logging {name} to Neuracore with timestamp {timestamp:.3f}...") + if name == "log_joint_positions": + nc.log_joint_positions(payload, timestamp=timestamp) + elif name == "log_joint_torques": + nc.log_joint_torques(payload, timestamp=timestamp) + elif name == "log_joint_target_positions": + nc.log_joint_target_positions(payload, timestamp=timestamp) + elif name == "log_parallel_gripper_open_amounts": + nc.log_parallel_gripper_open_amounts(payload, timestamp=timestamp) + elif name == "log_parallel_gripper_target_open_amounts": + nc.log_parallel_gripper_target_open_amounts(payload, timestamp=timestamp) + elif name == "log_rgb": + camera_name = next(iter(payload)) + image_array = payload[camera_name] + nc.log_rgb(camera_name, image_array, timestamp=timestamp) + else: + print(f"\n⚠️ Unknown logging function: {name}") + except Exception as e: + print(f"\n⚠️ Failed to call {name} to Neuracore. Exception: {e}") + print("Traceback:") + traceback.print_exc() + + +def on_button_a_pressed() -> None: + """Handle Button A press to toggle robot enable/disable state.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + # Disable robot + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + robot_controller.graceful_stop() + # Reset teleop state when disabling robot + data_manager.set_teleop_state(False, None, None) + print("✓ 🔴 Robot disabled (Button A)") + elif robot_activity_state == RobotActivityState.DISABLED: + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ 🟢 Robot enabled (Button A)") + else: + print("✗ Failed to enable robot") + + +def on_button_b_pressed() -> None: + """Handle Button B press to move robot to home position.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + print("🏠 Button B pressed - Moving to home position...") + # Set state to HOMING to prevent IK thread from sending robot commands + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + # Disable teleop during homing + data_manager.set_teleop_state(False, None, None) + ok = robot_controller.move_to_home() + if not ok: + print("✗ Failed to initiate home move") + # Revert to ENABLED on failure + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("⚠️ Button B pressed but robot is not enabled") + + +def on_button_rj_pressed() -> None: + """Handle Button Right Joystick press to toggle data recording state.""" + if not nc.is_recording(): + # Start recording + try: + nc.start_recording() + print("✓ 🔴 Recording started (Button RJ)") + play_audio_feedback("start") # <-- Trigger distinct START sound + except Exception as e: + print(f"✗ Failed to start recording. Exception: {e}") + print("Traceback:") + traceback.print_exc() + else: + # Stop recording + try: + nc.stop_recording() + print("✓ ⏹️ Recording stopped (Button RJ)") + play_audio_feedback("stop") # <-- Trigger distinct STOP sound + except Exception as e: + print(f"✗ Failed to stop recording. Exception: {e}") + print("Traceback:") + traceback.print_exc() + + +def _step_wrist_joint(delta_degrees: float) -> None: + """Apply a relative step to the wrist joint target angle.""" + # Prevent IK teleop loop from overwriting this manual joint adjustment. + data_manager.set_teleop_state(False, None, None) + robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) + + target_joint_angles = robot_controller.get_target_joint_angles() + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + base_wrist_joint_angle = float(current_joint_angles[-1]) + else: + base_wrist_joint_angle = float(target_joint_angles[-1]) + + target_joint_angles[-1] = base_wrist_joint_angle + delta_degrees + robot_controller.set_target_joint_angles(target_joint_angles) + data_manager.set_target_joint_angles(target_joint_angles) + + +def on_button_y_pressed() -> None: + """Handle Button Y press to add +5° on the wrist joint.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state != RobotActivityState.ENABLED: + print("⚠️ Button Y pressed but robot is not enabled") + return + _step_wrist_joint(WRIST_JOINT_BUTTON_STEP_DEGREES) + + +def on_button_x_pressed() -> None: + """Handle Button X press to subtract -5° on the wrist joint.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state != RobotActivityState.ENABLED: + print("⚠️ Button X pressed but robot is not enabled") + return + _step_wrist_joint(-WRIST_JOINT_BUTTON_STEP_DEGREES) + + +def on_button_lj_pressed() -> None: + """Toggle slow translation/rotation scaling mode from config values.""" + slow_scaling_mode_enabled = data_manager.toggle_slow_scaling_mode_enabled() + if slow_scaling_mode_enabled: + data_manager.set_teleop_scaling(SLOW_TRANSLATION_SCALE, SLOW_ROTATION_SCALE) + print( + "🐢 Button LJ pressed - Slow scaling enabled " + f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " + f"rotation={SLOW_ROTATION_SCALE:.3f})" + ) + else: + data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) + print( + "🐇 Button LJ pressed - Slow scaling disabled " + f"(translation={TRANSLATION_SCALE:.3f}, " + f"rotation={ROTATION_SCALE:.3f})" + ) + + +if __name__ == "__main__": + multiprocessing.set_start_method("spawn") + + parser = argparse.ArgumentParser( + description="Piper Robot Teleoperation with Neuracore Data Collection - REAL ROBOT CONTROL" + ) + parser.add_argument( + "--ip-address", + type=str, + default=None, + help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", + ) + parser.add_argument( + "--dataset-name", + type=str, + default=None, + help="Name for the dataset (optional, defaults to auto-generated timestamp-based name)", + ) + args = parser.parse_args() + + print("=" * 60) + print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL") + print("=" * 60) + print("Thread frequencies:") + print(f" 🎮 Quest Controller: {CONTROLLER_DATA_RATE} Hz") + print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") + print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") + print(f" 📸 Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz") + print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") + + # Connect to Neuracore + print("\n🔧 Initializing Neuracore...") + nc.login() + nc.connect_robot( + robot_name="AgileX PiPER", + urdf_path=str(URDF_PATH), + overwrite=False, + ) + + # Create dataset + dataset_name = ( + args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" + ) + print(f"\n🔧 Creating dataset {dataset_name}...") + nc.create_dataset( + name=dataset_name, + description="Teleop data collection for Piper robot", + ) + + # Initialize shared state + data_manager = DataManager() + data_manager.set_on_change_callback(log_to_neuracore_on_change_callback) + data_manager.set_controller_filter_params( + CONTROLLER_MIN_CUTOFF, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + ) + data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) + + # Initialize robot controller + print("\n🤖 Initializing Piper robot controller...") + robot_controller = PiperController( + can_interface="can0", + robot_rate=ROBOT_RATE, + control_mode=PiperController.ControlMode.JOINT_SPACE, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, + enable_joint_angle_limits=False, + debug_mode=False, + ) + + # Start robot control loop + print("\n🚀 Starting robot control loop...") + robot_controller.start_control_loop() + + # Start joint state thread + print("\n📊 Starting joint state thread...") + joint_state_thread_obj = threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True + ) + joint_state_thread_obj.start() + + # Initialize Meta Quest reader + print("\n🎮 Initializing Meta Quest reader...") + quest_reader = MetaQuestReader( + ip_address=args.ip_address, + port=5555, + run=True, + axis_mask=META_QUEST_AXIS_MASK, + ) + + # Register button callbacks (after state and robot_controller are initialized) + quest_reader.on("button_a_pressed", on_button_a_pressed) + quest_reader.on("button_b_pressed", on_button_b_pressed) + quest_reader.on("button_rj_pressed", on_button_rj_pressed) + quest_reader.on("button_y_pressed", on_button_y_pressed) + quest_reader.on("button_x_pressed", on_button_x_pressed) + quest_reader.on("button_lj_pressed", on_button_lj_pressed) + + # Start data collection thread + print("\n🎮 Starting quest reader thread...") + quest_thread = threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True + ) + quest_thread.start() + + # set initial configuration to current joint angles + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + initial_joint_angles = np.radians(current_joint_angles) + else: + initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) + + # Create Pink IK solver + print("\n🔧 Creating Pink IK solver...") + ik_solver = PinkIKSolver( + urdf_path=URDF_PATH, + end_effector_frame=GRIPPER_FRAME_NAME, + solver_name=SOLVER_NAME, + position_cost=POSITION_COST, + orientation_cost=ORIENTATION_COST, + frame_task_gain=FRAME_TASK_GAIN, + lm_damping=LM_DAMPING, + damping_cost=DAMPING_COST, + solver_damping_value=SOLVER_DAMPING_VALUE, + integration_time_step=1 / IK_SOLVER_RATE, + initial_configuration=initial_joint_angles, + posture_cost_vector=np.array(POSTURE_COST_VECTOR), + ) + + # Start IK solver thread + print("\n🧮 Starting IK solver thread...") + ik_thread = threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True + ) + ik_thread.start() + + # Start cameras threads + print("\n📷 Starting cameras threads...") + realsense_camera_thread_obj = threading.Thread( + target=realsense_camera_thread, args=(data_manager,), daemon=True + ) + realsense_camera_thread_obj.start() + + # usb_camera_thread_obj = threading.Thread( + # target=usb_camera_thread, args=(data_manager,), daemon=True + # ) + # usb_camera_thread_obj.start() + + print() + print("🚀 Starting teleoperation with REAL ROBOT CONTROL...") + print("🎮 CONTROLS:") + print(" 1. Press BUTTON A to enable/disable robot") + print(" 2. Hold RIGHT GRIP to activate teleoperation") + print(" 3. Move controller - robot follows!") + print(" 4. Hold RIGHT TRIGGER to close gripper") + print(" 5. Press BUTTON B to send robot home") + print(" 6. Press RIGHT JOYSTICK to start/stop data recording") + print(" 7. Press BUTTON Y to add +5° on the wrist joint") + print(" 8. Press BUTTON X to subtract 5° on the wrist joint") + print(" 9. Press LEFT JOYSTICK BUTTON to toggle slow scaling mode") + print(" 10. Release grip to stop") + print("⚠️ Press Ctrl+C to exit") + print() + + try: + while not data_manager.is_shutdown_requested(): + time.sleep(1) + except KeyboardInterrupt: + print("\n👋 Interrupt received - shutting down gracefully...") + data_manager.request_shutdown() + except Exception as e: + print(f"\n❌ Demo error. Exception: {e}") + print("Traceback:") + traceback.print_exc() + data_manager.request_shutdown() + else: + print("\n⚠️ Worker requested shutdown - cleaning up...") + # Cleanup + print("\n🧹 Cleaning up...") + + # Cancel recording if active + if nc.is_recording(): + try: + print("⚠️ Cancelling active recording...") + nc.cancel_recording() + print("✓ Recording cancelled") + except Exception as e: + print(f"⚠️ Error cancelling recording. Exception: {e}") + print("Traceback:") + traceback.print_exc() + + # shutdown threads and data producers before logging out + data_manager.request_shutdown() + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + quest_reader.stop() + quest_thread.join() + joint_state_thread_obj.join() + ik_thread.join() + realsense_camera_thread_obj.join() + # usb_camera_thread_obj.join() + nc.logout() + robot_controller.cleanup() + + print("\n👋 Demo stopped.") + + +============================================================ +FILE: 7_teleop_with_pedal.py +============================================================ + +#!/usr/bin/env python3 +"""Piper Robot Teleoperation with Meta Quest and Foot Pedal control. + +This script combines Meta Quest controller input for movement with Foot Pedal +control for session management (Activate, Home, Record). +""" + +import argparse +import multiprocessing +import sys +import threading +import time +from pathlib import Path +from typing import Any + +import neuracore as nc +import numpy as np + +# Add parent directory to path to import pink_ik_solver and piper_controller +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from common.configs import ( + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + CONTROLLER_MIN_CUTOFF, + GRIPPER_FRAME_NAME, + GRIPPER_LOGGING_NAME, + JOINT_NAMES, + NEUTRAL_JOINT_ANGLES, + POSTURE_COST_VECTOR, + ROBOT_RATE, + URDF_PATH, +) +from common.data_manager import DataManager, RobotActivityState +from common.foot_pedal import FootPedal +from common.threads.camera import camera_thread +from common.threads.ik_solver import ik_solver_thread +from common.threads.joint_state import joint_state_thread +from common.threads.quest_reader import quest_reader_thread +from meta_quest_teleop.reader import MetaQuestReader + +from pink_ik_solver import PinkIKSolver +from piper_controller import PiperController + +ENABLE_DISABLE_PEDAL = "a" +HOME_POSITION_PEDAL = "b" +RECORD_TOGGLE_PEDAL = "c" + + +def log_to_neuracore_on_change_callback( + name: str, value: Any, timestamp: float +) -> None: + """Callback triggered on state changes to log data to Neuracore. + + Args: + name: Name of the data stream. + value: Data value (float, array, or image). + timestamp: Time of the change. + """ + try: + if name == "log_joint_positions": + data_dict = {jn: np.radians(a) for jn, a in zip(JOINT_NAMES, value)} + nc.log_joint_positions(data_dict, timestamp=timestamp) + elif name == "log_joint_target_positions": + data_dict = {jn: np.radians(a) for jn, a in zip(JOINT_NAMES, value)} + nc.log_joint_target_positions(data_dict, timestamp=timestamp) + elif name == "log_parallel_gripper_open_amounts": + nc.log_parallel_gripper_open_amounts( + {GRIPPER_LOGGING_NAME: value}, timestamp=timestamp + ) + elif name == "log_parallel_gripper_target_open_amounts": + nc.log_parallel_gripper_target_open_amounts( + {GRIPPER_LOGGING_NAME: value}, timestamp=timestamp + ) + elif name == "log_rgb": + nc.log_rgb("rgb", value, timestamp=timestamp) + except Exception as e: + print(f"⚠️ Logging failed: {e}") + + +def toggle_robot_state() -> None: + """Toggle the robot's activity state between ENABLED and DISABLED.""" + print("🔘 Pedal toggled - Robot State") + state = data_manager.get_robot_activity_state() + + if state == RobotActivityState.ENABLED: + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + if robot_controller: + robot_controller.graceful_stop() + data_manager.set_teleop_state(False, None, None) + print("✓ 🔴 Robot disabled (Pedal)") + elif state == RobotActivityState.DISABLED or state == RobotActivityState.HOMING: + # If no robot, just toggle the state for dashboard visibility + if not robot_controller: + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ 🟢 Robot enabled (Pedal - Headless)") + return + + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ 🟢 Robot enabled (Pedal)") + else: + print("✗ [ACTION] Failed to enable robot") + + +def move_robot_home() -> None: + """Command the robot to move to its neutral/home position.""" + print("🏠 Pedal toggled - Move Home") + state = data_manager.get_robot_activity_state() + + if state == RobotActivityState.ENABLED: + print("🏠 Pedal pressed - Moving to home position...") + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + data_manager.set_teleop_state(False, None, None) + + if robot_controller: + if not robot_controller.move_to_home(): + print("✗ Failed to initiate home move") + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + # Headless simulation of homing + time.sleep(1) + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ 🏠 Robot homed (Headless)") + else: + print("⚠️ Pedal pressed but robot is not enabled") + + +def toggle_recording() -> None: + """Start or stop a data recording session in Neuracore.""" + print("⏺️ Pedal toggled - Recording") + if not nc.is_recording(): + try: + nc.start_recording() + print("✓ 🔴 Recording started (Pedal)") + except Exception as e: + print(f"✗ Failed to start recording: {e}") + else: + try: + nc.stop_recording() + print("✓ ⏹️ Recording stopped (Pedal)") + except Exception as e: + print(f"✗ Failed to stop recording: {e}") + + +if __name__ == "__main__": + multiprocessing.set_start_method("spawn") + + parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleoperation") + parser.add_argument("--ip-address", type=str, help="Meta Quest IP") + parser.add_argument("--dataset-name", type=str, help="Neuracore dataset name") + args = parser.parse_args() + + print("=" * 60) + print("PIPER TELEOP: META QUEST + FOOT PEDALS") + print("=" * 60) + + # Neuracore Init + print("\n🔧 Initializing Neuracore...") + try: + nc.login() + nc.connect_robot( + robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False + ) + ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%H-%M-%S')}" + nc.create_dataset(name=ds_name, description="Quest + Pedal unified collection") + except Exception as e: + print(f"⚠️ Neuracore initialization skipped/failed: {e}") + + # Shared State + data_manager = DataManager() + data_manager.set_on_change_callback(log_to_neuracore_on_change_callback) + data_manager.set_controller_filter_params( + CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF + ) + + # Robot Initialization + print("\n🤖 Initializing Piper...") + robot_controller = None + try: + robot_controller = PiperController(can_interface="can0", robot_rate=ROBOT_RATE) + robot_controller.start_control_loop() + except Exception as e: + print(f"⚠️ Robot controller initialization skipped/failed: {e}") + + # Threads + print("\n📊 Starting Threads (JointState, QuestReader, IKSolver, Camera)...") + pedal_thread = None + if robot_controller: + threading.Thread( + target=joint_state_thread, + args=(data_manager, robot_controller), + daemon=True, + ).start() + + quest_reader = None + try: + print("🔍 Searching for Meta Quest...") + # Adb initialization in the reader might call sys.exit(1), so we catch BaseException + quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) + threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True + ).start() + except (Exception, BaseException) as e: + print(f"⚠️ Quest reader initialization skipped/failed: {e}") + + # Sync IK solver to current position + try: + initial_joints = np.radians( + data_manager.get_current_joint_angles() or NEUTRAL_JOINT_ANGLES + ) + ik_solver = PinkIKSolver( + urdf_path=URDF_PATH, + end_effector_frame=GRIPPER_FRAME_NAME, + initial_configuration=initial_joints, + posture_cost_vector=np.array(POSTURE_COST_VECTOR), + ) + threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True + ).start() + except Exception as e: + print(f"⚠️ IK Solver initialization skipped/failed: {e}") + + try: + threading.Thread( + target=camera_thread, args=(data_manager,), daemon=True + ).start() + except Exception as e: + print(f"⚠️ Camera thread failed to start: {e}") + + # Foot Pedal – started as a daemon thread, callbacks wired inline + print("\n⌨️ Initializing Foot Pedals...") + pedal = FootPedal( + key_map={ + "button_a": ENABLE_DISABLE_PEDAL, + "button_b": HOME_POSITION_PEDAL, + "button_c": RECORD_TOGGLE_PEDAL, + }, + ) + pedal.bind("button_a", toggle_robot_state) + pedal.bind("button_b", move_robot_home) + pedal.bind("button_c", toggle_recording) + + pedal_thread = threading.Thread( + target=pedal.run_loop, args=(data_manager,), daemon=True + ) + pedal_thread.start() + + print("\n✅ SYSTEM ONLINE") + print("------------------------------------------------------------") + print("🎮 QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") + print("⌨️ PEDAL CONTROLS: ENABLE/DISABLE (A), HOME (B), RECORD (C)") + print("------------------------------------------------------------") + + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + print("\n👋 Shutting down...") + finally: + if pedal_thread: + pedal_thread.join(timeout=1.0) + try: + if nc.is_recording(): + nc.cancel_recording() + nc.logout() + except Exception: + pass + data_manager.request_shutdown() + if quest_reader: + try: + quest_reader.stop() + except Exception: + pass + if robot_controller: + try: + robot_controller.cleanup() + except Exception: + pass + + +============================================================ +FILE: 4_rollout_neuracore_policy.py +============================================================ + +#!/usr/bin/env python3 +"""Piper Robot Test with Neuracore policy. + +This script loads a trained Neuracore policy, reads status from the piper robot +controlled by the Meta Quest controller, and replays the prediction horizon virtually +on Viser to test the stability of the policy output. +""" + +import argparse +import sys +import threading +import time +import traceback +from pathlib import Path + +import neuracore as nc +import numpy as np +from neuracore.ml.preprocessing.methods.resize_pad import ResizePad +from neuracore.ml.utils.preprocessing_utils import PreprocessingConfiguration +from neuracore_types import ( + BatchedJointData, + BatchedNCData, + BatchedParallelGripperOpenAmountData, + DataType, + EmbodimentDescription, +) + +# Add parent directory to path to import pink_ik_solver and piper_controller +sys.path.insert(0, str(Path(__file__).parent.parent)) + +from common.configs import ( + CAMERA_FRAME_STREAMING_RATE, + CAMERA_NAMES, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + CONTROLLER_DATA_RATE, + CONTROLLER_MIN_CUTOFF, + DAMPING_COST, + FRAME_TASK_GAIN, + GRIPPER_FRAME_NAME, + GRIPPER_NAME, + IK_SOLVER_RATE, + JOINT_NAMES, + JOINT_STATE_STREAMING_RATE, + LM_DAMPING, + MAX_ACTION_ERROR_THRESHOLD, + MAX_SAFETY_THRESHOLD, + NEUTRAL_JOINT_ANGLES, + ORIENTATION_COST, + POLICY_EXECUTION_RATE, + POSITION_COST, + POSTURE_COST_VECTOR, + PREDICTION_HORIZON_EXECUTION_RATIO, + ROBOT_RATE, + SOLVER_DAMPING_VALUE, + SOLVER_NAME, + TARGETING_POSE_TIME_THRESHOLD, + URDF_PATH, + VISUALIZATION_RATE, +) +from common.data_manager import DataManager, RobotActivityState +from common.policy_state import PolicyState +from common.robot_visualizer import RobotVisualizer +from common.threads.ik_solver import ik_solver_thread +from common.threads.joint_state import joint_state_thread +from common.threads.quest_reader import quest_reader_thread +from common.threads.realsense_camera import camera_thread +from meta_quest_teleop.reader import MetaQuestReader + +from pink_ik_solver import PinkIKSolver +from piper_controller import PiperController + + +def _embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: + """Ordered channel names from an embodiment entry (list or index→name map).""" + if isinstance(spec, dict): + return [spec[i] for i in sorted(spec)] + return list(spec) + + +def convert_predictions_to_horizon( + predictions: dict[DataType, dict[str, BatchedNCData]], +) -> dict[str, list[float]]: + """Convert predictions to horizon dict.""" + print('[convert_predictions_to_horizon]') + horizon = {} + if DataType.JOINT_TARGET_POSITIONS in predictions: + joint_data = predictions[DataType.JOINT_TARGET_POSITIONS] + for joint_name in JOINT_NAMES: + if joint_name in joint_data: + batched = joint_data[joint_name] + if isinstance(batched, BatchedJointData): + values = batched.value[0, :, 0].cpu().numpy().tolist() + horizon[joint_name] = values + + # Extract gripper open amounts + if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: + print('[convert_predictions_to_horizon] Extracting gripper open amounts') + gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] + if GRIPPER_NAME in gripper_data: + batched = gripper_data[GRIPPER_NAME] + if isinstance(batched, BatchedParallelGripperOpenAmountData): + values = batched.open_amount[0, :, 0].cpu().numpy().tolist() + horizon[GRIPPER_NAME] = values + return horizon + + +def toggle_robot_enabled_status( + data_manager: DataManager, + robot_controller: PiperController, + visualizer: RobotVisualizer, +) -> None: + """Handle Button A press to toggle robot enable/disable state.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + # Disable robot + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + robot_controller.graceful_stop() + # Reset teleop state when disabling robot + data_manager.set_teleop_state(False, None, None) + visualizer.update_toggle_robot_enabled_status(False) + print("✓ 🔴 Robot disabled") + elif robot_activity_state == RobotActivityState.DISABLED: + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + visualizer.update_toggle_robot_enabled_status(True) + print("✓ 🟢 Robot enabled") + else: + print("✗ Failed to enable robot") + + +def home_robot(data_manager: DataManager, robot_controller: PiperController) -> None: + """Handle Button B press to move robot to home position.""" + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.ENABLED: + print("🏠 Button B pressed - Moving to home position...") + # Set state to HOMING to prevent IK thread from sending robot commands + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + # Disable teleop during homing + data_manager.set_teleop_state(False, None, None) + ok = robot_controller.move_to_home() + if not ok: + print("✗ Failed to initiate home move") + # Revert to ENABLED on failure + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("⚠️ Home command received but robot is not enabled") + + +def run_policy( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, +) -> bool: + """Handle Run Policy button press to capture state and get policy prediction.""" + print("Running policy...") + + # Get available data from data_manager (only log what the model expects) + current_joint_angles = None + gripper_open_value = None + rgb_image = None + + if DataType.JOINT_POSITIONS in input_embodiment_description: + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + joint_angles_rad = np.radians(current_joint_angles) + positions_by_name = { + jn: float(ang) for jn, ang in zip(JOINT_NAMES, joint_angles_rad) + } + policy_joint_order = _embodiment_names_ordered( + input_embodiment_description[DataType.JOINT_POSITIONS] + ) + joint_positions_dict = { + jn: positions_by_name[jn] for jn in policy_joint_order + } + nc.log_joint_positions(joint_positions_dict) + print(" ✓ Logged joint positions") + else: + print(" ⚠️ No current joint angles available") + + if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: + gripper_open_value = data_manager.get_current_gripper_open_value() + if gripper_open_value is not None: + nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_open_value) + print(" ✓ Logged gripper open amount") + else: + print(" ⚠️ No gripper open value available") + + if DataType.RGB_IMAGES in input_embodiment_description: + rgb_names = _embodiment_names_ordered( + input_embodiment_description[DataType.RGB_IMAGES] + ) + logged_any_rgb = False + for camera_name in rgb_names: + img = data_manager.get_rgb_image(camera_name) + if img is not None: + nc.log_rgb(camera_name, img) + logged_any_rgb = True + if logged_any_rgb: + print(" ✓ Logged RGB image(s)") + else: + print(" ⚠️ No RGB image available") + if rgb_names: + rgb_image = data_manager.get_rgb_image(rgb_names[0]) + + # Check if we have at least some data to run the policy + if ( + current_joint_angles is None + and gripper_open_value is None + and rgb_image is None + ): + print("✗ No data available to run policy") + return False + + # Get policy prediction + try: + start_time = time.time() + predictions = policy.predict(timeout=60) + inference_time = time.time() - start_time + prediction_horizon = convert_predictions_to_horizon(predictions) + conversion_time = time.time() - start_time - inference_time + + # Calculate length directly from the new prediction dictionary + horizon_length = 0 + if prediction_horizon: + first_key = next(iter(prediction_horizon.keys())) + horizon_length = len(prediction_horizon[first_key]) + + print( + f" ✓ Got policy prediction in {inference_time:.2f} seconds with horizon length {horizon_length}" + ) + print(f" ✓ Conversion time: {conversion_time:.2f} seconds") + + prediction_ratio = visualizer.get_prediction_ratio() + policy_state.set_execution_ratio(prediction_ratio) + + # Set policy inputs (only if available) + if rgb_image is not None: + policy_state.set_policy_rgb_image_input(rgb_image) + if current_joint_angles is not None: + policy_state.set_policy_state_input(current_joint_angles) + + # Store prediction horizon actions in policy state + policy_state.set_prediction_horizon(prediction_horizon) + + visualizer.update_ghost_robot_visibility(True) + policy_state.set_ghost_robot_playing(True) + policy_state.reset_ghost_action_index() + + except Exception as e: + print(f"✗ Failed to get policy prediction: {e}") + traceback.print_exc() + return False + + return True + + +def start_policy_execution( + data_manager: DataManager, policy_state: PolicyState +) -> bool: + """Handle Execute Policy button press to start policy execution.""" + print("Starting policy execution...") + + # Check if policy execution is already active + if ( + data_manager.get_robot_activity_state() == RobotActivityState.POLICY_CONTROLLED + and not policy_state.get_continuous_play_active() + ): + print("⚠️ Policy execution already in progress") + return False + # Check if robot is enabled + elif data_manager.get_robot_activity_state() == RobotActivityState.DISABLED: + print("⚠️ Cannot execute policy: Robot is disabled") + return False + + # Get prediction horizon + prediction_horizon = policy_state.get_prediction_horizon() + prediction_horizon_length = policy_state.get_prediction_horizon_length() + if prediction_horizon_length == 0: + print("⚠️ No prediction horizon available. Make sure policy was run first.") + return False + + # Check that we have joint data for all joints + if not all(joint_name in prediction_horizon for joint_name in JOINT_NAMES): + print("⚠️ First prediction in horizon has no joint targets") + return False + + # Safety check: verify robot is close enough to first action + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is None: + print("⚠️ Cannot execute policy: No current joint angles available") + return False + + # Get first action from horizon (index 0 for each joint) + current_joint_target_positions_rad = np.array( + [prediction_horizon[joint_name][0] for joint_name in JOINT_NAMES] + ) + current_target_deg = np.degrees(current_joint_target_positions_rad) + joint_differences = np.abs(current_joint_angles - current_target_deg) + + if np.any(joint_differences > MAX_SAFETY_THRESHOLD): + print("⚠️ Cannot execute policy: Robot too far from first predicted action") + print(" --- DIAGNOSTICS ---") + print(f" Current Angles: {[f'{d:.2f}' for d in current_joint_angles]}") + print(f" AI Predicted: {[f'{d:.2f}' for d in current_target_deg]}") + print(f" Differences: {[f'{d:.2f}' for d in joint_differences]}") + print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print(" 💡 TIP 1: Did the arm sag? Check if 'Current Angles' are drooping.") + print(" 💡 TIP 2: If the AI naturally predicts large first steps, increase MAX_SAFETY_THRESHOLD in common/configs.py") + return False + + # All checks passed - start execution + + # Stop ghost visualization + policy_state.set_ghost_robot_playing(False) + + # Deactivate teleop + data_manager.set_teleop_state(False, None, None) + + # Lock policy inputs and start execution + policy_state.start_policy_execution() + + # Verify locked horizon was created successfully + locked_horizon_length = policy_state.get_locked_prediction_horizon_length() + if locked_horizon_length == 0: + print("⚠️ Failed to lock prediction horizon - horizon is empty") + policy_state.end_policy_execution() + return False + + print(f"✓ Starting policy execution with {locked_horizon_length} actions") + + # Change robot state to POLICY_CONTROLLED + data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) + + return True + + +def end_policy_play( + data_manager: DataManager, + policy_state: PolicyState, + visualizer: RobotVisualizer, + policy_status_message: str, +) -> None: + """End continuous play and set robot activity state to ENABLED and update policy status.""" + if policy_state.get_continuous_play_active(): + policy_state.set_continuous_play_active(False) + + # Reset ghost robot color to default orange + visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) + + visualizer.update_play_policy_button_status(False) + + visualizer.update_play_policy_button_status(False) + policy_state.end_policy_execution() + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + data_manager.set_teleop_state(False, None, None) + visualizer.update_policy_status(policy_status_message) + +def continuous_prediction_worker( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", +) -> None: + """Background thread for continuous execution supporting pipelined and sequential modes.""" + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + + # 1. Bootstrap the very first prediction to get the robot moving + print(f"\n🚀 [Worker] Bootstrapping initial trajectory in '{continuous_mode}' mode...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + if success: + start_policy_execution(data_manager, policy_state) + + while policy_state.get_continuous_play_active(): + # Failsafe: if there's no active trajectory running yet, wait briefly + if policy_state.get_locked_prediction_horizon_length() == 0: + time.sleep(0.01) + continue + + if continuous_mode == "pipeline": + print("\n📸 [Pipeline Worker] Robot is moving! Prefetching next prediction in background...") + # Query the network in parallel while the execution thread is driving the motors + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + # Wait until the current trajectory buffer is running low before swapping + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + remaining = total_len - exec_idx + + # Hot-swap when 5 or fewer steps are left in the active trajectory + if remaining <= 5 or total_len == 0: + break + time.sleep(0.01) + + elif continuous_mode == "sequential": + # Wait until the current trajectory buffer is completely exhausted + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + if exec_idx >= total_len or total_len == 0: + break + time.sleep(0.01) + + if not policy_state.get_continuous_play_active(): + break + + print("\n📸 [Sequential Worker] Trajectory finished! Holding position and requesting next prediction...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + if not policy_state.get_continuous_play_active(): + break + + print("🔄 [Worker] Swapping to new trajectory buffer!") + # Seamlessly clear the lock and flash the new horizon into play + policy_state.end_policy_execution() + success = start_policy_execution(data_manager, policy_state) + + if success: + color_index = (color_index + 1) % len(VISUALIZATION_COLORS) + visualizer.set_ghost_robot_color(VISUALIZATION_COLORS[color_index]) + else: + print("❌ [Worker] Swap rejected by safety threshold. Retrying immediately...") + time.sleep(0.01) + +def play_policy( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", +) -> None: + """Handle Play Policy button press to start/stop continuous policy execution.""" + if not policy_state.get_continuous_play_active(): + # Start continuous play + print(f"▶️ Play Policy button pressed - Starting {continuous_mode.capitalize()} Mode...") + policy_state.set_continuous_play_active(True) + visualizer.update_play_policy_button_status(True) + + # Spawn the background worker + threading.Thread( + target=continuous_prediction_worker, + args=(data_manager, policy, policy_state, visualizer, input_embodiment_description, continuous_mode), + daemon=True + ).start() + else: + # Stop continuous play + print("⏹️ Stop Policy button pressed - Stopping continuous policy execution...") + policy_state.set_continuous_play_active(False) + end_policy_play( + data_manager, policy_state, visualizer, "Policy execution stopped" + ) + print("✓ Policy execution stopped and robot enabled") + +def policy_execution_thread( + policy: nc.policy, + data_manager: DataManager, + policy_state: PolicyState, + robot_controller: PiperController, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, +) -> None: + """Policy execution thread.""" + dt_execution = 1.0 / POLICY_EXECUTION_RATE + + # Define colors for continuous horizon visualization + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + + # Throttle visualization updates to ~30Hz to avoid overwhelming Viser + last_visualization_update = 0.0 + visualization_update_interval = 1.0 / 30.0 # 30 Hz + while True: + start_time = time.time() + + if ( + data_manager.get_robot_activity_state() + == RobotActivityState.POLICY_CONTROLLED + ): + locked_horizon = policy_state.get_locked_prediction_horizon() + execution_index = policy_state.get_execution_action_index() + locked_horizon_length = policy_state.get_locked_prediction_horizon_length() + + # Debug output on first execution step + if execution_index == 0 and locked_horizon_length > 0: + print( + f"🔄 Policy execution started: {locked_horizon_length} actions, " + f"robot enabled: {robot_controller.is_robot_enabled()}" + ) + + # If continuous play is active, only execute up to the chunk limit + if execution_index < locked_horizon_length: + # Check if previous goal was achieved, if any + current_joint_angles = data_manager.get_current_joint_angles() + if ( + execution_index > 0 + and current_joint_angles is not None + and policy_state.get_execution_mode() + == PolicyState.ExecutionMode.TARGETING_POSE + ): + targeting_pose_start_time = time.time() + while ( + time.time() - targeting_pose_start_time + < TARGETING_POSE_TIME_THRESHOLD + ): + # Get previous action from horizon + if not all( + joint_name in locked_horizon for joint_name in JOINT_NAMES + ): + break + previous_joint_target_positions_rad = np.array( + [ + locked_horizon[joint_name][execution_index - 1] + for joint_name in JOINT_NAMES + ] + ) + previous_joint_target_positions_deg = np.degrees( + previous_joint_target_positions_rad + ) + joint_errors = np.abs( + current_joint_angles - previous_joint_target_positions_deg + ) + if np.any(joint_errors <= MAX_ACTION_ERROR_THRESHOLD): + break + time.sleep(0.001) + + # Send current action to robot (if available) + if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): + current_joint_target_positions_rad = np.array( + [ + locked_horizon[joint_name][execution_index] + for joint_name in JOINT_NAMES + ] + ) + current_joint_target_positions_deg = np.degrees( + current_joint_target_positions_rad + ) + # Update data_manager with target joint angles for visualization + data_manager.set_target_joint_angles( + current_joint_target_positions_deg + ) + + # Verify robot controller is enabled before sending commands + if robot_controller.is_robot_enabled(): + robot_controller.set_target_joint_angles( + current_joint_target_positions_deg + ) + else: + print( + f"⚠️ Robot controller not enabled, skipping command at index {execution_index}" + ) + + # Send current gripper open value to robot (if available) + if GRIPPER_NAME in locked_horizon: + current_gripper_target_open_value = locked_horizon[GRIPPER_NAME][ + execution_index + ] + robot_controller.set_gripper_open_value( + current_gripper_target_open_value + ) + + # Update execution index + policy_state.increment_execution_action_index() + + # Update status + visualizer.update_policy_status( + f"Executing policy: {execution_index + 1}/{locked_horizon_length}" + ) + else: + # Horizon buffer exhausted + if not policy_state.get_continuous_play_active(): + print("✓ Policy execution completed") + end_policy_play( + data_manager, policy_state, visualizer, "Policy execution completed" + ) + else: + # Failsafe: If the background thread is running slightly late, + # hold the very last predicted position to maintain motor torque. + if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): + last_index = locked_horizon_length - 1 + hold_positions_rad = np.array([ + locked_horizon[jn][last_index] for jn in JOINT_NAMES + ]) + if robot_controller.is_robot_enabled(): + robot_controller.set_target_joint_angles(np.degrees(hold_positions_rad)) + + # NOTE: Update visualization less frequently to avoid blocking + # Throttle visualization updates to ~30Hz to prevent overwhelming Viser server + current_time = time.time() + if current_time - last_visualization_update >= visualization_update_interval: + update_visualization(data_manager, policy_state, visualizer) + last_visualization_update = current_time + + dt_execution = 1.0 / visualizer.get_policy_execution_rate() + elapsed = time.time() - start_time + if elapsed < dt_execution: + time.sleep(dt_execution - elapsed) + + +def update_visualization( + data_manager: DataManager, + policy_state: PolicyState, + visualizer: RobotVisualizer, +) -> None: + """Update visualization.""" + # Update actual robot visualization + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + joint_config_rad = np.radians(current_joint_angles) + visualizer.update_robot_pose(joint_config_rad) + + # Update RGB camera image in Viser GUI (if available) + rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) + if rgb_image is not None: + visualizer.update_rgb_image(rgb_image) + + # Get policy state for ghost robot + prediction_horizon = policy_state.get_prediction_horizon() + prediction_horizon_length = policy_state.get_prediction_horizon_length() + ghost_robot_playing = policy_state.get_ghost_robot_playing() + ghost_action_index = policy_state.get_ghost_action_index() + + # Update ghost robot based on current state + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.POLICY_CONTROLLED: + # During policy execution, make ghost robot show target joint angles + visualizer.update_ghost_robot_visibility(True) + target_joint_angles = data_manager.get_target_joint_angles() + if target_joint_angles is not None: + joint_config_rad = np.radians(target_joint_angles) + visualizer.update_ghost_robot_pose(joint_config_rad) + # Disable buttons during execution + visualizer.set_run_policy_button_disabled(True) + # Play/Stop button is enabled during execution so we can stop if needed + visualizer.set_play_policy_button_disabled(False) + + elif ( + robot_activity_state == RobotActivityState.ENABLED + and data_manager.get_teleop_active() + ): + # During teleoperation, make ghost robot show target joint angles + visualizer.update_ghost_robot_visibility(True) + target_joint_angles = data_manager.get_target_joint_angles() + if target_joint_angles is not None: + joint_config_rad = np.radians(target_joint_angles) + visualizer.update_ghost_robot_pose(joint_config_rad) + + elif ghost_robot_playing and prediction_horizon_length > 0: + # Enable execute policy button + visualizer.set_start_policy_execution_button_disabled(False) + # show ghost robot + visualizer.update_ghost_robot_visibility(True) + # Update ghost robot with prediction horizon actions (preview mode) + if ghost_action_index < prediction_horizon_length: + # Get ghost action from horizon + if all(joint_name in prediction_horizon for joint_name in JOINT_NAMES): + ghost_joint_config = np.array( + [ + prediction_horizon[joint_name][ghost_action_index] + for joint_name in JOINT_NAMES + ] + ) + visualizer.update_ghost_robot_pose(ghost_joint_config) + next_index = (ghost_action_index + 1) % prediction_horizon_length + policy_state.set_ghost_action_index(next_index) + else: + policy_state.reset_ghost_action_index() + + else: + # When not playing, hide the ghost robot + visualizer.update_ghost_robot_visibility(False) + + # Update button state and policy status when not policy controlled + robot_enabled = robot_activity_state == RobotActivityState.ENABLED + has_horizon = prediction_horizon_length > 0 + + # Update button enabled state + visualizer.set_start_policy_execution_button_disabled( + not (robot_enabled and has_horizon) + ) + visualizer.set_run_policy_button_disabled(not robot_enabled) + visualizer.set_play_policy_button_disabled(not robot_enabled) + + # Update policy status + if not has_horizon: + visualizer.update_policy_status( + "Ready - Press Right Joystick or 'Run Policy' button to get prediction" + ) + elif not robot_enabled: + visualizer.update_policy_status("Robot not enabled") + else: + visualizer.update_policy_status( + f"Ready - {prediction_horizon_length} actions in horizon" + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Piper Robot Test with Neuracore Policy - REAL ROBOT CONTROL" + ) + parser.add_argument( + "--ip-address", + type=str, + default=None, + help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", + ) + parser.add_argument( + "--no-quest", + action="store_true", + help="Disable Meta Quest controller integration entirely.", + ) + policy_group = parser.add_mutually_exclusive_group(required=True) + policy_group.add_argument( + "--train-run-name", + type=str, + default=None, + help="Name of the training run to load policy from (for cloud training).", + ) + + parser.add_argument( + "--continuous-mode", + type=str, + choices=["pipeline", "sequential"], + default="sequential", + help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", + ) + + policy_group.add_argument( + "--model-path", + type=str, + default=None, + help="Path to local model file to load policy from.", + ) + policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Name of remote Neuracore policy endpoint.", + ) + parser.add_argument( + "--robot-name", + type=str, + default="AgileX PiPER", + help="Neuracore robot name (policy embodiment resolution).", + ) + args = parser.parse_args() + + print("=" * 60) + print("PIPER ROBOT TEST WITH NEURACORE POLICY") + print("=" * 60) + print("Thread frequencies:") + print(f" 🎮 Quest Controller: {CONTROLLER_DATA_RATE} Hz") + print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") + print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") + print(f" 📸 Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz") + print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") + print(f" 🖥️ Visualization: {VISUALIZATION_RATE} Hz") + + # Connect to Neuracore + print("\n🔧 Initializing Neuracore...") + nc.login() + nc.connect_robot( + robot_name=args.robot_name, + urdf_path=str(URDF_PATH), + overwrite=False, + ) + + # Load policy (cross-embodiment + preprocessing; same pattern as example 6) + input_embodiment_description: EmbodimentDescription = { + DataType.JOINT_POSITIONS: { + 0: "joint1", + 1: "joint2", + 2: "joint3", + 3: "joint4", + 4: "joint5", + 5: "joint6", + }, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]}, + } + output_embodiment_description: EmbodimentDescription = { + DataType.JOINT_TARGET_POSITIONS: { + 0: "joint1", + 1: "joint2", + 2: "joint3", + 3: "joint4", + 4: "joint5", + 5: "joint6", + }, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + } + input_preprocessing_config: PreprocessingConfiguration = { + DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], + } + + print("\n📋 Input embodiment description:") + for data_type, spec in input_embodiment_description.items(): + print(f" {data_type.name}: {_embodiment_names_ordered(spec)}") + print("\n📋 Output embodiment description:") + for data_type, spec in output_embodiment_description.items(): + print(f" {data_type.name}: {_embodiment_names_ordered(spec)}") + + if args.remote_endpoint_name is not None: + print( + f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." + ) + try: + policy = nc.policy_remote_server(args.remote_endpoint_name) + except nc.EndpointError: + print( + f"❌ Endpoint '{args.remote_endpoint_name}' not available. " + "Please start it from the Neuracore dashboard." + ) + sys.exit(1) + elif args.train_run_name is not None: + print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") + policy = nc.policy( + train_run_name=args.train_run_name, + device="cuda", + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + ) + else: + print(f"\n🤖 Loading policy from model file: {args.model_path}...") + policy = nc.policy( + model_file=args.model_path, + device="cuda", + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + ) + print(" ✓ Policy loaded successfully") + + # Initialize policy state + policy_state = PolicyState() + policy_state.set_execution_mode(PolicyState.ExecutionMode.TARGETING_TIME) + + # Initialize shared state + data_manager = DataManager() + data_manager.set_controller_filter_params( + CONTROLLER_MIN_CUTOFF, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + ) + + # Initialize robot controller + print("\n🤖 Initializing Piper robot controller...") + robot_controller = PiperController( + can_interface="can0", + robot_rate=ROBOT_RATE, + control_mode=PiperController.ControlMode.JOINT_SPACE, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + debug_mode=False, + ) + + # Start robot control loop + print("\n🚀 Starting robot control loop...") + robot_controller.start_control_loop() + + # Start joint state thread + print("\n📊 Starting joint state thread...") + joint_state_thread_obj = threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True + ) + joint_state_thread_obj.start() + + # Initialize and Start Meta Quest reader (Conditional) + quest_reader = None + quest_thread = None + if not args.no_quest: + print("\n🎮 Initializing Meta Quest reader...") + quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) + + print("\n🎮 Starting quest reader thread...") + quest_thread = threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True + ) + quest_thread.start() + else: + print("\n🎮 Meta Quest reader disabled via --no-quest flag.") + + # set initial configuration to current joint angles + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + initial_joint_angles = np.radians(current_joint_angles) + else: + initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) + + # Create Pink IK solver + print("\n🔧 Creating Pink IK solver...") + ik_solver = PinkIKSolver( + urdf_path=URDF_PATH, + end_effector_frame=GRIPPER_FRAME_NAME, + solver_name=SOLVER_NAME, + position_cost=POSITION_COST, + orientation_cost=ORIENTATION_COST, + frame_task_gain=FRAME_TASK_GAIN, + lm_damping=LM_DAMPING, + damping_cost=DAMPING_COST, + solver_damping_value=SOLVER_DAMPING_VALUE, + integration_time_step=1 / IK_SOLVER_RATE, + initial_configuration=initial_joint_angles, + posture_cost_vector=np.array(POSTURE_COST_VECTOR), + ) + + # Start IK solver thread + print("\n🧮 Starting IK solver thread...") + ik_thread = threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True + ) + ik_thread.start() + + # Start camera thread + print("\n📷 Starting camera thread...") + camera_thread_obj = threading.Thread( + target=camera_thread, args=(data_manager,), daemon=True + ) + camera_thread_obj.start() + + # Set up visualization + print("\n🖥️ Starting Viser visualization...") + visualizer = RobotVisualizer(str(URDF_PATH)) + visualizer.add_policy_controls( + initial_prediction_ratio=PREDICTION_HORIZON_EXECUTION_RATIO, + initial_policy_rate=POLICY_EXECUTION_RATE, + initial_robot_rate=ROBOT_RATE, + initial_execution_mode=PolicyState.ExecutionMode.TARGETING_TIME.value, + ) + visualizer.add_toggle_robot_enabled_status_button() + visualizer.add_homing_controls() + visualizer.add_policy_buttons() + + # Set up button callbacks + visualizer.set_toggle_robot_enabled_status_callback( + lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer) + ) + visualizer.set_go_home_callback(lambda: home_robot(data_manager, robot_controller)) + visualizer.set_run_policy_callback( + lambda: run_policy( + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, + ) + ) + visualizer.set_start_policy_execution_callback( + lambda: start_policy_execution(data_manager, policy_state) + ) + + visualizer.set_play_policy_callback( + lambda: play_policy( + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, + args.continuous_mode, + ) + ) + + # Set up execution mode dropdown callback to sync with PolicyState + visualizer.set_execution_mode_callback( + lambda: policy_state.set_execution_mode( + PolicyState.ExecutionMode(visualizer.get_execution_mode()) + ) + ) + + # Register Quest reader button callbacks conditionally + if not args.no_quest and quest_reader is not None: + quest_reader.on( + "button_a_pressed", + lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer), + ) + quest_reader.on( + "button_b_pressed", lambda: home_robot(data_manager, robot_controller) + ) + + # Start policy execution thread + print("\n🤖 Starting policy execution thread...") + policy_execution_thread_obj = threading.Thread( + target=policy_execution_thread, + args=( + policy, + data_manager, + policy_state, + robot_controller, + visualizer, + input_embodiment_description, + ), + daemon=True, + ) + policy_execution_thread_obj.start() + + print() + print("🚀 Starting teleoperation with policy testing...") + print("🎮 CONTROLS:") + if not args.no_quest: + print(" 1. Press BUTTON A or Enable Robot button to enable/disable robot") + print(" 2. You have same control over the robot as in teleoperation.") + print(" - Hold RIGHT GRIP to activate teleoperation") + print(" - Move controller - robot follows!") + print(" - Hold RIGHT TRIGGER to close gripper") + print(" - Press BUTTON A or Enable Robot button to enable/disable robot") + print(" - Press BUTTON B or Home Robot button to send robot home") + else: + print(" 1. Click Enable Robot button in Viser to enable/disable robot") + print(" 2. (Meta Quest controls disabled via --no-quest flag)") + print(" 3. Click 'Run Policy' (Preview) to generate and visualize a prediction horizon") + print(" 4. Click 'Execute Policy' to run the currently previewed horizon") + print(" 5. Click 'Play Policy' (Receding Horizon) to constantly predict and execute the first action") + print("⚠️ Press Ctrl+C to exit") + print() + print("🌐 Open browser: http://localhost:8080") + + try: + while True: + time.sleep(1) + + except KeyboardInterrupt: + print("\n👋 Interrupt received - shutting down gracefully...") + except Exception as e: + print(f"\n❌ Demo error: {e}") + traceback.print_exc() + + # Cleanup + print("\n🧹 Cleaning up...") + + # Disconnect policy + policy.disconnect() + + # shutdown threads + data_manager.request_shutdown() + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + + # Conditional cleanup for Quest + if quest_thread is not None: + quest_thread.join() + if quest_reader is not None: + quest_reader.stop() + + ik_thread.join() + camera_thread_obj.join() + robot_controller.cleanup() + + nc.logout() + + print("\n👋 Demo stopped.") + +============================================================ +FILE: common/foot_pedal.py +============================================================ + +"""Foot pedal abstraction with callback dispatch and key mapping.""" + +import os +import sys +import time +import traceback +from typing import Callable + +# add the parent directory to the path +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +from common.data_manager import DataManager + + +class FootPedal: + """Task-agnostic pedal/keyboard mapping with dynamic button support.""" + + def __init__(self, key_map: dict[str, str] | None = None) -> None: + """Initialize dynamic button mapping and callback registry.""" + source = key_map if key_map is not None else {} + self._key_map = {name: str(key).strip().lower() for name, key in source.items()} + self._callbacks: dict[str, Callable[[], None]] = {} + + @property + def key_map(self) -> dict[str, str]: + """Return a copy of the current button-to-key map.""" + return dict(self._key_map) + + def set_key_map(self, key_map: dict[str, str]) -> None: + """Set the key map for the foot pedal. Will override the current key map. + + Args: + key_map: The key map to set for the foot pedal. + """ + self._key_map = { + name: str(key).strip().lower() for name, key in key_map.items() + } + + def bind(self, button_name: str, callback: Callable[[], None] | None) -> None: + """Bind a callback to a button name. + + Args: + button_name: The name of the button to bind the callback to. + callback: The callback to bind to the button name. + + NOTE: if callback is None, the callback is removed from the button name. + """ + if callback is None: + self._callbacks.pop(button_name, None) + return + self._callbacks[button_name] = callback + + def get_bound_buttons(self) -> list[str]: + """Return button names that currently have callbacks bound.""" + return list(self._callbacks.keys()) + + def _dispatch(self, char: str) -> None: + """Fire mapped callback for a pressed key.""" + normalized = str(char).strip().lower() + for button_name, mapped_key in self._key_map.items(): + if normalized == mapped_key: + callback = self._callbacks.get(button_name) + if callback: + callback() + + def run_loop(self, data_manager: DataManager) -> None: + """Block and dispatch pedal events until shutdown is requested.""" + print("⌨️ Foot pedal listener started.") + + try: + from pynput import keyboard + + print("⌨️ Foot pedal listener (pynput) started.") + + def on_press(key: object) -> None: + try: + char = key.char if hasattr(key, "char") else str(key) + self._dispatch(char) + except Exception: + pass + + with keyboard.Listener(on_press=on_press) as listener: + while not data_manager.is_shutdown_requested(): + if not listener.is_alive(): + break + time.sleep(0.1) + listener.stop() + + except Exception as e: + print(f"✗ Fatal error in foot pedal: {e}") + traceback.print_exc() + finally: + print("⌨️ Foot pedal listener stopped.") + + +if __name__ == "__main__": + print("🚀 Standalone Foot Pedal Hardware Test") + print("--------------------------------------") + data_manager = DataManager() + pedal = FootPedal({"button_a": "a", "button_b": "b", "button_c": "c"}) + pedal.bind("button_a", lambda: print("✓ 🟡 Pedal A (ENABLE/DISABLE) detected")) + pedal.bind("button_b", lambda: print("✓ 🏠 Pedal B (HOME) detected")) + pedal.bind("button_c", lambda: print("✓ 🔴 Pedal C (RECORD) detected")) + + try: + pedal.run_loop(data_manager) + except KeyboardInterrupt: + print("\n👋 Test stopped.") + + +============================================================ +FILE: common/policy_state.py +============================================================ + +"""Policy state - policy prediction, policy action, policy action index.""" + +import threading +from enum import Enum + +import numpy as np + + +class PolicyState: + """Policy state - policy prediction, policy action, policy action index.""" + + class ExecutionMode(Enum): + """Execution mode enumeration.""" + + TARGETING_TIME = "targeting_time" + TARGETING_POSE = "targeting_pose" + + def __init__(self) -> None: + """Initialize PolicyState with default values.""" + # Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names + self._prediction_horizon: dict[str, list[float]] = {} + self._prediction_horizon_lock = threading.Lock() + self._execution_ratio: float = 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in + + self._policy_rgb_image_input: np.ndarray | None = None + self._policy_rgb_image_input_lock = threading.Lock() + + self._policy_state_input: np.ndarray | None = None + self._policy_state_input_lock = threading.Lock() + + self._ghost_robot_playing: bool = False + self._ghost_action_index: int = 0 + + # Policy execution state + self._policy_inputs_locked: bool = False + self._locked_prediction_horizon: dict[str, list[float]] = {} + self._execution_action_index: int = 0 + self._execution_lock = threading.Lock() + + # Continuous play and execution mode + self._continuous_play_active: bool = False + self._execution_mode: PolicyState.ExecutionMode = ( + PolicyState.ExecutionMode.TARGETING_TIME + ) + + def get_prediction_horizon_length(self) -> int: + """Get prediction horizon length (thread-safe).""" + with self._prediction_horizon_lock: + if not self._prediction_horizon: + return 0 + # Get length from first list (all should have same length) + first_key = next(iter(self._prediction_horizon.keys())) + return len(self._prediction_horizon[first_key]) + + def get_prediction_horizon(self) -> dict[str, list[float]]: + """Get prediction horizon (thread-safe).""" + with self._prediction_horizon_lock: + # Return a deep copy to prevent external modifications + return { + key: list(values) for key, values in self._prediction_horizon.items() + } + + def set_prediction_horizon(self, horizon: dict[str, list[float]]) -> None: + """Set prediction horizon (thread-safe).""" + with self._prediction_horizon_lock: + # Store a deep copy to prevent external modifications + self._prediction_horizon = { + key: list(values) for key, values in horizon.items() + } + + def set_execution_ratio(self, ratio: float) -> None: + """Set execution ratio used when locking prediction horizon.""" + # Clamp to (0, 1] to avoid zero-length horizons + clamped_ratio = float(np.clip(ratio, 1e-6, 1.0)) + with self._prediction_horizon_lock: + self._execution_ratio = clamped_ratio + + def get_execution_ratio(self) -> float: + """Get execution ratio (thread-safe).""" + with self._prediction_horizon_lock: + return self._execution_ratio + + def get_policy_rgb_image_input(self) -> np.ndarray | None: + """Get policy RGB image (thread-safe).""" + with self._policy_rgb_image_input_lock: + return ( + self._policy_rgb_image_input.copy() + if self._policy_rgb_image_input is not None + else None + ) + + def set_policy_rgb_image_input(self, image: np.ndarray) -> None: + """Set policy RGB image (thread-safe).""" + with self._policy_rgb_image_input_lock: + self._policy_rgb_image_input = image.copy() if image is not None else None + + def get_policy_state_input(self) -> np.ndarray | None: + """Get policy state input (thread-safe).""" + with self._policy_state_input_lock: + return ( + self._policy_state_input.copy() + if self._policy_state_input is not None + else None + ) + + def set_policy_state_input(self, input: np.ndarray) -> None: + """Set policy state input (thread-safe).""" + with self._policy_state_input_lock: + self._policy_state_input = input.copy() if input is not None else None + + def get_ghost_robot_playing(self) -> bool: + """Get ghost robot playing (thread-safe).""" + return self._ghost_robot_playing + + def set_ghost_robot_playing(self, playing: bool) -> None: + """Set ghost robot playing (thread-safe).""" + self._ghost_robot_playing = playing + + def get_ghost_action_index(self) -> int: + """Get ghost action index (thread-safe).""" + return self._ghost_action_index + + def set_ghost_action_index(self, index: int) -> None: + """Set ghost action index (thread-safe).""" + self._ghost_action_index = index + + def reset_ghost_action_index(self) -> None: + """Reset ghost action index (thread-safe).""" + self._ghost_action_index = 0 + + # Policy execution methods + def start_policy_execution(self) -> None: + """Start policy execution by locking inputs and storing horizon (thread-safe).""" + with self._prediction_horizon_lock: + source_horizon = { + key: list(values) for key, values in self._prediction_horizon.items() + } + # Calculate length directly from source_horizon instead of calling get_prediction_horizon_length() + # which would try to acquire the same lock again (deadlock!) + if not source_horizon: + total = 0 + else: + first_key = next(iter(source_horizon.keys())) + total = len(source_horizon[first_key]) + + if total == 0: + locked_horizon = {} + else: + num_actions = int(total * self._execution_ratio) + num_actions = max(1, min(num_actions, total)) + # Slice each list in the horizon + locked_horizon = { + key: values[:num_actions] for key, values in source_horizon.items() + } + with self._execution_lock: + self._policy_inputs_locked = True + self._execution_action_index = 0 + self._locked_prediction_horizon = locked_horizon + + def end_policy_execution(self) -> None: + """Stop policy execution and unlock inputs (thread-safe).""" + with self._execution_lock: + self._policy_inputs_locked = False + self._locked_prediction_horizon = {} + self._execution_action_index = 0 + + def get_locked_prediction_horizon(self) -> dict[str, list[float]]: + """Get locked prediction horizon (thread-safe).""" + with self._execution_lock: + # Return a deep copy to prevent external modifications + return { + key: list(values) + for key, values in self._locked_prediction_horizon.items() + } + + def get_locked_prediction_horizon_length(self) -> int: + """Get locked prediction horizon length (thread-safe).""" + with self._execution_lock: + if not self._locked_prediction_horizon: + return 0 + # Get length from first list (all should have same length) + first_key = next(iter(self._locked_prediction_horizon.keys())) + return len(self._locked_prediction_horizon[first_key]) + + def get_locked_prediction_horizon_sync_points(self) -> dict[str, list[float]]: + """Get locked prediction horizon (legacy method name, calls get_locked_prediction_horizon).""" + return self.get_locked_prediction_horizon() + + def get_execution_action_index(self) -> int: + """Get current execution action index (thread-safe).""" + with self._execution_lock: + return self._execution_action_index + + def increment_execution_action_index(self) -> None: + """Increment execution action index (thread-safe).""" + with self._execution_lock: + self._execution_action_index += 1 + + def get_continuous_play_active(self) -> bool: + """Get continuous play active state (thread-safe).""" + return self._continuous_play_active + + def set_continuous_play_active(self, active: bool) -> None: + """Set continuous play active state (thread-safe).""" + self._continuous_play_active = active + + def get_execution_mode(self) -> ExecutionMode: + """Get execution mode (thread-safe).""" + return self._execution_mode + + def set_execution_mode(self, mode: ExecutionMode) -> None: + """Set execution mode (thread-safe).""" + self._execution_mode = mode + + +============================================================ +FILE: common/__init__.py +============================================================ + +"""Common modules for robot demos. + +This package contains shared utilities for robot visualization and teleoperation state management. +""" + + +============================================================ +FILE: common/policy_helpers.py +============================================================ + +"""Shared helpers for AgileX Neuracore policy rollout examples.""" + +from __future__ import annotations + +from typing import Any + +import neuracore as nc +import numpy as np +from neuracore_types import ( + BatchedJointData, + BatchedNCData, + BatchedParallelGripperOpenAmountData, + DataType, + EmbodimentDescription, +) + +from .configs import GRIPPER_NAME, JOINT_NAMES + +DEFAULT_ROBOT_NAME = "AgileX PiPER" + + +def embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: + """Ordered channel names from an embodiment entry (list or index→name map).""" + if isinstance(spec, dict): + return [spec[i] for i in sorted(spec)] + return list(spec) + + +def get_policy_embodiments( + policy: Any, +) -> tuple[EmbodimentDescription, EmbodimentDescription | None]: + """Read input/output embodiment specs resolved on the loaded policy.""" + if hasattr(policy, "_policy"): + inner = policy._policy + return inner.input_embodiment_description, inner.output_embodiment_description + input_emb = getattr(policy, "input_embodiment_description", None) + if input_emb is None: + raise AttributeError( + "Could not read input_embodiment_description from policy; " + "use nc.policy(..., robot_name=...) without overriding embodiments." + ) + output_emb = getattr(policy, "output_embodiment_description", None) + return input_emb, output_emb + + +def print_policy_embodiments( + input_embodiment: EmbodimentDescription, + output_embodiment: EmbodimentDescription | None, +) -> None: + """Print resolved policy embodiment channels.""" + print("\n📋 Policy input embodiment (from model):") + for data_type, spec in input_embodiment.items(): + print(f" {data_type.name}: {embodiment_names_ordered(spec)}") + if output_embodiment is not None: + print("\n📋 Policy output embodiment (from model):") + for data_type, spec in output_embodiment.items(): + print(f" {data_type.name}: {embodiment_names_ordered(spec)}") + + +def log_robot_state_for_policy( + data_manager: Any, + input_embodiment_description: EmbodimentDescription, +) -> bool: + """Log only sensor streams the policy expects. Returns True if anything was logged.""" + logged_any = False + + if DataType.JOINT_POSITIONS in input_embodiment_description: + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is not None: + joint_angles_rad = np.radians(current_joint_angles) + positions_by_name = { + jn: float(ang) for jn, ang in zip(JOINT_NAMES, joint_angles_rad) + } + policy_joint_order = embodiment_names_ordered( + input_embodiment_description[DataType.JOINT_POSITIONS] + ) + nc.log_joint_positions( + {jn: positions_by_name[jn] for jn in policy_joint_order} + ) + logged_any = True + + if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: + gripper_open_value = data_manager.get_current_gripper_open_value() + if gripper_open_value is not None: + for gripper_name in embodiment_names_ordered( + input_embodiment_description[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] + ): + nc.log_parallel_gripper_open_amount(gripper_name, gripper_open_value) + logged_any = True + + if DataType.RGB_IMAGES in input_embodiment_description: + for camera_name in embodiment_names_ordered( + input_embodiment_description[DataType.RGB_IMAGES] + ): + rgb_image = data_manager.get_rgb_image(camera_name) + if rgb_image is not None: + nc.log_rgb(camera_name, rgb_image) + logged_any = True + + return logged_any + + +def log_sync_step_for_policy( + step: Any, + input_embodiment_description: EmbodimentDescription, +) -> bool: + """Log a synchronized dataset step using only channels the policy expects.""" + logged_any = False + + if DataType.JOINT_POSITIONS in input_embodiment_description: + joint_data = step.data.get(DataType.JOINT_POSITIONS, {}) + joint_positions_dict = { + joint_name: joint_data[joint_name].value + for joint_name in embodiment_names_ordered( + input_embodiment_description[DataType.JOINT_POSITIONS] + ) + if joint_name in joint_data + } + if joint_positions_dict: + nc.log_joint_positions(joint_positions_dict) + logged_any = True + + if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: + gripper_data = step.data.get(DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, {}) + for gripper_name in embodiment_names_ordered( + input_embodiment_description[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] + ): + if gripper_name in gripper_data: + nc.log_parallel_gripper_open_amount( + gripper_name, gripper_data[gripper_name].open_amount + ) + logged_any = True + + if DataType.RGB_IMAGES in input_embodiment_description: + rgb_data = step.data.get(DataType.RGB_IMAGES, {}) + for camera_name in embodiment_names_ordered( + input_embodiment_description[DataType.RGB_IMAGES] + ): + if camera_name in rgb_data: + nc.log_rgb(camera_name, np.array(rgb_data[camera_name].frame)) + logged_any = True + + return logged_any + + +def convert_predictions_to_horizon( + predictions: dict[DataType, dict[str, BatchedNCData]], +) -> dict[str, list[float]]: + """Convert policy predictions to a per-channel horizon dict (model-driven keys).""" + horizon: dict[str, list[float]] = {} + + if DataType.JOINT_TARGET_POSITIONS in predictions: + for joint_name, batched in predictions[DataType.JOINT_TARGET_POSITIONS].items(): + if isinstance(batched, BatchedJointData): + horizon[joint_name] = batched.value[0, :, 0].cpu().numpy().tolist() + + if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: + for gripper_name, batched in predictions[ + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS + ].items(): + if isinstance(batched, BatchedParallelGripperOpenAmountData): + horizon[gripper_name] = ( + batched.open_amount[0, :, 0].cpu().numpy().tolist() + ) + + return horizon + + +def horizon_length(horizon: dict[str, list[float]]) -> int: + """Number of steps in a prediction horizon dict.""" + if not horizon: + return 0 + return len(next(iter(horizon.values()))) + + +def arm_joint_names_in_horizon(horizon: dict[str, list[float]]) -> list[str]: + """Body joint names present in a horizon (excludes gripper channels).""" + return [name for name in JOINT_NAMES if name in horizon] + + +def joint_targets_deg_at_index( + horizon: dict[str, list[float]], index: int +) -> np.ndarray | None: + """Arm joint targets in degrees at horizon index (Piper JOINT_NAMES order).""" + if not all(jn in horizon for jn in JOINT_NAMES): + return None + if any(index >= len(horizon[jn]) for jn in JOINT_NAMES): + return None + rad = np.array([horizon[jn][index] for jn in JOINT_NAMES], dtype=np.float64) + return np.degrees(rad) + + +def gripper_open_at_index( + horizon: dict[str, list[float]], + index: int, + gripper_names: list[str] | None = None, +) -> float | None: + """Gripper open amount in [0, 1] from the first matching horizon channel.""" + names = gripper_names or [GRIPPER_NAME] + for gripper_name in names: + if gripper_name in horizon and index < len(horizon[gripper_name]): + return float(np.clip(horizon[gripper_name][index], 0.0, 1.0)) + return None + + +def urdf_cfg_from_horizon( + horizon: dict[str, list[float]], index: int +) -> np.ndarray | None: + """Joint configuration in radians for Viser URDF (JOINT_NAMES order).""" + if not all(jn in horizon for jn in JOINT_NAMES): + return None + if any(index >= len(horizon[jn]) for jn in JOINT_NAMES): + return None + return np.array([float(horizon[jn][index]) for jn in JOINT_NAMES], dtype=np.float64) + + +============================================================ +FILE: common/data_manager.py +============================================================ + +#!/usr/bin/env python3 +"""Thread-safe teleoperation state management. + +This module provides shared state classes for teleoperation systems that need +to coordinate between multiple threads (data collection, IK solving, visualization). +""" +import threading +import time +import queue # Added for async queueing +from enum import Enum +from typing import Any, Callable + +import numpy as np + +from .configs import GRIPPER_NAME, JOINT_NAMES +from .one_euro_filter import OneEuroFilterTransform + + +class RobotActivityState(Enum): + """Robot activity state enumeration.""" + ENABLED = "ENABLED" + HOMING = "HOMING" + DISABLED = "DISABLED" + POLICY_CONTROLLED = "POLICY_CONTROLLED" + + +class ControllerState: + """Controller input state - Quest Reader writes, IK/Joint reads.""" + + def __init__(self) -> None: + """Initialize ControllerState with default values.""" + self._lock = threading.Lock() + + # 1€ Filter parameters + self.min_cutoff: float = 1.0 # Minimum cutoff frequency + self.beta: float = 0.0 # Speed coefficient + self.d_cutoff: float = 1.0 # Derivative cutoff frequency + + # Controller data + self.transform_raw: np.ndarray | None = None + self.transform: np.ndarray | None = None # Smoothed transform + self.grip_value: float = 0.0 + self.trigger_value: float = 0.0 + self._filter: OneEuroFilterTransform | None = None + + +class TeleopState: + """Teleop activation state - manages teleop start/stop.""" + + def __init__(self) -> None: + """Initialize TeleopState with default values.""" + self._lock = threading.Lock() + + self.active: bool = False + self.controller_initial_transform: np.ndarray | None = None + self.robot_initial_transform: np.ndarray | None = None + # Teleoperation scaling parameters (how much controller motion maps to robot motion) + self.translation_scale: float = 1.0 + self.rotation_scale: float = 1.0 + self.slow_scaling_mode_enabled: bool = False + + +class RobotState: + """Current robot state - joint angles, end effector pose, activity state.""" + + def __init__(self) -> None: + """Initialize RobotState with default values.""" + self._lock = threading.Lock() + + self.joint_angles: np.ndarray | None = None + self.joint_torques: np.ndarray | None = None + self.end_effector_pose: np.ndarray | None = None + self.current_gripper_open_value: float | None = None + self.target_gripper_open_value: float | None = None + self.activity_state: RobotActivityState = RobotActivityState.DISABLED + + +class IKState: + """IK solution state - target joint angles, pose, metrics.""" + + def __init__(self) -> None: + """Initialize IKState with default values.""" + self._lock = threading.Lock() + + self.target_joint_angles: np.ndarray | None = None + self.target_pose: np.ndarray | None = None + self.solve_time_ms: float = 0.0 + self.success: bool = True + + +class CameraState: + """Camera state - RGB images for one or more cameras.""" + + def __init__(self) -> None: + """Initialize CameraState with default values.""" + self._lock = threading.Lock() + + # Map from camera name -> latest RGB image + self.rgb_images: dict[str, np.ndarray] = {} + +class DataManager: + """Main state container coordinating all state groups. + + This class manages shared data between threads: + - Data collection thread: updates controller data + - IK solver thread: reads controller data, updates joint solutions + - Main thread: reads everything for visualization + + Uses separate locks for each state group to reduce contention. + """ + def __init__(self) -> None: + """Initialize DataManager with background callback processing.""" + self._controller_state = ControllerState() + self._teleop_state = TeleopState() + self._robot_state = RobotState() + self._ik_state = IKState() + self._camera_state = CameraState() + + # System state + self._shutdown_event = threading.Event() + + # Asynchronous processing elements + self._on_change_callback: ( + Callable[[str, dict[str, Any], float], None] | None + ) = None + + # Maxsize 60 matches ~1 second of video frames buffer if disk spikes + self._callback_queue: queue.Queue = queue.Queue(maxsize=60) + + self._worker_thread = threading.Thread( + target=self._callback_worker_loop, + name="NeuracoreCallbackWorker", + daemon=True + ) + self._worker_thread.start() + + def set_on_change_callback( + self, on_change_callback: Callable[[str, dict[str, Any], float], None] + ) -> None: + """Set on change callback (thread-safe).""" + self._on_change_callback = on_change_callback + + def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None: + """Helper to push payloads into the execution queue without blocking.""" + if self._on_change_callback is None: + return + + try: + # put_nowait drops data into the memory queue instantly (0.0ms blocking) + self._callback_queue.put_nowait((name, payload, timestamp)) + except queue.Full: + # Prevents out-of-memory if disk halts completely, without freezing telemetry loops + print(f"⚠️ Neuracore background queue full! Dropping log packet: {name}") + + def _callback_worker_loop(self) -> None: + """Background thread worker loop dedicated solely to performing slow disk IO updates.""" + while not self._shutdown_event.is_set() or not self._callback_queue.empty(): + try: + # Wait up to 100ms for a logging event + name, payload, timestamp = self._callback_queue.get(timeout=0.1) + + if self._on_change_callback is not None: + # Execute Neuracore disk operation safely here on a separate core + self._on_change_callback(name, payload, timestamp) + + self._callback_queue.task_done() + except queue.Empty: + continue + except Exception as e: + print(f"❌ Error in background logging callback: {e}") + + # ============================================================================ + # Camera State Methods + # ============================================================================ + + def get_rgb_image(self, camera_name: str) -> np.ndarray | None: + """Get RGB image for a specific camera (thread-safe).""" + with self._camera_state._lock: + if not self._camera_state.rgb_images: + return None + img = self._camera_state.rgb_images.get(camera_name) + return img.copy() if img is not None else None + + def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: + """Set RGB image for a specific camera (thread-safe and non-blocking).""" + with self._camera_state._lock: + self._camera_state.rgb_images[camera_name] = image.copy() + + if self._on_change_callback: + img_copy = self._camera_state.rgb_images[camera_name].copy() + # Queue it instead of executing directly! Camera loop returns immediately. + self._queue_callback("log_rgb", {camera_name: img_copy}, time.time()) + + # ============================================================================ + # Controller State Methods + # ============================================================================ + + def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: + with self._controller_state._lock: + return ( + ( + self._controller_state.transform.copy() + if self._controller_state.transform is not None + else None + ), + self._controller_state.grip_value, + self._controller_state.trigger_value, + ) + + def set_controller_data( + self, transform: np.ndarray | None, grip: float, trigger: float + ) -> None: + if transform is not None and transform.shape != (4, 4): + raise ValueError("Transform must be a 4x4 matrix") + if grip < 0.0 or grip > 1.0: + raise ValueError("Grip value must be between 0.0 and 1.0") + if trigger < 0.0 or trigger > 1.0: + raise ValueError("Trigger value must be between 0.0 and 1.0") + + with self._controller_state._lock: + self._controller_state.grip_value = grip + self._controller_state.trigger_value = trigger + + if transform is not None: + current_time = time.time() + self._controller_state.transform_raw = transform.copy() + + if self._controller_state._filter is None: + self._controller_state._filter = OneEuroFilterTransform( + current_time, + transform, + self._controller_state.min_cutoff, + self._controller_state.beta, + self._controller_state.d_cutoff, + ) + self._controller_state.transform = transform.copy() + else: + self._controller_state._filter.update_params( + self._controller_state.min_cutoff, + self._controller_state.beta, + self._controller_state.d_cutoff, + ) + self._controller_state.transform = self._controller_state._filter( + current_time, transform + ) + else: + self._controller_state.transform = None + self._controller_state.transform_raw = None + self._controller_state._filter = None + + def set_controller_filter_params( + self, min_cutoff: float, beta: float, d_cutoff: float + ) -> None: + with self._controller_state._lock: + self._controller_state.min_cutoff = min_cutoff + self._controller_state.beta = beta + self._controller_state.d_cutoff = d_cutoff + + def get_controller_filter_params(self) -> tuple[float, float, float]: + with self._controller_state._lock: + return ( + self._controller_state.min_cutoff, + self._controller_state.beta, + self._controller_state.d_cutoff, + ) + + # ============================================================================ + # Teleop State Methods + # ============================================================================ + + def set_teleop_state( + self, + active: bool, + controller_initial: np.ndarray | None, + robot_initial: np.ndarray | None, + ) -> None: + with self._teleop_state._lock: + self._teleop_state.active = active + self._teleop_state.controller_initial_transform = ( + controller_initial.copy() if controller_initial is not None else None + ) + self._teleop_state.robot_initial_transform = ( + robot_initial.copy() if robot_initial is not None else None + ) + + def set_teleop_scaling( + self, translation_scale: float, rotation_scale: float + ) -> None: + if translation_scale <= 0.0 or rotation_scale <= 0.0: + return + with self._teleop_state._lock: + self._teleop_state.translation_scale = translation_scale + self._teleop_state.rotation_scale = rotation_scale + + def get_teleop_scaling(self) -> tuple[float, float]: + with self._teleop_state._lock: + return ( + self._teleop_state.translation_scale, + self._teleop_state.rotation_scale, + ) + + def get_teleop_active(self) -> bool: + with self._teleop_state._lock: + return self._teleop_state.active + + def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: + with self._teleop_state._lock: + self._teleop_state.slow_scaling_mode_enabled = enabled + + def toggle_slow_scaling_mode_enabled(self) -> bool: + with self._teleop_state._lock: + self._teleop_state.slow_scaling_mode_enabled = ( + not self._teleop_state.slow_scaling_mode_enabled + ) + return self._teleop_state.slow_scaling_mode_enabled + + def get_slow_scaling_mode_enabled(self) -> bool: + with self._teleop_state._lock: + return self._teleop_state.slow_scaling_mode_enabled + + def get_initial_robot_controller_transforms( + self, + ) -> tuple[np.ndarray | None, np.ndarray | None]: + with self._teleop_state._lock: + return ( + ( + self._teleop_state.controller_initial_transform.copy() + if self._teleop_state.controller_initial_transform is not None + else None + ), + ( + self._teleop_state.robot_initial_transform.copy() + if self._teleop_state.robot_initial_transform is not None + else None + ), + ) + + # ============================================================================ + # Robot State Methods + # ============================================================================ + + def get_robot_activity_state(self) -> RobotActivityState: + with self._robot_state._lock: + return self._robot_state.activity_state + + def set_robot_activity_state(self, state: RobotActivityState) -> None: + with self._robot_state._lock: + self._robot_state.activity_state = state + + def get_current_joint_angles(self) -> np.ndarray | None: + with self._robot_state._lock: + return ( + self._robot_state.joint_angles.copy() + if self._robot_state.joint_angles is not None + else None + ) + + def set_current_joint_angles(self, angles: np.ndarray) -> None: + with self._robot_state._lock: + self._robot_state.joint_angles = angles.copy() + if self._on_change_callback: + angles = self._robot_state.joint_angles + if angles is not None: + payload = { + jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) + } + self._queue_callback("log_joint_positions", payload, time.time()) + + def get_current_joint_torques(self) -> np.ndarray | None: + with self._robot_state._lock: + return ( + self._robot_state.joint_torques.copy() + if self._robot_state.joint_torques is not None + else None + ) + + def set_current_joint_torques(self, torques: np.ndarray) -> None: + with self._robot_state._lock: + self._robot_state.joint_torques = torques.copy() + if self._on_change_callback: + torques = self._robot_state.joint_torques + if torques is not None: + payload = {jn: float(torques[i]) for i, jn in enumerate(JOINT_NAMES)} + self._queue_callback("log_joint_torques", payload, time.time()) + + def get_current_end_effector_pose(self) -> np.ndarray | None: + with self._robot_state._lock: + return ( + self._robot_state.end_effector_pose.copy() + if self._robot_state.end_effector_pose is not None + else None + ) + + def set_current_end_effector_pose(self, pose: np.ndarray) -> None: + with self._robot_state._lock: + self._robot_state.end_effector_pose = pose.copy() + + def get_current_gripper_open_value(self) -> float | None: + with self._robot_state._lock: + return self._robot_state.current_gripper_open_value + + def set_current_gripper_open_value(self, value: float) -> None: + with self._robot_state._lock: + self._robot_state.current_gripper_open_value = value + if self._on_change_callback: + self._queue_callback( + "log_parallel_gripper_open_amounts", + {GRIPPER_NAME: value}, + time.time(), + ) + + def get_target_gripper_open_value(self) -> float | None: + with self._robot_state._lock: + return self._robot_state.target_gripper_open_value + + def set_target_gripper_open_value(self, value: float) -> None: + with self._robot_state._lock: + self._robot_state.target_gripper_open_value = value + if self._on_change_callback: + self._queue_callback( + "log_parallel_gripper_target_open_amounts", + {GRIPPER_NAME: self._robot_state.target_gripper_open_value}, + time.time(), + ) + + # ============================================================================ + # IK State Methods + # ============================================================================ + + def get_target_joint_angles(self) -> np.ndarray | None: + with self._ik_state._lock: + return ( + self._ik_state.target_joint_angles.copy() + if self._ik_state.target_joint_angles is not None + else None + ) + + def set_target_joint_angles(self, angles: np.ndarray) -> None: + with self._ik_state._lock: + self._ik_state.target_joint_angles = angles.copy() + if self._on_change_callback: + angles = self._ik_state.target_joint_angles + if angles is not None: + payload = { + jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) + } + self._queue_callback( + "log_joint_target_positions", payload, time.time() + ) + + def set_target_pose(self, transform: np.ndarray | None) -> None: + with self._ik_state._lock: + self._ik_state.target_pose = ( + transform.copy() if transform is not None else None + ) + + def get_target_pose(self) -> np.ndarray | None: + with self._ik_state._lock: + return ( + self._ik_state.target_pose.copy() + if self._ik_state.target_pose is not None + else None + ) + + def set_ik_solve_time_ms(self, time_ms: float) -> None: + with self._ik_state._lock: + self._ik_state.solve_time_ms = time_ms + + def set_ik_success(self, success: bool) -> None: + with self._ik_state._lock: + self._ik_state.success = success + + def get_ik_solve_time_ms(self) -> float: + with self._ik_state._lock: + return self._ik_state.solve_time_ms + + def get_ik_success(self) -> bool: + with self._ik_state._lock: + return self._ik_state.success + + # ============================================================================ + # System State Methods + # ============================================================================ + + def request_shutdown(self) -> None: + """Request shutdown of all threads.""" + self._shutdown_event.set() + + def is_shutdown_requested(self) -> bool: + """Check if shutdown is requested.""" + return self._shutdown_event.is_set() + +============================================================ +FILE: common/utils.py +============================================================ + +"""Utility functions for the examples.""" + +import numpy as np +from scipy.spatial.transform import Rotation + + +def scale_and_add_delta_transform( + delta_position: np.ndarray, + delta_orientation: np.ndarray, + translation_scale: float, + rotation_scale: float, + initial_transform: np.ndarray, +) -> np.ndarray: + """Scale and add delta position and orientation to an initial transform. + + Args: + delta_position: 3D position vector + delta_orientation: 3x3 rotation matrix + translation_scale: translation scale + rotation_scale: rotation scale + initial_transform: 4x4 transformation matrix + + Returns: + 4x4 transformation matrix + """ + scaled_delta_position = delta_position * translation_scale + + # Scale the delta rotation by converting to axis-angle, scaling the angle, then converting back + delta_rotation = Rotation.from_matrix(delta_orientation) + delta_axis_angle = delta_rotation.as_rotvec() + scaled_delta_axis_angle = delta_axis_angle * rotation_scale + scaled_delta_rotation = Rotation.from_rotvec(scaled_delta_axis_angle).as_matrix() + + # Compose rotations by matrix multiplication (correct way to combine rotations) + initial_rotation = initial_transform[:3, :3] + new_rotation = initial_rotation @ scaled_delta_rotation + + target_transform = np.eye(4) + target_transform[:3, 3] = initial_transform[:3, 3] + scaled_delta_position + target_transform[:3, :3] = new_rotation + + return target_transform + + +============================================================ +FILE: common/one_euro_filter.py +============================================================ + +"""1€ Filter (One Euro Filter) for adaptive low-pass filtering.""" + +import numpy as np +from scipy.spatial.transform import Rotation + + +class OneEuroFilter: + """1€ Filter (One Euro Filter) for adaptive low-pass filtering. + + The 1€ Filter adapts its smoothing based on signal velocity: + - Fast motion: Low smoothing (low latency, responsive) + - Slow motion: High smoothing (high stability, reduces jitter) + """ + + def __init__( + self, + t0: float, + x0: float, + min_cutoff: float = 1.0, + beta: float = 0.0, + d_cutoff: float = 1.0, + ): + """Initialize 1€ Filter. + + Args: + t0: Initial timestamp (in seconds) + x0: Initial value + min_cutoff: Minimum cutoff frequency (stabilizes when holding still) + Higher = less lag, more jitter + beta: Speed coefficient (reduces lag when moving) + Higher = less lag, but more jitter during motion + d_cutoff: Cutoff frequency for derivative (speed) filtering + """ + self.min_cutoff = min_cutoff + self.beta = beta + self.d_cutoff = d_cutoff + self.x_prev = x0 + self.dx_prev = 0.0 + self.t_prev = t0 + + def smoothing_factor(self, t_e: float, cutoff: float) -> float: + """Calculate smoothing factor for exponential smoothing. + + Args: + t_e: Time elapsed since last update (in seconds) + cutoff: Cutoff frequency + + Returns: + Smoothing factor (0.0-1.0) + """ + r = 2 * np.pi * cutoff * t_e + return r / (r + 1) + + def exponential_smoothing(self, a: float, x: float, x_prev: float) -> float: + """Apply exponential smoothing. + + Args: + a: Smoothing factor (0.0-1.0) + x: Current value + x_prev: Previous smoothed value + + Returns: + Smoothed value + """ + return a * x + (1 - a) * x_prev + + def __call__(self, timestamp: float, x: float) -> float: + """Filter a new value. + + Args: + t: Current timestamp (in seconds) + x: Current noisy value + + Returns: + Filtered value + """ + t_e = timestamp - self.t_prev + + # Avoid division by zero in rare cases + if t_e <= 0.0: + return self.x_prev + + # Calculate the derivative (speed) of the signal + dx = (x - self.x_prev) / t_e + dx_hat = self.exponential_smoothing( + self.smoothing_factor(t_e, self.d_cutoff), dx, self.dx_prev + ) + + # Calculate the adaptive cutoff frequency + # This is the magic part: Cutoff increases with speed + cutoff = self.min_cutoff + self.beta * abs(dx_hat) + + # Filter the signal using the adaptive cutoff + x_hat = self.exponential_smoothing( + self.smoothing_factor(t_e, cutoff), x, self.x_prev + ) + + self.x_prev = x_hat + self.dx_prev = dx_hat + self.t_prev = timestamp + + return x_hat + + def update_params(self, min_cutoff: float, beta: float, d_cutoff: float) -> None: + """Update filter parameters without resetting state. + + Args: + min_cutoff: Minimum cutoff frequency + beta: Speed coefficient + d_cutoff: Cutoff frequency for derivative filtering + """ + self.min_cutoff = min_cutoff + self.beta = beta + self.d_cutoff = d_cutoff + + +class OneEuroFilterTransform: + """1€ Filter for 4x4 transformation matrices. + + Applies separate 1€ Filters to position (3D) and orientation (quaternion). + """ + + def __init__( + self, + t0: float, + transform: np.ndarray, + min_cutoff: float = 1.0, + beta: float = 0.0, + d_cutoff: float = 1.0, + ): + """Initialize 1€ Filter for transforms. + + Args: + t0: Initial timestamp (in seconds) + transform: Initial 4x4 transformation matrix + min_cutoff: Minimum cutoff frequency + beta: Speed coefficient + d_cutoff: Cutoff frequency for derivative filtering + """ + # Extract position and orientation + position = transform[:3, 3] + rotation = Rotation.from_matrix(transform[:3, :3]) + quat = rotation.as_quat() # [x, y, z, w] + + # Create filters for position (3 components) + self.position_filters = [ + OneEuroFilter(t0, pos, min_cutoff, beta, d_cutoff) for pos in position + ] + + # Create filters for quaternion (4 components) + self.quat_filters = [ + OneEuroFilter(t0, q, min_cutoff, beta, d_cutoff) for q in quat + ] + + # Store previous quaternion for sign correction + self.quat_prev = quat.copy() + + self.t_prev = t0 + + def __call__(self, timestamp: float, transform: np.ndarray) -> np.ndarray: + """Filter a new transform. + + Args: + t: Current timestamp (in seconds) + transform: Current 4x4 transformation matrix + + Returns: + Filtered 4x4 transformation matrix + """ + # Extract position and orientation + position = transform[:3, 3] + rotation = Rotation.from_matrix(transform[:3, :3]) + quat = rotation.as_quat() # [x, y, z, w] + + # Fix quaternion sign to avoid sudden flips (q and -q represent same rotation) + # Choose the quaternion closest to the previous one + if np.dot(quat, self.quat_prev) < 0: + quat = -quat + + # Filter position components + filtered_position = np.array( + [ + filter(timestamp, pos) + for filter, pos in zip(self.position_filters, position) + ] + ) + + # Filter quaternion components + filtered_quat = np.array( + [filter(timestamp, q) for filter, q in zip(self.quat_filters, quat)] + ) + + # Normalize quaternion (important!) + filtered_quat = filtered_quat / np.linalg.norm(filtered_quat) + + # Convert back to rotation matrix + filtered_rotation = Rotation.from_quat(filtered_quat) + + # Build filtered transform + filtered_transform = np.eye(4) + filtered_transform[:3, 3] = filtered_position + filtered_transform[:3, :3] = filtered_rotation.as_matrix() + + self.quat_prev = filtered_quat.copy() + self.t_prev = timestamp + + return filtered_transform + + def update_params(self, min_cutoff: float, beta: float, d_cutoff: float) -> None: + """Update filter parameters for all sub-filters without resetting state. + + Args: + min_cutoff: Minimum cutoff frequency + beta: Speed coefficient + d_cutoff: Cutoff frequency for derivative filtering + """ + self.min_cutoff = min_cutoff + self.beta = beta + self.d_cutoff = d_cutoff + for filter in self.position_filters + self.quat_filters: + filter.update_params(min_cutoff, beta, d_cutoff) + + +============================================================ +FILE: common/configs.py +============================================================ + +"""Configuration parameters for Piper robot demos.""" + +from pathlib import Path + +URDF_PATH = str( + Path(__file__).parent.parent.parent + / "piper_description" + / "urdf" + / "piper_description.urdf" +) + +GRIPPER_FRAME_NAME = "gripper_center" + + +# Pink IK parameters +SOLVER_NAME = "quadprog" +POSITION_COST = 1.0 +ORIENTATION_COST = 0.75 +FRAME_TASK_GAIN = 0.4 +LM_DAMPING = 0.01 #0.0 +DAMPING_COST = 0.25 +SOLVER_DAMPING_VALUE = 1e-4 #1e-12 + +# Controller 1€ Filter parameters +CONTROLLER_MIN_CUTOFF = 0.8 # Minimum cutoff frequency (stabilizes when holding still) +CONTROLLER_BETA = 0.05 #5.0 # Speed coefficient (reduces lag when moving) +CONTROLLER_D_CUTOFF = 0.9 # Derivative cutoff frequency + +# Controller parameters +GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control + +# Scaling factors for translation and rotation +TRANSLATION_SCALE = 1.5 +ROTATION_SCALE = 1.2 +SLOW_TRANSLATION_SCALE = 0.6 +SLOW_ROTATION_SCALE = 0.6 +WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 + +# Thread rates (Hz) +CONTROLLER_DATA_RATE = 50.0 # Controller input reading +IK_SOLVER_RATE = 100 #250.0 # IK solving and robot commands +VISUALIZATION_RATE = 60.0 # GUI updates +ROBOT_RATE = 100.0 + +# Neuracore data collection rates +JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore +CAMERA_FRAME_STREAMING_RATE = 30.0 # Data collection rate for camera frame + +# Meta quest axis mask +META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] # [x, y, z, roll, pitch, yaw] + +# USB webcam (OpenCV) configuration +CAMERA_DEVICE_INDEX = 5 # 0 = first camera, 1 = second, etc. +CAMERA_WIDTH = 640 +CAMERA_HEIGHT = 480 + +# # Initial neutral pose for robot (mm and degrees) +# NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] +# NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid +# Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) + +NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] + +# Posture task cost vector (one weight per joint) +POSTURE_COST_VECTOR = [0.0, 0.0, 0.0, 0.05, 0.0, 0.0] + + +POLICY_EXECUTION_RATE = 20.0 # Hz +PREDICTION_HORIZON_EXECUTION_RATIO = ( + 1.0 # percentage of the prediction horizon that is executed +) +MAX_SAFETY_THRESHOLD = 200.0 # degrees +MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees +TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds + +GRIPPER_NAME = "gripper" +GRIPPER_LOGGING_NAME = GRIPPER_NAME + +# Camera logging names for Neuracore (scene + wrist) +CAMERA_NAMES = ["rgb_scene", "rgb_wrist"] + +JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] + + +============================================================ +FILE: common/robot_visualizer.py +============================================================ + +#!/usr/bin/env python3 +"""Shared robot visualizer for Piper robot demos. + +This module provides a clean interface for visualizing robot state using Viser, +encapsulating all the repeated setup, GUI controls, and update logic. +""" + +from typing import Any, Callable + +import numpy as np +import viser +import yourdfpy +from scipy.spatial.transform import Rotation +from viser.extras import ViserUrdf + + +class RobotVisualizer: + """Shared visualizer for robot demos. + + Encapsulates viser server setup, GUI controls, and update logic. + """ + + def __init__(self, urdf_path: str) -> None: + """Initialize the visualizer. + + Args: + urdf_path: Path to URDF file for robot visualization + """ + # Initialize viser server + self._server = viser.ViserServer() + self._server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) + + # Load URDF for visualization + urdf = yourdfpy.URDF.load(urdf_path) + self._urdf_vis = ViserUrdf(self._server, urdf, root_node_name="/robot_actual") + + ghost_urdf = yourdfpy.URDF.load(urdf_path) + # Ghost robot with semi-transparent blue color to make it visually distinct + self._ghost_robot_urdf = ViserUrdf( + self._server, + ghost_urdf, + root_node_name="/robot_ghost", + mesh_color_override=(1.0, 0.65, 0.0, 0.25), # Orange with 25% opacity + ) + + # GUI handles (initialized as None, created on demand) - all private + self._timing_handle = None + self._joint_angles_handle = None + self._robot_status_handle = None + self._teleop_status_handle = None + self._controller_status_handle = None + self._gripper_status_handle = None + + # Pink parameter handles + self._position_weight_handle = None + self._orientation_weight_handle = None + self._frame_task_gain_handle = None + self._lm_damping_handle = None + self._damping_weight_handle = None + self._solver_damping_value_handle = None + self._posture_cost_handles: list[Any] = [] + + # Robot control handles + self._enable_robot_handle = None + self._disable_robot_handle = None + self._emergency_stop_handle = None + self._go_home_button = None + self._toggle_robot_enabled_status_button = None + + # Teleop-specific handles + self._grip_value_handle = None + self._trigger_value_handle = None + + # Rate control handles + self._controller_min_cutoff_handle = None + self._controller_beta_handle = None + self._controller_d_cutoff_handle = None + + self._translation_scale_handle = None + self._rotation_scale_handle = None + + # Visualization handles + self._controller_handle = None + self._target_frame_handle = None + self._rgb_image_handle = None + + # Policy-related handles + self._policy_status_handle = None + self._prediction_ratio_handle = None + self._policy_execution_rate_handle = None + self._robot_rate_handle = None + self._execution_mode_dropdown = None + self._run_policy_button = None + self._start_policy_execution_button = None + self._play_policy_button = None + + # Internal state + self._ema_timing = 0.001 + + def add_basic_controls(self) -> None: + """Add basic GUI controls (timing, joint angles).""" + self._timing_handle = self._server.gui.add_number( + "IK Solve Time (ms)", 0.001, disabled=True + ) + self._joint_angles_handle = self._server.gui.add_text( + "Joint Angles", "Waiting for IK solution..." + ) + + def add_robot_status_controls(self) -> None: + """Add robot status display controls.""" + self._robot_status_handle = self._server.gui.add_text( + "Robot Status", "Initializing..." + ) + + def add_teleop_controls(self) -> None: + """Add teleoperation-specific controls.""" + self._grip_value_handle = self._server.gui.add_number( + "Grip Value", 0.0, disabled=True + ) + self._trigger_value_handle = self._server.gui.add_number( + "Trigger Value (Gripper)", 0.0, disabled=True + ) + self._teleop_status_handle = self._server.gui.add_text( + "Teleop Status", "Inactive" + ) + self._controller_status_handle = self._server.gui.add_text( + "Controller Status", "Waiting..." + ) + + def add_gripper_status_controls(self) -> None: + """Add gripper status display controls.""" + self._gripper_status_handle = self._server.gui.add_text( + "Gripper Status", "Open (0%)" + ) + + def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None: + """Add an RGB image placeholder in the desired GUI position. + + This reserves a fixed location for the camera feed; the actual image + data will be injected later via update_rgb_image. + """ + if self._rgb_image_handle is not None: + return + + dummy_image = np.zeros((height, width, 3), dtype=np.uint8) + self._rgb_image_handle = self._server.gui.add_image( + dummy_image, + label="RGB Camera", + format="jpeg", + jpeg_quality=85, + ) + + def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: + """Show or update RGB camera image in the Viser GUI.""" + if rgb_image is None: + return + if self._rgb_image_handle is None: + # Fallback: if placeholder wasn't created, create it now + self.add_rgb_image_placeholder( + height=rgb_image.shape[0], width=rgb_image.shape[1] + ) + rgb_handle = self._rgb_image_handle + if rgb_handle is None: + return + rgb_handle.image = rgb_image + + def add_homing_controls(self) -> None: + """Add homing controls.""" + self._go_home_button = self._server.gui.add_button("Go Home") + + def add_robot_control_buttons(self) -> None: + """Add robot control buttons (enable, disable, emergency stop). + + Note: For homing functionality, use add_homing_controls() instead. + """ + self._enable_robot_handle = self._server.gui.add_button("Enable Robot") + self._disable_robot_handle = self._server.gui.add_button("Disable Robot") + self._emergency_stop_handle = self._server.gui.add_button("Emergency Stop") + + def add_toggle_robot_enabled_status_button(self) -> None: + """Add toggle robot enabled status button. + + This creates a single button that toggles between "Enable Robot" and "Disable Robot" + based on the current robot state. + """ + self._toggle_robot_enabled_status_button = self._server.gui.add_button( + "Enable Robot" + ) + + def update_toggle_robot_enabled_status(self, enabled: bool) -> None: + """Update toggle robot enabled status button label based on robot state. + + Args: + enabled: Whether robot is currently enabled + """ + if self._toggle_robot_enabled_status_button is not None: + self._toggle_robot_enabled_status_button.label = ( + "Enable Robot" if not enabled else "Disable Robot" + ) + + def set_toggle_robot_enabled_status_callback( + self, callback: Callable[[], Any] + ) -> None: + """Set callback for toggle robot enabled status button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._toggle_robot_enabled_status_button is not None: + self._toggle_robot_enabled_status_button.on_click(lambda _: callback()) + + def add_pink_parameter_controls( + self, + position_cost: float, + orientation_cost: float, + frame_task_gain: float, + lm_damping: float, + damping_cost: float, + solver_damping_value: float, + posture_cost_vector: list[float], + ) -> None: + """Add Pink IK parameter controls. + + Args: + position_cost: Initial position cost value + orientation_cost: Initial orientation cost value + frame_task_gain: Initial frame task gain value + lm_damping: Initial LM damping value + damping_cost: Initial damping cost value + solver_damping_value: Initial solver damping value + posture_cost_vector: Initial posture cost vector (one value per joint) + """ + self._position_weight_handle = self._server.gui.add_number( + "Position Weight", position_cost, min=0.0, max=10.0, step=0.1 + ) + self._orientation_weight_handle = self._server.gui.add_number( + "Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01 + ) + self._frame_task_gain_handle = self._server.gui.add_number( + "Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1 + ) + self._lm_damping_handle = self._server.gui.add_number( + "LM Damping", lm_damping, min=0.0, max=5.0, step=0.01 + ) + self._damping_weight_handle = self._server.gui.add_number( + "Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01 + ) + self._solver_damping_value_handle = self._server.gui.add_number( + "Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001 + ) + + # Posture cost controls (one per joint) + self._posture_cost_handles = [] + for i in range(len(posture_cost_vector)): + handle = self._server.gui.add_number( + f"Posture Cost J{i+1}", + posture_cost_vector[i], + min=0.0, + max=1.0, + step=0.01, + ) + self._posture_cost_handles.append(handle) + + def add_controller_filter_controls( + self, + initial_min_cutoff: float, + initial_beta: float, + initial_d_cutoff: float, + ) -> None: + """Add 1€ Filter parameter controls for controller. + + Args: + initial_min_cutoff: Initial minimum cutoff frequency + initial_beta: Initial speed coefficient + initial_d_cutoff: Initial derivative cutoff frequency + """ + self._controller_min_cutoff_handle = self._server.gui.add_number( + "Controller Min Cutoff", + initial_min_cutoff, + min=0.01, + max=10.0, + step=0.01, + ) + self._controller_beta_handle = self._server.gui.add_number( + "Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01 + ) + self._controller_d_cutoff_handle = self._server.gui.add_number( + "Controller D Cutoff", + initial_d_cutoff, + min=0.01, + max=10.0, + step=0.01, + ) + + def add_scaling_controls( + self, initial_translation_scale: float, initial_rotation_scale: float + ) -> None: + """Add scaling factor controls for translation and rotation. + + Args: + initial_translation_scale: Initial translation scale factor + initial_rotation_scale: Initial rotation scale factor + """ + self._translation_scale_handle = self._server.gui.add_number( + "Translation Scale", + initial_translation_scale, + min=0.1, + max=10.0, + step=0.001, + ) + self._rotation_scale_handle = self._server.gui.add_number( + "Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001 + ) + + def add_controller_visualization(self) -> None: + """Add controller transform visualization.""" + self._controller_handle = self._server.scene.add_transform_controls( + "/controller", + scale=0.15, + position=(0, 0, 0), + wxyz=(1, 0, 0, 0), + ) + + def add_target_frame_visualization(self) -> None: + """Add target/goal frame visualization.""" + self._target_frame_handle = self._server.scene.add_frame( + "/target_goal", axes_length=0.1, axes_radius=0.003 + ) + + def update_robot_pose(self, joint_config: np.ndarray) -> None: + """Update robot visualization from joint configuration. + + Args: + joint_config: Joint angles in radians + """ + self._urdf_vis.update_cfg(joint_config) + + def update_joint_angles_display( + self, joint_config: np.ndarray, show_gripper: bool = False + ) -> None: + """Update joint angles display. + + Args: + joint_config: Joint angles in radians + show_gripper: Whether to show gripper joints (joints 7&8) + """ + if self._joint_angles_handle is None: + return + + joint_angles_str = "Joint Angles (rad):\n" + joint_angles_deg = np.degrees(joint_config) + num_joints = len(joint_config) if show_gripper else 6 + + for i in range(num_joints): + angle_rad = joint_config[i] + angle_deg = joint_angles_deg[i] + joint_type = "Robot" if i < 6 else "Gripper" + label = f"Joint {i+1} ({joint_type})" if show_gripper else f"J{i+1}" + joint_angles_str += f" {label}: {angle_rad:.3f} rad ({angle_deg:.1f}°)\n" + + self._joint_angles_handle.value = joint_angles_str + + def update_timing(self, solve_time_ms: float) -> None: + """Update timing display with exponential moving average. + + Args: + solve_time_ms: IK solve time in milliseconds + """ + if self._timing_handle is None: + return + + self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms + self._timing_handle.value = self._ema_timing + + def update_robot_status(self, status: str) -> None: + """Update robot status display. + + Args: + status: Status string to display + """ + if self._robot_status_handle is not None: + self._robot_status_handle.value = status + + def update_teleop_status(self, active: bool) -> None: + """Update teleop status display. + + Args: + active: Whether teleop is active + """ + if self._teleop_status_handle is not None: + self._teleop_status_handle.value = ( + "Teleop Status: Active" if active else "Teleop Status: Inactive" + ) + + def update_controller_status_display( + self, position: np.ndarray | None, connected: bool = True + ) -> None: + """Update controller status display. + + Args: + position: Controller position (3D array) or None + connected: Whether controller is connected + """ + if self._controller_status_handle is None: + return + + if connected and position is not None: + controller_status_str = "Controller Status:\n" + controller_status_str += f" Position: [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}]\n" + controller_status_str += " Connected: ✓\n" + self._controller_status_handle.value = controller_status_str + else: + self._controller_status_handle.value = "Controller Status:\n Connected: ✗" + + def update_gripper_status( + self, trigger_value: float, robot_enabled: bool = True + ) -> None: + """Update gripper status display. + + Args: + trigger_value: Trigger value (0.0 = open, 1.0 = closed) + robot_enabled: Whether robot is enabled + """ + if self._gripper_status_handle is None: + return + + gripper_closed_percent = trigger_value * 100.0 + if trigger_value > 0.9: + gripper_state = "Closed" + elif trigger_value > 0.1: + gripper_state = "Closing" + else: + gripper_state = "Open" + + status = f"Gripper: {gripper_state} ({gripper_closed_percent:.0f}% closed)" + if not robot_enabled: + status += " [Disabled]" + + self._gripper_status_handle.value = status + + def update_controller_visualization(self, transform: np.ndarray | None) -> None: + """Update controller transform visualization. + + Args: + transform: 4x4 transformation matrix or None + """ + if self._controller_handle is None or transform is None: + return + + controller_pos = transform[:3, 3] + controller_rot = Rotation.from_matrix(transform[:3, :3]) + controller_quat_xyzw = controller_rot.as_quat() + controller_quat_wxyz = [ + controller_quat_xyzw[3], + controller_quat_xyzw[0], + controller_quat_xyzw[1], + controller_quat_xyzw[2], + ] + + self._controller_handle.position = tuple(controller_pos) + self._controller_handle.wxyz = tuple(controller_quat_wxyz) + + def update_target_visualization(self, transform: np.ndarray | None) -> None: + """Update target/goal frame visualization. + + Args: + transform: 4x4 transformation matrix or None + """ + if self._target_frame_handle is None or transform is None: + return + + target_pos = transform[:3, 3] + target_rot = Rotation.from_matrix(transform[:3, :3]) + target_quat_xyzw = target_rot.as_quat() + target_quat_wxyz = [ + target_quat_xyzw[3], + target_quat_xyzw[0], + target_quat_xyzw[1], + target_quat_xyzw[2], + ] + + self._target_frame_handle.position = tuple(target_pos) + self._target_frame_handle.wxyz = tuple(target_quat_wxyz) + + def get_pink_parameters(self) -> dict: + """Get Pink IK parameters from GUI controls. + + Returns: + Dictionary with parameter values + + Raises: + ValueError: If Pink parameter controls not initialized + """ + if not self._posture_cost_handles: + raise ValueError("Pink parameter controls not initialized") + + if ( + self._position_weight_handle is None + or self._orientation_weight_handle is None + or self._frame_task_gain_handle is None + or self._lm_damping_handle is None + or self._damping_weight_handle is None + or self._solver_damping_value_handle is None + ): + raise ValueError("Pink parameter controls not initialized") + + posture_cost_vector = np.array( + [handle.value for handle in self._posture_cost_handles] + ) + + params = { + "position_cost": self._position_weight_handle.value, + "orientation_cost": self._orientation_weight_handle.value, + "frame_task_gain": self._frame_task_gain_handle.value, + "lm_damping": self._lm_damping_handle.value, + "damping_cost": self._damping_weight_handle.value, + "solver_damping_value": self._solver_damping_value_handle.value, + "posture_cost_vector": posture_cost_vector, + } + return params + + def get_controller_filter_params(self) -> tuple[float, float, float]: + """Get 1€ Filter parameters from GUI. + + Returns: + Tuple of (min_cutoff, beta, d_cutoff) + + Raises: + ValueError: If controller filter controls not initialized + """ + if ( + self._controller_min_cutoff_handle is None + or self._controller_beta_handle is None + or self._controller_d_cutoff_handle is None + ): + raise ValueError("Controller filter controls not initialized") + return ( + self._controller_min_cutoff_handle.value, + self._controller_beta_handle.value, + self._controller_d_cutoff_handle.value, + ) + + def get_translation_scale(self) -> float: + """Get translation scale value from GUI. + + Returns: + Translation scale value + + Raises: + ValueError: If scaling controls not initialized + """ + if self._translation_scale_handle is None: + raise ValueError("Scaling controls not initialized") + return self._translation_scale_handle.value + + def get_rotation_scale(self) -> float: + """Get rotation scale value from GUI. + + Returns: + Rotation scale value + + Raises: + ValueError: If scaling controls not initialized + """ + if self._rotation_scale_handle is None: + raise ValueError("Scaling controls not initialized") + return self._rotation_scale_handle.value + + def set_grip_value(self, value: float) -> None: + """Set grip value display. + + Args: + value: Grip value (0.0-1.0) + + Raises: + ValueError: If grip value control not initialized + """ + if self._grip_value_handle is None: + raise ValueError("Grip value control not initialized") + self._grip_value_handle.value = value + + def set_trigger_value(self, value: float) -> None: + """Set trigger value display. + + Args: + value: Trigger value (0.0-1.0) + + Raises: + ValueError: If trigger value control not initialized + """ + if self._trigger_value_handle is None: + raise ValueError("Trigger value control not initialized") + self._trigger_value_handle.value = value + + def set_joint_angles_text(self, text: str) -> None: + """Set joint angles text display. + + Args: + text: Text to display + + Raises: + ValueError: If joint angles control not initialized + """ + if self._joint_angles_handle is None: + raise ValueError("Joint angles control not initialized") + self._joint_angles_handle.value = text + + def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None: + """Update ghost robot visualization from joint configuration. + + Args: + joint_config: Joint angles in radians + """ + if self._ghost_robot_urdf is not None: + self._ghost_robot_urdf.update_cfg(joint_config) + + def update_ghost_robot_visibility(self, flag: bool) -> None: + """Update ghost robot visibility. + + Args: + flag: Whether to show the ghost robot + """ + if self._ghost_robot_urdf is not None: + self._ghost_robot_urdf.show_visual = flag + + def add_policy_controls( + self, + initial_prediction_ratio: float = 0.8, + initial_policy_rate: float = 200.0, + initial_robot_rate: float = 200.0, + initial_execution_mode: str = "targeting_time", + ) -> None: + """Add policy-related GUI controls. + + Args: + initial_prediction_ratio: Initial prediction horizon execution ratio (0.0-1.0) + initial_policy_rate: Initial policy execution rate in Hz + initial_robot_rate: Initial robot rate in Hz + initial_execution_mode: Initial execution mode ("targeting_time" or "targeting_pose") + """ + self._policy_status_handle = self._server.gui.add_text( + "Policy Status", "Ready - Press Right Joystick to get prediction" + ) + + self._prediction_ratio_handle = self._server.gui.add_number( + "Prediction Horizon Execution Ratio", + initial_prediction_ratio, + min=0.0, + max=1.0, + step=0.01, + ) + + self._policy_execution_rate_handle = self._server.gui.add_number( + "Policy Execution Rate", + initial_policy_rate, + min=1.0, + max=200.0, + step=1.0, + ) + + self._robot_rate_handle = self._server.gui.add_number( + "Robot Rate", + initial_robot_rate, + min=1.0, + max=200.0, + step=1.0, + ) + + self._execution_mode_dropdown = self._server.gui.add_dropdown( + "Execution Mode", + options=["targeting_time", "targeting_pose"], + initial_value=initial_execution_mode, + ) + + def add_policy_buttons(self) -> None: + """Add policy control buttons.""" + self._run_policy_button = self._server.gui.add_button("Run Policy (Preview)") + self._start_policy_execution_button = self._server.gui.add_button( + "Execute Policy (Run Preview)" + ) + self._play_policy_button = self._server.gui.add_button("Continuous Receding Horizon") + + def update_policy_status(self, status: str) -> None: + """Update policy status display. + + Args: + status: Status string to display + """ + if self._policy_status_handle is not None: + self._policy_status_handle.value = status + + def get_prediction_ratio(self) -> float: + """Get prediction horizon execution ratio from GUI. + + Returns: + Prediction ratio (0.0-1.0) + + Raises: + ValueError: If policy controls not initialized + """ + if self._prediction_ratio_handle is None: + raise ValueError("Policy controls not initialized") + return self._prediction_ratio_handle.value + + def get_policy_execution_rate(self) -> float: + """Get policy execution rate from GUI. + + Returns: + Policy execution rate in Hz + + Raises: + ValueError: If policy controls not initialized + """ + if self._policy_execution_rate_handle is None: + raise ValueError("Policy controls not initialized") + return self._policy_execution_rate_handle.value + + def get_robot_rate(self) -> float: + """Get robot rate from GUI. + + Returns: + Robot rate in Hz + + Raises: + ValueError: If policy controls not initialized + """ + if self._robot_rate_handle is None: + raise ValueError("Policy controls not initialized") + return self._robot_rate_handle.value + + def get_execution_mode(self) -> str: + """Get execution mode from GUI. + + Returns: + Execution mode string ("targeting_time" or "targeting_pose") + + Raises: + ValueError: If policy controls not initialized + """ + if self._execution_mode_dropdown is None: + raise ValueError("Policy controls not initialized") + return self._execution_mode_dropdown.value + + def get_ghost_robot_visibility(self) -> bool: + """Get ghost robot visibility. + + Returns: + Whether the ghost robot is visible + """ + if self._ghost_robot_urdf is not None: + return self._ghost_robot_urdf.show_visual + return False + + def set_run_policy_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Run Policy button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._run_policy_button is not None: + self._run_policy_button.on_click(lambda _: callback()) + + def set_start_policy_execution_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Execute Policy button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._start_policy_execution_button is not None: + self._start_policy_execution_button.on_click(lambda _: callback()) + + + def set_play_policy_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Play Policy button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._play_policy_button is not None: + self._play_policy_button.on_click(lambda _: callback()) + + def set_execution_mode_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for execution mode dropdown. + + Args: + callback: Callback function to call when dropdown value changes + """ + if self._execution_mode_dropdown is not None: + self._execution_mode_dropdown.on_update(lambda _: callback()) + + def set_enable_robot_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Enable Robot button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._enable_robot_handle is not None: + self._enable_robot_handle.on_click(lambda _: callback()) + + def set_disable_robot_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Disable Robot button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._disable_robot_handle is not None: + self._disable_robot_handle.on_click(lambda _: callback()) + + def set_emergency_stop_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Emergency Stop button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._emergency_stop_handle is not None: + self._emergency_stop_handle.on_click(lambda _: callback()) + + def set_go_home_callback(self, callback: Callable[[], Any]) -> None: + """Set callback for Go Home button. + + Args: + callback: Callback function to call when button is clicked + """ + if self._go_home_button is not None: + self._go_home_button.on_click(lambda _: callback()) + + def set_run_policy_button_disabled(self, disabled: bool) -> None: + """Set Run Policy button disabled state. + + Args: + disabled: Whether button should be disabled + """ + if self._run_policy_button is not None: + self._run_policy_button.disabled = disabled + + def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: + """Set Execute Policy button disabled state. + + Args: + disabled: Whether button should be disabled + """ + if self._start_policy_execution_button is not None: + self._start_policy_execution_button.disabled = disabled + + + def set_play_policy_button_disabled(self, disabled: bool) -> None: + """Set Play Policy button disabled state. + + Args: + disabled: Whether button should be disabled + """ + if self._play_policy_button is not None: + self._play_policy_button.disabled = disabled + + def update_play_policy_button_status(self, active: bool) -> None: + """Update play policy button label based on active state. + + Args: + active: Whether continuous play is currently active + """ + if self._play_policy_button is not None: + self._play_policy_button.label = ( + "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + ) + def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + """Set the color of the ghost robot. + + Args: + color: RGBA tuple (0.0 to 1.0) + """ + if self._ghost_robot_urdf is not None: + self._ghost_robot_urdf.mesh_color_override = color + + def stop(self) -> None: + """Stop the visualizer server.""" + self._server.stop() + + +============================================================ +FILE: common/threads/__init__.py +============================================================ + +"""Shared thread functions for teleoperation examples.""" + + +============================================================ +FILE: common/threads/ik_solver.py +============================================================ + +"""IK solver thread - solves IK and updates state.""" + +import sys +import time +import traceback +from pathlib import Path + +import numpy as np + +# Add parent directory to path to import pink_ik_solver +sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.configs import IK_SOLVER_RATE +from common.data_manager import DataManager, RobotActivityState +from common.utils import scale_and_add_delta_transform + +from pink_ik_solver import PinkIKSolver + + +def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None: + """IK solver thread - solves IK and updates state.""" + print("🧮 IK solver thread started") + + dt: float = 1.0 / IK_SOLVER_RATE + DIVERGENCE_TOLERANCE = 0.1 + + try: + while not data_manager.is_shutdown_requested(): + iteration_start: float = time.time() + + # Get current robot joint angles from state + current_joint_angles = data_manager.get_current_joint_angles() + + # Get current end effector pose from IK solver and set in state + ik_ee_pose = ik_solver.get_current_end_effector_pose() + data_manager.set_current_end_effector_pose(ik_ee_pose) + + # Get current state + robot_activity_state = data_manager.get_robot_activity_state() + controller_transform, _, _ = data_manager.get_controller_data() + teleop_active = data_manager.get_teleop_active() + + # Keep IK anchored to the real robot whenever teleop is not actively solving. + # This avoids stale IK state after manual joint commands (e.g., Button Y toggle). + if current_joint_angles is not None: + if not teleop_active: + ik_solver.set_configuration_no_task_update( + np.radians(current_joint_angles) + ) + else: + current_ik_joint_angles = np.degrees( + ik_solver.get_current_configuration() + ) + # During active teleop, only hard-sync when IK and hardware are already close. + if current_ik_joint_angles is not None and np.all( + np.abs(current_joint_angles - current_ik_joint_angles) + <= DIVERGENCE_TOLERANCE + ): + ik_solver.set_configuration_no_task_update( + np.radians(current_joint_angles) + ) + + # Skip teleop-based IK if in POLICY_CONTROLLED state + # NOTE: During policy execution, the policy execution thread manages target joint angles + # We only update IK solver configuration to keep it in sync, but don't override targets + if robot_activity_state == RobotActivityState.POLICY_CONTROLLED: + if current_joint_angles is not None: + ik_solver.set_configuration(np.radians(current_joint_angles)) + current_end_effector_pose = ( + data_manager.get_current_end_effector_pose() + ) + data_manager.set_target_pose(current_end_effector_pose) + data_manager.set_ik_solve_time_ms(0.0) + data_manager.set_ik_success(True) + + elif teleop_active and controller_transform is not None: + controller_initial, robot_initial = ( + data_manager.get_initial_robot_controller_transforms() + ) + if controller_initial is None or robot_initial is None: + elapsed = time.time() - iteration_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + continue + + # Calculate delta transform + delta_position = controller_transform[:3, 3] - controller_initial[:3, 3] + delta_orientation = ( + controller_transform[:3, :3] @ controller_initial[:3, :3].T + ) + + # Get current teleop scaling factors (from GUI via DataManager) + translation_scale, rotation_scale = data_manager.get_teleop_scaling() + + T_robot_target = scale_and_add_delta_transform( + delta_position, + delta_orientation, + translation_scale, + rotation_scale, + robot_initial, + ) + + ik_solver.set_target_pose(T_robot_target[:3, 3], T_robot_target[:3, :3]) + data_manager.set_target_pose(T_robot_target) + + # Solve IK + success = ik_solver.solve_ik() + + if success: + joint_config = np.degrees(ik_solver.get_current_configuration()) + stats = ik_solver.get_statistics() + solve_time_ms = stats["last_solve_time_ms"] + + data_manager.set_target_joint_angles(joint_config) + data_manager.set_ik_solve_time_ms(solve_time_ms) + data_manager.set_ik_success(success) + else: + data_manager.set_ik_solve_time_ms(0.0) + data_manager.set_ik_success(False) + + else: # robot is HOMING or DISABLED + if current_joint_angles is not None: + ik_solver.set_configuration(np.radians(current_joint_angles)) + current_end_effector_pose = ( + data_manager.get_current_end_effector_pose() + ) + data_manager.set_target_pose(current_end_effector_pose) + data_manager.set_target_joint_angles(current_joint_angles) + data_manager.set_ik_solve_time_ms(0.0) + data_manager.set_ik_success(True) + + # Sleep to maintain loop rate + elapsed = time.time() - iteration_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + except Exception as e: + print(f"❌ IK solver thread error: {e}") + traceback.print_exc() + data_manager.request_shutdown() + finally: + print("🧮 IK solver thread stopped") + + +============================================================ +FILE: common/threads/quest_reader.py +============================================================ + +"""Quest reader thread - reads controller data and manages teleop state.""" + +import time +import traceback + +from common.configs import CONTROLLER_DATA_RATE, GRIP_THRESHOLD +from common.data_manager import DataManager, RobotActivityState +from meta_quest_teleop.reader import MetaQuestReader + + +def quest_reader_thread( + data_manager: DataManager, quest_reader: MetaQuestReader +) -> None: + """Quest reader thread - reads controller data and manages teleop state. + + This thread runs at high frequency to ensure responsive controller input. + Handles: + - Reading Meta Quest controller data + - Processing grip button (dead man's switch) + - Managing teleop activation/deactivation + - Capturing initial poses when teleop activates + + Args: + data_manager: DataManager object for thread-safe communication + quest_reader: MetaQuestReader instance + """ + print("🎮 Quest Controller thread started") + + dt: float = 1.0 / CONTROLLER_DATA_RATE + prev_grip_active: bool = False + + try: + while not data_manager.is_shutdown_requested(): + iteration_start = time.time() + + # Get controller data + grip_value = quest_reader.get_grip_value("right") + trigger_value = quest_reader.get_trigger_value("right") + controller_transform = quest_reader.get_hand_controller_transform_ros( + hand="right" + ) + + # Update shared state with controller data + data_manager.set_controller_data( + controller_transform, grip_value, trigger_value + ) + + # Grip button logic (dead man's switch) + robot_activity_state = data_manager.get_robot_activity_state() + # Teleop can only be activated if robot is ENABLED (not HOMING or DISABLED) + grip_active = ( + grip_value >= GRIP_THRESHOLD + and robot_activity_state == RobotActivityState.ENABLED + ) + + # Rising edge - grip just pressed AND robot is enabled + if ( + grip_active + and not prev_grip_active + and controller_transform is not None + ): + # Start teleop control + # capture initial poses + controller_initial_transform = controller_transform.copy() + + # Capture initial robot end-effector pose + robot_initial_transform = data_manager.get_current_end_effector_pose() + + data_manager.set_teleop_state( + True, controller_initial_transform, robot_initial_transform + ) + + print("✓ Teleop control activated") + print( + f" Controller initial position: {controller_initial_transform[:3, 3]}" + ) + if robot_initial_transform is not None: + print(f" Robot initial position: {robot_initial_transform[:3, 3]}") + else: + print(" Robot initial position: None") + + # Falling edge - grip just released OR robot disabled + elif not grip_active and prev_grip_active: + # Stop teleop control + data_manager.set_teleop_state(False, None, None) + print("✗ Teleop control deactivated") + + prev_grip_active = grip_active + + # Sleep to maintain loop rate (check shutdown more frequently) + elapsed = time.time() - iteration_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + except Exception as e: + print(f"❌ Quest reader thread error: {e}") + traceback.print_exc() + data_manager.request_shutdown() + finally: + # Ensure clean exit - deactivate teleop + data_manager.set_teleop_state(False, None, None) + print("🎮 Quest Controller thread stopped") + + +============================================================ +FILE: common/threads/camera_usb.py +============================================================ + +"""Camera thread - captures RGB images from a USB webcam (OpenCV).""" + +import time +import traceback + +import cv2 +from common.configs import ( + CAMERA_DEVICE_INDEX, + CAMERA_FRAME_STREAMING_RATE, + CAMERA_HEIGHT, + CAMERA_NAMES, + CAMERA_WIDTH, +) +from common.data_manager import DataManager + + +def camera_thread(data_manager: DataManager) -> None: + """Camera thread - captures RGB images from a USB webcam.""" + print("📷 Camera thread started (USB webcam)") + + dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE + cap: cv2.VideoCapture | None = None + camera_name = CAMERA_NAMES[1] + + try: + cap = cv2.VideoCapture(CAMERA_DEVICE_INDEX) + if not cap.isOpened(): + print( + f"❌ Could not open USB webcam (device index {CAMERA_DEVICE_INDEX}). " + "Check connection and permissions." + ) + data_manager.request_shutdown() + return + + cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) + cap.set(cv2.CAP_PROP_FPS, CAMERA_FRAME_STREAMING_RATE) + + # Read back actual resolution (some webcams don't support requested size) + actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + print( + f" Webcam opened: {actual_w}x{actual_h} @ ~{CAMERA_FRAME_STREAMING_RATE} Hz" + ) + + while not data_manager.is_shutdown_requested(): + iteration_start = time.time() + + ret, frame = cap.read() + if not ret or frame is None: + print("⚠️ Webcam read failed, skipping frame") + time.sleep(dt) + continue + + # OpenCV returns BGR; convert to RGB for consistency with pipeline + rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + # Camera may be mounted upside down; rotate 180° to keep images upright + rgb_image = cv2.rotate(rgb_image, cv2.ROTATE_180) + # Treat USB webcam as wrist camera + data_manager.set_rgb_image(rgb_image, camera_name) + + elapsed = time.time() - iteration_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + except Exception as e: + print(f"❌ Camera thread error: {e}") + traceback.print_exc() + data_manager.request_shutdown() + finally: + if cap is not None: + cap.release() + print(" ✓ USB webcam released") + print("📷 Camera thread stopped") + + +============================================================ +FILE: common/threads/joint_state.py +============================================================ + +"""Joint state thread - reads joint state and sends commands.""" + +import sys +import time +import traceback +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.configs import JOINT_STATE_STREAMING_RATE +from common.data_manager import DataManager, RobotActivityState + +from piper_controller import PiperController + + +def joint_state_thread( + data_manager: DataManager, robot_controller: PiperController +) -> None: + """Joint state thread - reads joint state and sends commands.""" + print("🔧 Joint state thread started") + + dt: float = 1.0 / JOINT_STATE_STREAMING_RATE + + try: + while not data_manager.is_shutdown_requested(): + iteration_start: float = time.time() + + # Get current joint angles and gripper value + current_joint_angles = robot_controller.get_current_joint_angles() + if current_joint_angles is not None: + data_manager.set_current_joint_angles(current_joint_angles) + + # Use measured joint currents as torque proxy for NeuraCore logging + current_joint_currents = robot_controller.get_current_joint_currents() + if current_joint_currents is not None: + data_manager.set_current_joint_torques(current_joint_currents) + + # Get current gripper open value and set in state + gripper_open_value = robot_controller.get_current_gripper_open_value() + if gripper_open_value is not None: + data_manager.set_current_gripper_open_value(gripper_open_value) + + target_joint_angles = data_manager.get_target_joint_angles() + _, _, trigger_value = data_manager.get_controller_data() + + # Check if robot is homing + robot_activity_state = data_manager.get_robot_activity_state() + if robot_activity_state == RobotActivityState.HOMING: + if robot_controller.is_robot_homed(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ Robot reached home position and is re-enabled") + + elif robot_activity_state == RobotActivityState.ENABLED: + if target_joint_angles is not None and data_manager.get_teleop_active(): + robot_controller.set_target_joint_angles(target_joint_angles) + + if trigger_value is not None and data_manager.get_teleop_active(): + target_gripper_open_value = 1.0 - trigger_value + data_manager.set_target_gripper_open_value( + target_gripper_open_value + ) + robot_controller.set_gripper_open_value(target_gripper_open_value) + + # Sleep to maintain streaming rate + elapsed = time.time() - iteration_start + sleep_time = dt - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + except Exception as e: + print(f"❌ Joint state thread error: {e}") + traceback.print_exc() + data_manager.request_shutdown() + finally: + print("🔧 Joint state thread stopped") + + +============================================================ +FILE: common/threads/realsense_camera.py +============================================================ + +"""Camera thread - captures RGB images from RealSense with drop detection.""" + +import time +import traceback +from collections import deque +import cv2 + +import numpy as np +import pyrealsense2 as rs +from common.configs import CAMERA_FRAME_STREAMING_RATE, CAMERA_NAMES +from common.data_manager import DataManager + + +def camera_thread(data_manager: DataManager) -> None: + """Camera thread - captures RGB images from RealSense and monitors health.""" + print("📷 Camera thread started") + + camera_name = CAMERA_NAMES[0] + pipeline: rs.pipeline | None = None + + # Diagnostic variables + last_frame_number = None + total_dropped_frames = 0 + fps_timer = time.time() + frame_count = 0 + # Store the last 100 frame intervals to check for extreme jitter + intervals = deque(maxlen=100) + last_frame_time = time.time() + + try: + # Configure RealSense pipeline + pipeline = rs.pipeline() + config = rs.config() + + config.enable_stream( + rs.stream.color, + 640, + 480, + rs.format.rgb8, + int(CAMERA_FRAME_STREAMING_RATE), + ) + + print(f"Starting RealSense pipeline at {CAMERA_FRAME_STREAMING_RATE} Hz...") + pipeline.start(config) + + while not data_manager.is_shutdown_requested(): + try: + # wait_for_frames naturally blocks at the target framerate (e.g., 60Hz) + frames = pipeline.wait_for_frames(timeout_ms=500) + except Exception as e: + print(f"⚠️ RealSense wait for frames error (Timeout?): {e}") + continue + + color_frame = frames.get_color_frame() + if not color_frame: + continue + + current_time = time.time() + intervals.append(current_time - last_frame_time) + last_frame_time = current_time + + # --------------------------------------------------------- + # DIAGNOSTICS: Check for dropped frames via hardware ID + # --------------------------------------------------------- + current_frame_number = color_frame.get_frame_number() + + if last_frame_number is not None: + # If frame numbers are not sequential, we missed something in software + drops = (current_frame_number - last_frame_number) - 1 + if drops > 0: + total_dropped_frames += drops + print(f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}") + + last_frame_number = current_frame_number + + # --------------------------------------------------------- + # DIAGNOSTICS: Calculate software-side FPS and Jitter + # --------------------------------------------------------- + frame_count += 1 + if current_time - fps_timer >= 5.0: # Report every 5 seconds + effective_fps = frame_count / (current_time - fps_timer) + + # Calculate jitter (max variance between frame arrivals) + max_interval = max(intervals) * 1000 # in ms + min_interval = min(intervals) * 1000 # in ms + + print(f"📊 Camera Health: {effective_fps:.1f} FPS | " + f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " + f"Total Drops: {total_dropped_frames}") + + # Reset counters for the next 5-second window + fps_timer = current_time + frame_count = 0 + + # --------------------------------------------------------- + # IMAGE PROCESSING + # --------------------------------------------------------- + # color_image = np.asanyarray(color_frame.get_data()) + # color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + # data_manager.set_rgb_image(color_image, camera_name) + + color_image = np.asanyarray(color_frame.get_data()) + color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + data_manager.set_rgb_image(color_image, camera_name) + + # Notice: The time.sleep() has been completely removed. + # wait_for_frames() manages the loop pace perfectly. + + except Exception as e: + print(f"❌ Camera thread error: {e}") + traceback.print_exc() + data_manager.request_shutdown() + finally: + if pipeline is not None: + try: + pipeline.stop() + print(" ✓ RealSense pipeline stopped") + except Exception as e: + print(f"⚠️ Error stopping pipeline: {e}") + print("📷 Camera thread stopped") diff --git a/examples/common/configs.py b/examples/common/configs.py index e65110c..de77001 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -57,8 +57,8 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] # NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] -NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) -NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid +NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick task pruthvi +# NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid # Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index bf3cc3e..9f46ecc 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -15,6 +15,17 @@ from .configs import GRIPPER_NAME, JOINT_NAMES from .one_euro_filter import OneEuroFilterTransform +import threading +import time +import queue +from typing import Any, Callable +import numpy as np + +from .configs import GRIPPER_NAME, JOINT_NAMES +from .states import ( + RobotActivityState, ControllerState, TeleopState, + RobotState, IKState, CameraState +) class RobotActivityState(Enum): """Robot activity state enumeration.""" @@ -98,33 +109,19 @@ def __init__(self) -> None: # Map from camera name -> latest RGB image self.rgb_images: dict[str, np.ndarray] = {} -class DataManager: - """Main state container coordinating all state groups. - This class manages shared data between threads: - - Data collection thread: updates controller data - - IK solver thread: reads controller data, updates joint solutions - - Main thread: reads everything for visualization - Uses separate locks for each state group to reduce contention. - """ +class DataManager: + """Main state container coordinating all state groups.""" def __init__(self) -> None: - """Initialize DataManager with background callback processing.""" self._controller_state = ControllerState() self._teleop_state = TeleopState() self._robot_state = RobotState() self._ik_state = IKState() self._camera_state = CameraState() - # System state self._shutdown_event = threading.Event() - - # Asynchronous processing elements - self._on_change_callback: ( - Callable[[str, dict[str, Any], float], None] | None - ) = None - - # Maxsize 60 matches ~1 second of video frames buffer if disk spikes + self._on_change_callback: Callable[[str, dict[str, Any], float], None] | None = None self._callback_queue: queue.Queue = queue.Queue(maxsize=60) self._worker_thread = threading.Thread( @@ -133,6 +130,8 @@ def __init__(self) -> None: daemon=True ) self._worker_thread.start() + + # ... (Keep the rest of the getter/setter methods like get_rgb_image, etc.) ... def set_on_change_callback( self, on_change_callback: Callable[[str, dict[str, Any], float], None] diff --git a/examples/common/dataset_helpers.py b/examples/common/dataset_helpers.py new file mode 100644 index 0000000..0eab6fe --- /dev/null +++ b/examples/common/dataset_helpers.py @@ -0,0 +1,40 @@ +import neuracore as nc +from neuracore_types import DataType, CrossEmbodimentDescription +from neuracore.core.utils.robot_data_spec_utils import merge_cross_embodiment_description + +def load_and_sync_dataset( + dataset_name: str, + frequency: int, + input_modalities: list[DataType] | None = None, + output_modalities: list[DataType] | None = None, + prefetch_videos: bool = False +): + """Loads a Neuracore dataset and synchronizes the specified modalities across embodiments.""" + print(f"\n🔍 Getting dataset '{dataset_name}' from Neuracore...") + dataset = nc.get_dataset(dataset_name) + + print("🔁 Building cross_embodiment_union for synchronization...") + input_cross_embodiment: CrossEmbodimentDescription = {} + output_cross_embodiment: CrossEmbodimentDescription = {} + + for robot_id in dataset.robot_ids: + full = dataset.get_full_embodiment_description(robot_id) + if input_modalities: + input_cross_embodiment[robot_id] = {dt: full[dt] for dt in input_modalities if dt in full} + if output_modalities: + output_cross_embodiment[robot_id] = {dt: full[dt] for dt in output_modalities if dt in full} + + cross_embodiment_union = merge_cross_embodiment_description( + input_cross_embodiment, output_cross_embodiment + ) + + print("🔁 Synchronizing dataset...") + synced_dataset = dataset.synchronize( + frequency=frequency, + cross_embodiment_union=cross_embodiment_union, + prefetch_videos=prefetch_videos, + max_prefetch_workers=2 if prefetch_videos else 0 + ) + + print(f" ✓ Dataset synchronized: {len(synced_dataset)} episodes") + return synced_dataset \ No newline at end of file diff --git a/examples/common/policy_actions.py b/examples/common/policy_actions.py new file mode 100644 index 0000000..057954a --- /dev/null +++ b/examples/common/policy_actions.py @@ -0,0 +1,269 @@ +"""Extracted actions for running and executing policies across different rollout scripts.""" + +import time +import threading +import numpy as np +from typing import Any, Optional + +from common.configs import ( + JOINT_NAMES, GRIPPER_NAME, MAX_SAFETY_THRESHOLD, MAX_ACTION_ERROR_THRESHOLD, + TARGETING_POSE_TIME_THRESHOLD, POLICY_EXECUTION_RATE, CAMERA_NAMES +) +from common.data_manager import DataManager, RobotActivityState +from common.policy_state import PolicyState +from common.policy_helpers import ( + convert_predictions_to_horizon, log_robot_state_for_policy +) + + +def run_policy( + data_manager: DataManager, policy: Any, policy_state: PolicyState, + visualizer: Optional[Any], input_embodiment_description: dict +) -> bool: + """Handle Run Policy trigger to capture state and get policy prediction.""" + print("Running policy...") + + # Use our helper to log only what the model expects + if not log_robot_state_for_policy(data_manager, input_embodiment_description): + print("✗ No data available to run policy") + return False + + try: + start_time = time.time() + predictions = policy.predict(timeout=60) + inference_time = time.time() - start_time + prediction_horizon = convert_predictions_to_horizon(predictions) + + horizon_length = len(next(iter(prediction_horizon.values()))) if prediction_horizon else 0 + print(f" ✓ Got policy prediction in {inference_time:.2f}s with horizon length {horizon_length}") + + if visualizer: + policy_state.set_execution_ratio(visualizer.get_prediction_ratio()) + + policy_state.set_prediction_horizon(prediction_horizon) + + if visualizer: + visualizer.update_ghost_robot_visibility(True) + + policy_state.set_ghost_robot_playing(True) + policy_state.reset_ghost_action_index() + return True + except Exception as e: + print(f"✗ Failed to get policy prediction: {e}") + return False + + +def start_policy_execution(data_manager: DataManager, policy_state: PolicyState) -> bool: + """Handle Execute Policy trigger to start policy execution.""" + print("Starting policy execution...") + state = data_manager.get_robot_activity_state() + + if state == RobotActivityState.POLICY_CONTROLLED and not policy_state.get_continuous_play_active(): + print("⚠️ Policy execution already in progress") + return False + if state == RobotActivityState.DISABLED: + print("⚠️ Cannot execute policy: Robot is disabled") + return False + + if policy_state.get_prediction_horizon_length() == 0: + print("⚠️ No prediction horizon available.") + return False + + current_joint_angles = data_manager.get_current_joint_angles() + if current_joint_angles is None: + return False + + prediction_horizon = policy_state.get_prediction_horizon() + first_targets = np.degrees([prediction_horizon[jn][0] for jn in JOINT_NAMES]) + + # Safety check: Prevent wild jumps + if np.any(np.abs(current_joint_angles - first_targets) > MAX_SAFETY_THRESHOLD): + print("⚠️ Cannot execute policy: Robot too far from first predicted action") + return False + + policy_state.set_ghost_robot_playing(False) + data_manager.set_teleop_state(False, None, None) + policy_state.start_policy_execution() + + if policy_state.get_locked_prediction_horizon_length() == 0: + policy_state.end_policy_execution() + return False + + data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) + return True + + +def end_policy_play(data_manager: DataManager, policy_state: PolicyState, visualizer: Optional[Any], status_msg: str) -> None: + """End continuous play and update system state.""" + if policy_state.get_continuous_play_active(): + policy_state.set_continuous_play_active(False) + + if visualizer: + visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) + visualizer.update_play_policy_button_status(False) + visualizer.update_policy_status(status_msg) + + policy_state.end_policy_execution() + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + data_manager.set_teleop_state(False, None, None) + + +def continuous_prediction_worker(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode): + """Background thread for continuous receding horizon execution.""" + colors = [(1.0, 0.65, 0.0, 0.25), (0.0, 1.0, 0.0, 0.25), (1.0, 0.0, 0.0, 0.25), (0.0, 0.0, 1.0, 0.25)] + c_idx = 0 + + print(f"\n🚀 Bootstrapping initial trajectory in '{continuous_mode}' mode...") + if run_policy(data_manager, policy, policy_state, visualizer, input_emb): + start_policy_execution(data_manager, policy_state) + + while policy_state.get_continuous_play_active(): + if policy_state.get_locked_prediction_horizon_length() == 0: + time.sleep(0.01); continue + + if continuous_mode == "pipeline": + # Predict next horizon in the background while moving + success = run_policy(data_manager, policy, policy_state, visualizer, input_emb) + if not success: + time.sleep(0.05); continue + + # Wait until current trajectory is almost finished before hot-swapping + while policy_state.get_continuous_play_active(): + rem = policy_state.get_locked_prediction_horizon_length() - policy_state.get_execution_action_index() + if rem <= 5 or policy_state.get_locked_prediction_horizon_length() == 0: + break + time.sleep(0.01) + + elif continuous_mode == "sequential": + # Wait for current trajectory to fully finish before querying network + while policy_state.get_continuous_play_active(): + if policy_state.get_execution_action_index() >= policy_state.get_locked_prediction_horizon_length(): + break + time.sleep(0.01) + + if not policy_state.get_continuous_play_active(): break + + success = run_policy(data_manager, policy, policy_state, visualizer, input_emb) + if not success: + time.sleep(0.05); continue + + if not policy_state.get_continuous_play_active(): break + + policy_state.end_policy_execution() + if start_policy_execution(data_manager, policy_state): + c_idx = (c_idx + 1) % len(colors) + if visualizer: visualizer.set_ghost_robot_color(colors[c_idx]) + else: + time.sleep(0.01) + + +def play_policy(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode="pipeline"): + """Toggle for starting/stopping continuous policy mode.""" + if not policy_state.get_continuous_play_active(): + print(f"▶️ Starting {continuous_mode.capitalize()} Mode...") + policy_state.set_continuous_play_active(True) + if visualizer: visualizer.update_play_policy_button_status(True) + threading.Thread( + target=continuous_prediction_worker, + args=(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode), + daemon=True + ).start() + else: + print("⏹️ Stopping continuous policy execution...") + end_policy_play(data_manager, policy_state, visualizer, "Policy execution stopped") + + +def policy_execution_thread(policy, data_manager, policy_state, robot_controller, visualizer, input_emb): + """The thread that continuously reads the locked horizon and sends joint commands.""" + dt_execution = 1.0 / POLICY_EXECUTION_RATE + last_vis_update = 0.0 + + while True: + start_time = time.time() + + if data_manager.get_robot_activity_state() == RobotActivityState.POLICY_CONTROLLED: + locked_horizon = policy_state.get_locked_prediction_horizon() + exec_idx = policy_state.get_execution_action_index() + locked_len = policy_state.get_locked_prediction_horizon_length() + + if exec_idx < locked_len: + current_angles = data_manager.get_current_joint_angles() + + # Check target pose tracking if in pose-execution mode + if exec_idx > 0 and current_angles is not None and policy_state.get_execution_mode() == PolicyState.ExecutionMode.TARGETING_POSE: + t_start = time.time() + while (time.time() - t_start) < TARGETING_POSE_TIME_THRESHOLD: + prev_targs = np.degrees([locked_horizon[jn][exec_idx - 1] for jn in JOINT_NAMES]) + if np.any(np.abs(current_angles - prev_targs) <= MAX_ACTION_ERROR_THRESHOLD): + break + time.sleep(0.001) + + if all(jn in locked_horizon for jn in JOINT_NAMES): + targs_deg = np.degrees([locked_horizon[jn][exec_idx] for jn in JOINT_NAMES]) + data_manager.set_target_joint_angles(targs_deg) + if robot_controller.is_robot_enabled(): + robot_controller.set_target_joint_angles(targs_deg) + + if GRIPPER_NAME in locked_horizon: + robot_controller.set_gripper_open_value(locked_horizon[GRIPPER_NAME][exec_idx]) + + policy_state.increment_execution_action_index() + if visualizer: + visualizer.update_policy_status(f"Executing: {exec_idx + 1}/{locked_len}") + else: + if not policy_state.get_continuous_play_active(): + end_policy_play(data_manager, policy_state, visualizer, "Execution completed") + elif all(jn in locked_horizon for jn in JOINT_NAMES) and robot_controller.is_robot_enabled(): + # Hold last position while waiting for background thread to swap horizon + robot_controller.set_target_joint_angles(np.degrees([locked_horizon[jn][-1] for jn in JOINT_NAMES])) + + # Throttle visualizer updates to 30Hz + if visualizer and (time.time() - last_vis_update >= 1.0 / 30.0): + _update_visualization(data_manager, policy_state, visualizer) + last_vis_update = time.time() + + time.sleep(max(0, dt_execution - (time.time() - start_time))) + + +def _update_visualization(data_manager: DataManager, policy_state: PolicyState, visualizer: Any) -> None: + """Sync the visualizer UI with the DataManager's internal state.""" + current_joints = data_manager.get_current_joint_angles() + if current_joints is not None: + visualizer.update_robot_pose(np.radians(current_joints)) + + rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) + if rgb_image is not None: + visualizer.update_rgb_image(rgb_image) + + state = data_manager.get_robot_activity_state() + if state == RobotActivityState.POLICY_CONTROLLED: + visualizer.update_ghost_robot_visibility(True) + t_joints = data_manager.get_target_joint_angles() + if t_joints is not None: visualizer.update_ghost_robot_pose(np.radians(t_joints)) + visualizer.set_run_policy_button_disabled(True) + visualizer.set_play_policy_button_disabled(False) + + elif state == RobotActivityState.ENABLED and data_manager.get_teleop_active(): + visualizer.update_ghost_robot_visibility(True) + t_joints = data_manager.get_target_joint_angles() + if t_joints is not None: visualizer.update_ghost_robot_pose(np.radians(t_joints)) + + elif policy_state.get_ghost_robot_playing() and policy_state.get_prediction_horizon_length() > 0: + visualizer.set_start_policy_execution_button_disabled(False) + visualizer.update_ghost_robot_visibility(True) + g_idx = policy_state.get_ghost_action_index() + horizon = policy_state.get_prediction_horizon() + + if g_idx < policy_state.get_prediction_horizon_length() and all(jn in horizon for jn in JOINT_NAMES): + visualizer.update_ghost_robot_pose(np.array([horizon[jn][g_idx] for jn in JOINT_NAMES])) + policy_state.set_ghost_action_index((g_idx + 1) % policy_state.get_prediction_horizon_length()) + else: + policy_state.reset_ghost_action_index() + else: + visualizer.update_ghost_robot_visibility(False) + has_horizon = policy_state.get_prediction_horizon_length() > 0 + enabled = state == RobotActivityState.ENABLED + + visualizer.set_start_policy_execution_button_disabled(not (enabled and has_horizon)) + visualizer.set_run_policy_button_disabled(not enabled) + visualizer.set_play_policy_button_disabled(not enabled) \ No newline at end of file diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index e8a3602..b2e47ab 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -1,876 +1,29 @@ #!/usr/bin/env python3 """Shared robot visualizer for Piper robot demos. - -This module provides a clean interface for visualizing robot state using Viser, -encapsulating all the repeated setup, GUI controls, and update logic. +This module acts as a facade, delegating 3D rendering to RobotVisualizerCore +and 2D UI elements to RobotVisualizerGUI. """ -from typing import Any, Callable - -import numpy as np -import viser -import yourdfpy -from scipy.spatial.transform import Rotation -from viser.extras import ViserUrdf - +from .visualizer_core import RobotVisualizerCore +from .visualizer_gui import RobotVisualizerGUI class RobotVisualizer: - """Shared visualizer for robot demos. - - Encapsulates viser server setup, GUI controls, and update logic. - """ + """Shared visualizer facade for robot demos.""" def __init__(self, urdf_path: str) -> None: - """Initialize the visualizer. - - Args: - urdf_path: Path to URDF file for robot visualization - """ - # Initialize viser server - self._server = viser.ViserServer() - self._server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) - - # Load URDF for visualization - urdf = yourdfpy.URDF.load(urdf_path) - self._urdf_vis = ViserUrdf(self._server, urdf, root_node_name="/robot_actual") - - ghost_urdf = yourdfpy.URDF.load(urdf_path) - # Ghost robot with semi-transparent blue color to make it visually distinct - self._ghost_robot_urdf = ViserUrdf( - self._server, - ghost_urdf, - root_node_name="/robot_ghost", - mesh_color_override=(1.0, 0.65, 0.0, 0.25), # Orange with 25% opacity - ) - - # GUI handles (initialized as None, created on demand) - all private - self._timing_handle = None - self._joint_angles_handle = None - self._robot_status_handle = None - self._teleop_status_handle = None - self._controller_status_handle = None - self._gripper_status_handle = None - - # Pink parameter handles - self._position_weight_handle = None - self._orientation_weight_handle = None - self._frame_task_gain_handle = None - self._lm_damping_handle = None - self._damping_weight_handle = None - self._solver_damping_value_handle = None - self._posture_cost_handles: list[Any] = [] - - # Robot control handles - self._enable_robot_handle = None - self._disable_robot_handle = None - self._emergency_stop_handle = None - self._go_home_button = None - self._toggle_robot_enabled_status_button = None - - # Teleop-specific handles - self._grip_value_handle = None - self._trigger_value_handle = None - - # Rate control handles - self._controller_min_cutoff_handle = None - self._controller_beta_handle = None - self._controller_d_cutoff_handle = None - - self._translation_scale_handle = None - self._rotation_scale_handle = None - - # Visualization handles - self._controller_handle = None - self._target_frame_handle = None - self._rgb_image_handle = None - - # Policy-related handles - self._policy_status_handle = None - self._prediction_ratio_handle = None - self._policy_execution_rate_handle = None - self._robot_rate_handle = None - self._execution_mode_dropdown = None - self._run_policy_button = None - self._start_policy_execution_button = None - self._play_policy_button = None - - # Internal state - self._ema_timing = 0.001 - - def add_basic_controls(self) -> None: - """Add basic GUI controls (timing, joint angles).""" - self._timing_handle = self._server.gui.add_number( - "IK Solve Time (ms)", 0.001, disabled=True - ) - self._joint_angles_handle = self._server.gui.add_text( - "Joint Angles", "Waiting for IK solution..." - ) - - def add_robot_status_controls(self) -> None: - """Add robot status display controls.""" - self._robot_status_handle = self._server.gui.add_text( - "Robot Status", "Initializing..." - ) - - def add_teleop_controls(self) -> None: - """Add teleoperation-specific controls.""" - self._grip_value_handle = self._server.gui.add_number( - "Grip Value", 0.0, disabled=True - ) - self._trigger_value_handle = self._server.gui.add_number( - "Trigger Value (Gripper)", 0.0, disabled=True - ) - self._teleop_status_handle = self._server.gui.add_text( - "Teleop Status", "Inactive" - ) - self._controller_status_handle = self._server.gui.add_text( - "Controller Status", "Waiting..." - ) - - def add_gripper_status_controls(self) -> None: - """Add gripper status display controls.""" - self._gripper_status_handle = self._server.gui.add_text( - "Gripper Status", "Open (0%)" - ) - - def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None: - """Add an RGB image placeholder in the desired GUI position. - - This reserves a fixed location for the camera feed; the actual image - data will be injected later via update_rgb_image. - """ - if self._rgb_image_handle is not None: - return - - dummy_image = np.zeros((height, width, 3), dtype=np.uint8) - self._rgb_image_handle = self._server.gui.add_image( - dummy_image, - label="RGB Camera", - format="jpeg", - jpeg_quality=85, - ) - - def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: - """Show or update RGB camera image in the Viser GUI.""" - if rgb_image is None: - return - if self._rgb_image_handle is None: - # Fallback: if placeholder wasn't created, create it now - self.add_rgb_image_placeholder( - height=rgb_image.shape[0], width=rgb_image.shape[1] - ) - rgb_handle = self._rgb_image_handle - if rgb_handle is None: - return - rgb_handle.image = rgb_image - - def add_homing_controls(self) -> None: - """Add homing controls.""" - self._go_home_button = self._server.gui.add_button("Go Home") - - def add_robot_control_buttons(self) -> None: - """Add robot control buttons (enable, disable, emergency stop). - - Note: For homing functionality, use add_homing_controls() instead. - """ - self._enable_robot_handle = self._server.gui.add_button("Enable Robot") - self._disable_robot_handle = self._server.gui.add_button("Disable Robot") - self._emergency_stop_handle = self._server.gui.add_button("Emergency Stop") - - def add_toggle_robot_enabled_status_button(self) -> None: - """Add toggle robot enabled status button. - - This creates a single button that toggles between "Enable Robot" and "Disable Robot" - based on the current robot state. - """ - self._toggle_robot_enabled_status_button = self._server.gui.add_button( - "Enable Robot" - ) - - def update_toggle_robot_enabled_status(self, enabled: bool) -> None: - """Update toggle robot enabled status button label based on robot state. - - Args: - enabled: Whether robot is currently enabled - """ - if self._toggle_robot_enabled_status_button is not None: - self._toggle_robot_enabled_status_button.label = ( - "Enable Robot" if not enabled else "Disable Robot" - ) - - def set_toggle_robot_enabled_status_callback( - self, callback: Callable[[], Any] - ) -> None: - """Set callback for toggle robot enabled status button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._toggle_robot_enabled_status_button is not None: - self._toggle_robot_enabled_status_button.on_click(lambda _: callback()) - - def add_pink_parameter_controls( - self, - position_cost: float, - orientation_cost: float, - frame_task_gain: float, - lm_damping: float, - damping_cost: float, - solver_damping_value: float, - posture_cost_vector: list[float], - ) -> None: - """Add Pink IK parameter controls. - - Args: - position_cost: Initial position cost value - orientation_cost: Initial orientation cost value - frame_task_gain: Initial frame task gain value - lm_damping: Initial LM damping value - damping_cost: Initial damping cost value - solver_damping_value: Initial solver damping value - posture_cost_vector: Initial posture cost vector (one value per joint) - """ - self._position_weight_handle = self._server.gui.add_number( - "Position Weight", position_cost, min=0.0, max=10.0, step=0.1 - ) - self._orientation_weight_handle = self._server.gui.add_number( - "Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01 - ) - self._frame_task_gain_handle = self._server.gui.add_number( - "Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1 - ) - self._lm_damping_handle = self._server.gui.add_number( - "LM Damping", lm_damping, min=0.0, max=5.0, step=0.01 - ) - self._damping_weight_handle = self._server.gui.add_number( - "Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01 - ) - self._solver_damping_value_handle = self._server.gui.add_number( - "Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001 - ) - - # Posture cost controls (one per joint) - self._posture_cost_handles = [] - for i in range(len(posture_cost_vector)): - handle = self._server.gui.add_number( - f"Posture Cost J{i+1}", - posture_cost_vector[i], - min=0.0, - max=1.0, - step=0.01, - ) - self._posture_cost_handles.append(handle) - - def add_controller_filter_controls( - self, - initial_min_cutoff: float, - initial_beta: float, - initial_d_cutoff: float, - ) -> None: - """Add 1€ Filter parameter controls for controller. - - Args: - initial_min_cutoff: Initial minimum cutoff frequency - initial_beta: Initial speed coefficient - initial_d_cutoff: Initial derivative cutoff frequency - """ - self._controller_min_cutoff_handle = self._server.gui.add_number( - "Controller Min Cutoff", - initial_min_cutoff, - min=0.01, - max=10.0, - step=0.01, - ) - self._controller_beta_handle = self._server.gui.add_number( - "Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01 - ) - self._controller_d_cutoff_handle = self._server.gui.add_number( - "Controller D Cutoff", - initial_d_cutoff, - min=0.01, - max=10.0, - step=0.01, - ) - - def add_scaling_controls( - self, initial_translation_scale: float, initial_rotation_scale: float - ) -> None: - """Add scaling factor controls for translation and rotation. - - Args: - initial_translation_scale: Initial translation scale factor - initial_rotation_scale: Initial rotation scale factor - """ - self._translation_scale_handle = self._server.gui.add_number( - "Translation Scale", - initial_translation_scale, - min=0.1, - max=10.0, - step=0.001, - ) - self._rotation_scale_handle = self._server.gui.add_number( - "Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001 - ) - - def add_controller_visualization(self) -> None: - """Add controller transform visualization.""" - self._controller_handle = self._server.scene.add_transform_controls( - "/controller", - scale=0.15, - position=(0, 0, 0), - wxyz=(1, 0, 0, 0), - ) - - def add_target_frame_visualization(self) -> None: - """Add target/goal frame visualization.""" - self._target_frame_handle = self._server.scene.add_frame( - "/target_goal", axes_length=0.1, axes_radius=0.003 - ) - - def update_robot_pose(self, joint_config: np.ndarray) -> None: - """Update robot visualization from joint configuration. - - Args: - joint_config: Joint angles in radians - """ - self._urdf_vis.update_cfg(joint_config) - - def update_joint_angles_display( - self, joint_config: np.ndarray, show_gripper: bool = False - ) -> None: - """Update joint angles display. - - Args: - joint_config: Joint angles in radians - show_gripper: Whether to show gripper joints (joints 7&8) - """ - if self._joint_angles_handle is None: - return - - joint_angles_str = "Joint Angles (rad):\n" - joint_angles_deg = np.degrees(joint_config) - num_joints = len(joint_config) if show_gripper else 6 - - for i in range(num_joints): - angle_rad = joint_config[i] - angle_deg = joint_angles_deg[i] - joint_type = "Robot" if i < 6 else "Gripper" - label = f"Joint {i+1} ({joint_type})" if show_gripper else f"J{i+1}" - joint_angles_str += f" {label}: {angle_rad:.3f} rad ({angle_deg:.1f}°)\n" - - self._joint_angles_handle.value = joint_angles_str - - def update_timing(self, solve_time_ms: float) -> None: - """Update timing display with exponential moving average. - - Args: - solve_time_ms: IK solve time in milliseconds - """ - if self._timing_handle is None: - return - - self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms - self._timing_handle.value = self._ema_timing - - def update_robot_status(self, status: str) -> None: - """Update robot status display. - - Args: - status: Status string to display - """ - if self._robot_status_handle is not None: - self._robot_status_handle.value = status - - def update_teleop_status(self, active: bool) -> None: - """Update teleop status display. - - Args: - active: Whether teleop is active - """ - if self._teleop_status_handle is not None: - self._teleop_status_handle.value = ( - "Teleop Status: Active" if active else "Teleop Status: Inactive" - ) - - def update_controller_status_display( - self, position: np.ndarray | None, connected: bool = True - ) -> None: - """Update controller status display. - - Args: - position: Controller position (3D array) or None - connected: Whether controller is connected - """ - if self._controller_status_handle is None: - return - - if connected and position is not None: - controller_status_str = "Controller Status:\n" - controller_status_str += f" Position: [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}]\n" - controller_status_str += " Connected: ✓\n" - self._controller_status_handle.value = controller_status_str - else: - self._controller_status_handle.value = "Controller Status:\n Connected: ✗" - - def update_gripper_status( - self, trigger_value: float, robot_enabled: bool = True - ) -> None: - """Update gripper status display. - - Args: - trigger_value: Trigger value (0.0 = open, 1.0 = closed) - robot_enabled: Whether robot is enabled - """ - if self._gripper_status_handle is None: - return - - gripper_closed_percent = trigger_value * 100.0 - if trigger_value > 0.9: - gripper_state = "Closed" - elif trigger_value > 0.1: - gripper_state = "Closing" - else: - gripper_state = "Open" - - status = f"Gripper: {gripper_state} ({gripper_closed_percent:.0f}% closed)" - if not robot_enabled: - status += " [Disabled]" - - self._gripper_status_handle.value = status - - def update_controller_visualization(self, transform: np.ndarray | None) -> None: - """Update controller transform visualization. - - Args: - transform: 4x4 transformation matrix or None - """ - if self._controller_handle is None or transform is None: - return - - controller_pos = transform[:3, 3] - controller_rot = Rotation.from_matrix(transform[:3, :3]) - controller_quat_xyzw = controller_rot.as_quat() - controller_quat_wxyz = [ - controller_quat_xyzw[3], - controller_quat_xyzw[0], - controller_quat_xyzw[1], - controller_quat_xyzw[2], - ] - - self._controller_handle.position = tuple(controller_pos) - self._controller_handle.wxyz = tuple(controller_quat_wxyz) - - def update_target_visualization(self, transform: np.ndarray | None) -> None: - """Update target/goal frame visualization. - - Args: - transform: 4x4 transformation matrix or None - """ - if self._target_frame_handle is None or transform is None: - return - - target_pos = transform[:3, 3] - target_rot = Rotation.from_matrix(transform[:3, :3]) - target_quat_xyzw = target_rot.as_quat() - target_quat_wxyz = [ - target_quat_xyzw[3], - target_quat_xyzw[0], - target_quat_xyzw[1], - target_quat_xyzw[2], - ] - - self._target_frame_handle.position = tuple(target_pos) - self._target_frame_handle.wxyz = tuple(target_quat_wxyz) - - def get_pink_parameters(self) -> dict: - """Get Pink IK parameters from GUI controls. - - Returns: - Dictionary with parameter values - - Raises: - ValueError: If Pink parameter controls not initialized - """ - if not self._posture_cost_handles: - raise ValueError("Pink parameter controls not initialized") - - if ( - self._position_weight_handle is None - or self._orientation_weight_handle is None - or self._frame_task_gain_handle is None - or self._lm_damping_handle is None - or self._damping_weight_handle is None - or self._solver_damping_value_handle is None - ): - raise ValueError("Pink parameter controls not initialized") - - posture_cost_vector = np.array( - [handle.value for handle in self._posture_cost_handles] - ) - - params = { - "position_cost": self._position_weight_handle.value, - "orientation_cost": self._orientation_weight_handle.value, - "frame_task_gain": self._frame_task_gain_handle.value, - "lm_damping": self._lm_damping_handle.value, - "damping_cost": self._damping_weight_handle.value, - "solver_damping_value": self._solver_damping_value_handle.value, - "posture_cost_vector": posture_cost_vector, - } - return params - - def get_controller_filter_params(self) -> tuple[float, float, float]: - """Get 1€ Filter parameters from GUI. - - Returns: - Tuple of (min_cutoff, beta, d_cutoff) - - Raises: - ValueError: If controller filter controls not initialized - """ - if ( - self._controller_min_cutoff_handle is None - or self._controller_beta_handle is None - or self._controller_d_cutoff_handle is None - ): - raise ValueError("Controller filter controls not initialized") - return ( - self._controller_min_cutoff_handle.value, - self._controller_beta_handle.value, - self._controller_d_cutoff_handle.value, - ) - - def get_translation_scale(self) -> float: - """Get translation scale value from GUI. - - Returns: - Translation scale value - - Raises: - ValueError: If scaling controls not initialized - """ - if self._translation_scale_handle is None: - raise ValueError("Scaling controls not initialized") - return self._translation_scale_handle.value - - def get_rotation_scale(self) -> float: - """Get rotation scale value from GUI. - - Returns: - Rotation scale value - - Raises: - ValueError: If scaling controls not initialized - """ - if self._rotation_scale_handle is None: - raise ValueError("Scaling controls not initialized") - return self._rotation_scale_handle.value - - def set_grip_value(self, value: float) -> None: - """Set grip value display. - - Args: - value: Grip value (0.0-1.0) - - Raises: - ValueError: If grip value control not initialized - """ - if self._grip_value_handle is None: - raise ValueError("Grip value control not initialized") - self._grip_value_handle.value = value - - def set_trigger_value(self, value: float) -> None: - """Set trigger value display. - - Args: - value: Trigger value (0.0-1.0) - - Raises: - ValueError: If trigger value control not initialized - """ - if self._trigger_value_handle is None: - raise ValueError("Trigger value control not initialized") - self._trigger_value_handle.value = value - - def set_joint_angles_text(self, text: str) -> None: - """Set joint angles text display. - - Args: - text: Text to display - - Raises: - ValueError: If joint angles control not initialized - """ - if self._joint_angles_handle is None: - raise ValueError("Joint angles control not initialized") - self._joint_angles_handle.value = text - - def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None: - """Update ghost robot visualization from joint configuration. - - Args: - joint_config: Joint angles in radians - """ - if self._ghost_robot_urdf is not None: - self._ghost_robot_urdf.update_cfg(joint_config) - - def update_ghost_robot_visibility(self, flag: bool) -> None: - """Update ghost robot visibility. - - Args: - flag: Whether to show the ghost robot - """ - if self._ghost_robot_urdf is not None: - self._ghost_robot_urdf.show_visual = flag - - def add_policy_controls( - self, - initial_prediction_ratio: float = 0.8, - initial_policy_rate: float = 200.0, - initial_robot_rate: float = 200.0, - initial_execution_mode: str = "targeting_time", - ) -> None: - """Add policy-related GUI controls. - - Args: - initial_prediction_ratio: Initial prediction horizon execution ratio (0.0-1.0) - initial_policy_rate: Initial policy execution rate in Hz - initial_robot_rate: Initial robot rate in Hz - initial_execution_mode: Initial execution mode ("targeting_time" or "targeting_pose") - """ - self._policy_status_handle = self._server.gui.add_text( - "Policy Status", "Ready - Press Right Joystick to get prediction" - ) - - self._prediction_ratio_handle = self._server.gui.add_number( - "Prediction Horizon Execution Ratio", - initial_prediction_ratio, - min=0.0, - max=1.0, - step=0.01, - ) - - self._policy_execution_rate_handle = self._server.gui.add_number( - "Policy Execution Rate", - initial_policy_rate, - min=1.0, - max=200.0, - step=1.0, - ) - - self._robot_rate_handle = self._server.gui.add_number( - "Robot Rate", - initial_robot_rate, - min=1.0, - max=200.0, - step=1.0, - ) - - self._execution_mode_dropdown = self._server.gui.add_dropdown( - "Execution Mode", - options=["targeting_time", "targeting_pose"], - initial_value=initial_execution_mode, - ) - - def add_policy_buttons(self) -> None: - """Add policy control buttons.""" - self._run_policy_button = self._server.gui.add_button("Run Policy (Preview)") - self._start_policy_execution_button = self._server.gui.add_button( - "Execute Policy (Run Preview)" - ) - self._play_policy_button = self._server.gui.add_button("Continuous Receding Horizon") - - def update_policy_status(self, status: str) -> None: - """Update policy status display. - - Args: - status: Status string to display - """ - if self._policy_status_handle is not None: - self._policy_status_handle.value = status - - def get_prediction_ratio(self) -> float: - """Get prediction horizon execution ratio from GUI. - - Returns: - Prediction ratio (0.0-1.0) - - Raises: - ValueError: If policy controls not initialized - """ - if self._prediction_ratio_handle is None: - raise ValueError("Policy controls not initialized") - return self._prediction_ratio_handle.value - - def get_policy_execution_rate(self) -> float: - """Get policy execution rate from GUI. - - Returns: - Policy execution rate in Hz - - Raises: - ValueError: If policy controls not initialized - """ - if self._policy_execution_rate_handle is None: - raise ValueError("Policy controls not initialized") - return self._policy_execution_rate_handle.value - - def get_robot_rate(self) -> float: - """Get robot rate from GUI. - - Returns: - Robot rate in Hz - - Raises: - ValueError: If policy controls not initialized - """ - if self._robot_rate_handle is None: - raise ValueError("Policy controls not initialized") - return self._robot_rate_handle.value - - def get_execution_mode(self) -> str: - """Get execution mode from GUI. - - Returns: - Execution mode string ("targeting_time" or "targeting_pose") - - Raises: - ValueError: If policy controls not initialized - """ - if self._execution_mode_dropdown is None: - raise ValueError("Policy controls not initialized") - return self._execution_mode_dropdown.value - - def get_ghost_robot_visibility(self) -> bool: - """Get ghost robot visibility. - - Returns: - Whether the ghost robot is visible - """ - if self._ghost_robot_urdf is not None: - return self._ghost_robot_urdf.show_visual - return False - - def set_run_policy_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Run Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._run_policy_button is not None: - self._run_policy_button.on_click(lambda _: callback()) - - def set_start_policy_execution_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Execute Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._start_policy_execution_button is not None: - self._start_policy_execution_button.on_click(lambda _: callback()) - - - def set_play_policy_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Play Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._play_policy_button is not None: - self._play_policy_button.on_click(lambda _: callback()) - - def set_execution_mode_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for execution mode dropdown. - - Args: - callback: Callback function to call when dropdown value changes - """ - if self._execution_mode_dropdown is not None: - self._execution_mode_dropdown.on_update(lambda _: callback()) - - def set_enable_robot_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Enable Robot button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._enable_robot_handle is not None: - self._enable_robot_handle.on_click(lambda _: callback()) - - def set_disable_robot_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Disable Robot button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._disable_robot_handle is not None: - self._disable_robot_handle.on_click(lambda _: callback()) - - def set_emergency_stop_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Emergency Stop button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._emergency_stop_handle is not None: - self._emergency_stop_handle.on_click(lambda _: callback()) - - def set_go_home_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Go Home button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._go_home_button is not None: - self._go_home_button.on_click(lambda _: callback()) - - def set_run_policy_button_disabled(self, disabled: bool) -> None: - """Set Run Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._run_policy_button is not None: - self._run_policy_button.disabled = disabled - - def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: - """Set Execute Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._start_policy_execution_button is not None: - self._start_policy_execution_button.disabled = disabled - - - def set_play_policy_button_disabled(self, disabled: bool) -> None: - """Set Play Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._play_policy_button is not None: - self._play_policy_button.disabled = disabled - - def update_play_policy_button_status(self, active: bool) -> None: - """Update play policy button label based on active state. - - Args: - active: Whether continuous play is currently active - """ - if self._play_policy_button is not None: - self._play_policy_button.label = ( - "Stop Continuous Horizon" if active else "Continuous Receding Horizon" - ) - def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: - """Set the color of the ghost robot. - - Args: - color: RGBA tuple (0.0 to 1.0) - """ - if self._ghost_robot_urdf is not None: - self._ghost_robot_urdf.mesh_color_override = color - - def stop(self) -> None: - """Stop the visualizer server.""" - self._server.stop() + """Initialize the visualizer components.""" + self._core = RobotVisualizerCore(urdf_path) + self._gui = RobotVisualizerGUI(self._core.server) + + def __getattr__(self, name: str): + """ + Dynamically delegates method calls to the Core or GUI modules. + If a top-level script calls `visualizer.add_basic_controls()`, + this routes it to `_gui.add_basic_controls()`. + """ + if hasattr(self._core, name): + return getattr(self._core, name) + if hasattr(self._gui, name): + return getattr(self._gui, name) + + raise AttributeError(f"'RobotVisualizer' object has no attribute '{name}'") \ No newline at end of file diff --git a/examples/common/shared_actions.py b/examples/common/shared_actions.py new file mode 100644 index 0000000..9526427 --- /dev/null +++ b/examples/common/shared_actions.py @@ -0,0 +1,96 @@ +from common.data_manager import DataManager, RobotActivityState +from piper_controller import PiperController +import subprocess +import neuracore as nc +import numpy as np +import traceback +from typing import Any +from common.configs import JOINT_NAMES, GRIPPER_LOGGING_NAME + +def toggle_robot_enabled(data_manager: DataManager, robot_controller: PiperController, visualizer=None): + """Safely toggles the robot between ENABLED and DISABLED states.""" + state = data_manager.get_robot_activity_state() + + if state == RobotActivityState.ENABLED: + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + if robot_controller: + robot_controller.graceful_stop() + data_manager.set_teleop_state(False, None, None) + if visualizer: + visualizer.update_toggle_robot_enabled_status(False) + print("✓ 🔴 Robot disabled") + + elif state in (RobotActivityState.DISABLED, RobotActivityState.HOMING): + if not robot_controller: + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("✓ 🟢 Robot enabled (Headless)") + return + + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + if visualizer: + visualizer.update_toggle_robot_enabled_status(True) + print("✓ 🟢 Robot enabled") + else: + print("✗ Failed to enable robot") + + +def move_robot_home(data_manager: DataManager, robot_controller: PiperController): + """Safely commands the robot to return to its home position.""" + state = data_manager.get_robot_activity_state() + + if state == RobotActivityState.ENABLED: + print("🏠 Moving to home position...") + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + data_manager.set_teleop_state(False, None, None) + + if robot_controller: + if not robot_controller.move_to_home(): + print("✗ Failed to initiate home move") + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("✓ 🏠 Robot homed (Headless)") + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("⚠️ Robot is not enabled, cannot go home") + + + + +def toggle_recording(play_audio: bool = False) -> None: + """Safely start or stop a Neuracore data recording session.""" + if not nc.is_recording(): + try: + nc.start_recording() + print("✓ 🔴 Recording started") + if play_audio: + subprocess.Popen(["play", "-q", "-n", "synth", "0.3", "sine", "880"], stderr=subprocess.DEVNULL) + except Exception as e: + print(f"✗ Failed to start recording: {e}") + else: + try: + nc.stop_recording() + print("✓ ⏹️ Recording stopped") + if play_audio: + subprocess.Popen(["play", "-q", "-n", "synth", "0.3", "sine", "440"], stderr=subprocess.DEVNULL) + except Exception as e: + print(f"✗ Failed to stop recording: {e}") + +def neuracore_logging_callback(name: str, payload: dict[str, Any], timestamp: float) -> None: + """Unified callback to map DataManager state changes to Neuracore log streams.""" + try: + if name == "log_joint_positions": + nc.log_joint_positions(payload, timestamp=timestamp) + elif name == "log_joint_torques": + nc.log_joint_torques(payload, timestamp=timestamp) + elif name == "log_joint_target_positions": + nc.log_joint_target_positions(payload, timestamp=timestamp) + elif name == "log_parallel_gripper_open_amounts": + nc.log_parallel_gripper_open_amounts(payload, timestamp=timestamp) + elif name == "log_parallel_gripper_target_open_amounts": + nc.log_parallel_gripper_target_open_amounts(payload, timestamp=timestamp) + elif name == "log_rgb": + camera_name = next(iter(payload)) + nc.log_rgb(camera_name, payload[camera_name], timestamp=timestamp) + except Exception as e: + print(f"⚠️ Logging failed for {name}: {e}") \ No newline at end of file diff --git a/examples/common/states.py b/examples/common/states.py new file mode 100644 index 0000000..0901703 --- /dev/null +++ b/examples/common/states.py @@ -0,0 +1,61 @@ +import threading +from enum import Enum +import numpy as np +from common.one_euro_filter import OneEuroFilterTransform + +class RobotActivityState(Enum): + """Robot activity state enumeration.""" + ENABLED = "ENABLED" + HOMING = "HOMING" + DISABLED = "DISABLED" + POLICY_CONTROLLED = "POLICY_CONTROLLED" + +class ControllerState: + """Controller input state - Quest Reader writes, IK/Joint reads.""" + def __init__(self) -> None: + self._lock = threading.Lock() + self.min_cutoff: float = 1.0 + self.beta: float = 0.0 + self.d_cutoff: float = 1.0 + self.transform_raw: np.ndarray | None = None + self.transform: np.ndarray | None = None + self.grip_value: float = 0.0 + self.trigger_value: float = 0.0 + self._filter: OneEuroFilterTransform | None = None + +class TeleopState: + """Teleop activation state - manages teleop start/stop.""" + def __init__(self) -> None: + self._lock = threading.Lock() + self.active: bool = False + self.controller_initial_transform: np.ndarray | None = None + self.robot_initial_transform: np.ndarray | None = None + self.translation_scale: float = 1.0 + self.rotation_scale: float = 1.0 + self.slow_scaling_mode_enabled: bool = False + +class RobotState: + """Current robot state - joint angles, end effector pose, activity state.""" + def __init__(self) -> None: + self._lock = threading.Lock() + self.joint_angles: np.ndarray | None = None + self.joint_torques: np.ndarray | None = None + self.end_effector_pose: np.ndarray | None = None + self.current_gripper_open_value: float | None = None + self.target_gripper_open_value: float | None = None + self.activity_state: RobotActivityState = RobotActivityState.DISABLED + +class IKState: + """IK solution state - target joint angles, pose, metrics.""" + def __init__(self) -> None: + self._lock = threading.Lock() + self.target_joint_angles: np.ndarray | None = None + self.target_pose: np.ndarray | None = None + self.solve_time_ms: float = 0.0 + self.success: bool = True + +class CameraState: + """Camera state - RGB images for one or more cameras.""" + def __init__(self) -> None: + self._lock = threading.Lock() + self.rgb_images: dict[str, np.ndarray] = {} \ No newline at end of file diff --git a/examples/common/system_bootstrap.py b/examples/common/system_bootstrap.py new file mode 100644 index 0000000..e65621d --- /dev/null +++ b/examples/common/system_bootstrap.py @@ -0,0 +1,87 @@ +import threading +import numpy as np +from typing import Tuple, List, Optional + +from common.configs import ( + CONTROLLER_BETA, CONTROLLER_D_CUTOFF, CONTROLLER_MIN_CUTOFF, + ROBOT_RATE, NEUTRAL_JOINT_ANGLES, NEUTRAL_END_EFFECTOR_POSE, + URDF_PATH, GRIPPER_FRAME_NAME, SOLVER_NAME, POSITION_COST, + ORIENTATION_COST, FRAME_TASK_GAIN, LM_DAMPING, DAMPING_COST, + SOLVER_DAMPING_VALUE, IK_SOLVER_RATE, POSTURE_COST_VECTOR, + TRANSLATION_SCALE, ROTATION_SCALE +) +from common.data_manager import DataManager +from piper_controller import PiperController +from pink_ik_solver import PinkIKSolver +from common.threads.joint_state import joint_state_thread +from common.threads.ik_solver import ik_solver_thread +from common.threads.realsense_camera import camera_thread + +def bootstrap_robot_system( + start_ik: bool = True, + start_camera: bool = True +) -> Tuple[DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread]]: + """Initializes DataManager, PiperController, IK, and base background threads.""" + + # 1. Initialize Data Manager + data_manager = DataManager() + data_manager.set_controller_filter_params( + CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF + ) + data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) + + # 2. Initialize Robot Controller + print("\n🤖 Initializing Piper robot controller...") + robot_controller = PiperController( + can_interface="can0", + robot_rate=ROBOT_RATE, + control_mode=PiperController.ControlMode.JOINT_SPACE, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, + enable_joint_angle_limits=False, + debug_mode=False, + ) + robot_controller.start_control_loop() + + # 3. Start Threads + active_threads = [] + + print("\n📊 Starting joint state thread...") + js_thread = threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True + ) + js_thread.start() + active_threads.append(js_thread) + + ik_solver = None + if start_ik: + print("\n🔧 Creating Pink IK solver...") + current_angles = data_manager.get_current_joint_angles() + init_angles = np.radians(current_angles) if current_angles is not None else np.radians(NEUTRAL_JOINT_ANGLES) + + ik_solver = PinkIKSolver( + urdf_path=URDF_PATH, end_effector_frame=GRIPPER_FRAME_NAME, + solver_name=SOLVER_NAME, position_cost=POSITION_COST, + orientation_cost=ORIENTATION_COST, frame_task_gain=FRAME_TASK_GAIN, + lm_damping=LM_DAMPING, damping_cost=DAMPING_COST, + solver_damping_value=SOLVER_DAMPING_VALUE, + integration_time_step=1 / IK_SOLVER_RATE, + initial_configuration=init_angles, + posture_cost_vector=np.array(POSTURE_COST_VECTOR), + ) + print("\n🧮 Starting IK solver thread...") + ik_thread = threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True + ) + ik_thread.start() + active_threads.append(ik_thread) + + if start_camera: + print("\n📷 Starting camera thread...") + cam_thread = threading.Thread( + target=camera_thread, args=(data_manager,), daemon=True + ) + cam_thread.start() + active_threads.append(cam_thread) + + return data_manager, robot_controller, ik_solver, active_threads \ No newline at end of file diff --git a/examples/common/visualizer_core.py b/examples/common/visualizer_core.py new file mode 100644 index 0000000..0cffe26 --- /dev/null +++ b/examples/common/visualizer_core.py @@ -0,0 +1,92 @@ +import numpy as np +import viser +import yourdfpy +from scipy.spatial.transform import Rotation +from viser.extras import ViserUrdf + +class RobotVisualizerCore: + """Handles the 3D rendering of the robot, ghost robot, and target frames.""" + + def __init__(self, urdf_path: str) -> None: + self.server = viser.ViserServer() + self.server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) + + # Load actual robot URDF + urdf = yourdfpy.URDF.load(urdf_path) + self.urdf_vis = ViserUrdf(self.server, urdf, root_node_name="/robot_actual") + + # Load ghost robot URDF + ghost_urdf = yourdfpy.URDF.load(urdf_path) + self.ghost_robot_urdf = ViserUrdf( + self.server, + ghost_urdf, + root_node_name="/robot_ghost", + mesh_color_override=(1.0, 0.65, 0.0, 0.25), + ) + + self.controller_handle = None + self.target_frame_handle = None + self.rgb_image_handle = None + + def add_controller_visualization(self) -> None: + self.controller_handle = self.server.scene.add_transform_controls( + "/controller", scale=0.15, position=(0, 0, 0), wxyz=(1, 0, 0, 0) + ) + + def add_target_frame_visualization(self) -> None: + self.target_frame_handle = self.server.scene.add_frame( + "/target_goal", axes_length=0.1, axes_radius=0.003 + ) + + def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None: + if self.rgb_image_handle is None: + dummy_image = np.zeros((height, width, 3), dtype=np.uint8) + self.rgb_image_handle = self.server.gui.add_image( + dummy_image, label="RGB Camera", format="jpeg", jpeg_quality=85 + ) + + def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: + if rgb_image is None: + return + if self.rgb_image_handle is None: + self.add_rgb_image_placeholder(height=rgb_image.shape[0], width=rgb_image.shape[1]) + self.rgb_image_handle.image = rgb_image + + def update_robot_pose(self, joint_config: np.ndarray) -> None: + self.urdf_vis.update_cfg(joint_config) + + def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None: + if self.ghost_robot_urdf: + self.ghost_robot_urdf.update_cfg(joint_config) + + def update_ghost_robot_visibility(self, flag: bool) -> None: + if self.ghost_robot_urdf: + self.ghost_robot_urdf.show_visual = flag + + def get_ghost_robot_visibility(self) -> bool: + if self.ghost_robot_urdf: + return self.ghost_robot_urdf.show_visual + return False + + def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + if self.ghost_robot_urdf: + self.ghost_robot_urdf.mesh_color_override = color + + def update_controller_visualization(self, transform: np.ndarray | None) -> None: + if self.controller_handle is None or transform is None: + return + pos = transform[:3, 3] + rot = Rotation.from_matrix(transform[:3, :3]).as_quat() + self.controller_handle.position = tuple(pos) + self.controller_handle.wxyz = (rot[3], rot[0], rot[1], rot[2]) + + def update_target_visualization(self, transform: np.ndarray | None) -> None: + if self.target_frame_handle is None or transform is None: + return + pos = transform[:3, 3] + rot = Rotation.from_matrix(transform[:3, :3]).as_quat() + self.target_frame_handle.position = tuple(pos) + self.target_frame_handle.wxyz = (rot[3], rot[0], rot[1], rot[2]) + + def stop(self) -> None: + self.server.stop() \ No newline at end of file diff --git a/examples/common/visualizer_gui.py b/examples/common/visualizer_gui.py new file mode 100644 index 0000000..b268062 --- /dev/null +++ b/examples/common/visualizer_gui.py @@ -0,0 +1,201 @@ +from typing import Any, Callable +import numpy as np +import viser + +class RobotVisualizerGUI: + """Handles all the 2D UI elements: buttons, sliders, and text readouts.""" + + def __init__(self, server: viser.ViserServer) -> None: + self.server = server + self._ema_timing = 0.001 + + # State Handles + self._timing_handle = None + self._joint_angles_handle = None + self._robot_status_handle = None + self._teleop_status_handle = None + self._controller_status_handle = None + self._gripper_status_handle = None + self._policy_status_handle = None + + # Input Handles + self._position_weight_handle = None + self._orientation_weight_handle = None + self._frame_task_gain_handle = None + self._lm_damping_handle = None + self._damping_weight_handle = None + self._solver_damping_value_handle = None + self._posture_cost_handles = [] + + self._controller_min_cutoff_handle = None + self._controller_beta_handle = None + self._controller_d_cutoff_handle = None + self._translation_scale_handle = None + self._rotation_scale_handle = None + + self._prediction_ratio_handle = None + self._policy_execution_rate_handle = None + self._robot_rate_handle = None + self._execution_mode_dropdown = None + + self._grip_value_handle = None + self._trigger_value_handle = None + + # Button Handles + self._enable_robot_handle = None + self._disable_robot_handle = None + self._emergency_stop_handle = None + self._go_home_button = None + self._toggle_robot_enabled_status_button = None + self._run_policy_button = None + self._start_policy_execution_button = None + self._play_policy_button = None + + # --- UI Setup Methods --- + def add_basic_controls(self) -> None: + self._timing_handle = self.server.gui.add_number("IK Solve Time (ms)", 0.001, disabled=True) + self._joint_angles_handle = self.server.gui.add_text("Joint Angles", "Waiting...") + + def add_robot_status_controls(self) -> None: + self._robot_status_handle = self.server.gui.add_text("Robot Status", "Initializing...") + + def add_teleop_controls(self) -> None: + self._grip_value_handle = self.server.gui.add_number("Grip Value", 0.0, disabled=True) + self._trigger_value_handle = self.server.gui.add_number("Trigger", 0.0, disabled=True) + self._teleop_status_handle = self.server.gui.add_text("Teleop Status", "Inactive") + self._controller_status_handle = self.server.gui.add_text("Controller Status", "Waiting...") + + def add_gripper_status_controls(self) -> None: + self._gripper_status_handle = self.server.gui.add_text("Gripper Status", "Open (0%)") + + def add_homing_controls(self) -> None: + self._go_home_button = self.server.gui.add_button("Go Home") + + def add_toggle_robot_enabled_status_button(self) -> None: + self._toggle_robot_enabled_status_button = self.server.gui.add_button("Enable Robot") + + def add_controller_filter_controls(self, initial_min_cutoff: float, initial_beta: float, initial_d_cutoff: float) -> None: + self._controller_min_cutoff_handle = self.server.gui.add_number("Controller Min Cutoff", initial_min_cutoff, min=0.01, max=10.0, step=0.01) + self._controller_beta_handle = self.server.gui.add_number("Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01) + self._controller_d_cutoff_handle = self.server.gui.add_number("Controller D Cutoff", initial_d_cutoff, min=0.01, max=10.0, step=0.01) + + def add_scaling_controls(self, initial_translation_scale: float, initial_rotation_scale: float) -> None: + self._translation_scale_handle = self.server.gui.add_number("Translation Scale", initial_translation_scale, min=0.1, max=10.0, step=0.001) + self._rotation_scale_handle = self.server.gui.add_number("Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001) + + def add_pink_parameter_controls(self, position_cost: float, orientation_cost: float, frame_task_gain: float, lm_damping: float, damping_cost: float, solver_damping_value: float, posture_cost_vector: list[float]) -> None: + self._position_weight_handle = self.server.gui.add_number("Position Weight", position_cost, min=0.0, max=10.0, step=0.1) + self._orientation_weight_handle = self.server.gui.add_number("Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01) + self._frame_task_gain_handle = self.server.gui.add_number("Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1) + self._lm_damping_handle = self.server.gui.add_number("LM Damping", lm_damping, min=0.0, max=5.0, step=0.01) + self._damping_weight_handle = self.server.gui.add_number("Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01) + self._solver_damping_value_handle = self.server.gui.add_number("Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001) + + for i, cost in enumerate(posture_cost_vector): + self._posture_cost_handles.append(self.server.gui.add_number(f"Posture Cost J{i+1}", cost, min=0.0, max=1.0, step=0.01)) + + def add_policy_controls(self, initial_prediction_ratio: float = 0.8, initial_policy_rate: float = 200.0, initial_robot_rate: float = 200.0, initial_execution_mode: str = "targeting_time") -> None: + self._policy_status_handle = self.server.gui.add_text("Policy Status", "Ready") + self._prediction_ratio_handle = self.server.gui.add_number("Prediction Ratio", initial_prediction_ratio, min=0.0, max=1.0, step=0.01) + self._policy_execution_rate_handle = self.server.gui.add_number("Policy Rate", initial_policy_rate, min=1.0, max=200.0, step=1.0) + self._robot_rate_handle = self.server.gui.add_number("Robot Rate", initial_robot_rate, min=1.0, max=200.0, step=1.0) + self._execution_mode_dropdown = self.server.gui.add_dropdown("Execution Mode", options=["targeting_time", "targeting_pose"], initial_value=initial_execution_mode) + + def add_policy_buttons(self) -> None: + self._run_policy_button = self.server.gui.add_button("Run Policy (Preview)") + self._start_policy_execution_button = self.server.gui.add_button("Execute Policy (Run Preview)") + self._play_policy_button = self.server.gui.add_button("Continuous Receding Horizon") + + # --- UI Update Methods --- + def update_timing(self, solve_time_ms: float) -> None: + if self._timing_handle: + self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms + self._timing_handle.value = self._ema_timing + + def update_robot_status(self, status: str) -> None: + if self._robot_status_handle: self._robot_status_handle.value = status + + def update_teleop_status(self, active: bool) -> None: + if self._teleop_status_handle: self._teleop_status_handle.value = f"Teleop Status: {'Active' if active else 'Inactive'}" + + def update_controller_status_display(self, position: np.ndarray | None, connected: bool = True) -> None: + if not self._controller_status_handle: return + if connected and position is not None: + self._controller_status_handle.value = f"Controller Status:\n Position: [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}]\n Connected: ✓" + else: + self._controller_status_handle.value = "Controller Status:\n Connected: ✗" + + def update_gripper_status(self, trigger_value: float, robot_enabled: bool = True) -> None: + if not self._gripper_status_handle: return + state = "Closed" if trigger_value > 0.9 else "Closing" if trigger_value > 0.1 else "Open" + status = f"Gripper: {state} ({trigger_value * 100.0:.0f}% closed)" + self._gripper_status_handle.value = status + ("" if robot_enabled else " [Disabled]") + + def update_policy_status(self, status: str) -> None: + if self._policy_status_handle: self._policy_status_handle.value = status + + def update_toggle_robot_enabled_status(self, enabled: bool) -> None: + if self._toggle_robot_enabled_status_button: + self._toggle_robot_enabled_status_button.label = "Disable Robot" if enabled else "Enable Robot" + + def update_play_policy_button_status(self, active: bool) -> None: + if self._play_policy_button: + self._play_policy_button.label = "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + + def update_joint_angles_display(self, joint_config: np.ndarray, show_gripper: bool = False) -> None: + if not self._joint_angles_handle: return + lines = ["Joint Angles (rad):"] + num_joints = len(joint_config) if show_gripper else 6 + for i in range(num_joints): + lbl = f"Joint {i+1} ({'Robot' if i < 6 else 'Gripper'})" if show_gripper else f"J{i+1}" + lines.append(f" {lbl}: {joint_config[i]:.3f} rad ({np.degrees(joint_config[i]):.1f}°)") + self._joint_angles_handle.value = "\n".join(lines) + + def set_grip_value(self, value: float) -> None: + if self._grip_value_handle: self._grip_value_handle.value = value + + def set_trigger_value(self, value: float) -> None: + if self._trigger_value_handle: self._trigger_value_handle.value = value + + # --- Getters --- + def get_controller_filter_params(self) -> tuple[float, float, float]: + return (self._controller_min_cutoff_handle.value, self._controller_beta_handle.value, self._controller_d_cutoff_handle.value) + + def get_translation_scale(self) -> float: return self._translation_scale_handle.value + def get_rotation_scale(self) -> float: return self._rotation_scale_handle.value + def get_prediction_ratio(self) -> float: return self._prediction_ratio_handle.value + def get_policy_execution_rate(self) -> float: return self._policy_execution_rate_handle.value + def get_robot_rate(self) -> float: return self._robot_rate_handle.value + def get_execution_mode(self) -> str: return self._execution_mode_dropdown.value + + def get_pink_parameters(self) -> dict: + return { + "position_cost": self._position_weight_handle.value, + "orientation_cost": self._orientation_weight_handle.value, + "frame_task_gain": self._frame_task_gain_handle.value, + "lm_damping": self._lm_damping_handle.value, + "damping_cost": self._damping_weight_handle.value, + "solver_damping_value": self._solver_damping_value_handle.value, + "posture_cost_vector": np.array([h.value for h in self._posture_cost_handles]), + } + + # --- Button Setters/Callbacks --- + def set_toggle_robot_enabled_status_callback(self, cb: Callable[[], Any]) -> None: + if self._toggle_robot_enabled_status_button: self._toggle_robot_enabled_status_button.on_click(lambda _: cb()) + def set_go_home_callback(self, cb: Callable[[], Any]) -> None: + if self._go_home_button: self._go_home_button.on_click(lambda _: cb()) + def set_run_policy_callback(self, cb: Callable[[], Any]) -> None: + if self._run_policy_button: self._run_policy_button.on_click(lambda _: cb()) + def set_start_policy_execution_callback(self, cb: Callable[[], Any]) -> None: + if self._start_policy_execution_button: self._start_policy_execution_button.on_click(lambda _: cb()) + def set_play_policy_callback(self, cb: Callable[[], Any]) -> None: + if self._play_policy_button: self._play_policy_button.on_click(lambda _: cb()) + def set_execution_mode_callback(self, cb: Callable[[], Any]) -> None: + if self._execution_mode_dropdown: self._execution_mode_dropdown.on_update(lambda _: cb()) + + def set_run_policy_button_disabled(self, disabled: bool) -> None: + if self._run_policy_button: self._run_policy_button.disabled = disabled + def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: + if self._start_policy_execution_button: self._start_policy_execution_button.disabled = disabled + def set_play_policy_button_disabled(self, disabled: bool) -> None: + if self._play_policy_button: self._play_policy_button.disabled = disabled \ No newline at end of file From fc6ce076b4987162c4e7bafb1c5d2a9c9bf2e4b3 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 20 May 2026 15:46:55 +0100 Subject: [PATCH 06/22] fix: make the policy inference work sequentially from remote inference. --- README.md | 4 +- examples/4_rollout_neuracore_policy.py | 295 +++++++++++--------- examples/common/configs.py | 21 +- examples/common/data_manager.py | 255 ++++------------- examples/common/policy_state.py | 18 +- examples/common/robot_visualizer.py | 44 +-- examples/common/threads/realsense_camera.py | 83 ++++-- halid_notes.txt | 0 piper_controller.py | 30 +- 9 files changed, 346 insertions(+), 404 deletions(-) create mode 100644 halid_notes.txt diff --git a/README.md b/README.md index 805298f..7e2beb6 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ Clone the Meta Quest teleoperation repository and install it: **NOTE**: Make sure you're installing it inside the `piper-teleop` conda environment. ```bash -git clone https://github.com/NeuracoreAI/meta_quest_teleop.git +git clone https://github.com/NeuracoreAI/meta_quest_teleop.git ## Halid Note: it is not clear where it should be cloned exactly. cd meta_quest_teleop pip install -e . cd .. @@ -124,6 +124,8 @@ python examples/2_collect_teleop_data_with_neuracore.py [--ip-address **Script**: `examples/3_replay_neuracore_episodes.py` +**Halid Note**: This step can be dangerous. always ready to press the emergency. The index start from 0 not 1, hence if you see number as x in the frontend, you need to run x-1. I cannot rerun this command again successfully after the first time running. The frequency should be default as 20. + Replay recorded episodes from a Neuracore dataset on the physical robot. ```bash diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 6ceb550..d613ae5 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -142,7 +142,13 @@ def run_policy( predictions = policy.predict(timeout=60) prediction_horizon = convert_predictions_to_horizon(predictions) end_time = time.time() - horizon_length = policy_state.get_prediction_horizon_length() + + # Calculate length directly from the new prediction dictionary + horizon_length = 0 + if prediction_horizon: + first_key = next(iter(prediction_horizon.keys())) + horizon_length = len(prediction_horizon[first_key]) + print( f" ✓ Got {horizon_length} actions in {end_time - start_time:.3f} seconds" ) @@ -206,17 +212,23 @@ def start_policy_execution( if current_joint_angles is None: print("⚠️ Cannot execute policy: No current joint angles available") return False + # Get first action from horizon (index 0 for each joint) current_joint_target_positions_rad = np.array( [prediction_horizon[joint_name][0] for joint_name in JOINT_NAMES] ) - joint_differences = np.abs( - current_joint_angles - np.degrees(current_joint_target_positions_rad) - ) + current_target_deg = np.degrees(current_joint_target_positions_rad) + joint_differences = np.abs(current_joint_angles - current_target_deg) + if np.any(joint_differences > MAX_SAFETY_THRESHOLD): - print("⚠️ Cannot execute policy: Robot too far from first action") - print(f" Differences: {[f'{d:.3f}' for d in joint_differences]}") - print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print("⚠️ Cannot execute policy: Robot too far from first predicted action") + print(" --- DIAGNOSTICS ---") + print(f" Current Angles: {[f'{d:.2f}' for d in current_joint_angles]}") + print(f" AI Predicted: {[f'{d:.2f}' for d in current_target_deg]}") + print(f" Differences: {[f'{d:.2f}' for d in joint_differences]}") + print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") + print(" 💡 TIP 1: Did the arm sag? Check if 'Current Angles' are drooping.") + print(" 💡 TIP 2: If the AI naturally predicts large first steps, increase MAX_SAFETY_THRESHOLD in common/configs.py") return False # All checks passed - start execution @@ -245,25 +257,6 @@ def start_policy_execution( return True -def run_and_start_policy_execution( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> None: - """Handle Run and Execute Policy button press to capture state, get policy prediction, and immediately execute it.""" - print("Run and Execute Policy for one prediction horizon") - run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - start_policy_execution(data_manager, policy_state) - - def end_policy_play( data_manager: DataManager, policy_state: PolicyState, @@ -273,12 +266,100 @@ def end_policy_play( """End continuous play and set robot activity state to ENABLED and update policy status.""" if policy_state.get_continuous_play_active(): policy_state.set_continuous_play_active(False) + + # Reset ghost robot color to default orange + visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) + + visualizer.update_play_policy_button_status(False) + visualizer.update_play_policy_button_status(False) policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) data_manager.set_teleop_state(False, None, None) visualizer.update_policy_status(policy_status_message) +def continuous_prediction_worker( + data_manager: DataManager, + policy: nc.policy, + policy_state: PolicyState, + visualizer: RobotVisualizer, + input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", +) -> None: + """Background thread for continuous execution supporting pipelined and sequential modes.""" + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + + # 1. Bootstrap the very first prediction to get the robot moving + print(f"\n🚀 [Worker] Bootstrapping initial trajectory in '{continuous_mode}' mode...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + if success: + start_policy_execution(data_manager, policy_state) + + while policy_state.get_continuous_play_active(): + # Failsafe: if there's no active trajectory running yet, wait briefly + if policy_state.get_locked_prediction_horizon_length() == 0: + time.sleep(0.01) + continue + + if continuous_mode == "pipeline": + print("\n📸 [Pipeline Worker] Robot is moving! Prefetching next prediction in background...") + # Query the network in parallel while the execution thread is driving the motors + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + # Wait until the current trajectory buffer is running low before swapping + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + remaining = total_len - exec_idx + + # Hot-swap when 5 or fewer steps are left in the active trajectory + if remaining <= 5 or total_len == 0: + break + time.sleep(0.01) + + elif continuous_mode == "sequential": + # Wait until the current trajectory buffer is completely exhausted + while policy_state.get_continuous_play_active(): + exec_idx = policy_state.get_execution_action_index() + total_len = policy_state.get_locked_prediction_horizon_length() + if exec_idx >= total_len or total_len == 0: + break + time.sleep(0.01) + + if not policy_state.get_continuous_play_active(): + break + + print("\n📸 [Sequential Worker] Trajectory finished! Holding position and requesting next prediction...") + success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) + + if not success or not policy_state.get_continuous_play_active(): + time.sleep(0.05) + continue + + if not policy_state.get_continuous_play_active(): + break + + print("🔄 [Worker] Swapping to new trajectory buffer!") + # Seamlessly clear the lock and flash the new horizon into play + policy_state.end_policy_execution() + success = start_policy_execution(data_manager, policy_state) + + if success: + color_index = (color_index + 1) % len(VISUALIZATION_COLORS) + visualizer.set_ghost_robot_color(VISUALIZATION_COLORS[color_index]) + else: + print("❌ [Worker] Swap rejected by safety threshold. Retrying immediately...") + time.sleep(0.01) def play_policy( data_manager: DataManager, @@ -286,56 +367,30 @@ def play_policy( policy_state: PolicyState, visualizer: RobotVisualizer, input_embodiment_description: EmbodimentDescription, + continuous_mode: str = "pipeline", ) -> None: """Handle Play Policy button press to start/stop continuous policy execution.""" if not policy_state.get_continuous_play_active(): # Start continuous play - print("▶️ Play Policy button pressed - Starting continuous policy execution...") - - # Run policy to get prediction horizon - success = run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - if not success: - print("⚠️ Failed to run policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - return - - # Execute policy - success = start_policy_execution(data_manager, policy_state) - if not success: - print("⚠️ Failed to execute policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - execution failed", - ) - return - + print(f"▶️ Play Policy button pressed - Starting {continuous_mode.capitalize()} Mode...") policy_state.set_continuous_play_active(True) visualizer.update_play_policy_button_status(True) - + + # Spawn the background worker + threading.Thread( + target=continuous_prediction_worker, + args=(data_manager, policy, policy_state, visualizer, input_embodiment_description, continuous_mode), + daemon=True + ).start() else: # Stop continuous play print("⏹️ Stop Policy button pressed - Stopping continuous policy execution...") policy_state.set_continuous_play_active(False) end_policy_play( - data_manager, policy_state, visualizer, "Policy execution stopped " + data_manager, policy_state, visualizer, "Policy execution stopped" ) - print("✓ Policy execution stopped and robot enabled") - def policy_execution_thread( policy: nc.policy, data_manager: DataManager, @@ -347,6 +402,16 @@ def policy_execution_thread( ) -> None: """Policy execution thread.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE + + # Define colors for continuous horizon visualization + VISUALIZATION_COLORS = [ + (1.0, 0.65, 0.0, 0.25), # Orange (Default) + (0.0, 1.0, 0.0, 0.25), # Green + (1.0, 0.0, 0.0, 0.25), # Red + (0.0, 0.0, 1.0, 0.25), # Blue + ] + color_index = 0 + # Throttle visualization updates to ~30Hz to avoid overwhelming Viser last_visualization_update = 0.0 visualization_update_interval = 1.0 / 30.0 # 30 Hz @@ -368,6 +433,7 @@ def policy_execution_thread( f"robot enabled: {robot_controller.is_robot_enabled()}" ) + # If continuous play is active, only execute up to the chunk limit if execution_index < locked_horizon_length: # Check if previous goal was achieved, if any current_joint_angles = data_manager.get_current_joint_angles() @@ -427,57 +493,23 @@ def policy_execution_thread( visualizer.update_policy_status( f"Executing policy: {execution_index + 1}/{locked_horizon_length}" ) - # Check if continuous play is active - elif policy_state.get_continuous_play_active(): - # Automatically get new prediction and execute - try: - # End policy execution to clear input lock - policy_state.end_policy_execution() - # Run policy to get prediction horizon - success = run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - if not success: - print("⚠️ Failed to run policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - continue - - # Execute policy - success = start_policy_execution(data_manager, policy_state) - if not success: - print("⚠️ Failed to execute policy") - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - execution failed", - ) - continue - - except Exception as e: - print(f"✗ Failed to get next policy prediction: {e}") - traceback.print_exc() + else: + # Horizon buffer exhausted + if not policy_state.get_continuous_play_active(): + print("✓ Policy execution completed") end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", + data_manager, policy_state, visualizer, "Policy execution completed" ) - else: - # Execution complete - print("✓ Policy execution completed") - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution completed" - ) + else: + # Failsafe: If the background thread is running slightly late, + # hold the very last predicted position to maintain motor torque. + if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): + last_index = locked_horizon_length - 1 + hold_positions_rad = np.array([ + locked_horizon[jn][last_index] for jn in JOINT_NAMES + ]) + if robot_controller.is_robot_enabled(): + robot_controller.set_target_joint_angles(np.degrees(hold_positions_rad)) # NOTE: Update visualization less frequently to avoid blocking # Throttle visualization updates to ~30Hz to prevent overwhelming Viser server @@ -525,9 +557,9 @@ def update_visualization( joint_config_rad = np.radians(target_joint_angles) visualizer.update_ghost_robot_pose(joint_config_rad) # Disable buttons during execution - visualizer.set_start_policy_execution_button_disabled(True) + # visualizer.set_start_policy_execution_button_disabled(True) visualizer.set_run_policy_button_disabled(True) - visualizer.set_run_and_start_policy_execution_button_disabled(True) + # visualizer.set_run_and_start_policy_execution_button_disabled(True) # Play/Stop button is enabled during execution so we can stop if needed visualizer.set_play_policy_button_disabled(False) @@ -576,7 +608,7 @@ def update_visualization( not (robot_enabled and has_horizon) ) visualizer.set_run_policy_button_disabled(not robot_enabled) - visualizer.set_run_and_start_policy_execution_button_disabled(not robot_enabled) + #visualizer.set_run_and_start_policy_execution_button_disabled(not robot_enabled) visualizer.set_play_policy_button_disabled(not robot_enabled) # Update policy status @@ -609,6 +641,15 @@ def update_visualization( default=None, help="Name of the training run to load policy from (for cloud training).", ) + + parser.add_argument( + "--continuous-mode", + type=str, + choices=["pipeline", "sequential"], + default="pipeline", + help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", + ) + policy_group.add_argument( "--model-path", type=str, @@ -666,6 +707,12 @@ def update_visualization( policy = nc.policy( train_run_name=args.train_run_name, device="cuda", +<<<<<<< HEAD +======= + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + #input_preprocessing_config=input_preprocessing_config, +>>>>>>> 895900c (fix: make the policy inference work sequentially from remote inference.) robot_name=args.robot_name, ) else: @@ -802,15 +849,7 @@ def update_visualization( visualizer.set_start_policy_execution_callback( lambda: start_policy_execution(data_manager, policy_state) ) - visualizer.set_run_and_start_policy_execution_callback( - lambda: run_and_start_policy_execution( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - ) + visualizer.set_play_policy_callback( lambda: play_policy( data_manager, @@ -818,8 +857,10 @@ def update_visualization( policy_state, visualizer, input_embodiment_description, + args.continuous_mode, ) ) + # Set up execution mode dropdown callback to sync with PolicyState visualizer.set_execution_mode_callback( lambda: policy_state.set_execution_mode( @@ -863,10 +904,10 @@ def update_visualization( print(" - Hold RIGHT TRIGGER to close gripper") print(" - Press BUTTON A or Enable Robot button to enable/disable robot") print(" - Press BUTTON B or Home Robot button to send robot home") - print(" 3. Click 'Run Policy' button to run policy (without executing)") - print(" 4. Click 'Execute Policy' button to execute prediction horizon") - print(" 5. Click 'Run and Execute Policy' button to run and execute policy") - print(" 6. Click 'Play Policy' button to play policy") + print(" 3. Click 'Run Policy' (Preview) to generate and visualize a prediction horizon") + print(" 4. Click 'Execute Policy' to run the currently previewed horizon") + print(" 5. Click 'Play Policy' (Receding Horizon) to constantly predict and execute the first action") + # print(" 6. Click 'Play Policy' button to play policy") print("⚠️ Press Ctrl+C to exit") print() print("🌐 Open browser: http://localhost:8080") diff --git a/examples/common/configs.py b/examples/common/configs.py index e8142b0..6ed3569 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -17,34 +17,34 @@ POSITION_COST = 1.0 ORIENTATION_COST = 0.75 FRAME_TASK_GAIN = 0.4 -LM_DAMPING = 0.0 +LM_DAMPING = 0.01 #0.0 DAMPING_COST = 0.25 -SOLVER_DAMPING_VALUE = 1e-12 +SOLVER_DAMPING_VALUE = 1e-4 #1e-12 # Controller 1€ Filter parameters CONTROLLER_MIN_CUTOFF = 0.8 # Minimum cutoff frequency (stabilizes when holding still) -CONTROLLER_BETA = 5.0 # Speed coefficient (reduces lag when moving) +CONTROLLER_BETA = 0.05 #5.0 # Speed coefficient (reduces lag when moving) CONTROLLER_D_CUTOFF = 0.9 # Derivative cutoff frequency # Controller parameters GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control # Scaling factors for translation and rotation -TRANSLATION_SCALE = 1.0 -ROTATION_SCALE = 1.0 +TRANSLATION_SCALE = 1.5 +ROTATION_SCALE = 1.2 SLOW_TRANSLATION_SCALE = 0.6 SLOW_ROTATION_SCALE = 0.6 WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 # Thread rates (Hz) CONTROLLER_DATA_RATE = 50.0 # Controller input reading -IK_SOLVER_RATE = 250.0 # IK solving and robot commands +IK_SOLVER_RATE = 100 #250.0 # IK solving and robot commands VISUALIZATION_RATE = 60.0 # GUI updates ROBOT_RATE = 100.0 # Neuracore data collection rates JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore -CAMERA_FRAME_STREAMING_RATE = 60.0 # Data collection rate for camera frame +CAMERA_FRAME_STREAMING_RATE = 30.0 # Data collection rate for camera frame # Meta quest axis mask META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] # [x, y, z, roll, pitch, yaw] @@ -56,7 +56,10 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] -NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +# NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # in degrees +# Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) + NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] # Posture task cost vector (one weight per joint) @@ -67,7 +70,7 @@ PREDICTION_HORIZON_EXECUTION_RATIO = ( 1.0 # percentage of the prediction horizon that is executed ) -MAX_SAFETY_THRESHOLD = 20.0 # degrees +MAX_SAFETY_THRESHOLD = 50.0 # degrees MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index fec186d..bf3cc3e 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -4,9 +4,9 @@ This module provides shared state classes for teleoperation systems that need to coordinate between multiple threads (data collection, IK solving, visualization). """ - import threading import time +import queue # Added for async queueing from enum import Enum from typing import Any, Callable @@ -18,7 +18,6 @@ class RobotActivityState(Enum): """Robot activity state enumeration.""" - ENABLED = "ENABLED" HOMING = "HOMING" DISABLED = "DISABLED" @@ -99,7 +98,6 @@ def __init__(self) -> None: # Map from camera name -> latest RGB image self.rgb_images: dict[str, np.ndarray] = {} - class DataManager: """Main state container coordinating all state groups. @@ -110,10 +108,8 @@ class DataManager: Uses separate locks for each state group to reduce contention. """ - def __init__(self) -> None: - """Initialize DataManager with default values.""" - # State groups with individual locks + """Initialize DataManager with background callback processing.""" self._controller_state = ControllerState() self._teleop_state = TeleopState() self._robot_state = RobotState() @@ -123,12 +119,20 @@ def __init__(self) -> None: # System state self._shutdown_event = threading.Event() - # Callback for state changes (RGB, target joints, current joints). - # The callable takes (name: str, payload: dict[str, Any], timestamp: float). - # payload is the data dict e.g. {joint1: v, joint2: v}, {gripper: v}, {rgb_scene: array}. + # Asynchronous processing elements self._on_change_callback: ( Callable[[str, dict[str, Any], float], None] | None ) = None + + # Maxsize 60 matches ~1 second of video frames buffer if disk spikes + self._callback_queue: queue.Queue = queue.Queue(maxsize=60) + + self._worker_thread = threading.Thread( + target=self._callback_worker_loop, + name="NeuracoreCallbackWorker", + daemon=True + ) + self._worker_thread.start() def set_on_change_callback( self, on_change_callback: Callable[[str, dict[str, Any], float], None] @@ -136,6 +140,35 @@ def set_on_change_callback( """Set on change callback (thread-safe).""" self._on_change_callback = on_change_callback + def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None: + """Helper to push payloads into the execution queue without blocking.""" + if self._on_change_callback is None: + return + + try: + # put_nowait drops data into the memory queue instantly (0.0ms blocking) + self._callback_queue.put_nowait((name, payload, timestamp)) + except queue.Full: + # Prevents out-of-memory if disk halts completely, without freezing telemetry loops + print(f"⚠️ Neuracore background queue full! Dropping log packet: {name}") + + def _callback_worker_loop(self) -> None: + """Background thread worker loop dedicated solely to performing slow disk IO updates.""" + while not self._shutdown_event.is_set() or not self._callback_queue.empty(): + try: + # Wait up to 100ms for a logging event + name, payload, timestamp = self._callback_queue.get(timeout=0.1) + + if self._on_change_callback is not None: + # Execute Neuracore disk operation safely here on a separate core + self._on_change_callback(name, payload, timestamp) + + self._callback_queue.task_done() + except queue.Empty: + continue + except Exception as e: + print(f"❌ Error in background logging callback: {e}") + # ============================================================================ # Camera State Methods # ============================================================================ @@ -145,28 +178,24 @@ def get_rgb_image(self, camera_name: str) -> np.ndarray | None: with self._camera_state._lock: if not self._camera_state.rgb_images: return None - img = self._camera_state.rgb_images.get(camera_name) return img.copy() if img is not None else None def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: - """Set RGB image for a specific camera (thread-safe).""" + """Set RGB image for a specific camera (thread-safe and non-blocking).""" with self._camera_state._lock: self._camera_state.rgb_images[camera_name] = image.copy() + if self._on_change_callback: img_copy = self._camera_state.rgb_images[camera_name].copy() - self._on_change_callback("log_rgb", {camera_name: img_copy}, time.time()) + # Queue it instead of executing directly! Camera loop returns immediately. + self._queue_callback("log_rgb", {camera_name: img_copy}, time.time()) # ============================================================================ # Controller State Methods # ============================================================================ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: - """Get current controller data (thread-safe). - - Returns: - Tuple of (controller_transform, grip_value, trigger_value) - """ with self._controller_state._lock: return ( ( @@ -181,18 +210,6 @@ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: def set_controller_data( self, transform: np.ndarray | None, grip: float, trigger: float ) -> None: - """Set controller data (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None - grip: float - grip value - trigger: float - trigger value - - Raises: - ValueError: If the transform is not a 4x4 matrix - ValueError: If the grip value is not between 0.0 and 1.0 - ValueError: If the trigger value is not between 0.0 and 1.0 - """ if transform is not None and transform.shape != (4, 4): raise ValueError("Transform must be a 4x4 matrix") if grip < 0.0 or grip > 1.0: @@ -206,11 +223,8 @@ def set_controller_data( if transform is not None: current_time = time.time() - - # Store raw transform self._controller_state.transform_raw = transform.copy() - # Initialize filter if needed if self._controller_state._filter is None: self._controller_state._filter = OneEuroFilterTransform( current_time, @@ -221,45 +235,28 @@ def set_controller_data( ) self._controller_state.transform = transform.copy() else: - # Update filter parameters if they changed self._controller_state._filter.update_params( self._controller_state.min_cutoff, self._controller_state.beta, self._controller_state.d_cutoff, ) - - # Apply filter self._controller_state.transform = self._controller_state._filter( current_time, transform ) else: self._controller_state.transform = None self._controller_state.transform_raw = None - self._controller_state._filter = ( - None # Reset filter when transform is None - ) + self._controller_state._filter = None def set_controller_filter_params( self, min_cutoff: float, beta: float, d_cutoff: float ) -> None: - """Update 1€ Filter parameters for controller transform (thread-safe). - - Args: - min_cutoff: Minimum cutoff frequency (stabilizes when holding still) - beta: Speed coefficient (reduces lag when moving) - d_cutoff: Cutoff frequency for derivative filtering - """ with self._controller_state._lock: self._controller_state.min_cutoff = min_cutoff self._controller_state.beta = beta self._controller_state.d_cutoff = d_cutoff def get_controller_filter_params(self) -> tuple[float, float, float]: - """Get 1€ Filter parameters for controller transform (thread-safe). - - Returns: - Tuple of (min_cutoff, beta, d_cutoff) - """ with self._controller_state._lock: return ( self._controller_state.min_cutoff, @@ -277,13 +274,6 @@ def set_teleop_state( controller_initial: np.ndarray | None, robot_initial: np.ndarray | None, ) -> None: - """Set teleoperation state (thread-safe). - - Args: - active: bool - whether teleop is active - controller_initial: np.ndarray | None - 4x4 transformation matrix for initial controller transform or None to clear - robot_initial: np.ndarray | None - 4x4 transformation matrix for initial robot transform or None to clear - """ with self._teleop_state._lock: self._teleop_state.active = active self._teleop_state.controller_initial_transform = ( @@ -296,26 +286,13 @@ def set_teleop_state( def set_teleop_scaling( self, translation_scale: float, rotation_scale: float ) -> None: - """Set teleoperation scaling factors (thread-safe). - - Args: - translation_scale: Scale applied to controller translation deltas - rotation_scale: Scale applied to controller rotation deltas - """ - # Ignore invalid values (e.g. transient 0 while typing) and keep old scaling. if translation_scale <= 0.0 or rotation_scale <= 0.0: return - with self._teleop_state._lock: self._teleop_state.translation_scale = translation_scale self._teleop_state.rotation_scale = rotation_scale def get_teleop_scaling(self) -> tuple[float, float]: - """Get teleoperation scaling factors (thread-safe). - - Returns: - Tuple of (translation_scale, rotation_scale) - """ with self._teleop_state._lock: return ( self._teleop_state.translation_scale, @@ -323,17 +300,14 @@ def get_teleop_scaling(self) -> tuple[float, float]: ) def get_teleop_active(self) -> bool: - """Get teleoperation active state (thread-safe).""" with self._teleop_state._lock: return self._teleop_state.active def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: - """Set slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = enabled def toggle_slow_scaling_mode_enabled(self) -> bool: - """Toggle and return slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = ( not self._teleop_state.slow_scaling_mode_enabled @@ -341,21 +315,12 @@ def toggle_slow_scaling_mode_enabled(self) -> bool: return self._teleop_state.slow_scaling_mode_enabled def get_slow_scaling_mode_enabled(self) -> bool: - """Get slow-scaling mode status (thread-safe).""" with self._teleop_state._lock: return self._teleop_state.slow_scaling_mode_enabled def get_initial_robot_controller_transforms( self, ) -> tuple[np.ndarray | None, np.ndarray | None]: - """Get initial robot and controller transforms. - - These two transforms are captured on rising edge of grip button - and reset on falling edge of grip button. (thread-safe) - - Returns: - Tuple of (controller_initial_transform, robot_initial_transform) - """ with self._teleop_state._lock: return ( ( @@ -375,29 +340,14 @@ def get_initial_robot_controller_transforms( # ============================================================================ def get_robot_activity_state(self) -> RobotActivityState: - """Get robot activity state (thread-safe). - - Returns: - RobotActivityState - current robot activity state - """ with self._robot_state._lock: return self._robot_state.activity_state def set_robot_activity_state(self, state: RobotActivityState) -> None: - """Set robot activity state (thread-safe). - - Args: - state: RobotActivityState - new robot activity state - """ with self._robot_state._lock: self._robot_state.activity_state = state def get_current_joint_angles(self) -> np.ndarray | None: - """Get current joint angles (thread-safe). - - Returns: - Current joint angles or None if not available - """ with self._robot_state._lock: return ( self._robot_state.joint_angles.copy() @@ -406,11 +356,6 @@ def get_current_joint_angles(self) -> np.ndarray | None: ) def set_current_joint_angles(self, angles: np.ndarray) -> None: - """Set current joint angles (thread-safe). - - Args: - angles: np.ndarray - current joint angles - """ with self._robot_state._lock: self._robot_state.joint_angles = angles.copy() if self._on_change_callback: @@ -419,14 +364,9 @@ def set_current_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._on_change_callback("log_joint_positions", payload, time.time()) + self._queue_callback("log_joint_positions", payload, time.time()) def get_current_joint_torques(self) -> np.ndarray | None: - """Get current joint torques/currents proxy (thread-safe). - - Returns: - Current joint torques/currents vector or None if not available - """ with self._robot_state._lock: return ( self._robot_state.joint_torques.copy() @@ -435,25 +375,15 @@ def get_current_joint_torques(self) -> np.ndarray | None: ) def set_current_joint_torques(self, torques: np.ndarray) -> None: - """Set current joint torques/currents proxy and log to NeuraCore. - - Args: - torques: np.ndarray - current joint torques/currents vector - """ with self._robot_state._lock: self._robot_state.joint_torques = torques.copy() if self._on_change_callback: torques = self._robot_state.joint_torques if torques is not None: payload = {jn: float(torques[i]) for i, jn in enumerate(JOINT_NAMES)} - self._on_change_callback("log_joint_torques", payload, time.time()) + self._queue_callback("log_joint_torques", payload, time.time()) def get_current_end_effector_pose(self) -> np.ndarray | None: - """Get current end effector pose (thread-safe). - - Returns: - Current end effector pose or None if not available - """ with self._robot_state._lock: return ( self._robot_state.end_effector_pose.copy() @@ -462,61 +392,32 @@ def get_current_end_effector_pose(self) -> np.ndarray | None: ) def set_current_end_effector_pose(self, pose: np.ndarray) -> None: - """Set current end effector pose (thread-safe). - - Args: - pose: np.ndarray - current end effector pose - """ with self._robot_state._lock: self._robot_state.end_effector_pose = pose.copy() def get_current_gripper_open_value(self) -> float | None: - """Get current gripper open value (thread-safe). - - Returns: - Current gripper open value or None if not available - """ with self._robot_state._lock: - return ( - self._robot_state.current_gripper_open_value - if self._robot_state.current_gripper_open_value is not None - else None - ) + return self._robot_state.current_gripper_open_value def set_current_gripper_open_value(self, value: float) -> None: - """Set current gripper open value (thread-safe). - - Args: - value: float - current gripper open value - """ with self._robot_state._lock: self._robot_state.current_gripper_open_value = value if self._on_change_callback: - self._on_change_callback( + self._queue_callback( "log_parallel_gripper_open_amounts", {GRIPPER_NAME: value}, time.time(), ) def get_target_gripper_open_value(self) -> float | None: - """Get target gripper open value (thread-safe). - - Returns: - Target gripper open value or None if not available - """ with self._robot_state._lock: return self._robot_state.target_gripper_open_value def set_target_gripper_open_value(self, value: float) -> None: - """Set target gripper open value (thread-safe). - - Args: - value: float - target gripper open value - """ with self._robot_state._lock: self._robot_state.target_gripper_open_value = value if self._on_change_callback: - self._on_change_callback( + self._queue_callback( "log_parallel_gripper_target_open_amounts", {GRIPPER_NAME: self._robot_state.target_gripper_open_value}, time.time(), @@ -527,11 +428,6 @@ def set_target_gripper_open_value(self, value: float) -> None: # ============================================================================ def get_target_joint_angles(self) -> np.ndarray | None: - """Get current joint configuration (thread-safe). - - Returns: - Current target joint angles or None if not available - """ with self._ik_state._lock: return ( self._ik_state.target_joint_angles.copy() @@ -540,11 +436,6 @@ def get_target_joint_angles(self) -> np.ndarray | None: ) def set_target_joint_angles(self, angles: np.ndarray) -> None: - """Set target joint angles (thread-safe). - - Args: - angles: np.ndarray - target joint angles - """ with self._ik_state._lock: self._ik_state.target_joint_angles = angles.copy() if self._on_change_callback: @@ -553,27 +444,17 @@ def set_target_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._on_change_callback( + self._queue_callback( "log_joint_target_positions", payload, time.time() ) def set_target_pose(self, transform: np.ndarray | None) -> None: - """Set target transform for visualization (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None to clear target transform - """ with self._ik_state._lock: self._ik_state.target_pose = ( transform.copy() if transform is not None else None ) def get_target_pose(self) -> np.ndarray | None: - """Get target transform for visualization (thread-safe). - - Returns: - Target transform or None if target transform is not set - """ with self._ik_state._lock: return ( self._ik_state.target_pose.copy() @@ -582,38 +463,18 @@ def get_target_pose(self) -> np.ndarray | None: ) def set_ik_solve_time_ms(self, time_ms: float) -> None: - """Set IK solve time (thread-safe). - - Args: - time_ms: float - IK solve time in milliseconds - """ with self._ik_state._lock: self._ik_state.solve_time_ms = time_ms def set_ik_success(self, success: bool) -> None: - """Set IK success (thread-safe). - - Args: - success: bool - True if IK was successful, False otherwise - """ with self._ik_state._lock: self._ik_state.success = success def get_ik_solve_time_ms(self) -> float: - """Get IK solve time (thread-safe). - - Returns: - IK solve time in milliseconds - """ with self._ik_state._lock: return self._ik_state.solve_time_ms def get_ik_success(self) -> bool: - """Get IK success (thread-safe). - - Returns: - True if IK was successful, False otherwise - """ with self._ik_state._lock: return self._ik_state.success @@ -622,13 +483,9 @@ def get_ik_success(self) -> bool: # ============================================================================ def request_shutdown(self) -> None: - """Request shutdown of all threads (lock-free using Event).""" + """Request shutdown of all threads.""" self._shutdown_event.set() def is_shutdown_requested(self) -> bool: - """Check if shutdown is requested (lock-free using Event). - - Returns: - True if shutdown is requested, False otherwise - """ - return self._shutdown_event.is_set() + """Check if shutdown is requested.""" + return self._shutdown_event.is_set() \ No newline at end of file diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 42e90b6..37dda03 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -90,14 +90,7 @@ def get_policy_rgb_image_input(self) -> np.ndarray | None: ) def set_policy_rgb_image_input(self, image: np.ndarray) -> None: - """Set policy RGB image (thread-safe). - - Raises: - RuntimeError: If policy inputs are locked (during execution). - """ - with self._execution_lock: - if self._policy_inputs_locked: - raise RuntimeError("Policy inputs are locked during execution") + """Set policy RGB image (thread-safe).""" with self._policy_rgb_image_input_lock: self._policy_rgb_image_input = image.copy() if image is not None else None @@ -111,14 +104,7 @@ def get_policy_state_input(self) -> np.ndarray | None: ) def set_policy_state_input(self, input: np.ndarray) -> None: - """Set policy state input (thread-safe). - - Raises: - RuntimeError: If policy inputs are locked (during execution). - """ - with self._execution_lock: - if self._policy_inputs_locked: - raise RuntimeError("Policy inputs are locked during execution") + """Set policy state input (thread-safe).""" with self._policy_state_input_lock: self._policy_state_input = input.copy() if input is not None else None diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index 0547095..e8a3602 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -40,7 +40,7 @@ def __init__(self, urdf_path: str) -> None: self._server, ghost_urdf, root_node_name="/robot_ghost", - mesh_color_override=(0.2, 0.4, 1.0, 0.25), # Blue with 60% opacity + mesh_color_override=(1.0, 0.65, 0.0, 0.25), # Orange with 25% opacity ) # GUI handles (initialized as None, created on demand) - all private @@ -92,7 +92,6 @@ def __init__(self, urdf_path: str) -> None: self._execution_mode_dropdown = None self._run_policy_button = None self._start_policy_execution_button = None - self._run_and_start_policy_execution_button = None self._play_policy_button = None # Internal state @@ -675,14 +674,11 @@ def add_policy_controls( def add_policy_buttons(self) -> None: """Add policy control buttons.""" - self._run_policy_button = self._server.gui.add_button("Run Policy") + self._run_policy_button = self._server.gui.add_button("Run Policy (Preview)") self._start_policy_execution_button = self._server.gui.add_button( - "Start Policy Execution" + "Execute Policy (Run Preview)" ) - self._run_and_start_policy_execution_button = self._server.gui.add_button( - "Run and Execute Policy" - ) - self._play_policy_button = self._server.gui.add_button("Play Policy") + self._play_policy_button = self._server.gui.add_button("Continuous Receding Horizon") def update_policy_status(self, status: str) -> None: """Update policy status display. @@ -773,16 +769,6 @@ def set_start_policy_execution_callback(self, callback: Callable[[], Any]) -> No if self._start_policy_execution_button is not None: self._start_policy_execution_button.on_click(lambda _: callback()) - def set_run_and_start_policy_execution_callback( - self, callback: Callable[[], Any] - ) -> None: - """Set callback for Run and Execute Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._run_and_start_policy_execution_button is not None: - self._run_and_start_policy_execution_button.on_click(lambda _: callback()) def set_play_policy_callback(self, callback: Callable[[], Any]) -> None: """Set callback for Play Policy button. @@ -856,16 +842,6 @@ def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: if self._start_policy_execution_button is not None: self._start_policy_execution_button.disabled = disabled - def set_run_and_start_policy_execution_button_disabled( - self, disabled: bool - ) -> None: - """Set Run and Execute Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._run_and_start_policy_execution_button is not None: - self._run_and_start_policy_execution_button.disabled = disabled def set_play_policy_button_disabled(self, disabled: bool) -> None: """Set Play Policy button disabled state. @@ -883,8 +859,18 @@ def update_play_policy_button_status(self, active: bool) -> None: active: Whether continuous play is currently active """ if self._play_policy_button is not None: - self._play_policy_button.label = "Stop Policy" if active else "Play Policy" + self._play_policy_button.label = ( + "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + ) + def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + """Set the color of the ghost robot. + Args: + color: RGBA tuple (0.0 to 1.0) + """ + if self._ghost_robot_urdf is not None: + self._ghost_robot_urdf.mesh_color_override = color + def stop(self) -> None: """Stop the visualizer server.""" self._server.stop() diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py index 24e6dfe..8573eae 100644 --- a/examples/common/threads/realsense_camera.py +++ b/examples/common/threads/realsense_camera.py @@ -1,7 +1,9 @@ -"""Camera thread - captures RGB images from RealSense.""" +"""Camera thread - captures RGB images from RealSense with drop detection.""" import time import traceback +from collections import deque +import cv2 import numpy as np import pyrealsense2 as rs @@ -10,13 +12,21 @@ def camera_thread(data_manager: DataManager) -> None: - """Camera thread - captures RGB images from RealSense.""" + """Camera thread - captures RGB images from RealSense and monitors health.""" print("📷 Camera thread started") camera_name = CAMERA_NAMES[0] - dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE pipeline: rs.pipeline | None = None + # Diagnostic variables + last_frame_number = None + total_dropped_frames = 0 + fps_timer = time.time() + frame_count = 0 + # Store the last 100 frame intervals to check for extreme jitter + intervals = deque(maxlen=100) + last_frame_time = time.time() + try: # Configure RealSense pipeline pipeline = rs.pipeline() @@ -34,26 +44,67 @@ def camera_thread(data_manager: DataManager) -> None: pipeline.start(config) while not data_manager.is_shutdown_requested(): - iteration_start = time.time() - try: + # wait_for_frames naturally blocks at the target framerate (e.g., 60Hz) frames = pipeline.wait_for_frames(timeout_ms=500) except Exception as e: - print(f"⚠️ RealSense wait for frames error: {e}") + print(f"⚠️ RealSense wait for frames error (Timeout?): {e}") continue color_frame = frames.get_color_frame() + if not color_frame: + continue + + current_time = time.time() + intervals.append(current_time - last_frame_time) + last_frame_time = current_time + + # --------------------------------------------------------- + # DIAGNOSTICS: Check for dropped frames via hardware ID + # --------------------------------------------------------- + current_frame_number = color_frame.get_frame_number() + + if last_frame_number is not None: + # If frame numbers are not sequential, we missed something in software + drops = (current_frame_number - last_frame_number) - 1 + if drops > 0: + total_dropped_frames += drops + print(f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}") + + last_frame_number = current_frame_number + + # --------------------------------------------------------- + # DIAGNOSTICS: Calculate software-side FPS and Jitter + # --------------------------------------------------------- + frame_count += 1 + if current_time - fps_timer >= 5.0: # Report every 5 seconds + effective_fps = frame_count / (current_time - fps_timer) + + # Calculate jitter (max variance between frame arrivals) + max_interval = max(intervals) * 1000 # in ms + min_interval = min(intervals) * 1000 # in ms + + print(f"📊 Camera Health: {effective_fps:.1f} FPS | " + f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " + f"Total Drops: {total_dropped_frames}") + + # Reset counters for the next 5-second window + fps_timer = current_time + frame_count = 0 + + # --------------------------------------------------------- + # IMAGE PROCESSING + # --------------------------------------------------------- + # color_image = np.asanyarray(color_frame.get_data()) + # color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + # data_manager.set_rgb_image(color_image, camera_name) - if color_frame: - color_image = np.asanyarray(color_frame.get_data()) - color_image = np.rot90(color_image, k=3) # Rotate 270 degrees - data_manager.set_rgb_image(color_image, camera_name) + color_image = np.asanyarray(color_frame.get_data()) + color_image = np.rot90(color_image, k=3) # Rotate 270 degrees + data_manager.set_rgb_image(color_image, camera_name) - # Sleep to maintain loop rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) + # Notice: The time.sleep() has been completely removed. + # wait_for_frames() manages the loop pace perfectly. except Exception as e: print(f"❌ Camera thread error: {e}") @@ -66,4 +117,4 @@ def camera_thread(data_manager: DataManager) -> None: print(" ✓ RealSense pipeline stopped") except Exception as e: print(f"⚠️ Error stopping pipeline: {e}") - print("📷 Camera thread stopped") + print("📷 Camera thread stopped") \ No newline at end of file diff --git a/halid_notes.txt b/halid_notes.txt new file mode 100644 index 0000000..e69de29 diff --git a/piper_controller.py b/piper_controller.py index e979bcc..63b96a4 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -91,7 +91,7 @@ def __init__( # Gripper range in degrees (for internal SDK communication) self.GRIPPER_DEGREES_MIN = -10.00 - self.GRIPPER_DEGREES_MAX = 30.00 + self.GRIPPER_DEGREES_MAX = 100.00 self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees @@ -223,18 +223,34 @@ def get_control_mode(self) -> "PiperController.ControlMode": with self.state_lock: return self._control_mode - def set_control_mode(self, mode: "PiperController.ControlMode") -> None: - """Set the control mode. + # def set_control_mode(self, mode: "PiperController.ControlMode") -> None: + # """Set the control mode. - Args: - mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE) - """ + # Args: + # mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE) + # """ + # with self.state_lock: + # old_mode = self._control_mode + # self._control_mode = mode + # if self.debug_mode: + # print(f"Control mode changed: {old_mode.value} -> {mode.value}") + + + def set_control_mode(self, mode: "PiperController.ControlMode") -> None: with self.state_lock: old_mode = self._control_mode self._control_mode = mode + + # Send the mode configuration ONCE here, not in the 100Hz loop + if hasattr(self, "piper") and self.piper.get_connect_status(): + if mode == PiperController.ControlMode.JOINT_SPACE: + self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) + elif mode == PiperController.ControlMode.END_EFFECTOR: + self.piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) + if self.debug_mode: print(f"Control mode changed: {old_mode.value} -> {mode.value}") - + @staticmethod def _pose_6d_to_4x4(pose_6d: np.ndarray) -> np.ndarray: """Convert 6D pose [x, y, z, rx, ry, rz] to 4x4 transformation matrix. From 68a21e7354e16d9afa3ba92c20c2ac944a463916 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 20 May 2026 15:46:55 +0100 Subject: [PATCH 07/22] fix: make the policy inference work sequentially from remote inference. --- examples/4_rollout_neuracore_policy.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index d613ae5..6d114e1 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -707,13 +707,6 @@ def update_visualization( policy = nc.policy( train_run_name=args.train_run_name, device="cuda", -<<<<<<< HEAD -======= - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - #input_preprocessing_config=input_preprocessing_config, ->>>>>>> 895900c (fix: make the policy inference work sequentially from remote inference.) - robot_name=args.robot_name, ) else: print(f"\n🤖 Loading policy from model file: {args.model_path}...") From bf9f775757866843e3667af080d510018a62e574 Mon Sep 17 00:00:00 2001 From: Halid Date: Thu, 21 May 2026 11:56:57 +0100 Subject: [PATCH 08/22] fix the seuqntial inference --- examples/4_rollout_neuracore_policy.py | 2 +- examples/common/configs.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 6d114e1..6555873 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -646,7 +646,7 @@ def update_visualization( "--continuous-mode", type=str, choices=["pipeline", "sequential"], - default="pipeline", + default="sequential", help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", ) diff --git a/examples/common/configs.py b/examples/common/configs.py index 6ed3569..eff056d 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -57,7 +57,8 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] # NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] -NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # in degrees +NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) +# NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid # Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] From 3fda92721cff792be95bbf27b1c8de16282ede6d Mon Sep 17 00:00:00 2001 From: Halid Date: Tue, 26 May 2026 11:57:56 +0100 Subject: [PATCH 09/22] fix:agile_x is working with teleoperation smoothly --- .../2_collect_teleop_data_with_neuracore.py | 17 ++++++ examples/combine_code.py | 54 +++++++++++++++++++ examples/common/configs.py | 4 +- examples/common/policy_state.py | 2 +- 4 files changed, 74 insertions(+), 3 deletions(-) create mode 100644 examples/combine_code.py diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 2d04bd5..434a9b5 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -8,6 +8,7 @@ import argparse import multiprocessing +import subprocess import sys import threading import time @@ -60,12 +61,26 @@ from piper_controller import PiperController +def play_audio_feedback(action: str) -> None: + """Play distinct synthesized tones asynchronously using SoX.""" + # Start = High pitch (880 Hz), Stop = Low pitch (440 Hz) + freq = "880" if action == "start" else "440" + try: + # -q: quiet, -n: no input file, synth: generate audio, 0.3: duration, sine: wave type + subprocess.Popen( + ["play", "-q", "-n", "synth", "0.3", "sine", freq], + stderr=subprocess.DEVNULL + ) + except Exception as e: + print(f"⚠️ Failed to play tone (is SoX installed?): {e}") + def log_to_neuracore_on_change_callback( name: str, payload: dict[str, Any], timestamp: float ) -> None: """Log data to queue on change callback.""" # Call appropriate Neuracore logging function try: + #print(f"\n📤 Logging {name} to Neuracore with timestamp {timestamp:.3f}...") if name == "log_joint_positions": nc.log_joint_positions(payload, timestamp=timestamp) elif name == "log_joint_torques": @@ -131,6 +146,7 @@ def on_button_rj_pressed() -> None: try: nc.start_recording() print("✓ 🔴 Recording started (Button RJ)") + play_audio_feedback("start") # <-- Trigger distinct START sound except Exception as e: print(f"✗ Failed to start recording. Exception: {e}") print("Traceback:") @@ -140,6 +156,7 @@ def on_button_rj_pressed() -> None: try: nc.stop_recording() print("✓ ⏹️ Recording stopped (Button RJ)") + play_audio_feedback("stop") # <-- Trigger distinct STOP sound except Exception as e: print(f"✗ Failed to stop recording. Exception: {e}") print("Traceback:") diff --git a/examples/combine_code.py b/examples/combine_code.py new file mode 100644 index 0000000..043cf46 --- /dev/null +++ b/examples/combine_code.py @@ -0,0 +1,54 @@ +import pathlib + +def combine_python_files(directory_path, output_filename): + """ + Recursively finds all .py files in a directory and combines them into a single text file. + + Args: + directory_path (str): The path to the root directory to search. + output_filename (str): The name/path of the output text file. + """ + # Create a Path object for the target directory + source_dir = pathlib.Path(directory_path) + + # Ensure the directory exists + if not source_dir.is_dir(): + print(f"Error: The directory '{directory_path}' does not exist.") + return + + # Open the output file in write mode + with open(output_filename, 'w', encoding='utf-8') as outfile: + # rglob('*.py') recursively searches for all files ending in .py + py_files = list(source_dir.rglob('*.py')) + + if not py_files: + print(f"No Python files found in '{directory_path}'.") + return + + print(f"Found {len(py_files)} Python files. Combining...") + + for file_path in py_files: + # Write a clear separator and the file path as a header + outfile.write(f"\n{'='*60}\n") + outfile.write(f"FILE: {file_path}\n") + outfile.write(f"{'='*60}\n\n") + + # Read the python file and append its contents + try: + with open(file_path, 'r', encoding='utf-8') as infile: + outfile.write(infile.read()) + outfile.write("\n") + except Exception as e: + error_msg = f"Error reading file {file_path}: {e}\n" + print(error_msg) + outfile.write(error_msg) + + print(f"Success! All files have been combined into '{output_filename}'.") + +# --- Example Usage --- +if __name__ == "__main__": + # Replace these variables with your actual paths + TARGET_DIRECTORY = "./" # "./" means current directory + OUTPUT_FILE = "combined_code.txt" + + combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) \ No newline at end of file diff --git a/examples/common/configs.py b/examples/common/configs.py index eff056d..e65110c 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -58,7 +58,7 @@ # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] # NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) -# NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid # Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] @@ -71,7 +71,7 @@ PREDICTION_HORIZON_EXECUTION_RATIO = ( 1.0 # percentage of the prediction horizon that is executed ) -MAX_SAFETY_THRESHOLD = 50.0 # degrees +MAX_SAFETY_THRESHOLD = 200.0 # degrees MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 37dda03..8695aa1 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -20,7 +20,7 @@ def __init__(self) -> None: # Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names self._prediction_horizon: dict[str, list[float]] = {} self._prediction_horizon_lock = threading.Lock() - self._execution_ratio: float = 1.0 + self._execution_ratio: float = 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in self._policy_rgb_image_input: np.ndarray | None = None self._policy_rgb_image_input_lock = threading.Lock() From f7e0f5e953347d50d029e5696ce30ff426fffb0a Mon Sep 17 00:00:00 2001 From: Halid Date: Tue, 26 May 2026 15:50:38 +0100 Subject: [PATCH 10/22] chore:about to mearge from main --- examples/common/configs.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/common/configs.py b/examples/common/configs.py index e65110c..0366486 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -57,7 +57,7 @@ # # Initial neutral pose for robot (mm and degrees) # NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] # NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] -NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) +# NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid # Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) From be6b4ac65aa1cfe1fdf507f8baaa58971d886683 Mon Sep 17 00:00:00 2001 From: Halid Date: Tue, 26 May 2026 15:53:21 +0100 Subject: [PATCH 11/22] chore:about to mearge from main again --- README.md | 1 + piper_controller.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 7e2beb6..c2d1c2a 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,7 @@ This project is a complete example showcasing how to use Neuracore with the Agil ## Prerequisites - Python 3.10 +- sudo apt-get install sox - Conda (for environment management) - Meta Quest device setup (see `meta_quest_teleop/README.md` for details) - Realsense camera diff --git a/piper_controller.py b/piper_controller.py index 63b96a4..3e37f93 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -91,7 +91,7 @@ def __init__( # Gripper range in degrees (for internal SDK communication) self.GRIPPER_DEGREES_MIN = -10.00 - self.GRIPPER_DEGREES_MAX = 100.00 + self.GRIPPER_DEGREES_MAX = 80.00 # cube stacking halid is 80, lemon picking pruthvi is 80 self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees From 6c8aa34ce73ae69d29b4abb44829c77d2b1b4afa Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 11:33:48 +0100 Subject: [PATCH 12/22] fix:add the adative ik-tuning config --- examples/1_tune_teleop_params.py | 175 ++++++++++++---- .../2_collect_teleop_data_with_neuracore.py | 155 ++++++++++---- examples/4_rollout_neuracore_policy.py | 193 ++++++++++++------ examples/common/config_parser.py | 12 ++ examples/common/configs.py | 93 +++------ examples/common/system_bootstrap.py | 48 ++--- examples/common/visualizer_gui.py | 26 ++- examples/ik_conf/default.yaml | 24 +++ examples/ik_conf/tuned_teleop_configs.yaml | 24 +++ 9 files changed, 523 insertions(+), 227 deletions(-) create mode 100644 examples/common/config_parser.py create mode 100644 examples/ik_conf/default.yaml create mode 100644 examples/ik_conf/tuned_teleop_configs.yaml diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index 49e7d1e..44ae16b 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -1,76 +1,160 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller - REAL ROBOT CONTROL.""" +""" +Piper Robot Teleoperation Tuning - Real Robot Control. + +This script allows developers to iteratively tune the robot's teleoperation parameters +in real-time. It provides a Viser-based web UI where you can adjust Inverse Kinematics (IK) +weights, 1-Euro Filter lag/smoothness coefficients, and spatial scaling on the fly while +piloting the AgileX Piper arm using a Meta Quest controller. +""" import argparse import sys import threading import time import traceback +import yaml +import os from pathlib import Path import numpy as np # Add parent directory to path to import local modules sys.path.insert(0, str(Path(__file__).parent.parent)) +# Only import the fixed hardware constants that are left in configs.py from common.configs import ( - CAMERA_NAMES, META_QUEST_AXIS_MASK, ROTATION_SCALE, - SLOW_ROTATION_SCALE, SLOW_TRANSLATION_SCALE, TRANSLATION_SCALE, - URDF_PATH, VISUALIZATION_RATE, WRIST_JOINT_BUTTON_STEP_DEGREES, - CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF, - POSITION_COST, ORIENTATION_COST, FRAME_TASK_GAIN, LM_DAMPING, - DAMPING_COST, SOLVER_DAMPING_VALUE, POSTURE_COST_VECTOR + CAMERA_NAMES, META_QUEST_AXIS_MASK, URDF_PATH, VISUALIZATION_RATE ) from common.data_manager import RobotActivityState from common.system_bootstrap import bootstrap_robot_system from common.shared_actions import toggle_robot_enabled, move_robot_home from common.robot_visualizer import RobotVisualizer from common.threads.quest_reader import quest_reader_thread +from common.config_parser import load_ik_config from meta_quest_teleop.reader import MetaQuestReader from piper_controller import PiperController -def _step_wrist_joint(data_manager, robot_controller, delta_degrees: float) -> None: - """Apply a relative step to the wrist joint target angle.""" +# --------------------------------------------------------------------------- +# Dynamic Action Callbacks +# --------------------------------------------------------------------------- + +def _step_wrist_joint(data_manager, robot_controller, visualizer, direction: int) -> None: + """Apply a relative manual step to the wrist joint using the dynamically tuned step degree.""" data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) target_joint_angles = robot_controller.get_target_joint_angles() current_joint_angles = data_manager.get_current_joint_angles() + base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1]) - + + # Read the tuned value directly from the UI and apply direction multiplier (1 or -1) + delta_degrees = visualizer.get_wrist_step_degrees() * direction target_joint_angles[-1] = base_wrist + delta_degrees + robot_controller.set_target_joint_angles(target_joint_angles) data_manager.set_target_joint_angles(target_joint_angles) -def toggle_slow_scaling(data_manager): - """Toggle the movement scaling mode.""" +def toggle_slow_scaling(data_manager, visualizer): + """Toggle the movement scaling mode between standard and dynamically-tuned precision.""" enabled = data_manager.toggle_slow_scaling_mode_enabled() if enabled: - print(f"🐢 Slow scaling enabled (trans={SLOW_TRANSLATION_SCALE:.3f}, rot={SLOW_ROTATION_SCALE:.3f})") + print(f"🐢 Slow scaling enabled (trans={visualizer.get_slow_translation_scale():.3f}, rot={visualizer.get_slow_rotation_scale():.3f})") else: - print(f"🐇 Slow scaling disabled (trans={TRANSLATION_SCALE:.3f}, rot={ROTATION_SCALE:.3f})") + print(f"🐇 Slow scaling disabled (trans={visualizer.get_translation_scale():.3f}, rot={visualizer.get_rotation_scale():.3f})") + + +def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml") -> None: + """Extracts all current UI slider values and saves them to a YAML file.""" + try: + # Ensure the directory exists before saving + os.makedirs(os.path.dirname(filepath), exist_ok=True) + + pink_params = visualizer.get_pink_parameters() + min_c, beta, d_c = visualizer.get_controller_filter_params() + + # Cast to standard python floats/lists so PyYAML doesn't choke on numpy types + config_dict = { + "ik_parameters": { + "position_cost": float(pink_params["position_cost"]), + "orientation_cost": float(pink_params["orientation_cost"]), + "frame_task_gain": float(pink_params["frame_task_gain"]), + "lm_damping": float(pink_params["lm_damping"]), + "damping_cost": float(pink_params["damping_cost"]), + "solver_damping_value": float(pink_params["solver_damping_value"]), + "posture_cost_vector": pink_params["posture_cost_vector"].tolist(), + }, + "filter_parameters": { + "controller_min_cutoff": float(min_c), + "controller_beta": float(beta), + "controller_d_cutoff": float(d_c), + }, + "teleop_parameters": { + "translation_scale": float(visualizer.get_translation_scale()), + "rotation_scale": float(visualizer.get_rotation_scale()), + "slow_translation_scale": float(visualizer.get_slow_translation_scale()), + "slow_rotation_scale": float(visualizer.get_slow_rotation_scale()), + "wrist_joint_button_step_degrees": float(visualizer.get_wrist_step_degrees()), + } + } + + with open(filepath, 'w') as file: + yaml.dump(config_dict, file, default_flow_style=False, sort_keys=False) + print(f"\n✅ SUCCESS: Configuration securely saved to {os.path.abspath(filepath)}") + except Exception as e: + print(f"\n❌ ERROR: Failed to save YAML config: {e}") + +# --------------------------------------------------------------------------- +# Main Execution +# --------------------------------------------------------------------------- if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Piper Robot Teleoperation") - parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP") + parser = argparse.ArgumentParser(description="Piper Robot Teleoperation Tuning") + parser.add_argument("--ip-address", type=str, default=None) + parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.") args = parser.parse_args() - print("=" * 60 + "\nPIPER ROBOT TELEOPERATION\n" + "=" * 60) - # 1. Bootstrap Core System (Data Manager, Robot Controller, IK Solver, Base Threads) - data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( - start_ik=True, start_camera=True - ) + print("=" * 60 + "\nPIPER ROBOT TELEOPERATION TUNING\n" + "=" * 60) + + # Load the YAML dict + config = load_ik_config(args.ik_config) + ik_p = config.get("ik_parameters", {}) + filt_p = config.get("filter_parameters", {}) + tele_p = config.get("teleop_parameters", {}) + + # 1. Bootstrap Core System + data_manager, robot_controller, ik_solver, active_threads = \ + bootstrap_robot_system(config, start_ik=True, start_camera=True) - # 2. Initialize Quest Reader + # 2. Initialize Quest Reader & Handle ADB Exceptions print("\n🎮 Initializing Meta Quest reader...") - quest_reader = MetaQuestReader( - ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK - ) + try: + quest_reader = MetaQuestReader( + ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ) + except (Exception, SystemExit) as e: + print("\n" + "!" * 60) + print("❌ FAILED TO ACCESS META QUEST") + print("!" * 60) + print("The headset is plugged in, but ADB debugging permissions are missing.") + print("\nPLEASE FOLLOW THESE STEPS:") + print(" 1. Put the Meta Quest headset on your head.") + print(" 2. Look for a notification in your menu that says 'USB Detected'.") + print(" 3. Click on that notification and select 'Allow' to grant data access.") + print(" 4. Rerun this script.") + print("!" * 60 + "\n") + + data_manager.request_shutdown() + for t in active_threads: + t.join(timeout=1.0) + robot_controller.cleanup() + sys.exit(1) - # 3. Initialize Visualizer + # 3. Initialize Visualizer Web UI print("\n🖥️ Starting visualization...") visualizer = RobotVisualizer(urdf_path=URDF_PATH) visualizer.add_basic_controls() @@ -81,29 +165,42 @@ def toggle_slow_scaling(data_manager): visualizer.add_rgb_image_placeholder() visualizer.add_target_frame_visualization() visualizer.add_controller_visualization() - visualizer.add_controller_filter_controls(CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF) - visualizer.add_scaling_controls(TRANSLATION_SCALE, ROTATION_SCALE) + + # Add tuning sliders + visualizer.add_controller_filter_controls( + filt_p.get("controller_min_cutoff", 0.8), filt_p.get("controller_beta", 0.05), filt_p.get("controller_d_cutoff", 0.9) + ) + visualizer.add_scaling_controls( + tele_p.get("translation_scale", 1.5), tele_p.get("rotation_scale", 1.2) + ) visualizer.add_pink_parameter_controls( - POSITION_COST, ORIENTATION_COST, FRAME_TASK_GAIN, LM_DAMPING, - DAMPING_COST, SOLVER_DAMPING_VALUE, POSTURE_COST_VECTOR + ik_p.get("position_cost", 1.0), ik_p.get("orientation_cost", 0.75), ik_p.get("frame_task_gain", 0.4), + ik_p.get("lm_damping", 0.01), ik_p.get("damping_cost", 0.25), ik_p.get("solver_damping_value", 1e-4), + ik_p.get("posture_cost_vector", [0.0, 0.0, 0.0, 0.05, 0.0, 0.0]) + ) + visualizer.add_advanced_tuning_controls( + tele_p.get("slow_translation_scale", 0.6), tele_p.get("slow_rotation_scale", 0.6), tele_p.get("wrist_joint_button_step_degrees", 5.0) ) # 4. Bind Shared Callbacks (Quest Buttons & GUI Buttons) quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) - quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, WRIST_JOINT_BUTTON_STEP_DEGREES)) - quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -WRIST_JOINT_BUTTON_STEP_DEGREES)) - quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager)) + quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, visualizer, direction=1)) + quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, visualizer, direction=-1)) + quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager, visualizer)) visualizer.set_toggle_robot_enabled_status_callback(lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) visualizer.set_go_home_callback(lambda: move_robot_home(data_manager, robot_controller)) + + # Safely bind the save config callback with the correct arguments + visualizer.set_save_config_callback(lambda: save_config_to_yaml(visualizer, filepath='ik_conf/tuned_teleop_configs.yaml')) # 5. Start Quest Thread quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) quest_thread.start() active_threads.append(quest_thread) - print("\n🚀 Ready! Use Meta Quest controllers to move the robot. Press Ctrl+C to exit.\n") + print("\n🚀 Ready! Use Meta Quest controllers to move the robot. Open http://localhost:8080 to tune. Press Ctrl+C to exit.\n") # 6. Main Visualization Loop dt = 1.0 / VISUALIZATION_RATE @@ -111,9 +208,9 @@ def toggle_slow_scaling(data_manager): while True: iteration_start = time.time() - # Pass GUI scaling updates to Data Manager - t_scale = SLOW_TRANSLATION_SCALE if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_translation_scale() - r_scale = SLOW_ROTATION_SCALE if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_rotation_scale() + # Poll GUI for real-time scale tuning updates + t_scale = visualizer.get_slow_translation_scale() if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_translation_scale() + r_scale = visualizer.get_slow_rotation_scale() if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_rotation_scale() data_manager.set_teleop_scaling(t_scale, r_scale) # Retrieve State Data @@ -153,7 +250,7 @@ def toggle_slow_scaling(data_manager): visualizer.update_robot_status(f"Robot Status: {state.value.capitalize()}") visualizer.update_gripper_status(trigger_value, robot_enabled=(state == RobotActivityState.ENABLED)) - # Maintain Loop Rate + # Maintain UI refresh rate time.sleep(max(0, dt - (time.time() - iteration_start))) except KeyboardInterrupt: @@ -168,7 +265,7 @@ def toggle_slow_scaling(data_manager): data_manager.set_robot_activity_state(RobotActivityState.DISABLED) quest_reader.stop() for t in active_threads: - t.join() + t.join(timeout=2.0) robot_controller.cleanup() visualizer.stop() print("👋 Demo stopped.") \ No newline at end of file diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 3a29a76..11ac518 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -1,25 +1,48 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller and Neuracore data collection.""" +""" +Piper Robot Teleoperation and Neuracore Data Collection. + +This script facilitates real-time teleoperation of the AgileX Piper robotic arm +using Meta Quest controllers, while simultaneously streaming perfectly synchronized +telemetry data (joint states, RGB camera frames, gripper states) to the Neuracore cloud. + +Key Features: + - Connects to Neuracore and initializes a new dataset session. + - Bootstraps background threads for hardware control (CAN bus), IK solving, and camera streaming. + - Initializes the Meta Quest ADB bridge for VR controller tracking. + - Implements robust error handling for common ADB/USB debugging permission failures. + - Maps physical Quest controller buttons to robot state actions (Home, Enable, Record). + - Dynamically loads scaling and IK parameters from a YAML config file. + +Hardware Requirements: + - AgileX Piper robot arm connected via CAN interface ('can0'). + - RealSense camera or USB WebCam configured in `common.configs`. + - Meta Quest headset connected via USB with 'USB Debugging' explicitly allowed. + +Usage: + python 2_collect_teleop_data_with_neuracore.py --ip-address --dataset-name --ik-config +""" import argparse import multiprocessing -import subprocess import sys import threading import time import traceback from pathlib import Path + import numpy as np import neuracore as nc -# Add parent directory to path +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- +# Append parent directory to sys.path to resolve local 'common' modules. sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import ( - META_QUEST_AXIS_MASK, URDF_PATH, WRIST_JOINT_BUTTON_STEP_DEGREES, - SLOW_TRANSLATION_SCALE, SLOW_ROTATION_SCALE, TRANSLATION_SCALE, ROTATION_SCALE -) -from common.data_manager import RobotActivityState +from common.configs import META_QUEST_AXIS_MASK, URDF_PATH +from common.config_parser import load_ik_config +from common.data_manager import RobotActivityState, DataManager from common.system_bootstrap import bootstrap_robot_system from common.shared_actions import ( toggle_robot_enabled, move_robot_home, @@ -29,14 +52,21 @@ from meta_quest_teleop.reader import MetaQuestReader from piper_controller import PiperController +# --------------------------------------------------------------------------- +# Helper Functions +# --------------------------------------------------------------------------- -def _step_wrist_joint(data_manager, robot_controller, delta_degrees: float) -> None: - """Apply a relative step to the wrist joint target angle.""" +def _step_wrist_joint(data_manager: DataManager, robot_controller: PiperController, delta_degrees: float) -> None: + """ + Applies a relative step adjustment to the robot's wrist joint target angle. + """ data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) target_joint_angles = robot_controller.get_target_joint_angles() current_joint_angles = data_manager.get_current_joint_angles() + + # Fallback to target angles if hardware telemetry is temporarily unavailable base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1]) target_joint_angles[-1] = base_wrist + delta_degrees @@ -44,81 +74,134 @@ def _step_wrist_joint(data_manager, robot_controller, delta_degrees: float) -> N data_manager.set_target_joint_angles(target_joint_angles) -def toggle_slow_scaling(data_manager): +def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: + """ + Toggles the teleoperation sensitivity between standard and slow (precision) scaling. + """ enabled = data_manager.toggle_slow_scaling_mode_enabled() if enabled: - data_manager.set_teleop_scaling(SLOW_TRANSLATION_SCALE, SLOW_ROTATION_SCALE) - print("🐢 Slow scaling enabled") + data_manager.set_teleop_scaling( + tele_p.get("slow_translation_scale", 0.6), + tele_p.get("slow_rotation_scale", 0.6) + ) + print("🐢 Slow scaling (precision mode) enabled") else: - data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) - print("🐇 Slow scaling disabled") + data_manager.set_teleop_scaling( + tele_p.get("translation_scale", 1.5), + tele_p.get("rotation_scale", 1.2) + ) + print("🐇 Slow scaling disabled (standard mode)") if __name__ == "__main__": + # Ensure safe multiprocess spawning for UI/Background threads across different OS environments multiprocessing.set_start_method("spawn") + parser = argparse.ArgumentParser(description="Teleop with Neuracore Data Collection") - parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP") - parser.add_argument("--dataset-name", type=str, default=None) + parser.add_argument("--ip-address", type=str, default=None, help="IP address of the Meta Quest headset.") + parser.add_argument("--dataset-name", type=str, default=None, help="Override the auto-generated dataset name.") + parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.") args = parser.parse_args() print("=" * 60 + "\nPIPER TELEOP DATA COLLECTION\n" + "=" * 60) - - # 1. Connect to Neuracore & Create Dataset + + # Load the YAML configuration dictionary + config = load_ik_config(args.ik_config) + tele_p = config.get("teleop_parameters", {}) + wrist_step = tele_p.get("wrist_joint_button_step_degrees", 5.0) + + # --------------------------------------------------------- + # 1. Neuracore Initialization & Dataset Creation + # --------------------------------------------------------- print("\n🔧 Initializing Neuracore...") nc.login() nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) + # Auto-generate a timestamped dataset name if one was not provided via CLI dataset_name = args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" nc.create_dataset(name=dataset_name, description="Teleop data collection for Piper robot") - # 2. Bootstrap Core System + # --------------------------------------------------------- + # 2. Hardware & Subsystem Bootstrapping + # --------------------------------------------------------- data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( - start_ik=True, start_camera=True + config, start_ik=True, start_camera=True ) - # Wire Neuracore logging to DataManager + # Bind the unified Neuracore logger so state changes automatically push to the cloud data_manager.set_on_change_callback(neuracore_logging_callback) - # 3. Initialize Quest Reader & Bind Controls + # --------------------------------------------------------- + # 3. Meta Quest Initialization & Error Handling + # --------------------------------------------------------- print("\n🎮 Initializing Meta Quest reader...") - quest_reader = MetaQuestReader( - ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK - ) - + try: + quest_reader = MetaQuestReader( + ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ) + except (Exception, SystemExit) as e: + print("\n" + "!" * 60) + print("❌ FAILED TO ACCESS META QUEST") + print("!" * 60) + print("The headset is plugged in, but ADB debugging permissions are missing.") + print("\nPLEASE FOLLOW THESE STEPS:") + print(" 1. Put the Meta Quest headset on your head.") + print(" 2. Look for a notification in your menu that says 'USB Detected'.") + print(" 3. Click on that notification and select 'Allow' to grant data access.") + print(" 4. Rerun this script.") + print("!" * 60 + "\n") + + data_manager.request_shutdown() + for t in active_threads: + t.join(timeout=1.0) + robot_controller.cleanup() + nc.logout() + sys.exit(1) + + # --------------------------------------------------------- + # 4. Input Binding & Background Threading + # --------------------------------------------------------- quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller)) quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) quest_reader.on("button_rj_pressed", lambda: toggle_recording(play_audio=True)) - quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, WRIST_JOINT_BUTTON_STEP_DEGREES)) - quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -WRIST_JOINT_BUTTON_STEP_DEGREES)) - quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager)) + quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, wrist_step)) + quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -wrist_step)) + quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager, tele_p)) - # 4. Start Quest Thread quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) quest_thread.start() active_threads.append(quest_thread) print("\n🚀 Ready! Use Meta Quest controllers. Press Right Joystick to Record. Press Ctrl+C to exit.\n") - # 5. Wait Loop (Work happens in background threads) + # --------------------------------------------------------- + # 5. Main Daemon Loop + # --------------------------------------------------------- try: while not data_manager.is_shutdown_requested(): time.sleep(1) + except KeyboardInterrupt: print("\n👋 Interrupt received - shutting down gracefully...") data_manager.request_shutdown() except Exception as e: - print(f"\n❌ Demo error: {e}") + print(f"\n❌ Unhandled Demo error: {e}") data_manager.request_shutdown() - # 6. Cleanup - print("\n🧹 Cleaning up...") + # --------------------------------------------------------- + # 6. Graceful Teardown & Cleanup + # --------------------------------------------------------- + print("\n🧹 Cleaning up subsystems...") + if nc.is_recording(): nc.cancel_recording() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) quest_reader.stop() + for t in active_threads: - t.join() + t.join(timeout=2.0) + nc.logout() robot_controller.cleanup() print("👋 Demo stopped.") \ No newline at end of file diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index da510fb..831427a 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -1,5 +1,21 @@ #!/usr/bin/env python3 -"""Piper Robot Test with Neuracore policy.""" +""" +Piper Robot Policy Rollout and Visualization. + +This script serves as a production-ready entry point for executing and visualizing +AI policies trained via Neuracore on the AgileX Piper robotic arm. It handles: + 1. Establishing a connection to the Neuracore API and authenticating the hardware. + 2. Dynamically loading a trained policy via a local model, a training run, or a remote endpoint. + 3. Extracting and validating the expected input/output embodiment descriptions. + 4. Bootstrapping the core robot control and background state-management threads. + 5. Launching a Viser-based web UI for real-time 3D visualization and manual policy control. + 6. Managing background threads dedicated to continuous policy prediction and execution. + +Usage Examples: + python 4_rollout_neuracore_policy.py --model-path ./model.nc.zip + python 4_rollout_neuracore_policy.py --train-run-name my_awesome_training_run + python 4_rollout_neuracore_policy.py --remote-endpoint-name live_cloud_endpoint +""" import argparse import sys @@ -7,9 +23,10 @@ import time import traceback from pathlib import Path + import neuracore as nc -# Add parent directory to path +# Dynamically append the parent directory to sys.path to resolve local 'common' modules. sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( @@ -17,46 +34,65 @@ PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, ROBOT_RATE, ) from neuracore_types import DataType, EmbodimentDescription -from neuracore.ml.preprocessing.methods.resize_pad import ResizePad from common.data_manager import RobotActivityState from common.policy_state import PolicyState -from common.policy_helpers import embodiment_names_ordered +from common.policy_helpers import ( + embodiment_names_ordered, + get_policy_embodiments, + print_policy_embodiments +) from common.system_bootstrap import bootstrap_robot_system from common.shared_actions import toggle_robot_enabled, move_robot_home from common.robot_visualizer import RobotVisualizer -from common.threads.quest_reader import quest_reader_thread -from meta_quest_teleop.reader import MetaQuestReader -from common.policy_helpers import get_policy_embodiments, print_policy_embodiments -# Import the newly extracted policy actions +# Extracted policy execution and lifecycle management actions from common.policy_actions import ( run_policy, start_policy_execution, play_policy, policy_execution_thread ) if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Piper Robot Policy Test") - parser.add_argument("--ip-address", type=str, default=None) - parser.add_argument("--no-quest", action="store_true") - parser.add_argument("--continuous-mode", choices=["pipeline", "sequential"], default="sequential") + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- + parser = argparse.ArgumentParser( + description="Execute and visualize Neuracore policies on the Piper Robot." + ) + parser.add_argument( + "--continuous-mode", + choices=["pipeline", "sequential"], + default="sequential", + help="Execution strategy for the receding horizon loop." + ) + parser.add_argument( + "--robot-name", + type=str, + default="AgileX PiPER", + help="The registered hardware name in the Neuracore ecosystem." + ) + + # Require exactly one method of loading the policy policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument("--train-run-name", type=str, default=None) - policy_group.add_argument("--model-path", type=str, default=None) - policy_group.add_argument("--remote-endpoint-name", type=str, default=None) - parser.add_argument("--robot-name", type=str, default="AgileX PiPER") + policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.") + policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.") + policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.") + args = parser.parse_args() print("=" * 60 + "\nPIPER ROBOT TEST WITH NEURACORE POLICY\n" + "=" * 60) - # 1. Connect Neuracore & Define Embodiments + # --------------------------------------------------------- + # 2. Neuracore Initialization & Authentication + # --------------------------------------------------------- nc.login() nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + # --------------------------------------------------------- + # 3. Policy Loading & Embodiment Resolution + # --------------------------------------------------------- if args.remote_endpoint_name is not None: - print( - f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." - ) + print(f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") try: policy = nc.policy_remote_server(args.remote_endpoint_name) except nc.EndpointError: @@ -65,91 +101,126 @@ "Please start it from the Neuracore dashboard." ) sys.exit(1) + elif args.train_run_name is not None: - print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") + print(f"\n🤖 Loading policy from cloud training run: {args.train_run_name}...") policy = nc.policy( train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name, ) + else: + print(f"\n🤖 Loading policy from local model: {args.model_path}...") policy = nc.policy( model_file=args.model_path, device="cuda", robot_name=args.robot_name, ) - policy_state = PolicyState() - + # Dynamically extract and log what sensor streams the model expects to see input_emb, output_emb = get_policy_embodiments(policy) print_policy_embodiments(input_emb, output_emb) + # Initialize shared policy state container + policy_state = PolicyState() policy_state.set_execution_mode(PolicyState.ExecutionMode.TARGETING_TIME) - # 2. Bootstrap Core System - data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system(start_ik=True, start_camera=True) - - # 3. Initialize Quest Reader (Conditional) - quest_reader = None - if not args.no_quest: - print("\n🎮 Initializing Meta Quest reader...") - quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) - quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) - quest_thread.start() - active_threads.append(quest_thread) + # --------------------------------------------------------- + # 4. Hardware & Subsystem Bootstrapping + # --------------------------------------------------------- + # Instantiates the shared DataManager, CAN hardware interface, IK solver, + # and base telemetry threads (joint states, camera streams). + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + start_ik=True, start_camera=True + ) - # 4. Initialize Visualizer & Bind Callbacks - print("\n🖥️ Starting Viser visualization...") + # --------------------------------------------------------- + # 5. Visualization & UI Setup + # --------------------------------------------------------- + print("\n🖥️ Starting Viser visualization server...") visualizer = RobotVisualizer(str(URDF_PATH)) - visualizer.add_policy_controls(PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, ROBOT_RATE, "targeting_time") + + # Inject UI components + visualizer.add_policy_controls( + PREDICTION_HORIZON_EXECUTION_RATIO, + POLICY_EXECUTION_RATE, + ROBOT_RATE, + "targeting_time" + ) visualizer.add_toggle_robot_enabled_status_button() visualizer.add_homing_controls() visualizer.add_policy_buttons() - # Shared Button Binds - visualizer.set_toggle_robot_enabled_status_callback(lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) - visualizer.set_go_home_callback(lambda: move_robot_home(data_manager, robot_controller)) - if quest_reader: - quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) - quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) + # Bind hardware action callbacks to the UI buttons + visualizer.set_toggle_robot_enabled_status_callback( + lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer) + ) + visualizer.set_go_home_callback( + lambda: move_robot_home(data_manager, robot_controller) + ) - # Policy Button Binds - visualizer.set_run_policy_callback(lambda: run_policy(data_manager, policy, policy_state, visualizer, input_emb)) - visualizer.set_start_policy_execution_callback(lambda: start_policy_execution(data_manager, policy_state)) - visualizer.set_play_policy_callback(lambda: play_policy(data_manager, policy, policy_state, visualizer, input_emb, args.continuous_mode)) - visualizer.set_execution_mode_callback(lambda: policy_state.set_execution_mode(PolicyState.ExecutionMode(visualizer.get_execution_mode()))) + # Bind policy execution callbacks to the UI buttons + visualizer.set_run_policy_callback( + lambda: run_policy(data_manager, policy, policy_state, visualizer, input_emb) + ) + visualizer.set_start_policy_execution_callback( + lambda: start_policy_execution(data_manager, policy_state) + ) + visualizer.set_play_policy_callback( + lambda: play_policy(data_manager, policy, policy_state, visualizer, input_emb, args.continuous_mode) + ) + visualizer.set_execution_mode_callback( + lambda: policy_state.set_execution_mode(PolicyState.ExecutionMode(visualizer.get_execution_mode())) + ) - # 5. Start Policy Execution Thread + # --------------------------------------------------------- + # 6. Dispatch Policy Execution Thread + # --------------------------------------------------------- policy_exec_thread = threading.Thread( target=policy_execution_thread, args=(policy, data_manager, policy_state, robot_controller, visualizer, input_emb), - daemon=True + daemon=True, + name="PolicyExecutionWorker" ) policy_exec_thread.start() active_threads.append(policy_exec_thread) - print("\n🚀 Ready! Check http://localhost:8080 to visualize and run policies.\n") + print("\n🚀 System Online! Open http://localhost:8080 in your browser to visualize and run the policy.\n") - # 6. Main Loop + # --------------------------------------------------------- + # 7. Main Daemon Loop + # --------------------------------------------------------- try: + # Keep the main thread alive. All heavy lifting is handled via background threads. while True: time.sleep(1) + except KeyboardInterrupt: - print("\n👋 Shutting down gracefully...") + print("\n👋 Interrupt signal received. Initiating graceful shutdown...") except Exception as e: - print(f"\n❌ Demo error: {e}") + print(f"\n❌ Unhandled Exception in main loop: {e}") traceback.print_exc() - # 7. Cleanup - print("\n🧹 Cleaning up...") + # --------------------------------------------------------- + # 8. Graceful Teardown & Cleanup + # --------------------------------------------------------- + print("\n🧹 Cleaning up subsystems...") + + # Safely sever the connection to the policy backend policy.disconnect() + + # Broadcast shutdown signal to all worker threads data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - if quest_reader: - quest_reader.stop() - for t in active_threads: - t.join() + + # Wait for daemon threads to finish their current execution cycles + for thread in active_threads: + thread.join(timeout=2.0) + + # Relinquish hardware interfaces and network ports robot_controller.cleanup() visualizer.stop() nc.logout() - print("👋 Demo stopped.") \ No newline at end of file + + print("👋 Shutdown complete. Goodbye.") \ No newline at end of file diff --git a/examples/common/config_parser.py b/examples/common/config_parser.py new file mode 100644 index 0000000..cab642f --- /dev/null +++ b/examples/common/config_parser.py @@ -0,0 +1,12 @@ +import yaml +from pathlib import Path + +def load_ik_config(config_path: str) -> dict: + """Loads the teleoperation and IK parameters from a YAML file.""" + path = Path(config_path) + if not path.exists(): + print(f"⚠️ Warning: Config file not found at {path}. Using safe fallback defaults.") + return {} + + with open(path, 'r') as f: + return yaml.safe_load(f) \ No newline at end of file diff --git a/examples/common/configs.py b/examples/common/configs.py index 7b7cabc..fd5646d 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -1,84 +1,43 @@ -"""Configuration parameters for Piper robot demos.""" - +"""Configuration parameters for AgileX Piper robot demos.""" from pathlib import Path -URDF_PATH = str( - Path(__file__).parent.parent.parent - / "piper_description" - / "urdf" - / "piper_description.urdf" -) - +# Hardware & Kinematics Paths +URDF_PATH = str(Path(__file__).parent.parent.parent / "piper_description" / "urdf" / "piper_description.urdf") GRIPPER_FRAME_NAME = "gripper_center" - -# Pink IK parameters +# IK Solver SOLVER_NAME = "quadprog" -POSITION_COST = 1.0 -ORIENTATION_COST = 0.75 -FRAME_TASK_GAIN = 0.4 -LM_DAMPING = 0.01 #0.0 -DAMPING_COST = 0.25 -SOLVER_DAMPING_VALUE = 1e-4 #1e-12 - -# Controller 1€ Filter parameters -CONTROLLER_MIN_CUTOFF = 0.8 # Minimum cutoff frequency (stabilizes when holding still) -CONTROLLER_BETA = 0.05 #5.0 # Speed coefficient (reduces lag when moving) -CONTROLLER_D_CUTOFF = 0.9 # Derivative cutoff frequency -# Controller parameters -GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control +# Teleoperation Thresholds +GRIP_THRESHOLD = 0.9 -# Scaling factors for translation and rotation -TRANSLATION_SCALE = 1.5 -ROTATION_SCALE = 1.2 -SLOW_TRANSLATION_SCALE = 0.6 -SLOW_ROTATION_SCALE = 0.6 -WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 +# Thread Execution Rates (Hz) +CONTROLLER_DATA_RATE = 50.0 +IK_SOLVER_RATE = 100 +VISUALIZATION_RATE = 60.0 +ROBOT_RATE = 100.0 +JOINT_STATE_STREAMING_RATE = 100.0 +CAMERA_FRAME_STREAMING_RATE = 30.0 -# Thread rates (Hz) -CONTROLLER_DATA_RATE = 50.0 # Controller input reading -IK_SOLVER_RATE = 100 #250.0 # IK solving and robot commands -VISUALIZATION_RATE = 60.0 # GUI updates -ROBOT_RATE = 100.0 - -# Neuracore data collection rates -JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore -CAMERA_FRAME_STREAMING_RATE = 30.0 # Data collection rate for camera frame - -# Meta quest axis mask -META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] # [x, y, z, roll, pitch, yaw] - -# USB webcam (OpenCV) configuration -CAMERA_DEVICE_INDEX = 5 # 0 = first camera, 1 = second, etc. +# Hardware Context +META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] +CAMERA_DEVICE_INDEX = 5 CAMERA_WIDTH = 640 CAMERA_HEIGHT = 480 +CAMERA_NAMES = ["rgb_scene", "rgb_wrist"] -# # Initial neutral pose for robot (mm and degrees) -# NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] -# NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] -#NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick task pruthvi -NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid -# Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) - +# Default Robot Poses +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] +#NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick task pruthvi -# Posture task cost vector (one weight per joint) -POSTURE_COST_VECTOR = [0.0, 0.0, 0.0, 0.05, 0.0, 0.0] - - -POLICY_EXECUTION_RATE = 20.0 # Hz -PREDICTION_HORIZON_EXECUTION_RATIO = ( - 1.0 # percentage of the prediction horizon that is executed -) -MAX_SAFETY_THRESHOLD = 200.0 # degrees -MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees -TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds +# AI Policy Execution Parameters +POLICY_EXECUTION_RATE = 20.0 +PREDICTION_HORIZON_EXECUTION_RATIO = 1.0 +MAX_SAFETY_THRESHOLD = 200.0 +MAX_ACTION_ERROR_THRESHOLD = 3.0 +TARGETING_POSE_TIME_THRESHOLD = 1.0 GRIPPER_NAME = "gripper" GRIPPER_LOGGING_NAME = GRIPPER_NAME - -# Camera logging names for Neuracore (scene + wrist) -CAMERA_NAMES = ["rgb_scene", "rgb_wrist"] - JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] diff --git a/examples/common/system_bootstrap.py b/examples/common/system_bootstrap.py index e65621d..8949ec3 100644 --- a/examples/common/system_bootstrap.py +++ b/examples/common/system_bootstrap.py @@ -3,12 +3,8 @@ from typing import Tuple, List, Optional from common.configs import ( - CONTROLLER_BETA, CONTROLLER_D_CUTOFF, CONTROLLER_MIN_CUTOFF, ROBOT_RATE, NEUTRAL_JOINT_ANGLES, NEUTRAL_END_EFFECTOR_POSE, - URDF_PATH, GRIPPER_FRAME_NAME, SOLVER_NAME, POSITION_COST, - ORIENTATION_COST, FRAME_TASK_GAIN, LM_DAMPING, DAMPING_COST, - SOLVER_DAMPING_VALUE, IK_SOLVER_RATE, POSTURE_COST_VECTOR, - TRANSLATION_SCALE, ROTATION_SCALE + URDF_PATH, GRIPPER_FRAME_NAME, SOLVER_NAME, IK_SOLVER_RATE ) from common.data_manager import DataManager from piper_controller import PiperController @@ -18,17 +14,27 @@ from common.threads.realsense_camera import camera_thread def bootstrap_robot_system( + config: dict, start_ik: bool = True, start_camera: bool = True ) -> Tuple[DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread]]: - """Initializes DataManager, PiperController, IK, and base background threads.""" + # Extract config sections safely + filt_p = config.get("filter_parameters", {}) + tele_p = config.get("teleop_parameters", {}) + ik_p = config.get("ik_parameters", {}) + # 1. Initialize Data Manager data_manager = DataManager() data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF + filt_p.get("controller_min_cutoff", 0.8), + filt_p.get("controller_beta", 0.05), + filt_p.get("controller_d_cutoff", 0.9) + ) + data_manager.set_teleop_scaling( + tele_p.get("translation_scale", 1.5), + tele_p.get("rotation_scale", 1.2) ) - data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) # 2. Initialize Robot Controller print("\n🤖 Initializing Piper robot controller...") @@ -45,11 +51,8 @@ def bootstrap_robot_system( # 3. Start Threads active_threads = [] - print("\n📊 Starting joint state thread...") - js_thread = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) + js_thread = threading.Thread(target=joint_state_thread, args=(data_manager, robot_controller), daemon=True) js_thread.start() active_threads.append(js_thread) @@ -61,26 +64,25 @@ def bootstrap_robot_system( ik_solver = PinkIKSolver( urdf_path=URDF_PATH, end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, + solver_name=SOLVER_NAME, + position_cost=ik_p.get("position_cost", 1.0), + orientation_cost=ik_p.get("orientation_cost", 0.75), + frame_task_gain=ik_p.get("frame_task_gain", 0.4), + lm_damping=ik_p.get("lm_damping", 0.01), + damping_cost=ik_p.get("damping_cost", 0.25), + solver_damping_value=ik_p.get("solver_damping_value", 1e-4), integration_time_step=1 / IK_SOLVER_RATE, initial_configuration=init_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), + posture_cost_vector=np.array(ik_p.get("posture_cost_vector", [0.0, 0.0, 0.0, 0.05, 0.0, 0.0])), ) print("\n🧮 Starting IK solver thread...") - ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ) + ik_thread = threading.Thread(target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True) ik_thread.start() active_threads.append(ik_thread) if start_camera: print("\n📷 Starting camera thread...") - cam_thread = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ) + cam_thread = threading.Thread(target=camera_thread, args=(data_manager,), daemon=True) cam_thread.start() active_threads.append(cam_thread) diff --git a/examples/common/visualizer_gui.py b/examples/common/visualizer_gui.py index b268062..7852e99 100644 --- a/examples/common/visualizer_gui.py +++ b/examples/common/visualizer_gui.py @@ -51,7 +51,31 @@ def __init__(self, server: viser.ViserServer) -> None: self._start_policy_execution_button = None self._play_policy_button = None - # --- UI Setup Methods --- + self._slow_translation_scale_handle = None + self._slow_rotation_scale_handle = None + self._wrist_step_handle = None + self._save_config_button = None + + def add_advanced_tuning_controls(self, initial_slow_t: float, initial_slow_r: float, initial_wrist_step: float) -> None: + """Adds advanced sliders for precision movement and button toggles.""" + self._slow_translation_scale_handle = self.server.gui.add_number("Slow Translation Scale", initial_slow_t, min=0.1, max=5.0, step=0.01) + self._slow_rotation_scale_handle = self.server.gui.add_number("Slow Rotation Scale", initial_slow_r, min=0.1, max=5.0, step=0.01) + self._wrist_step_handle = self.server.gui.add_number("Wrist Nudge (Degrees)", initial_wrist_step, min=1.0, max=45.0, step=1.0) + self._save_config_button = self.server.gui.add_button("💾 Save Config to YAML") + + def get_slow_translation_scale(self) -> float: + return self._slow_translation_scale_handle.value + + def get_slow_rotation_scale(self) -> float: + return self._slow_rotation_scale_handle.value + + def get_wrist_step_degrees(self) -> float: + return self._wrist_step_handle.value + + def set_save_config_callback(self, cb: Callable[[], Any]) -> None: + if self._save_config_button: + self._save_config_button.on_click(lambda _: cb()) + def add_basic_controls(self) -> None: self._timing_handle = self.server.gui.add_number("IK Solve Time (ms)", 0.001, disabled=True) self._joint_angles_handle = self.server.gui.add_text("Joint Angles", "Waiting...") diff --git a/examples/ik_conf/default.yaml b/examples/ik_conf/default.yaml new file mode 100644 index 0000000..c6568dd --- /dev/null +++ b/examples/ik_conf/default.yaml @@ -0,0 +1,24 @@ +ik_parameters: + position_cost: 1.0 + orientation_cost: 0.75 + frame_task_gain: 0.4 + lm_damping: 0.01 + damping_cost: 0.25 + solver_damping_value: 0.0001 + posture_cost_vector: + - 0.0 + - 0.0 + - 0.0 + - 0.05 + - 0.0 + - 0.0 +filter_parameters: + controller_min_cutoff: 0.8 + controller_beta: 0.05 + controller_d_cutoff: 0.9 +teleop_parameters: + translation_scale: 1.5 + rotation_scale: 1.2 + slow_translation_scale: 0.6 + slow_rotation_scale: 0.6 + wrist_joint_button_step_degrees: 5.0 diff --git a/examples/ik_conf/tuned_teleop_configs.yaml b/examples/ik_conf/tuned_teleop_configs.yaml new file mode 100644 index 0000000..38369da --- /dev/null +++ b/examples/ik_conf/tuned_teleop_configs.yaml @@ -0,0 +1,24 @@ +ik_parameters: + position_cost: 1.0 + orientation_cost: 0.75 + frame_task_gain: 0.4 + lm_damping: 0.01 + damping_cost: 0.25 + solver_damping_value: 0.0001 + posture_cost_vector: + - 0.0 + - 0.0 + - 0.0 + - 0.05 + - 0.0 + - 0.0 +filter_parameters: + controller_min_cutoff: 0.8 + controller_beta: 0.05 + controller_d_cutoff: 0.9 +teleop_parameters: + translation_scale: 1.5 + rotation_scale: 1.2 + slow_translation_scale: 0.6 + slow_rotation_scale: 0.6 + wrist_joint_button_step_degrees: 7.0 From 65bf72efaf5514dc6e733c07ef3d114e8e28bea8 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 11:43:56 +0100 Subject: [PATCH 13/22] fix:new example 4 is working with yaml ik configuration --- examples/4_rollout_neuracore_policy.py | 44 ++++++++++++++++++++------ 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 831427a..2c089e0 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -7,14 +7,13 @@ 1. Establishing a connection to the Neuracore API and authenticating the hardware. 2. Dynamically loading a trained policy via a local model, a training run, or a remote endpoint. 3. Extracting and validating the expected input/output embodiment descriptions. - 4. Bootstrapping the core robot control and background state-management threads. + 4. Bootstrapping the core robot control and background state-management threads using a YAML config. 5. Launching a Viser-based web UI for real-time 3D visualization and manual policy control. 6. Managing background threads dedicated to continuous policy prediction and execution. Usage Examples: - python 4_rollout_neuracore_policy.py --model-path ./model.nc.zip - python 4_rollout_neuracore_policy.py --train-run-name my_awesome_training_run - python 4_rollout_neuracore_policy.py --remote-endpoint-name live_cloud_endpoint + python 4_rollout_neuracore_policy.py --model-path ./model.nc.zip --ik-config ik_conf/default.yaml + python 4_rollout_neuracore_policy.py --remote-endpoint-name test_deploy """ import argparse @@ -42,6 +41,7 @@ get_policy_embodiments, print_policy_embodiments ) +from common.config_parser import load_ik_config from common.system_bootstrap import bootstrap_robot_system from common.shared_actions import toggle_robot_enabled, move_robot_home from common.robot_visualizer import RobotVisualizer @@ -71,6 +71,12 @@ default="AgileX PiPER", help="The registered hardware name in the Neuracore ecosystem." ) + parser.add_argument( + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to IK/teleop YAML configuration file." + ) # Require exactly one method of loading the policy policy_group = parser.add_mutually_exclusive_group(required=True) @@ -83,8 +89,11 @@ print("=" * 60 + "\nPIPER ROBOT TEST WITH NEURACORE POLICY\n" + "=" * 60) # --------------------------------------------------------- - # 2. Neuracore Initialization & Authentication + # 2. Configuration & Neuracore Initialization # --------------------------------------------------------- + # Load the YAML configuration dictionary + config = load_ik_config(args.ik_config) + nc.login() nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) @@ -118,8 +127,23 @@ robot_name=args.robot_name, ) - # Dynamically extract and log what sensor streams the model expects to see - input_emb, output_emb = get_policy_embodiments(policy) + # Dynamically extract what sensor streams the model expects to see. + # Remote endpoints (policy_remote_server) do not expose these attributes, + # so we catch the AttributeError and fall back to the default Piper embodiment. + try: + input_emb, output_emb = get_policy_embodiments(policy) + except AttributeError: + print("\n⚠️ Could not dynamically extract embodiments from remote endpoint. Using default Piper configuration...") + input_emb = { + DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]}, + } + output_emb = { + DataType.JOINT_TARGET_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + } + print_policy_embodiments(input_emb, output_emb) # Initialize shared policy state container @@ -130,9 +154,9 @@ # 4. Hardware & Subsystem Bootstrapping # --------------------------------------------------------- # Instantiates the shared DataManager, CAN hardware interface, IK solver, - # and base telemetry threads (joint states, camera streams). + # and base telemetry threads using the parsed YAML config parameters. data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( - start_ik=True, start_camera=True + config, start_ik=True, start_camera=True ) # --------------------------------------------------------- @@ -141,7 +165,7 @@ print("\n🖥️ Starting Viser visualization server...") visualizer = RobotVisualizer(str(URDF_PATH)) - # Inject UI components + # Inject UI components using fixed AI policy rates visualizer.add_policy_controls( PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, From 390729230836c1a916aa18ee8e3a77838d8fb2c1 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 11:54:40 +0100 Subject: [PATCH 14/22] fix:example 3 is working with our new architecture --- examples/3_replay_neuracore_episodes.py | 201 ++++++++++++++++++++---- examples/common/dataset_helpers.py | 18 ++- 2 files changed, 183 insertions(+), 36 deletions(-) diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index 391db93..3a5cdc3 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -1,16 +1,53 @@ #!/usr/bin/env python3 -"""Replay a recorded Neuracore dataset on the Piper robot.""" +""" +Piper Robot Dataset Replay and Validation. + +This script acts as a production-grade utility for replaying recorded Neuracore +teleoperation datasets directly on the physical AgileX Piper robotic arm. + +============================================================================= +⚠️ HALID'S SAFETY NOTES & OPERATIONAL WARNINGS: + 1. DANGER: This step can be dangerous. The robot will start moving on the + exact trajectory that was recorded. ALWAYS be ready to press the emergency stop. + 2. EPISODE INDEXING: The episode index starts at 0, not 1. If you see an + episode numbered 'X' in the Neuracore frontend, you must run index 'X-1'. + 3. RERUN INSTABILITY: You may experience issues running this command successfully + more than once in a row without a system reset. + 4. FREQUENCY: The default playback frequency should be 20 Hz. Setting the + frequency to 0 plays the data aperiodically (as it was recorded). + 5. EMERGENCY STOP: Pressing Ctrl+C will gracefully disable the robot and cut + power to the motors after 5 seconds. +============================================================================= + +Hardware Requirements: + - AgileX Piper robot arm connected via CAN interface ('can0'). + +Usage Examples: + # Play the first episode (index 0) at the default 20 Hz: + python 3_replay_neuracore_episodes.py --dataset-name my_dataset + + # Play a specific episode index (e.g., frontend episode 3 -> index 2): + python 3_replay_neuracore_episodes.py --dataset-name my_dataset --episode-index 2 + + # Play aperiodically (as recorded): + python 3_replay_neuracore_episodes.py --dataset-name my_dataset --frequency 0 +""" import argparse import sys import time from pathlib import Path from typing import cast + import cv2 import neuracore as nc import numpy as np from tqdm import tqdm +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- +# Dynamically append the parent directory to sys.path to resolve local 'common' modules. sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import GRIPPER_NAME, JOINT_NAMES, NEUTRAL_JOINT_ANGLES, ROBOT_RATE @@ -18,87 +55,191 @@ from neuracore_types import DataType, SynchronizedPoint from piper_controller import PiperController -def wait_for_home(robot_controller, timeout: int = 10): +# --------------------------------------------------------------------------- +# Helper Functions +# --------------------------------------------------------------------------- +def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: + """ + Commands the robot to its home position and blocks until it arrives. + """ robot_controller.move_to_home() - for _ in range(timeout): + for remaining in range(timeout, 0, -1): if robot_controller.is_robot_homed(): - print("✓ Robot is at home position.") + print("✓ Robot has safely reached the home position.") return True + print(f" Waiting for robot to home... ({remaining}s remaining)", end="\r") time.sleep(1) - print("❌ Robot did not reach home position.") + + print("\n❌ Error: Robot did not reach home position within the timeout.") return False +# --------------------------------------------------------------------------- +# Main Execution +# --------------------------------------------------------------------------- if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("--dataset-name", type=str, required=True) - parser.add_argument("--frequency", type=int, required=True) - parser.add_argument("--episode-index", type=int, default=0) + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- + parser = argparse.ArgumentParser(description="Replay Neuracore datasets on the Piper robot.") + parser.add_argument( + "--dataset-name", type=str, required=True, + help="Name of the Neuracore dataset to load." + ) + parser.add_argument( + "--frequency", type=int, default=20, + help="Playback frequency in Hz. 0 plays the data aperiodically (default: 20)." + ) + parser.add_argument( + "--episode-index", type=int, default=0, + help="Episode index to replay (Frontend Number - 1). -1 plays all sequentially (default: 0)." + ) args = parser.parse_args() + print("=" * 60) + print("PIPER DATASET REPLAY".center(60)) + print("=" * 60) + print("⚠️ WARNING: Robot will move identically to the recorded trajectory.") + print("⚠️ Be ready to press the emergency stop or Ctrl+C.") + if args.frequency == 0: + print("⏱️ Playback set to APERIODIC (Frequency = 0).") + else: + print(f"⏱️ Playback synchronized at {args.frequency} Hz.") + print("=" * 60) + + # --------------------------------------------------------- + # 2. Hardware Initialization + # --------------------------------------------------------- print("\n🤖 Initializing Piper robot controller...") robot_controller = PiperController( - can_interface="can0", robot_rate=ROBOT_RATE, + can_interface="can0", + robot_rate=ROBOT_RATE, control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, debug_mode=False, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + debug_mode=False, ) robot_controller.start_control_loop() + # --------------------------------------------------------- + # 3. Neuracore Connection & Dataset Synchronization + # --------------------------------------------------------- print("\n🔑 Logging in to Neuracore...") nc.login() input_mods = [DataType.JOINT_POSITIONS, DataType.RGB_IMAGES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] output_mods = [DataType.JOINT_TARGET_POSITIONS, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - synced_dataset = load_and_sync_dataset(args.dataset_name, args.frequency, input_mods, output_mods) + synced_dataset = load_and_sync_dataset( + args.dataset_name, + args.frequency, + input_mods, + output_mods + ) - episode_indices = list(range(len(synced_dataset))) if args.episode_index == -1 else [args.episode_index] - print(f"\n📊 Playing {len(episode_indices)} episode(s).") + if len(synced_dataset) == 0: + print("❌ Error: The synchronized dataset is empty. Exiting.") + robot_controller.stop_control_loop() + robot_controller.cleanup() + sys.exit(1) + + if args.episode_index == -1: + episode_indices = list(range(len(synced_dataset))) + print(f"\n📊 Found {len(synced_dataset)} episodes. Will play all sequentially.") + else: + if args.episode_index >= len(synced_dataset) or args.episode_index < 0: + print(f"❌ Error: Index {args.episode_index} is out of bounds (Dataset has {len(synced_dataset)} episodes).") + print("💡 Reminder: If the frontend says episode '3', you must request index '2'.") + robot_controller.stop_control_loop() + robot_controller.cleanup() + sys.exit(1) + + episode_indices = [args.episode_index] + print(f"\n📊 Playing episode {args.episode_index} only.") + # --------------------------------------------------------- + # 4. Playback Loop + # --------------------------------------------------------- try: for episode_idx in episode_indices: - if not wait_for_home(robot_controller): continue - - print(f"\n{'='*60}\n🎬 Playing Episode {episode_idx} / {len(synced_dataset) - 1}\n{'='*60}") episode = synced_dataset[episode_idx] + + if len(episode) == 0: + print(f"⚠️ Warning: Episode {episode_idx} is empty. Skipping...") + continue + + if not wait_for_home(robot_controller): + print("⚠️ Skipping episode due to homing failure.") + continue - rgb_frames_per_step, parallel_gripper_open_amounts, joint_positions = [], [], [] + print(f"\n{'='*60}\n🎬 Extracting Episode {episode_idx} / {len(synced_dataset) - 1}\n{'='*60}") - for step in tqdm(episode, desc=f"Collecting data"): + rgb_frames_per_step = [] + parallel_gripper_open_amounts = [] + joint_positions = [] + + # Pre-compute and Unpack + for step in tqdm(episode, desc="Unpacking frames into memory"): step = cast(SynchronizedPoint, step) - # Extract Joints j_data = step.data.get(DataType.JOINT_TARGET_POSITIONS, {}) - joint_positions.append([j_data[jn].value for jn in JOINT_NAMES if jn in j_data]) + if all(jn in j_data for jn in JOINT_NAMES): + joint_positions.append([j_data[jn].value for jn in JOINT_NAMES]) + else: + if len(joint_positions) > 0: + joint_positions.append(joint_positions[-1]) + else: + continue - # Extract Gripper g_data = step.data.get(DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, {}) - parallel_gripper_open_amounts.append(g_data[GRIPPER_NAME].open_amount if GRIPPER_NAME in g_data else 0.0) + parallel_gripper_open_amounts.append( + g_data[GRIPPER_NAME].open_amount if GRIPPER_NAME in g_data else 0.0 + ) - # Extract Camera rgb_data = step.data.get(DataType.RGB_IMAGES, {}) rgb_frames_per_step.append({c: img.frame for c, img in rgb_data.items()}) + if not joint_positions: + print(f"⚠️ Warning: No valid joint data found in episode {episode_idx}. Skipping...") + continue + joint_positions = np.degrees(np.array(joint_positions)) - print(f"\n🚀 Replaying episode {episode_idx}...") + # Hardware Execution + print(f"\n🚀 Replaying episode {episode_idx} on hardware...") for idx in tqdm(range(len(joint_positions)), desc="Replaying"): start_time = time.time() + robot_controller.set_target_joint_angles(joint_positions[idx]) robot_controller.set_gripper_open_value(parallel_gripper_open_amounts[idx]) if idx < len(rgb_frames_per_step): for cam_name, frame_rgb in rgb_frames_per_step[idx].items(): - cv2.imshow(f"Replay: {cam_name}", cv2.cvtColor(np.asarray(frame_rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR)) + frame_bgr = cv2.cvtColor(np.asarray(frame_rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR) + cv2.imshow(f"Replay: {cam_name}", frame_bgr) + if cv2.waitKey(1) & 0xFF == ord("q"): - print("\n🛑 'q' pressed, stopping replay..."); break + print("\n🛑 'q' pressed, skipping remainder of replay...") + break - time.sleep(max(0, 1 / args.frequency - (time.time() - start_time))) + # Handle Frequency Timing (0 = aperiodic, >0 = synchronized) + if args.frequency > 0: + elapsed = time.time() - start_time + time.sleep(max(0.0, (1.0 / args.frequency) - elapsed)) cv2.destroyAllWindows() except KeyboardInterrupt: - print("\n🛑 Keyboard interrupt detected.") + print("\n🛑 Keyboard interrupt detected. Halting playback...") + print("⚠️ Gracefully disabling the robot. Power to motors will be cut off.") + cv2.destroyAllWindows() + except Exception as e: + print(f"\n❌ Unhandled exception during playback: {e}") cv2.destroyAllWindows() + # --------------------------------------------------------- + # 5. Graceful Teardown & Cleanup + # --------------------------------------------------------- + print("\n🧹 Cleaning up subsystems...") + # Triggering the cleanup gracefully disables the robot (cutting power securely) robot_controller.stop_control_loop() - robot_controller.cleanup() \ No newline at end of file + robot_controller.cleanup() + print("👋 Replay complete.") \ No newline at end of file diff --git a/examples/common/dataset_helpers.py b/examples/common/dataset_helpers.py index 0eab6fe..a0676a0 100644 --- a/examples/common/dataset_helpers.py +++ b/examples/common/dataset_helpers.py @@ -29,12 +29,18 @@ def load_and_sync_dataset( ) print("🔁 Synchronizing dataset...") - synced_dataset = dataset.synchronize( - frequency=frequency, - cross_embodiment_union=cross_embodiment_union, - prefetch_videos=prefetch_videos, - max_prefetch_workers=2 if prefetch_videos else 0 - ) + + # Dynamically build arguments to avoid passing 0 workers to the ThreadPoolExecutor + sync_kwargs = { + "frequency": frequency, + "cross_embodiment_union": cross_embodiment_union, + "prefetch_videos": prefetch_videos + } + + if prefetch_videos: + sync_kwargs["max_prefetch_workers"] = 2 + + synced_dataset = dataset.synchronize(**sync_kwargs) print(f" ✓ Dataset synchronized: {len(synced_dataset)} episodes") return synced_dataset \ No newline at end of file From 27408fb7dc412935fb745734fadac440b09ae4c8 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 12:01:20 +0100 Subject: [PATCH 15/22] fix:let the example 5 work with our new setup --- .../5_rollout_neuracore_policy_minimal.py | 165 +++++++++++++++--- 1 file changed, 137 insertions(+), 28 deletions(-) diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 7815694..67cb266 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -1,25 +1,64 @@ #!/usr/bin/env python3 -"""Minimal Piper Robot Policy Test - Terminal only, no GUI.""" +""" +Minimal Piper Robot Policy Rollout (Headless). + +This script serves as a lightweight, terminal-only entry point for executing +AI policies trained via Neuracore on the AgileX Piper robotic arm. It removes +the overhead of the Viser 3D GUI and Meta Quest tracking, making it ideal for +deployment on constrained compute environments or for automated testing. + +Key Features: + - Connects to Neuracore and loads policies (local, cloud run, or remote endpoint). + - Bootstraps the core hardware control (CAN bus) and camera telemetry without GUI/IK overhead. + - Extracts configuration settings dynamically from a YAML file. + - Executes a continuous predict-and-act loop directly in the terminal. + +Usage Examples: + python 5_rollout_neuracore_policy_minimal.py --model-path ./model.nc.zip + python 5_rollout_neuracore_policy_minimal.py --train-run-name my_awesome_training_run + python 5_rollout_neuracore_policy_minimal.py --remote-endpoint-name live_cloud_endpoint +""" import argparse import sys import time import traceback from pathlib import Path + import neuracore as nc +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import URDF_PATH, POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO +from common.configs import ( + CAMERA_NAMES, GRIPPER_NAME, URDF_PATH, + POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO +) +from neuracore_types import DataType + from common.data_manager import RobotActivityState -from common.policy_helpers import get_policy_embodiments, print_policy_embodiments, embodiment_names_ordered, gripper_open_at_index, joint_targets_deg_at_index, log_robot_state_for_policy +from common.policy_helpers import ( + get_policy_embodiments, print_policy_embodiments, + embodiment_names_ordered, gripper_open_at_index, + joint_targets_deg_at_index, log_robot_state_for_policy +) from common.policy_state import PolicyState +from common.config_parser import load_ik_config from common.system_bootstrap import bootstrap_robot_system from common.policy_actions import run_policy -from neuracore_types import DataType +# --------------------------------------------------------------------------- +# Helper Functions +# --------------------------------------------------------------------------- def execute_horizon(data_manager, policy_state, robot_controller, frequency, input_embodiment, output_grippers): - """Minimal terminal-only execution loop for the prediction horizon.""" + """ + Minimal terminal-only execution loop for the prediction horizon. + + Locks the predicted horizon and streams the joint/gripper targets to the + robot at the specified frequency until the horizon is exhausted. + """ policy_state.start_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) @@ -30,59 +69,107 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp for i in range(horizon_length): start_time = time.time() + # Dispatch joint targets joint_targets_deg = joint_targets_deg_at_index(locked_horizon, i) if joint_targets_deg is not None: robot_controller.set_target_joint_angles(joint_targets_deg) + # Dispatch gripper targets gripper_target = gripper_open_at_index(locked_horizon, i, gripper_names=output_grippers) if gripper_target is not None: robot_controller.set_gripper_open_value(gripper_target) + # Log state back to the policy for the next inference cycle log_robot_state_for_policy(data_manager, input_embodiment) - time.sleep(max(0, dt - (time.time() - start_time))) + + # Maintain loop frequency + time.sleep(max(0.0, dt - (time.time() - start_time))) policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - +# --------------------------------------------------------------------------- +# Main Execution +# --------------------------------------------------------------------------- if __name__ == "__main__": parser = argparse.ArgumentParser(description="Minimal Piper Policy Test") + policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument("--train-run-name", type=str, default=None) - policy_group.add_argument("--model-path", type=str, default=None) - policy_group.add_argument("--remote-endpoint-name", type=str, default=None) + policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.") + policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.") + policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.") + parser.add_argument("--robot-name", type=str, default="AgileX PiPER") parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE) parser.add_argument("--execution-ratio", type=float, default=PREDICTION_HORIZON_EXECUTION_RATIO) + parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to YAML configuration.") args = parser.parse_args() print("=" * 60 + "\nPIPER POLICY ROLLOUT (MINIMAL)\n" + "=" * 60) - # 1. Connect & Load Policy + # 1. Configuration & Neuracore Connection + config = load_ik_config(args.ik_config) nc.login() nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + # 2. Policy Loading if args.remote_endpoint_name: - policy = nc.policy_remote_server(args.remote_endpoint_name) + print(f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") + try: + policy = nc.policy_remote_server(args.remote_endpoint_name) + except Exception as e: + # Catch the EndpointError and provide explicit, actionable instructions + print("\n" + "!" * 60) + print(f"❌ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'") + print("!" * 60) + print("The script successfully reached Neuracore, but the remote server is down.") + print("\nPLEASE FOLLOW THESE STEPS:") + print(" 1. Open your browser and go to the Neuracore website/dashboard.") + print(f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'.") + print(" 3. Click 'Deploy' or 'Activate' to spin up the cloud server.") + print(" 4. Wait for the status to show as 'Active', then rerun this script.") + print("!" * 60 + "\n") + sys.exit(1) + elif args.train_run_name: + print(f"\n🤖 Loading policy from cloud training run: {args.train_run_name}...") policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name) else: + print(f"\n🤖 Loading policy from local model: {args.model_path}...") policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name) - input_emb, output_emb = get_policy_embodiments(policy) + # 3. Embodiment Extraction with Remote Fallback + try: + input_emb, output_emb = get_policy_embodiments(policy) + except AttributeError: + print("\n⚠️ Could not dynamically extract embodiments. Using default Piper configuration...") + input_emb = { + DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]}, + } + output_emb = { + DataType.JOINT_TARGET_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + } + print_policy_embodiments(input_emb, output_emb) output_gripper_names = None if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb: output_gripper_names = embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) - # 2. Bootstrap Core System (No Quest, No IK needed for minimal playback) - data_manager, robot_controller, _, active_threads = bootstrap_robot_system(start_ik=False, start_camera=True) + # 4. Bootstrap Core System (No Quest, No IK needed for minimal playback) + data_manager, robot_controller, _, active_threads = bootstrap_robot_system( + config, start_ik=False, start_camera=True + ) + policy_state = PolicyState() policy_state.set_execution_ratio(args.execution_ratio) - time.sleep(2.0) # Wait for threads to init + time.sleep(2.0) # Wait for hardware interfaces and threads to initialize + # 5. Continuous Execution Loop try: print("\n🟢 Enabling and homing robot...") robot_controller.resume_robot() @@ -95,30 +182,52 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp data_manager.set_robot_activity_state(RobotActivityState.ENABLED) print("✓ Robot ready. Starting policy execution loop... Press Ctrl+C to stop.\n") - # 3. Continuous Execution Loop while True: + # Query the network for the next prediction if not run_policy(data_manager, policy, policy_state, visualizer=None, input_embodiment_description=input_emb): - print("⚠️ Policy run failed, retrying...") + print("⚠️ Policy inference failed or timed out. Retrying...") time.sleep(0.5) continue - execute_horizon(data_manager, policy_state, robot_controller, args.frequency, input_emb, output_gripper_names) + # Execute the predicted horizon on the hardware + execute_horizon( + data_manager, policy_state, robot_controller, + args.frequency, input_emb, output_gripper_names + ) except KeyboardInterrupt: - print("\n👋 Shutting down...") + print("\n👋 Interrupt received. Halting execution loop...") except Exception as e: - print(f"\n❌ Error: {e}") + print(f"\n❌ Unhandled error during execution: {e}") traceback.print_exc() + + # 6. Graceful Teardown & Cleanup finally: - print("\n🧹 Cleaning up...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - robot_controller.move_to_home() - time.sleep(1.0) + print("\n🧹 Cleaning up subsystems...") + try: + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + robot_controller.move_to_home() + # Wait safely for homing to finish before severing the hardware connection + for _ in range(10): + if robot_controller.is_robot_homed(): + break + time.sleep(1) + except Exception: + pass policy.disconnect() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) data_manager.request_shutdown() + for t in active_threads: - t.join() - robot_controller.cleanup() - nc.logout() \ No newline at end of file + t.join(timeout=2.0) + + if robot_controller: + try: + robot_controller.graceful_stop() + except AttributeError: + pass + robot_controller.cleanup() + + nc.logout() + print("👋 Shutdown complete.") \ No newline at end of file From 08b4cba469d73b9d4a32f332a950053e7246be33 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 12:10:17 +0100 Subject: [PATCH 16/22] fix:made example 6 works, and I do not think I can fix example 7 as the functinality has not been developed and integrated properly --- examples/6_visualize_policy_from_dataset.py | 171 +- examples/7_teleop_with_pedal.py | 154 +- examples/combined_code_old.txt | 6025 ------------------- 3 files changed, 271 insertions(+), 6079 deletions(-) delete mode 100644 examples/combined_code_old.txt diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index 7d5847e..4ba32cf 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -1,12 +1,40 @@ #!/usr/bin/env python3 -"""Simple policy visualization from dataset.""" +""" +Offline Policy Visualization and Validation. + +This script acts as a safe, simulated testing ground for your trained AI policies. +Instead of running on the physical robot, it: + 1. Loads a synchronized teleoperation dataset from Neuracore. + 2. Selects a random step (observation) from the dataset. + 3. Feeds that real-world camera and joint data into the trained policy. + 4. Renders the AI's predicted future action horizon in a 3D Viser web UI. + +It is highly recommended to run this script to visually validate a model's sanity +before deploying it to the physical AgileX Piper arm (via Scripts 4 or 5). + +Usage Examples: + python 6_visualize_policy_from_dataset.py --dataset-name my_dataset --model-path ./model.nc.zip + python 6_visualize_policy_from_dataset.py --dataset-name my_dataset --train-run-name cloud_run +""" import argparse import random import sys import time import traceback +import logging from pathlib import Path + +# --------------------------------------------------------------------------- +# Suppress Noisy WebRTC/STUN Networking Errors from Viser +# --------------------------------------------------------------------------- +# Viser attempts to use STUN servers for WebRTC streaming. If a firewall/VPN +# blocks it, it throws massive 401 errors before falling back to WebSockets. +# This silences those unhandled asyncio exceptions so the terminal stays clean. +logging.getLogger("asyncio").setLevel(logging.CRITICAL) +logging.getLogger("aioice").setLevel(logging.CRITICAL) +logging.getLogger("aiortc").setLevel(logging.CRITICAL) + import neuracore as nc import numpy as np import viser @@ -14,9 +42,14 @@ from PIL import Image from viser.extras import ViserUrdf +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH +from common.configs import ( + JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH, CAMERA_NAMES, GRIPPER_NAME +) from common.dataset_helpers import load_and_sync_dataset from common.policy_helpers import ( DEFAULT_ROBOT_NAME, convert_predictions_to_horizon, embodiment_names_ordered, @@ -25,77 +58,152 @@ ) from neuracore_types import DataType + if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Visualize policy predictions") - parser.add_argument("--dataset-name", type=str, required=True) + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- + parser = argparse.ArgumentParser(description="Visualize policy predictions offline using dataset states.") + parser.add_argument("--dataset-name", type=str, required=True, help="Neuracore dataset to draw observations from.") + policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument("--train-run-name", type=str, default=None) - policy_group.add_argument("--model-path", type=str, default=None) - policy_group.add_argument("--remote-endpoint-name", type=str, default=None) + policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.") + policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.") + policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.") + parser.add_argument("--robot-name", type=str, default=DEFAULT_ROBOT_NAME) parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE) args = parser.parse_args() + print("=" * 60 + "\nOFFLINE POLICY VISUALIZATION\n" + "=" * 60) + + # --------------------------------------------------------- + # 2. Neuracore Initialization & Policy Loading + # --------------------------------------------------------- nc.login() nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) if args.remote_endpoint_name: - policy = nc.policy_remote_server(args.remote_endpoint_name) + print(f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") + try: + policy = nc.policy_remote_server(args.remote_endpoint_name) + except Exception as e: + print("\n" + "!" * 60) + print(f"❌ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'") + print("!" * 60) + print("The script successfully reached Neuracore, but the remote server is down.") + print("\nPLEASE FOLLOW THESE STEPS:") + print(" 1. Open your browser and go to the Neuracore website/dashboard.") + print(f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'.") + print(" 3. Click 'Deploy' or 'Activate' to spin up the cloud server.") + print(" 4. Wait for the status to show as 'Active', then rerun this script.") + print("!" * 60 + "\n") + sys.exit(1) + elif args.train_run_name: + print(f"\n🤖 Loading policy from cloud training run: {args.train_run_name}...") policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name) else: + print(f"\n🤖 Loading policy from local model: {args.model_path}...") policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name) - input_emb, output_emb = get_policy_embodiments(policy) + # --------------------------------------------------------- + # 3. Embodiment Extraction & Fallback + # --------------------------------------------------------- + try: + input_emb, output_emb = get_policy_embodiments(policy) + except AttributeError: + print("\n⚠️ Could not dynamically extract embodiments. Using default Piper configuration...") + input_emb = { + DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]}, + } + output_emb = { + DataType.JOINT_TARGET_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME}, + } + print_policy_embodiments(input_emb, output_emb) - out_grippers = embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb else None + out_grippers = ( + embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) + if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb + else None + ) + # --------------------------------------------------------- + # 4. Dataset Synchronization + # --------------------------------------------------------- input_mods = list(input_emb.keys()) output_mods = list(output_emb.keys()) if output_emb else [] - synced_dataset = load_and_sync_dataset(args.dataset_name, args.frequency, input_mods, output_mods, prefetch_videos=True) - - print("🖥️ Starting Viser...") + + synced_dataset = load_and_sync_dataset( + args.dataset_name, args.frequency, input_mods, output_mods, prefetch_videos=True + ) + + if len(synced_dataset) == 0: + print("❌ Error: The synchronized dataset is empty. Exiting.") + sys.exit(1) + + # --------------------------------------------------------- + # 5. Viser 3D UI Setup + # --------------------------------------------------------- + print("\n🖥️ Starting Viser simulation environment...") server = viser.ViserServer() server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) + urdf_vis = ViserUrdf(server, yourdfpy.URDF.load(str(URDF_PATH)), root_node_name="/robot") urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) state = {"horizon": None, "action_idx": 0, "playing": False, "rgb_handle": None} def select_random_state() -> None: + """Pulls a random observation from the dataset and queries the AI policy.""" episode = synced_dataset[random.randint(0, len(synced_dataset) - 1)] - if not len(episode): return + if not len(episode): + return step = episode[random.randint(0, len(episode) - 1)] - if not log_sync_step_for_policy(step, input_emb): return + if not log_sync_step_for_policy(step, input_emb): + return rgb_data = getattr(step, "data", {}).get(DataType.RGB_IMAGES, {}) for _, frame in rgb_data.items(): rgb_arr = np.array(frame.frame) Image.fromarray(rgb_arr).save("current_image.png") + if state["rgb_handle"] is None: - state["rgb_handle"] = server.gui.add_image(rgb_arr, label="RGB (current step)", format="jpeg") + state["rgb_handle"] = server.gui.add_image(rgb_arr, label="RGB Observation", format="jpeg") else: state["rgb_handle"].image = rgb_arr break try: - state["horizon"] = convert_predictions_to_horizon(policy.predict(timeout=60)) + print("🧠 Querying AI model for prediction horizon...") + predictions = policy.predict(timeout=60) + state["horizon"] = convert_predictions_to_horizon(predictions) state["action_idx"] = 0 state["playing"] = True joint_cfg = urdf_cfg_from_horizon(state["horizon"], 0) - if joint_cfg is not None: urdf_vis.update_cfg(joint_cfg) + if joint_cfg is not None: + urdf_vis.update_cfg(joint_cfg) + + print(f"✓ Prediction successful. Horizon length: {horizon_length(state['horizon'])}") except Exception as e: - print(f"✗ Failed prediction: {e}") + print(f"❌ Failed prediction inference: {e}") - server.gui.add_button("Random Selection").on_click(lambda _: select_random_state()) - gripper_handle = server.gui.add_slider("Gripper Open Amount", min=0.0, max=1.0, step=0.01, initial_value=0.0, disabled=True) - freq_handle = server.gui.add_number("Visualization Frequency (Hz)", initial_value=args.frequency, min=1.0, max=500.0, step=1.0) + server.gui.add_button("🎲 Pull Random Observation").on_click(lambda _: select_random_state()) + gripper_handle = server.gui.add_slider("Predicted Gripper State", min=0.0, max=1.0, step=0.01, initial_value=0.0, disabled=True) + freq_handle = server.gui.add_number("Simulation Speed (Hz)", initial_value=args.frequency, min=1.0, max=500.0, step=1.0) select_random_state() + print("\n🚀 System Online! Open http://localhost:8080 to view the simulation. Press Ctrl+C to exit.\n") + # --------------------------------------------------------- + # 6. Main Visualization Loop + # --------------------------------------------------------- try: while True: start_time = time.time() @@ -109,13 +217,24 @@ def select_random_state() -> None: nc.log_joint_positions({jn: float(j_cfg[i]) for i, jn in enumerate(JOINT_NAMES)}) g_val = gripper_open_at_index(state["horizon"], state["action_idx"], out_grippers) - if g_val is not None: gripper_handle.value = round(g_val, 2) + if g_val is not None: + gripper_handle.value = round(g_val, 2) + state["action_idx"] = (state["action_idx"] + 1) % h_len - time.sleep(max(0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time))) + time.sleep(max(0.0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time))) except KeyboardInterrupt: - print("\n👋 Shutting down...") + print("\n👋 Interrupt received. Shutting down gracefully...") + except Exception as e: + print(f"\n❌ Unhandled error in rendering loop: {e}") + traceback.print_exc() + + # --------------------------------------------------------- + # 7. Cleanup + # --------------------------------------------------------- finally: + print("\n🧹 Severing backend connections...") policy.disconnect() - nc.logout() \ No newline at end of file + nc.logout() + print("👋 Offline validation complete.") \ No newline at end of file diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index ead1a2e..a377301 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -1,17 +1,45 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest and Foot Pedal control.""" +""" +Piper Robot Teleoperation with Meta Quest and Foot Pedals. + +This script provides a unified control interface for the AgileX Piper robot, +combining the 6D spatial tracking of a Meta Quest controller with the +hands-free convenience of a USB foot pedal. It simultaneously streams +synchronized telemetry data to the Neuracore cloud. + +Key Features: + - Meta Quest for precise 6-DoF end-effector manipulation. + - Foot Pedals mapped to system states (Enable/Disable, Home, Record). + - Dynamically loads IK and scaling configurations from a YAML file. + - Robust ADB connection handling for the VR headset. + +Hardware Requirements: + - AgileX Piper robot arm connected via CAN interface ('can0'). + - Meta Quest headset connected via USB with 'USB Debugging' explicitly allowed. + - 3-Button USB Foot Pedal (mapped to keystrokes 'a', 'b', 'c'). + +Usage: + python 7_teleop_with_pedal.py --ip-address --ik-config ik_conf/default.yaml +""" import argparse import multiprocessing import sys import threading import time +import traceback from pathlib import Path + import neuracore as nc +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import URDF_PATH +from common.configs import URDF_PATH, META_QUEST_AXIS_MASK +from common.config_parser import load_ik_config +from common.data_manager import RobotActivityState from common.system_bootstrap import bootstrap_robot_system from common.shared_actions import ( toggle_robot_enabled, move_robot_home, @@ -21,46 +49,86 @@ from common.threads.quest_reader import quest_reader_thread from meta_quest_teleop.reader import MetaQuestReader +# Hardware key mappings for the USB foot pedal ENABLE_DISABLE_PEDAL = "a" HOME_POSITION_PEDAL = "b" RECORD_TOGGLE_PEDAL = "c" if __name__ == "__main__": + # Ensure safe multiprocess spawning for UI/Background threads multiprocessing.set_start_method("spawn") + + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleop") - parser.add_argument("--ip-address", type=str, help="Meta Quest IP") - parser.add_argument("--dataset-name", type=str, help="Neuracore dataset name") + parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP address on the local network.") + parser.add_argument("--dataset-name", type=str, default=None, help="Override the auto-generated dataset name.") + parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.") args = parser.parse_args() print("=" * 60 + "\nPIPER TELEOP: META QUEST + FOOT PEDALS\n" + "=" * 60) - # 1. Init Neuracore + # Load the YAML configuration dictionary + config = load_ik_config(args.ik_config) + + # --------------------------------------------------------- + # 2. Neuracore Initialization & Dataset Creation + # --------------------------------------------------------- + print("\n🔧 Initializing Neuracore...") try: nc.login() nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) - ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%H-%M-%S')}" - nc.create_dataset(name=ds_name, description="Quest + Pedal unified collection") + + ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%Y-%m-%d-%H-%M-%S')}" + nc.create_dataset(name=ds_name, description="Quest + Pedal unified teleop collection") except Exception as e: - print(f"⚠️ Neuracore init skipped/failed: {e}") + print(f"⚠️ Neuracore initialization skipped/failed: {e}") - # 2. Bootstrap Core System + # --------------------------------------------------------- + # 3. Hardware & Subsystem Bootstrapping + # --------------------------------------------------------- data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( - start_ik=True, start_camera=True + config, start_ik=True, start_camera=True ) + + # Wire Neuracore logging to DataManager so states push to the cloud data_manager.set_on_change_callback(neuracore_logging_callback) - # 3. Quest Reader + # --------------------------------------------------------- + # 4. Meta Quest Initialization & Error Handling + # --------------------------------------------------------- + print("\n🎮 Initializing Meta Quest reader...") quest_reader = None try: - print("🔍 Searching for Meta Quest...") - quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) + quest_reader = MetaQuestReader( + ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ) quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) quest_thread.start() active_threads.append(quest_thread) - except (Exception, BaseException) as e: - print(f"⚠️ Quest reader init skipped/failed: {e}") + except (Exception, SystemExit) as e: + print("\n" + "!" * 60) + print("❌ FAILED TO ACCESS META QUEST") + print("!" * 60) + print("The headset is plugged in, but ADB debugging permissions are missing.") + print("\nPLEASE FOLLOW THESE STEPS:") + print(" 1. Put the Meta Quest headset on your head.") + print(" 2. Look for a notification in your menu that says 'USB Detected'.") + print(" 3. Click on that notification and select 'Allow' to grant data access.") + print(" 4. Rerun this script.") + print("!" * 60 + "\n") + + data_manager.request_shutdown() + for t in active_threads: + t.join(timeout=1.0) + if robot_controller: + robot_controller.cleanup() + sys.exit(1) - # 4. Foot Pedal Init & Binding + # --------------------------------------------------------- + # 5. Foot Pedal Initialization & Binding + # --------------------------------------------------------- print("\n⌨️ Initializing Foot Pedals...") pedal = FootPedal({ "button_a": ENABLE_DISABLE_PEDAL, @@ -68,7 +136,7 @@ "button_c": RECORD_TOGGLE_PEDAL, }) - # 🔗 Bind the Shared Actions + # Bind the hardware actions to the pedal inputs pedal.bind("button_a", lambda: toggle_robot_enabled(data_manager, robot_controller)) pedal.bind("button_b", lambda: move_robot_home(data_manager, robot_controller)) pedal.bind("button_c", lambda: toggle_recording(play_audio=False)) @@ -78,20 +146,50 @@ active_threads.append(pedal_thread) print("\n✅ SYSTEM ONLINE") - print("🎮 QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") - print("⌨️ PEDAL CONTROLS: ENABLE/DISABLE (A), HOME (B), RECORD (C)") + print("🎮 QUEST CONTROLS: Hold GRIP to move arm, TRIGGER to close gripper") + print("⌨️ PEDAL CONTROLS: ENABLE/DISABLE (Left), HOME (Middle), RECORD (Right)\n") - # 5. Wait Loop + # --------------------------------------------------------- + # 6. Main Daemon Loop + # --------------------------------------------------------- try: - while True: + # Keep the main thread alive while background threads handle execution + while not data_manager.is_shutdown_requested(): time.sleep(1) + except KeyboardInterrupt: - print("\n👋 Shutting down...") + print("\n👋 Interrupt received - shutting down gracefully...") + data_manager.request_shutdown() + except Exception as e: + print(f"\n❌ Unhandled Demo error: {e}") + traceback.print_exc() + data_manager.request_shutdown() + + # --------------------------------------------------------- + # 7. Graceful Teardown & Cleanup + # --------------------------------------------------------- finally: - if nc.is_recording(): nc.cancel_recording() - try: nc.logout() - except: pass + print("\n🧹 Cleaning up subsystems...") + + # Ensure we don't leave dangling, unterminated recordings on the cloud + if nc.is_recording(): + nc.cancel_recording() + + try: + nc.logout() + except Exception: + pass + + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) data_manager.request_shutdown() - if quest_reader: quest_reader.stop() - for t in active_threads: t.join(timeout=1.0) - if robot_controller: robot_controller.cleanup() \ No newline at end of file + + if quest_reader: + quest_reader.stop() + + for t in active_threads: + t.join(timeout=2.0) + + if robot_controller: + robot_controller.cleanup() + + print("👋 Demo stopped.") \ No newline at end of file diff --git a/examples/combined_code_old.txt b/examples/combined_code_old.txt deleted file mode 100644 index d002516..0000000 --- a/examples/combined_code_old.txt +++ /dev/null @@ -1,6025 +0,0 @@ - -============================================================ -FILE: 1_tune_teleop_params.py -============================================================ - -#!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller - REAL ROBOT CONTROL. - -This demo uses Pink IK control with Meta Quest controller input to control the REAL robot. -- REAL ROBOT CONTROL - sends commands to physical robot! -- Uses right hand controller grip as dead man's button -- Uses ROS pointer frame for natural pointing control -- Applies relative transformations accounting for different coordinate frames -""" - -import argparse -import sys -import threading -import time -import traceback -from pathlib import Path - -import numpy as np - -# Add parent directory to path to import pink_ik_solver and piper_controller -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, - CAMERA_NAMES, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - IK_SOLVER_RATE, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - META_QUEST_AXIS_MASK, - NEUTRAL_END_EFFECTOR_POSE, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POSITION_COST, - POSTURE_COST_VECTOR, - ROBOT_RATE, - ROTATION_SCALE, - SLOW_ROTATION_SCALE, - SLOW_TRANSLATION_SCALE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, - TRANSLATION_SCALE, - URDF_PATH, - VISUALIZATION_RATE, - WRIST_JOINT_BUTTON_STEP_DEGREES, -) -from common.data_manager import DataManager, RobotActivityState -from common.robot_visualizer import RobotVisualizer -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread -from common.threads.quest_reader import quest_reader_thread -from common.threads.realsense_camera import camera_thread -from meta_quest_teleop.reader import MetaQuestReader - -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - - -def on_button_a_pressed() -> None: - """Handle Button A press to toggle robot enable/disable state.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - print("✓ 🔴 Robot disabled (Button A)") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Button A)") - else: - print("✗ Failed to enable robot") - - -def on_button_b_pressed() -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 Button B pressed - Moving to home position...") - # Set state to HOMING to prevent IK thread from sending robot commands - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - # Disable teleop during homing - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Button B pressed but robot is not enabled") - - -def _step_wrist_joint(delta_degrees: float) -> None: - """Apply a relative step to the wrist joint target angle.""" - # Prevent IK teleop loop from overwriting this manual joint adjustment. - data_manager.set_teleop_state(False, None, None) - robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) - - target_joint_angles = robot_controller.get_target_joint_angles() - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - base_wrist_joint_angle = float(current_joint_angles[-1]) - else: - base_wrist_joint_angle = float(target_joint_angles[-1]) - - target_joint_angles[-1] = base_wrist_joint_angle + delta_degrees - robot_controller.set_target_joint_angles(target_joint_angles) - data_manager.set_target_joint_angles(target_joint_angles) - - -def on_button_y_pressed() -> None: - """Handle Button Y press to add +5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button Y pressed but robot is not enabled") - return - _step_wrist_joint(WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_x_pressed() -> None: - """Handle Button X press to subtract -5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button X pressed but robot is not enabled") - return - _step_wrist_joint(-WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_lj_pressed() -> None: - """Toggle slow translation/rotation scaling mode from config values.""" - slow_scaling_mode_enabled = data_manager.toggle_slow_scaling_mode_enabled() - if slow_scaling_mode_enabled: - print( - "🐢 Button LJ pressed - Slow scaling enabled " - f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " - f"rotation={SLOW_ROTATION_SCALE:.3f})" - ) - else: - print( - "🐇 Button LJ pressed - Slow scaling disabled " - f"(using GUI/default scales, currently translation={TRANSLATION_SCALE:.3f}, " - f"rotation={ROTATION_SCALE:.3f})" - ) - - -parser = argparse.ArgumentParser( - description="Piper Robot Teleoperation - REAL ROBOT CONTROL" -) -parser.add_argument( - "--ip-address", - type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", -) -args = parser.parse_args() - -print("=" * 60) -print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL") -print("=" * 60) -print("Thread frequencies:") -print(f" 🎮 Quest Reader: {CONTROLLER_DATA_RATE} Hz") -print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") -print(f" 🖥️ Visualization: {VISUALIZATION_RATE} Hz (running in the main thread)") -print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") -print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") -print(f" 📷 Camera: {CAMERA_FRAME_STREAMING_RATE} Hz") - - -# Initialize shared state -data_manager = DataManager() -data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, -) - -# Initialize robot controller -print("\n🤖 Initializing Piper robot controller...") -robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, - enable_joint_angle_limits=False, - debug_mode=False, -) - -# Start robot control loop -print("\n🚀 Starting robot control loop...") -robot_controller.start_control_loop() - -# Start joint state thread -print("\n📊 Starting joint state thread...") -joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True -) -joint_state_thread_obj.start() - -# Initialize Meta Quest reader -print("\n🎮 Initializing Meta Quest reader...") -quest_reader = MetaQuestReader( - ip_address=args.ip_address, - port=5555, - run=True, - axis_mask=META_QUEST_AXIS_MASK, -) - -# Register button callbacks (after state and robot_controller are initialized) -quest_reader.on("button_a_pressed", on_button_a_pressed) -quest_reader.on("button_b_pressed", on_button_b_pressed) -quest_reader.on("button_y_pressed", on_button_y_pressed) -quest_reader.on("button_x_pressed", on_button_x_pressed) -quest_reader.on("button_lj_pressed", on_button_lj_pressed) - -# Start quest reader thread -print("\n🎮 Starting quest reader thread...") -quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True -) -quest_thread.start() - -# set initial configuration to current joint angles -current_joint_angles = data_manager.get_current_joint_angles() -if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) -else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - -# Create Pink IK solver -print("\n🔧 Creating Pink IK solver...") -ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), -) - -# Start IK solver thread -print("\n🧮 Starting IK solver thread...") -ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True -) -ik_thread.start() - -# Start camera thread (if RealSense is available) -print("\n📷 Starting camera thread...") -camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True -) -camera_thread_obj.start() - - -# Set up visualizer -print("\n🖥️ Starting visualization...") -visualizer = RobotVisualizer(urdf_path=URDF_PATH) -visualizer.add_basic_controls() -visualizer.add_teleop_controls() -visualizer.add_gripper_status_controls() -visualizer.add_homing_controls() -visualizer.add_toggle_robot_enabled_status_button() -visualizer.add_rgb_image_placeholder() -visualizer.add_target_frame_visualization() -visualizer.add_controller_filter_controls( - initial_min_cutoff=CONTROLLER_MIN_CUTOFF, - initial_beta=CONTROLLER_BETA, - initial_d_cutoff=CONTROLLER_D_CUTOFF, -) -visualizer.add_scaling_controls( - initial_translation_scale=TRANSLATION_SCALE, - initial_rotation_scale=ROTATION_SCALE, -) -visualizer.add_pink_parameter_controls( - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - posture_cost_vector=POSTURE_COST_VECTOR, -) -visualizer.add_controller_visualization() - - -# Set up button callbacks -def toggle_robot_enabled_status() -> None: - """Toggle robot enabled/disabled state and update GUI button label.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - # Disable robot - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - visualizer.update_toggle_robot_enabled_status(False) - print("✓ 🔴 Robot disabled") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - visualizer.update_toggle_robot_enabled_status(True) - print("✓ 🟢 Robot enabled") - else: - print("✗ Failed to enable robot") - - -def on_go_home() -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 GUI: Moving to home position...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Cannot home: robot not enabled") - - -visualizer.set_toggle_robot_enabled_status_callback(toggle_robot_enabled_status) -visualizer.set_go_home_callback(on_go_home) - -print() -print("🚀 Starting teleoperation with REAL ROBOT CONTROL...") -print("🎮 CONTROLS:") -print(" 1. Press BUTTON A to enable/disable robot (or use GUI)") -print(" 2. Hold RIGHT GRIP to activate teleoperation") -print(" 3. Move controller - robot follows!") -print(" 4. Hold RIGHT TRIGGER to close gripper") -print(" 5. Press BUTTON B to send robot home (or use GUI)") -print(" 6. Press BUTTON Y to add +5° on the wrist joint") -print(" 7. Press BUTTON X to subtract 5° on the wrist joint") -print(" 8. Press LEFT JOYSTICK BUTTON to toggle slow scaling mode") -print(" 9. Release grip to stop") -print(" 10. Use 'Emergency Stop' in GUI if needed") -print("⚠️ Press Ctrl+C to exit") -print() - -# Visualization loop variables -dt: float = 1.0 / VISUALIZATION_RATE - -try: - while True: - iteration_start: float = time.time() - - # Update filter parameters - min_cutoff, beta, d_cutoff = visualizer.get_controller_filter_params() - data_manager.set_controller_filter_params(min_cutoff, beta, d_cutoff) - - # Update scaling factors (shared with IK thread via DataManager) - translation_scale = visualizer.get_translation_scale() - rotation_scale = visualizer.get_rotation_scale() - if data_manager.get_slow_scaling_mode_enabled(): - translation_scale = SLOW_TRANSLATION_SCALE - rotation_scale = SLOW_ROTATION_SCALE - data_manager.set_teleop_scaling(translation_scale, rotation_scale) - - # Update Pink parameters (GUI controls) - pink_params = visualizer.get_pink_parameters() - ik_solver.update_task_parameters(**pink_params) - - # Get data from shared state - controller_transform, grip_value, trigger_value = ( - data_manager.get_controller_data() - ) - teleop_active = data_manager.get_teleop_active() - # Get updated robot_activity_state and joint states (may have changed if homing completed) - robot_activity_state = data_manager.get_robot_activity_state() - current_joint_angles = data_manager.get_current_joint_angles() - target_joint_angles = data_manager.get_target_joint_angles() - solve_time_ms = data_manager.get_ik_solve_time_ms() - ik_success = data_manager.get_ik_success() - target_pose = data_manager.get_target_pose() - rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) - - # Update GUI displays - visualizer.set_grip_value(grip_value) - visualizer.set_trigger_value(trigger_value) - - # Update timing display - visualizer.update_timing(solve_time_ms) - - # Update controller visualization - visualizer.update_controller_visualization(controller_transform) - if controller_transform is not None: - visualizer.update_controller_status_display( - controller_transform[:3, 3], connected=True - ) - else: - visualizer.update_controller_status_display(None, connected=False) - - # Update teleop status display - visualizer.update_teleop_status(teleop_active) - - # Update RGB camera image in Viser GUI (if available) - if rgb_image is not None: - visualizer.update_rgb_image(rgb_image) - - # Update target/goal visualization - visualizer.update_target_visualization(target_pose) - - # Update main robot visualization from CURRENT joint angles (DataManager uses degrees, Viser expects radians) - if current_joint_angles is not None: - current_joint_rad = np.radians(current_joint_angles) - visualizer.update_robot_pose(current_joint_rad) - visualizer.update_joint_angles_display(current_joint_rad) - - # Update ghost robot to show TARGET joint angles when available (also in radians) - if ( - target_joint_angles is not None - and robot_activity_state == RobotActivityState.ENABLED - ): - target_joint_rad = np.radians(target_joint_angles) - visualizer.update_ghost_robot_visibility(True) - visualizer.update_ghost_robot_pose(target_joint_rad) - else: - # Hide ghost robot when no valid target is available or robot not enabled - visualizer.update_ghost_robot_visibility(False) - - # Update robot status - simple Enabled/Homing/Disabled - if robot_activity_state == RobotActivityState.ENABLED: - visualizer.update_robot_status("Robot Status: Enabled") - elif robot_activity_state == RobotActivityState.HOMING: - visualizer.update_robot_status("Robot Status: Homing") - else: # DISABLED - visualizer.update_robot_status("Robot Status: Disabled") - - # Update gripper status - visualizer.update_gripper_status( - trigger_value, - robot_enabled=(robot_activity_state == RobotActivityState.ENABLED), - ) - - # Sleep to maintain visualization rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - -except KeyboardInterrupt: - print("\n\n👋 Interrupt received - shutting down gracefully...") -except Exception as e: - print(f"\n❌ Demo error: {e}") - import traceback - - traceback.print_exc() - -# Cleanup (outside try/except so it always runs) -print("\n🧹 Cleaning up...") - -data_manager.request_shutdown() -data_manager.set_robot_activity_state(RobotActivityState.DISABLED) -quest_thread.join() -quest_reader.stop() -ik_thread.join() -joint_state_thread_obj.join() -robot_controller.cleanup() -visualizer.stop() - -print("\n👋 Demo stopped.") - - -============================================================ -FILE: 6_visualize_policy_from_dataset.py -============================================================ - -#!/usr/bin/env python3 -"""Simple policy visualization from dataset. - -Loads a policy and a dataset, and randomly selects a state -from the dataset to run the policy with and visualize the results. -""" - -import argparse -import random -import sys -import time -import traceback -from pathlib import Path - -import neuracore as nc -import numpy as np -import viser -import yourdfpy -from neuracore.core.utils.robot_data_spec_utils import ( - merge_cross_embodiment_description, -) -from neuracore_types import DataType -from PIL import Image -from viser.extras import ViserUrdf - -# Add parent directory to path -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH -from common.policy_helpers import ( - DEFAULT_ROBOT_NAME, - convert_predictions_to_horizon, - embodiment_names_ordered, - get_policy_embodiments, - gripper_open_at_index, - horizon_length, - log_sync_step_for_policy, - print_policy_embodiments, - urdf_cfg_from_horizon, -) - -# Parse arguments -parser = argparse.ArgumentParser( - description="Visualize policy predictions from dataset" -) -parser.add_argument("--dataset-name", type=str, required=True, help="Dataset name") -policy_group = parser.add_mutually_exclusive_group(required=True) -policy_group.add_argument( - "--train-run-name", type=str, default=None, help="Training run name" -) -policy_group.add_argument( - "--model-path", type=str, default=None, help="Model file path" -) -policy_group.add_argument( - "--remote-endpoint-name", - type=str, - default=None, - help="Name of remote Neuracore policy endpoint to use instead of a local policy.", -) -parser.add_argument( - "--robot-name", - type=str, - default=DEFAULT_ROBOT_NAME, - help="Neuracore robot name (policy embodiment resolution).", -) -parser.add_argument( - "--frequency", - type=int, - default=POLICY_EXECUTION_RATE, - help="Frequency of visualization", -) -args = parser.parse_args() - -# Connect to Neuracore -print("🔧 Initializing Neuracore...") -nc.login() -nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) - -if args.remote_endpoint_name: - print(f"🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") - try: - policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: - print( - f"❌ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." - ) - sys.exit(1) -elif args.train_run_name: - print(f"🤖 Loading policy from training run: {args.train_run_name}...") - policy = nc.policy( - train_run_name=args.train_run_name, - device="cuda", - robot_name=args.robot_name, - ) -else: - print(f"🤖 Loading policy from model file: {args.model_path}...") - policy = nc.policy( - model_file=args.model_path, - device="cuda", - robot_name=args.robot_name, - ) -print(" ✓ Policy loaded") -input_embodiment_description, output_embodiment_description = get_policy_embodiments( - policy -) -print_policy_embodiments(input_embodiment_description, output_embodiment_description) - -output_gripper_names = None -if output_embodiment_description is not None: - gripper_spec = output_embodiment_description.get( - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ) - if gripper_spec is not None: - output_gripper_names = embodiment_names_ordered(gripper_spec) - -# Load and synchronize dataset -print(f"🔍 Loading dataset: {args.dataset_name}...") -dataset = nc.get_dataset(args.dataset_name) -print(f" ✓ Dataset loaded: {len(dataset)} episodes") - -print("🔁 Building cross_embodiment_union for synchronization...") -input_cross_embodiment_description = { - robot_id: input_embodiment_description for robot_id in dataset.robot_ids -} -output_cross_embodiment_description = ( - {robot_id: output_embodiment_description for robot_id in dataset.robot_ids} - if output_embodiment_description is not None - else {} -) -cross_embodiment_union = merge_cross_embodiment_description( - input_cross_embodiment_description, - output_cross_embodiment_description, -) - -print("🔁 Synchronizing dataset...") -synced_dataset = dataset.synchronize( - frequency=args.frequency, - cross_embodiment_union=cross_embodiment_union, - prefetch_videos=True, - max_prefetch_workers=2, -) -print(f" ✓ Dataset synchronized: {len(synced_dataset)} episodes") - -# Setup Viser -print("🖥️ Starting Viser...") -server = viser.ViserServer() -server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) - -# Load URDF -urdf = yourdfpy.URDF.load(str(URDF_PATH)) -urdf_vis = ViserUrdf(server, urdf, root_node_name="/robot") -urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) - -# State variables -current_horizon = None -current_action_idx = 0 -playing = False -rgb_gui_handle = None - - -def select_random_state() -> None: - """Select random state and run policy.""" - global current_horizon, current_action_idx, playing, rgb_gui_handle - - episode_idx = random.randint(0, len(synced_dataset) - 1) - episode = synced_dataset[episode_idx] - if len(episode) == 0: - print(f"⚠️ Episode {episode_idx} is empty") - return - - step_idx = random.randint(0, len(episode) - 1) - step = episode[step_idx] - print(f"📊 Selected episode {episode_idx}, step {step_idx}") - - if not log_sync_step_for_policy(step, input_embodiment_description): - print("⚠️ Step has no data for policy input channels") - return - - if hasattr(step, "data"): - rgb_data = step.data.get(DataType.RGB_IMAGES, {}) - for _camera_name, frame in rgb_data.items(): - rgb_image = np.array(frame.frame) - image_pil = Image.fromarray(rgb_image) - image_pil.save("current_image.png") - print("💾 Saved image to current_image.png") - if rgb_gui_handle is None: - rgb_gui_handle = server.gui.add_image( - rgb_image, - label="RGB (current step)", - format="jpeg", - jpeg_quality=85, - ) - else: - rgb_gui_handle.image = rgb_image - break - - print("🎯 Getting policy prediction...") - start_time = time.time() - try: - predictions = policy.predict(timeout=60) - except nc.EndpointError as e: - print(f"✗ Failed to get policy prediction: {e}") - traceback.print_exc() - return - duration = time.time() - start_time - current_horizon = convert_predictions_to_horizon(predictions) - current_action_idx = 0 - playing = True - print(f"FINISHED PREDICTION in {duration:.3f} s") - - joint_cfg = urdf_cfg_from_horizon(current_horizon or {}, 0) - if joint_cfg is not None: - urdf_vis.update_cfg(joint_cfg) - - print(f"✅ Prediction received: {horizon_length(current_horizon or {})} actions") - - -# Add button -random_button = server.gui.add_button("Random Selection") -random_button.on_click(lambda _: select_random_state()) - -# Add gripper value display -gripper_handle = server.gui.add_slider( - "Gripper Open Amount", - min=0.0, - max=1.0, - step=0.01, - initial_value=0.0, - disabled=True, -) - -# Add frequency control -frequency_handle = server.gui.add_number( - "Visualization Frequency (Hz)", - initial_value=args.frequency, - min=1.0, - max=500.0, - step=1.0, -) - -select_random_state() -try: - while True: - start_time = time.time() - - h_len = horizon_length(current_horizon or {}) - if playing and current_horizon and h_len > 0: - if current_action_idx < h_len: - joint_cfg = urdf_cfg_from_horizon(current_horizon, current_action_idx) - if joint_cfg is not None: - urdf_vis.update_cfg(joint_cfg) - nc.log_joint_positions( - {jn: float(joint_cfg[i]) for i, jn in enumerate(JOINT_NAMES)} - ) - - gripper_value = gripper_open_at_index( - current_horizon, - current_action_idx, - gripper_names=output_gripper_names, - ) - if gripper_value is not None: - gripper_handle.value = round(gripper_value, 2) - - current_action_idx = (current_action_idx + 1) % h_len - - elapsed = time.time() - start_time - frequency = max(frequency_handle.value, 0.1) - time.sleep(max(0, 1.0 / frequency - elapsed)) - -except KeyboardInterrupt: - print("\n👋 Shutting down...") -finally: - policy.disconnect() - nc.logout() - - -============================================================ -FILE: 5_rollout_neuracore_policy_minimal.py -============================================================ - -#!/usr/bin/env python3 -"""Minimal Piper Robot Policy Test - Terminal only, no GUI. - -Simple script that: -1. Enables robot -2. Sends robot home -3. Runs policy in continuous loop (get image, run policy, execute horizon, repeat) -4. On cancellation: sends robot home and exits -""" - -import argparse -import sys -import threading -import time -import traceback -from pathlib import Path - -import neuracore as nc - -# Add parent directory to path -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import ( - NEUTRAL_JOINT_ANGLES, - POLICY_EXECUTION_RATE, - PREDICTION_HORIZON_EXECUTION_RATIO, - ROBOT_RATE, - URDF_PATH, -) -from common.data_manager import DataManager, RobotActivityState -from common.policy_helpers import ( - convert_predictions_to_horizon, - embodiment_names_ordered, - get_policy_embodiments, - gripper_open_at_index, - joint_targets_deg_at_index, - log_robot_state_for_policy, - print_policy_embodiments, -) -from common.policy_state import PolicyState -from common.threads.joint_state import joint_state_thread -from common.threads.realsense_camera import camera_thread -from neuracore_types import DataType, EmbodimentDescription - -from piper_controller import PiperController - - -def run_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - input_embodiment_description: EmbodimentDescription, -) -> bool: - """Run policy and get prediction horizon.""" - if not log_robot_state_for_policy(data_manager, input_embodiment_description): - print("⚠️ No policy input data available") - return False - - # Get policy prediction - try: - start_time = time.time() - predictions = policy.predict(timeout=5) - prediction_horizon = convert_predictions_to_horizon(predictions) - elapsed = time.time() - start_time - - # Get horizon length from the first joint (all should have same length) - horizon_length = policy_state.get_prediction_horizon_length() - print(f"✓ Got {horizon_length} actions in {elapsed:.3f}s") - - policy_state.set_prediction_horizon(prediction_horizon) - return True - - except Exception as e: - print(f"✗ Policy prediction failed: {e}") - traceback.print_exc() - return False - - -def execute_horizon( - data_manager: DataManager, - policy_state: PolicyState, - robot_controller: PiperController, - frequency: int, - input_embodiment_description: EmbodimentDescription, - output_gripper_names: list[str] | None, -) -> None: - """Execute prediction horizon.""" - policy_state.start_policy_execution() - data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) - - locked_horizon = policy_state.get_locked_prediction_horizon() - horizon_length = policy_state.get_locked_prediction_horizon_length() - dt = 1.0 / frequency - - for i in range(horizon_length): - start_time = time.time() - - joint_targets_deg = joint_targets_deg_at_index(locked_horizon, i) - if joint_targets_deg is not None: - robot_controller.set_target_joint_angles(joint_targets_deg) - - gripper_target = gripper_open_at_index( - locked_horizon, i, gripper_names=output_gripper_names - ) - if gripper_target is not None: - robot_controller.set_gripper_open_value(gripper_target) - - log_robot_state_for_policy(data_manager, input_embodiment_description) - - # Sleep to maintain rate - elapsed = time.time() - start_time - time.sleep(max(0, dt - elapsed)) - - # End execution - policy_state.end_policy_execution() - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Minimal Piper Policy Test") - policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument( - "--train-run-name", - type=str, - default=None, - help="Name of the training run to load policy from (for cloud training).", - ) - policy_group.add_argument( - "--model-path", - type=str, - default=None, - help="Path to local model file to load policy from.", - ) - policy_group.add_argument( - "--remote-endpoint-name", - type=str, - default=None, - help="Name of remote Neuracore policy endpoint.", - ) - parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Neuracore robot name (policy embodiment resolution).", - ) - parser.add_argument( - "--frequency", - type=int, - default=POLICY_EXECUTION_RATE, - help="Frequency of policy execution", - ) - parser.add_argument( - "--execution-ratio", - type=float, - default=PREDICTION_HORIZON_EXECUTION_RATIO, - help="Execution ratio of the policy", - ) - args = parser.parse_args() - - print("=" * 60) - print("PIPER POLICY ROLLOUT") - print("=" * 60) - - # Initialize Neuracore - print("\n🔧 Initializing Neuracore...") - nc.login() - nc.connect_robot( - robot_name=args.robot_name, - urdf_path=str(URDF_PATH), - overwrite=False, - ) - - if args.remote_endpoint_name is not None: - print( - f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." - ) - try: - policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: - print( - f"❌ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." - ) - sys.exit(1) - elif args.train_run_name is not None: - print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") - policy = nc.policy( - train_run_name=args.train_run_name, - device="cuda", - robot_name=args.robot_name, - ) - else: - print(f"\n🤖 Loading policy from model file: {args.model_path}...") - policy = nc.policy( - model_file=args.model_path, - device="cuda", - robot_name=args.robot_name, - ) - print(" ✓ Policy loaded successfully") - input_embodiment_description, output_embodiment_description = ( - get_policy_embodiments(policy) - ) - print_policy_embodiments( - input_embodiment_description, output_embodiment_description - ) - output_gripper_names = None - if output_embodiment_description is not None: - gripper_spec = output_embodiment_description.get( - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ) - if gripper_spec is not None: - output_gripper_names = embodiment_names_ordered(gripper_spec) - - # Initialize state - data_manager = DataManager() - policy_state = PolicyState() - policy_state.set_execution_ratio(args.execution_ratio) - - # Initialize robot controller - print("\n🤖 Initializing robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - debug_mode=False, - ) - robot_controller.start_control_loop() - - # Start joint state thread - print("\n📊 Starting joint state thread...") - joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) - joint_state_thread_obj.start() - - # Start camera thread - print("\n📷 Starting camera thread...") - camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ) - camera_thread_obj.start() - - # Wait for threads to initialize - print("\n⏳ Waiting for initialization...") - time.sleep(2.0) - - try: - # Enable robot - print("\n🟢 Enabling robot...") - robot_controller.resume_robot() - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ Robot enabled") - - # Home robot - print("\n🏠 Moving to home position...") - robot_controller.move_to_home() - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - - # Wait for homing to complete - start_time = time.time() - while ( - data_manager.get_robot_activity_state() == RobotActivityState.HOMING - and not robot_controller.is_robot_homed() - and time.time() - start_time < 5.0 - ): - time.sleep(0.1) - print("✓ Robot homed") - - # Policy execution loop - print("\n🚀 Starting policy execution loop...") - print("Press Ctrl+C to stop\n") - - while True: - # Run policy - if not run_policy( - data_manager, policy, policy_state, input_embodiment_description - ): - print("⚠️ Policy run failed, retrying...") - time.sleep(0.5) - continue - - # Execute horizon - execute_horizon( - data_manager, - policy_state, - robot_controller, - args.frequency, - input_embodiment_description, - output_gripper_names, - ) - - except KeyboardInterrupt: - print("\n\n👋 Interrupt received - shutting down...") - - except Exception as e: - print(f"\n❌ Error: {e}") - traceback.print_exc() - - finally: - # Cleanup - print("\n🧹 Cleaning up...") - - # Home robot - print("\n🏠 Moving to home position...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - robot_controller.move_to_home() - - # Wait for homing to complete - start_time = time.time() - while ( - data_manager.get_robot_activity_state() == RobotActivityState.HOMING - and not robot_controller.is_robot_homed() - and time.time() - start_time < 5.0 - ): - time.sleep(0.1) - print("✓ Robot homed") - - # Shutdown - policy.disconnect() - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - data_manager.request_shutdown() - joint_state_thread_obj.join() - camera_thread_obj.join() - time.sleep(0.5) # Give threads time to stop - - robot_controller.cleanup() - nc.logout() - - print("✓ Cleanup complete") - print("\n👋 Done.") - - -============================================================ -FILE: 3_replay_neuracore_episodes.py -============================================================ - -"""Replay a recorded Neuracore dataset on the Piper robot.""" - -import argparse -import sys -import time -from pathlib import Path -from typing import cast - -import cv2 -import neuracore as nc -import numpy as np -from common.configs import ( - GRIPPER_NAME, - JOINT_NAMES, - NEUTRAL_JOINT_ANGLES, - ROBOT_RATE, -) -from neuracore.core.utils.robot_data_spec_utils import ( - merge_cross_embodiment_description, -) -from neuracore_types import ( - CrossEmbodimentDescription, - DataType, - SynchronizedPoint, -) -from tqdm import tqdm - -# Add parent directory to path to piper_controller -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from piper_controller import PiperController - - -def main() -> None: - """Main function for replaying a Neuracore dataset on the Piper robot.""" - parser = argparse.ArgumentParser() - parser.add_argument("--dataset-name", type=str, required=True) - parser.add_argument("--frequency", type=int, required=True) - parser.add_argument("--episode-index", type=int, required=False, default=0) - args = parser.parse_args() - - # Initialize robot controller - print("\n🤖 Initializing Piper robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - debug_mode=False, - ) - # Start robot control loop - print("\n🚀 Starting robot control loop...") - robot_controller.start_control_loop() - - # Login to Neuracore - print("\n🔑 Logging in to Neuracore...") - nc.login() - - # Get dataset from Neuracore - print("\n🔍 Getting dataset from Neuracore...") - dataset = nc.get_dataset(args.dataset_name) - - # Cross-embodiment sync (same pattern as examples/6_visualize_policy_from_dataset.py) - print("\n🔁 Building cross_embodiment_union for synchronization...") - input_modalities: list[DataType] = [ - DataType.JOINT_POSITIONS, - DataType.RGB_IMAGES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, - ] - output_modalities: list[DataType] = [ - DataType.JOINT_TARGET_POSITIONS, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, - ] - input_cross_embodiment_description: CrossEmbodimentDescription = {} - output_cross_embodiment_description: CrossEmbodimentDescription = {} - for robot_id in dataset.robot_ids: - full = dataset.get_full_embodiment_description(robot_id) - input_cross_embodiment_description[robot_id] = { - dt: full[dt] for dt in input_modalities if dt in full - } - output_cross_embodiment_description[robot_id] = { - dt: full[dt] for dt in output_modalities if dt in full - } - cross_embodiment_union = merge_cross_embodiment_description( - input_cross_embodiment_description, - output_cross_embodiment_description, - ) - - # Synchronize dataset - print("\n🔁 Synchronizing dataset...") - synced_dataset = dataset.synchronize( - frequency=args.frequency, - cross_embodiment_union=cross_embodiment_union, - ) - - # Determine which episodes to play - episode_indices: list[int] = [] - if args.episode_index == -1: - episode_indices = list(range(len(synced_dataset))) - print(f"\n📊 Found {len(synced_dataset)} episodes. Will play all episodes.") - else: - episode_indices = [args.episode_index] - print(f"\n📊 Playing episode {args.episode_index} only.") - - # Play episodes - try: - for episode_idx in episode_indices: - - robot_controller.move_to_home() - seconds_to_wait = 10 - while not robot_controller.is_robot_homed(): - time.sleep(1) - seconds_to_wait -= 1 - if seconds_to_wait <= 0: - break - print( - f"🔍 Waiting for robot to reach home position... {seconds_to_wait} seconds remaining." - ) - if robot_controller.is_robot_homed(): - print("✓ Robot is at home position.") - else: - print("❌ Robot did not reach home position within 10 seconds.") - print( - f"🔍 Current joint angles: {robot_controller.get_current_joint_angles()}" - ) - print(f"🔍 Home joint angles: {robot_controller.HOME_JOINT_ANGLES}") - - print(f"\n{'='*60}") - print(f"🎬 Playing Episode {episode_idx} / {len(synced_dataset) - 1}") - print(f"{'='*60}") - - episode = synced_dataset[episode_idx] - - print(f"\n🚀 Collecting episode {episode_idx} data...") - rgb_frames_per_step: list[dict[str, np.ndarray]] = [] - parallel_gripper_open_amounts = [] - joint_positions = [] - for step in tqdm(episode, desc=f"Collecting episode {episode_idx}"): - step = cast(SynchronizedPoint, step) - - # Extract joint positions - joint_positions_dict = {} - if DataType.JOINT_TARGET_POSITIONS in step.data: - joint_data = step.data[DataType.JOINT_TARGET_POSITIONS] - for joint_name in JOINT_NAMES: - if joint_name in joint_data: - joint_positions_dict[joint_name] = joint_data[ - joint_name - ].value - joint_positions.append([joint_positions_dict[jn] for jn in JOINT_NAMES]) - - # Extract gripper - gripper_value = 0.0 - if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in step.data: - gripper_data = step.data[ - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ] - if GRIPPER_NAME in gripper_data: - gripper_value = gripper_data[GRIPPER_NAME].open_amount - parallel_gripper_open_amounts.append(gripper_value) - - # Extract RGB for all cameras - step_frames: dict[str, np.ndarray] = {} - if DataType.RGB_IMAGES in step.data: - rgb_data = step.data[DataType.RGB_IMAGES] - for camera_name, img_value in rgb_data.items(): - step_frames[camera_name] = img_value.frame - rgb_frames_per_step.append(step_frames) - - joint_positions = np.degrees(np.array(joint_positions)) - parallel_gripper_open_amounts = np.array(parallel_gripper_open_amounts) - - print(f"\n🚀 Replaying episode {episode_idx} data...") - for index in tqdm( - range(len(joint_positions)), desc=f"Replaying episode {episode_idx}" - ): - start_time = time.time() - robot_controller.set_target_joint_angles(joint_positions[index]) - robot_controller.set_gripper_open_value( - parallel_gripper_open_amounts[index] - ) - - # Display camera frames (dataset stores RGB; OpenCV expects BGR) - if index < len(rgb_frames_per_step): - for camera_name, frame_rgb in rgb_frames_per_step[index].items(): - arr = np.asarray(frame_rgb, dtype=np.uint8) - frame_bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) - cv2.imshow(f"Replay: {camera_name}", frame_bgr) - if cv2.waitKey(1) & 0xFF == ord("q"): - print("\n🛑 'q' pressed, stopping replay...") - break - - end_time = time.time() - time.sleep(max(0, 1 / args.frequency - (end_time - start_time))) - cv2.destroyAllWindows() - print(f"🎉 Episode {episode_idx} replay completed.") - - if args.episode_index == -1: - print(f"\n{'='*60}") - print(f"🎉 All {len(synced_dataset)} episodes replay completed!") - print(f"{'='*60}") - except KeyboardInterrupt: - print("\n🛑 Keyboard interrupt detected, stopping robot control loop...") - cv2.destroyAllWindows() - - robot_controller.stop_control_loop() - robot_controller.cleanup() - - -if __name__ == "__main__": - main() - - -============================================================ -FILE: combine_code.py -============================================================ - -import pathlib - -def combine_python_files(directory_path, output_filename): - """ - Recursively finds all .py files in a directory and combines them into a single text file. - - Args: - directory_path (str): The path to the root directory to search. - output_filename (str): The name/path of the output text file. - """ - # Create a Path object for the target directory - source_dir = pathlib.Path(directory_path) - - # Ensure the directory exists - if not source_dir.is_dir(): - print(f"Error: The directory '{directory_path}' does not exist.") - return - - # Open the output file in write mode - with open(output_filename, 'w', encoding='utf-8') as outfile: - # rglob('*.py') recursively searches for all files ending in .py - py_files = list(source_dir.rglob('*.py')) - - if not py_files: - print(f"No Python files found in '{directory_path}'.") - return - - print(f"Found {len(py_files)} Python files. Combining...") - - for file_path in py_files: - # Write a clear separator and the file path as a header - outfile.write(f"\n{'='*60}\n") - outfile.write(f"FILE: {file_path}\n") - outfile.write(f"{'='*60}\n\n") - - # Read the python file and append its contents - try: - with open(file_path, 'r', encoding='utf-8') as infile: - outfile.write(infile.read()) - outfile.write("\n") - except Exception as e: - error_msg = f"Error reading file {file_path}: {e}\n" - print(error_msg) - outfile.write(error_msg) - - print(f"Success! All files have been combined into '{output_filename}'.") - -# --- Example Usage --- -if __name__ == "__main__": - # Replace these variables with your actual paths - TARGET_DIRECTORY = "./" # "./" means current directory - OUTPUT_FILE = "combined_code.txt" - - combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) - -============================================================ -FILE: 2_collect_teleop_data_with_neuracore.py -============================================================ - -#!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller and Neuracore data collection. - -This demo uses Pink IK control with Meta Quest controller input to control the Piper robot and -logs data to Neuracore. -""" - - -import argparse -import multiprocessing -import subprocess -import sys -import threading -import time -import traceback -from pathlib import Path -from typing import Any - -import neuracore as nc -import numpy as np - -# Add parent directory to path to import pink_ik_solver and piper_controller -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - IK_SOLVER_RATE, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - META_QUEST_AXIS_MASK, - NEUTRAL_END_EFFECTOR_POSE, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POSITION_COST, - POSTURE_COST_VECTOR, - ROBOT_RATE, - ROTATION_SCALE, - SLOW_ROTATION_SCALE, - SLOW_TRANSLATION_SCALE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, - TRANSLATION_SCALE, - URDF_PATH, - WRIST_JOINT_BUTTON_STEP_DEGREES, -) -from common.data_manager import DataManager, RobotActivityState -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread -from common.threads.quest_reader import quest_reader_thread -from common.threads.realsense_camera import camera_thread as realsense_camera_thread -from meta_quest_teleop.reader import MetaQuestReader - -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - - -def play_audio_feedback(action: str) -> None: - """Play distinct synthesized tones asynchronously using SoX.""" - # Start = High pitch (880 Hz), Stop = Low pitch (440 Hz) - freq = "880" if action == "start" else "440" - try: - # -q: quiet, -n: no input file, synth: generate audio, 0.3: duration, sine: wave type - subprocess.Popen( - ["play", "-q", "-n", "synth", "0.3", "sine", freq], - stderr=subprocess.DEVNULL - ) - except Exception as e: - print(f"⚠️ Failed to play tone (is SoX installed?): {e}") - -def log_to_neuracore_on_change_callback( - name: str, payload: dict[str, Any], timestamp: float -) -> None: - """Log data to queue on change callback.""" - # Call appropriate Neuracore logging function - try: - #print(f"\n📤 Logging {name} to Neuracore with timestamp {timestamp:.3f}...") - if name == "log_joint_positions": - nc.log_joint_positions(payload, timestamp=timestamp) - elif name == "log_joint_torques": - nc.log_joint_torques(payload, timestamp=timestamp) - elif name == "log_joint_target_positions": - nc.log_joint_target_positions(payload, timestamp=timestamp) - elif name == "log_parallel_gripper_open_amounts": - nc.log_parallel_gripper_open_amounts(payload, timestamp=timestamp) - elif name == "log_parallel_gripper_target_open_amounts": - nc.log_parallel_gripper_target_open_amounts(payload, timestamp=timestamp) - elif name == "log_rgb": - camera_name = next(iter(payload)) - image_array = payload[camera_name] - nc.log_rgb(camera_name, image_array, timestamp=timestamp) - else: - print(f"\n⚠️ Unknown logging function: {name}") - except Exception as e: - print(f"\n⚠️ Failed to call {name} to Neuracore. Exception: {e}") - print("Traceback:") - traceback.print_exc() - - -def on_button_a_pressed() -> None: - """Handle Button A press to toggle robot enable/disable state.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - # Disable robot - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - print("✓ 🔴 Robot disabled (Button A)") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Button A)") - else: - print("✗ Failed to enable robot") - - -def on_button_b_pressed() -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 Button B pressed - Moving to home position...") - # Set state to HOMING to prevent IK thread from sending robot commands - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - # Disable teleop during homing - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Button B pressed but robot is not enabled") - - -def on_button_rj_pressed() -> None: - """Handle Button Right Joystick press to toggle data recording state.""" - if not nc.is_recording(): - # Start recording - try: - nc.start_recording() - print("✓ 🔴 Recording started (Button RJ)") - play_audio_feedback("start") # <-- Trigger distinct START sound - except Exception as e: - print(f"✗ Failed to start recording. Exception: {e}") - print("Traceback:") - traceback.print_exc() - else: - # Stop recording - try: - nc.stop_recording() - print("✓ ⏹️ Recording stopped (Button RJ)") - play_audio_feedback("stop") # <-- Trigger distinct STOP sound - except Exception as e: - print(f"✗ Failed to stop recording. Exception: {e}") - print("Traceback:") - traceback.print_exc() - - -def _step_wrist_joint(delta_degrees: float) -> None: - """Apply a relative step to the wrist joint target angle.""" - # Prevent IK teleop loop from overwriting this manual joint adjustment. - data_manager.set_teleop_state(False, None, None) - robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) - - target_joint_angles = robot_controller.get_target_joint_angles() - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - base_wrist_joint_angle = float(current_joint_angles[-1]) - else: - base_wrist_joint_angle = float(target_joint_angles[-1]) - - target_joint_angles[-1] = base_wrist_joint_angle + delta_degrees - robot_controller.set_target_joint_angles(target_joint_angles) - data_manager.set_target_joint_angles(target_joint_angles) - - -def on_button_y_pressed() -> None: - """Handle Button Y press to add +5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button Y pressed but robot is not enabled") - return - _step_wrist_joint(WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_x_pressed() -> None: - """Handle Button X press to subtract -5° on the wrist joint.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state != RobotActivityState.ENABLED: - print("⚠️ Button X pressed but robot is not enabled") - return - _step_wrist_joint(-WRIST_JOINT_BUTTON_STEP_DEGREES) - - -def on_button_lj_pressed() -> None: - """Toggle slow translation/rotation scaling mode from config values.""" - slow_scaling_mode_enabled = data_manager.toggle_slow_scaling_mode_enabled() - if slow_scaling_mode_enabled: - data_manager.set_teleop_scaling(SLOW_TRANSLATION_SCALE, SLOW_ROTATION_SCALE) - print( - "🐢 Button LJ pressed - Slow scaling enabled " - f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " - f"rotation={SLOW_ROTATION_SCALE:.3f})" - ) - else: - data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) - print( - "🐇 Button LJ pressed - Slow scaling disabled " - f"(translation={TRANSLATION_SCALE:.3f}, " - f"rotation={ROTATION_SCALE:.3f})" - ) - - -if __name__ == "__main__": - multiprocessing.set_start_method("spawn") - - parser = argparse.ArgumentParser( - description="Piper Robot Teleoperation with Neuracore Data Collection - REAL ROBOT CONTROL" - ) - parser.add_argument( - "--ip-address", - type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", - ) - parser.add_argument( - "--dataset-name", - type=str, - default=None, - help="Name for the dataset (optional, defaults to auto-generated timestamp-based name)", - ) - args = parser.parse_args() - - print("=" * 60) - print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL") - print("=" * 60) - print("Thread frequencies:") - print(f" 🎮 Quest Controller: {CONTROLLER_DATA_RATE} Hz") - print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") - print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") - print(f" 📸 Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz") - print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") - - # Connect to Neuracore - print("\n🔧 Initializing Neuracore...") - nc.login() - nc.connect_robot( - robot_name="AgileX PiPER", - urdf_path=str(URDF_PATH), - overwrite=False, - ) - - # Create dataset - dataset_name = ( - args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" - ) - print(f"\n🔧 Creating dataset {dataset_name}...") - nc.create_dataset( - name=dataset_name, - description="Teleop data collection for Piper robot", - ) - - # Initialize shared state - data_manager = DataManager() - data_manager.set_on_change_callback(log_to_neuracore_on_change_callback) - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - ) - data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) - - # Initialize robot controller - print("\n🤖 Initializing Piper robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - neutral_end_effector_pose=NEUTRAL_END_EFFECTOR_POSE, - enable_joint_angle_limits=False, - debug_mode=False, - ) - - # Start robot control loop - print("\n🚀 Starting robot control loop...") - robot_controller.start_control_loop() - - # Start joint state thread - print("\n📊 Starting joint state thread...") - joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) - joint_state_thread_obj.start() - - # Initialize Meta Quest reader - print("\n🎮 Initializing Meta Quest reader...") - quest_reader = MetaQuestReader( - ip_address=args.ip_address, - port=5555, - run=True, - axis_mask=META_QUEST_AXIS_MASK, - ) - - # Register button callbacks (after state and robot_controller are initialized) - quest_reader.on("button_a_pressed", on_button_a_pressed) - quest_reader.on("button_b_pressed", on_button_b_pressed) - quest_reader.on("button_rj_pressed", on_button_rj_pressed) - quest_reader.on("button_y_pressed", on_button_y_pressed) - quest_reader.on("button_x_pressed", on_button_x_pressed) - quest_reader.on("button_lj_pressed", on_button_lj_pressed) - - # Start data collection thread - print("\n🎮 Starting quest reader thread...") - quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ) - quest_thread.start() - - # set initial configuration to current joint angles - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) - else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - - # Create Pink IK solver - print("\n🔧 Creating Pink IK solver...") - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - - # Start IK solver thread - print("\n🧮 Starting IK solver thread...") - ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ) - ik_thread.start() - - # Start cameras threads - print("\n📷 Starting cameras threads...") - realsense_camera_thread_obj = threading.Thread( - target=realsense_camera_thread, args=(data_manager,), daemon=True - ) - realsense_camera_thread_obj.start() - - # usb_camera_thread_obj = threading.Thread( - # target=usb_camera_thread, args=(data_manager,), daemon=True - # ) - # usb_camera_thread_obj.start() - - print() - print("🚀 Starting teleoperation with REAL ROBOT CONTROL...") - print("🎮 CONTROLS:") - print(" 1. Press BUTTON A to enable/disable robot") - print(" 2. Hold RIGHT GRIP to activate teleoperation") - print(" 3. Move controller - robot follows!") - print(" 4. Hold RIGHT TRIGGER to close gripper") - print(" 5. Press BUTTON B to send robot home") - print(" 6. Press RIGHT JOYSTICK to start/stop data recording") - print(" 7. Press BUTTON Y to add +5° on the wrist joint") - print(" 8. Press BUTTON X to subtract 5° on the wrist joint") - print(" 9. Press LEFT JOYSTICK BUTTON to toggle slow scaling mode") - print(" 10. Release grip to stop") - print("⚠️ Press Ctrl+C to exit") - print() - - try: - while not data_manager.is_shutdown_requested(): - time.sleep(1) - except KeyboardInterrupt: - print("\n👋 Interrupt received - shutting down gracefully...") - data_manager.request_shutdown() - except Exception as e: - print(f"\n❌ Demo error. Exception: {e}") - print("Traceback:") - traceback.print_exc() - data_manager.request_shutdown() - else: - print("\n⚠️ Worker requested shutdown - cleaning up...") - # Cleanup - print("\n🧹 Cleaning up...") - - # Cancel recording if active - if nc.is_recording(): - try: - print("⚠️ Cancelling active recording...") - nc.cancel_recording() - print("✓ Recording cancelled") - except Exception as e: - print(f"⚠️ Error cancelling recording. Exception: {e}") - print("Traceback:") - traceback.print_exc() - - # shutdown threads and data producers before logging out - data_manager.request_shutdown() - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - quest_reader.stop() - quest_thread.join() - joint_state_thread_obj.join() - ik_thread.join() - realsense_camera_thread_obj.join() - # usb_camera_thread_obj.join() - nc.logout() - robot_controller.cleanup() - - print("\n👋 Demo stopped.") - - -============================================================ -FILE: 7_teleop_with_pedal.py -============================================================ - -#!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest and Foot Pedal control. - -This script combines Meta Quest controller input for movement with Foot Pedal -control for session management (Activate, Home, Record). -""" - -import argparse -import multiprocessing -import sys -import threading -import time -from pathlib import Path -from typing import Any - -import neuracore as nc -import numpy as np - -# Add parent directory to path to import pink_ik_solver and piper_controller -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import ( - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_MIN_CUTOFF, - GRIPPER_FRAME_NAME, - GRIPPER_LOGGING_NAME, - JOINT_NAMES, - NEUTRAL_JOINT_ANGLES, - POSTURE_COST_VECTOR, - ROBOT_RATE, - URDF_PATH, -) -from common.data_manager import DataManager, RobotActivityState -from common.foot_pedal import FootPedal -from common.threads.camera import camera_thread -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread -from common.threads.quest_reader import quest_reader_thread -from meta_quest_teleop.reader import MetaQuestReader - -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - -ENABLE_DISABLE_PEDAL = "a" -HOME_POSITION_PEDAL = "b" -RECORD_TOGGLE_PEDAL = "c" - - -def log_to_neuracore_on_change_callback( - name: str, value: Any, timestamp: float -) -> None: - """Callback triggered on state changes to log data to Neuracore. - - Args: - name: Name of the data stream. - value: Data value (float, array, or image). - timestamp: Time of the change. - """ - try: - if name == "log_joint_positions": - data_dict = {jn: np.radians(a) for jn, a in zip(JOINT_NAMES, value)} - nc.log_joint_positions(data_dict, timestamp=timestamp) - elif name == "log_joint_target_positions": - data_dict = {jn: np.radians(a) for jn, a in zip(JOINT_NAMES, value)} - nc.log_joint_target_positions(data_dict, timestamp=timestamp) - elif name == "log_parallel_gripper_open_amounts": - nc.log_parallel_gripper_open_amounts( - {GRIPPER_LOGGING_NAME: value}, timestamp=timestamp - ) - elif name == "log_parallel_gripper_target_open_amounts": - nc.log_parallel_gripper_target_open_amounts( - {GRIPPER_LOGGING_NAME: value}, timestamp=timestamp - ) - elif name == "log_rgb": - nc.log_rgb("rgb", value, timestamp=timestamp) - except Exception as e: - print(f"⚠️ Logging failed: {e}") - - -def toggle_robot_state() -> None: - """Toggle the robot's activity state between ENABLED and DISABLED.""" - print("🔘 Pedal toggled - Robot State") - state = data_manager.get_robot_activity_state() - - if state == RobotActivityState.ENABLED: - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - if robot_controller: - robot_controller.graceful_stop() - data_manager.set_teleop_state(False, None, None) - print("✓ 🔴 Robot disabled (Pedal)") - elif state == RobotActivityState.DISABLED or state == RobotActivityState.HOMING: - # If no robot, just toggle the state for dashboard visibility - if not robot_controller: - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Pedal - Headless)") - return - - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🟢 Robot enabled (Pedal)") - else: - print("✗ [ACTION] Failed to enable robot") - - -def move_robot_home() -> None: - """Command the robot to move to its neutral/home position.""" - print("🏠 Pedal toggled - Move Home") - state = data_manager.get_robot_activity_state() - - if state == RobotActivityState.ENABLED: - print("🏠 Pedal pressed - Moving to home position...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - data_manager.set_teleop_state(False, None, None) - - if robot_controller: - if not robot_controller.move_to_home(): - print("✗ Failed to initiate home move") - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - # Headless simulation of homing - time.sleep(1) - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ 🏠 Robot homed (Headless)") - else: - print("⚠️ Pedal pressed but robot is not enabled") - - -def toggle_recording() -> None: - """Start or stop a data recording session in Neuracore.""" - print("⏺️ Pedal toggled - Recording") - if not nc.is_recording(): - try: - nc.start_recording() - print("✓ 🔴 Recording started (Pedal)") - except Exception as e: - print(f"✗ Failed to start recording: {e}") - else: - try: - nc.stop_recording() - print("✓ ⏹️ Recording stopped (Pedal)") - except Exception as e: - print(f"✗ Failed to stop recording: {e}") - - -if __name__ == "__main__": - multiprocessing.set_start_method("spawn") - - parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleoperation") - parser.add_argument("--ip-address", type=str, help="Meta Quest IP") - parser.add_argument("--dataset-name", type=str, help="Neuracore dataset name") - args = parser.parse_args() - - print("=" * 60) - print("PIPER TELEOP: META QUEST + FOOT PEDALS") - print("=" * 60) - - # Neuracore Init - print("\n🔧 Initializing Neuracore...") - try: - nc.login() - nc.connect_robot( - robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False - ) - ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%H-%M-%S')}" - nc.create_dataset(name=ds_name, description="Quest + Pedal unified collection") - except Exception as e: - print(f"⚠️ Neuracore initialization skipped/failed: {e}") - - # Shared State - data_manager = DataManager() - data_manager.set_on_change_callback(log_to_neuracore_on_change_callback) - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF - ) - - # Robot Initialization - print("\n🤖 Initializing Piper...") - robot_controller = None - try: - robot_controller = PiperController(can_interface="can0", robot_rate=ROBOT_RATE) - robot_controller.start_control_loop() - except Exception as e: - print(f"⚠️ Robot controller initialization skipped/failed: {e}") - - # Threads - print("\n📊 Starting Threads (JointState, QuestReader, IKSolver, Camera)...") - pedal_thread = None - if robot_controller: - threading.Thread( - target=joint_state_thread, - args=(data_manager, robot_controller), - daemon=True, - ).start() - - quest_reader = None - try: - print("🔍 Searching for Meta Quest...") - # Adb initialization in the reader might call sys.exit(1), so we catch BaseException - quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) - threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ).start() - except (Exception, BaseException) as e: - print(f"⚠️ Quest reader initialization skipped/failed: {e}") - - # Sync IK solver to current position - try: - initial_joints = np.radians( - data_manager.get_current_joint_angles() or NEUTRAL_JOINT_ANGLES - ) - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - initial_configuration=initial_joints, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ).start() - except Exception as e: - print(f"⚠️ IK Solver initialization skipped/failed: {e}") - - try: - threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ).start() - except Exception as e: - print(f"⚠️ Camera thread failed to start: {e}") - - # Foot Pedal – started as a daemon thread, callbacks wired inline - print("\n⌨️ Initializing Foot Pedals...") - pedal = FootPedal( - key_map={ - "button_a": ENABLE_DISABLE_PEDAL, - "button_b": HOME_POSITION_PEDAL, - "button_c": RECORD_TOGGLE_PEDAL, - }, - ) - pedal.bind("button_a", toggle_robot_state) - pedal.bind("button_b", move_robot_home) - pedal.bind("button_c", toggle_recording) - - pedal_thread = threading.Thread( - target=pedal.run_loop, args=(data_manager,), daemon=True - ) - pedal_thread.start() - - print("\n✅ SYSTEM ONLINE") - print("------------------------------------------------------------") - print("🎮 QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") - print("⌨️ PEDAL CONTROLS: ENABLE/DISABLE (A), HOME (B), RECORD (C)") - print("------------------------------------------------------------") - - try: - while True: - time.sleep(1) - except KeyboardInterrupt: - print("\n👋 Shutting down...") - finally: - if pedal_thread: - pedal_thread.join(timeout=1.0) - try: - if nc.is_recording(): - nc.cancel_recording() - nc.logout() - except Exception: - pass - data_manager.request_shutdown() - if quest_reader: - try: - quest_reader.stop() - except Exception: - pass - if robot_controller: - try: - robot_controller.cleanup() - except Exception: - pass - - -============================================================ -FILE: 4_rollout_neuracore_policy.py -============================================================ - -#!/usr/bin/env python3 -"""Piper Robot Test with Neuracore policy. - -This script loads a trained Neuracore policy, reads status from the piper robot -controlled by the Meta Quest controller, and replays the prediction horizon virtually -on Viser to test the stability of the policy output. -""" - -import argparse -import sys -import threading -import time -import traceback -from pathlib import Path - -import neuracore as nc -import numpy as np -from neuracore.ml.preprocessing.methods.resize_pad import ResizePad -from neuracore.ml.utils.preprocessing_utils import PreprocessingConfiguration -from neuracore_types import ( - BatchedJointData, - BatchedNCData, - BatchedParallelGripperOpenAmountData, - DataType, - EmbodimentDescription, -) - -# Add parent directory to path to import pink_ik_solver and piper_controller -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, - CAMERA_NAMES, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - GRIPPER_NAME, - IK_SOLVER_RATE, - JOINT_NAMES, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - MAX_ACTION_ERROR_THRESHOLD, - MAX_SAFETY_THRESHOLD, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POLICY_EXECUTION_RATE, - POSITION_COST, - POSTURE_COST_VECTOR, - PREDICTION_HORIZON_EXECUTION_RATIO, - ROBOT_RATE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, - TARGETING_POSE_TIME_THRESHOLD, - URDF_PATH, - VISUALIZATION_RATE, -) -from common.data_manager import DataManager, RobotActivityState -from common.policy_state import PolicyState -from common.robot_visualizer import RobotVisualizer -from common.threads.ik_solver import ik_solver_thread -from common.threads.joint_state import joint_state_thread -from common.threads.quest_reader import quest_reader_thread -from common.threads.realsense_camera import camera_thread -from meta_quest_teleop.reader import MetaQuestReader - -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - - -def _embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: - """Ordered channel names from an embodiment entry (list or index→name map).""" - if isinstance(spec, dict): - return [spec[i] for i in sorted(spec)] - return list(spec) - - -def convert_predictions_to_horizon( - predictions: dict[DataType, dict[str, BatchedNCData]], -) -> dict[str, list[float]]: - """Convert predictions to horizon dict.""" - print('[convert_predictions_to_horizon]') - horizon = {} - if DataType.JOINT_TARGET_POSITIONS in predictions: - joint_data = predictions[DataType.JOINT_TARGET_POSITIONS] - for joint_name in JOINT_NAMES: - if joint_name in joint_data: - batched = joint_data[joint_name] - if isinstance(batched, BatchedJointData): - values = batched.value[0, :, 0].cpu().numpy().tolist() - horizon[joint_name] = values - - # Extract gripper open amounts - if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: - print('[convert_predictions_to_horizon] Extracting gripper open amounts') - gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - if GRIPPER_NAME in gripper_data: - batched = gripper_data[GRIPPER_NAME] - if isinstance(batched, BatchedParallelGripperOpenAmountData): - values = batched.open_amount[0, :, 0].cpu().numpy().tolist() - horizon[GRIPPER_NAME] = values - return horizon - - -def toggle_robot_enabled_status( - data_manager: DataManager, - robot_controller: PiperController, - visualizer: RobotVisualizer, -) -> None: - """Handle Button A press to toggle robot enable/disable state.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - # Disable robot - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - robot_controller.graceful_stop() - # Reset teleop state when disabling robot - data_manager.set_teleop_state(False, None, None) - visualizer.update_toggle_robot_enabled_status(False) - print("✓ 🔴 Robot disabled") - elif robot_activity_state == RobotActivityState.DISABLED: - if robot_controller.resume_robot(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - visualizer.update_toggle_robot_enabled_status(True) - print("✓ 🟢 Robot enabled") - else: - print("✗ Failed to enable robot") - - -def home_robot(data_manager: DataManager, robot_controller: PiperController) -> None: - """Handle Button B press to move robot to home position.""" - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.ENABLED: - print("🏠 Button B pressed - Moving to home position...") - # Set state to HOMING to prevent IK thread from sending robot commands - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - # Disable teleop during homing - data_manager.set_teleop_state(False, None, None) - ok = robot_controller.move_to_home() - if not ok: - print("✗ Failed to initiate home move") - # Revert to ENABLED on failure - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - else: - print("⚠️ Home command received but robot is not enabled") - - -def run_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> bool: - """Handle Run Policy button press to capture state and get policy prediction.""" - print("Running policy...") - - # Get available data from data_manager (only log what the model expects) - current_joint_angles = None - gripper_open_value = None - rgb_image = None - - if DataType.JOINT_POSITIONS in input_embodiment_description: - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - joint_angles_rad = np.radians(current_joint_angles) - positions_by_name = { - jn: float(ang) for jn, ang in zip(JOINT_NAMES, joint_angles_rad) - } - policy_joint_order = _embodiment_names_ordered( - input_embodiment_description[DataType.JOINT_POSITIONS] - ) - joint_positions_dict = { - jn: positions_by_name[jn] for jn in policy_joint_order - } - nc.log_joint_positions(joint_positions_dict) - print(" ✓ Logged joint positions") - else: - print(" ⚠️ No current joint angles available") - - if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: - gripper_open_value = data_manager.get_current_gripper_open_value() - if gripper_open_value is not None: - nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_open_value) - print(" ✓ Logged gripper open amount") - else: - print(" ⚠️ No gripper open value available") - - if DataType.RGB_IMAGES in input_embodiment_description: - rgb_names = _embodiment_names_ordered( - input_embodiment_description[DataType.RGB_IMAGES] - ) - logged_any_rgb = False - for camera_name in rgb_names: - img = data_manager.get_rgb_image(camera_name) - if img is not None: - nc.log_rgb(camera_name, img) - logged_any_rgb = True - if logged_any_rgb: - print(" ✓ Logged RGB image(s)") - else: - print(" ⚠️ No RGB image available") - if rgb_names: - rgb_image = data_manager.get_rgb_image(rgb_names[0]) - - # Check if we have at least some data to run the policy - if ( - current_joint_angles is None - and gripper_open_value is None - and rgb_image is None - ): - print("✗ No data available to run policy") - return False - - # Get policy prediction - try: - start_time = time.time() - predictions = policy.predict(timeout=60) - inference_time = time.time() - start_time - prediction_horizon = convert_predictions_to_horizon(predictions) - conversion_time = time.time() - start_time - inference_time - - # Calculate length directly from the new prediction dictionary - horizon_length = 0 - if prediction_horizon: - first_key = next(iter(prediction_horizon.keys())) - horizon_length = len(prediction_horizon[first_key]) - - print( - f" ✓ Got policy prediction in {inference_time:.2f} seconds with horizon length {horizon_length}" - ) - print(f" ✓ Conversion time: {conversion_time:.2f} seconds") - - prediction_ratio = visualizer.get_prediction_ratio() - policy_state.set_execution_ratio(prediction_ratio) - - # Set policy inputs (only if available) - if rgb_image is not None: - policy_state.set_policy_rgb_image_input(rgb_image) - if current_joint_angles is not None: - policy_state.set_policy_state_input(current_joint_angles) - - # Store prediction horizon actions in policy state - policy_state.set_prediction_horizon(prediction_horizon) - - visualizer.update_ghost_robot_visibility(True) - policy_state.set_ghost_robot_playing(True) - policy_state.reset_ghost_action_index() - - except Exception as e: - print(f"✗ Failed to get policy prediction: {e}") - traceback.print_exc() - return False - - return True - - -def start_policy_execution( - data_manager: DataManager, policy_state: PolicyState -) -> bool: - """Handle Execute Policy button press to start policy execution.""" - print("Starting policy execution...") - - # Check if policy execution is already active - if ( - data_manager.get_robot_activity_state() == RobotActivityState.POLICY_CONTROLLED - and not policy_state.get_continuous_play_active() - ): - print("⚠️ Policy execution already in progress") - return False - # Check if robot is enabled - elif data_manager.get_robot_activity_state() == RobotActivityState.DISABLED: - print("⚠️ Cannot execute policy: Robot is disabled") - return False - - # Get prediction horizon - prediction_horizon = policy_state.get_prediction_horizon() - prediction_horizon_length = policy_state.get_prediction_horizon_length() - if prediction_horizon_length == 0: - print("⚠️ No prediction horizon available. Make sure policy was run first.") - return False - - # Check that we have joint data for all joints - if not all(joint_name in prediction_horizon for joint_name in JOINT_NAMES): - print("⚠️ First prediction in horizon has no joint targets") - return False - - # Safety check: verify robot is close enough to first action - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is None: - print("⚠️ Cannot execute policy: No current joint angles available") - return False - - # Get first action from horizon (index 0 for each joint) - current_joint_target_positions_rad = np.array( - [prediction_horizon[joint_name][0] for joint_name in JOINT_NAMES] - ) - current_target_deg = np.degrees(current_joint_target_positions_rad) - joint_differences = np.abs(current_joint_angles - current_target_deg) - - if np.any(joint_differences > MAX_SAFETY_THRESHOLD): - print("⚠️ Cannot execute policy: Robot too far from first predicted action") - print(" --- DIAGNOSTICS ---") - print(f" Current Angles: {[f'{d:.2f}' for d in current_joint_angles]}") - print(f" AI Predicted: {[f'{d:.2f}' for d in current_target_deg]}") - print(f" Differences: {[f'{d:.2f}' for d in joint_differences]}") - print(f" Threshold: {MAX_SAFETY_THRESHOLD}°") - print(" 💡 TIP 1: Did the arm sag? Check if 'Current Angles' are drooping.") - print(" 💡 TIP 2: If the AI naturally predicts large first steps, increase MAX_SAFETY_THRESHOLD in common/configs.py") - return False - - # All checks passed - start execution - - # Stop ghost visualization - policy_state.set_ghost_robot_playing(False) - - # Deactivate teleop - data_manager.set_teleop_state(False, None, None) - - # Lock policy inputs and start execution - policy_state.start_policy_execution() - - # Verify locked horizon was created successfully - locked_horizon_length = policy_state.get_locked_prediction_horizon_length() - if locked_horizon_length == 0: - print("⚠️ Failed to lock prediction horizon - horizon is empty") - policy_state.end_policy_execution() - return False - - print(f"✓ Starting policy execution with {locked_horizon_length} actions") - - # Change robot state to POLICY_CONTROLLED - data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) - - return True - - -def end_policy_play( - data_manager: DataManager, - policy_state: PolicyState, - visualizer: RobotVisualizer, - policy_status_message: str, -) -> None: - """End continuous play and set robot activity state to ENABLED and update policy status.""" - if policy_state.get_continuous_play_active(): - policy_state.set_continuous_play_active(False) - - # Reset ghost robot color to default orange - visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) - - visualizer.update_play_policy_button_status(False) - - visualizer.update_play_policy_button_status(False) - policy_state.end_policy_execution() - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - data_manager.set_teleop_state(False, None, None) - visualizer.update_policy_status(policy_status_message) - -def continuous_prediction_worker( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, - continuous_mode: str = "pipeline", -) -> None: - """Background thread for continuous execution supporting pipelined and sequential modes.""" - VISUALIZATION_COLORS = [ - (1.0, 0.65, 0.0, 0.25), # Orange (Default) - (0.0, 1.0, 0.0, 0.25), # Green - (1.0, 0.0, 0.0, 0.25), # Red - (0.0, 0.0, 1.0, 0.25), # Blue - ] - color_index = 0 - - # 1. Bootstrap the very first prediction to get the robot moving - print(f"\n🚀 [Worker] Bootstrapping initial trajectory in '{continuous_mode}' mode...") - success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) - if success: - start_policy_execution(data_manager, policy_state) - - while policy_state.get_continuous_play_active(): - # Failsafe: if there's no active trajectory running yet, wait briefly - if policy_state.get_locked_prediction_horizon_length() == 0: - time.sleep(0.01) - continue - - if continuous_mode == "pipeline": - print("\n📸 [Pipeline Worker] Robot is moving! Prefetching next prediction in background...") - # Query the network in parallel while the execution thread is driving the motors - success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) - - if not success or not policy_state.get_continuous_play_active(): - time.sleep(0.05) - continue - - # Wait until the current trajectory buffer is running low before swapping - while policy_state.get_continuous_play_active(): - exec_idx = policy_state.get_execution_action_index() - total_len = policy_state.get_locked_prediction_horizon_length() - remaining = total_len - exec_idx - - # Hot-swap when 5 or fewer steps are left in the active trajectory - if remaining <= 5 or total_len == 0: - break - time.sleep(0.01) - - elif continuous_mode == "sequential": - # Wait until the current trajectory buffer is completely exhausted - while policy_state.get_continuous_play_active(): - exec_idx = policy_state.get_execution_action_index() - total_len = policy_state.get_locked_prediction_horizon_length() - if exec_idx >= total_len or total_len == 0: - break - time.sleep(0.01) - - if not policy_state.get_continuous_play_active(): - break - - print("\n📸 [Sequential Worker] Trajectory finished! Holding position and requesting next prediction...") - success = run_policy(data_manager, policy, policy_state, visualizer, input_embodiment_description) - - if not success or not policy_state.get_continuous_play_active(): - time.sleep(0.05) - continue - - if not policy_state.get_continuous_play_active(): - break - - print("🔄 [Worker] Swapping to new trajectory buffer!") - # Seamlessly clear the lock and flash the new horizon into play - policy_state.end_policy_execution() - success = start_policy_execution(data_manager, policy_state) - - if success: - color_index = (color_index + 1) % len(VISUALIZATION_COLORS) - visualizer.set_ghost_robot_color(VISUALIZATION_COLORS[color_index]) - else: - print("❌ [Worker] Swap rejected by safety threshold. Retrying immediately...") - time.sleep(0.01) - -def play_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, - continuous_mode: str = "pipeline", -) -> None: - """Handle Play Policy button press to start/stop continuous policy execution.""" - if not policy_state.get_continuous_play_active(): - # Start continuous play - print(f"▶️ Play Policy button pressed - Starting {continuous_mode.capitalize()} Mode...") - policy_state.set_continuous_play_active(True) - visualizer.update_play_policy_button_status(True) - - # Spawn the background worker - threading.Thread( - target=continuous_prediction_worker, - args=(data_manager, policy, policy_state, visualizer, input_embodiment_description, continuous_mode), - daemon=True - ).start() - else: - # Stop continuous play - print("⏹️ Stop Policy button pressed - Stopping continuous policy execution...") - policy_state.set_continuous_play_active(False) - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution stopped" - ) - print("✓ Policy execution stopped and robot enabled") - -def policy_execution_thread( - policy: nc.policy, - data_manager: DataManager, - policy_state: PolicyState, - robot_controller: PiperController, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> None: - """Policy execution thread.""" - dt_execution = 1.0 / POLICY_EXECUTION_RATE - - # Define colors for continuous horizon visualization - VISUALIZATION_COLORS = [ - (1.0, 0.65, 0.0, 0.25), # Orange (Default) - (0.0, 1.0, 0.0, 0.25), # Green - (1.0, 0.0, 0.0, 0.25), # Red - (0.0, 0.0, 1.0, 0.25), # Blue - ] - color_index = 0 - - # Throttle visualization updates to ~30Hz to avoid overwhelming Viser - last_visualization_update = 0.0 - visualization_update_interval = 1.0 / 30.0 # 30 Hz - while True: - start_time = time.time() - - if ( - data_manager.get_robot_activity_state() - == RobotActivityState.POLICY_CONTROLLED - ): - locked_horizon = policy_state.get_locked_prediction_horizon() - execution_index = policy_state.get_execution_action_index() - locked_horizon_length = policy_state.get_locked_prediction_horizon_length() - - # Debug output on first execution step - if execution_index == 0 and locked_horizon_length > 0: - print( - f"🔄 Policy execution started: {locked_horizon_length} actions, " - f"robot enabled: {robot_controller.is_robot_enabled()}" - ) - - # If continuous play is active, only execute up to the chunk limit - if execution_index < locked_horizon_length: - # Check if previous goal was achieved, if any - current_joint_angles = data_manager.get_current_joint_angles() - if ( - execution_index > 0 - and current_joint_angles is not None - and policy_state.get_execution_mode() - == PolicyState.ExecutionMode.TARGETING_POSE - ): - targeting_pose_start_time = time.time() - while ( - time.time() - targeting_pose_start_time - < TARGETING_POSE_TIME_THRESHOLD - ): - # Get previous action from horizon - if not all( - joint_name in locked_horizon for joint_name in JOINT_NAMES - ): - break - previous_joint_target_positions_rad = np.array( - [ - locked_horizon[joint_name][execution_index - 1] - for joint_name in JOINT_NAMES - ] - ) - previous_joint_target_positions_deg = np.degrees( - previous_joint_target_positions_rad - ) - joint_errors = np.abs( - current_joint_angles - previous_joint_target_positions_deg - ) - if np.any(joint_errors <= MAX_ACTION_ERROR_THRESHOLD): - break - time.sleep(0.001) - - # Send current action to robot (if available) - if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): - current_joint_target_positions_rad = np.array( - [ - locked_horizon[joint_name][execution_index] - for joint_name in JOINT_NAMES - ] - ) - current_joint_target_positions_deg = np.degrees( - current_joint_target_positions_rad - ) - # Update data_manager with target joint angles for visualization - data_manager.set_target_joint_angles( - current_joint_target_positions_deg - ) - - # Verify robot controller is enabled before sending commands - if robot_controller.is_robot_enabled(): - robot_controller.set_target_joint_angles( - current_joint_target_positions_deg - ) - else: - print( - f"⚠️ Robot controller not enabled, skipping command at index {execution_index}" - ) - - # Send current gripper open value to robot (if available) - if GRIPPER_NAME in locked_horizon: - current_gripper_target_open_value = locked_horizon[GRIPPER_NAME][ - execution_index - ] - robot_controller.set_gripper_open_value( - current_gripper_target_open_value - ) - - # Update execution index - policy_state.increment_execution_action_index() - - # Update status - visualizer.update_policy_status( - f"Executing policy: {execution_index + 1}/{locked_horizon_length}" - ) - else: - # Horizon buffer exhausted - if not policy_state.get_continuous_play_active(): - print("✓ Policy execution completed") - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution completed" - ) - else: - # Failsafe: If the background thread is running slightly late, - # hold the very last predicted position to maintain motor torque. - if all(joint_name in locked_horizon for joint_name in JOINT_NAMES): - last_index = locked_horizon_length - 1 - hold_positions_rad = np.array([ - locked_horizon[jn][last_index] for jn in JOINT_NAMES - ]) - if robot_controller.is_robot_enabled(): - robot_controller.set_target_joint_angles(np.degrees(hold_positions_rad)) - - # NOTE: Update visualization less frequently to avoid blocking - # Throttle visualization updates to ~30Hz to prevent overwhelming Viser server - current_time = time.time() - if current_time - last_visualization_update >= visualization_update_interval: - update_visualization(data_manager, policy_state, visualizer) - last_visualization_update = current_time - - dt_execution = 1.0 / visualizer.get_policy_execution_rate() - elapsed = time.time() - start_time - if elapsed < dt_execution: - time.sleep(dt_execution - elapsed) - - -def update_visualization( - data_manager: DataManager, - policy_state: PolicyState, - visualizer: RobotVisualizer, -) -> None: - """Update visualization.""" - # Update actual robot visualization - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - joint_config_rad = np.radians(current_joint_angles) - visualizer.update_robot_pose(joint_config_rad) - - # Update RGB camera image in Viser GUI (if available) - rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) - if rgb_image is not None: - visualizer.update_rgb_image(rgb_image) - - # Get policy state for ghost robot - prediction_horizon = policy_state.get_prediction_horizon() - prediction_horizon_length = policy_state.get_prediction_horizon_length() - ghost_robot_playing = policy_state.get_ghost_robot_playing() - ghost_action_index = policy_state.get_ghost_action_index() - - # Update ghost robot based on current state - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.POLICY_CONTROLLED: - # During policy execution, make ghost robot show target joint angles - visualizer.update_ghost_robot_visibility(True) - target_joint_angles = data_manager.get_target_joint_angles() - if target_joint_angles is not None: - joint_config_rad = np.radians(target_joint_angles) - visualizer.update_ghost_robot_pose(joint_config_rad) - # Disable buttons during execution - visualizer.set_run_policy_button_disabled(True) - # Play/Stop button is enabled during execution so we can stop if needed - visualizer.set_play_policy_button_disabled(False) - - elif ( - robot_activity_state == RobotActivityState.ENABLED - and data_manager.get_teleop_active() - ): - # During teleoperation, make ghost robot show target joint angles - visualizer.update_ghost_robot_visibility(True) - target_joint_angles = data_manager.get_target_joint_angles() - if target_joint_angles is not None: - joint_config_rad = np.radians(target_joint_angles) - visualizer.update_ghost_robot_pose(joint_config_rad) - - elif ghost_robot_playing and prediction_horizon_length > 0: - # Enable execute policy button - visualizer.set_start_policy_execution_button_disabled(False) - # show ghost robot - visualizer.update_ghost_robot_visibility(True) - # Update ghost robot with prediction horizon actions (preview mode) - if ghost_action_index < prediction_horizon_length: - # Get ghost action from horizon - if all(joint_name in prediction_horizon for joint_name in JOINT_NAMES): - ghost_joint_config = np.array( - [ - prediction_horizon[joint_name][ghost_action_index] - for joint_name in JOINT_NAMES - ] - ) - visualizer.update_ghost_robot_pose(ghost_joint_config) - next_index = (ghost_action_index + 1) % prediction_horizon_length - policy_state.set_ghost_action_index(next_index) - else: - policy_state.reset_ghost_action_index() - - else: - # When not playing, hide the ghost robot - visualizer.update_ghost_robot_visibility(False) - - # Update button state and policy status when not policy controlled - robot_enabled = robot_activity_state == RobotActivityState.ENABLED - has_horizon = prediction_horizon_length > 0 - - # Update button enabled state - visualizer.set_start_policy_execution_button_disabled( - not (robot_enabled and has_horizon) - ) - visualizer.set_run_policy_button_disabled(not robot_enabled) - visualizer.set_play_policy_button_disabled(not robot_enabled) - - # Update policy status - if not has_horizon: - visualizer.update_policy_status( - "Ready - Press Right Joystick or 'Run Policy' button to get prediction" - ) - elif not robot_enabled: - visualizer.update_policy_status("Robot not enabled") - else: - visualizer.update_policy_status( - f"Ready - {prediction_horizon_length} actions in horizon" - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description="Piper Robot Test with Neuracore Policy - REAL ROBOT CONTROL" - ) - parser.add_argument( - "--ip-address", - type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", - ) - parser.add_argument( - "--no-quest", - action="store_true", - help="Disable Meta Quest controller integration entirely.", - ) - policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument( - "--train-run-name", - type=str, - default=None, - help="Name of the training run to load policy from (for cloud training).", - ) - - parser.add_argument( - "--continuous-mode", - type=str, - choices=["pipeline", "sequential"], - default="sequential", - help="Execution mode for Play Policy: 'pipeline' (smooth hot-swapping) or 'sequential' (execute full horizon, then pause to predict next).", - ) - - policy_group.add_argument( - "--model-path", - type=str, - default=None, - help="Path to local model file to load policy from.", - ) - policy_group.add_argument( - "--remote-endpoint-name", - type=str, - default=None, - help="Name of remote Neuracore policy endpoint.", - ) - parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Neuracore robot name (policy embodiment resolution).", - ) - args = parser.parse_args() - - print("=" * 60) - print("PIPER ROBOT TEST WITH NEURACORE POLICY") - print("=" * 60) - print("Thread frequencies:") - print(f" 🎮 Quest Controller: {CONTROLLER_DATA_RATE} Hz") - print(f" 🧮 IK Solver: {IK_SOLVER_RATE} Hz") - print(f" 🤖 Robot Controller: {ROBOT_RATE} Hz") - print(f" 📸 Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz") - print(f" 📊 Joint State: {JOINT_STATE_STREAMING_RATE} Hz") - print(f" 🖥️ Visualization: {VISUALIZATION_RATE} Hz") - - # Connect to Neuracore - print("\n🔧 Initializing Neuracore...") - nc.login() - nc.connect_robot( - robot_name=args.robot_name, - urdf_path=str(URDF_PATH), - overwrite=False, - ) - - # Load policy (cross-embodiment + preprocessing; same pattern as example 6) - input_embodiment_description: EmbodimentDescription = { - DataType.JOINT_POSITIONS: { - 0: "joint1", - 1: "joint2", - 2: "joint3", - 3: "joint4", - 4: "joint5", - 5: "joint6", - }, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, - DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]}, - } - output_embodiment_description: EmbodimentDescription = { - DataType.JOINT_TARGET_POSITIONS: { - 0: "joint1", - 1: "joint2", - 2: "joint3", - 3: "joint4", - 4: "joint5", - 5: "joint6", - }, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME}, - } - input_preprocessing_config: PreprocessingConfiguration = { - DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], - } - - print("\n📋 Input embodiment description:") - for data_type, spec in input_embodiment_description.items(): - print(f" {data_type.name}: {_embodiment_names_ordered(spec)}") - print("\n📋 Output embodiment description:") - for data_type, spec in output_embodiment_description.items(): - print(f" {data_type.name}: {_embodiment_names_ordered(spec)}") - - if args.remote_endpoint_name is not None: - print( - f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." - ) - try: - policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: - print( - f"❌ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." - ) - sys.exit(1) - elif args.train_run_name is not None: - print(f"\n🤖 Loading policy from training run: {args.train_run_name}...") - policy = nc.policy( - train_run_name=args.train_run_name, - device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - ) - else: - print(f"\n🤖 Loading policy from model file: {args.model_path}...") - policy = nc.policy( - model_file=args.model_path, - device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - ) - print(" ✓ Policy loaded successfully") - - # Initialize policy state - policy_state = PolicyState() - policy_state.set_execution_mode(PolicyState.ExecutionMode.TARGETING_TIME) - - # Initialize shared state - data_manager = DataManager() - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - ) - - # Initialize robot controller - print("\n🤖 Initializing Piper robot controller...") - robot_controller = PiperController( - can_interface="can0", - robot_rate=ROBOT_RATE, - control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, - debug_mode=False, - ) - - # Start robot control loop - print("\n🚀 Starting robot control loop...") - robot_controller.start_control_loop() - - # Start joint state thread - print("\n📊 Starting joint state thread...") - joint_state_thread_obj = threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ) - joint_state_thread_obj.start() - - # Initialize and Start Meta Quest reader (Conditional) - quest_reader = None - quest_thread = None - if not args.no_quest: - print("\n🎮 Initializing Meta Quest reader...") - quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) - - print("\n🎮 Starting quest reader thread...") - quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ) - quest_thread.start() - else: - print("\n🎮 Meta Quest reader disabled via --no-quest flag.") - - # set initial configuration to current joint angles - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) - else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - - # Create Pink IK solver - print("\n🔧 Creating Pink IK solver...") - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - - # Start IK solver thread - print("\n🧮 Starting IK solver thread...") - ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ) - ik_thread.start() - - # Start camera thread - print("\n📷 Starting camera thread...") - camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ) - camera_thread_obj.start() - - # Set up visualization - print("\n🖥️ Starting Viser visualization...") - visualizer = RobotVisualizer(str(URDF_PATH)) - visualizer.add_policy_controls( - initial_prediction_ratio=PREDICTION_HORIZON_EXECUTION_RATIO, - initial_policy_rate=POLICY_EXECUTION_RATE, - initial_robot_rate=ROBOT_RATE, - initial_execution_mode=PolicyState.ExecutionMode.TARGETING_TIME.value, - ) - visualizer.add_toggle_robot_enabled_status_button() - visualizer.add_homing_controls() - visualizer.add_policy_buttons() - - # Set up button callbacks - visualizer.set_toggle_robot_enabled_status_callback( - lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer) - ) - visualizer.set_go_home_callback(lambda: home_robot(data_manager, robot_controller)) - visualizer.set_run_policy_callback( - lambda: run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) - ) - visualizer.set_start_policy_execution_callback( - lambda: start_policy_execution(data_manager, policy_state) - ) - - visualizer.set_play_policy_callback( - lambda: play_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - args.continuous_mode, - ) - ) - - # Set up execution mode dropdown callback to sync with PolicyState - visualizer.set_execution_mode_callback( - lambda: policy_state.set_execution_mode( - PolicyState.ExecutionMode(visualizer.get_execution_mode()) - ) - ) - - # Register Quest reader button callbacks conditionally - if not args.no_quest and quest_reader is not None: - quest_reader.on( - "button_a_pressed", - lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer), - ) - quest_reader.on( - "button_b_pressed", lambda: home_robot(data_manager, robot_controller) - ) - - # Start policy execution thread - print("\n🤖 Starting policy execution thread...") - policy_execution_thread_obj = threading.Thread( - target=policy_execution_thread, - args=( - policy, - data_manager, - policy_state, - robot_controller, - visualizer, - input_embodiment_description, - ), - daemon=True, - ) - policy_execution_thread_obj.start() - - print() - print("🚀 Starting teleoperation with policy testing...") - print("🎮 CONTROLS:") - if not args.no_quest: - print(" 1. Press BUTTON A or Enable Robot button to enable/disable robot") - print(" 2. You have same control over the robot as in teleoperation.") - print(" - Hold RIGHT GRIP to activate teleoperation") - print(" - Move controller - robot follows!") - print(" - Hold RIGHT TRIGGER to close gripper") - print(" - Press BUTTON A or Enable Robot button to enable/disable robot") - print(" - Press BUTTON B or Home Robot button to send robot home") - else: - print(" 1. Click Enable Robot button in Viser to enable/disable robot") - print(" 2. (Meta Quest controls disabled via --no-quest flag)") - print(" 3. Click 'Run Policy' (Preview) to generate and visualize a prediction horizon") - print(" 4. Click 'Execute Policy' to run the currently previewed horizon") - print(" 5. Click 'Play Policy' (Receding Horizon) to constantly predict and execute the first action") - print("⚠️ Press Ctrl+C to exit") - print() - print("🌐 Open browser: http://localhost:8080") - - try: - while True: - time.sleep(1) - - except KeyboardInterrupt: - print("\n👋 Interrupt received - shutting down gracefully...") - except Exception as e: - print(f"\n❌ Demo error: {e}") - traceback.print_exc() - - # Cleanup - print("\n🧹 Cleaning up...") - - # Disconnect policy - policy.disconnect() - - # shutdown threads - data_manager.request_shutdown() - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - - # Conditional cleanup for Quest - if quest_thread is not None: - quest_thread.join() - if quest_reader is not None: - quest_reader.stop() - - ik_thread.join() - camera_thread_obj.join() - robot_controller.cleanup() - - nc.logout() - - print("\n👋 Demo stopped.") - -============================================================ -FILE: common/foot_pedal.py -============================================================ - -"""Foot pedal abstraction with callback dispatch and key mapping.""" - -import os -import sys -import time -import traceback -from typing import Callable - -# add the parent directory to the path -sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from common.data_manager import DataManager - - -class FootPedal: - """Task-agnostic pedal/keyboard mapping with dynamic button support.""" - - def __init__(self, key_map: dict[str, str] | None = None) -> None: - """Initialize dynamic button mapping and callback registry.""" - source = key_map if key_map is not None else {} - self._key_map = {name: str(key).strip().lower() for name, key in source.items()} - self._callbacks: dict[str, Callable[[], None]] = {} - - @property - def key_map(self) -> dict[str, str]: - """Return a copy of the current button-to-key map.""" - return dict(self._key_map) - - def set_key_map(self, key_map: dict[str, str]) -> None: - """Set the key map for the foot pedal. Will override the current key map. - - Args: - key_map: The key map to set for the foot pedal. - """ - self._key_map = { - name: str(key).strip().lower() for name, key in key_map.items() - } - - def bind(self, button_name: str, callback: Callable[[], None] | None) -> None: - """Bind a callback to a button name. - - Args: - button_name: The name of the button to bind the callback to. - callback: The callback to bind to the button name. - - NOTE: if callback is None, the callback is removed from the button name. - """ - if callback is None: - self._callbacks.pop(button_name, None) - return - self._callbacks[button_name] = callback - - def get_bound_buttons(self) -> list[str]: - """Return button names that currently have callbacks bound.""" - return list(self._callbacks.keys()) - - def _dispatch(self, char: str) -> None: - """Fire mapped callback for a pressed key.""" - normalized = str(char).strip().lower() - for button_name, mapped_key in self._key_map.items(): - if normalized == mapped_key: - callback = self._callbacks.get(button_name) - if callback: - callback() - - def run_loop(self, data_manager: DataManager) -> None: - """Block and dispatch pedal events until shutdown is requested.""" - print("⌨️ Foot pedal listener started.") - - try: - from pynput import keyboard - - print("⌨️ Foot pedal listener (pynput) started.") - - def on_press(key: object) -> None: - try: - char = key.char if hasattr(key, "char") else str(key) - self._dispatch(char) - except Exception: - pass - - with keyboard.Listener(on_press=on_press) as listener: - while not data_manager.is_shutdown_requested(): - if not listener.is_alive(): - break - time.sleep(0.1) - listener.stop() - - except Exception as e: - print(f"✗ Fatal error in foot pedal: {e}") - traceback.print_exc() - finally: - print("⌨️ Foot pedal listener stopped.") - - -if __name__ == "__main__": - print("🚀 Standalone Foot Pedal Hardware Test") - print("--------------------------------------") - data_manager = DataManager() - pedal = FootPedal({"button_a": "a", "button_b": "b", "button_c": "c"}) - pedal.bind("button_a", lambda: print("✓ 🟡 Pedal A (ENABLE/DISABLE) detected")) - pedal.bind("button_b", lambda: print("✓ 🏠 Pedal B (HOME) detected")) - pedal.bind("button_c", lambda: print("✓ 🔴 Pedal C (RECORD) detected")) - - try: - pedal.run_loop(data_manager) - except KeyboardInterrupt: - print("\n👋 Test stopped.") - - -============================================================ -FILE: common/policy_state.py -============================================================ - -"""Policy state - policy prediction, policy action, policy action index.""" - -import threading -from enum import Enum - -import numpy as np - - -class PolicyState: - """Policy state - policy prediction, policy action, policy action index.""" - - class ExecutionMode(Enum): - """Execution mode enumeration.""" - - TARGETING_TIME = "targeting_time" - TARGETING_POSE = "targeting_pose" - - def __init__(self) -> None: - """Initialize PolicyState with default values.""" - # Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names - self._prediction_horizon: dict[str, list[float]] = {} - self._prediction_horizon_lock = threading.Lock() - self._execution_ratio: float = 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in - - self._policy_rgb_image_input: np.ndarray | None = None - self._policy_rgb_image_input_lock = threading.Lock() - - self._policy_state_input: np.ndarray | None = None - self._policy_state_input_lock = threading.Lock() - - self._ghost_robot_playing: bool = False - self._ghost_action_index: int = 0 - - # Policy execution state - self._policy_inputs_locked: bool = False - self._locked_prediction_horizon: dict[str, list[float]] = {} - self._execution_action_index: int = 0 - self._execution_lock = threading.Lock() - - # Continuous play and execution mode - self._continuous_play_active: bool = False - self._execution_mode: PolicyState.ExecutionMode = ( - PolicyState.ExecutionMode.TARGETING_TIME - ) - - def get_prediction_horizon_length(self) -> int: - """Get prediction horizon length (thread-safe).""" - with self._prediction_horizon_lock: - if not self._prediction_horizon: - return 0 - # Get length from first list (all should have same length) - first_key = next(iter(self._prediction_horizon.keys())) - return len(self._prediction_horizon[first_key]) - - def get_prediction_horizon(self) -> dict[str, list[float]]: - """Get prediction horizon (thread-safe).""" - with self._prediction_horizon_lock: - # Return a deep copy to prevent external modifications - return { - key: list(values) for key, values in self._prediction_horizon.items() - } - - def set_prediction_horizon(self, horizon: dict[str, list[float]]) -> None: - """Set prediction horizon (thread-safe).""" - with self._prediction_horizon_lock: - # Store a deep copy to prevent external modifications - self._prediction_horizon = { - key: list(values) for key, values in horizon.items() - } - - def set_execution_ratio(self, ratio: float) -> None: - """Set execution ratio used when locking prediction horizon.""" - # Clamp to (0, 1] to avoid zero-length horizons - clamped_ratio = float(np.clip(ratio, 1e-6, 1.0)) - with self._prediction_horizon_lock: - self._execution_ratio = clamped_ratio - - def get_execution_ratio(self) -> float: - """Get execution ratio (thread-safe).""" - with self._prediction_horizon_lock: - return self._execution_ratio - - def get_policy_rgb_image_input(self) -> np.ndarray | None: - """Get policy RGB image (thread-safe).""" - with self._policy_rgb_image_input_lock: - return ( - self._policy_rgb_image_input.copy() - if self._policy_rgb_image_input is not None - else None - ) - - def set_policy_rgb_image_input(self, image: np.ndarray) -> None: - """Set policy RGB image (thread-safe).""" - with self._policy_rgb_image_input_lock: - self._policy_rgb_image_input = image.copy() if image is not None else None - - def get_policy_state_input(self) -> np.ndarray | None: - """Get policy state input (thread-safe).""" - with self._policy_state_input_lock: - return ( - self._policy_state_input.copy() - if self._policy_state_input is not None - else None - ) - - def set_policy_state_input(self, input: np.ndarray) -> None: - """Set policy state input (thread-safe).""" - with self._policy_state_input_lock: - self._policy_state_input = input.copy() if input is not None else None - - def get_ghost_robot_playing(self) -> bool: - """Get ghost robot playing (thread-safe).""" - return self._ghost_robot_playing - - def set_ghost_robot_playing(self, playing: bool) -> None: - """Set ghost robot playing (thread-safe).""" - self._ghost_robot_playing = playing - - def get_ghost_action_index(self) -> int: - """Get ghost action index (thread-safe).""" - return self._ghost_action_index - - def set_ghost_action_index(self, index: int) -> None: - """Set ghost action index (thread-safe).""" - self._ghost_action_index = index - - def reset_ghost_action_index(self) -> None: - """Reset ghost action index (thread-safe).""" - self._ghost_action_index = 0 - - # Policy execution methods - def start_policy_execution(self) -> None: - """Start policy execution by locking inputs and storing horizon (thread-safe).""" - with self._prediction_horizon_lock: - source_horizon = { - key: list(values) for key, values in self._prediction_horizon.items() - } - # Calculate length directly from source_horizon instead of calling get_prediction_horizon_length() - # which would try to acquire the same lock again (deadlock!) - if not source_horizon: - total = 0 - else: - first_key = next(iter(source_horizon.keys())) - total = len(source_horizon[first_key]) - - if total == 0: - locked_horizon = {} - else: - num_actions = int(total * self._execution_ratio) - num_actions = max(1, min(num_actions, total)) - # Slice each list in the horizon - locked_horizon = { - key: values[:num_actions] for key, values in source_horizon.items() - } - with self._execution_lock: - self._policy_inputs_locked = True - self._execution_action_index = 0 - self._locked_prediction_horizon = locked_horizon - - def end_policy_execution(self) -> None: - """Stop policy execution and unlock inputs (thread-safe).""" - with self._execution_lock: - self._policy_inputs_locked = False - self._locked_prediction_horizon = {} - self._execution_action_index = 0 - - def get_locked_prediction_horizon(self) -> dict[str, list[float]]: - """Get locked prediction horizon (thread-safe).""" - with self._execution_lock: - # Return a deep copy to prevent external modifications - return { - key: list(values) - for key, values in self._locked_prediction_horizon.items() - } - - def get_locked_prediction_horizon_length(self) -> int: - """Get locked prediction horizon length (thread-safe).""" - with self._execution_lock: - if not self._locked_prediction_horizon: - return 0 - # Get length from first list (all should have same length) - first_key = next(iter(self._locked_prediction_horizon.keys())) - return len(self._locked_prediction_horizon[first_key]) - - def get_locked_prediction_horizon_sync_points(self) -> dict[str, list[float]]: - """Get locked prediction horizon (legacy method name, calls get_locked_prediction_horizon).""" - return self.get_locked_prediction_horizon() - - def get_execution_action_index(self) -> int: - """Get current execution action index (thread-safe).""" - with self._execution_lock: - return self._execution_action_index - - def increment_execution_action_index(self) -> None: - """Increment execution action index (thread-safe).""" - with self._execution_lock: - self._execution_action_index += 1 - - def get_continuous_play_active(self) -> bool: - """Get continuous play active state (thread-safe).""" - return self._continuous_play_active - - def set_continuous_play_active(self, active: bool) -> None: - """Set continuous play active state (thread-safe).""" - self._continuous_play_active = active - - def get_execution_mode(self) -> ExecutionMode: - """Get execution mode (thread-safe).""" - return self._execution_mode - - def set_execution_mode(self, mode: ExecutionMode) -> None: - """Set execution mode (thread-safe).""" - self._execution_mode = mode - - -============================================================ -FILE: common/__init__.py -============================================================ - -"""Common modules for robot demos. - -This package contains shared utilities for robot visualization and teleoperation state management. -""" - - -============================================================ -FILE: common/policy_helpers.py -============================================================ - -"""Shared helpers for AgileX Neuracore policy rollout examples.""" - -from __future__ import annotations - -from typing import Any - -import neuracore as nc -import numpy as np -from neuracore_types import ( - BatchedJointData, - BatchedNCData, - BatchedParallelGripperOpenAmountData, - DataType, - EmbodimentDescription, -) - -from .configs import GRIPPER_NAME, JOINT_NAMES - -DEFAULT_ROBOT_NAME = "AgileX PiPER" - - -def embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: - """Ordered channel names from an embodiment entry (list or index→name map).""" - if isinstance(spec, dict): - return [spec[i] for i in sorted(spec)] - return list(spec) - - -def get_policy_embodiments( - policy: Any, -) -> tuple[EmbodimentDescription, EmbodimentDescription | None]: - """Read input/output embodiment specs resolved on the loaded policy.""" - if hasattr(policy, "_policy"): - inner = policy._policy - return inner.input_embodiment_description, inner.output_embodiment_description - input_emb = getattr(policy, "input_embodiment_description", None) - if input_emb is None: - raise AttributeError( - "Could not read input_embodiment_description from policy; " - "use nc.policy(..., robot_name=...) without overriding embodiments." - ) - output_emb = getattr(policy, "output_embodiment_description", None) - return input_emb, output_emb - - -def print_policy_embodiments( - input_embodiment: EmbodimentDescription, - output_embodiment: EmbodimentDescription | None, -) -> None: - """Print resolved policy embodiment channels.""" - print("\n📋 Policy input embodiment (from model):") - for data_type, spec in input_embodiment.items(): - print(f" {data_type.name}: {embodiment_names_ordered(spec)}") - if output_embodiment is not None: - print("\n📋 Policy output embodiment (from model):") - for data_type, spec in output_embodiment.items(): - print(f" {data_type.name}: {embodiment_names_ordered(spec)}") - - -def log_robot_state_for_policy( - data_manager: Any, - input_embodiment_description: EmbodimentDescription, -) -> bool: - """Log only sensor streams the policy expects. Returns True if anything was logged.""" - logged_any = False - - if DataType.JOINT_POSITIONS in input_embodiment_description: - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - joint_angles_rad = np.radians(current_joint_angles) - positions_by_name = { - jn: float(ang) for jn, ang in zip(JOINT_NAMES, joint_angles_rad) - } - policy_joint_order = embodiment_names_ordered( - input_embodiment_description[DataType.JOINT_POSITIONS] - ) - nc.log_joint_positions( - {jn: positions_by_name[jn] for jn in policy_joint_order} - ) - logged_any = True - - if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: - gripper_open_value = data_manager.get_current_gripper_open_value() - if gripper_open_value is not None: - for gripper_name in embodiment_names_ordered( - input_embodiment_description[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] - ): - nc.log_parallel_gripper_open_amount(gripper_name, gripper_open_value) - logged_any = True - - if DataType.RGB_IMAGES in input_embodiment_description: - for camera_name in embodiment_names_ordered( - input_embodiment_description[DataType.RGB_IMAGES] - ): - rgb_image = data_manager.get_rgb_image(camera_name) - if rgb_image is not None: - nc.log_rgb(camera_name, rgb_image) - logged_any = True - - return logged_any - - -def log_sync_step_for_policy( - step: Any, - input_embodiment_description: EmbodimentDescription, -) -> bool: - """Log a synchronized dataset step using only channels the policy expects.""" - logged_any = False - - if DataType.JOINT_POSITIONS in input_embodiment_description: - joint_data = step.data.get(DataType.JOINT_POSITIONS, {}) - joint_positions_dict = { - joint_name: joint_data[joint_name].value - for joint_name in embodiment_names_ordered( - input_embodiment_description[DataType.JOINT_POSITIONS] - ) - if joint_name in joint_data - } - if joint_positions_dict: - nc.log_joint_positions(joint_positions_dict) - logged_any = True - - if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description: - gripper_data = step.data.get(DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, {}) - for gripper_name in embodiment_names_ordered( - input_embodiment_description[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] - ): - if gripper_name in gripper_data: - nc.log_parallel_gripper_open_amount( - gripper_name, gripper_data[gripper_name].open_amount - ) - logged_any = True - - if DataType.RGB_IMAGES in input_embodiment_description: - rgb_data = step.data.get(DataType.RGB_IMAGES, {}) - for camera_name in embodiment_names_ordered( - input_embodiment_description[DataType.RGB_IMAGES] - ): - if camera_name in rgb_data: - nc.log_rgb(camera_name, np.array(rgb_data[camera_name].frame)) - logged_any = True - - return logged_any - - -def convert_predictions_to_horizon( - predictions: dict[DataType, dict[str, BatchedNCData]], -) -> dict[str, list[float]]: - """Convert policy predictions to a per-channel horizon dict (model-driven keys).""" - horizon: dict[str, list[float]] = {} - - if DataType.JOINT_TARGET_POSITIONS in predictions: - for joint_name, batched in predictions[DataType.JOINT_TARGET_POSITIONS].items(): - if isinstance(batched, BatchedJointData): - horizon[joint_name] = batched.value[0, :, 0].cpu().numpy().tolist() - - if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: - for gripper_name, batched in predictions[ - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS - ].items(): - if isinstance(batched, BatchedParallelGripperOpenAmountData): - horizon[gripper_name] = ( - batched.open_amount[0, :, 0].cpu().numpy().tolist() - ) - - return horizon - - -def horizon_length(horizon: dict[str, list[float]]) -> int: - """Number of steps in a prediction horizon dict.""" - if not horizon: - return 0 - return len(next(iter(horizon.values()))) - - -def arm_joint_names_in_horizon(horizon: dict[str, list[float]]) -> list[str]: - """Body joint names present in a horizon (excludes gripper channels).""" - return [name for name in JOINT_NAMES if name in horizon] - - -def joint_targets_deg_at_index( - horizon: dict[str, list[float]], index: int -) -> np.ndarray | None: - """Arm joint targets in degrees at horizon index (Piper JOINT_NAMES order).""" - if not all(jn in horizon for jn in JOINT_NAMES): - return None - if any(index >= len(horizon[jn]) for jn in JOINT_NAMES): - return None - rad = np.array([horizon[jn][index] for jn in JOINT_NAMES], dtype=np.float64) - return np.degrees(rad) - - -def gripper_open_at_index( - horizon: dict[str, list[float]], - index: int, - gripper_names: list[str] | None = None, -) -> float | None: - """Gripper open amount in [0, 1] from the first matching horizon channel.""" - names = gripper_names or [GRIPPER_NAME] - for gripper_name in names: - if gripper_name in horizon and index < len(horizon[gripper_name]): - return float(np.clip(horizon[gripper_name][index], 0.0, 1.0)) - return None - - -def urdf_cfg_from_horizon( - horizon: dict[str, list[float]], index: int -) -> np.ndarray | None: - """Joint configuration in radians for Viser URDF (JOINT_NAMES order).""" - if not all(jn in horizon for jn in JOINT_NAMES): - return None - if any(index >= len(horizon[jn]) for jn in JOINT_NAMES): - return None - return np.array([float(horizon[jn][index]) for jn in JOINT_NAMES], dtype=np.float64) - - -============================================================ -FILE: common/data_manager.py -============================================================ - -#!/usr/bin/env python3 -"""Thread-safe teleoperation state management. - -This module provides shared state classes for teleoperation systems that need -to coordinate between multiple threads (data collection, IK solving, visualization). -""" -import threading -import time -import queue # Added for async queueing -from enum import Enum -from typing import Any, Callable - -import numpy as np - -from .configs import GRIPPER_NAME, JOINT_NAMES -from .one_euro_filter import OneEuroFilterTransform - - -class RobotActivityState(Enum): - """Robot activity state enumeration.""" - ENABLED = "ENABLED" - HOMING = "HOMING" - DISABLED = "DISABLED" - POLICY_CONTROLLED = "POLICY_CONTROLLED" - - -class ControllerState: - """Controller input state - Quest Reader writes, IK/Joint reads.""" - - def __init__(self) -> None: - """Initialize ControllerState with default values.""" - self._lock = threading.Lock() - - # 1€ Filter parameters - self.min_cutoff: float = 1.0 # Minimum cutoff frequency - self.beta: float = 0.0 # Speed coefficient - self.d_cutoff: float = 1.0 # Derivative cutoff frequency - - # Controller data - self.transform_raw: np.ndarray | None = None - self.transform: np.ndarray | None = None # Smoothed transform - self.grip_value: float = 0.0 - self.trigger_value: float = 0.0 - self._filter: OneEuroFilterTransform | None = None - - -class TeleopState: - """Teleop activation state - manages teleop start/stop.""" - - def __init__(self) -> None: - """Initialize TeleopState with default values.""" - self._lock = threading.Lock() - - self.active: bool = False - self.controller_initial_transform: np.ndarray | None = None - self.robot_initial_transform: np.ndarray | None = None - # Teleoperation scaling parameters (how much controller motion maps to robot motion) - self.translation_scale: float = 1.0 - self.rotation_scale: float = 1.0 - self.slow_scaling_mode_enabled: bool = False - - -class RobotState: - """Current robot state - joint angles, end effector pose, activity state.""" - - def __init__(self) -> None: - """Initialize RobotState with default values.""" - self._lock = threading.Lock() - - self.joint_angles: np.ndarray | None = None - self.joint_torques: np.ndarray | None = None - self.end_effector_pose: np.ndarray | None = None - self.current_gripper_open_value: float | None = None - self.target_gripper_open_value: float | None = None - self.activity_state: RobotActivityState = RobotActivityState.DISABLED - - -class IKState: - """IK solution state - target joint angles, pose, metrics.""" - - def __init__(self) -> None: - """Initialize IKState with default values.""" - self._lock = threading.Lock() - - self.target_joint_angles: np.ndarray | None = None - self.target_pose: np.ndarray | None = None - self.solve_time_ms: float = 0.0 - self.success: bool = True - - -class CameraState: - """Camera state - RGB images for one or more cameras.""" - - def __init__(self) -> None: - """Initialize CameraState with default values.""" - self._lock = threading.Lock() - - # Map from camera name -> latest RGB image - self.rgb_images: dict[str, np.ndarray] = {} - -class DataManager: - """Main state container coordinating all state groups. - - This class manages shared data between threads: - - Data collection thread: updates controller data - - IK solver thread: reads controller data, updates joint solutions - - Main thread: reads everything for visualization - - Uses separate locks for each state group to reduce contention. - """ - def __init__(self) -> None: - """Initialize DataManager with background callback processing.""" - self._controller_state = ControllerState() - self._teleop_state = TeleopState() - self._robot_state = RobotState() - self._ik_state = IKState() - self._camera_state = CameraState() - - # System state - self._shutdown_event = threading.Event() - - # Asynchronous processing elements - self._on_change_callback: ( - Callable[[str, dict[str, Any], float], None] | None - ) = None - - # Maxsize 60 matches ~1 second of video frames buffer if disk spikes - self._callback_queue: queue.Queue = queue.Queue(maxsize=60) - - self._worker_thread = threading.Thread( - target=self._callback_worker_loop, - name="NeuracoreCallbackWorker", - daemon=True - ) - self._worker_thread.start() - - def set_on_change_callback( - self, on_change_callback: Callable[[str, dict[str, Any], float], None] - ) -> None: - """Set on change callback (thread-safe).""" - self._on_change_callback = on_change_callback - - def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None: - """Helper to push payloads into the execution queue without blocking.""" - if self._on_change_callback is None: - return - - try: - # put_nowait drops data into the memory queue instantly (0.0ms blocking) - self._callback_queue.put_nowait((name, payload, timestamp)) - except queue.Full: - # Prevents out-of-memory if disk halts completely, without freezing telemetry loops - print(f"⚠️ Neuracore background queue full! Dropping log packet: {name}") - - def _callback_worker_loop(self) -> None: - """Background thread worker loop dedicated solely to performing slow disk IO updates.""" - while not self._shutdown_event.is_set() or not self._callback_queue.empty(): - try: - # Wait up to 100ms for a logging event - name, payload, timestamp = self._callback_queue.get(timeout=0.1) - - if self._on_change_callback is not None: - # Execute Neuracore disk operation safely here on a separate core - self._on_change_callback(name, payload, timestamp) - - self._callback_queue.task_done() - except queue.Empty: - continue - except Exception as e: - print(f"❌ Error in background logging callback: {e}") - - # ============================================================================ - # Camera State Methods - # ============================================================================ - - def get_rgb_image(self, camera_name: str) -> np.ndarray | None: - """Get RGB image for a specific camera (thread-safe).""" - with self._camera_state._lock: - if not self._camera_state.rgb_images: - return None - img = self._camera_state.rgb_images.get(camera_name) - return img.copy() if img is not None else None - - def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: - """Set RGB image for a specific camera (thread-safe and non-blocking).""" - with self._camera_state._lock: - self._camera_state.rgb_images[camera_name] = image.copy() - - if self._on_change_callback: - img_copy = self._camera_state.rgb_images[camera_name].copy() - # Queue it instead of executing directly! Camera loop returns immediately. - self._queue_callback("log_rgb", {camera_name: img_copy}, time.time()) - - # ============================================================================ - # Controller State Methods - # ============================================================================ - - def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: - with self._controller_state._lock: - return ( - ( - self._controller_state.transform.copy() - if self._controller_state.transform is not None - else None - ), - self._controller_state.grip_value, - self._controller_state.trigger_value, - ) - - def set_controller_data( - self, transform: np.ndarray | None, grip: float, trigger: float - ) -> None: - if transform is not None and transform.shape != (4, 4): - raise ValueError("Transform must be a 4x4 matrix") - if grip < 0.0 or grip > 1.0: - raise ValueError("Grip value must be between 0.0 and 1.0") - if trigger < 0.0 or trigger > 1.0: - raise ValueError("Trigger value must be between 0.0 and 1.0") - - with self._controller_state._lock: - self._controller_state.grip_value = grip - self._controller_state.trigger_value = trigger - - if transform is not None: - current_time = time.time() - self._controller_state.transform_raw = transform.copy() - - if self._controller_state._filter is None: - self._controller_state._filter = OneEuroFilterTransform( - current_time, - transform, - self._controller_state.min_cutoff, - self._controller_state.beta, - self._controller_state.d_cutoff, - ) - self._controller_state.transform = transform.copy() - else: - self._controller_state._filter.update_params( - self._controller_state.min_cutoff, - self._controller_state.beta, - self._controller_state.d_cutoff, - ) - self._controller_state.transform = self._controller_state._filter( - current_time, transform - ) - else: - self._controller_state.transform = None - self._controller_state.transform_raw = None - self._controller_state._filter = None - - def set_controller_filter_params( - self, min_cutoff: float, beta: float, d_cutoff: float - ) -> None: - with self._controller_state._lock: - self._controller_state.min_cutoff = min_cutoff - self._controller_state.beta = beta - self._controller_state.d_cutoff = d_cutoff - - def get_controller_filter_params(self) -> tuple[float, float, float]: - with self._controller_state._lock: - return ( - self._controller_state.min_cutoff, - self._controller_state.beta, - self._controller_state.d_cutoff, - ) - - # ============================================================================ - # Teleop State Methods - # ============================================================================ - - def set_teleop_state( - self, - active: bool, - controller_initial: np.ndarray | None, - robot_initial: np.ndarray | None, - ) -> None: - with self._teleop_state._lock: - self._teleop_state.active = active - self._teleop_state.controller_initial_transform = ( - controller_initial.copy() if controller_initial is not None else None - ) - self._teleop_state.robot_initial_transform = ( - robot_initial.copy() if robot_initial is not None else None - ) - - def set_teleop_scaling( - self, translation_scale: float, rotation_scale: float - ) -> None: - if translation_scale <= 0.0 or rotation_scale <= 0.0: - return - with self._teleop_state._lock: - self._teleop_state.translation_scale = translation_scale - self._teleop_state.rotation_scale = rotation_scale - - def get_teleop_scaling(self) -> tuple[float, float]: - with self._teleop_state._lock: - return ( - self._teleop_state.translation_scale, - self._teleop_state.rotation_scale, - ) - - def get_teleop_active(self) -> bool: - with self._teleop_state._lock: - return self._teleop_state.active - - def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: - with self._teleop_state._lock: - self._teleop_state.slow_scaling_mode_enabled = enabled - - def toggle_slow_scaling_mode_enabled(self) -> bool: - with self._teleop_state._lock: - self._teleop_state.slow_scaling_mode_enabled = ( - not self._teleop_state.slow_scaling_mode_enabled - ) - return self._teleop_state.slow_scaling_mode_enabled - - def get_slow_scaling_mode_enabled(self) -> bool: - with self._teleop_state._lock: - return self._teleop_state.slow_scaling_mode_enabled - - def get_initial_robot_controller_transforms( - self, - ) -> tuple[np.ndarray | None, np.ndarray | None]: - with self._teleop_state._lock: - return ( - ( - self._teleop_state.controller_initial_transform.copy() - if self._teleop_state.controller_initial_transform is not None - else None - ), - ( - self._teleop_state.robot_initial_transform.copy() - if self._teleop_state.robot_initial_transform is not None - else None - ), - ) - - # ============================================================================ - # Robot State Methods - # ============================================================================ - - def get_robot_activity_state(self) -> RobotActivityState: - with self._robot_state._lock: - return self._robot_state.activity_state - - def set_robot_activity_state(self, state: RobotActivityState) -> None: - with self._robot_state._lock: - self._robot_state.activity_state = state - - def get_current_joint_angles(self) -> np.ndarray | None: - with self._robot_state._lock: - return ( - self._robot_state.joint_angles.copy() - if self._robot_state.joint_angles is not None - else None - ) - - def set_current_joint_angles(self, angles: np.ndarray) -> None: - with self._robot_state._lock: - self._robot_state.joint_angles = angles.copy() - if self._on_change_callback: - angles = self._robot_state.joint_angles - if angles is not None: - payload = { - jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) - } - self._queue_callback("log_joint_positions", payload, time.time()) - - def get_current_joint_torques(self) -> np.ndarray | None: - with self._robot_state._lock: - return ( - self._robot_state.joint_torques.copy() - if self._robot_state.joint_torques is not None - else None - ) - - def set_current_joint_torques(self, torques: np.ndarray) -> None: - with self._robot_state._lock: - self._robot_state.joint_torques = torques.copy() - if self._on_change_callback: - torques = self._robot_state.joint_torques - if torques is not None: - payload = {jn: float(torques[i]) for i, jn in enumerate(JOINT_NAMES)} - self._queue_callback("log_joint_torques", payload, time.time()) - - def get_current_end_effector_pose(self) -> np.ndarray | None: - with self._robot_state._lock: - return ( - self._robot_state.end_effector_pose.copy() - if self._robot_state.end_effector_pose is not None - else None - ) - - def set_current_end_effector_pose(self, pose: np.ndarray) -> None: - with self._robot_state._lock: - self._robot_state.end_effector_pose = pose.copy() - - def get_current_gripper_open_value(self) -> float | None: - with self._robot_state._lock: - return self._robot_state.current_gripper_open_value - - def set_current_gripper_open_value(self, value: float) -> None: - with self._robot_state._lock: - self._robot_state.current_gripper_open_value = value - if self._on_change_callback: - self._queue_callback( - "log_parallel_gripper_open_amounts", - {GRIPPER_NAME: value}, - time.time(), - ) - - def get_target_gripper_open_value(self) -> float | None: - with self._robot_state._lock: - return self._robot_state.target_gripper_open_value - - def set_target_gripper_open_value(self, value: float) -> None: - with self._robot_state._lock: - self._robot_state.target_gripper_open_value = value - if self._on_change_callback: - self._queue_callback( - "log_parallel_gripper_target_open_amounts", - {GRIPPER_NAME: self._robot_state.target_gripper_open_value}, - time.time(), - ) - - # ============================================================================ - # IK State Methods - # ============================================================================ - - def get_target_joint_angles(self) -> np.ndarray | None: - with self._ik_state._lock: - return ( - self._ik_state.target_joint_angles.copy() - if self._ik_state.target_joint_angles is not None - else None - ) - - def set_target_joint_angles(self, angles: np.ndarray) -> None: - with self._ik_state._lock: - self._ik_state.target_joint_angles = angles.copy() - if self._on_change_callback: - angles = self._ik_state.target_joint_angles - if angles is not None: - payload = { - jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) - } - self._queue_callback( - "log_joint_target_positions", payload, time.time() - ) - - def set_target_pose(self, transform: np.ndarray | None) -> None: - with self._ik_state._lock: - self._ik_state.target_pose = ( - transform.copy() if transform is not None else None - ) - - def get_target_pose(self) -> np.ndarray | None: - with self._ik_state._lock: - return ( - self._ik_state.target_pose.copy() - if self._ik_state.target_pose is not None - else None - ) - - def set_ik_solve_time_ms(self, time_ms: float) -> None: - with self._ik_state._lock: - self._ik_state.solve_time_ms = time_ms - - def set_ik_success(self, success: bool) -> None: - with self._ik_state._lock: - self._ik_state.success = success - - def get_ik_solve_time_ms(self) -> float: - with self._ik_state._lock: - return self._ik_state.solve_time_ms - - def get_ik_success(self) -> bool: - with self._ik_state._lock: - return self._ik_state.success - - # ============================================================================ - # System State Methods - # ============================================================================ - - def request_shutdown(self) -> None: - """Request shutdown of all threads.""" - self._shutdown_event.set() - - def is_shutdown_requested(self) -> bool: - """Check if shutdown is requested.""" - return self._shutdown_event.is_set() - -============================================================ -FILE: common/utils.py -============================================================ - -"""Utility functions for the examples.""" - -import numpy as np -from scipy.spatial.transform import Rotation - - -def scale_and_add_delta_transform( - delta_position: np.ndarray, - delta_orientation: np.ndarray, - translation_scale: float, - rotation_scale: float, - initial_transform: np.ndarray, -) -> np.ndarray: - """Scale and add delta position and orientation to an initial transform. - - Args: - delta_position: 3D position vector - delta_orientation: 3x3 rotation matrix - translation_scale: translation scale - rotation_scale: rotation scale - initial_transform: 4x4 transformation matrix - - Returns: - 4x4 transformation matrix - """ - scaled_delta_position = delta_position * translation_scale - - # Scale the delta rotation by converting to axis-angle, scaling the angle, then converting back - delta_rotation = Rotation.from_matrix(delta_orientation) - delta_axis_angle = delta_rotation.as_rotvec() - scaled_delta_axis_angle = delta_axis_angle * rotation_scale - scaled_delta_rotation = Rotation.from_rotvec(scaled_delta_axis_angle).as_matrix() - - # Compose rotations by matrix multiplication (correct way to combine rotations) - initial_rotation = initial_transform[:3, :3] - new_rotation = initial_rotation @ scaled_delta_rotation - - target_transform = np.eye(4) - target_transform[:3, 3] = initial_transform[:3, 3] + scaled_delta_position - target_transform[:3, :3] = new_rotation - - return target_transform - - -============================================================ -FILE: common/one_euro_filter.py -============================================================ - -"""1€ Filter (One Euro Filter) for adaptive low-pass filtering.""" - -import numpy as np -from scipy.spatial.transform import Rotation - - -class OneEuroFilter: - """1€ Filter (One Euro Filter) for adaptive low-pass filtering. - - The 1€ Filter adapts its smoothing based on signal velocity: - - Fast motion: Low smoothing (low latency, responsive) - - Slow motion: High smoothing (high stability, reduces jitter) - """ - - def __init__( - self, - t0: float, - x0: float, - min_cutoff: float = 1.0, - beta: float = 0.0, - d_cutoff: float = 1.0, - ): - """Initialize 1€ Filter. - - Args: - t0: Initial timestamp (in seconds) - x0: Initial value - min_cutoff: Minimum cutoff frequency (stabilizes when holding still) - Higher = less lag, more jitter - beta: Speed coefficient (reduces lag when moving) - Higher = less lag, but more jitter during motion - d_cutoff: Cutoff frequency for derivative (speed) filtering - """ - self.min_cutoff = min_cutoff - self.beta = beta - self.d_cutoff = d_cutoff - self.x_prev = x0 - self.dx_prev = 0.0 - self.t_prev = t0 - - def smoothing_factor(self, t_e: float, cutoff: float) -> float: - """Calculate smoothing factor for exponential smoothing. - - Args: - t_e: Time elapsed since last update (in seconds) - cutoff: Cutoff frequency - - Returns: - Smoothing factor (0.0-1.0) - """ - r = 2 * np.pi * cutoff * t_e - return r / (r + 1) - - def exponential_smoothing(self, a: float, x: float, x_prev: float) -> float: - """Apply exponential smoothing. - - Args: - a: Smoothing factor (0.0-1.0) - x: Current value - x_prev: Previous smoothed value - - Returns: - Smoothed value - """ - return a * x + (1 - a) * x_prev - - def __call__(self, timestamp: float, x: float) -> float: - """Filter a new value. - - Args: - t: Current timestamp (in seconds) - x: Current noisy value - - Returns: - Filtered value - """ - t_e = timestamp - self.t_prev - - # Avoid division by zero in rare cases - if t_e <= 0.0: - return self.x_prev - - # Calculate the derivative (speed) of the signal - dx = (x - self.x_prev) / t_e - dx_hat = self.exponential_smoothing( - self.smoothing_factor(t_e, self.d_cutoff), dx, self.dx_prev - ) - - # Calculate the adaptive cutoff frequency - # This is the magic part: Cutoff increases with speed - cutoff = self.min_cutoff + self.beta * abs(dx_hat) - - # Filter the signal using the adaptive cutoff - x_hat = self.exponential_smoothing( - self.smoothing_factor(t_e, cutoff), x, self.x_prev - ) - - self.x_prev = x_hat - self.dx_prev = dx_hat - self.t_prev = timestamp - - return x_hat - - def update_params(self, min_cutoff: float, beta: float, d_cutoff: float) -> None: - """Update filter parameters without resetting state. - - Args: - min_cutoff: Minimum cutoff frequency - beta: Speed coefficient - d_cutoff: Cutoff frequency for derivative filtering - """ - self.min_cutoff = min_cutoff - self.beta = beta - self.d_cutoff = d_cutoff - - -class OneEuroFilterTransform: - """1€ Filter for 4x4 transformation matrices. - - Applies separate 1€ Filters to position (3D) and orientation (quaternion). - """ - - def __init__( - self, - t0: float, - transform: np.ndarray, - min_cutoff: float = 1.0, - beta: float = 0.0, - d_cutoff: float = 1.0, - ): - """Initialize 1€ Filter for transforms. - - Args: - t0: Initial timestamp (in seconds) - transform: Initial 4x4 transformation matrix - min_cutoff: Minimum cutoff frequency - beta: Speed coefficient - d_cutoff: Cutoff frequency for derivative filtering - """ - # Extract position and orientation - position = transform[:3, 3] - rotation = Rotation.from_matrix(transform[:3, :3]) - quat = rotation.as_quat() # [x, y, z, w] - - # Create filters for position (3 components) - self.position_filters = [ - OneEuroFilter(t0, pos, min_cutoff, beta, d_cutoff) for pos in position - ] - - # Create filters for quaternion (4 components) - self.quat_filters = [ - OneEuroFilter(t0, q, min_cutoff, beta, d_cutoff) for q in quat - ] - - # Store previous quaternion for sign correction - self.quat_prev = quat.copy() - - self.t_prev = t0 - - def __call__(self, timestamp: float, transform: np.ndarray) -> np.ndarray: - """Filter a new transform. - - Args: - t: Current timestamp (in seconds) - transform: Current 4x4 transformation matrix - - Returns: - Filtered 4x4 transformation matrix - """ - # Extract position and orientation - position = transform[:3, 3] - rotation = Rotation.from_matrix(transform[:3, :3]) - quat = rotation.as_quat() # [x, y, z, w] - - # Fix quaternion sign to avoid sudden flips (q and -q represent same rotation) - # Choose the quaternion closest to the previous one - if np.dot(quat, self.quat_prev) < 0: - quat = -quat - - # Filter position components - filtered_position = np.array( - [ - filter(timestamp, pos) - for filter, pos in zip(self.position_filters, position) - ] - ) - - # Filter quaternion components - filtered_quat = np.array( - [filter(timestamp, q) for filter, q in zip(self.quat_filters, quat)] - ) - - # Normalize quaternion (important!) - filtered_quat = filtered_quat / np.linalg.norm(filtered_quat) - - # Convert back to rotation matrix - filtered_rotation = Rotation.from_quat(filtered_quat) - - # Build filtered transform - filtered_transform = np.eye(4) - filtered_transform[:3, 3] = filtered_position - filtered_transform[:3, :3] = filtered_rotation.as_matrix() - - self.quat_prev = filtered_quat.copy() - self.t_prev = timestamp - - return filtered_transform - - def update_params(self, min_cutoff: float, beta: float, d_cutoff: float) -> None: - """Update filter parameters for all sub-filters without resetting state. - - Args: - min_cutoff: Minimum cutoff frequency - beta: Speed coefficient - d_cutoff: Cutoff frequency for derivative filtering - """ - self.min_cutoff = min_cutoff - self.beta = beta - self.d_cutoff = d_cutoff - for filter in self.position_filters + self.quat_filters: - filter.update_params(min_cutoff, beta, d_cutoff) - - -============================================================ -FILE: common/configs.py -============================================================ - -"""Configuration parameters for Piper robot demos.""" - -from pathlib import Path - -URDF_PATH = str( - Path(__file__).parent.parent.parent - / "piper_description" - / "urdf" - / "piper_description.urdf" -) - -GRIPPER_FRAME_NAME = "gripper_center" - - -# Pink IK parameters -SOLVER_NAME = "quadprog" -POSITION_COST = 1.0 -ORIENTATION_COST = 0.75 -FRAME_TASK_GAIN = 0.4 -LM_DAMPING = 0.01 #0.0 -DAMPING_COST = 0.25 -SOLVER_DAMPING_VALUE = 1e-4 #1e-12 - -# Controller 1€ Filter parameters -CONTROLLER_MIN_CUTOFF = 0.8 # Minimum cutoff frequency (stabilizes when holding still) -CONTROLLER_BETA = 0.05 #5.0 # Speed coefficient (reduces lag when moving) -CONTROLLER_D_CUTOFF = 0.9 # Derivative cutoff frequency - -# Controller parameters -GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control - -# Scaling factors for translation and rotation -TRANSLATION_SCALE = 1.5 -ROTATION_SCALE = 1.2 -SLOW_TRANSLATION_SCALE = 0.6 -SLOW_ROTATION_SCALE = 0.6 -WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 - -# Thread rates (Hz) -CONTROLLER_DATA_RATE = 50.0 # Controller input reading -IK_SOLVER_RATE = 100 #250.0 # IK solving and robot commands -VISUALIZATION_RATE = 60.0 # GUI updates -ROBOT_RATE = 100.0 - -# Neuracore data collection rates -JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore -CAMERA_FRAME_STREAMING_RATE = 30.0 # Data collection rate for camera frame - -# Meta quest axis mask -META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] # [x, y, z, roll, pitch, yaw] - -# USB webcam (OpenCV) configuration -CAMERA_DEVICE_INDEX = 5 # 0 = first camera, 1 = second, etc. -CAMERA_WIDTH = 640 -CAMERA_HEIGHT = 480 - -# # Initial neutral pose for robot (mm and degrees) -# NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] -# NEUTRAL_JOINT_ANGLES = [0.4, 112.3, -101.0, -2.6, 63.2, 18.3] -NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick taskpruthvi) -NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] # halid -# Joint Angles (rad): J1: -0.028 rad (-1.6°) J2: 0.910 rad (52.2°) J3: -0.948 rad (-54.3°) J4: -0.057 rad (-3.2°) J5: 0.752 rad (43.1°) J6: 0.083 rad (4.7°) - -NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] - -# Posture task cost vector (one weight per joint) -POSTURE_COST_VECTOR = [0.0, 0.0, 0.0, 0.05, 0.0, 0.0] - - -POLICY_EXECUTION_RATE = 20.0 # Hz -PREDICTION_HORIZON_EXECUTION_RATIO = ( - 1.0 # percentage of the prediction horizon that is executed -) -MAX_SAFETY_THRESHOLD = 200.0 # degrees -MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees -TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds - -GRIPPER_NAME = "gripper" -GRIPPER_LOGGING_NAME = GRIPPER_NAME - -# Camera logging names for Neuracore (scene + wrist) -CAMERA_NAMES = ["rgb_scene", "rgb_wrist"] - -JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] - - -============================================================ -FILE: common/robot_visualizer.py -============================================================ - -#!/usr/bin/env python3 -"""Shared robot visualizer for Piper robot demos. - -This module provides a clean interface for visualizing robot state using Viser, -encapsulating all the repeated setup, GUI controls, and update logic. -""" - -from typing import Any, Callable - -import numpy as np -import viser -import yourdfpy -from scipy.spatial.transform import Rotation -from viser.extras import ViserUrdf - - -class RobotVisualizer: - """Shared visualizer for robot demos. - - Encapsulates viser server setup, GUI controls, and update logic. - """ - - def __init__(self, urdf_path: str) -> None: - """Initialize the visualizer. - - Args: - urdf_path: Path to URDF file for robot visualization - """ - # Initialize viser server - self._server = viser.ViserServer() - self._server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) - - # Load URDF for visualization - urdf = yourdfpy.URDF.load(urdf_path) - self._urdf_vis = ViserUrdf(self._server, urdf, root_node_name="/robot_actual") - - ghost_urdf = yourdfpy.URDF.load(urdf_path) - # Ghost robot with semi-transparent blue color to make it visually distinct - self._ghost_robot_urdf = ViserUrdf( - self._server, - ghost_urdf, - root_node_name="/robot_ghost", - mesh_color_override=(1.0, 0.65, 0.0, 0.25), # Orange with 25% opacity - ) - - # GUI handles (initialized as None, created on demand) - all private - self._timing_handle = None - self._joint_angles_handle = None - self._robot_status_handle = None - self._teleop_status_handle = None - self._controller_status_handle = None - self._gripper_status_handle = None - - # Pink parameter handles - self._position_weight_handle = None - self._orientation_weight_handle = None - self._frame_task_gain_handle = None - self._lm_damping_handle = None - self._damping_weight_handle = None - self._solver_damping_value_handle = None - self._posture_cost_handles: list[Any] = [] - - # Robot control handles - self._enable_robot_handle = None - self._disable_robot_handle = None - self._emergency_stop_handle = None - self._go_home_button = None - self._toggle_robot_enabled_status_button = None - - # Teleop-specific handles - self._grip_value_handle = None - self._trigger_value_handle = None - - # Rate control handles - self._controller_min_cutoff_handle = None - self._controller_beta_handle = None - self._controller_d_cutoff_handle = None - - self._translation_scale_handle = None - self._rotation_scale_handle = None - - # Visualization handles - self._controller_handle = None - self._target_frame_handle = None - self._rgb_image_handle = None - - # Policy-related handles - self._policy_status_handle = None - self._prediction_ratio_handle = None - self._policy_execution_rate_handle = None - self._robot_rate_handle = None - self._execution_mode_dropdown = None - self._run_policy_button = None - self._start_policy_execution_button = None - self._play_policy_button = None - - # Internal state - self._ema_timing = 0.001 - - def add_basic_controls(self) -> None: - """Add basic GUI controls (timing, joint angles).""" - self._timing_handle = self._server.gui.add_number( - "IK Solve Time (ms)", 0.001, disabled=True - ) - self._joint_angles_handle = self._server.gui.add_text( - "Joint Angles", "Waiting for IK solution..." - ) - - def add_robot_status_controls(self) -> None: - """Add robot status display controls.""" - self._robot_status_handle = self._server.gui.add_text( - "Robot Status", "Initializing..." - ) - - def add_teleop_controls(self) -> None: - """Add teleoperation-specific controls.""" - self._grip_value_handle = self._server.gui.add_number( - "Grip Value", 0.0, disabled=True - ) - self._trigger_value_handle = self._server.gui.add_number( - "Trigger Value (Gripper)", 0.0, disabled=True - ) - self._teleop_status_handle = self._server.gui.add_text( - "Teleop Status", "Inactive" - ) - self._controller_status_handle = self._server.gui.add_text( - "Controller Status", "Waiting..." - ) - - def add_gripper_status_controls(self) -> None: - """Add gripper status display controls.""" - self._gripper_status_handle = self._server.gui.add_text( - "Gripper Status", "Open (0%)" - ) - - def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None: - """Add an RGB image placeholder in the desired GUI position. - - This reserves a fixed location for the camera feed; the actual image - data will be injected later via update_rgb_image. - """ - if self._rgb_image_handle is not None: - return - - dummy_image = np.zeros((height, width, 3), dtype=np.uint8) - self._rgb_image_handle = self._server.gui.add_image( - dummy_image, - label="RGB Camera", - format="jpeg", - jpeg_quality=85, - ) - - def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: - """Show or update RGB camera image in the Viser GUI.""" - if rgb_image is None: - return - if self._rgb_image_handle is None: - # Fallback: if placeholder wasn't created, create it now - self.add_rgb_image_placeholder( - height=rgb_image.shape[0], width=rgb_image.shape[1] - ) - rgb_handle = self._rgb_image_handle - if rgb_handle is None: - return - rgb_handle.image = rgb_image - - def add_homing_controls(self) -> None: - """Add homing controls.""" - self._go_home_button = self._server.gui.add_button("Go Home") - - def add_robot_control_buttons(self) -> None: - """Add robot control buttons (enable, disable, emergency stop). - - Note: For homing functionality, use add_homing_controls() instead. - """ - self._enable_robot_handle = self._server.gui.add_button("Enable Robot") - self._disable_robot_handle = self._server.gui.add_button("Disable Robot") - self._emergency_stop_handle = self._server.gui.add_button("Emergency Stop") - - def add_toggle_robot_enabled_status_button(self) -> None: - """Add toggle robot enabled status button. - - This creates a single button that toggles between "Enable Robot" and "Disable Robot" - based on the current robot state. - """ - self._toggle_robot_enabled_status_button = self._server.gui.add_button( - "Enable Robot" - ) - - def update_toggle_robot_enabled_status(self, enabled: bool) -> None: - """Update toggle robot enabled status button label based on robot state. - - Args: - enabled: Whether robot is currently enabled - """ - if self._toggle_robot_enabled_status_button is not None: - self._toggle_robot_enabled_status_button.label = ( - "Enable Robot" if not enabled else "Disable Robot" - ) - - def set_toggle_robot_enabled_status_callback( - self, callback: Callable[[], Any] - ) -> None: - """Set callback for toggle robot enabled status button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._toggle_robot_enabled_status_button is not None: - self._toggle_robot_enabled_status_button.on_click(lambda _: callback()) - - def add_pink_parameter_controls( - self, - position_cost: float, - orientation_cost: float, - frame_task_gain: float, - lm_damping: float, - damping_cost: float, - solver_damping_value: float, - posture_cost_vector: list[float], - ) -> None: - """Add Pink IK parameter controls. - - Args: - position_cost: Initial position cost value - orientation_cost: Initial orientation cost value - frame_task_gain: Initial frame task gain value - lm_damping: Initial LM damping value - damping_cost: Initial damping cost value - solver_damping_value: Initial solver damping value - posture_cost_vector: Initial posture cost vector (one value per joint) - """ - self._position_weight_handle = self._server.gui.add_number( - "Position Weight", position_cost, min=0.0, max=10.0, step=0.1 - ) - self._orientation_weight_handle = self._server.gui.add_number( - "Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01 - ) - self._frame_task_gain_handle = self._server.gui.add_number( - "Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1 - ) - self._lm_damping_handle = self._server.gui.add_number( - "LM Damping", lm_damping, min=0.0, max=5.0, step=0.01 - ) - self._damping_weight_handle = self._server.gui.add_number( - "Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01 - ) - self._solver_damping_value_handle = self._server.gui.add_number( - "Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001 - ) - - # Posture cost controls (one per joint) - self._posture_cost_handles = [] - for i in range(len(posture_cost_vector)): - handle = self._server.gui.add_number( - f"Posture Cost J{i+1}", - posture_cost_vector[i], - min=0.0, - max=1.0, - step=0.01, - ) - self._posture_cost_handles.append(handle) - - def add_controller_filter_controls( - self, - initial_min_cutoff: float, - initial_beta: float, - initial_d_cutoff: float, - ) -> None: - """Add 1€ Filter parameter controls for controller. - - Args: - initial_min_cutoff: Initial minimum cutoff frequency - initial_beta: Initial speed coefficient - initial_d_cutoff: Initial derivative cutoff frequency - """ - self._controller_min_cutoff_handle = self._server.gui.add_number( - "Controller Min Cutoff", - initial_min_cutoff, - min=0.01, - max=10.0, - step=0.01, - ) - self._controller_beta_handle = self._server.gui.add_number( - "Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01 - ) - self._controller_d_cutoff_handle = self._server.gui.add_number( - "Controller D Cutoff", - initial_d_cutoff, - min=0.01, - max=10.0, - step=0.01, - ) - - def add_scaling_controls( - self, initial_translation_scale: float, initial_rotation_scale: float - ) -> None: - """Add scaling factor controls for translation and rotation. - - Args: - initial_translation_scale: Initial translation scale factor - initial_rotation_scale: Initial rotation scale factor - """ - self._translation_scale_handle = self._server.gui.add_number( - "Translation Scale", - initial_translation_scale, - min=0.1, - max=10.0, - step=0.001, - ) - self._rotation_scale_handle = self._server.gui.add_number( - "Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001 - ) - - def add_controller_visualization(self) -> None: - """Add controller transform visualization.""" - self._controller_handle = self._server.scene.add_transform_controls( - "/controller", - scale=0.15, - position=(0, 0, 0), - wxyz=(1, 0, 0, 0), - ) - - def add_target_frame_visualization(self) -> None: - """Add target/goal frame visualization.""" - self._target_frame_handle = self._server.scene.add_frame( - "/target_goal", axes_length=0.1, axes_radius=0.003 - ) - - def update_robot_pose(self, joint_config: np.ndarray) -> None: - """Update robot visualization from joint configuration. - - Args: - joint_config: Joint angles in radians - """ - self._urdf_vis.update_cfg(joint_config) - - def update_joint_angles_display( - self, joint_config: np.ndarray, show_gripper: bool = False - ) -> None: - """Update joint angles display. - - Args: - joint_config: Joint angles in radians - show_gripper: Whether to show gripper joints (joints 7&8) - """ - if self._joint_angles_handle is None: - return - - joint_angles_str = "Joint Angles (rad):\n" - joint_angles_deg = np.degrees(joint_config) - num_joints = len(joint_config) if show_gripper else 6 - - for i in range(num_joints): - angle_rad = joint_config[i] - angle_deg = joint_angles_deg[i] - joint_type = "Robot" if i < 6 else "Gripper" - label = f"Joint {i+1} ({joint_type})" if show_gripper else f"J{i+1}" - joint_angles_str += f" {label}: {angle_rad:.3f} rad ({angle_deg:.1f}°)\n" - - self._joint_angles_handle.value = joint_angles_str - - def update_timing(self, solve_time_ms: float) -> None: - """Update timing display with exponential moving average. - - Args: - solve_time_ms: IK solve time in milliseconds - """ - if self._timing_handle is None: - return - - self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms - self._timing_handle.value = self._ema_timing - - def update_robot_status(self, status: str) -> None: - """Update robot status display. - - Args: - status: Status string to display - """ - if self._robot_status_handle is not None: - self._robot_status_handle.value = status - - def update_teleop_status(self, active: bool) -> None: - """Update teleop status display. - - Args: - active: Whether teleop is active - """ - if self._teleop_status_handle is not None: - self._teleop_status_handle.value = ( - "Teleop Status: Active" if active else "Teleop Status: Inactive" - ) - - def update_controller_status_display( - self, position: np.ndarray | None, connected: bool = True - ) -> None: - """Update controller status display. - - Args: - position: Controller position (3D array) or None - connected: Whether controller is connected - """ - if self._controller_status_handle is None: - return - - if connected and position is not None: - controller_status_str = "Controller Status:\n" - controller_status_str += f" Position: [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}]\n" - controller_status_str += " Connected: ✓\n" - self._controller_status_handle.value = controller_status_str - else: - self._controller_status_handle.value = "Controller Status:\n Connected: ✗" - - def update_gripper_status( - self, trigger_value: float, robot_enabled: bool = True - ) -> None: - """Update gripper status display. - - Args: - trigger_value: Trigger value (0.0 = open, 1.0 = closed) - robot_enabled: Whether robot is enabled - """ - if self._gripper_status_handle is None: - return - - gripper_closed_percent = trigger_value * 100.0 - if trigger_value > 0.9: - gripper_state = "Closed" - elif trigger_value > 0.1: - gripper_state = "Closing" - else: - gripper_state = "Open" - - status = f"Gripper: {gripper_state} ({gripper_closed_percent:.0f}% closed)" - if not robot_enabled: - status += " [Disabled]" - - self._gripper_status_handle.value = status - - def update_controller_visualization(self, transform: np.ndarray | None) -> None: - """Update controller transform visualization. - - Args: - transform: 4x4 transformation matrix or None - """ - if self._controller_handle is None or transform is None: - return - - controller_pos = transform[:3, 3] - controller_rot = Rotation.from_matrix(transform[:3, :3]) - controller_quat_xyzw = controller_rot.as_quat() - controller_quat_wxyz = [ - controller_quat_xyzw[3], - controller_quat_xyzw[0], - controller_quat_xyzw[1], - controller_quat_xyzw[2], - ] - - self._controller_handle.position = tuple(controller_pos) - self._controller_handle.wxyz = tuple(controller_quat_wxyz) - - def update_target_visualization(self, transform: np.ndarray | None) -> None: - """Update target/goal frame visualization. - - Args: - transform: 4x4 transformation matrix or None - """ - if self._target_frame_handle is None or transform is None: - return - - target_pos = transform[:3, 3] - target_rot = Rotation.from_matrix(transform[:3, :3]) - target_quat_xyzw = target_rot.as_quat() - target_quat_wxyz = [ - target_quat_xyzw[3], - target_quat_xyzw[0], - target_quat_xyzw[1], - target_quat_xyzw[2], - ] - - self._target_frame_handle.position = tuple(target_pos) - self._target_frame_handle.wxyz = tuple(target_quat_wxyz) - - def get_pink_parameters(self) -> dict: - """Get Pink IK parameters from GUI controls. - - Returns: - Dictionary with parameter values - - Raises: - ValueError: If Pink parameter controls not initialized - """ - if not self._posture_cost_handles: - raise ValueError("Pink parameter controls not initialized") - - if ( - self._position_weight_handle is None - or self._orientation_weight_handle is None - or self._frame_task_gain_handle is None - or self._lm_damping_handle is None - or self._damping_weight_handle is None - or self._solver_damping_value_handle is None - ): - raise ValueError("Pink parameter controls not initialized") - - posture_cost_vector = np.array( - [handle.value for handle in self._posture_cost_handles] - ) - - params = { - "position_cost": self._position_weight_handle.value, - "orientation_cost": self._orientation_weight_handle.value, - "frame_task_gain": self._frame_task_gain_handle.value, - "lm_damping": self._lm_damping_handle.value, - "damping_cost": self._damping_weight_handle.value, - "solver_damping_value": self._solver_damping_value_handle.value, - "posture_cost_vector": posture_cost_vector, - } - return params - - def get_controller_filter_params(self) -> tuple[float, float, float]: - """Get 1€ Filter parameters from GUI. - - Returns: - Tuple of (min_cutoff, beta, d_cutoff) - - Raises: - ValueError: If controller filter controls not initialized - """ - if ( - self._controller_min_cutoff_handle is None - or self._controller_beta_handle is None - or self._controller_d_cutoff_handle is None - ): - raise ValueError("Controller filter controls not initialized") - return ( - self._controller_min_cutoff_handle.value, - self._controller_beta_handle.value, - self._controller_d_cutoff_handle.value, - ) - - def get_translation_scale(self) -> float: - """Get translation scale value from GUI. - - Returns: - Translation scale value - - Raises: - ValueError: If scaling controls not initialized - """ - if self._translation_scale_handle is None: - raise ValueError("Scaling controls not initialized") - return self._translation_scale_handle.value - - def get_rotation_scale(self) -> float: - """Get rotation scale value from GUI. - - Returns: - Rotation scale value - - Raises: - ValueError: If scaling controls not initialized - """ - if self._rotation_scale_handle is None: - raise ValueError("Scaling controls not initialized") - return self._rotation_scale_handle.value - - def set_grip_value(self, value: float) -> None: - """Set grip value display. - - Args: - value: Grip value (0.0-1.0) - - Raises: - ValueError: If grip value control not initialized - """ - if self._grip_value_handle is None: - raise ValueError("Grip value control not initialized") - self._grip_value_handle.value = value - - def set_trigger_value(self, value: float) -> None: - """Set trigger value display. - - Args: - value: Trigger value (0.0-1.0) - - Raises: - ValueError: If trigger value control not initialized - """ - if self._trigger_value_handle is None: - raise ValueError("Trigger value control not initialized") - self._trigger_value_handle.value = value - - def set_joint_angles_text(self, text: str) -> None: - """Set joint angles text display. - - Args: - text: Text to display - - Raises: - ValueError: If joint angles control not initialized - """ - if self._joint_angles_handle is None: - raise ValueError("Joint angles control not initialized") - self._joint_angles_handle.value = text - - def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None: - """Update ghost robot visualization from joint configuration. - - Args: - joint_config: Joint angles in radians - """ - if self._ghost_robot_urdf is not None: - self._ghost_robot_urdf.update_cfg(joint_config) - - def update_ghost_robot_visibility(self, flag: bool) -> None: - """Update ghost robot visibility. - - Args: - flag: Whether to show the ghost robot - """ - if self._ghost_robot_urdf is not None: - self._ghost_robot_urdf.show_visual = flag - - def add_policy_controls( - self, - initial_prediction_ratio: float = 0.8, - initial_policy_rate: float = 200.0, - initial_robot_rate: float = 200.0, - initial_execution_mode: str = "targeting_time", - ) -> None: - """Add policy-related GUI controls. - - Args: - initial_prediction_ratio: Initial prediction horizon execution ratio (0.0-1.0) - initial_policy_rate: Initial policy execution rate in Hz - initial_robot_rate: Initial robot rate in Hz - initial_execution_mode: Initial execution mode ("targeting_time" or "targeting_pose") - """ - self._policy_status_handle = self._server.gui.add_text( - "Policy Status", "Ready - Press Right Joystick to get prediction" - ) - - self._prediction_ratio_handle = self._server.gui.add_number( - "Prediction Horizon Execution Ratio", - initial_prediction_ratio, - min=0.0, - max=1.0, - step=0.01, - ) - - self._policy_execution_rate_handle = self._server.gui.add_number( - "Policy Execution Rate", - initial_policy_rate, - min=1.0, - max=200.0, - step=1.0, - ) - - self._robot_rate_handle = self._server.gui.add_number( - "Robot Rate", - initial_robot_rate, - min=1.0, - max=200.0, - step=1.0, - ) - - self._execution_mode_dropdown = self._server.gui.add_dropdown( - "Execution Mode", - options=["targeting_time", "targeting_pose"], - initial_value=initial_execution_mode, - ) - - def add_policy_buttons(self) -> None: - """Add policy control buttons.""" - self._run_policy_button = self._server.gui.add_button("Run Policy (Preview)") - self._start_policy_execution_button = self._server.gui.add_button( - "Execute Policy (Run Preview)" - ) - self._play_policy_button = self._server.gui.add_button("Continuous Receding Horizon") - - def update_policy_status(self, status: str) -> None: - """Update policy status display. - - Args: - status: Status string to display - """ - if self._policy_status_handle is not None: - self._policy_status_handle.value = status - - def get_prediction_ratio(self) -> float: - """Get prediction horizon execution ratio from GUI. - - Returns: - Prediction ratio (0.0-1.0) - - Raises: - ValueError: If policy controls not initialized - """ - if self._prediction_ratio_handle is None: - raise ValueError("Policy controls not initialized") - return self._prediction_ratio_handle.value - - def get_policy_execution_rate(self) -> float: - """Get policy execution rate from GUI. - - Returns: - Policy execution rate in Hz - - Raises: - ValueError: If policy controls not initialized - """ - if self._policy_execution_rate_handle is None: - raise ValueError("Policy controls not initialized") - return self._policy_execution_rate_handle.value - - def get_robot_rate(self) -> float: - """Get robot rate from GUI. - - Returns: - Robot rate in Hz - - Raises: - ValueError: If policy controls not initialized - """ - if self._robot_rate_handle is None: - raise ValueError("Policy controls not initialized") - return self._robot_rate_handle.value - - def get_execution_mode(self) -> str: - """Get execution mode from GUI. - - Returns: - Execution mode string ("targeting_time" or "targeting_pose") - - Raises: - ValueError: If policy controls not initialized - """ - if self._execution_mode_dropdown is None: - raise ValueError("Policy controls not initialized") - return self._execution_mode_dropdown.value - - def get_ghost_robot_visibility(self) -> bool: - """Get ghost robot visibility. - - Returns: - Whether the ghost robot is visible - """ - if self._ghost_robot_urdf is not None: - return self._ghost_robot_urdf.show_visual - return False - - def set_run_policy_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Run Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._run_policy_button is not None: - self._run_policy_button.on_click(lambda _: callback()) - - def set_start_policy_execution_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Execute Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._start_policy_execution_button is not None: - self._start_policy_execution_button.on_click(lambda _: callback()) - - - def set_play_policy_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Play Policy button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._play_policy_button is not None: - self._play_policy_button.on_click(lambda _: callback()) - - def set_execution_mode_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for execution mode dropdown. - - Args: - callback: Callback function to call when dropdown value changes - """ - if self._execution_mode_dropdown is not None: - self._execution_mode_dropdown.on_update(lambda _: callback()) - - def set_enable_robot_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Enable Robot button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._enable_robot_handle is not None: - self._enable_robot_handle.on_click(lambda _: callback()) - - def set_disable_robot_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Disable Robot button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._disable_robot_handle is not None: - self._disable_robot_handle.on_click(lambda _: callback()) - - def set_emergency_stop_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Emergency Stop button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._emergency_stop_handle is not None: - self._emergency_stop_handle.on_click(lambda _: callback()) - - def set_go_home_callback(self, callback: Callable[[], Any]) -> None: - """Set callback for Go Home button. - - Args: - callback: Callback function to call when button is clicked - """ - if self._go_home_button is not None: - self._go_home_button.on_click(lambda _: callback()) - - def set_run_policy_button_disabled(self, disabled: bool) -> None: - """Set Run Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._run_policy_button is not None: - self._run_policy_button.disabled = disabled - - def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: - """Set Execute Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._start_policy_execution_button is not None: - self._start_policy_execution_button.disabled = disabled - - - def set_play_policy_button_disabled(self, disabled: bool) -> None: - """Set Play Policy button disabled state. - - Args: - disabled: Whether button should be disabled - """ - if self._play_policy_button is not None: - self._play_policy_button.disabled = disabled - - def update_play_policy_button_status(self, active: bool) -> None: - """Update play policy button label based on active state. - - Args: - active: Whether continuous play is currently active - """ - if self._play_policy_button is not None: - self._play_policy_button.label = ( - "Stop Continuous Horizon" if active else "Continuous Receding Horizon" - ) - def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: - """Set the color of the ghost robot. - - Args: - color: RGBA tuple (0.0 to 1.0) - """ - if self._ghost_robot_urdf is not None: - self._ghost_robot_urdf.mesh_color_override = color - - def stop(self) -> None: - """Stop the visualizer server.""" - self._server.stop() - - -============================================================ -FILE: common/threads/__init__.py -============================================================ - -"""Shared thread functions for teleoperation examples.""" - - -============================================================ -FILE: common/threads/ik_solver.py -============================================================ - -"""IK solver thread - solves IK and updates state.""" - -import sys -import time -import traceback -from pathlib import Path - -import numpy as np - -# Add parent directory to path to import pink_ik_solver -sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import IK_SOLVER_RATE -from common.data_manager import DataManager, RobotActivityState -from common.utils import scale_and_add_delta_transform - -from pink_ik_solver import PinkIKSolver - - -def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None: - """IK solver thread - solves IK and updates state.""" - print("🧮 IK solver thread started") - - dt: float = 1.0 / IK_SOLVER_RATE - DIVERGENCE_TOLERANCE = 0.1 - - try: - while not data_manager.is_shutdown_requested(): - iteration_start: float = time.time() - - # Get current robot joint angles from state - current_joint_angles = data_manager.get_current_joint_angles() - - # Get current end effector pose from IK solver and set in state - ik_ee_pose = ik_solver.get_current_end_effector_pose() - data_manager.set_current_end_effector_pose(ik_ee_pose) - - # Get current state - robot_activity_state = data_manager.get_robot_activity_state() - controller_transform, _, _ = data_manager.get_controller_data() - teleop_active = data_manager.get_teleop_active() - - # Keep IK anchored to the real robot whenever teleop is not actively solving. - # This avoids stale IK state after manual joint commands (e.g., Button Y toggle). - if current_joint_angles is not None: - if not teleop_active: - ik_solver.set_configuration_no_task_update( - np.radians(current_joint_angles) - ) - else: - current_ik_joint_angles = np.degrees( - ik_solver.get_current_configuration() - ) - # During active teleop, only hard-sync when IK and hardware are already close. - if current_ik_joint_angles is not None and np.all( - np.abs(current_joint_angles - current_ik_joint_angles) - <= DIVERGENCE_TOLERANCE - ): - ik_solver.set_configuration_no_task_update( - np.radians(current_joint_angles) - ) - - # Skip teleop-based IK if in POLICY_CONTROLLED state - # NOTE: During policy execution, the policy execution thread manages target joint angles - # We only update IK solver configuration to keep it in sync, but don't override targets - if robot_activity_state == RobotActivityState.POLICY_CONTROLLED: - if current_joint_angles is not None: - ik_solver.set_configuration(np.radians(current_joint_angles)) - current_end_effector_pose = ( - data_manager.get_current_end_effector_pose() - ) - data_manager.set_target_pose(current_end_effector_pose) - data_manager.set_ik_solve_time_ms(0.0) - data_manager.set_ik_success(True) - - elif teleop_active and controller_transform is not None: - controller_initial, robot_initial = ( - data_manager.get_initial_robot_controller_transforms() - ) - if controller_initial is None or robot_initial is None: - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - continue - - # Calculate delta transform - delta_position = controller_transform[:3, 3] - controller_initial[:3, 3] - delta_orientation = ( - controller_transform[:3, :3] @ controller_initial[:3, :3].T - ) - - # Get current teleop scaling factors (from GUI via DataManager) - translation_scale, rotation_scale = data_manager.get_teleop_scaling() - - T_robot_target = scale_and_add_delta_transform( - delta_position, - delta_orientation, - translation_scale, - rotation_scale, - robot_initial, - ) - - ik_solver.set_target_pose(T_robot_target[:3, 3], T_robot_target[:3, :3]) - data_manager.set_target_pose(T_robot_target) - - # Solve IK - success = ik_solver.solve_ik() - - if success: - joint_config = np.degrees(ik_solver.get_current_configuration()) - stats = ik_solver.get_statistics() - solve_time_ms = stats["last_solve_time_ms"] - - data_manager.set_target_joint_angles(joint_config) - data_manager.set_ik_solve_time_ms(solve_time_ms) - data_manager.set_ik_success(success) - else: - data_manager.set_ik_solve_time_ms(0.0) - data_manager.set_ik_success(False) - - else: # robot is HOMING or DISABLED - if current_joint_angles is not None: - ik_solver.set_configuration(np.radians(current_joint_angles)) - current_end_effector_pose = ( - data_manager.get_current_end_effector_pose() - ) - data_manager.set_target_pose(current_end_effector_pose) - data_manager.set_target_joint_angles(current_joint_angles) - data_manager.set_ik_solve_time_ms(0.0) - data_manager.set_ik_success(True) - - # Sleep to maintain loop rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - except Exception as e: - print(f"❌ IK solver thread error: {e}") - traceback.print_exc() - data_manager.request_shutdown() - finally: - print("🧮 IK solver thread stopped") - - -============================================================ -FILE: common/threads/quest_reader.py -============================================================ - -"""Quest reader thread - reads controller data and manages teleop state.""" - -import time -import traceback - -from common.configs import CONTROLLER_DATA_RATE, GRIP_THRESHOLD -from common.data_manager import DataManager, RobotActivityState -from meta_quest_teleop.reader import MetaQuestReader - - -def quest_reader_thread( - data_manager: DataManager, quest_reader: MetaQuestReader -) -> None: - """Quest reader thread - reads controller data and manages teleop state. - - This thread runs at high frequency to ensure responsive controller input. - Handles: - - Reading Meta Quest controller data - - Processing grip button (dead man's switch) - - Managing teleop activation/deactivation - - Capturing initial poses when teleop activates - - Args: - data_manager: DataManager object for thread-safe communication - quest_reader: MetaQuestReader instance - """ - print("🎮 Quest Controller thread started") - - dt: float = 1.0 / CONTROLLER_DATA_RATE - prev_grip_active: bool = False - - try: - while not data_manager.is_shutdown_requested(): - iteration_start = time.time() - - # Get controller data - grip_value = quest_reader.get_grip_value("right") - trigger_value = quest_reader.get_trigger_value("right") - controller_transform = quest_reader.get_hand_controller_transform_ros( - hand="right" - ) - - # Update shared state with controller data - data_manager.set_controller_data( - controller_transform, grip_value, trigger_value - ) - - # Grip button logic (dead man's switch) - robot_activity_state = data_manager.get_robot_activity_state() - # Teleop can only be activated if robot is ENABLED (not HOMING or DISABLED) - grip_active = ( - grip_value >= GRIP_THRESHOLD - and robot_activity_state == RobotActivityState.ENABLED - ) - - # Rising edge - grip just pressed AND robot is enabled - if ( - grip_active - and not prev_grip_active - and controller_transform is not None - ): - # Start teleop control - # capture initial poses - controller_initial_transform = controller_transform.copy() - - # Capture initial robot end-effector pose - robot_initial_transform = data_manager.get_current_end_effector_pose() - - data_manager.set_teleop_state( - True, controller_initial_transform, robot_initial_transform - ) - - print("✓ Teleop control activated") - print( - f" Controller initial position: {controller_initial_transform[:3, 3]}" - ) - if robot_initial_transform is not None: - print(f" Robot initial position: {robot_initial_transform[:3, 3]}") - else: - print(" Robot initial position: None") - - # Falling edge - grip just released OR robot disabled - elif not grip_active and prev_grip_active: - # Stop teleop control - data_manager.set_teleop_state(False, None, None) - print("✗ Teleop control deactivated") - - prev_grip_active = grip_active - - # Sleep to maintain loop rate (check shutdown more frequently) - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - except Exception as e: - print(f"❌ Quest reader thread error: {e}") - traceback.print_exc() - data_manager.request_shutdown() - finally: - # Ensure clean exit - deactivate teleop - data_manager.set_teleop_state(False, None, None) - print("🎮 Quest Controller thread stopped") - - -============================================================ -FILE: common/threads/camera_usb.py -============================================================ - -"""Camera thread - captures RGB images from a USB webcam (OpenCV).""" - -import time -import traceback - -import cv2 -from common.configs import ( - CAMERA_DEVICE_INDEX, - CAMERA_FRAME_STREAMING_RATE, - CAMERA_HEIGHT, - CAMERA_NAMES, - CAMERA_WIDTH, -) -from common.data_manager import DataManager - - -def camera_thread(data_manager: DataManager) -> None: - """Camera thread - captures RGB images from a USB webcam.""" - print("📷 Camera thread started (USB webcam)") - - dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE - cap: cv2.VideoCapture | None = None - camera_name = CAMERA_NAMES[1] - - try: - cap = cv2.VideoCapture(CAMERA_DEVICE_INDEX) - if not cap.isOpened(): - print( - f"❌ Could not open USB webcam (device index {CAMERA_DEVICE_INDEX}). " - "Check connection and permissions." - ) - data_manager.request_shutdown() - return - - cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) - cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) - cap.set(cv2.CAP_PROP_FPS, CAMERA_FRAME_STREAMING_RATE) - - # Read back actual resolution (some webcams don't support requested size) - actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) - actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) - print( - f" Webcam opened: {actual_w}x{actual_h} @ ~{CAMERA_FRAME_STREAMING_RATE} Hz" - ) - - while not data_manager.is_shutdown_requested(): - iteration_start = time.time() - - ret, frame = cap.read() - if not ret or frame is None: - print("⚠️ Webcam read failed, skipping frame") - time.sleep(dt) - continue - - # OpenCV returns BGR; convert to RGB for consistency with pipeline - rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - # Camera may be mounted upside down; rotate 180° to keep images upright - rgb_image = cv2.rotate(rgb_image, cv2.ROTATE_180) - # Treat USB webcam as wrist camera - data_manager.set_rgb_image(rgb_image, camera_name) - - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - except Exception as e: - print(f"❌ Camera thread error: {e}") - traceback.print_exc() - data_manager.request_shutdown() - finally: - if cap is not None: - cap.release() - print(" ✓ USB webcam released") - print("📷 Camera thread stopped") - - -============================================================ -FILE: common/threads/joint_state.py -============================================================ - -"""Joint state thread - reads joint state and sends commands.""" - -import sys -import time -import traceback -from pathlib import Path - -sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import JOINT_STATE_STREAMING_RATE -from common.data_manager import DataManager, RobotActivityState - -from piper_controller import PiperController - - -def joint_state_thread( - data_manager: DataManager, robot_controller: PiperController -) -> None: - """Joint state thread - reads joint state and sends commands.""" - print("🔧 Joint state thread started") - - dt: float = 1.0 / JOINT_STATE_STREAMING_RATE - - try: - while not data_manager.is_shutdown_requested(): - iteration_start: float = time.time() - - # Get current joint angles and gripper value - current_joint_angles = robot_controller.get_current_joint_angles() - if current_joint_angles is not None: - data_manager.set_current_joint_angles(current_joint_angles) - - # Use measured joint currents as torque proxy for NeuraCore logging - current_joint_currents = robot_controller.get_current_joint_currents() - if current_joint_currents is not None: - data_manager.set_current_joint_torques(current_joint_currents) - - # Get current gripper open value and set in state - gripper_open_value = robot_controller.get_current_gripper_open_value() - if gripper_open_value is not None: - data_manager.set_current_gripper_open_value(gripper_open_value) - - target_joint_angles = data_manager.get_target_joint_angles() - _, _, trigger_value = data_manager.get_controller_data() - - # Check if robot is homing - robot_activity_state = data_manager.get_robot_activity_state() - if robot_activity_state == RobotActivityState.HOMING: - if robot_controller.is_robot_homed(): - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ Robot reached home position and is re-enabled") - - elif robot_activity_state == RobotActivityState.ENABLED: - if target_joint_angles is not None and data_manager.get_teleop_active(): - robot_controller.set_target_joint_angles(target_joint_angles) - - if trigger_value is not None and data_manager.get_teleop_active(): - target_gripper_open_value = 1.0 - trigger_value - data_manager.set_target_gripper_open_value( - target_gripper_open_value - ) - robot_controller.set_gripper_open_value(target_gripper_open_value) - - # Sleep to maintain streaming rate - elapsed = time.time() - iteration_start - sleep_time = dt - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - except Exception as e: - print(f"❌ Joint state thread error: {e}") - traceback.print_exc() - data_manager.request_shutdown() - finally: - print("🔧 Joint state thread stopped") - - -============================================================ -FILE: common/threads/realsense_camera.py -============================================================ - -"""Camera thread - captures RGB images from RealSense with drop detection.""" - -import time -import traceback -from collections import deque -import cv2 - -import numpy as np -import pyrealsense2 as rs -from common.configs import CAMERA_FRAME_STREAMING_RATE, CAMERA_NAMES -from common.data_manager import DataManager - - -def camera_thread(data_manager: DataManager) -> None: - """Camera thread - captures RGB images from RealSense and monitors health.""" - print("📷 Camera thread started") - - camera_name = CAMERA_NAMES[0] - pipeline: rs.pipeline | None = None - - # Diagnostic variables - last_frame_number = None - total_dropped_frames = 0 - fps_timer = time.time() - frame_count = 0 - # Store the last 100 frame intervals to check for extreme jitter - intervals = deque(maxlen=100) - last_frame_time = time.time() - - try: - # Configure RealSense pipeline - pipeline = rs.pipeline() - config = rs.config() - - config.enable_stream( - rs.stream.color, - 640, - 480, - rs.format.rgb8, - int(CAMERA_FRAME_STREAMING_RATE), - ) - - print(f"Starting RealSense pipeline at {CAMERA_FRAME_STREAMING_RATE} Hz...") - pipeline.start(config) - - while not data_manager.is_shutdown_requested(): - try: - # wait_for_frames naturally blocks at the target framerate (e.g., 60Hz) - frames = pipeline.wait_for_frames(timeout_ms=500) - except Exception as e: - print(f"⚠️ RealSense wait for frames error (Timeout?): {e}") - continue - - color_frame = frames.get_color_frame() - if not color_frame: - continue - - current_time = time.time() - intervals.append(current_time - last_frame_time) - last_frame_time = current_time - - # --------------------------------------------------------- - # DIAGNOSTICS: Check for dropped frames via hardware ID - # --------------------------------------------------------- - current_frame_number = color_frame.get_frame_number() - - if last_frame_number is not None: - # If frame numbers are not sequential, we missed something in software - drops = (current_frame_number - last_frame_number) - 1 - if drops > 0: - total_dropped_frames += drops - print(f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}") - - last_frame_number = current_frame_number - - # --------------------------------------------------------- - # DIAGNOSTICS: Calculate software-side FPS and Jitter - # --------------------------------------------------------- - frame_count += 1 - if current_time - fps_timer >= 5.0: # Report every 5 seconds - effective_fps = frame_count / (current_time - fps_timer) - - # Calculate jitter (max variance between frame arrivals) - max_interval = max(intervals) * 1000 # in ms - min_interval = min(intervals) * 1000 # in ms - - print(f"📊 Camera Health: {effective_fps:.1f} FPS | " - f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " - f"Total Drops: {total_dropped_frames}") - - # Reset counters for the next 5-second window - fps_timer = current_time - frame_count = 0 - - # --------------------------------------------------------- - # IMAGE PROCESSING - # --------------------------------------------------------- - # color_image = np.asanyarray(color_frame.get_data()) - # color_image = np.rot90(color_image, k=3) # Rotate 270 degrees - # data_manager.set_rgb_image(color_image, camera_name) - - color_image = np.asanyarray(color_frame.get_data()) - color_image = np.rot90(color_image, k=3) # Rotate 270 degrees - data_manager.set_rgb_image(color_image, camera_name) - - # Notice: The time.sleep() has been completely removed. - # wait_for_frames() manages the loop pace perfectly. - - except Exception as e: - print(f"❌ Camera thread error: {e}") - traceback.print_exc() - data_manager.request_shutdown() - finally: - if pipeline is not None: - try: - pipeline.stop() - print(" ✓ RealSense pipeline stopped") - except Exception as e: - print(f"⚠️ Error stopping pipeline: {e}") - print("📷 Camera thread stopped") From c158874e9a8cd9c76856b63067e24c10188d78b1 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 12:13:14 +0100 Subject: [PATCH 17/22] add explanations for ik configs --- .gitignore | 3 +- examples/ik_conf/default.yaml | 57 +++++++++++++++++++++- examples/ik_conf/tuned_teleop_configs.yaml | 24 --------- 3 files changed, 57 insertions(+), 27 deletions(-) delete mode 100644 examples/ik_conf/tuned_teleop_configs.yaml diff --git a/.gitignore b/.gitignore index 8979a3f..6eacefb 100644 --- a/.gitignore +++ b/.gitignore @@ -13,4 +13,5 @@ venv.bak/ env.bak/ logs/ -*.zip \ No newline at end of file +*.zip +tuned_teleop_configs.yaml \ No newline at end of file diff --git a/examples/ik_conf/default.yaml b/examples/ik_conf/default.yaml index c6568dd..c538ad7 100644 --- a/examples/ik_conf/default.yaml +++ b/examples/ik_conf/default.yaml @@ -1,24 +1,77 @@ +# ================================================================================= +# PIPER TELEOPERATION & IK CONFIGURATION +# ================================================================================= + ik_parameters: + # POSITION & ORIENTATION + # These weights dictate how the robot prioritizes reaching the exact XYZ coordinate + # versus matching the exact wrist rotation when it can't perfectly do both. + # Tune: Increase position_cost if the robot struggles to reach far XYZ coordinates. + # Increase orientation_cost if you need strict tool alignment (e.g., pouring). position_cost: 1.0 orientation_cost: 0.75 + + # KINEMATIC AGGRESSIVENESS + # How quickly the robot tries to close the distance between its current pose + # and the VR controller's target pose. + # Tune: Decrease to 0.1-0.2 for smoother, delayed, robotic movement. + # Increase to 0.7-0.9 for highly aggressive, snappy tracking. frame_task_gain: 0.4 + + # SINGULARITY & MATHEMATICAL DAMPING + # These parameters prevent the robot from violently flailing or crashing when + # it reaches the absolute edge of its workspace (singularities). + # Tune: Increase lm_damping (e.g., 0.05) if the robot twitches at full extension. + # solver_damping_value is strict math regularization; usually leave as 0.0001. lm_damping: 0.01 damping_cost: 0.25 solver_damping_value: 0.0001 + + # POSTURE PREFERENCE + # A 6-value array (one for each joint). Non-zero values act like a rubber band, + # gently pulling that specific joint back toward its default "neutral" angle. + # Tune: We put 0.05 on Joint 4 (the elbow) to stop the arm from bending backward + # into awkward, unnatural shapes during complex rotations. posture_cost_vector: - 0.0 - 0.0 - 0.0 - - 0.05 + - 0.05 # Gentle pull on the elbow - 0.0 - 0.0 + filter_parameters: + # 1-EURO FILTER (VR Controller Smoothing) + # VR controllers inherently have micro-jitters (from your hands or sensor noise). + + # controller_min_cutoff: Handles slow movements and holding completely still. + # Tune: Lower it (e.g., 0.1) to completely eliminate hand shaking (but adds lag). + # Raise it (e.g., 1.5) if you want zero lag, but the robot will vibrate more. controller_min_cutoff: 0.8 + + # controller_beta: Handles fast, sweeping movements. + # Tune: Increase (e.g., 0.5) to make the robot instantly snap to fast hand swipes. + # Decrease (e.g., 0.01) if fast movements feel too jerky. controller_beta: 0.05 + + # controller_d_cutoff: Internal speed calculation filter. Best left at 0.9-1.0. controller_d_cutoff: 0.9 + teleop_parameters: + # STANDARD MOVEMENT SCALING + # Maps how physical VR movement translates to actual robot movement. + # Tune: A translation_scale of 1.5 means moving your hand 10cm moves the robot 15cm. + # A rotation_scale of 1.2 means twisting your wrist 90° twists the robot 108°. translation_scale: 1.5 rotation_scale: 1.2 + + # PRECISION MOVEMENT SCALING (SLOW MODE) + # The alternate scaling profile used when you toggle the left joystick. + # Tune: Keep these below 1.0 for delicate tasks like thread insertion or grasping tiny objects. slow_translation_scale: 0.6 slow_rotation_scale: 0.6 - wrist_joint_button_step_degrees: 5.0 + + # MANUAL WRIST OVERRIDE + # How many degrees the 6th joint (wrist roll) rotates when you press the X or Y buttons. + # Tune: Adjust based on how fast you need to manually spin the gripper. + wrist_joint_button_step_degrees: 5.0 \ No newline at end of file diff --git a/examples/ik_conf/tuned_teleop_configs.yaml b/examples/ik_conf/tuned_teleop_configs.yaml deleted file mode 100644 index 38369da..0000000 --- a/examples/ik_conf/tuned_teleop_configs.yaml +++ /dev/null @@ -1,24 +0,0 @@ -ik_parameters: - position_cost: 1.0 - orientation_cost: 0.75 - frame_task_gain: 0.4 - lm_damping: 0.01 - damping_cost: 0.25 - solver_damping_value: 0.0001 - posture_cost_vector: - - 0.0 - - 0.0 - - 0.0 - - 0.05 - - 0.0 - - 0.0 -filter_parameters: - controller_min_cutoff: 0.8 - controller_beta: 0.05 - controller_d_cutoff: 0.9 -teleop_parameters: - translation_scale: 1.5 - rotation_scale: 1.2 - slow_translation_scale: 0.6 - slow_rotation_scale: 0.6 - wrist_joint_button_step_degrees: 7.0 From ec1001b68dd22f3cba27afcd80be4a02af6236a1 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 12:22:34 +0100 Subject: [PATCH 18/22] fix:updated README for clearer explanation --- README.md | 340 +++++++++++++++++++++++++----------------------------- 1 file changed, 159 insertions(+), 181 deletions(-) diff --git a/README.md b/README.md index c2d1c2a..cd25403 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ + # AgileX Piper Robot Teleoperation with Neuracore
@@ -12,289 +13,266 @@
-This project is a complete example showcasing how to use Neuracore with the AgileX Piper robot. The project provides examples teleoperating the AgileX Piper robot using a Meta Quest controller, collecting demonstration data with [Neuracore](https://neuracore.com/), deploying trained policies, and an easy interface to tune most of the associated parameters. +This repository provides a complete, production-ready framework for integrating the AgileX Piper robotic arm with [Neuracore](https://neuracore.com/). It includes tools for teleoperating the robot using a Meta Quest VR headset, collecting perfectly synchronized demonstration datasets, validating and replaying data, and deploying trained AI policies directly to the hardware. + +--- -## Prerequisites +## 📋 Prerequisites -- Python 3.10 -- sudo apt-get install sox -- Conda (for environment management) -- Meta Quest device setup (see `meta_quest_teleop/README.md` for details) -- Realsense camera +Before starting, ensure your system meets the following requirements: +* **OS/Environment:** Ubuntu/Linux with Conda installed. +* **Python:** Version 3.10 or higher. +* **System Packages:** `sudo apt-get install sox` +* **Hardware:** * AgileX Piper robot arm connected via CAN bus. + * RealSense camera (or compatible USB webcam). + * Meta Quest headset (Developer Mode enabled, ADB USB debugging allowed). -## Installation +--- -### 1. Clone the Repository +## ⚙️ Installation +### 1. Clone the Main Repository +First, clone this repository to your workspace: ```bash git clone git@github.com:NeuracoreAI/example_agilex.git cd example_agilex + ``` -### 2. Create Conda Environment +### 2. Create the Conda Environment -Create and activate the conda environment: +Create and activate the dedicated Python environment: ```bash conda env create -f environment.yaml conda activate piper-teleop -``` -### 3. Clone and Install Meta Quest Teleop Package +``` -Clone the Meta Quest teleoperation repository and install it: +### 3. Install the Meta Quest Teleop Package -**NOTE**: Make sure you're installing it inside the `piper-teleop` conda environment. +To keep dependencies organized, clone the Meta Quest teleoperation package **alongside** (in the parent directory of) your `example_agilex` folder, then install it into your active conda environment: ```bash -git clone https://github.com/NeuracoreAI/meta_quest_teleop.git ## Halid Note: it is not clear where it should be cloned exactly. +# Navigate out of example_agilex +cd .. + +# Clone and install the Meta Quest reader +git clone [https://github.com/NeuracoreAI/meta_quest_teleop.git](https://github.com/NeuracoreAI/meta_quest_teleop.git) cd meta_quest_teleop pip install -e . -cd .. + +# Return to the main project directory +cd ../example_agilex + ``` -## Data Flow +--- -The teleoperation system follows this data flow: +## 🔄 System Architecture & Data Flow + +The teleoperation stack operates asynchronously to ensure smooth hardware control and precise data logging: + +```text +[ Meta Quest VR Headset ] + ↓ (ADB / WiFi) +[ Meta Quest Reader ] (Translates 6D poses & button states) + ↓ +[ Pink IK Solver ] (Converts spatial targets to joint angles) + ↓ +[ Piper Controller ] (Applies safety limits & smoothing) + ↓ (CAN Bus) +[ AgileX Piper Robot ] -``` -Meta Quest Controller - ↓ -Meta Quest Reader (originally by RAIL, improved by Neuracore) - ↓ -Piper Controller (improved by Neuracore) - ↓ -Pink IK Solver - ↓ -AgileX Piper Robot ``` -**Components**: -- **Meta Quest Reader**: Reads controller pose and button inputs from the Meta Quest device -- **Piper Controller**: Manages robot state and sends joint commands via CAN interface -- **Pink IK Solver**: Solves inverse kinematics to convert end-effector poses from the meta quest into joint angles +--- -## Usage +## 🚀 Usage Guide -**IMPORTANT NOTE:** Make sure that you have followed the instructions to set up the meta quest (according to [meta_quest_teleop](https://github.com/NeuracoreAI/meta_quest_teleop) README instructions) and that your equipment are functioning before starting the examples. Same applies to the piper robot! +> ⚠️ **HARDWARE SAFETY NOTE**: This software controls a physical, high-torque industrial robot. **Always keep your hand on the physical emergency stop.** Before running any script, ensure the robot's workspace is entirely clear of obstacles and personnel. ### 1. Tune Teleoperation Parameters -**Script**: `examples/1_tune_teleop_params.py` - -This script allows you to teleoperate the robot and tune control parameters using a GUI before you start collecting data. Useful for finding the best hyperparameters (optimal IK parameters, controller filter settings, and scaling factors) for your style of teleoperation. +**Script:** `examples/1_tune_teleop_params.py` -**NOTE:** The provided default hyperparameters were tuned on the AgileX Piper robot with a meta quest 3 and worked well for our demos. Make sure to -copy your values into the `configs.py` if you change them in the GUI and find more suitable parameters. +Use this script to safely test the robot and dial in your movement preferences before collecting data. It launches a web UI where you can adjust IK weights, motion smoothing (1-Euro filter), and scaling factors on the fly. ```bash -python examples/1_tune_teleop_params.py [--ip-address ] -``` +python examples/1_tune_teleop_params.py [--ip-address ] [--ik-config ik_conf/default.yaml] -**Arguments**: -- `--ip-address`: IP address of Meta Quest device (optional). Only needed when using WiFi connection. If not provided, defaults to auto-discovery via USB. +``` -**Controls**: -- **Button A**: Enable/disable robot -- **Right Grip**: Activate teleoperation (dead man's switch) -- **Right Trigger**: Close/open gripper -- **Button B**: Move robot to home position -- **GUI**: Adjust IK parameters, filter settings, scaling factors +* **Button A:** Enable/Disable robot hardware. +* **Right Grip (Hold):** Dead man's switch. You must hold this to move the robot. +* **Right Trigger:** Open/Close parallel gripper. +* **Button B:** Return robot to home position. +* **Save Config:** Once tuned, use the UI button to save your perfect settings to YAML. ### 2. Collect Teleoperation Data -**Script**: `examples/2_collect_teleop_data_with_neuracore.py` +**Script:** `examples/2_collect_teleop_data_with_neuracore.py` -Now that you have a well-tuned setup, you can use this script to record teleoperation demonstrations to Neuracore. +Once your parameters are tuned, use this script to record demonstrations directly to the Neuracore cloud. ```bash -python examples/2_collect_teleop_data_with_neuracore.py [--ip-address ] [--dataset-name ] -``` +python examples/2_collect_teleop_data_with_neuracore.py [--dataset-name ] [--ik-config ik_conf/default.yaml] -**Note**: You must be logged into Neuracore. - -**Arguments**: -- `--ip-address`: IP address of Meta Quest device (optional). Only needed when using WiFi connection. If not provided, defaults to auto-discovery via USB. -- `--dataset-name`: Name for the dataset (optional). If an existing dataset name is passed, the script will resume logging into this dataset. Otherwise, it will create a dataset with the specified name (or auto-generate one if not provided). +``` -**Controls**: -- Same as script 1, plus: -- **Right Joystick Press**: Start/stop data recording +* **Right Joystick (Press):** Start/Stop recording an episode. +* *(Note: Passing an existing `--dataset-name` will append new episodes to it. Otherwise, a new timestamped dataset is created).* -### 3. Replay Neuracore Episodes +### 3. Replay Neuracore Episodes (Hardware Validation) -**Script**: `examples/3_replay_neuracore_episodes.py` +**Script:** `examples/3_replay_neuracore_episodes.py` -**Halid Note**: This step can be dangerous. always ready to press the emergency. The index start from 0 not 1, hence if you see number as x in the frontend, you need to run x-1. I cannot rerun this command again successfully after the first time running. The frequency should be default as 20. +This script downloads a dataset from Neuracore and forces the physical robot to perfectly re-enact the recorded trajectory. -Replay recorded episodes from a Neuracore dataset on the physical robot. +> 🛑 **CRITICAL SAFETY WARNINGS**: +> * **Movement:** The robot will move identically to the recorded data. **Be ready to press the emergency stop.** Pressing `Ctrl+C` will gracefully halt the script and cut power to the motors after 5 seconds. +> * **Episode Indexing:** The index starts at 0. If you want to replay the episode labeled "3" in the Neuracore web dashboard, you must pass `--episode-index 2`. +> * **Session Resets:** You may need to fully restart the script/robot if you wish to run multiple different replays back-to-back. +> +> ```bash -python examples/3_replay_neuracore_episodes.py --dataset-name [--frequency ] [--episode-index ] -``` +python examples/3_replay_neuracore_episodes.py --dataset-name [--episode-index ] [--frequency 20] -**Arguments**: -- `--dataset-name`: Name of the Neuracore dataset to replay -- `--frequency`: Playback frequency in Hz (default: 0). 0 plays the data aperiodically (not synchronized at a certain frequency as it was recorded). -- `--episode-index`: Which episode to replay (default: 0). -1 will start replaying all the episodes one after the other. +``` -**NOTE:** please be careful that the robot **will start moving** on the same trajectory that was recorded. Pressing `ctrl+C` -will gracefully disable the robot and it will cut power to the motors after 5 seconds. +* **`--frequency`:** Playback speed in Hz. The default is `20`. Passing `0` plays the data aperiodically (exactly as the raw packets were recorded). +* **`--episode-index`:** Set to `-1` to replay all episodes in the dataset sequentially. ### 4. Rollout Neuracore Policy (Full GUI) -**Script**: `examples/4_rollout_neuracore_policy.py` - -This script should be run after you have trained a policy and want to see it running on the robot. From a GUI, it allows you to: -- run the policy first and see the prediction horizon without moving the robot, -- then execute this prediction horizon, if wanted. -- or you can run and execute the prediction horizon immediately (for one prediction horizon only). -- or you can play the policy where prediction horizon will be generated and executed in a loop, until stopped - -You also have all the same controls from **example_1**. This is to help you to manually move -the robot to a certain state and then run the policy for testing. +**Script:** `examples/4_rollout_neuracore_policy.py` +Deploy a trained AI model to the robot. This script opens a Viser 3D dashboard where you can manually pilot the robot to a starting state using the VR controller, preview the AI's predicted trajectory as a "ghost robot," and then execute the AI's actions. ```bash -python examples/4_rollout_neuracore_policy.py --train-run-name [--ip-address ] -``` - -or +# Load from a local model file +python examples/4_rollout_neuracore_policy.py --model-path -```bash -python examples/4_rollout_neuracore_policy.py --model-path [--ip-address ] -``` +# Load from a cloud training run +python examples/4_rollout_neuracore_policy.py --train-run-name -**Arguments**: -- `--train-run-name`: Name of the Neuracore training run (fetches model from Neuracore) -- `--model-path`: Local path to model file (alternative to train-run-name) -- `--ip-address`: IP address of Meta Quest device (optional). Only needed when using WiFi connection. If not provided, defaults to auto-discovery via USB. +# Connect to an active remote inference server +python examples/4_rollout_neuracore_policy.py --remote-endpoint-name +``` -### 5. Rollout Neuracore Policy (Minimal) +### 5. Rollout Neuracore Policy (Minimal / Headless) -**Script**: `examples/5_rollout_neuracore_policy_minimal.py` +**Script:** `examples/5_rollout_neuracore_policy_minimal.py` -Minimal version of policy rollout without GUI - This is a minimal clear example on how to deploy your policy with no extra features. +A lightweight, terminal-only version of the rollout script. It strips away the 3D GUI and Meta Quest tracking, making it perfect for rapid, automated deployments in constrained compute environments. ```bash python examples/5_rollout_neuracore_policy_minimal.py --train-run-name -``` - -or -```bash -python examples/5_rollout_neuracore_policy_minimal.py --model-path ``` -**Arguments**: -- `--train-run-name`: Name of the Neuracore training run (fetches model from Neuracore) -- `--model-path`: Local path to model file (alternative to train-run-name) +### 6. Visualize Policy Offline -### 6. Visualize Policy from Dataset +**Script:** `examples/6_visualize_policy_from_dataset.py` -**Script**: `examples/6_visualize_policy_from_dataset.py` - -This script is useful when you don't have the robot ready but want to visualize how well the policy would perform. It visualizes policy predictions on episodes from a dataset without running on the robot. Useful for debugging and analysis. +The safest way to validate a model. This script pulls camera and joint data from a recorded dataset, feeds it to your AI policy, and renders the AI's predictions in a 3D web simulation. No physical robot is required. ```bash python examples/6_visualize_policy_from_dataset.py --dataset-name --train-run-name + ``` -or +--- -```bash -python examples/6_visualize_policy_from_dataset.py --dataset-name --model-path -``` +## 🛠️ Configuration (`ik_conf/default.yaml`) -**Arguments**: -- `--dataset-name`: Name of the Neuracore dataset to visualize -- `--train-run-name`: Name of the Neuracore training run (fetches model from Neuracore) -- `--model-path`: Local path to model file (alternative to train-run-name) +To ensure maximum flexibility, all hardware tuning parameters have been extracted into YAML configuration files located in the `ik_conf/` directory. -## Configuration +You can modify `ik_conf/default.yaml` (or create your own profiles like `heavy_payload.yaml`) to adjust: -Most configuration parameters are defined in `examples/common/configs.py`. Key parameters include: +* **IK Parameters:** Position vs. orientation costs, joint damping, and posture preferences. +* **Filter Parameters:** 1-Euro smoothing coefficients to eliminate VR controller jitter. +* **Teleop Parameters:** Spatial translation/rotation scaling (e.g., mapping 1cm of hand movement to 2cm of robot movement). -- **IK Solver Parameters**: Position/orientation costs, damping, solver settings -- **Controller Filter**: One-euro filter parameters for smoothing controller input -- **Scaling Factors**: Translation and rotation scaling for teleoperation -- **Thread Rates**: Control loop frequencies for different components -- **Robot Parameters**: Neutral joint angles, joint names, frame names +Simply pass `--ik-config ik_conf/your_config.yaml` to any script to load a specific profile. -## Project Structure +--- -``` +## 📁 Project Structure + +```text example_agilex/ -├── examples/ # Example scripts +├── docs/ # Documentation assets +│ └── demo.gif # Main teleoperation demonstration file +├── examples/ # Task-specific execution scripts │ ├── 1_tune_teleop_params.py │ ├── 2_collect_teleop_data_with_neuracore.py │ ├── 3_replay_neuracore_episodes.py │ ├── 4_rollout_neuracore_policy.py │ ├── 5_rollout_neuracore_policy_minimal.py │ ├── 6_visualize_policy_from_dataset.py -│ └── common/ # Shared utilities -│ ├── configs.py # Configuration parameters -│ ├── data_manager.py +│ ├── 7_teleop_with_pedal.py +│ ├── combine_code.py # Source aggregation helper script +│ ├── ik_conf/ # Runtime parameter configurations +│ │ └── default.yaml # Tunable profile (IK, smoothing, and scaling targets) +│ └── common/ # Monolithic background framework layers +│ ├── config_parser.py +│ ├── configs.py # Hardcoded system constants +│ ├── data_manager.py # Thread-safe global state machine +│ ├── dataset_helpers.py +│ ├── foot_pedal.py # Keystroke mapping for foot pedals +│ ├── one_euro_filter.py +│ ├── policy_actions.py +│ ├── policy_helpers.py │ ├── policy_state.py │ ├── robot_visualizer.py -│ └── threads/ # Background thread implementations -├── meta_quest_teleop/ # Meta Quest controller interface -├── piper_controller.py # Robot controller interface -├── pink_ik_solver.py # Inverse kinematics solver -├── piper_description/ # Robot URDF and meshes -└── environment.yaml # Conda environment specification -``` - -## Troubleshooting - -### Import Errors - -- Ensure conda environment is activated: `conda activate piper-teleop` -- Verify all dependencies are installed: `conda env update -f environment.yaml` -- Check that `meta_quest_teleop` is cloned and installed: - ```bash - git clone https://github.com/NeuracoreAI/meta_quest_teleop.git - cd meta_quest_teleop - pip install -e . - cd .. - pip show meta_quest_teleop - ``` - -### Robot Communication Issues - -- Verify CAN interface is active: `ip link show can0` -- You can activate CAN interface with: `bash scripts/piper/can_activate.sh can0 1000000` -- **NOTE:** you'll most probably need to activate the CAN interface when you first connect your robot to your machine. -- Check robot power and CAN bus connection -- Ensure robot is in the correct mode for control +│ ├── shared_actions.py +│ ├── states.py +│ ├── system_bootstrap.py +│ ├── utils.py +│ ├── visualizer_core.py +│ ├── visualizer_gui.py +│ └── threads/ # Parallel processing workers +│ ├── camera_usb.py +│ ├── ik_solver.py +│ ├── joint_state.py +│ ├── quest_reader.py +│ └── realsense_camera.py +├── piper_description/ # Physical specifications of the manipulator +│ ├── meshes/ # Component link geometry files (base_link.STL to link8.STL) +│ └── urdf/ # AgileX Piper robot model definition file (.urdf) +├── scripts/ # Isolated automation and helper scripts +│ ├── oculus/ # Standalone VR tracking debug tools +│ │ └── monitor_hand_movement.py +│ └── piper/ # Hardware utilities +│ ├── can_activate.sh # Bash routine to initialize the 'can0' interface +│ └── piper_gui_control.py +├── environment.yaml # Conda dependency definitions +├── pink_ik_solver.py # Intermediary task space inverse kinematics layer +├── piper_controller.py # Native CAN-bus mapping driver for the robot +└── vectorised_posture_task.py -### Neuracore Connection Issues - -- Verify you're logged in: `neuracore login` -- Check network connectivity to Neuracore servers -- Verify dataset names/run names/model paths are correct - -## Safety Notes +``` -⚠️ **IMPORTANT**: This software controls a physical robot. Always: +--- -- Keep emergency stop accessible -- Start with robot disabled (Button A) -- Test in a safe area with no obstacles -- Monitor robot behavior closely, especially during first use -- Use the dead man's switch (grip button) - robot stops when released -- Ensure proper workspace clearance +## 🔧 Troubleshooting -## License +* **ADB / Meta Quest Permission Errors:** If the scripts immediately crash complaining about Meta Quest access, put the headset on, look for a "USB Detected" popup, and click "Allow". +* **CAN Bus / Robot Communication Issues:** If the script hangs on `Initializing robot on can0...`, verify your CAN interface is active. Run `ip link show can0`. If it is down, activate it using: +```bash +bash scripts/piper/can_activate.sh can0 1000000 -See LICENSE file for details. +``` -## Contributing -Contributions are welcome! Please follow the project's coding standards and submit pull requests for review. +* **Remote Endpoint Crashes:** +If Scripts 4, 5, or 6 throw an `EndpointError`, verify that your deployment server is actually set to "Active" in the Neuracore web dashboard. -## Support +--- -For issues and questions, please open an issue on the repository or contact the maintainers. +## 📄 License & Support +See the [LICENSE](https://www.google.com/search?q=LICENSE) file for open-source terms. For questions, bug reports, or feature requests, please open an Issue on GitHub or join our community Discord server. From 16ede79103eca6694cf13166c2f69ac673d98d6b Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 12:35:23 +0100 Subject: [PATCH 19/22] fix:update readme for example file command --- README.md | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index cd25403..ea6ead0 100644 --- a/README.md +++ b/README.md @@ -99,7 +99,8 @@ The teleoperation stack operates asynchronously to ensure smooth hardware contro Use this script to safely test the robot and dial in your movement preferences before collecting data. It launches a web UI where you can adjust IK weights, motion smoothing (1-Euro filter), and scaling factors on the fly. ```bash -python examples/1_tune_teleop_params.py [--ip-address ] [--ik-config ik_conf/default.yaml] +cd examples +python 1_tune_teleop_params.py [--ip-address ] [--ik-config ik_conf/default.yaml] ``` @@ -116,7 +117,8 @@ python examples/1_tune_teleop_params.py [--ip-address ] [--ik-config i Once your parameters are tuned, use this script to record demonstrations directly to the Neuracore cloud. ```bash -python examples/2_collect_teleop_data_with_neuracore.py [--dataset-name ] [--ik-config ik_conf/default.yaml] +cd examples +python 2_collect_teleop_data_with_neuracore.py [--dataset-name ] [--ik-config ik_conf/default.yaml] ``` @@ -137,7 +139,8 @@ This script downloads a dataset from Neuracore and forces the physical robot to > ```bash -python examples/3_replay_neuracore_episodes.py --dataset-name [--episode-index ] [--frequency 20] +cd examples +python 3_replay_neuracore_episodes.py --dataset-name [--episode-index ] [--frequency 20] ``` @@ -152,13 +155,14 @@ Deploy a trained AI model to the robot. This script opens a Viser 3D dashboard w ```bash # Load from a local model file -python examples/4_rollout_neuracore_policy.py --model-path +cd examples +python 4_rollout_neuracore_policy.py --model-path # Load from a cloud training run -python examples/4_rollout_neuracore_policy.py --train-run-name +python 4_rollout_neuracore_policy.py --train-run-name # Connect to an active remote inference server -python examples/4_rollout_neuracore_policy.py --remote-endpoint-name +python 4_rollout_neuracore_policy.py --remote-endpoint-name ``` @@ -169,7 +173,8 @@ python examples/4_rollout_neuracore_policy.py --remote-endpoint-name +cd examples +python 5_rollout_neuracore_policy_minimal.py --train-run-name ``` @@ -180,7 +185,8 @@ python examples/5_rollout_neuracore_policy_minimal.py --train-run-name --train-run-name +cd examples +python 6_visualize_policy_from_dataset.py --dataset-name --train-run-name ``` From d048107a457befb532ed268d70b73d290a36e39e Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 15:58:23 +0100 Subject: [PATCH 20/22] fix: fix the pre-commit errors --- examples/1_tune_teleop_params.py | 207 +++++++---- .../2_collect_teleop_data_with_neuracore.py | 139 +++++--- examples/3_replay_neuracore_episodes.py | 115 ++++--- examples/4_rollout_neuracore_policy.py | 148 +++++--- .../5_rollout_neuracore_policy_minimal.py | 169 ++++++--- examples/6_visualize_policy_from_dataset.py | 176 +++++++--- examples/7_teleop_with_pedal.py | 118 ++++--- examples/combine_code.py | 26 +- examples/common/config_parser.py | 14 +- examples/common/configs.py | 40 ++- examples/common/data_manager.py | 50 +-- examples/common/dataset_helpers.py | 37 +- examples/common/policy_actions.py | 246 +++++++++---- examples/common/policy_state.py | 4 +- examples/common/robot_visualizer.py | 5 +- examples/common/shared_actions.py | 47 ++- examples/common/states.py | 15 +- examples/common/system_bootstrap.py | 75 ++-- examples/common/threads/realsense_camera.py | 27 +- examples/common/visualizer_core.py | 9 +- examples/common/visualizer_gui.py | 324 +++++++++++++----- piper_controller.py | 15 +- 22 files changed, 1361 insertions(+), 645 deletions(-) diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index 44ae16b..817528a 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -9,50 +9,62 @@ """ import argparse +import os import sys import threading import time import traceback -import yaml -import os from pathlib import Path + import numpy as np +import yaml # Add parent directory to path to import local modules sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.config_parser import load_ik_config + # Only import the fixed hardware constants that are left in configs.py from common.configs import ( - CAMERA_NAMES, META_QUEST_AXIS_MASK, URDF_PATH, VISUALIZATION_RATE + CAMERA_NAMES, + META_QUEST_AXIS_MASK, + URDF_PATH, + VISUALIZATION_RATE, ) from common.data_manager import RobotActivityState -from common.system_bootstrap import bootstrap_robot_system -from common.shared_actions import toggle_robot_enabled, move_robot_home from common.robot_visualizer import RobotVisualizer +from common.shared_actions import move_robot_home, toggle_robot_enabled +from common.system_bootstrap import bootstrap_robot_system from common.threads.quest_reader import quest_reader_thread -from common.config_parser import load_ik_config from meta_quest_teleop.reader import MetaQuestReader -from piper_controller import PiperController +from piper_controller import PiperController # --------------------------------------------------------------------------- # Dynamic Action Callbacks # --------------------------------------------------------------------------- -def _step_wrist_joint(data_manager, robot_controller, visualizer, direction: int) -> None: + +def _step_wrist_joint( + data_manager, robot_controller, visualizer, direction: int +) -> None: """Apply a relative manual step to the wrist joint using the dynamically tuned step degree.""" data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) target_joint_angles = robot_controller.get_target_joint_angles() current_joint_angles = data_manager.get_current_joint_angles() - - base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1]) - + + base_wrist = ( + float(current_joint_angles[-1]) + if current_joint_angles is not None + else float(target_joint_angles[-1]) + ) + # Read the tuned value directly from the UI and apply direction multiplier (1 or -1) delta_degrees = visualizer.get_wrist_step_degrees() * direction target_joint_angles[-1] = base_wrist + delta_degrees - + robot_controller.set_target_joint_angles(target_joint_angles) data_manager.set_target_joint_angles(target_joint_angles) @@ -61,20 +73,26 @@ def toggle_slow_scaling(data_manager, visualizer): """Toggle the movement scaling mode between standard and dynamically-tuned precision.""" enabled = data_manager.toggle_slow_scaling_mode_enabled() if enabled: - print(f"🐢 Slow scaling enabled (trans={visualizer.get_slow_translation_scale():.3f}, rot={visualizer.get_slow_rotation_scale():.3f})") + print( + f"🐢 Slow scaling enabled (trans={visualizer.get_slow_translation_scale():.3f}, rot={visualizer.get_slow_rotation_scale():.3f})" + ) else: - print(f"🐇 Slow scaling disabled (trans={visualizer.get_translation_scale():.3f}, rot={visualizer.get_rotation_scale():.3f})") + print( + f"🐇 Slow scaling disabled (trans={visualizer.get_translation_scale():.3f}, rot={visualizer.get_rotation_scale():.3f})" + ) -def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml") -> None: +def save_config_to_yaml( + visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" +) -> None: """Extracts all current UI slider values and saves them to a YAML file.""" try: # Ensure the directory exists before saving os.makedirs(os.path.dirname(filepath), exist_ok=True) - + pink_params = visualizer.get_pink_parameters() min_c, beta, d_c = visualizer.get_controller_filter_params() - + # Cast to standard python floats/lists so PyYAML doesn't choke on numpy types config_dict = { "ik_parameters": { @@ -94,15 +112,21 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" "teleop_parameters": { "translation_scale": float(visualizer.get_translation_scale()), "rotation_scale": float(visualizer.get_rotation_scale()), - "slow_translation_scale": float(visualizer.get_slow_translation_scale()), + "slow_translation_scale": float( + visualizer.get_slow_translation_scale() + ), "slow_rotation_scale": float(visualizer.get_slow_rotation_scale()), - "wrist_joint_button_step_degrees": float(visualizer.get_wrist_step_degrees()), - } + "wrist_joint_button_step_degrees": float( + visualizer.get_wrist_step_degrees() + ), + }, } - - with open(filepath, 'w') as file: + + with open(filepath, "w") as file: yaml.dump(config_dict, file, default_flow_style=False, sort_keys=False) - print(f"\n✅ SUCCESS: Configuration securely saved to {os.path.abspath(filepath)}") + print( + f"\n✅ SUCCESS: Configuration securely saved to {os.path.abspath(filepath)}" + ) except Exception as e: print(f"\n❌ ERROR: Failed to save YAML config: {e}") @@ -114,12 +138,16 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" if __name__ == "__main__": parser = argparse.ArgumentParser(description="Piper Robot Teleoperation Tuning") parser.add_argument("--ip-address", type=str, default=None) - parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.") + parser.add_argument( + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to IK/teleop YAML config.", + ) args = parser.parse_args() - print("=" * 60 + "\nPIPER ROBOT TELEOPERATION TUNING\n" + "=" * 60) - + # Load the YAML dict config = load_ik_config(args.ik_config) ik_p = config.get("ik_parameters", {}) @@ -127,16 +155,20 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" tele_p = config.get("teleop_parameters", {}) # 1. Bootstrap Core System - data_manager, robot_controller, ik_solver, active_threads = \ - bootstrap_robot_system(config, start_ik=True, start_camera=True) + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + config, start_ik=True, start_camera=True + ) # 2. Initialize Quest Reader & Handle ADB Exceptions print("\n🎮 Initializing Meta Quest reader...") try: quest_reader = MetaQuestReader( - ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ip_address=args.ip_address, + port=5555, + run=True, + axis_mask=META_QUEST_AXIS_MASK, ) - except (Exception, SystemExit) as e: + except (Exception, SystemExit): print("\n" + "!" * 60) print("❌ FAILED TO ACCESS META QUEST") print("!" * 60) @@ -144,16 +176,18 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" print("\nPLEASE FOLLOW THESE STEPS:") print(" 1. Put the Meta Quest headset on your head.") print(" 2. Look for a notification in your menu that says 'USB Detected'.") - print(" 3. Click on that notification and select 'Allow' to grant data access.") + print( + " 3. Click on that notification and select 'Allow' to grant data access." + ) print(" 4. Rerun this script.") print("!" * 60 + "\n") - + data_manager.request_shutdown() for t in active_threads: t.join(timeout=1.0) robot_controller.cleanup() sys.exit(1) - + # 3. Initialize Visualizer Web UI print("\n🖥️ Starting visualization...") visualizer = RobotVisualizer(urdf_path=URDF_PATH) @@ -165,42 +199,79 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" visualizer.add_rgb_image_placeholder() visualizer.add_target_frame_visualization() visualizer.add_controller_visualization() - + # Add tuning sliders visualizer.add_controller_filter_controls( - filt_p.get("controller_min_cutoff", 0.8), filt_p.get("controller_beta", 0.05), filt_p.get("controller_d_cutoff", 0.9) + filt_p.get("controller_min_cutoff", 0.8), + filt_p.get("controller_beta", 0.05), + filt_p.get("controller_d_cutoff", 0.9), ) visualizer.add_scaling_controls( tele_p.get("translation_scale", 1.5), tele_p.get("rotation_scale", 1.2) ) visualizer.add_pink_parameter_controls( - ik_p.get("position_cost", 1.0), ik_p.get("orientation_cost", 0.75), ik_p.get("frame_task_gain", 0.4), - ik_p.get("lm_damping", 0.01), ik_p.get("damping_cost", 0.25), ik_p.get("solver_damping_value", 1e-4), - ik_p.get("posture_cost_vector", [0.0, 0.0, 0.0, 0.05, 0.0, 0.0]) + ik_p.get("position_cost", 1.0), + ik_p.get("orientation_cost", 0.75), + ik_p.get("frame_task_gain", 0.4), + ik_p.get("lm_damping", 0.01), + ik_p.get("damping_cost", 0.25), + ik_p.get("solver_damping_value", 1e-4), + ik_p.get("posture_cost_vector", [0.0, 0.0, 0.0, 0.05, 0.0, 0.0]), ) visualizer.add_advanced_tuning_controls( - tele_p.get("slow_translation_scale", 0.6), tele_p.get("slow_rotation_scale", 0.6), tele_p.get("wrist_joint_button_step_degrees", 5.0) + tele_p.get("slow_translation_scale", 0.6), + tele_p.get("slow_rotation_scale", 0.6), + tele_p.get("wrist_joint_button_step_degrees", 5.0), ) # 4. Bind Shared Callbacks (Quest Buttons & GUI Buttons) - quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) - quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) - quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, visualizer, direction=1)) - quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, visualizer, direction=-1)) - quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager, visualizer)) - - visualizer.set_toggle_robot_enabled_status_callback(lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer)) - visualizer.set_go_home_callback(lambda: move_robot_home(data_manager, robot_controller)) - + quest_reader.on( + "button_a_pressed", + lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer), + ) + quest_reader.on( + "button_b_pressed", lambda: move_robot_home(data_manager, robot_controller) + ) + quest_reader.on( + "button_y_pressed", + lambda: _step_wrist_joint( + data_manager, robot_controller, visualizer, direction=1 + ), + ) + quest_reader.on( + "button_x_pressed", + lambda: _step_wrist_joint( + data_manager, robot_controller, visualizer, direction=-1 + ), + ) + quest_reader.on( + "button_lj_pressed", lambda: toggle_slow_scaling(data_manager, visualizer) + ) + + visualizer.set_toggle_robot_enabled_status_callback( + lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer) + ) + visualizer.set_go_home_callback( + lambda: move_robot_home(data_manager, robot_controller) + ) + # Safely bind the save config callback with the correct arguments - visualizer.set_save_config_callback(lambda: save_config_to_yaml(visualizer, filepath='ik_conf/tuned_teleop_configs.yaml')) + visualizer.set_save_config_callback( + lambda: save_config_to_yaml( + visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" + ) + ) # 5. Start Quest Thread - quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) + quest_thread = threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True + ) quest_thread.start() active_threads.append(quest_thread) - print("\n🚀 Ready! Use Meta Quest controllers to move the robot. Open http://localhost:8080 to tune. Press Ctrl+C to exit.\n") + print( + "\n🚀 Ready! Use Meta Quest controllers to move the robot. Open http://localhost:8080 to tune. Press Ctrl+C to exit.\n" + ) # 6. Main Visualization Loop dt = 1.0 / VISUALIZATION_RATE @@ -209,12 +280,22 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" iteration_start = time.time() # Poll GUI for real-time scale tuning updates - t_scale = visualizer.get_slow_translation_scale() if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_translation_scale() - r_scale = visualizer.get_slow_rotation_scale() if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_rotation_scale() + t_scale = ( + visualizer.get_slow_translation_scale() + if data_manager.get_slow_scaling_mode_enabled() + else visualizer.get_translation_scale() + ) + r_scale = ( + visualizer.get_slow_rotation_scale() + if data_manager.get_slow_scaling_mode_enabled() + else visualizer.get_rotation_scale() + ) data_manager.set_teleop_scaling(t_scale, r_scale) # Retrieve State Data - controller_transform, grip_value, trigger_value = data_manager.get_controller_data() + controller_transform, grip_value, trigger_value = ( + data_manager.get_controller_data() + ) teleop_active = data_manager.get_teleop_active() state = data_manager.get_robot_activity_state() current_joints = data_manager.get_current_joint_angles() @@ -227,13 +308,17 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" visualizer.update_timing(data_manager.get_ik_solve_time_ms()) visualizer.update_controller_visualization(controller_transform) visualizer.update_controller_status_display( - controller_transform[:3, 3] if controller_transform is not None else None, - connected=controller_transform is not None + ( + controller_transform[:3, 3] + if controller_transform is not None + else None + ), + connected=controller_transform is not None, ) visualizer.update_teleop_status(teleop_active) visualizer.update_target_visualization(data_manager.get_target_pose()) visualizer.update_rgb_image(rgb_image) - + # Render Actual Robot if current_joints is not None: rad_joints = np.radians(current_joints) @@ -248,7 +333,9 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" visualizer.update_ghost_robot_visibility(False) visualizer.update_robot_status(f"Robot Status: {state.value.capitalize()}") - visualizer.update_gripper_status(trigger_value, robot_enabled=(state == RobotActivityState.ENABLED)) + visualizer.update_gripper_status( + trigger_value, robot_enabled=(state == RobotActivityState.ENABLED) + ) # Maintain UI refresh rate time.sleep(max(0, dt - (time.time() - iteration_start))) @@ -264,8 +351,8 @@ def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) quest_reader.stop() - for t in active_threads: + for t in active_threads: t.join(timeout=2.0) robot_controller.cleanup() visualizer.stop() - print("👋 Demo stopped.") \ No newline at end of file + print("👋 Demo stopped.") diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 11ac518..cdc560e 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -3,7 +3,7 @@ Piper Robot Teleoperation and Neuracore Data Collection. This script facilitates real-time teleoperation of the AgileX Piper robotic arm -using Meta Quest controllers, while simultaneously streaming perfectly synchronized +using Meta Quest controllers, while simultaneously streaming perfectly synchronized telemetry data (joint states, RGB camera frames, gripper states) to the Neuracore cloud. Key Features: @@ -28,10 +28,8 @@ import sys import threading import time -import traceback from pathlib import Path -import numpy as np import neuracore as nc # --------------------------------------------------------------------------- @@ -40,23 +38,29 @@ # Append parent directory to sys.path to resolve local 'common' modules. sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import META_QUEST_AXIS_MASK, URDF_PATH from common.config_parser import load_ik_config -from common.data_manager import RobotActivityState, DataManager -from common.system_bootstrap import bootstrap_robot_system +from common.configs import META_QUEST_AXIS_MASK, URDF_PATH +from common.data_manager import DataManager, RobotActivityState from common.shared_actions import ( - toggle_robot_enabled, move_robot_home, - toggle_recording, neuracore_logging_callback + move_robot_home, + neuracore_logging_callback, + toggle_recording, + toggle_robot_enabled, ) +from common.system_bootstrap import bootstrap_robot_system from common.threads.quest_reader import quest_reader_thread from meta_quest_teleop.reader import MetaQuestReader + from piper_controller import PiperController # --------------------------------------------------------------------------- # Helper Functions # --------------------------------------------------------------------------- -def _step_wrist_joint(data_manager: DataManager, robot_controller: PiperController, delta_degrees: float) -> None: + +def _step_wrist_joint( + data_manager: DataManager, robot_controller: PiperController, delta_degrees: float +) -> None: """ Applies a relative step adjustment to the robot's wrist joint target angle. """ @@ -65,9 +69,13 @@ def _step_wrist_joint(data_manager: DataManager, robot_controller: PiperControll target_joint_angles = robot_controller.get_target_joint_angles() current_joint_angles = data_manager.get_current_joint_angles() - + # Fallback to target angles if hardware telemetry is temporarily unavailable - base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1]) + base_wrist = ( + float(current_joint_angles[-1]) + if current_joint_angles is not None + else float(target_joint_angles[-1]) + ) target_joint_angles[-1] = base_wrist + delta_degrees robot_controller.set_target_joint_angles(target_joint_angles) @@ -81,14 +89,13 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: enabled = data_manager.toggle_slow_scaling_mode_enabled() if enabled: data_manager.set_teleop_scaling( - tele_p.get("slow_translation_scale", 0.6), - tele_p.get("slow_rotation_scale", 0.6) + tele_p.get("slow_translation_scale", 0.6), + tele_p.get("slow_rotation_scale", 0.6), ) print("🐢 Slow scaling (precision mode) enabled") else: data_manager.set_teleop_scaling( - tele_p.get("translation_scale", 1.5), - tele_p.get("rotation_scale", 1.2) + tele_p.get("translation_scale", 1.5), tele_p.get("rotation_scale", 1.2) ) print("🐇 Slow scaling disabled (standard mode)") @@ -96,15 +103,32 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: if __name__ == "__main__": # Ensure safe multiprocess spawning for UI/Background threads across different OS environments multiprocessing.set_start_method("spawn") - - parser = argparse.ArgumentParser(description="Teleop with Neuracore Data Collection") - parser.add_argument("--ip-address", type=str, default=None, help="IP address of the Meta Quest headset.") - parser.add_argument("--dataset-name", type=str, default=None, help="Override the auto-generated dataset name.") - parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.") + + parser = argparse.ArgumentParser( + description="Teleop with Neuracore Data Collection" + ) + parser.add_argument( + "--ip-address", + type=str, + default=None, + help="IP address of the Meta Quest headset.", + ) + parser.add_argument( + "--dataset-name", + type=str, + default=None, + help="Override the auto-generated dataset name.", + ) + parser.add_argument( + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to IK/teleop YAML config.", + ) args = parser.parse_args() print("=" * 60 + "\nPIPER TELEOP DATA COLLECTION\n" + "=" * 60) - + # Load the YAML configuration dictionary config = load_ik_config(args.ik_config) tele_p = config.get("teleop_parameters", {}) @@ -115,11 +139,17 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: # --------------------------------------------------------- print("\n🔧 Initializing Neuracore...") nc.login() - nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) - + nc.connect_robot( + robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False + ) + # Auto-generate a timestamped dataset name if one was not provided via CLI - dataset_name = args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" - nc.create_dataset(name=dataset_name, description="Teleop data collection for Piper robot") + dataset_name = ( + args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}" + ) + nc.create_dataset( + name=dataset_name, description="Teleop data collection for Piper robot" + ) # --------------------------------------------------------- # 2. Hardware & Subsystem Bootstrapping @@ -127,7 +157,7 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( config, start_ik=True, start_camera=True ) - + # Bind the unified Neuracore logger so state changes automatically push to the cloud data_manager.set_on_change_callback(neuracore_logging_callback) @@ -137,9 +167,12 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: print("\n🎮 Initializing Meta Quest reader...") try: quest_reader = MetaQuestReader( - ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ip_address=args.ip_address, + port=5555, + run=True, + axis_mask=META_QUEST_AXIS_MASK, ) - except (Exception, SystemExit) as e: + except (Exception, SystemExit): print("\n" + "!" * 60) print("❌ FAILED TO ACCESS META QUEST") print("!" * 60) @@ -147,10 +180,12 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: print("\nPLEASE FOLLOW THESE STEPS:") print(" 1. Put the Meta Quest headset on your head.") print(" 2. Look for a notification in your menu that says 'USB Detected'.") - print(" 3. Click on that notification and select 'Allow' to grant data access.") + print( + " 3. Click on that notification and select 'Allow' to grant data access." + ) print(" 4. Rerun this script.") print("!" * 60 + "\n") - + data_manager.request_shutdown() for t in active_threads: t.join(timeout=1.0) @@ -161,18 +196,34 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: # --------------------------------------------------------- # 4. Input Binding & Background Threading # --------------------------------------------------------- - quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller)) - quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller)) + quest_reader.on( + "button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller) + ) + quest_reader.on( + "button_b_pressed", lambda: move_robot_home(data_manager, robot_controller) + ) quest_reader.on("button_rj_pressed", lambda: toggle_recording(play_audio=True)) - quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, wrist_step)) - quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -wrist_step)) - quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager, tele_p)) + quest_reader.on( + "button_y_pressed", + lambda: _step_wrist_joint(data_manager, robot_controller, wrist_step), + ) + quest_reader.on( + "button_x_pressed", + lambda: _step_wrist_joint(data_manager, robot_controller, -wrist_step), + ) + quest_reader.on( + "button_lj_pressed", lambda: toggle_slow_scaling(data_manager, tele_p) + ) - quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) + quest_thread = threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True + ) quest_thread.start() active_threads.append(quest_thread) - print("\n🚀 Ready! Use Meta Quest controllers. Press Right Joystick to Record. Press Ctrl+C to exit.\n") + print( + "\n🚀 Ready! Use Meta Quest controllers. Press Right Joystick to Record. Press Ctrl+C to exit.\n" + ) # --------------------------------------------------------- # 5. Main Daemon Loop @@ -180,7 +231,7 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: try: while not data_manager.is_shutdown_requested(): time.sleep(1) - + except KeyboardInterrupt: print("\n👋 Interrupt received - shutting down gracefully...") data_manager.request_shutdown() @@ -192,16 +243,16 @@ def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: # 6. Graceful Teardown & Cleanup # --------------------------------------------------------- print("\n🧹 Cleaning up subsystems...") - + if nc.is_recording(): nc.cancel_recording() - + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) quest_reader.stop() - - for t in active_threads: + + for t in active_threads: t.join(timeout=2.0) - + nc.logout() robot_controller.cleanup() - print("👋 Demo stopped.") \ No newline at end of file + print("👋 Demo stopped.") diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index 3a5cdc3..473e91f 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -7,15 +7,15 @@ ============================================================================= ⚠️ HALID'S SAFETY NOTES & OPERATIONAL WARNINGS: - 1. DANGER: This step can be dangerous. The robot will start moving on the + 1. DANGER: This step can be dangerous. The robot will start moving on the exact trajectory that was recorded. ALWAYS be ready to press the emergency stop. - 2. EPISODE INDEXING: The episode index starts at 0, not 1. If you see an + 2. EPISODE INDEXING: The episode index starts at 0, not 1. If you see an episode numbered 'X' in the Neuracore frontend, you must run index 'X-1'. - 3. RERUN INSTABILITY: You may experience issues running this command successfully - more than once in a row without a system reset. - 4. FREQUENCY: The default playback frequency should be 20 Hz. Setting the + 3. RERUN INSTABILITY: You may experience issues running this command successfully + more than once in a row without a system reset. + 4. FREQUENCY: The default playback frequency should be 20 Hz. Setting the frequency to 0 plays the data aperiodically (as it was recorded). - 5. EMERGENCY STOP: Pressing Ctrl+C will gracefully disable the robot and cut + 5. EMERGENCY STOP: Pressing Ctrl+C will gracefully disable the robot and cut power to the motors after 5 seconds. ============================================================================= @@ -25,10 +25,10 @@ Usage Examples: # Play the first episode (index 0) at the default 20 Hz: python 3_replay_neuracore_episodes.py --dataset-name my_dataset - + # Play a specific episode index (e.g., frontend episode 3 -> index 2): python 3_replay_neuracore_episodes.py --dataset-name my_dataset --episode-index 2 - + # Play aperiodically (as recorded): python 3_replay_neuracore_episodes.py --dataset-name my_dataset --frequency 0 """ @@ -53,8 +53,10 @@ from common.configs import GRIPPER_NAME, JOINT_NAMES, NEUTRAL_JOINT_ANGLES, ROBOT_RATE from common.dataset_helpers import load_and_sync_dataset from neuracore_types import DataType, SynchronizedPoint + from piper_controller import PiperController + # --------------------------------------------------------------------------- # Helper Functions # --------------------------------------------------------------------------- @@ -69,10 +71,11 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: return True print(f" Waiting for robot to home... ({remaining}s remaining)", end="\r") time.sleep(1) - + print("\n❌ Error: Robot did not reach home position within the timeout.") return False + # --------------------------------------------------------------------------- # Main Execution # --------------------------------------------------------------------------- @@ -80,18 +83,26 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: # --------------------------------------------------------- # 1. Argument Parsing # --------------------------------------------------------- - parser = argparse.ArgumentParser(description="Replay Neuracore datasets on the Piper robot.") + parser = argparse.ArgumentParser( + description="Replay Neuracore datasets on the Piper robot." + ) parser.add_argument( - "--dataset-name", type=str, required=True, - help="Name of the Neuracore dataset to load." + "--dataset-name", + type=str, + required=True, + help="Name of the Neuracore dataset to load.", ) parser.add_argument( - "--frequency", type=int, default=20, - help="Playback frequency in Hz. 0 plays the data aperiodically (default: 20)." + "--frequency", + type=int, + default=20, + help="Playback frequency in Hz. 0 plays the data aperiodically (default: 20).", ) parser.add_argument( - "--episode-index", type=int, default=0, - help="Episode index to replay (Frontend Number - 1). -1 plays all sequentially (default: 0)." + "--episode-index", + type=int, + default=0, + help="Episode index to replay (Frontend Number - 1). -1 plays all sequentially (default: 0).", ) args = parser.parse_args() @@ -111,10 +122,10 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: # --------------------------------------------------------- print("\n🤖 Initializing Piper robot controller...") robot_controller = PiperController( - can_interface="can0", + can_interface="can0", robot_rate=ROBOT_RATE, control_mode=PiperController.ControlMode.JOINT_SPACE, - neutral_joint_angles=NEUTRAL_JOINT_ANGLES, + neutral_joint_angles=NEUTRAL_JOINT_ANGLES, debug_mode=False, ) robot_controller.start_control_loop() @@ -125,14 +136,18 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: print("\n🔑 Logging in to Neuracore...") nc.login() - input_mods = [DataType.JOINT_POSITIONS, DataType.RGB_IMAGES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] - output_mods = [DataType.JOINT_TARGET_POSITIONS, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - + input_mods = [ + DataType.JOINT_POSITIONS, + DataType.RGB_IMAGES, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, + ] + output_mods = [ + DataType.JOINT_TARGET_POSITIONS, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, + ] + synced_dataset = load_and_sync_dataset( - args.dataset_name, - args.frequency, - input_mods, - output_mods + args.dataset_name, args.frequency, input_mods, output_mods ) if len(synced_dataset) == 0: @@ -146,12 +161,16 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: print(f"\n📊 Found {len(synced_dataset)} episodes. Will play all sequentially.") else: if args.episode_index >= len(synced_dataset) or args.episode_index < 0: - print(f"❌ Error: Index {args.episode_index} is out of bounds (Dataset has {len(synced_dataset)} episodes).") - print("💡 Reminder: If the frontend says episode '3', you must request index '2'.") + print( + f"❌ Error: Index {args.episode_index} is out of bounds (Dataset has {len(synced_dataset)} episodes)." + ) + print( + "💡 Reminder: If the frontend says episode '3', you must request index '2'." + ) robot_controller.stop_control_loop() robot_controller.cleanup() sys.exit(1) - + episode_indices = [args.episode_index] print(f"\n📊 Playing episode {args.episode_index} only.") @@ -161,7 +180,7 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: try: for episode_idx in episode_indices: episode = synced_dataset[episode_idx] - + if len(episode) == 0: print(f"⚠️ Warning: Episode {episode_idx} is empty. Skipping...") continue @@ -170,7 +189,9 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: print("⚠️ Skipping episode due to homing failure.") continue - print(f"\n{'='*60}\n🎬 Extracting Episode {episode_idx} / {len(synced_dataset) - 1}\n{'='*60}") + print( + f"\n{'='*60}\n🎬 Extracting Episode {episode_idx} / {len(synced_dataset) - 1}\n{'='*60}" + ) rgb_frames_per_step = [] parallel_gripper_open_amounts = [] @@ -179,7 +200,7 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: # Pre-compute and Unpack for step in tqdm(episode, desc="Unpacking frames into memory"): step = cast(SynchronizedPoint, step) - + j_data = step.data.get(DataType.JOINT_TARGET_POSITIONS, {}) if all(jn in j_data for jn in JOINT_NAMES): joint_positions.append([j_data[jn].value for jn in JOINT_NAMES]) @@ -187,18 +208,24 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: if len(joint_positions) > 0: joint_positions.append(joint_positions[-1]) else: - continue + continue - g_data = step.data.get(DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, {}) + g_data = step.data.get( + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, {} + ) parallel_gripper_open_amounts.append( g_data[GRIPPER_NAME].open_amount if GRIPPER_NAME in g_data else 0.0 ) rgb_data = step.data.get(DataType.RGB_IMAGES, {}) - rgb_frames_per_step.append({c: img.frame for c, img in rgb_data.items()}) + rgb_frames_per_step.append( + {c: img.frame for c, img in rgb_data.items()} + ) if not joint_positions: - print(f"⚠️ Warning: No valid joint data found in episode {episode_idx}. Skipping...") + print( + f"⚠️ Warning: No valid joint data found in episode {episode_idx}. Skipping..." + ) continue joint_positions = np.degrees(np.array(joint_positions)) @@ -207,15 +234,19 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: print(f"\n🚀 Replaying episode {episode_idx} on hardware...") for idx in tqdm(range(len(joint_positions)), desc="Replaying"): start_time = time.time() - + robot_controller.set_target_joint_angles(joint_positions[idx]) - robot_controller.set_gripper_open_value(parallel_gripper_open_amounts[idx]) + robot_controller.set_gripper_open_value( + parallel_gripper_open_amounts[idx] + ) if idx < len(rgb_frames_per_step): for cam_name, frame_rgb in rgb_frames_per_step[idx].items(): - frame_bgr = cv2.cvtColor(np.asarray(frame_rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR) + frame_bgr = cv2.cvtColor( + np.asarray(frame_rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR + ) cv2.imshow(f"Replay: {cam_name}", frame_bgr) - + if cv2.waitKey(1) & 0xFF == ord("q"): print("\n🛑 'q' pressed, skipping remainder of replay...") break @@ -224,9 +255,9 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: if args.frequency > 0: elapsed = time.time() - start_time time.sleep(max(0.0, (1.0 / args.frequency) - elapsed)) - + cv2.destroyAllWindows() - + except KeyboardInterrupt: print("\n🛑 Keyboard interrupt detected. Halting playback...") print("⚠️ Gracefully disabling the robot. Power to motors will be cut off.") @@ -242,4 +273,4 @@ def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: # Triggering the cleanup gracefully disables the robot (cutting power securely) robot_controller.stop_control_loop() robot_controller.cleanup() - print("👋 Replay complete.") \ No newline at end of file + print("👋 Replay complete.") diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 2c089e0..171982d 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -28,28 +28,33 @@ # Dynamically append the parent directory to sys.path to resolve local 'common' modules. sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.config_parser import load_ik_config from common.configs import ( - CAMERA_NAMES, GRIPPER_NAME, URDF_PATH, - PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, ROBOT_RATE, + CAMERA_NAMES, + GRIPPER_NAME, + POLICY_EXECUTION_RATE, + PREDICTION_HORIZON_EXECUTION_RATIO, + ROBOT_RATE, + URDF_PATH, ) -from neuracore_types import DataType, EmbodimentDescription - from common.data_manager import RobotActivityState -from common.policy_state import PolicyState -from common.policy_helpers import ( - embodiment_names_ordered, - get_policy_embodiments, - print_policy_embodiments -) -from common.config_parser import load_ik_config -from common.system_bootstrap import bootstrap_robot_system -from common.shared_actions import toggle_robot_enabled, move_robot_home -from common.robot_visualizer import RobotVisualizer # Extracted policy execution and lifecycle management actions from common.policy_actions import ( - run_policy, start_policy_execution, play_policy, policy_execution_thread + play_policy, + policy_execution_thread, + run_policy, + start_policy_execution, ) +from common.policy_helpers import ( + get_policy_embodiments, + print_policy_embodiments, +) +from common.policy_state import PolicyState +from common.robot_visualizer import RobotVisualizer +from common.shared_actions import move_robot_home, toggle_robot_enabled +from common.system_bootstrap import bootstrap_robot_system +from neuracore_types import DataType if __name__ == "__main__": # --------------------------------------------------------- @@ -58,32 +63,41 @@ parser = argparse.ArgumentParser( description="Execute and visualize Neuracore policies on the Piper Robot." ) - + parser.add_argument( - "--continuous-mode", - choices=["pipeline", "sequential"], + "--continuous-mode", + choices=["pipeline", "sequential"], default="sequential", - help="Execution strategy for the receding horizon loop." + help="Execution strategy for the receding horizon loop.", ) parser.add_argument( - "--robot-name", - type=str, + "--robot-name", + type=str, default="AgileX PiPER", - help="The registered hardware name in the Neuracore ecosystem." + help="The registered hardware name in the Neuracore ecosystem.", ) parser.add_argument( - "--ik-config", - type=str, - default="ik_conf/default.yaml", - help="Path to IK/teleop YAML configuration file." + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to IK/teleop YAML configuration file.", ) - + # Require exactly one method of loading the policy policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.") - policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.") - policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.") - + policy_group.add_argument( + "--train-run-name", type=str, default=None, help="Cloud training run name." + ) + policy_group.add_argument( + "--model-path", type=str, default=None, help="Path to local .nc.zip model file." + ) + policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Active remote inference endpoint.", + ) + args = parser.parse_args() print("=" * 60 + "\nPIPER ROBOT TEST WITH NEURACORE POLICY\n" + "=" * 60) @@ -95,13 +109,17 @@ config = load_ik_config(args.ik_config) nc.login() - nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + nc.connect_robot( + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False + ) # --------------------------------------------------------- # 3. Policy Loading & Embodiment Resolution # --------------------------------------------------------- if args.remote_endpoint_name is not None: - print(f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") + print( + f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." + ) try: policy = nc.policy_remote_server(args.remote_endpoint_name) except nc.EndpointError: @@ -110,7 +128,7 @@ "Please start it from the Neuracore dashboard." ) sys.exit(1) - + elif args.train_run_name is not None: print(f"\n🤖 Loading policy from cloud training run: {args.train_run_name}...") policy = nc.policy( @@ -118,7 +136,7 @@ device="cuda", robot_name=args.robot_name, ) - + else: print(f"\n🤖 Loading policy from local model: {args.model_path}...") policy = nc.policy( @@ -128,12 +146,14 @@ ) # Dynamically extract what sensor streams the model expects to see. - # Remote endpoints (policy_remote_server) do not expose these attributes, + # Remote endpoints (policy_remote_server) do not expose these attributes, # so we catch the AttributeError and fall back to the default Piper embodiment. try: input_emb, output_emb = get_policy_embodiments(policy) except AttributeError: - print("\n⚠️ Could not dynamically extract embodiments from remote endpoint. Using default Piper configuration...") + print( + "\n⚠️ Could not dynamically extract embodiments from remote endpoint. Using default Piper configuration..." + ) input_emb = { DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, @@ -153,7 +173,7 @@ # --------------------------------------------------------- # 4. Hardware & Subsystem Bootstrapping # --------------------------------------------------------- - # Instantiates the shared DataManager, CAN hardware interface, IK solver, + # Instantiates the shared DataManager, CAN hardware interface, IK solver, # and base telemetry threads using the parsed YAML config parameters. data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( config, start_ik=True, start_camera=True @@ -164,13 +184,13 @@ # --------------------------------------------------------- print("\n🖥️ Starting Viser visualization server...") visualizer = RobotVisualizer(str(URDF_PATH)) - + # Inject UI components using fixed AI policy rates visualizer.add_policy_controls( - PREDICTION_HORIZON_EXECUTION_RATIO, - POLICY_EXECUTION_RATE, - ROBOT_RATE, - "targeting_time" + PREDICTION_HORIZON_EXECUTION_RATIO, + POLICY_EXECUTION_RATE, + ROBOT_RATE, + "targeting_time", ) visualizer.add_toggle_robot_enabled_status_button() visualizer.add_homing_controls() @@ -192,10 +212,19 @@ lambda: start_policy_execution(data_manager, policy_state) ) visualizer.set_play_policy_callback( - lambda: play_policy(data_manager, policy, policy_state, visualizer, input_emb, args.continuous_mode) + lambda: play_policy( + data_manager, + policy, + policy_state, + visualizer, + input_emb, + args.continuous_mode, + ) ) visualizer.set_execution_mode_callback( - lambda: policy_state.set_execution_mode(PolicyState.ExecutionMode(visualizer.get_execution_mode())) + lambda: policy_state.set_execution_mode( + PolicyState.ExecutionMode(visualizer.get_execution_mode()) + ) ) # --------------------------------------------------------- @@ -203,14 +232,23 @@ # --------------------------------------------------------- policy_exec_thread = threading.Thread( target=policy_execution_thread, - args=(policy, data_manager, policy_state, robot_controller, visualizer, input_emb), + args=( + policy, + data_manager, + policy_state, + robot_controller, + visualizer, + input_emb, + ), daemon=True, - name="PolicyExecutionWorker" + name="PolicyExecutionWorker", ) policy_exec_thread.start() active_threads.append(policy_exec_thread) - print("\n🚀 System Online! Open http://localhost:8080 in your browser to visualize and run the policy.\n") + print( + "\n🚀 System Online! Open http://localhost:8080 in your browser to visualize and run the policy.\n" + ) # --------------------------------------------------------- # 7. Main Daemon Loop @@ -219,7 +257,7 @@ # Keep the main thread alive. All heavy lifting is handled via background threads. while True: time.sleep(1) - + except KeyboardInterrupt: print("\n👋 Interrupt signal received. Initiating graceful shutdown...") except Exception as e: @@ -230,21 +268,21 @@ # 8. Graceful Teardown & Cleanup # --------------------------------------------------------- print("\n🧹 Cleaning up subsystems...") - + # Safely sever the connection to the policy backend policy.disconnect() - + # Broadcast shutdown signal to all worker threads data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - + # Wait for daemon threads to finish their current execution cycles for thread in active_threads: thread.join(timeout=2.0) - + # Relinquish hardware interfaces and network ports robot_controller.cleanup() visualizer.stop() nc.logout() - - print("👋 Shutdown complete. Goodbye.") \ No newline at end of file + + print("👋 Shutdown complete. Goodbye.") diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 67cb266..4a11792 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -32,31 +32,44 @@ # --------------------------------------------------------------------------- sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.config_parser import load_ik_config from common.configs import ( - CAMERA_NAMES, GRIPPER_NAME, URDF_PATH, - POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO + CAMERA_NAMES, + GRIPPER_NAME, + POLICY_EXECUTION_RATE, + PREDICTION_HORIZON_EXECUTION_RATIO, + URDF_PATH, ) -from neuracore_types import DataType - from common.data_manager import RobotActivityState +from common.policy_actions import run_policy from common.policy_helpers import ( - get_policy_embodiments, print_policy_embodiments, - embodiment_names_ordered, gripper_open_at_index, - joint_targets_deg_at_index, log_robot_state_for_policy + embodiment_names_ordered, + get_policy_embodiments, + gripper_open_at_index, + joint_targets_deg_at_index, + log_robot_state_for_policy, + print_policy_embodiments, ) from common.policy_state import PolicyState -from common.config_parser import load_ik_config from common.system_bootstrap import bootstrap_robot_system -from common.policy_actions import run_policy +from neuracore_types import DataType + # --------------------------------------------------------------------------- # Helper Functions # --------------------------------------------------------------------------- -def execute_horizon(data_manager, policy_state, robot_controller, frequency, input_embodiment, output_grippers): +def execute_horizon( + data_manager, + policy_state, + robot_controller, + frequency, + input_embodiment, + output_grippers, +): """ Minimal terminal-only execution loop for the prediction horizon. - - Locks the predicted horizon and streams the joint/gripper targets to the + + Locks the predicted horizon and streams the joint/gripper targets to the robot at the specified frequency until the horizon is exhausted. """ policy_state.start_policy_execution() @@ -75,34 +88,53 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp robot_controller.set_target_joint_angles(joint_targets_deg) # Dispatch gripper targets - gripper_target = gripper_open_at_index(locked_horizon, i, gripper_names=output_grippers) + gripper_target = gripper_open_at_index( + locked_horizon, i, gripper_names=output_grippers + ) if gripper_target is not None: robot_controller.set_gripper_open_value(gripper_target) # Log state back to the policy for the next inference cycle log_robot_state_for_policy(data_manager, input_embodiment) - + # Maintain loop frequency time.sleep(max(0.0, dt - (time.time() - start_time))) policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + # --------------------------------------------------------------------------- # Main Execution # --------------------------------------------------------------------------- if __name__ == "__main__": parser = argparse.ArgumentParser(description="Minimal Piper Policy Test") - + policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.") - policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.") - policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.") - + policy_group.add_argument( + "--train-run-name", type=str, default=None, help="Cloud training run name." + ) + policy_group.add_argument( + "--model-path", type=str, default=None, help="Path to local .nc.zip model file." + ) + policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Active remote inference endpoint.", + ) + parser.add_argument("--robot-name", type=str, default="AgileX PiPER") parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE) - parser.add_argument("--execution-ratio", type=float, default=PREDICTION_HORIZON_EXECUTION_RATIO) - parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to YAML configuration.") + parser.add_argument( + "--execution-ratio", type=float, default=PREDICTION_HORIZON_EXECUTION_RATIO + ) + parser.add_argument( + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to YAML configuration.", + ) args = parser.parse_args() print("=" * 60 + "\nPIPER POLICY ROLLOUT (MINIMAL)\n" + "=" * 60) @@ -110,39 +142,57 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp # 1. Configuration & Neuracore Connection config = load_ik_config(args.ik_config) nc.login() - nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + nc.connect_robot( + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False + ) # 2. Policy Loading if args.remote_endpoint_name: - print(f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") + print( + f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." + ) try: policy = nc.policy_remote_server(args.remote_endpoint_name) - except Exception as e: + except Exception: # Catch the EndpointError and provide explicit, actionable instructions print("\n" + "!" * 60) print(f"❌ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'") print("!" * 60) - print("The script successfully reached Neuracore, but the remote server is down.") + print( + "The script successfully reached Neuracore, but the remote server is down." + ) print("\nPLEASE FOLLOW THESE STEPS:") print(" 1. Open your browser and go to the Neuracore website/dashboard.") - print(f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'.") + print( + f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'." + ) print(" 3. Click 'Deploy' or 'Activate' to spin up the cloud server.") - print(" 4. Wait for the status to show as 'Active', then rerun this script.") + print( + " 4. Wait for the status to show as 'Active', then rerun this script." + ) print("!" * 60 + "\n") sys.exit(1) - + elif args.train_run_name: print(f"\n🤖 Loading policy from cloud training run: {args.train_run_name}...") - policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name) + policy = nc.policy( + train_run_name=args.train_run_name, + device="cuda", + robot_name=args.robot_name, + ) else: print(f"\n🤖 Loading policy from local model: {args.model_path}...") - policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name) - + policy = nc.policy( + model_file=args.model_path, device="cuda", robot_name=args.robot_name + ) + # 3. Embodiment Extraction with Remote Fallback try: input_emb, output_emb = get_policy_embodiments(policy) except AttributeError: - print("\n⚠️ Could not dynamically extract embodiments. Using default Piper configuration...") + print( + "\n⚠️ Could not dynamically extract embodiments. Using default Piper configuration..." + ) input_emb = { DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, @@ -154,20 +204,22 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp } print_policy_embodiments(input_emb, output_emb) - + output_gripper_names = None if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb: - output_gripper_names = embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) + output_gripper_names = embodiment_names_ordered( + output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] + ) # 4. Bootstrap Core System (No Quest, No IK needed for minimal playback) data_manager, robot_controller, _, active_threads = bootstrap_robot_system( config, start_ik=False, start_camera=True ) - + policy_state = PolicyState() policy_state.set_execution_ratio(args.execution_ratio) - time.sleep(2.0) # Wait for hardware interfaces and threads to initialize + time.sleep(2.0) # Wait for hardware interfaces and threads to initialize # 5. Continuous Execution Loop try: @@ -175,24 +227,39 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp robot_controller.resume_robot() data_manager.set_robot_activity_state(RobotActivityState.HOMING) robot_controller.move_to_home() - - while data_manager.get_robot_activity_state() == RobotActivityState.HOMING and not robot_controller.is_robot_homed(): + + while ( + data_manager.get_robot_activity_state() == RobotActivityState.HOMING + and not robot_controller.is_robot_homed() + ): time.sleep(0.1) - + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("✓ Robot ready. Starting policy execution loop... Press Ctrl+C to stop.\n") + print( + "✓ Robot ready. Starting policy execution loop... Press Ctrl+C to stop.\n" + ) while True: # Query the network for the next prediction - if not run_policy(data_manager, policy, policy_state, visualizer=None, input_embodiment_description=input_emb): + if not run_policy( + data_manager, + policy, + policy_state, + visualizer=None, + input_embodiment_description=input_emb, + ): print("⚠️ Policy inference failed or timed out. Retrying...") time.sleep(0.5) continue - + # Execute the predicted horizon on the hardware execute_horizon( - data_manager, policy_state, robot_controller, - args.frequency, input_emb, output_gripper_names + data_manager, + policy_state, + robot_controller, + args.frequency, + input_emb, + output_gripper_names, ) except KeyboardInterrupt: @@ -200,7 +267,7 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp except Exception as e: print(f"\n❌ Unhandled error during execution: {e}") traceback.print_exc() - + # 6. Graceful Teardown & Cleanup finally: print("\n🧹 Cleaning up subsystems...") @@ -214,20 +281,20 @@ def execute_horizon(data_manager, policy_state, robot_controller, frequency, inp time.sleep(1) except Exception: pass - + policy.disconnect() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) data_manager.request_shutdown() - - for t in active_threads: + + for t in active_threads: t.join(timeout=2.0) - + if robot_controller: try: robot_controller.graceful_stop() except AttributeError: pass robot_controller.cleanup() - + nc.logout() - print("👋 Shutdown complete.") \ No newline at end of file + print("👋 Shutdown complete.") diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index 4ba32cf..e002e3f 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -2,14 +2,14 @@ """ Offline Policy Visualization and Validation. -This script acts as a safe, simulated testing ground for your trained AI policies. +This script acts as a safe, simulated testing ground for your trained AI policies. Instead of running on the physical robot, it: 1. Loads a synchronized teleoperation dataset from Neuracore. 2. Selects a random step (observation) from the dataset. 3. Feeds that real-world camera and joint data into the trained policy. 4. Renders the AI's predicted future action horizon in a 3D Viser web UI. -It is highly recommended to run this script to visually validate a model's sanity +It is highly recommended to run this script to visually validate a model's sanity before deploying it to the physical AgileX Piper arm (via Scripts 4 or 5). Usage Examples: @@ -18,17 +18,17 @@ """ import argparse +import logging import random import sys import time import traceback -import logging from pathlib import Path # --------------------------------------------------------------------------- # Suppress Noisy WebRTC/STUN Networking Errors from Viser # --------------------------------------------------------------------------- -# Viser attempts to use STUN servers for WebRTC streaming. If a firewall/VPN +# Viser attempts to use STUN servers for WebRTC streaming. If a firewall/VPN # blocks it, it throws massive 401 errors before falling back to WebSockets. # This silences those unhandled asyncio exceptions so the terminal stays clean. logging.getLogger("asyncio").setLevel(logging.CRITICAL) @@ -48,29 +48,54 @@ sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH, CAMERA_NAMES, GRIPPER_NAME + CAMERA_NAMES, + GRIPPER_NAME, + JOINT_NAMES, + POLICY_EXECUTION_RATE, + URDF_PATH, ) from common.dataset_helpers import load_and_sync_dataset from common.policy_helpers import ( - DEFAULT_ROBOT_NAME, convert_predictions_to_horizon, embodiment_names_ordered, - get_policy_embodiments, gripper_open_at_index, horizon_length, - log_sync_step_for_policy, print_policy_embodiments, urdf_cfg_from_horizon, + DEFAULT_ROBOT_NAME, + convert_predictions_to_horizon, + embodiment_names_ordered, + get_policy_embodiments, + gripper_open_at_index, + horizon_length, + log_sync_step_for_policy, + print_policy_embodiments, + urdf_cfg_from_horizon, ) from neuracore_types import DataType - if __name__ == "__main__": # --------------------------------------------------------- # 1. Argument Parsing # --------------------------------------------------------- - parser = argparse.ArgumentParser(description="Visualize policy predictions offline using dataset states.") - parser.add_argument("--dataset-name", type=str, required=True, help="Neuracore dataset to draw observations from.") - + parser = argparse.ArgumentParser( + description="Visualize policy predictions offline using dataset states." + ) + parser.add_argument( + "--dataset-name", + type=str, + required=True, + help="Neuracore dataset to draw observations from.", + ) + policy_group = parser.add_mutually_exclusive_group(required=True) - policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.") - policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.") - policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.") - + policy_group.add_argument( + "--train-run-name", type=str, default=None, help="Cloud training run name." + ) + policy_group.add_argument( + "--model-path", type=str, default=None, help="Path to local .nc.zip model file." + ) + policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Active remote inference endpoint.", + ) + parser.add_argument("--robot-name", type=str, default=DEFAULT_ROBOT_NAME) parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE) args = parser.parse_args() @@ -81,31 +106,47 @@ # 2. Neuracore Initialization & Policy Loading # --------------------------------------------------------- nc.login() - nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) + nc.connect_robot( + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False + ) if args.remote_endpoint_name: - print(f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}...") + print( + f"\n🤖 Connecting to remote policy endpoint: {args.remote_endpoint_name}..." + ) try: policy = nc.policy_remote_server(args.remote_endpoint_name) - except Exception as e: + except Exception: print("\n" + "!" * 60) print(f"❌ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'") print("!" * 60) - print("The script successfully reached Neuracore, but the remote server is down.") + print( + "The script successfully reached Neuracore, but the remote server is down." + ) print("\nPLEASE FOLLOW THESE STEPS:") print(" 1. Open your browser and go to the Neuracore website/dashboard.") - print(f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'.") + print( + f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'." + ) print(" 3. Click 'Deploy' or 'Activate' to spin up the cloud server.") - print(" 4. Wait for the status to show as 'Active', then rerun this script.") + print( + " 4. Wait for the status to show as 'Active', then rerun this script." + ) print("!" * 60 + "\n") sys.exit(1) - + elif args.train_run_name: print(f"\n🤖 Loading policy from cloud training run: {args.train_run_name}...") - policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name) + policy = nc.policy( + train_run_name=args.train_run_name, + device="cuda", + robot_name=args.robot_name, + ) else: print(f"\n🤖 Loading policy from local model: {args.model_path}...") - policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name) + policy = nc.policy( + model_file=args.model_path, device="cuda", robot_name=args.robot_name + ) # --------------------------------------------------------- # 3. Embodiment Extraction & Fallback @@ -113,7 +154,9 @@ try: input_emb, output_emb = get_policy_embodiments(policy) except AttributeError: - print("\n⚠️ Could not dynamically extract embodiments. Using default Piper configuration...") + print( + "\n⚠️ Could not dynamically extract embodiments. Using default Piper configuration..." + ) input_emb = { DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)}, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME}, @@ -127,8 +170,10 @@ print_policy_embodiments(input_emb, output_emb) out_grippers = ( - embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]) - if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb + embodiment_names_ordered( + output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] + ) + if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb else None ) @@ -137,11 +182,11 @@ # --------------------------------------------------------- input_mods = list(input_emb.keys()) output_mods = list(output_emb.keys()) if output_emb else [] - + synced_dataset = load_and_sync_dataset( args.dataset_name, args.frequency, input_mods, output_mods, prefetch_videos=True ) - + if len(synced_dataset) == 0: print("❌ Error: The synchronized dataset is empty. Exiting.") sys.exit(1) @@ -152,8 +197,10 @@ print("\n🖥️ Starting Viser simulation environment...") server = viser.ViserServer() server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) - - urdf_vis = ViserUrdf(server, yourdfpy.URDF.load(str(URDF_PATH)), root_node_name="/robot") + + urdf_vis = ViserUrdf( + server, yourdfpy.URDF.load(str(URDF_PATH)), root_node_name="/robot" + ) urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) state = {"horizon": None, "action_idx": 0, "playing": False, "rgb_handle": None} @@ -161,20 +208,22 @@ def select_random_state() -> None: """Pulls a random observation from the dataset and queries the AI policy.""" episode = synced_dataset[random.randint(0, len(synced_dataset) - 1)] - if not len(episode): + if not len(episode): return step = episode[random.randint(0, len(episode) - 1)] - if not log_sync_step_for_policy(step, input_emb): + if not log_sync_step_for_policy(step, input_emb): return rgb_data = getattr(step, "data", {}).get(DataType.RGB_IMAGES, {}) for _, frame in rgb_data.items(): rgb_arr = np.array(frame.frame) Image.fromarray(rgb_arr).save("current_image.png") - + if state["rgb_handle"] is None: - state["rgb_handle"] = server.gui.add_image(rgb_arr, label="RGB Observation", format="jpeg") + state["rgb_handle"] = server.gui.add_image( + rgb_arr, label="RGB Observation", format="jpeg" + ) else: state["rgb_handle"].image = rgb_arr break @@ -185,21 +234,40 @@ def select_random_state() -> None: state["horizon"] = convert_predictions_to_horizon(predictions) state["action_idx"] = 0 state["playing"] = True - + joint_cfg = urdf_cfg_from_horizon(state["horizon"], 0) - if joint_cfg is not None: + if joint_cfg is not None: urdf_vis.update_cfg(joint_cfg) - - print(f"✓ Prediction successful. Horizon length: {horizon_length(state['horizon'])}") + + print( + f"✓ Prediction successful. Horizon length: {horizon_length(state['horizon'])}" + ) except Exception as e: print(f"❌ Failed prediction inference: {e}") - server.gui.add_button("🎲 Pull Random Observation").on_click(lambda _: select_random_state()) - gripper_handle = server.gui.add_slider("Predicted Gripper State", min=0.0, max=1.0, step=0.01, initial_value=0.0, disabled=True) - freq_handle = server.gui.add_number("Simulation Speed (Hz)", initial_value=args.frequency, min=1.0, max=500.0, step=1.0) + server.gui.add_button("🎲 Pull Random Observation").on_click( + lambda _: select_random_state() + ) + gripper_handle = server.gui.add_slider( + "Predicted Gripper State", + min=0.0, + max=1.0, + step=0.01, + initial_value=0.0, + disabled=True, + ) + freq_handle = server.gui.add_number( + "Simulation Speed (Hz)", + initial_value=args.frequency, + min=1.0, + max=500.0, + step=1.0, + ) select_random_state() - print("\n🚀 System Online! Open http://localhost:8080 to view the simulation. Press Ctrl+C to exit.\n") + print( + "\n🚀 System Online! Open http://localhost:8080 to view the simulation. Press Ctrl+C to exit.\n" + ) # --------------------------------------------------------- # 6. Main Visualization Loop @@ -214,22 +282,28 @@ def select_random_state() -> None: j_cfg = urdf_cfg_from_horizon(state["horizon"], state["action_idx"]) if j_cfg is not None: urdf_vis.update_cfg(j_cfg) - nc.log_joint_positions({jn: float(j_cfg[i]) for i, jn in enumerate(JOINT_NAMES)}) + nc.log_joint_positions( + {jn: float(j_cfg[i]) for i, jn in enumerate(JOINT_NAMES)} + ) - g_val = gripper_open_at_index(state["horizon"], state["action_idx"], out_grippers) - if g_val is not None: + g_val = gripper_open_at_index( + state["horizon"], state["action_idx"], out_grippers + ) + if g_val is not None: gripper_handle.value = round(g_val, 2) - + state["action_idx"] = (state["action_idx"] + 1) % h_len - time.sleep(max(0.0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time))) + time.sleep( + max(0.0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time)) + ) except KeyboardInterrupt: print("\n👋 Interrupt received. Shutting down gracefully...") except Exception as e: print(f"\n❌ Unhandled error in rendering loop: {e}") traceback.print_exc() - + # --------------------------------------------------------- # 7. Cleanup # --------------------------------------------------------- @@ -237,4 +311,4 @@ def select_random_state() -> None: print("\n🧹 Severing backend connections...") policy.disconnect() nc.logout() - print("👋 Offline validation complete.") \ No newline at end of file + print("👋 Offline validation complete.") diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index a377301..45d444a 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -3,8 +3,8 @@ Piper Robot Teleoperation with Meta Quest and Foot Pedals. This script provides a unified control interface for the AgileX Piper robot, -combining the 6D spatial tracking of a Meta Quest controller with the -hands-free convenience of a USB foot pedal. It simultaneously streams +combining the 6D spatial tracking of a Meta Quest controller with the +hands-free convenience of a USB foot pedal. It simultaneously streams synchronized telemetry data to the Neuracore cloud. Key Features: @@ -37,15 +37,17 @@ # --------------------------------------------------------------------------- sys.path.insert(0, str(Path(__file__).parent.parent)) -from common.configs import URDF_PATH, META_QUEST_AXIS_MASK from common.config_parser import load_ik_config +from common.configs import META_QUEST_AXIS_MASK, URDF_PATH from common.data_manager import RobotActivityState -from common.system_bootstrap import bootstrap_robot_system +from common.foot_pedal import FootPedal from common.shared_actions import ( - toggle_robot_enabled, move_robot_home, - toggle_recording, neuracore_logging_callback + move_robot_home, + neuracore_logging_callback, + toggle_recording, + toggle_robot_enabled, ) -from common.foot_pedal import FootPedal +from common.system_bootstrap import bootstrap_robot_system from common.threads.quest_reader import quest_reader_thread from meta_quest_teleop.reader import MetaQuestReader @@ -57,14 +59,29 @@ if __name__ == "__main__": # Ensure safe multiprocess spawning for UI/Background threads multiprocessing.set_start_method("spawn") - + # --------------------------------------------------------- # 1. Argument Parsing # --------------------------------------------------------- parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleop") - parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP address on the local network.") - parser.add_argument("--dataset-name", type=str, default=None, help="Override the auto-generated dataset name.") - parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.") + parser.add_argument( + "--ip-address", + type=str, + default=None, + help="Meta Quest IP address on the local network.", + ) + parser.add_argument( + "--dataset-name", + type=str, + default=None, + help="Override the auto-generated dataset name.", + ) + parser.add_argument( + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to IK/teleop YAML config.", + ) args = parser.parse_args() print("=" * 60 + "\nPIPER TELEOP: META QUEST + FOOT PEDALS\n" + "=" * 60) @@ -78,10 +95,16 @@ print("\n🔧 Initializing Neuracore...") try: nc.login() - nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) - - ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%Y-%m-%d-%H-%M-%S')}" - nc.create_dataset(name=ds_name, description="Quest + Pedal unified teleop collection") + nc.connect_robot( + robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False + ) + + ds_name = ( + args.dataset_name or f"pedal-teleop-{time.strftime('%Y-%m-%d-%H-%M-%S')}" + ) + nc.create_dataset( + name=ds_name, description="Quest + Pedal unified teleop collection" + ) except Exception as e: print(f"⚠️ Neuracore initialization skipped/failed: {e}") @@ -91,7 +114,7 @@ data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( config, start_ik=True, start_camera=True ) - + # Wire Neuracore logging to DataManager so states push to the cloud data_manager.set_on_change_callback(neuracore_logging_callback) @@ -102,12 +125,17 @@ quest_reader = None try: quest_reader = MetaQuestReader( - ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK + ip_address=args.ip_address, + port=5555, + run=True, + axis_mask=META_QUEST_AXIS_MASK, + ) + quest_thread = threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True ) - quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True) quest_thread.start() active_threads.append(quest_thread) - except (Exception, SystemExit) as e: + except (Exception, SystemExit): print("\n" + "!" * 60) print("❌ FAILED TO ACCESS META QUEST") print("!" * 60) @@ -115,10 +143,12 @@ print("\nPLEASE FOLLOW THESE STEPS:") print(" 1. Put the Meta Quest headset on your head.") print(" 2. Look for a notification in your menu that says 'USB Detected'.") - print(" 3. Click on that notification and select 'Allow' to grant data access.") + print( + " 3. Click on that notification and select 'Allow' to grant data access." + ) print(" 4. Rerun this script.") print("!" * 60 + "\n") - + data_manager.request_shutdown() for t in active_threads: t.join(timeout=1.0) @@ -130,18 +160,22 @@ # 5. Foot Pedal Initialization & Binding # --------------------------------------------------------- print("\n⌨️ Initializing Foot Pedals...") - pedal = FootPedal({ - "button_a": ENABLE_DISABLE_PEDAL, - "button_b": HOME_POSITION_PEDAL, - "button_c": RECORD_TOGGLE_PEDAL, - }) - + pedal = FootPedal( + { + "button_a": ENABLE_DISABLE_PEDAL, + "button_b": HOME_POSITION_PEDAL, + "button_c": RECORD_TOGGLE_PEDAL, + } + ) + # Bind the hardware actions to the pedal inputs pedal.bind("button_a", lambda: toggle_robot_enabled(data_manager, robot_controller)) pedal.bind("button_b", lambda: move_robot_home(data_manager, robot_controller)) pedal.bind("button_c", lambda: toggle_recording(play_audio=False)) - pedal_thread = threading.Thread(target=pedal.run_loop, args=(data_manager,), daemon=True) + pedal_thread = threading.Thread( + target=pedal.run_loop, args=(data_manager,), daemon=True + ) pedal_thread.start() active_threads.append(pedal_thread) @@ -156,7 +190,7 @@ # Keep the main thread alive while background threads handle execution while not data_manager.is_shutdown_requested(): time.sleep(1) - + except KeyboardInterrupt: print("\n👋 Interrupt received - shutting down gracefully...") data_manager.request_shutdown() @@ -170,26 +204,26 @@ # --------------------------------------------------------- finally: print("\n🧹 Cleaning up subsystems...") - + # Ensure we don't leave dangling, unterminated recordings on the cloud - if nc.is_recording(): + if nc.is_recording(): nc.cancel_recording() - + try: nc.logout() - except Exception: + except Exception: pass - + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) data_manager.request_shutdown() - - if quest_reader: + + if quest_reader: quest_reader.stop() - - for t in active_threads: + + for t in active_threads: t.join(timeout=2.0) - - if robot_controller: + + if robot_controller: robot_controller.cleanup() - - print("👋 Demo stopped.") \ No newline at end of file + + print("👋 Demo stopped.") diff --git a/examples/combine_code.py b/examples/combine_code.py index 043cf46..d12a634 100644 --- a/examples/combine_code.py +++ b/examples/combine_code.py @@ -1,54 +1,56 @@ import pathlib + def combine_python_files(directory_path, output_filename): """ Recursively finds all .py files in a directory and combines them into a single text file. - + Args: directory_path (str): The path to the root directory to search. output_filename (str): The name/path of the output text file. """ # Create a Path object for the target directory source_dir = pathlib.Path(directory_path) - + # Ensure the directory exists if not source_dir.is_dir(): print(f"Error: The directory '{directory_path}' does not exist.") return # Open the output file in write mode - with open(output_filename, 'w', encoding='utf-8') as outfile: + with open(output_filename, "w", encoding="utf-8") as outfile: # rglob('*.py') recursively searches for all files ending in .py - py_files = list(source_dir.rglob('*.py')) - + py_files = list(source_dir.rglob("*.py")) + if not py_files: print(f"No Python files found in '{directory_path}'.") return - + print(f"Found {len(py_files)} Python files. Combining...") - + for file_path in py_files: # Write a clear separator and the file path as a header outfile.write(f"\n{'='*60}\n") outfile.write(f"FILE: {file_path}\n") outfile.write(f"{'='*60}\n\n") - + # Read the python file and append its contents try: - with open(file_path, 'r', encoding='utf-8') as infile: + with open(file_path, "r", encoding="utf-8") as infile: outfile.write(infile.read()) outfile.write("\n") except Exception as e: error_msg = f"Error reading file {file_path}: {e}\n" print(error_msg) outfile.write(error_msg) - + print(f"Success! All files have been combined into '{output_filename}'.") + # --- Example Usage --- if __name__ == "__main__": # Replace these variables with your actual paths TARGET_DIRECTORY = "./" # "./" means current directory OUTPUT_FILE = "combined_code.txt" - - combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) \ No newline at end of file + + combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) diff --git a/examples/common/config_parser.py b/examples/common/config_parser.py index cab642f..2e3bc3c 100644 --- a/examples/common/config_parser.py +++ b/examples/common/config_parser.py @@ -1,12 +1,16 @@ -import yaml from pathlib import Path +import yaml + + def load_ik_config(config_path: str) -> dict: """Loads the teleoperation and IK parameters from a YAML file.""" path = Path(config_path) if not path.exists(): - print(f"⚠️ Warning: Config file not found at {path}. Using safe fallback defaults.") + print( + f"⚠️ Warning: Config file not found at {path}. Using safe fallback defaults." + ) return {} - - with open(path, 'r') as f: - return yaml.safe_load(f) \ No newline at end of file + + with open(path, "r") as f: + return yaml.safe_load(f) diff --git a/examples/common/configs.py b/examples/common/configs.py index fd5646d..5d7314f 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -1,42 +1,48 @@ """Configuration parameters for AgileX Piper robot demos.""" + from pathlib import Path # Hardware & Kinematics Paths -URDF_PATH = str(Path(__file__).parent.parent.parent / "piper_description" / "urdf" / "piper_description.urdf") +URDF_PATH = str( + Path(__file__).parent.parent.parent + / "piper_description" + / "urdf" + / "piper_description.urdf" +) GRIPPER_FRAME_NAME = "gripper_center" -# IK Solver +# IK Solver SOLVER_NAME = "quadprog" # Teleoperation Thresholds -GRIP_THRESHOLD = 0.9 +GRIP_THRESHOLD = 0.9 # Thread Execution Rates (Hz) -CONTROLLER_DATA_RATE = 50.0 -IK_SOLVER_RATE = 100 -VISUALIZATION_RATE = 60.0 -ROBOT_RATE = 100.0 -JOINT_STATE_STREAMING_RATE = 100.0 -CAMERA_FRAME_STREAMING_RATE = 30.0 +CONTROLLER_DATA_RATE = 50.0 +IK_SOLVER_RATE = 100 +VISUALIZATION_RATE = 60.0 +ROBOT_RATE = 100.0 +JOINT_STATE_STREAMING_RATE = 100.0 +CAMERA_FRAME_STREAMING_RATE = 30.0 # Hardware Context META_QUEST_AXIS_MASK = [1, 1, 1, 1, 1, 1] -CAMERA_DEVICE_INDEX = 5 +CAMERA_DEVICE_INDEX = 5 CAMERA_WIDTH = 640 CAMERA_HEIGHT = 480 CAMERA_NAMES = ["rgb_scene", "rgb_wrist"] # Default Robot Poses -NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] +NEUTRAL_JOINT_ANGLES = [-1.6, 52.2, -54.3, -3.2, 43.1, 4.7] NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] -#NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick task pruthvi +# NEUTRAL_JOINT_ANGLES = [-9.3, 86.7, -86.6, 1.8, 61.7, -5.1] #Lemon pick task pruthvi # AI Policy Execution Parameters -POLICY_EXECUTION_RATE = 20.0 -PREDICTION_HORIZON_EXECUTION_RATIO = 1.0 -MAX_SAFETY_THRESHOLD = 200.0 -MAX_ACTION_ERROR_THRESHOLD = 3.0 -TARGETING_POSE_TIME_THRESHOLD = 1.0 +POLICY_EXECUTION_RATE = 20.0 +PREDICTION_HORIZON_EXECUTION_RATIO = 1.0 +MAX_SAFETY_THRESHOLD = 200.0 +MAX_ACTION_ERROR_THRESHOLD = 3.0 +TARGETING_POSE_TIME_THRESHOLD = 1.0 GRIPPER_NAME = "gripper" GRIPPER_LOGGING_NAME = GRIPPER_NAME diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index 82a7ec9..4f93f36 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -4,9 +4,9 @@ This module provides shared state classes for teleoperation systems that need to coordinate between multiple threads (data collection, IK solving, visualization). """ +import queue # Added for async queueing import threading import time -import queue # Added for async queueing from enum import Enum from typing import Any, Callable @@ -14,21 +14,19 @@ from .configs import GRIPPER_NAME, JOINT_NAMES from .one_euro_filter import OneEuroFilterTransform - -import threading -import time -import queue -from typing import Any, Callable -import numpy as np - -from .configs import GRIPPER_NAME, JOINT_NAMES from .states import ( - RobotActivityState, ControllerState, TeleopState, - RobotState, IKState, CameraState + CameraState, + ControllerState, + IKState, + RobotActivityState, + RobotState, + TeleopState, ) + class RobotActivityState(Enum): """Robot activity state enumeration.""" + ENABLED = "ENABLED" HOMING = "HOMING" DISABLED = "DISABLED" @@ -109,6 +107,7 @@ def __init__(self) -> None: # Map from camera name -> latest RGB image self.rgb_images: dict[str, np.ndarray] = {} + class DataManager: """Main state container coordinating all state groups. @@ -119,6 +118,7 @@ class DataManager: Uses separate locks for each state group to reduce contention. """ + def __init__(self) -> None: """Initialize DataManager with background callback processing.""" self._controller_state = ControllerState() @@ -133,14 +133,14 @@ def __init__(self) -> None: self._on_change_callback: ( Callable[[str, dict[str, Any], float], None] | None ) = None - + # Maxsize 60 matches ~1 second of video frames buffer if disk spikes self._callback_queue: queue.Queue = queue.Queue(maxsize=60) - + self._worker_thread = threading.Thread( - target=self._callback_worker_loop, - name="NeuracoreCallbackWorker", - daemon=True + target=self._callback_worker_loop, + name="NeuracoreCallbackWorker", + daemon=True, ) self._worker_thread.start() @@ -150,11 +150,13 @@ def set_on_change_callback( """Set on change callback (thread-safe).""" self._on_change_callback = on_change_callback - def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None: + def _queue_callback( + self, name: str, payload: dict[str, Any], timestamp: float + ) -> None: """Helper to push payloads into the execution queue without blocking.""" if self._on_change_callback is None: return - + try: # put_nowait drops data into the memory queue instantly (0.0ms blocking) self._callback_queue.put_nowait((name, payload, timestamp)) @@ -168,11 +170,11 @@ def _callback_worker_loop(self) -> None: try: # Wait up to 100ms for a logging event name, payload, timestamp = self._callback_queue.get(timeout=0.1) - + if self._on_change_callback is not None: # Execute Neuracore disk operation safely here on a separate core self._on_change_callback(name, payload, timestamp) - + self._callback_queue.task_done() except queue.Empty: continue @@ -195,7 +197,7 @@ def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: """Set RGB image for a specific camera (thread-safe and non-blocking).""" with self._camera_state._lock: self._camera_state.rgb_images[camera_name] = image.copy() - + if self._on_change_callback: img_copy = self._camera_state.rgb_images[camera_name].copy() # Queue it instead of executing directly! Camera loop returns immediately. @@ -454,9 +456,7 @@ def set_target_joint_angles(self, angles: np.ndarray) -> None: payload = { jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES) } - self._queue_callback( - "log_joint_target_positions", payload, time.time() - ) + self._queue_callback("log_joint_target_positions", payload, time.time()) def set_target_pose(self, transform: np.ndarray | None) -> None: with self._ik_state._lock: @@ -498,4 +498,4 @@ def request_shutdown(self) -> None: def is_shutdown_requested(self) -> bool: """Check if shutdown is requested.""" - return self._shutdown_event.is_set() \ No newline at end of file + return self._shutdown_event.is_set() diff --git a/examples/common/dataset_helpers.py b/examples/common/dataset_helpers.py index a0676a0..a52528b 100644 --- a/examples/common/dataset_helpers.py +++ b/examples/common/dataset_helpers.py @@ -1,13 +1,16 @@ import neuracore as nc -from neuracore_types import DataType, CrossEmbodimentDescription -from neuracore.core.utils.robot_data_spec_utils import merge_cross_embodiment_description +from neuracore.core.utils.robot_data_spec_utils import ( + merge_cross_embodiment_description, +) +from neuracore_types import CrossEmbodimentDescription, DataType + def load_and_sync_dataset( - dataset_name: str, - frequency: int, + dataset_name: str, + frequency: int, input_modalities: list[DataType] | None = None, output_modalities: list[DataType] | None = None, - prefetch_videos: bool = False + prefetch_videos: bool = False, ): """Loads a Neuracore dataset and synchronizes the specified modalities across embodiments.""" print(f"\n🔍 Getting dataset '{dataset_name}' from Neuracore...") @@ -16,31 +19,35 @@ def load_and_sync_dataset( print("🔁 Building cross_embodiment_union for synchronization...") input_cross_embodiment: CrossEmbodimentDescription = {} output_cross_embodiment: CrossEmbodimentDescription = {} - + for robot_id in dataset.robot_ids: full = dataset.get_full_embodiment_description(robot_id) if input_modalities: - input_cross_embodiment[robot_id] = {dt: full[dt] for dt in input_modalities if dt in full} + input_cross_embodiment[robot_id] = { + dt: full[dt] for dt in input_modalities if dt in full + } if output_modalities: - output_cross_embodiment[robot_id] = {dt: full[dt] for dt in output_modalities if dt in full} - + output_cross_embodiment[robot_id] = { + dt: full[dt] for dt in output_modalities if dt in full + } + cross_embodiment_union = merge_cross_embodiment_description( input_cross_embodiment, output_cross_embodiment ) print("🔁 Synchronizing dataset...") - + # Dynamically build arguments to avoid passing 0 workers to the ThreadPoolExecutor sync_kwargs = { "frequency": frequency, "cross_embodiment_union": cross_embodiment_union, - "prefetch_videos": prefetch_videos + "prefetch_videos": prefetch_videos, } - + if prefetch_videos: sync_kwargs["max_prefetch_workers"] = 2 - + synced_dataset = dataset.synchronize(**sync_kwargs) - + print(f" ✓ Dataset synchronized: {len(synced_dataset)} episodes") - return synced_dataset \ No newline at end of file + return synced_dataset diff --git a/examples/common/policy_actions.py b/examples/common/policy_actions.py index 057954a..ff52097 100644 --- a/examples/common/policy_actions.py +++ b/examples/common/policy_actions.py @@ -1,28 +1,37 @@ """Extracted actions for running and executing policies across different rollout scripts.""" -import time import threading -import numpy as np +import time from typing import Any, Optional +import numpy as np from common.configs import ( - JOINT_NAMES, GRIPPER_NAME, MAX_SAFETY_THRESHOLD, MAX_ACTION_ERROR_THRESHOLD, - TARGETING_POSE_TIME_THRESHOLD, POLICY_EXECUTION_RATE, CAMERA_NAMES + CAMERA_NAMES, + GRIPPER_NAME, + JOINT_NAMES, + MAX_ACTION_ERROR_THRESHOLD, + MAX_SAFETY_THRESHOLD, + POLICY_EXECUTION_RATE, + TARGETING_POSE_TIME_THRESHOLD, ) from common.data_manager import DataManager, RobotActivityState -from common.policy_state import PolicyState from common.policy_helpers import ( - convert_predictions_to_horizon, log_robot_state_for_policy + convert_predictions_to_horizon, + log_robot_state_for_policy, ) +from common.policy_state import PolicyState def run_policy( - data_manager: DataManager, policy: Any, policy_state: PolicyState, - visualizer: Optional[Any], input_embodiment_description: dict + data_manager: DataManager, + policy: Any, + policy_state: PolicyState, + visualizer: Optional[Any], + input_embodiment_description: dict, ) -> bool: """Handle Run Policy trigger to capture state and get policy prediction.""" print("Running policy...") - + # Use our helper to log only what the model expects if not log_robot_state_for_policy(data_manager, input_embodiment_description): print("✗ No data available to run policy") @@ -34,17 +43,21 @@ def run_policy( inference_time = time.time() - start_time prediction_horizon = convert_predictions_to_horizon(predictions) - horizon_length = len(next(iter(prediction_horizon.values()))) if prediction_horizon else 0 - print(f" ✓ Got policy prediction in {inference_time:.2f}s with horizon length {horizon_length}") + horizon_length = ( + len(next(iter(prediction_horizon.values()))) if prediction_horizon else 0 + ) + print( + f" ✓ Got policy prediction in {inference_time:.2f}s with horizon length {horizon_length}" + ) if visualizer: policy_state.set_execution_ratio(visualizer.get_prediction_ratio()) - + policy_state.set_prediction_horizon(prediction_horizon) if visualizer: visualizer.update_ghost_robot_visibility(True) - + policy_state.set_ghost_robot_playing(True) policy_state.reset_ghost_action_index() return True @@ -53,12 +66,17 @@ def run_policy( return False -def start_policy_execution(data_manager: DataManager, policy_state: PolicyState) -> bool: +def start_policy_execution( + data_manager: DataManager, policy_state: PolicyState +) -> bool: """Handle Execute Policy trigger to start policy execution.""" print("Starting policy execution...") state = data_manager.get_robot_activity_state() - - if state == RobotActivityState.POLICY_CONTROLLED and not policy_state.get_continuous_play_active(): + + if ( + state == RobotActivityState.POLICY_CONTROLLED + and not policy_state.get_continuous_play_active() + ): print("⚠️ Policy execution already in progress") return False if state == RobotActivityState.DISABLED: @@ -93,24 +111,36 @@ def start_policy_execution(data_manager: DataManager, policy_state: PolicyState) return True -def end_policy_play(data_manager: DataManager, policy_state: PolicyState, visualizer: Optional[Any], status_msg: str) -> None: +def end_policy_play( + data_manager: DataManager, + policy_state: PolicyState, + visualizer: Optional[Any], + status_msg: str, +) -> None: """End continuous play and update system state.""" if policy_state.get_continuous_play_active(): policy_state.set_continuous_play_active(False) - + if visualizer: visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25)) visualizer.update_play_policy_button_status(False) visualizer.update_policy_status(status_msg) - + policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) data_manager.set_teleop_state(False, None, None) -def continuous_prediction_worker(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode): +def continuous_prediction_worker( + data_manager, policy, policy_state, visualizer, input_emb, continuous_mode +): """Background thread for continuous receding horizon execution.""" - colors = [(1.0, 0.65, 0.0, 0.25), (0.0, 1.0, 0.0, 0.25), (1.0, 0.0, 0.0, 0.25), (0.0, 0.0, 1.0, 0.25)] + colors = [ + (1.0, 0.65, 0.0, 0.25), + (0.0, 1.0, 0.0, 0.25), + (1.0, 0.0, 0.0, 0.25), + (0.0, 0.0, 1.0, 0.25), + ] c_idx = 0 print(f"\n🚀 Bootstrapping initial trajectory in '{continuous_mode}' mode...") @@ -119,103 +149,164 @@ def continuous_prediction_worker(data_manager, policy, policy_state, visualizer, while policy_state.get_continuous_play_active(): if policy_state.get_locked_prediction_horizon_length() == 0: - time.sleep(0.01); continue + time.sleep(0.01) + continue if continuous_mode == "pipeline": # Predict next horizon in the background while moving - success = run_policy(data_manager, policy, policy_state, visualizer, input_emb) - if not success: - time.sleep(0.05); continue + success = run_policy( + data_manager, policy, policy_state, visualizer, input_emb + ) + if not success: + time.sleep(0.05) + continue # Wait until current trajectory is almost finished before hot-swapping while policy_state.get_continuous_play_active(): - rem = policy_state.get_locked_prediction_horizon_length() - policy_state.get_execution_action_index() - if rem <= 5 or policy_state.get_locked_prediction_horizon_length() == 0: + rem = ( + policy_state.get_locked_prediction_horizon_length() + - policy_state.get_execution_action_index() + ) + if rem <= 5 or policy_state.get_locked_prediction_horizon_length() == 0: break time.sleep(0.01) elif continuous_mode == "sequential": # Wait for current trajectory to fully finish before querying network while policy_state.get_continuous_play_active(): - if policy_state.get_execution_action_index() >= policy_state.get_locked_prediction_horizon_length(): + if ( + policy_state.get_execution_action_index() + >= policy_state.get_locked_prediction_horizon_length() + ): break time.sleep(0.01) - - if not policy_state.get_continuous_play_active(): break - - success = run_policy(data_manager, policy, policy_state, visualizer, input_emb) - if not success: - time.sleep(0.05); continue - if not policy_state.get_continuous_play_active(): break + if not policy_state.get_continuous_play_active(): + break + + success = run_policy( + data_manager, policy, policy_state, visualizer, input_emb + ) + if not success: + time.sleep(0.05) + continue + + if not policy_state.get_continuous_play_active(): + break policy_state.end_policy_execution() if start_policy_execution(data_manager, policy_state): c_idx = (c_idx + 1) % len(colors) - if visualizer: visualizer.set_ghost_robot_color(colors[c_idx]) + if visualizer: + visualizer.set_ghost_robot_color(colors[c_idx]) else: time.sleep(0.01) -def play_policy(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode="pipeline"): +def play_policy( + data_manager, + policy, + policy_state, + visualizer, + input_emb, + continuous_mode="pipeline", +): """Toggle for starting/stopping continuous policy mode.""" if not policy_state.get_continuous_play_active(): print(f"▶️ Starting {continuous_mode.capitalize()} Mode...") policy_state.set_continuous_play_active(True) - if visualizer: visualizer.update_play_policy_button_status(True) + if visualizer: + visualizer.update_play_policy_button_status(True) threading.Thread( - target=continuous_prediction_worker, - args=(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode), - daemon=True + target=continuous_prediction_worker, + args=( + data_manager, + policy, + policy_state, + visualizer, + input_emb, + continuous_mode, + ), + daemon=True, ).start() else: print("⏹️ Stopping continuous policy execution...") - end_policy_play(data_manager, policy_state, visualizer, "Policy execution stopped") + end_policy_play( + data_manager, policy_state, visualizer, "Policy execution stopped" + ) -def policy_execution_thread(policy, data_manager, policy_state, robot_controller, visualizer, input_emb): +def policy_execution_thread( + policy, data_manager, policy_state, robot_controller, visualizer, input_emb +): """The thread that continuously reads the locked horizon and sends joint commands.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE last_vis_update = 0.0 while True: start_time = time.time() - - if data_manager.get_robot_activity_state() == RobotActivityState.POLICY_CONTROLLED: + + if ( + data_manager.get_robot_activity_state() + == RobotActivityState.POLICY_CONTROLLED + ): locked_horizon = policy_state.get_locked_prediction_horizon() exec_idx = policy_state.get_execution_action_index() locked_len = policy_state.get_locked_prediction_horizon_length() if exec_idx < locked_len: current_angles = data_manager.get_current_joint_angles() - + # Check target pose tracking if in pose-execution mode - if exec_idx > 0 and current_angles is not None and policy_state.get_execution_mode() == PolicyState.ExecutionMode.TARGETING_POSE: + if ( + exec_idx > 0 + and current_angles is not None + and policy_state.get_execution_mode() + == PolicyState.ExecutionMode.TARGETING_POSE + ): t_start = time.time() while (time.time() - t_start) < TARGETING_POSE_TIME_THRESHOLD: - prev_targs = np.degrees([locked_horizon[jn][exec_idx - 1] for jn in JOINT_NAMES]) - if np.any(np.abs(current_angles - prev_targs) <= MAX_ACTION_ERROR_THRESHOLD): + prev_targs = np.degrees( + [locked_horizon[jn][exec_idx - 1] for jn in JOINT_NAMES] + ) + if np.any( + np.abs(current_angles - prev_targs) + <= MAX_ACTION_ERROR_THRESHOLD + ): break time.sleep(0.001) if all(jn in locked_horizon for jn in JOINT_NAMES): - targs_deg = np.degrees([locked_horizon[jn][exec_idx] for jn in JOINT_NAMES]) + targs_deg = np.degrees( + [locked_horizon[jn][exec_idx] for jn in JOINT_NAMES] + ) data_manager.set_target_joint_angles(targs_deg) - if robot_controller.is_robot_enabled(): + if robot_controller.is_robot_enabled(): robot_controller.set_target_joint_angles(targs_deg) if GRIPPER_NAME in locked_horizon: - robot_controller.set_gripper_open_value(locked_horizon[GRIPPER_NAME][exec_idx]) + robot_controller.set_gripper_open_value( + locked_horizon[GRIPPER_NAME][exec_idx] + ) policy_state.increment_execution_action_index() - if visualizer: - visualizer.update_policy_status(f"Executing: {exec_idx + 1}/{locked_len}") + if visualizer: + visualizer.update_policy_status( + f"Executing: {exec_idx + 1}/{locked_len}" + ) else: if not policy_state.get_continuous_play_active(): - end_policy_play(data_manager, policy_state, visualizer, "Execution completed") - elif all(jn in locked_horizon for jn in JOINT_NAMES) and robot_controller.is_robot_enabled(): + end_policy_play( + data_manager, policy_state, visualizer, "Execution completed" + ) + elif ( + all(jn in locked_horizon for jn in JOINT_NAMES) + and robot_controller.is_robot_enabled() + ): # Hold last position while waiting for background thread to swap horizon - robot_controller.set_target_joint_angles(np.degrees([locked_horizon[jn][-1] for jn in JOINT_NAMES])) + robot_controller.set_target_joint_angles( + np.degrees([locked_horizon[jn][-1] for jn in JOINT_NAMES]) + ) # Throttle visualizer updates to 30Hz if visualizer and (time.time() - last_vis_update >= 1.0 / 30.0): @@ -225,7 +316,9 @@ def policy_execution_thread(policy, data_manager, policy_state, robot_controller time.sleep(max(0, dt_execution - (time.time() - start_time))) -def _update_visualization(data_manager: DataManager, policy_state: PolicyState, visualizer: Any) -> None: +def _update_visualization( + data_manager: DataManager, policy_state: PolicyState, visualizer: Any +) -> None: """Sync the visualizer UI with the DataManager's internal state.""" current_joints = data_manager.get_current_joint_angles() if current_joints is not None: @@ -239,31 +332,44 @@ def _update_visualization(data_manager: DataManager, policy_state: PolicyState, if state == RobotActivityState.POLICY_CONTROLLED: visualizer.update_ghost_robot_visibility(True) t_joints = data_manager.get_target_joint_angles() - if t_joints is not None: visualizer.update_ghost_robot_pose(np.radians(t_joints)) + if t_joints is not None: + visualizer.update_ghost_robot_pose(np.radians(t_joints)) visualizer.set_run_policy_button_disabled(True) visualizer.set_play_policy_button_disabled(False) - + elif state == RobotActivityState.ENABLED and data_manager.get_teleop_active(): visualizer.update_ghost_robot_visibility(True) t_joints = data_manager.get_target_joint_angles() - if t_joints is not None: visualizer.update_ghost_robot_pose(np.radians(t_joints)) - - elif policy_state.get_ghost_robot_playing() and policy_state.get_prediction_horizon_length() > 0: + if t_joints is not None: + visualizer.update_ghost_robot_pose(np.radians(t_joints)) + + elif ( + policy_state.get_ghost_robot_playing() + and policy_state.get_prediction_horizon_length() > 0 + ): visualizer.set_start_policy_execution_button_disabled(False) visualizer.update_ghost_robot_visibility(True) g_idx = policy_state.get_ghost_action_index() horizon = policy_state.get_prediction_horizon() - - if g_idx < policy_state.get_prediction_horizon_length() and all(jn in horizon for jn in JOINT_NAMES): - visualizer.update_ghost_robot_pose(np.array([horizon[jn][g_idx] for jn in JOINT_NAMES])) - policy_state.set_ghost_action_index((g_idx + 1) % policy_state.get_prediction_horizon_length()) + + if g_idx < policy_state.get_prediction_horizon_length() and all( + jn in horizon for jn in JOINT_NAMES + ): + visualizer.update_ghost_robot_pose( + np.array([horizon[jn][g_idx] for jn in JOINT_NAMES]) + ) + policy_state.set_ghost_action_index( + (g_idx + 1) % policy_state.get_prediction_horizon_length() + ) else: policy_state.reset_ghost_action_index() else: visualizer.update_ghost_robot_visibility(False) has_horizon = policy_state.get_prediction_horizon_length() > 0 enabled = state == RobotActivityState.ENABLED - - visualizer.set_start_policy_execution_button_disabled(not (enabled and has_horizon)) + + visualizer.set_start_policy_execution_button_disabled( + not (enabled and has_horizon) + ) visualizer.set_run_policy_button_disabled(not enabled) - visualizer.set_play_policy_button_disabled(not enabled) \ No newline at end of file + visualizer.set_play_policy_button_disabled(not enabled) diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 8695aa1..b75e035 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -20,7 +20,9 @@ def __init__(self) -> None: # Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names self._prediction_horizon: dict[str, list[float]] = {} self._prediction_horizon_lock = threading.Lock() - self._execution_ratio: float = 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in + self._execution_ratio: float = ( + 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in + ) self._policy_rgb_image_input: np.ndarray | None = None self._policy_rgb_image_input_lock = threading.Lock() diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index 137636c..af84a25 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -7,6 +7,7 @@ from .visualizer_core import RobotVisualizerCore from .visualizer_gui import RobotVisualizerGUI + class RobotVisualizer: """Shared visualizer facade for robot demos.""" @@ -18,12 +19,12 @@ def __init__(self, urdf_path: str) -> None: def __getattr__(self, name: str): """ Dynamically delegates method calls to the Core or GUI modules. - If a top-level script calls `visualizer.add_basic_controls()`, + If a top-level script calls `visualizer.add_basic_controls()`, this routes it to `_gui.add_basic_controls()`. """ if hasattr(self._core, name): return getattr(self._core, name) if hasattr(self._gui, name): return getattr(self._gui, name) - + raise AttributeError(f"'RobotVisualizer' object has no attribute '{name}'") diff --git a/examples/common/shared_actions.py b/examples/common/shared_actions.py index 9526427..4a3d6ca 100644 --- a/examples/common/shared_actions.py +++ b/examples/common/shared_actions.py @@ -1,25 +1,27 @@ -from common.data_manager import DataManager, RobotActivityState -from piper_controller import PiperController import subprocess -import neuracore as nc -import numpy as np -import traceback from typing import Any -from common.configs import JOINT_NAMES, GRIPPER_LOGGING_NAME -def toggle_robot_enabled(data_manager: DataManager, robot_controller: PiperController, visualizer=None): +import neuracore as nc +from common.data_manager import DataManager, RobotActivityState + +from piper_controller import PiperController + + +def toggle_robot_enabled( + data_manager: DataManager, robot_controller: PiperController, visualizer=None +): """Safely toggles the robot between ENABLED and DISABLED states.""" state = data_manager.get_robot_activity_state() - + if state == RobotActivityState.ENABLED: data_manager.set_robot_activity_state(RobotActivityState.DISABLED) if robot_controller: robot_controller.graceful_stop() data_manager.set_teleop_state(False, None, None) - if visualizer: + if visualizer: visualizer.update_toggle_robot_enabled_status(False) print("✓ 🔴 Robot disabled") - + elif state in (RobotActivityState.DISABLED, RobotActivityState.HOMING): if not robot_controller: data_manager.set_robot_activity_state(RobotActivityState.ENABLED) @@ -28,7 +30,7 @@ def toggle_robot_enabled(data_manager: DataManager, robot_controller: PiperContr if robot_controller.resume_robot(): data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - if visualizer: + if visualizer: visualizer.update_toggle_robot_enabled_status(True) print("✓ 🟢 Robot enabled") else: @@ -38,12 +40,12 @@ def toggle_robot_enabled(data_manager: DataManager, robot_controller: PiperContr def move_robot_home(data_manager: DataManager, robot_controller: PiperController): """Safely commands the robot to return to its home position.""" state = data_manager.get_robot_activity_state() - + if state == RobotActivityState.ENABLED: print("🏠 Moving to home position...") data_manager.set_robot_activity_state(RobotActivityState.HOMING) data_manager.set_teleop_state(False, None, None) - + if robot_controller: if not robot_controller.move_to_home(): print("✗ Failed to initiate home move") @@ -55,8 +57,6 @@ def move_robot_home(data_manager: DataManager, robot_controller: PiperController print("⚠️ Robot is not enabled, cannot go home") - - def toggle_recording(play_audio: bool = False) -> None: """Safely start or stop a Neuracore data recording session.""" if not nc.is_recording(): @@ -64,7 +64,10 @@ def toggle_recording(play_audio: bool = False) -> None: nc.start_recording() print("✓ 🔴 Recording started") if play_audio: - subprocess.Popen(["play", "-q", "-n", "synth", "0.3", "sine", "880"], stderr=subprocess.DEVNULL) + subprocess.Popen( + ["play", "-q", "-n", "synth", "0.3", "sine", "880"], + stderr=subprocess.DEVNULL, + ) except Exception as e: print(f"✗ Failed to start recording: {e}") else: @@ -72,11 +75,17 @@ def toggle_recording(play_audio: bool = False) -> None: nc.stop_recording() print("✓ ⏹️ Recording stopped") if play_audio: - subprocess.Popen(["play", "-q", "-n", "synth", "0.3", "sine", "440"], stderr=subprocess.DEVNULL) + subprocess.Popen( + ["play", "-q", "-n", "synth", "0.3", "sine", "440"], + stderr=subprocess.DEVNULL, + ) except Exception as e: print(f"✗ Failed to stop recording: {e}") -def neuracore_logging_callback(name: str, payload: dict[str, Any], timestamp: float) -> None: + +def neuracore_logging_callback( + name: str, payload: dict[str, Any], timestamp: float +) -> None: """Unified callback to map DataManager state changes to Neuracore log streams.""" try: if name == "log_joint_positions": @@ -93,4 +102,4 @@ def neuracore_logging_callback(name: str, payload: dict[str, Any], timestamp: fl camera_name = next(iter(payload)) nc.log_rgb(camera_name, payload[camera_name], timestamp=timestamp) except Exception as e: - print(f"⚠️ Logging failed for {name}: {e}") \ No newline at end of file + print(f"⚠️ Logging failed for {name}: {e}") diff --git a/examples/common/states.py b/examples/common/states.py index 0901703..525e217 100644 --- a/examples/common/states.py +++ b/examples/common/states.py @@ -1,17 +1,22 @@ import threading from enum import Enum + import numpy as np from common.one_euro_filter import OneEuroFilterTransform + class RobotActivityState(Enum): """Robot activity state enumeration.""" + ENABLED = "ENABLED" HOMING = "HOMING" DISABLED = "DISABLED" POLICY_CONTROLLED = "POLICY_CONTROLLED" + class ControllerState: """Controller input state - Quest Reader writes, IK/Joint reads.""" + def __init__(self) -> None: self._lock = threading.Lock() self.min_cutoff: float = 1.0 @@ -23,8 +28,10 @@ def __init__(self) -> None: self.trigger_value: float = 0.0 self._filter: OneEuroFilterTransform | None = None + class TeleopState: """Teleop activation state - manages teleop start/stop.""" + def __init__(self) -> None: self._lock = threading.Lock() self.active: bool = False @@ -34,8 +41,10 @@ def __init__(self) -> None: self.rotation_scale: float = 1.0 self.slow_scaling_mode_enabled: bool = False + class RobotState: """Current robot state - joint angles, end effector pose, activity state.""" + def __init__(self) -> None: self._lock = threading.Lock() self.joint_angles: np.ndarray | None = None @@ -45,8 +54,10 @@ def __init__(self) -> None: self.target_gripper_open_value: float | None = None self.activity_state: RobotActivityState = RobotActivityState.DISABLED + class IKState: """IK solution state - target joint angles, pose, metrics.""" + def __init__(self) -> None: self._lock = threading.Lock() self.target_joint_angles: np.ndarray | None = None @@ -54,8 +65,10 @@ def __init__(self) -> None: self.solve_time_ms: float = 0.0 self.success: bool = True + class CameraState: """Camera state - RGB images for one or more cameras.""" + def __init__(self) -> None: self._lock = threading.Lock() - self.rgb_images: dict[str, np.ndarray] = {} \ No newline at end of file + self.rgb_images: dict[str, np.ndarray] = {} diff --git a/examples/common/system_bootstrap.py b/examples/common/system_bootstrap.py index 8949ec3..3008517 100644 --- a/examples/common/system_bootstrap.py +++ b/examples/common/system_bootstrap.py @@ -1,24 +1,31 @@ import threading -import numpy as np -from typing import Tuple, List, Optional +from typing import List, Optional, Tuple +import numpy as np from common.configs import ( - ROBOT_RATE, NEUTRAL_JOINT_ANGLES, NEUTRAL_END_EFFECTOR_POSE, - URDF_PATH, GRIPPER_FRAME_NAME, SOLVER_NAME, IK_SOLVER_RATE + GRIPPER_FRAME_NAME, + IK_SOLVER_RATE, + NEUTRAL_END_EFFECTOR_POSE, + NEUTRAL_JOINT_ANGLES, + ROBOT_RATE, + SOLVER_NAME, + URDF_PATH, ) from common.data_manager import DataManager -from piper_controller import PiperController -from pink_ik_solver import PinkIKSolver -from common.threads.joint_state import joint_state_thread from common.threads.ik_solver import ik_solver_thread +from common.threads.joint_state import joint_state_thread from common.threads.realsense_camera import camera_thread +from pink_ik_solver import PinkIKSolver +from piper_controller import PiperController + + def bootstrap_robot_system( - config: dict, - start_ik: bool = True, - start_camera: bool = True -) -> Tuple[DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread]]: - + config: dict, start_ik: bool = True, start_camera: bool = True +) -> Tuple[ + DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread] +]: + # Extract config sections safely filt_p = config.get("filter_parameters", {}) tele_p = config.get("teleop_parameters", {}) @@ -27,13 +34,12 @@ def bootstrap_robot_system( # 1. Initialize Data Manager data_manager = DataManager() data_manager.set_controller_filter_params( - filt_p.get("controller_min_cutoff", 0.8), - filt_p.get("controller_beta", 0.05), - filt_p.get("controller_d_cutoff", 0.9) + filt_p.get("controller_min_cutoff", 0.8), + filt_p.get("controller_beta", 0.05), + filt_p.get("controller_d_cutoff", 0.9), ) data_manager.set_teleop_scaling( - tele_p.get("translation_scale", 1.5), - tele_p.get("rotation_scale", 1.2) + tele_p.get("translation_scale", 1.5), tele_p.get("rotation_scale", 1.2) ) # 2. Initialize Robot Controller @@ -52,7 +58,9 @@ def bootstrap_robot_system( # 3. Start Threads active_threads = [] print("\n📊 Starting joint state thread...") - js_thread = threading.Thread(target=joint_state_thread, args=(data_manager, robot_controller), daemon=True) + js_thread = threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True + ) js_thread.start() active_threads.append(js_thread) @@ -60,30 +68,41 @@ def bootstrap_robot_system( if start_ik: print("\n🔧 Creating Pink IK solver...") current_angles = data_manager.get_current_joint_angles() - init_angles = np.radians(current_angles) if current_angles is not None else np.radians(NEUTRAL_JOINT_ANGLES) - + init_angles = ( + np.radians(current_angles) + if current_angles is not None + else np.radians(NEUTRAL_JOINT_ANGLES) + ) + ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, + urdf_path=URDF_PATH, + end_effector_frame=GRIPPER_FRAME_NAME, + solver_name=SOLVER_NAME, position_cost=ik_p.get("position_cost", 1.0), - orientation_cost=ik_p.get("orientation_cost", 0.75), + orientation_cost=ik_p.get("orientation_cost", 0.75), frame_task_gain=ik_p.get("frame_task_gain", 0.4), - lm_damping=ik_p.get("lm_damping", 0.01), + lm_damping=ik_p.get("lm_damping", 0.01), damping_cost=ik_p.get("damping_cost", 0.25), solver_damping_value=ik_p.get("solver_damping_value", 1e-4), integration_time_step=1 / IK_SOLVER_RATE, initial_configuration=init_angles, - posture_cost_vector=np.array(ik_p.get("posture_cost_vector", [0.0, 0.0, 0.0, 0.05, 0.0, 0.0])), + posture_cost_vector=np.array( + ik_p.get("posture_cost_vector", [0.0, 0.0, 0.0, 0.05, 0.0, 0.0]) + ), ) print("\n🧮 Starting IK solver thread...") - ik_thread = threading.Thread(target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True) + ik_thread = threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True + ) ik_thread.start() active_threads.append(ik_thread) if start_camera: print("\n📷 Starting camera thread...") - cam_thread = threading.Thread(target=camera_thread, args=(data_manager,), daemon=True) + cam_thread = threading.Thread( + target=camera_thread, args=(data_manager,), daemon=True + ) cam_thread.start() active_threads.append(cam_thread) - return data_manager, robot_controller, ik_solver, active_threads \ No newline at end of file + return data_manager, robot_controller, ik_solver, active_threads diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py index 8573eae..95b6baa 100644 --- a/examples/common/threads/realsense_camera.py +++ b/examples/common/threads/realsense_camera.py @@ -3,7 +3,6 @@ import time import traceback from collections import deque -import cv2 import numpy as np import pyrealsense2 as rs @@ -63,14 +62,16 @@ def camera_thread(data_manager: DataManager) -> None: # DIAGNOSTICS: Check for dropped frames via hardware ID # --------------------------------------------------------- current_frame_number = color_frame.get_frame_number() - + if last_frame_number is not None: # If frame numbers are not sequential, we missed something in software drops = (current_frame_number - last_frame_number) - 1 if drops > 0: total_dropped_frames += drops - print(f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}") - + print( + f"⚠️ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}" + ) + last_frame_number = current_frame_number # --------------------------------------------------------- @@ -79,15 +80,17 @@ def camera_thread(data_manager: DataManager) -> None: frame_count += 1 if current_time - fps_timer >= 5.0: # Report every 5 seconds effective_fps = frame_count / (current_time - fps_timer) - + # Calculate jitter (max variance between frame arrivals) max_interval = max(intervals) * 1000 # in ms min_interval = min(intervals) * 1000 # in ms - - print(f"📊 Camera Health: {effective_fps:.1f} FPS | " - f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " - f"Total Drops: {total_dropped_frames}") - + + print( + f"📊 Camera Health: {effective_fps:.1f} FPS | " + f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | " + f"Total Drops: {total_dropped_frames}" + ) + # Reset counters for the next 5-second window fps_timer = current_time frame_count = 0 @@ -103,7 +106,7 @@ def camera_thread(data_manager: DataManager) -> None: color_image = np.rot90(color_image, k=3) # Rotate 270 degrees data_manager.set_rgb_image(color_image, camera_name) - # Notice: The time.sleep() has been completely removed. + # Notice: The time.sleep() has been completely removed. # wait_for_frames() manages the loop pace perfectly. except Exception as e: @@ -117,4 +120,4 @@ def camera_thread(data_manager: DataManager) -> None: print(" ✓ RealSense pipeline stopped") except Exception as e: print(f"⚠️ Error stopping pipeline: {e}") - print("📷 Camera thread stopped") \ No newline at end of file + print("📷 Camera thread stopped") diff --git a/examples/common/visualizer_core.py b/examples/common/visualizer_core.py index 0cffe26..37ba756 100644 --- a/examples/common/visualizer_core.py +++ b/examples/common/visualizer_core.py @@ -4,9 +4,10 @@ from scipy.spatial.transform import Rotation from viser.extras import ViserUrdf + class RobotVisualizerCore: """Handles the 3D rendering of the robot, ghost robot, and target frames.""" - + def __init__(self, urdf_path: str) -> None: self.server = viser.ViserServer() self.server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) @@ -49,7 +50,9 @@ def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: if rgb_image is None: return if self.rgb_image_handle is None: - self.add_rgb_image_placeholder(height=rgb_image.shape[0], width=rgb_image.shape[1]) + self.add_rgb_image_placeholder( + height=rgb_image.shape[0], width=rgb_image.shape[1] + ) self.rgb_image_handle.image = rgb_image def update_robot_pose(self, joint_config: np.ndarray) -> None: @@ -89,4 +92,4 @@ def update_target_visualization(self, transform: np.ndarray | None) -> None: self.target_frame_handle.wxyz = (rot[3], rot[0], rot[1], rot[2]) def stop(self) -> None: - self.server.stop() \ No newline at end of file + self.server.stop() diff --git a/examples/common/visualizer_gui.py b/examples/common/visualizer_gui.py index 7852e99..35b2c06 100644 --- a/examples/common/visualizer_gui.py +++ b/examples/common/visualizer_gui.py @@ -1,14 +1,16 @@ from typing import Any, Callable + import numpy as np import viser + class RobotVisualizerGUI: """Handles all the 2D UI elements: buttons, sliders, and text readouts.""" - + def __init__(self, server: viser.ViserServer) -> None: self.server = server self._ema_timing = 0.001 - + # State Handles self._timing_handle = None self._joint_angles_handle = None @@ -17,7 +19,7 @@ def __init__(self, server: viser.ViserServer) -> None: self._controller_status_handle = None self._gripper_status_handle = None self._policy_status_handle = None - + # Input Handles self._position_weight_handle = None self._orientation_weight_handle = None @@ -26,21 +28,21 @@ def __init__(self, server: viser.ViserServer) -> None: self._damping_weight_handle = None self._solver_damping_value_handle = None self._posture_cost_handles = [] - + self._controller_min_cutoff_handle = None self._controller_beta_handle = None self._controller_d_cutoff_handle = None self._translation_scale_handle = None self._rotation_scale_handle = None - + self._prediction_ratio_handle = None self._policy_execution_rate_handle = None self._robot_rate_handle = None self._execution_mode_dropdown = None - + self._grip_value_handle = None self._trigger_value_handle = None - + # Button Handles self._enable_robot_handle = None self._disable_robot_handle = None @@ -56,11 +58,19 @@ def __init__(self, server: viser.ViserServer) -> None: self._wrist_step_handle = None self._save_config_button = None - def add_advanced_tuning_controls(self, initial_slow_t: float, initial_slow_r: float, initial_wrist_step: float) -> None: + def add_advanced_tuning_controls( + self, initial_slow_t: float, initial_slow_r: float, initial_wrist_step: float + ) -> None: """Adds advanced sliders for precision movement and button toggles.""" - self._slow_translation_scale_handle = self.server.gui.add_number("Slow Translation Scale", initial_slow_t, min=0.1, max=5.0, step=0.01) - self._slow_rotation_scale_handle = self.server.gui.add_number("Slow Rotation Scale", initial_slow_r, min=0.1, max=5.0, step=0.01) - self._wrist_step_handle = self.server.gui.add_number("Wrist Nudge (Degrees)", initial_wrist_step, min=1.0, max=45.0, step=1.0) + self._slow_translation_scale_handle = self.server.gui.add_number( + "Slow Translation Scale", initial_slow_t, min=0.1, max=5.0, step=0.01 + ) + self._slow_rotation_scale_handle = self.server.gui.add_number( + "Slow Rotation Scale", initial_slow_r, min=0.1, max=5.0, step=0.01 + ) + self._wrist_step_handle = self.server.gui.add_number( + "Wrist Nudge (Degrees)", initial_wrist_step, min=1.0, max=45.0, step=1.0 + ) self._save_config_button = self.server.gui.add_button("💾 Save Config to YAML") def get_slow_translation_scale(self) -> float: @@ -73,62 +83,143 @@ def get_wrist_step_degrees(self) -> float: return self._wrist_step_handle.value def set_save_config_callback(self, cb: Callable[[], Any]) -> None: - if self._save_config_button: + if self._save_config_button: self._save_config_button.on_click(lambda _: cb()) def add_basic_controls(self) -> None: - self._timing_handle = self.server.gui.add_number("IK Solve Time (ms)", 0.001, disabled=True) - self._joint_angles_handle = self.server.gui.add_text("Joint Angles", "Waiting...") + self._timing_handle = self.server.gui.add_number( + "IK Solve Time (ms)", 0.001, disabled=True + ) + self._joint_angles_handle = self.server.gui.add_text( + "Joint Angles", "Waiting..." + ) def add_robot_status_controls(self) -> None: - self._robot_status_handle = self.server.gui.add_text("Robot Status", "Initializing...") + self._robot_status_handle = self.server.gui.add_text( + "Robot Status", "Initializing..." + ) def add_teleop_controls(self) -> None: - self._grip_value_handle = self.server.gui.add_number("Grip Value", 0.0, disabled=True) - self._trigger_value_handle = self.server.gui.add_number("Trigger", 0.0, disabled=True) - self._teleop_status_handle = self.server.gui.add_text("Teleop Status", "Inactive") - self._controller_status_handle = self.server.gui.add_text("Controller Status", "Waiting...") + self._grip_value_handle = self.server.gui.add_number( + "Grip Value", 0.0, disabled=True + ) + self._trigger_value_handle = self.server.gui.add_number( + "Trigger", 0.0, disabled=True + ) + self._teleop_status_handle = self.server.gui.add_text( + "Teleop Status", "Inactive" + ) + self._controller_status_handle = self.server.gui.add_text( + "Controller Status", "Waiting..." + ) def add_gripper_status_controls(self) -> None: - self._gripper_status_handle = self.server.gui.add_text("Gripper Status", "Open (0%)") + self._gripper_status_handle = self.server.gui.add_text( + "Gripper Status", "Open (0%)" + ) def add_homing_controls(self) -> None: self._go_home_button = self.server.gui.add_button("Go Home") def add_toggle_robot_enabled_status_button(self) -> None: - self._toggle_robot_enabled_status_button = self.server.gui.add_button("Enable Robot") - - def add_controller_filter_controls(self, initial_min_cutoff: float, initial_beta: float, initial_d_cutoff: float) -> None: - self._controller_min_cutoff_handle = self.server.gui.add_number("Controller Min Cutoff", initial_min_cutoff, min=0.01, max=10.0, step=0.01) - self._controller_beta_handle = self.server.gui.add_number("Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01) - self._controller_d_cutoff_handle = self.server.gui.add_number("Controller D Cutoff", initial_d_cutoff, min=0.01, max=10.0, step=0.01) - - def add_scaling_controls(self, initial_translation_scale: float, initial_rotation_scale: float) -> None: - self._translation_scale_handle = self.server.gui.add_number("Translation Scale", initial_translation_scale, min=0.1, max=10.0, step=0.001) - self._rotation_scale_handle = self.server.gui.add_number("Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001) - - def add_pink_parameter_controls(self, position_cost: float, orientation_cost: float, frame_task_gain: float, lm_damping: float, damping_cost: float, solver_damping_value: float, posture_cost_vector: list[float]) -> None: - self._position_weight_handle = self.server.gui.add_number("Position Weight", position_cost, min=0.0, max=10.0, step=0.1) - self._orientation_weight_handle = self.server.gui.add_number("Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01) - self._frame_task_gain_handle = self.server.gui.add_number("Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1) - self._lm_damping_handle = self.server.gui.add_number("LM Damping", lm_damping, min=0.0, max=5.0, step=0.01) - self._damping_weight_handle = self.server.gui.add_number("Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01) - self._solver_damping_value_handle = self.server.gui.add_number("Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001) - - for i, cost in enumerate(posture_cost_vector): - self._posture_cost_handles.append(self.server.gui.add_number(f"Posture Cost J{i+1}", cost, min=0.0, max=1.0, step=0.01)) + self._toggle_robot_enabled_status_button = self.server.gui.add_button( + "Enable Robot" + ) + + def add_controller_filter_controls( + self, initial_min_cutoff: float, initial_beta: float, initial_d_cutoff: float + ) -> None: + self._controller_min_cutoff_handle = self.server.gui.add_number( + "Controller Min Cutoff", initial_min_cutoff, min=0.01, max=10.0, step=0.01 + ) + self._controller_beta_handle = self.server.gui.add_number( + "Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01 + ) + self._controller_d_cutoff_handle = self.server.gui.add_number( + "Controller D Cutoff", initial_d_cutoff, min=0.01, max=10.0, step=0.01 + ) + + def add_scaling_controls( + self, initial_translation_scale: float, initial_rotation_scale: float + ) -> None: + self._translation_scale_handle = self.server.gui.add_number( + "Translation Scale", + initial_translation_scale, + min=0.1, + max=10.0, + step=0.001, + ) + self._rotation_scale_handle = self.server.gui.add_number( + "Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001 + ) + + def add_pink_parameter_controls( + self, + position_cost: float, + orientation_cost: float, + frame_task_gain: float, + lm_damping: float, + damping_cost: float, + solver_damping_value: float, + posture_cost_vector: list[float], + ) -> None: + self._position_weight_handle = self.server.gui.add_number( + "Position Weight", position_cost, min=0.0, max=10.0, step=0.1 + ) + self._orientation_weight_handle = self.server.gui.add_number( + "Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01 + ) + self._frame_task_gain_handle = self.server.gui.add_number( + "Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1 + ) + self._lm_damping_handle = self.server.gui.add_number( + "LM Damping", lm_damping, min=0.0, max=5.0, step=0.01 + ) + self._damping_weight_handle = self.server.gui.add_number( + "Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01 + ) + self._solver_damping_value_handle = self.server.gui.add_number( + "Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001 + ) - def add_policy_controls(self, initial_prediction_ratio: float = 0.8, initial_policy_rate: float = 200.0, initial_robot_rate: float = 200.0, initial_execution_mode: str = "targeting_time") -> None: + for i, cost in enumerate(posture_cost_vector): + self._posture_cost_handles.append( + self.server.gui.add_number( + f"Posture Cost J{i+1}", cost, min=0.0, max=1.0, step=0.01 + ) + ) + + def add_policy_controls( + self, + initial_prediction_ratio: float = 0.8, + initial_policy_rate: float = 200.0, + initial_robot_rate: float = 200.0, + initial_execution_mode: str = "targeting_time", + ) -> None: self._policy_status_handle = self.server.gui.add_text("Policy Status", "Ready") - self._prediction_ratio_handle = self.server.gui.add_number("Prediction Ratio", initial_prediction_ratio, min=0.0, max=1.0, step=0.01) - self._policy_execution_rate_handle = self.server.gui.add_number("Policy Rate", initial_policy_rate, min=1.0, max=200.0, step=1.0) - self._robot_rate_handle = self.server.gui.add_number("Robot Rate", initial_robot_rate, min=1.0, max=200.0, step=1.0) - self._execution_mode_dropdown = self.server.gui.add_dropdown("Execution Mode", options=["targeting_time", "targeting_pose"], initial_value=initial_execution_mode) + self._prediction_ratio_handle = self.server.gui.add_number( + "Prediction Ratio", initial_prediction_ratio, min=0.0, max=1.0, step=0.01 + ) + self._policy_execution_rate_handle = self.server.gui.add_number( + "Policy Rate", initial_policy_rate, min=1.0, max=200.0, step=1.0 + ) + self._robot_rate_handle = self.server.gui.add_number( + "Robot Rate", initial_robot_rate, min=1.0, max=200.0, step=1.0 + ) + self._execution_mode_dropdown = self.server.gui.add_dropdown( + "Execution Mode", + options=["targeting_time", "targeting_pose"], + initial_value=initial_execution_mode, + ) def add_policy_buttons(self) -> None: self._run_policy_button = self.server.gui.add_button("Run Policy (Preview)") - self._start_policy_execution_button = self.server.gui.add_button("Execute Policy (Run Preview)") - self._play_policy_button = self.server.gui.add_button("Continuous Receding Horizon") + self._start_policy_execution_button = self.server.gui.add_button( + "Execute Policy (Run Preview)" + ) + self._play_policy_button = self.server.gui.add_button( + "Continuous Receding Horizon" + ) # --- UI Update Methods --- def update_timing(self, solve_time_ms: float) -> None: @@ -137,60 +228,107 @@ def update_timing(self, solve_time_ms: float) -> None: self._timing_handle.value = self._ema_timing def update_robot_status(self, status: str) -> None: - if self._robot_status_handle: self._robot_status_handle.value = status + if self._robot_status_handle: + self._robot_status_handle.value = status def update_teleop_status(self, active: bool) -> None: - if self._teleop_status_handle: self._teleop_status_handle.value = f"Teleop Status: {'Active' if active else 'Inactive'}" - - def update_controller_status_display(self, position: np.ndarray | None, connected: bool = True) -> None: - if not self._controller_status_handle: return + if self._teleop_status_handle: + self._teleop_status_handle.value = ( + f"Teleop Status: {'Active' if active else 'Inactive'}" + ) + + def update_controller_status_display( + self, position: np.ndarray | None, connected: bool = True + ) -> None: + if not self._controller_status_handle: + return if connected and position is not None: self._controller_status_handle.value = f"Controller Status:\n Position: [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}]\n Connected: ✓" else: self._controller_status_handle.value = "Controller Status:\n Connected: ✗" - def update_gripper_status(self, trigger_value: float, robot_enabled: bool = True) -> None: - if not self._gripper_status_handle: return - state = "Closed" if trigger_value > 0.9 else "Closing" if trigger_value > 0.1 else "Open" + def update_gripper_status( + self, trigger_value: float, robot_enabled: bool = True + ) -> None: + if not self._gripper_status_handle: + return + state = ( + "Closed" + if trigger_value > 0.9 + else "Closing" if trigger_value > 0.1 else "Open" + ) status = f"Gripper: {state} ({trigger_value * 100.0:.0f}% closed)" - self._gripper_status_handle.value = status + ("" if robot_enabled else " [Disabled]") + self._gripper_status_handle.value = status + ( + "" if robot_enabled else " [Disabled]" + ) def update_policy_status(self, status: str) -> None: - if self._policy_status_handle: self._policy_status_handle.value = status + if self._policy_status_handle: + self._policy_status_handle.value = status def update_toggle_robot_enabled_status(self, enabled: bool) -> None: if self._toggle_robot_enabled_status_button: - self._toggle_robot_enabled_status_button.label = "Disable Robot" if enabled else "Enable Robot" + self._toggle_robot_enabled_status_button.label = ( + "Disable Robot" if enabled else "Enable Robot" + ) def update_play_policy_button_status(self, active: bool) -> None: if self._play_policy_button: - self._play_policy_button.label = "Stop Continuous Horizon" if active else "Continuous Receding Horizon" - - def update_joint_angles_display(self, joint_config: np.ndarray, show_gripper: bool = False) -> None: - if not self._joint_angles_handle: return + self._play_policy_button.label = ( + "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + ) + + def update_joint_angles_display( + self, joint_config: np.ndarray, show_gripper: bool = False + ) -> None: + if not self._joint_angles_handle: + return lines = ["Joint Angles (rad):"] num_joints = len(joint_config) if show_gripper else 6 for i in range(num_joints): - lbl = f"Joint {i+1} ({'Robot' if i < 6 else 'Gripper'})" if show_gripper else f"J{i+1}" - lines.append(f" {lbl}: {joint_config[i]:.3f} rad ({np.degrees(joint_config[i]):.1f}°)") + lbl = ( + f"Joint {i+1} ({'Robot' if i < 6 else 'Gripper'})" + if show_gripper + else f"J{i+1}" + ) + lines.append( + f" {lbl}: {joint_config[i]:.3f} rad ({np.degrees(joint_config[i]):.1f}°)" + ) self._joint_angles_handle.value = "\n".join(lines) def set_grip_value(self, value: float) -> None: - if self._grip_value_handle: self._grip_value_handle.value = value + if self._grip_value_handle: + self._grip_value_handle.value = value def set_trigger_value(self, value: float) -> None: - if self._trigger_value_handle: self._trigger_value_handle.value = value + if self._trigger_value_handle: + self._trigger_value_handle.value = value # --- Getters --- def get_controller_filter_params(self) -> tuple[float, float, float]: - return (self._controller_min_cutoff_handle.value, self._controller_beta_handle.value, self._controller_d_cutoff_handle.value) + return ( + self._controller_min_cutoff_handle.value, + self._controller_beta_handle.value, + self._controller_d_cutoff_handle.value, + ) + + def get_translation_scale(self) -> float: + return self._translation_scale_handle.value - def get_translation_scale(self) -> float: return self._translation_scale_handle.value - def get_rotation_scale(self) -> float: return self._rotation_scale_handle.value - def get_prediction_ratio(self) -> float: return self._prediction_ratio_handle.value - def get_policy_execution_rate(self) -> float: return self._policy_execution_rate_handle.value - def get_robot_rate(self) -> float: return self._robot_rate_handle.value - def get_execution_mode(self) -> str: return self._execution_mode_dropdown.value + def get_rotation_scale(self) -> float: + return self._rotation_scale_handle.value + + def get_prediction_ratio(self) -> float: + return self._prediction_ratio_handle.value + + def get_policy_execution_rate(self) -> float: + return self._policy_execution_rate_handle.value + + def get_robot_rate(self) -> float: + return self._robot_rate_handle.value + + def get_execution_mode(self) -> str: + return self._execution_mode_dropdown.value def get_pink_parameters(self) -> dict: return { @@ -200,26 +338,44 @@ def get_pink_parameters(self) -> dict: "lm_damping": self._lm_damping_handle.value, "damping_cost": self._damping_weight_handle.value, "solver_damping_value": self._solver_damping_value_handle.value, - "posture_cost_vector": np.array([h.value for h in self._posture_cost_handles]), + "posture_cost_vector": np.array( + [h.value for h in self._posture_cost_handles] + ), } # --- Button Setters/Callbacks --- def set_toggle_robot_enabled_status_callback(self, cb: Callable[[], Any]) -> None: - if self._toggle_robot_enabled_status_button: self._toggle_robot_enabled_status_button.on_click(lambda _: cb()) + if self._toggle_robot_enabled_status_button: + self._toggle_robot_enabled_status_button.on_click(lambda _: cb()) + def set_go_home_callback(self, cb: Callable[[], Any]) -> None: - if self._go_home_button: self._go_home_button.on_click(lambda _: cb()) + if self._go_home_button: + self._go_home_button.on_click(lambda _: cb()) + def set_run_policy_callback(self, cb: Callable[[], Any]) -> None: - if self._run_policy_button: self._run_policy_button.on_click(lambda _: cb()) + if self._run_policy_button: + self._run_policy_button.on_click(lambda _: cb()) + def set_start_policy_execution_callback(self, cb: Callable[[], Any]) -> None: - if self._start_policy_execution_button: self._start_policy_execution_button.on_click(lambda _: cb()) + if self._start_policy_execution_button: + self._start_policy_execution_button.on_click(lambda _: cb()) + def set_play_policy_callback(self, cb: Callable[[], Any]) -> None: - if self._play_policy_button: self._play_policy_button.on_click(lambda _: cb()) + if self._play_policy_button: + self._play_policy_button.on_click(lambda _: cb()) + def set_execution_mode_callback(self, cb: Callable[[], Any]) -> None: - if self._execution_mode_dropdown: self._execution_mode_dropdown.on_update(lambda _: cb()) + if self._execution_mode_dropdown: + self._execution_mode_dropdown.on_update(lambda _: cb()) def set_run_policy_button_disabled(self, disabled: bool) -> None: - if self._run_policy_button: self._run_policy_button.disabled = disabled + if self._run_policy_button: + self._run_policy_button.disabled = disabled + def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: - if self._start_policy_execution_button: self._start_policy_execution_button.disabled = disabled + if self._start_policy_execution_button: + self._start_policy_execution_button.disabled = disabled + def set_play_policy_button_disabled(self, disabled: bool) -> None: - if self._play_policy_button: self._play_policy_button.disabled = disabled \ No newline at end of file + if self._play_policy_button: + self._play_policy_button.disabled = disabled diff --git a/piper_controller.py b/piper_controller.py index 3e37f93..44605b6 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -91,7 +91,9 @@ def __init__( # Gripper range in degrees (for internal SDK communication) self.GRIPPER_DEGREES_MIN = -10.00 - self.GRIPPER_DEGREES_MAX = 80.00 # cube stacking halid is 80, lemon picking pruthvi is 80 + self.GRIPPER_DEGREES_MAX = ( + 80.00 # cube stacking halid is 80, lemon picking pruthvi is 80 + ) self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees @@ -234,23 +236,22 @@ def get_control_mode(self) -> "PiperController.ControlMode": # self._control_mode = mode # if self.debug_mode: # print(f"Control mode changed: {old_mode.value} -> {mode.value}") - def set_control_mode(self, mode: "PiperController.ControlMode") -> None: with self.state_lock: old_mode = self._control_mode self._control_mode = mode - + # Send the mode configuration ONCE here, not in the 100Hz loop if hasattr(self, "piper") and self.piper.get_connect_status(): if mode == PiperController.ControlMode.JOINT_SPACE: self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00) elif mode == PiperController.ControlMode.END_EFFECTOR: self.piper.MotionCtrl_2(0x01, 0x00, 100, 0x00) - + if self.debug_mode: print(f"Control mode changed: {old_mode.value} -> {mode.value}") - + @staticmethod def _pose_6d_to_4x4(pose_6d: np.ndarray) -> np.ndarray: """Convert 6D pose [x, y, z, rx, ry, rz] to 4x4 transformation matrix. @@ -507,7 +508,9 @@ def control_loop(self) -> None: if now - last_reenable_time >= reenable_interval: if not self.piper.EnablePiper(): if self.debug_mode: - print("Control loop: EnablePiper re-enable returned False") + print( + "Control loop: EnablePiper re-enable returned False" + ) last_reenable_time = now # Get current control mode From fd7bf564bfc9b968e6dd815c4774c43cf5323037 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 16:35:10 +0100 Subject: [PATCH 21/22] fix: all pre-commit errors --- example-agilex-dictionary.txt | 8 + examples/1_tune_teleop_params.py | 16 +- .../2_collect_teleop_data_with_neuracore.py | 11 +- examples/3_replay_neuracore_episodes.py | 7 +- examples/4_rollout_neuracore_policy.py | 3 +- .../5_rollout_neuracore_policy_minimal.py | 29 ++-- examples/6_visualize_policy_from_dataset.py | 77 +++++----- examples/7_teleop_with_pedal.py | 3 +- examples/combine_code.py | 56 ------- examples/common/config_parser.py | 4 +- examples/common/data_manager.py | 40 ++++- examples/common/dataset_helpers.py | 6 +- examples/common/policy_actions.py | 32 ++-- examples/common/robot_visualizer.py | 9 +- examples/common/shared_actions.py | 12 +- examples/common/states.py | 7 + examples/common/system_bootstrap.py | 4 +- examples/common/threads/realsense_camera.py | 2 +- examples/common/visualizer_core.py | 23 ++- examples/common/visualizer_gui.py | 144 +++++++++++++----- piper_controller.py | 1 + 21 files changed, 289 insertions(+), 205 deletions(-) delete mode 100644 examples/combine_code.py diff --git a/example-agilex-dictionary.txt b/example-agilex-dictionary.txt index 5a0a7f0..57315b0 100644 --- a/example-agilex-dictionary.txt +++ b/example-agilex-dictionary.txt @@ -56,4 +56,12 @@ evdev keystate pynput ungrab +tele +filt +targs +infile +aioice +aiortc +halid +pruthvi diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index 817528a..1b3ef40 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Piper Robot Teleoperation Tuning - Real Robot Control. +"""Piper Robot Teleoperation Tuning - Real Robot Control. This script allows developers to iteratively tune the robot's teleoperation parameters in real-time. It provides a Viser-based web UI where you can adjust Inverse Kinematics (IK) @@ -17,7 +16,7 @@ from pathlib import Path import numpy as np -import yaml +import yaml # type: ignore[import] # Add parent directory to path to import local modules sys.path.insert(0, str(Path(__file__).parent.parent)) @@ -31,7 +30,7 @@ URDF_PATH, VISUALIZATION_RATE, ) -from common.data_manager import RobotActivityState +from common.data_manager import DataManager, RobotActivityState from common.robot_visualizer import RobotVisualizer from common.shared_actions import move_robot_home, toggle_robot_enabled from common.system_bootstrap import bootstrap_robot_system @@ -46,7 +45,10 @@ def _step_wrist_joint( - data_manager, robot_controller, visualizer, direction: int + data_manager: DataManager, + robot_controller: PiperController, + visualizer: RobotVisualizer, + direction: int, ) -> None: """Apply a relative manual step to the wrist joint using the dynamically tuned step degree.""" data_manager.set_teleop_state(False, None, None) @@ -69,7 +71,7 @@ def _step_wrist_joint( data_manager.set_target_joint_angles(target_joint_angles) -def toggle_slow_scaling(data_manager, visualizer): +def toggle_slow_scaling(data_manager: DataManager, visualizer: RobotVisualizer) -> None: """Toggle the movement scaling mode between standard and dynamically-tuned precision.""" enabled = data_manager.toggle_slow_scaling_mode_enabled() if enabled: @@ -83,7 +85,7 @@ def toggle_slow_scaling(data_manager, visualizer): def save_config_to_yaml( - visualizer, filepath="ik_conf/tuned_teleop_configs.yaml" + visualizer: RobotVisualizer, filepath: str = "ik_conf/tuned_teleop_configs.yaml" ) -> None: """Extracts all current UI slider values and saves them to a YAML file.""" try: diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index cdc560e..0d3d697 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Piper Robot Teleoperation and Neuracore Data Collection. +"""Piper Robot Teleoperation and Neuracore Data Collection. This script facilitates real-time teleoperation of the AgileX Piper robotic arm using Meta Quest controllers, while simultaneously streaming perfectly synchronized @@ -61,9 +60,7 @@ def _step_wrist_joint( data_manager: DataManager, robot_controller: PiperController, delta_degrees: float ) -> None: - """ - Applies a relative step adjustment to the robot's wrist joint target angle. - """ + """Apply a relative step adjustment to the robot's wrist joint target angle.""" data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) @@ -83,9 +80,7 @@ def _step_wrist_joint( def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: - """ - Toggles the teleoperation sensitivity between standard and slow (precision) scaling. - """ + """Toggle the teleoperation sensitivity between standard and slow (precision) scaling.""" enabled = data_manager.toggle_slow_scaling_mode_enabled() if enabled: data_manager.set_teleop_scaling( diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index 473e91f..d52150c 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Piper Robot Dataset Replay and Validation. +"""Piper Robot Dataset Replay and Validation. This script acts as a production-grade utility for replaying recorded Neuracore teleoperation datasets directly on the physical AgileX Piper robotic arm. @@ -61,9 +60,7 @@ # Helper Functions # --------------------------------------------------------------------------- def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: - """ - Commands the robot to its home position and blocks until it arrives. - """ + """Command the robot to its home position and block until it arrives.""" robot_controller.move_to_home() for remaining in range(timeout, 0, -1): if robot_controller.is_robot_homed(): diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 171982d..052c2c2 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Piper Robot Policy Rollout and Visualization. +"""Piper Robot Policy Rollout and Visualization. This script serves as a production-ready entry point for executing and visualizing AI policies trained via Neuracore on the AgileX Piper robotic arm. It handles: diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 4a11792..0e0134f 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Minimal Piper Robot Policy Rollout (Headless). +"""Minimal Piper Robot Policy Rollout (Headless). This script serves as a lightweight, terminal-only entry point for executing AI policies trained via Neuracore on the AgileX Piper robotic arm. It removes @@ -24,6 +23,7 @@ import time import traceback from pathlib import Path +from typing import Any import neuracore as nc @@ -59,19 +59,14 @@ # Helper Functions # --------------------------------------------------------------------------- def execute_horizon( - data_manager, - policy_state, - robot_controller, - frequency, - input_embodiment, - output_grippers, -): - """ - Minimal terminal-only execution loop for the prediction horizon. - - Locks the predicted horizon and streams the joint/gripper targets to the - robot at the specified frequency until the horizon is exhausted. - """ + data_manager: Any, + policy_state: PolicyState, + robot_controller: Any, + frequency: int, + input_embodiment: dict, + output_grippers: list[str] | None, +) -> None: + """Minimal terminal-only execution loop for the prediction horizon.""" policy_state.start_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) @@ -205,14 +200,14 @@ def execute_horizon( print_policy_embodiments(input_emb, output_emb) - output_gripper_names = None + output_gripper_names: list[str] | None = None if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb: output_gripper_names = embodiment_names_ordered( output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] ) # 4. Bootstrap Core System (No Quest, No IK needed for minimal playback) - data_manager, robot_controller, _, active_threads = bootstrap_robot_system( + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( config, start_ik=False, start_camera=True ) diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index e002e3f..9e09dda 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Offline Policy Visualization and Validation. +"""Offline Policy Visualization and Validation. This script acts as a safe, simulated testing ground for your trained AI policies. Instead of running on the physical robot, it: @@ -24,16 +23,9 @@ import time import traceback from pathlib import Path +from typing import Any -# --------------------------------------------------------------------------- -# Suppress Noisy WebRTC/STUN Networking Errors from Viser -# --------------------------------------------------------------------------- -# Viser attempts to use STUN servers for WebRTC streaming. If a firewall/VPN -# blocks it, it throws massive 401 errors before falling back to WebSockets. -# This silences those unhandled asyncio exceptions so the terminal stays clean. -logging.getLogger("asyncio").setLevel(logging.CRITICAL) -logging.getLogger("aioice").setLevel(logging.CRITICAL) -logging.getLogger("aiortc").setLevel(logging.CRITICAL) +sys.path.insert(0, str(Path(__file__).parent.parent)) import neuracore as nc import numpy as np @@ -43,32 +35,42 @@ from viser.extras import ViserUrdf # --------------------------------------------------------------------------- -# Path Configuration & Local Imports +# Suppress Noisy WebRTC/STUN Networking Errors from Viser # --------------------------------------------------------------------------- -sys.path.insert(0, str(Path(__file__).parent.parent)) - -from common.configs import ( - CAMERA_NAMES, - GRIPPER_NAME, - JOINT_NAMES, - POLICY_EXECUTION_RATE, - URDF_PATH, -) -from common.dataset_helpers import load_and_sync_dataset -from common.policy_helpers import ( - DEFAULT_ROBOT_NAME, - convert_predictions_to_horizon, - embodiment_names_ordered, - get_policy_embodiments, - gripper_open_at_index, - horizon_length, - log_sync_step_for_policy, - print_policy_embodiments, - urdf_cfg_from_horizon, -) -from neuracore_types import DataType +# Viser attempts to use STUN servers for WebRTC streaming. If a firewall/VPN +# blocks it, it throws massive 401 errors before falling back to WebSockets. +# This silences those unhandled asyncio exceptions so the terminal stays clean. +logging.getLogger("asyncio").setLevel(logging.CRITICAL) +logging.getLogger("aioice").setLevel(logging.CRITICAL) +logging.getLogger("aiortc").setLevel(logging.CRITICAL) if __name__ == "__main__": + # --------------------------------------------------------------------------- + # Path Configuration & Local Imports + # --------------------------------------------------------------------------- + sys.path.insert(0, str(Path(__file__).parent.parent)) + + from common.configs import ( + CAMERA_NAMES, + GRIPPER_NAME, + JOINT_NAMES, + POLICY_EXECUTION_RATE, + URDF_PATH, + ) + from common.dataset_helpers import load_and_sync_dataset + from common.policy_helpers import ( + DEFAULT_ROBOT_NAME, + convert_predictions_to_horizon, + embodiment_names_ordered, + get_policy_embodiments, + gripper_open_at_index, + horizon_length, + log_sync_step_for_policy, + print_policy_embodiments, + urdf_cfg_from_horizon, + ) + from neuracore_types import DataType + # --------------------------------------------------------- # 1. Argument Parsing # --------------------------------------------------------- @@ -203,7 +205,12 @@ ) urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) - state = {"horizon": None, "action_idx": 0, "playing": False, "rgb_handle": None} + state: dict[str, Any] = { + "horizon": None, + "action_idx": 0, + "playing": False, + "rgb_handle": None, + } def select_random_state() -> None: """Pulls a random observation from the dataset and queries the AI policy.""" diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 45d444a..67d2d0a 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -""" -Piper Robot Teleoperation with Meta Quest and Foot Pedals. +"""Piper Robot Teleoperation with Meta Quest and Foot Pedals. This script provides a unified control interface for the AgileX Piper robot, combining the 6D spatial tracking of a Meta Quest controller with the diff --git a/examples/combine_code.py b/examples/combine_code.py deleted file mode 100644 index d12a634..0000000 --- a/examples/combine_code.py +++ /dev/null @@ -1,56 +0,0 @@ -import pathlib - - -def combine_python_files(directory_path, output_filename): - """ - Recursively finds all .py files in a directory and combines them into a single text file. - - Args: - directory_path (str): The path to the root directory to search. - output_filename (str): The name/path of the output text file. - """ - # Create a Path object for the target directory - source_dir = pathlib.Path(directory_path) - - # Ensure the directory exists - if not source_dir.is_dir(): - print(f"Error: The directory '{directory_path}' does not exist.") - return - - # Open the output file in write mode - with open(output_filename, "w", encoding="utf-8") as outfile: - # rglob('*.py') recursively searches for all files ending in .py - py_files = list(source_dir.rglob("*.py")) - - if not py_files: - print(f"No Python files found in '{directory_path}'.") - return - - print(f"Found {len(py_files)} Python files. Combining...") - - for file_path in py_files: - # Write a clear separator and the file path as a header - outfile.write(f"\n{'='*60}\n") - outfile.write(f"FILE: {file_path}\n") - outfile.write(f"{'='*60}\n\n") - - # Read the python file and append its contents - try: - with open(file_path, "r", encoding="utf-8") as infile: - outfile.write(infile.read()) - outfile.write("\n") - except Exception as e: - error_msg = f"Error reading file {file_path}: {e}\n" - print(error_msg) - outfile.write(error_msg) - - print(f"Success! All files have been combined into '{output_filename}'.") - - -# --- Example Usage --- -if __name__ == "__main__": - # Replace these variables with your actual paths - TARGET_DIRECTORY = "./" # "./" means current directory - OUTPUT_FILE = "combined_code.txt" - - combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE) diff --git a/examples/common/config_parser.py b/examples/common/config_parser.py index 2e3bc3c..bf1c8e2 100644 --- a/examples/common/config_parser.py +++ b/examples/common/config_parser.py @@ -1,6 +1,8 @@ +"""Utilities for loading teleoperation and IK configuration from YAML files.""" + from pathlib import Path -import yaml +import yaml # type: ignore[import] def load_ik_config(config_path: str) -> dict: diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index 4f93f36..da0461c 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -14,14 +14,6 @@ from .configs import GRIPPER_NAME, JOINT_NAMES from .one_euro_filter import OneEuroFilterTransform -from .states import ( - CameraState, - ControllerState, - IKState, - RobotActivityState, - RobotState, - TeleopState, -) class RobotActivityState(Enum): @@ -208,6 +200,7 @@ def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: # ============================================================================ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: + """Return the latest controller pose, grip, and trigger values.""" with self._controller_state._lock: return ( ( @@ -222,6 +215,7 @@ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: def set_controller_data( self, transform: np.ndarray | None, grip: float, trigger: float ) -> None: + """Update the controller transform and button values.""" if transform is not None and transform.shape != (4, 4): raise ValueError("Transform must be a 4x4 matrix") if grip < 0.0 or grip > 1.0: @@ -263,12 +257,14 @@ def set_controller_data( def set_controller_filter_params( self, min_cutoff: float, beta: float, d_cutoff: float ) -> None: + """Set the filter parameters used to smooth controller motion.""" with self._controller_state._lock: self._controller_state.min_cutoff = min_cutoff self._controller_state.beta = beta self._controller_state.d_cutoff = d_cutoff def get_controller_filter_params(self) -> tuple[float, float, float]: + """Return the current controller filter parameters.""" with self._controller_state._lock: return ( self._controller_state.min_cutoff, @@ -286,6 +282,7 @@ def set_teleop_state( controller_initial: np.ndarray | None, robot_initial: np.ndarray | None, ) -> None: + """Enable or disable teleoperation and store initial transforms.""" with self._teleop_state._lock: self._teleop_state.active = active self._teleop_state.controller_initial_transform = ( @@ -298,6 +295,7 @@ def set_teleop_state( def set_teleop_scaling( self, translation_scale: float, rotation_scale: float ) -> None: + """Update the teleoperation scaling factors.""" if translation_scale <= 0.0 or rotation_scale <= 0.0: return with self._teleop_state._lock: @@ -305,6 +303,7 @@ def set_teleop_scaling( self._teleop_state.rotation_scale = rotation_scale def get_teleop_scaling(self) -> tuple[float, float]: + """Return the current teleoperation scaling settings.""" with self._teleop_state._lock: return ( self._teleop_state.translation_scale, @@ -312,14 +311,17 @@ def get_teleop_scaling(self) -> tuple[float, float]: ) def get_teleop_active(self) -> bool: + """Return whether teleoperation is currently active.""" with self._teleop_state._lock: return self._teleop_state.active def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: + """Enable or disable slow scaling mode.""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = enabled def toggle_slow_scaling_mode_enabled(self) -> bool: + """Toggle slow scaling mode and return the new state.""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = ( not self._teleop_state.slow_scaling_mode_enabled @@ -327,12 +329,14 @@ def toggle_slow_scaling_mode_enabled(self) -> bool: return self._teleop_state.slow_scaling_mode_enabled def get_slow_scaling_mode_enabled(self) -> bool: + """Return whether slow scaling mode is enabled.""" with self._teleop_state._lock: return self._teleop_state.slow_scaling_mode_enabled def get_initial_robot_controller_transforms( self, ) -> tuple[np.ndarray | None, np.ndarray | None]: + """Return the initial controller and robot transforms for teleop.""" with self._teleop_state._lock: return ( ( @@ -352,14 +356,17 @@ def get_initial_robot_controller_transforms( # ============================================================================ def get_robot_activity_state(self) -> RobotActivityState: + """Return the current robot activity state.""" with self._robot_state._lock: return self._robot_state.activity_state def set_robot_activity_state(self, state: RobotActivityState) -> None: + """Change the robot activity state.""" with self._robot_state._lock: self._robot_state.activity_state = state def get_current_joint_angles(self) -> np.ndarray | None: + """Return the current robot joint angles in degrees.""" with self._robot_state._lock: return ( self._robot_state.joint_angles.copy() @@ -368,6 +375,7 @@ def get_current_joint_angles(self) -> np.ndarray | None: ) def set_current_joint_angles(self, angles: np.ndarray) -> None: + """Update the current joint angles and log them if callbacks are enabled.""" with self._robot_state._lock: self._robot_state.joint_angles = angles.copy() if self._on_change_callback: @@ -379,6 +387,7 @@ def set_current_joint_angles(self, angles: np.ndarray) -> None: self._queue_callback("log_joint_positions", payload, time.time()) def get_current_joint_torques(self) -> np.ndarray | None: + """Return the current robot joint torques.""" with self._robot_state._lock: return ( self._robot_state.joint_torques.copy() @@ -387,6 +396,7 @@ def get_current_joint_torques(self) -> np.ndarray | None: ) def set_current_joint_torques(self, torques: np.ndarray) -> None: + """Update current robot joint torque readings.""" with self._robot_state._lock: self._robot_state.joint_torques = torques.copy() if self._on_change_callback: @@ -396,6 +406,7 @@ def set_current_joint_torques(self, torques: np.ndarray) -> None: self._queue_callback("log_joint_torques", payload, time.time()) def get_current_end_effector_pose(self) -> np.ndarray | None: + """Return the current end effector pose.""" with self._robot_state._lock: return ( self._robot_state.end_effector_pose.copy() @@ -404,14 +415,17 @@ def get_current_end_effector_pose(self) -> np.ndarray | None: ) def set_current_end_effector_pose(self, pose: np.ndarray) -> None: + """Update the current end effector pose.""" with self._robot_state._lock: self._robot_state.end_effector_pose = pose.copy() def get_current_gripper_open_value(self) -> float | None: + """Return the current gripper open amount.""" with self._robot_state._lock: return self._robot_state.current_gripper_open_value def set_current_gripper_open_value(self, value: float) -> None: + """Update the current gripper open value.""" with self._robot_state._lock: self._robot_state.current_gripper_open_value = value if self._on_change_callback: @@ -422,10 +436,12 @@ def set_current_gripper_open_value(self, value: float) -> None: ) def get_target_gripper_open_value(self) -> float | None: + """Return the target gripper open amount.""" with self._robot_state._lock: return self._robot_state.target_gripper_open_value def set_target_gripper_open_value(self, value: float) -> None: + """Update the target gripper open amount.""" with self._robot_state._lock: self._robot_state.target_gripper_open_value = value if self._on_change_callback: @@ -440,6 +456,7 @@ def set_target_gripper_open_value(self, value: float) -> None: # ============================================================================ def get_target_joint_angles(self) -> np.ndarray | None: + """Return the latest target joint angles from the IK solver.""" with self._ik_state._lock: return ( self._ik_state.target_joint_angles.copy() @@ -448,6 +465,7 @@ def get_target_joint_angles(self) -> np.ndarray | None: ) def set_target_joint_angles(self, angles: np.ndarray) -> None: + """Store the target joint angle command from the IK solver.""" with self._ik_state._lock: self._ik_state.target_joint_angles = angles.copy() if self._on_change_callback: @@ -459,12 +477,14 @@ def set_target_joint_angles(self, angles: np.ndarray) -> None: self._queue_callback("log_joint_target_positions", payload, time.time()) def set_target_pose(self, transform: np.ndarray | None) -> None: + """Store the latest IK target end effector pose.""" with self._ik_state._lock: self._ik_state.target_pose = ( transform.copy() if transform is not None else None ) def get_target_pose(self) -> np.ndarray | None: + """Return the latest IK target pose.""" with self._ik_state._lock: return ( self._ik_state.target_pose.copy() @@ -473,18 +493,22 @@ def get_target_pose(self) -> np.ndarray | None: ) def set_ik_solve_time_ms(self, time_ms: float) -> None: + """Record the latest IK solve duration in milliseconds.""" with self._ik_state._lock: self._ik_state.solve_time_ms = time_ms def set_ik_success(self, success: bool) -> None: + """Record whether the last IK solve succeeded.""" with self._ik_state._lock: self._ik_state.success = success def get_ik_solve_time_ms(self) -> float: + """Return the last IK solve time in milliseconds.""" with self._ik_state._lock: return self._ik_state.solve_time_ms def get_ik_success(self) -> bool: + """Return whether the last IK solve succeeded.""" with self._ik_state._lock: return self._ik_state.success diff --git a/examples/common/dataset_helpers.py b/examples/common/dataset_helpers.py index a52528b..6519e31 100644 --- a/examples/common/dataset_helpers.py +++ b/examples/common/dataset_helpers.py @@ -1,3 +1,7 @@ +"""Helpers for loading and synchronizing Neuracore datasets.""" + +from typing import Any + import neuracore as nc from neuracore.core.utils.robot_data_spec_utils import ( merge_cross_embodiment_description, @@ -11,7 +15,7 @@ def load_and_sync_dataset( input_modalities: list[DataType] | None = None, output_modalities: list[DataType] | None = None, prefetch_videos: bool = False, -): +) -> Any: """Loads a Neuracore dataset and synchronizes the specified modalities across embodiments.""" print(f"\n🔍 Getting dataset '{dataset_name}' from Neuracore...") dataset = nc.get_dataset(dataset_name) diff --git a/examples/common/policy_actions.py b/examples/common/policy_actions.py index ff52097..3d273ed 100644 --- a/examples/common/policy_actions.py +++ b/examples/common/policy_actions.py @@ -132,8 +132,13 @@ def end_policy_play( def continuous_prediction_worker( - data_manager, policy, policy_state, visualizer, input_emb, continuous_mode -): + data_manager: DataManager, + policy: Any, + policy_state: PolicyState, + visualizer: Optional[Any], + input_emb: dict, + continuous_mode: str, +) -> None: """Background thread for continuous receding horizon execution.""" colors = [ (1.0, 0.65, 0.0, 0.25), @@ -204,13 +209,13 @@ def continuous_prediction_worker( def play_policy( - data_manager, - policy, - policy_state, - visualizer, - input_emb, - continuous_mode="pipeline", -): + data_manager: DataManager, + policy: Any, + policy_state: PolicyState, + visualizer: Optional[Any], + input_emb: dict, + continuous_mode: str = "pipeline", +) -> None: """Toggle for starting/stopping continuous policy mode.""" if not policy_state.get_continuous_play_active(): print(f"▶️ Starting {continuous_mode.capitalize()} Mode...") @@ -237,8 +242,13 @@ def play_policy( def policy_execution_thread( - policy, data_manager, policy_state, robot_controller, visualizer, input_emb -): + policy: Any, + data_manager: DataManager, + policy_state: PolicyState, + robot_controller: Any, + visualizer: Optional[Any], + input_emb: dict, +) -> None: """The thread that continuously reads the locked horizon and sends joint commands.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE last_vis_update = 0.0 diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index af84a25..c34bdf5 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -1,9 +1,12 @@ #!/usr/bin/env python3 """Shared robot visualizer for Piper robot demos. + This module acts as a facade, delegating 3D rendering to RobotVisualizerCore and 2D UI elements to RobotVisualizerGUI. """ +from typing import Any + from .visualizer_core import RobotVisualizerCore from .visualizer_gui import RobotVisualizerGUI @@ -16,9 +19,9 @@ def __init__(self, urdf_path: str) -> None: self._core = RobotVisualizerCore(urdf_path) self._gui = RobotVisualizerGUI(self._core.server) - def __getattr__(self, name: str): - """ - Dynamically delegates method calls to the Core or GUI modules. + def __getattr__(self, name: str) -> Any: + """Dynamically delegates method calls to the Core or GUI modules. + If a top-level script calls `visualizer.add_basic_controls()`, this routes it to `_gui.add_basic_controls()`. """ diff --git a/examples/common/shared_actions.py b/examples/common/shared_actions.py index 4a3d6ca..cd64537 100644 --- a/examples/common/shared_actions.py +++ b/examples/common/shared_actions.py @@ -1,3 +1,5 @@ +"""Common robot action helpers shared by example scripts.""" + import subprocess from typing import Any @@ -8,8 +10,10 @@ def toggle_robot_enabled( - data_manager: DataManager, robot_controller: PiperController, visualizer=None -): + data_manager: DataManager, + robot_controller: PiperController, + visualizer: Any = None, +) -> None: """Safely toggles the robot between ENABLED and DISABLED states.""" state = data_manager.get_robot_activity_state() @@ -37,7 +41,9 @@ def toggle_robot_enabled( print("✗ Failed to enable robot") -def move_robot_home(data_manager: DataManager, robot_controller: PiperController): +def move_robot_home( + data_manager: DataManager, robot_controller: PiperController +) -> None: """Safely commands the robot to return to its home position.""" state = data_manager.get_robot_activity_state() diff --git a/examples/common/states.py b/examples/common/states.py index 525e217..5c5971b 100644 --- a/examples/common/states.py +++ b/examples/common/states.py @@ -1,3 +1,5 @@ +"""State containers used by the Piper example suite.""" + import threading from enum import Enum @@ -18,6 +20,7 @@ class ControllerState: """Controller input state - Quest Reader writes, IK/Joint reads.""" def __init__(self) -> None: + """Initialize controller input state and filtering state.""" self._lock = threading.Lock() self.min_cutoff: float = 1.0 self.beta: float = 0.0 @@ -33,6 +36,7 @@ class TeleopState: """Teleop activation state - manages teleop start/stop.""" def __init__(self) -> None: + """Initialize teleop activation and scaling state.""" self._lock = threading.Lock() self.active: bool = False self.controller_initial_transform: np.ndarray | None = None @@ -46,6 +50,7 @@ class RobotState: """Current robot state - joint angles, end effector pose, activity state.""" def __init__(self) -> None: + """Initialize robot telemetry and activity state.""" self._lock = threading.Lock() self.joint_angles: np.ndarray | None = None self.joint_torques: np.ndarray | None = None @@ -59,6 +64,7 @@ class IKState: """IK solution state - target joint angles, pose, metrics.""" def __init__(self) -> None: + """Initialize IK target and status information.""" self._lock = threading.Lock() self.target_joint_angles: np.ndarray | None = None self.target_pose: np.ndarray | None = None @@ -70,5 +76,6 @@ class CameraState: """Camera state - RGB images for one or more cameras.""" def __init__(self) -> None: + """Initialize camera image storage.""" self._lock = threading.Lock() self.rgb_images: dict[str, np.ndarray] = {} diff --git a/examples/common/system_bootstrap.py b/examples/common/system_bootstrap.py index 3008517..c8beee8 100644 --- a/examples/common/system_bootstrap.py +++ b/examples/common/system_bootstrap.py @@ -1,3 +1,5 @@ +"""Bootstrap robot subsystems for the AgileX Piper example suite.""" + import threading from typing import List, Optional, Tuple @@ -25,7 +27,7 @@ def bootstrap_robot_system( ) -> Tuple[ DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread] ]: - + """Create and start the shared robot subsystems used by deployment scripts.""" # Extract config sections safely filt_p = config.get("filter_parameters", {}) tele_p = config.get("teleop_parameters", {}) diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py index 95b6baa..eee28b6 100644 --- a/examples/common/threads/realsense_camera.py +++ b/examples/common/threads/realsense_camera.py @@ -23,7 +23,7 @@ def camera_thread(data_manager: DataManager) -> None: fps_timer = time.time() frame_count = 0 # Store the last 100 frame intervals to check for extreme jitter - intervals = deque(maxlen=100) + intervals: deque[float] = deque(maxlen=100) last_frame_time = time.time() try: diff --git a/examples/common/visualizer_core.py b/examples/common/visualizer_core.py index 37ba756..c71b1be 100644 --- a/examples/common/visualizer_core.py +++ b/examples/common/visualizer_core.py @@ -1,3 +1,7 @@ +"""Module-level helpers for the robot visualizer.""" + +from typing import Any + import numpy as np import viser import yourdfpy @@ -9,6 +13,7 @@ class RobotVisualizerCore: """Handles the 3D rendering of the robot, ghost robot, and target frames.""" def __init__(self, urdf_path: str) -> None: + """TODO: Document this method.""" self.server = viser.ViserServer() self.server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1) @@ -25,21 +30,24 @@ def __init__(self, urdf_path: str) -> None: mesh_color_override=(1.0, 0.65, 0.0, 0.25), ) - self.controller_handle = None - self.target_frame_handle = None - self.rgb_image_handle = None + self.controller_handle: Any = None + self.target_frame_handle: Any = None + self.rgb_image_handle: Any = None def add_controller_visualization(self) -> None: + """TODO: Document this method.""" self.controller_handle = self.server.scene.add_transform_controls( "/controller", scale=0.15, position=(0, 0, 0), wxyz=(1, 0, 0, 0) ) def add_target_frame_visualization(self) -> None: + """TODO: Document this method.""" self.target_frame_handle = self.server.scene.add_frame( "/target_goal", axes_length=0.1, axes_radius=0.003 ) def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None: + """TODO: Document this method.""" if self.rgb_image_handle is None: dummy_image = np.zeros((height, width, 3), dtype=np.uint8) self.rgb_image_handle = self.server.gui.add_image( @@ -47,6 +55,7 @@ def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None ) def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: + """TODO: Document this method.""" if rgb_image is None: return if self.rgb_image_handle is None: @@ -56,26 +65,32 @@ def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: self.rgb_image_handle.image = rgb_image def update_robot_pose(self, joint_config: np.ndarray) -> None: + """TODO: Document this method.""" self.urdf_vis.update_cfg(joint_config) def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None: + """TODO: Document this method.""" if self.ghost_robot_urdf: self.ghost_robot_urdf.update_cfg(joint_config) def update_ghost_robot_visibility(self, flag: bool) -> None: + """TODO: Document this method.""" if self.ghost_robot_urdf: self.ghost_robot_urdf.show_visual = flag def get_ghost_robot_visibility(self) -> bool: + """TODO: Document this method.""" if self.ghost_robot_urdf: return self.ghost_robot_urdf.show_visual return False def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + """TODO: Document this method.""" if self.ghost_robot_urdf: self.ghost_robot_urdf.mesh_color_override = color def update_controller_visualization(self, transform: np.ndarray | None) -> None: + """TODO: Document this method.""" if self.controller_handle is None or transform is None: return pos = transform[:3, 3] @@ -84,6 +99,7 @@ def update_controller_visualization(self, transform: np.ndarray | None) -> None: self.controller_handle.wxyz = (rot[3], rot[0], rot[1], rot[2]) def update_target_visualization(self, transform: np.ndarray | None) -> None: + """TODO: Document this method.""" if self.target_frame_handle is None or transform is None: return pos = transform[:3, 3] @@ -92,4 +108,5 @@ def update_target_visualization(self, transform: np.ndarray | None) -> None: self.target_frame_handle.wxyz = (rot[3], rot[0], rot[1], rot[2]) def stop(self) -> None: + """TODO: Document this method.""" self.server.stop() diff --git a/examples/common/visualizer_gui.py b/examples/common/visualizer_gui.py index 35b2c06..7c5f249 100644 --- a/examples/common/visualizer_gui.py +++ b/examples/common/visualizer_gui.py @@ -1,3 +1,5 @@ +"""Module-level helpers for the robot visualizer.""" + from typing import Any, Callable import numpy as np @@ -8,55 +10,56 @@ class RobotVisualizerGUI: """Handles all the 2D UI elements: buttons, sliders, and text readouts.""" def __init__(self, server: viser.ViserServer) -> None: + """TODO: Document this method.""" self.server = server self._ema_timing = 0.001 # State Handles - self._timing_handle = None - self._joint_angles_handle = None - self._robot_status_handle = None - self._teleop_status_handle = None - self._controller_status_handle = None - self._gripper_status_handle = None - self._policy_status_handle = None + self._timing_handle: Any = None + self._joint_angles_handle: Any = None + self._robot_status_handle: Any = None + self._teleop_status_handle: Any = None + self._controller_status_handle: Any = None + self._gripper_status_handle: Any = None + self._policy_status_handle: Any = None # Input Handles - self._position_weight_handle = None - self._orientation_weight_handle = None - self._frame_task_gain_handle = None - self._lm_damping_handle = None - self._damping_weight_handle = None - self._solver_damping_value_handle = None - self._posture_cost_handles = [] - - self._controller_min_cutoff_handle = None - self._controller_beta_handle = None - self._controller_d_cutoff_handle = None - self._translation_scale_handle = None - self._rotation_scale_handle = None - - self._prediction_ratio_handle = None - self._policy_execution_rate_handle = None - self._robot_rate_handle = None - self._execution_mode_dropdown = None - - self._grip_value_handle = None - self._trigger_value_handle = None + self._position_weight_handle: Any = None + self._orientation_weight_handle: Any = None + self._frame_task_gain_handle: Any = None + self._lm_damping_handle: Any = None + self._damping_weight_handle: Any = None + self._solver_damping_value_handle: Any = None + self._posture_cost_handles: list[Any] = [] + + self._controller_min_cutoff_handle: Any = None + self._controller_beta_handle: Any = None + self._controller_d_cutoff_handle: Any = None + self._translation_scale_handle: Any = None + self._rotation_scale_handle: Any = None + + self._prediction_ratio_handle: Any = None + self._policy_execution_rate_handle: Any = None + self._robot_rate_handle: Any = None + self._execution_mode_dropdown: Any = None + + self._grip_value_handle: Any = None + self._trigger_value_handle: Any = None # Button Handles - self._enable_robot_handle = None - self._disable_robot_handle = None - self._emergency_stop_handle = None - self._go_home_button = None - self._toggle_robot_enabled_status_button = None - self._run_policy_button = None - self._start_policy_execution_button = None - self._play_policy_button = None - - self._slow_translation_scale_handle = None - self._slow_rotation_scale_handle = None - self._wrist_step_handle = None - self._save_config_button = None + self._enable_robot_handle: Any = None + self._disable_robot_handle: Any = None + self._emergency_stop_handle: Any = None + self._go_home_button: Any = None + self._toggle_robot_enabled_status_button: Any = None + self._run_policy_button: Any = None + self._start_policy_execution_button: Any = None + self._play_policy_button: Any = None + + self._slow_translation_scale_handle: Any = None + self._slow_rotation_scale_handle: Any = None + self._wrist_step_handle: Any = None + self._save_config_button: Any = None def add_advanced_tuning_controls( self, initial_slow_t: float, initial_slow_r: float, initial_wrist_step: float @@ -74,19 +77,24 @@ def add_advanced_tuning_controls( self._save_config_button = self.server.gui.add_button("💾 Save Config to YAML") def get_slow_translation_scale(self) -> float: + """TODO: Document this method.""" return self._slow_translation_scale_handle.value def get_slow_rotation_scale(self) -> float: + """TODO: Document this method.""" return self._slow_rotation_scale_handle.value def get_wrist_step_degrees(self) -> float: + """TODO: Document this method.""" return self._wrist_step_handle.value def set_save_config_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._save_config_button: self._save_config_button.on_click(lambda _: cb()) def add_basic_controls(self) -> None: + """TODO: Document this method.""" self._timing_handle = self.server.gui.add_number( "IK Solve Time (ms)", 0.001, disabled=True ) @@ -95,11 +103,13 @@ def add_basic_controls(self) -> None: ) def add_robot_status_controls(self) -> None: + """TODO: Document this method.""" self._robot_status_handle = self.server.gui.add_text( "Robot Status", "Initializing..." ) def add_teleop_controls(self) -> None: + """TODO: Document this method.""" self._grip_value_handle = self.server.gui.add_number( "Grip Value", 0.0, disabled=True ) @@ -114,14 +124,17 @@ def add_teleop_controls(self) -> None: ) def add_gripper_status_controls(self) -> None: + """TODO: Document this method.""" self._gripper_status_handle = self.server.gui.add_text( "Gripper Status", "Open (0%)" ) def add_homing_controls(self) -> None: + """TODO: Document this method.""" self._go_home_button = self.server.gui.add_button("Go Home") def add_toggle_robot_enabled_status_button(self) -> None: + """TODO: Document this method.""" self._toggle_robot_enabled_status_button = self.server.gui.add_button( "Enable Robot" ) @@ -129,6 +142,7 @@ def add_toggle_robot_enabled_status_button(self) -> None: def add_controller_filter_controls( self, initial_min_cutoff: float, initial_beta: float, initial_d_cutoff: float ) -> None: + """TODO: Document this method.""" self._controller_min_cutoff_handle = self.server.gui.add_number( "Controller Min Cutoff", initial_min_cutoff, min=0.01, max=10.0, step=0.01 ) @@ -142,6 +156,7 @@ def add_controller_filter_controls( def add_scaling_controls( self, initial_translation_scale: float, initial_rotation_scale: float ) -> None: + """TODO: Document this method.""" self._translation_scale_handle = self.server.gui.add_number( "Translation Scale", initial_translation_scale, @@ -163,6 +178,7 @@ def add_pink_parameter_controls( solver_damping_value: float, posture_cost_vector: list[float], ) -> None: + """TODO: Document this method.""" self._position_weight_handle = self.server.gui.add_number( "Position Weight", position_cost, min=0.0, max=10.0, step=0.1 ) @@ -196,6 +212,7 @@ def add_policy_controls( initial_robot_rate: float = 200.0, initial_execution_mode: str = "targeting_time", ) -> None: + """TODO: Document this method.""" self._policy_status_handle = self.server.gui.add_text("Policy Status", "Ready") self._prediction_ratio_handle = self.server.gui.add_number( "Prediction Ratio", initial_prediction_ratio, min=0.0, max=1.0, step=0.01 @@ -213,6 +230,7 @@ def add_policy_controls( ) def add_policy_buttons(self) -> None: + """TODO: Document this method.""" self._run_policy_button = self.server.gui.add_button("Run Policy (Preview)") self._start_policy_execution_button = self.server.gui.add_button( "Execute Policy (Run Preview)" @@ -223,16 +241,21 @@ def add_policy_buttons(self) -> None: # --- UI Update Methods --- def update_timing(self, solve_time_ms: float) -> None: + """TODO: Document this method.""" if self._timing_handle: self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms self._timing_handle.value = self._ema_timing def update_robot_status(self, status: str) -> None: + """TODO: Document this method.""" if self._robot_status_handle: + """TODO: Document this method.""" self._robot_status_handle.value = status def update_teleop_status(self, active: bool) -> None: + """TODO: Document this method.""" if self._teleop_status_handle: + """TODO: Document this method.""" self._teleop_status_handle.value = ( f"Teleop Status: {'Active' if active else 'Inactive'}" ) @@ -240,6 +263,7 @@ def update_teleop_status(self, active: bool) -> None: def update_controller_status_display( self, position: np.ndarray | None, connected: bool = True ) -> None: + """TODO: Document this method.""" if not self._controller_status_handle: return if connected and position is not None: @@ -250,6 +274,7 @@ def update_controller_status_display( def update_gripper_status( self, trigger_value: float, robot_enabled: bool = True ) -> None: + """TODO: Document this method.""" if not self._gripper_status_handle: return state = ( @@ -263,17 +288,23 @@ def update_gripper_status( ) def update_policy_status(self, status: str) -> None: + """TODO: Document this method.""" if self._policy_status_handle: + """TODO: Document this method.""" self._policy_status_handle.value = status def update_toggle_robot_enabled_status(self, enabled: bool) -> None: + """TODO: Document this method.""" if self._toggle_robot_enabled_status_button: + """TODO: Document this method.""" self._toggle_robot_enabled_status_button.label = ( "Disable Robot" if enabled else "Enable Robot" ) def update_play_policy_button_status(self, active: bool) -> None: + """TODO: Document this method.""" if self._play_policy_button: + """TODO: Document this method.""" self._play_policy_button.label = ( "Stop Continuous Horizon" if active else "Continuous Receding Horizon" ) @@ -281,6 +312,7 @@ def update_play_policy_button_status(self, active: bool) -> None: def update_joint_angles_display( self, joint_config: np.ndarray, show_gripper: bool = False ) -> None: + """TODO: Document this method.""" if not self._joint_angles_handle: return lines = ["Joint Angles (rad):"] @@ -297,15 +329,20 @@ def update_joint_angles_display( self._joint_angles_handle.value = "\n".join(lines) def set_grip_value(self, value: float) -> None: + """TODO: Document this method.""" if self._grip_value_handle: + """TODO: Document this method.""" self._grip_value_handle.value = value def set_trigger_value(self, value: float) -> None: + """TODO: Document this method.""" if self._trigger_value_handle: + """TODO: Document this method.""" self._trigger_value_handle.value = value # --- Getters --- def get_controller_filter_params(self) -> tuple[float, float, float]: + """TODO: Document this method.""" return ( self._controller_min_cutoff_handle.value, self._controller_beta_handle.value, @@ -313,24 +350,31 @@ def get_controller_filter_params(self) -> tuple[float, float, float]: ) def get_translation_scale(self) -> float: + """TODO: Document this method.""" return self._translation_scale_handle.value def get_rotation_scale(self) -> float: + """TODO: Document this method.""" return self._rotation_scale_handle.value def get_prediction_ratio(self) -> float: + """TODO: Document this method.""" return self._prediction_ratio_handle.value def get_policy_execution_rate(self) -> float: + """TODO: Document this method.""" return self._policy_execution_rate_handle.value def get_robot_rate(self) -> float: + """TODO: Document this method.""" return self._robot_rate_handle.value def get_execution_mode(self) -> str: + """TODO: Document this method.""" return self._execution_mode_dropdown.value def get_pink_parameters(self) -> dict: + """TODO: Document this method.""" return { "position_cost": self._position_weight_handle.value, "orientation_cost": self._orientation_weight_handle.value, @@ -345,37 +389,55 @@ def get_pink_parameters(self) -> dict: # --- Button Setters/Callbacks --- def set_toggle_robot_enabled_status_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._toggle_robot_enabled_status_button: + """TODO: Document this method.""" self._toggle_robot_enabled_status_button.on_click(lambda _: cb()) def set_go_home_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._go_home_button: + """TODO: Document this method.""" self._go_home_button.on_click(lambda _: cb()) def set_run_policy_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._run_policy_button: + """TODO: Document this method.""" self._run_policy_button.on_click(lambda _: cb()) def set_start_policy_execution_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._start_policy_execution_button: + """TODO: Document this method.""" self._start_policy_execution_button.on_click(lambda _: cb()) def set_play_policy_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._play_policy_button: + """TODO: Document this method.""" self._play_policy_button.on_click(lambda _: cb()) def set_execution_mode_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" if self._execution_mode_dropdown: + """TODO: Document this method.""" self._execution_mode_dropdown.on_update(lambda _: cb()) def set_run_policy_button_disabled(self, disabled: bool) -> None: + """TODO: Document this method.""" if self._run_policy_button: + """TODO: Document this method.""" self._run_policy_button.disabled = disabled def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: + """TODO: Document this method.""" if self._start_policy_execution_button: + """TODO: Document this method.""" self._start_policy_execution_button.disabled = disabled def set_play_policy_button_disabled(self, disabled: bool) -> None: + """TODO: Document this method.""" if self._play_policy_button: + """TODO: Document this method.""" self._play_policy_button.disabled = disabled diff --git a/piper_controller.py b/piper_controller.py index 44605b6..bc87a60 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -238,6 +238,7 @@ def get_control_mode(self) -> "PiperController.ControlMode": # print(f"Control mode changed: {old_mode.value} -> {mode.value}") def set_control_mode(self, mode: "PiperController.ControlMode") -> None: + """Set the current robot control mode and propagate the configuration to the hardware.""" with self.state_lock: old_mode = self._control_mode self._control_mode = mode From ed41cf4c285263e54d579c037a891f3938f4eaac Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 27 May 2026 16:58:07 +0100 Subject: [PATCH 22/22] fix:remove the unnecessary note file --- halid_notes.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 halid_notes.txt diff --git a/halid_notes.txt b/halid_notes.txt deleted file mode 100644 index e69de29..0000000