From f70c27db23c460a8fdacaea1298bd1e723172e51 Mon Sep 17 00:00:00 2001 From: kwangneuraco Date: Thu, 28 May 2026 17:32:38 +0100 Subject: [PATCH 1/2] feat: (1)remove the meta quest dependency on example 4 (2) set default position of robot for icra --- examples/4_rollout_neuracore_policy.py | 120 ++----------------------- examples/common/configs.py | 3 +- piper_controller.py | 6 +- 3 files changed, 10 insertions(+), 119 deletions(-) diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 6ceb550..88fc1b3 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -17,34 +17,20 @@ import numpy as np from neuracore_types import DataType, EmbodimentDescription -# Add parent directory to path to import pink_ik_solver and piper_controller +# Add parent directory to path to import 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_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, @@ -61,13 +47,9 @@ ) 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 @@ -82,8 +64,6 @@ def toggle_robot_enabled_status( # 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 (Button A)") elif robot_activity_state == RobotActivityState.DISABLED: @@ -100,10 +80,7 @@ def home_robot(data_manager: DataManager, robot_controller: PiperController) -> 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") @@ -224,9 +201,6 @@ def start_policy_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() @@ -276,7 +250,6 @@ def end_policy_play( 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) @@ -531,17 +504,6 @@ def update_visualization( # 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) @@ -596,12 +558,6 @@ def update_visualization( 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)", - ) policy_group = parser.add_mutually_exclusive_group(required=True) policy_group.add_argument( "--train-run-name", @@ -633,8 +589,6 @@ def update_visualization( 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") @@ -696,11 +650,6 @@ def update_visualization( # 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...") @@ -723,48 +672,6 @@ def update_visualization( ) 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) - - # 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 camera thread print("\nšŸ“· Starting camera thread...") camera_thread_obj = threading.Thread( @@ -827,15 +734,6 @@ def update_visualization( ) ) - # Register Quest reader button callbacks (after visualizer is created) - 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( @@ -854,15 +752,10 @@ def update_visualization( policy_execution_thread_obj.start() print() - print("šŸš€ Starting teleoperation with policy testing...") - print("šŸŽ® CONTROLS:") - 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") + print("šŸš€ Starting policy rollout...") + print("šŸ–„ļø CONTROLS (Viser):") + print(" 1. Click 'Enable Robot' button to enable/disable robot") + print(" 2. Click '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") @@ -890,9 +783,6 @@ def update_visualization( # shutdown threads data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - quest_thread.join() - quest_reader.stop() - ik_thread.join() camera_thread_obj.join() robot_controller.cleanup() diff --git a/examples/common/configs.py b/examples/common/configs.py index e8142b0..40abc7b 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -55,8 +55,7 @@ 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] NEUTRAL_END_EFFECTOR_POSE = [455.257, -46.344, 172.213, 176.205, -14.545, 29.621] # Posture task cost vector (one weight per joint) diff --git a/piper_controller.py b/piper_controller.py index e979bcc..6ba1121 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 = 80.00 self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees @@ -491,7 +491,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 47d87e7c66d9411a367153002e3aef3e95c8f9a0 Mon Sep 17 00:00:00 2001 From: kwangneuraco Date: Sun, 31 May 2026 16:14:11 +0100 Subject: [PATCH 2/2] feat: make use meta-quest optional --- examples/4_rollout_neuracore_policy.py | 161 ++++++++++++++++++++++++- 1 file changed, 157 insertions(+), 4 deletions(-) diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 88fc1b3..0478227 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -23,14 +23,28 @@ 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_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, @@ -52,6 +66,11 @@ from piper_controller import PiperController +# Default for Meta Quest teleop. Set to True to enable teleop by default, or +# pass --use-meta-quest on the command line to force it on for a single run. +# When disabled, the script runs as a pure policy rollout via Viser. +USE_META_QUEST = True + def toggle_robot_enabled_status( data_manager: DataManager, @@ -64,6 +83,8 @@ def toggle_robot_enabled_status( # 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 (Button A)") elif robot_activity_state == RobotActivityState.DISABLED: @@ -80,7 +101,10 @@ def home_robot(data_manager: DataManager, robot_controller: PiperController) -> 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") @@ -201,6 +225,9 @@ def start_policy_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() @@ -250,6 +277,7 @@ def end_policy_play( 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) @@ -504,6 +532,17 @@ def update_visualization( # 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) @@ -558,6 +597,24 @@ def update_visualization( parser = argparse.ArgumentParser( description="Piper Robot Test with Neuracore Policy - REAL ROBOT CONTROL" ) + parser.add_argument( + "--use-meta-quest", + action="store_true", + help=( + "Enable Meta Quest teleoperation (Quest controller drives the robot " + f"via the IK solver). Overrides the USE_META_QUEST={USE_META_QUEST} " + "default in this file." + ), + ) + parser.add_argument( + "--ip-address", + type=str, + default=None, + help=( + "IP address of Meta Quest device (optional, defaults to None for " + "auto-discovery). Only used when Meta Quest teleop is enabled." + ), + ) policy_group = parser.add_mutually_exclusive_group(required=True) policy_group.add_argument( "--train-run-name", @@ -585,10 +642,25 @@ def update_visualization( ) args = parser.parse_args() + # Resolve whether to use Meta Quest teleop: in-code default OR CLI flag. + use_meta_quest = USE_META_QUEST or args.use_meta_quest + + # Lazily import Meta Quest / IK dependencies only when teleop is enabled, so + # the script can run policy-only on machines without meta_quest_teleop. + if use_meta_quest: + from common.threads.ik_solver import ik_solver_thread + from common.threads.quest_reader import quest_reader_thread + from meta_quest_teleop.reader import MetaQuestReader + + from pink_ik_solver import PinkIKSolver + print("=" * 60) print("PIPER ROBOT TEST WITH NEURACORE POLICY") print("=" * 60) print("Thread frequencies:") + if use_meta_quest: + 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") @@ -650,6 +722,12 @@ def update_visualization( # Initialize shared state data_manager = DataManager() + if use_meta_quest: + data_manager.set_controller_filter_params( + CONTROLLER_MIN_CUTOFF, + CONTROLLER_BETA, + CONTROLLER_D_CUTOFF, + ) # Initialize robot controller print("\nšŸ¤– Initializing Piper robot controller...") @@ -672,6 +750,52 @@ def update_visualization( ) joint_state_thread_obj.start() + quest_reader = None + quest_thread = None + ik_thread = None + if use_meta_quest: + # Initialize Meta Quest reader + print("\nšŸŽ® Initializing Meta Quest reader...") + quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) + + # 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 camera thread print("\nšŸ“· Starting camera thread...") camera_thread_obj = threading.Thread( @@ -734,6 +858,18 @@ def update_visualization( ) ) + if use_meta_quest and quest_reader is not None: + # Register Quest reader button callbacks (after visualizer is created) + 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( @@ -752,10 +888,21 @@ def update_visualization( policy_execution_thread_obj.start() print() - print("šŸš€ Starting policy rollout...") - print("šŸ–„ļø CONTROLS (Viser):") - print(" 1. Click 'Enable Robot' button to enable/disable robot") - print(" 2. Click 'Home Robot' button to send robot home") + if use_meta_quest: + print("šŸš€ Starting teleoperation with policy testing...") + print("šŸŽ® CONTROLS:") + 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("šŸš€ Starting policy rollout...") + print("šŸ–„ļø CONTROLS (Viser):") + print(" 1. Click 'Enable Robot' button to enable/disable robot") + print(" 2. Click '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") @@ -783,6 +930,12 @@ def update_visualization( # shutdown threads data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + if quest_thread is not None: + quest_thread.join() + if quest_reader is not None: + quest_reader.stop() + if ik_thread is not None: + ik_thread.join() camera_thread_obj.join() robot_controller.cleanup()