diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 6ceb550..0478227 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -17,7 +17,7 @@ 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 ( @@ -61,15 +61,16 @@ ) 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 +# 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, @@ -596,11 +597,23 @@ 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)", + 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( @@ -629,12 +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:") - print(f" šŸŽ® Quest Controller: {CONTROLLER_DATA_RATE} Hz") - print(f" šŸ§® IK Solver: {IK_SOLVER_RATE} Hz") + 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") @@ -696,11 +722,12 @@ def update_visualization( # Initialize shared state data_manager = DataManager() - data_manager.set_controller_filter_params( - CONTROLLER_MIN_CUTOFF, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - ) + 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...") @@ -723,47 +750,51 @@ 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() + 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), - ) + # 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 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...") @@ -827,14 +858,17 @@ 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) - ) + 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...") @@ -854,15 +888,21 @@ 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") + 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") @@ -890,9 +930,12 @@ 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() + 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() 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