From 3d2d81d91b07e2f15ebeebfead0b526c855d4f79 Mon Sep 17 00:00:00 2001 From: Pruthvi-Neuracore Date: Thu, 5 Mar 2026 16:59:05 +0000 Subject: [PATCH 01/31] feat: add foot pedal control for neuracore recording --- examples/10_leader_arm_teleop_agilex.py | 271 ++++++++++++++++++++++++ examples/7_teleop_with_pedal.py | 191 +++++++++++++++++ scripts/foot_pedal/README.md | 51 +++++ scripts/foot_pedal/pedal_demo.py | 107 ++++++++++ scripts/foot_pedal/run_pedal_config.sh | 9 + 5 files changed, 629 insertions(+) create mode 100644 examples/10_leader_arm_teleop_agilex.py create mode 100644 examples/7_teleop_with_pedal.py create mode 100644 scripts/foot_pedal/README.md create mode 100755 scripts/foot_pedal/pedal_demo.py create mode 100755 scripts/foot_pedal/run_pedal_config.sh diff --git a/examples/10_leader_arm_teleop_agilex.py b/examples/10_leader_arm_teleop_agilex.py new file mode 100644 index 0000000..74b1e0a --- /dev/null +++ b/examples/10_leader_arm_teleop_agilex.py @@ -0,0 +1,271 @@ +#!/usr/bin/env python3 +"""AgileX PiPER Teleoperation using LeRobot Leader Arm and Foot Pedal. + +This script maps a physical LeRobot leader arm and a 3-button Foot Pedal +to a simulated AgileX PiPER robot in Neuracore. +""" + +import argparse +import os +import sys +import threading +import time +import traceback +from pathlib import Path + +import numpy as np + +# Resolve workspace paths +_file_path = Path(__file__).resolve() +_example_agilex_root = _file_path.parent.parent +_ws_root = _example_agilex_root.parent + +# Add example_so101 to path for the leader_arm module +_example_so101_root = _ws_root / "example_so101" +sys.path.insert(0, str(_example_so101_root)) +sys.path.insert(0, str(_example_so101_root / "examples")) + +# Add neuracore and other dependencies +_neuracore_pkg_root = _ws_root / "neuracore" +sys.path.insert(0, str(_neuracore_pkg_root)) + +# CRITICAL: Set PYTHONPATH so that spawned subprocesses (like the data daemon) can find neuracore +os.environ["PYTHONPATH"] = str(_neuracore_pkg_root) + os.pathsep + os.environ.get("PYTHONPATH", "") + +import neuracore as nc + +# Import from example_so101 common components +from common.data_manager import DataManager, RobotActivityState +from common.leader_arm import LerobotSO101LeaderArm +from neuracore.core.input_devices.foot_pedal import FootPedal + +# AgileX PiPER Configurations +PIPER_JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] +# Limits in degrees +PIPER_LIMITS_DEG = np.array([ + (-150.0, 150.0), (0.0, 180.0), (-170.0, 0.0), + (-100.0, 100.0), (-70.0, 70.0), (-120.0, 120.0) +], dtype=np.float64) + +PIPER_OFFSETS_DEG = np.zeros(6, dtype=np.float64) +PIPER_DIRECTIONS = np.ones(6, dtype=np.float64) + +# Leader (5 joints) to AgileX (6 joints) mapping +# Leader 0 (Pan) -> Piper joint1 +# Leader 1 (Lift) -> Piper joint2 +# Leader 2 (Elbow) -> Piper joint3 +# Leader 3 (Wrist Flex) -> Piper joint5 +# Leader 4 (Wrist Roll) -> Piper joint6 +LEADER_TO_PIPER_JOINT = {0: 0, 1: 1, 2: 2, 3: 4, 4: 5} +PIPER_FIXED_JOINTS = {3: 0.0} # joint4 fixed at 0 + +URDF_PATH = _example_agilex_root / "piper_description" / "urdf" / "piper_description.urdf" + +def leader_reader_thread(data_manager: DataManager, leader: LerobotSO101LeaderArm, rate: float): + dt = 1.0 / rate + while not data_manager.is_shutdown_requested(): + try: + if leader.is_connected: + angles, gripper = leader.read_mapped() + data_manager.set_leader_mapped_state(angles, gripper) + except Exception as e: + print(f"Error in leader reader thread: {e}") + time.sleep(dt) + +def main(): + parser = argparse.ArgumentParser(description="AgileX PiPER Teleop with LeRobot Leader and Foot Pedal") + parser.add_argument("--leader-port", type=str, default="/dev/ttyACM0", help="USB port for leader arm") + parser.add_argument("--leader-id", type=str, default="my_awesome_leader_arm", help="LeRobot calibration id") + parser.add_argument("--leader-rate", type=float, default=50.0, help="Reading rate in Hz") + parser.add_argument("--robot-name", type=str, default="AgileX PiPER", help="Robot name in Neuracore") + + # Pedal keys + parser.add_argument("--pedal-activate", type=str, default="a") + parser.add_argument("--pedal-home", type=str, default="b") + parser.add_argument("--pedal-record", type=str, default="c") + + args = parser.parse_args() + + print("=" * 60) + print("AGILEX PIPER LE ROBOT + FOOT PEDAL TELEOP") + print("=" * 60) + + # 1. Initialize Leader Arm + print(f"\nšŸ¦¾ Connecting to Le Robot leader on {args.leader_port}...") + leader = LerobotSO101LeaderArm(port=args.leader_port, calibration_id=args.leader_id) + try: + leader.connect(calibrate=False) + except Exception as e: + print(f"āŒ Failed to connect to leader: {e}") + print("Tip: Run the robust calibration script first.") + sys.exit(1) + + # Adjusted Piper Config for better mapping + local_limits_deg = np.array([ + (-150.0, 150.0), (0.0, 160.0), (-160.0, 0.0), # J1, J2, J3 + (-100.0, 100.0), (-100.0, 100.0), (-180.0, 180.0) # J4, J5, J6 + ], dtype=np.float64) + + # Official Neutral Angles: [-1.0, 80.0, -51.0, -4.0, 16.0, 2.6] + # We use these as offsets so leader 0 -> official neutral + local_offsets_deg = np.array([-1.0, 80.0, -51.0, 0.0, 16.0, 2.6]) + + # Directions: + # J3 (Elbow) often needs to be flipped if leader fold -> piper fold + # J2 (Lift) leader up -> piper lift up + local_directions = np.array([1.0, 1.0, -1.0, 1.0, 1.0, 1.0]) + + leader.configure_follower( + follower_limits_deg=local_limits_deg, + follower_offsets_deg=local_offsets_deg, + follower_directions=local_directions, + leader_to_follower_joint=LEADER_TO_PIPER_JOINT, + fixed_joints=PIPER_FIXED_JOINTS + ) + print("āœ“ Leader arm configured with OFFICIAL offsets") + + # Neutral Pose for Parking/Home (degrees) - Using official values + NEUTRAL_POSE_DEG = np.array([-1.0, 80.0, -51.0, 0.0, 16.0, 2.6]) + + # 2. Initialize Data Manager and Foot Pedal + data_manager = DataManager() + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) # Start DISABLED + + pedal = FootPedal() + pedal.save_config(activate_key=args.pedal_activate, home_key=args.pedal_home, record_key=args.pedal_record) + + # Define callbacks first + def on_activate(): + state = data_manager.get_robot_activity_state() + if state == RobotActivityState.ENABLED: + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + print("āœ“ [PEDAL] Robot DISABLED (Parking...)") + else: + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("āœ“ [PEDAL] Robot ENABLED (Following Leader)") + + def on_home(): + print("šŸ  [PEDAL] Moving to home position...") + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + + def on_record(): + print("āŗļø [PEDAL] Toggling recording...") + if not nc.is_recording(): + try: + nc.start_recording() + print("āœ“ [PEDAL] Recording STARTED") + except Exception as e: + print(f"āœ— [PEDAL] Failed to start recording: {e}") + else: + try: + nc.stop_recording() + print("āœ“ [PEDAL] Recording STOPPED") + except Exception as e: + print(f"āœ— [PEDAL] Failed to stop recording: {e}") + + # Explicitly register + pedal.on("activate", on_activate) + pedal.on("home", on_home) + pedal.on("record", on_record) + + pedal.start() + print(f"āŒØļø Foot Pedal listener started (Keys: {args.pedal_activate}, {args.pedal_home}, {args.pedal_record})") + + # 3. Initialize Neuracore + print("\nšŸ”§ Initializing Neuracore...") + nc.login() + print(f"šŸ“¦ Connecting to Robot: {args.robot_name}") + nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH)) + + ds_name = f"piper-leader-teleop-{time.strftime('%Y%m%d-%H%M%S')}" + print(f"šŸ“‚ Creating Dataset: {ds_name}") + nc.create_dataset(name=ds_name) + + # 4. Start Leader Reading Thread + reader_thread = threading.Thread( + target=leader_reader_thread, + args=(data_manager, leader, args.leader_rate), + daemon=True + ) + reader_thread.start() + + print("\nšŸš€ TELEOP READY") + print("------------------------------------------------------------") + print("1. Robot starts in DISABLED (Neutral) pose.") + print(f"2. Press '{args.pedal_activate}' to ENABLE (Mirror Leader).") + print(f"3. Press '{args.pedal_record}' to Toggle Recording.") + print("------------------------------------------------------------") + + loop_count = 0 + try: + while True: + t0 = time.time() + mapped_angles, mapped_gripper = data_manager.get_leader_mapped_state() + state = data_manager.get_robot_activity_state() + + if state == RobotActivityState.HOMING: + angles_deg = NEUTRAL_POSE_DEG + gripper_val = 0.5 + time.sleep(0.5) + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + elif state == RobotActivityState.DISABLED: + angles_deg = NEUTRAL_POSE_DEG + gripper_val = 0.5 + else: + # ENABLED: Follow Leader + if mapped_angles is not None: + angles_deg = mapped_angles + gripper_val = mapped_gripper + else: + angles_deg = NEUTRAL_POSE_DEG + gripper_val = 0.5 + + # Streaming and Logging + if angles_deg is not None: + loop_count += 1 + # Debug every 30 loops (~2Hz) + if loop_count % 30 == 0: + if state == RobotActivityState.DISABLED: + print(f"šŸ’” [REMINDER] Robot is DISABLED. Press '{args.pedal_activate}' to Mirror. [Raw Grip: {gripper_val:.2f}]") + else: + print(f"šŸ“” [MIRRORING] J1-3: {np.round(angles_deg[:3], 1)} | Grip: {gripper_val:.2f}") + + # Log to Neuracore + data_dict = {} + for i, deg in enumerate(angles_deg): + val = float(np.radians(deg)) + # Explicit small offset for joint4 if 0.0 is problematic + if i == 3 and abs(val) < 0.0001: + val = 0.0001 + data_dict[f"joint{i+1}"] = val + + # Visual Gripper Joints (joint7, joint8) + if gripper_val is not None: + # joint7: 0 (closed) to 0.035 (open) + # joint8: 0 (closed) to -0.035 (open) + data_dict["joint7"] = float(gripper_val * 0.035) + data_dict["joint8"] = float(-gripper_val * 0.035) + + nc.log_joint_positions(data_dict, timestamp=t0) + + else: + if loop_count % 60 == 0: + print("āš ļø [WARNING] No leader data received! Is it plugged in?") + + time.sleep(max(0, 1.0 / 60.0 - (time.time() - t0))) + + except KeyboardInterrupt: + print("\nšŸ‘‹ Teleop stopped by user.") + except Exception as e: + print(f"\nāŒ Error: {e}") + traceback.print_exc() + finally: + data_manager.request_shutdown() + pedal.stop() + reader_thread.join(timeout=1.0) + leader.disconnect() + nc.logout() + print("šŸ‘‹ Shutdown complete.") + +if __name__ == "__main__": + main() diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py new file mode 100644 index 0000000..9f2fd76 --- /dev/null +++ b/examples/7_teleop_with_pedal.py @@ -0,0 +1,191 @@ +#!/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 +import traceback +from pathlib import Path + +import neuracore as nc +import numpy as np + +# Add parent directory to path +sys.path.insert(0, str(Path(__file__).parent.parent)) +# Add neuracore path for local imports if needed +sys.path.insert(0, str(Path(__file__).parent.parent.parent / "neuracore")) + +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, + GRIPPER_LOGGING_NAME, + IK_SOLVER_RATE, + JOINT_NAMES, + JOINT_STATE_STREAMING_RATE, + LM_DAMPING, + NEUTRAL_JOINT_ANGLES, + ORIENTATION_COST, + POSITION_COST, + POSTURE_COST_VECTOR, + ROBOT_RATE, + SOLVER_DAMPING_VALUE, + SOLVER_NAME, + URDF_PATH, +) +from common.data_manager import DataManager, RobotActivityState +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 +from neuracore.core.input_devices.foot_pedal import FootPedal + + +def log_to_neuracore_on_change_callback(name: str, value: float, timestamp: float) -> None: + 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: + state = data_manager.get_robot_activity_state() + if state == RobotActivityState.ENABLED: + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) + robot_controller.graceful_stop() + data_manager.set_teleop_state(False, None, None) + print("āœ“ [ACTION] Robot DISABLED") + elif state == RobotActivityState.DISABLED: + if robot_controller.resume_robot(): + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + print("āœ“ [ACTION] Robot ENABLED") + else: + print("āœ— [ACTION] Failed to enable robot") + + +def move_robot_home() -> None: + state = data_manager.get_robot_activity_state() + if state == RobotActivityState.ENABLED: + print("šŸ  [ACTION] Moving to home position...") + data_manager.set_robot_activity_state(RobotActivityState.HOMING) + data_manager.set_teleop_state(False, None, None) + if not robot_controller.move_to_home(): + print("āœ— [ACTION] Homing failed") + data_manager.set_robot_activity_state(RobotActivityState.ENABLED) + else: + print("āš ļø [ACTION] Cannot home: robot not enabled") + + +def toggle_recording() -> None: + if not nc.is_recording(): + try: + nc.start_recording() + print("āœ“ [ACTION] Recording STARTED") + except Exception as e: + print(f"āœ— [ACTION] Recording failed to start: {e}") + else: + try: + nc.stop_recording() + print("āœ“ [ACTION] Recording STOPPED") + except Exception as e: + print(f"āœ— [ACTION] Recording failed to stop: {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...") + 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") + + # 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 = PiperController(can_interface="can0", robot_rate=ROBOT_RATE) + robot_controller.start_control_loop() + + # Threads + print("\nšŸ“Š Starting Threads (JointState, QuestReader, IKSolver, Camera)...") + threading.Thread(target=joint_state_thread, args=(data_manager, robot_controller), daemon=True).start() + + 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() + + # Sync IK solver to current position + 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() + threading.Thread(target=camera_thread, args=(data_manager,), daemon=True).start() + + # Foot Pedal Initialization + print("\nāŒØļø Initializing Foot Pedals...") + pedal = FootPedal() + pedal.on("activate", toggle_robot_state) + pedal.on("home", move_robot_home) + pedal.on("record", toggle_recording) + pedal.start() + + print("\nāœ… SYSTEM ONLINE") + print("------------------------------------------------------------") + print("šŸŽ® QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") + print("āŒØļø PEDAL CONTROLS: ACTIVATE (Enable), HOME (Reset), RECORD") + print("------------------------------------------------------------") + + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + print("\nšŸ‘‹ Shutting down...") + finally: + pedal.stop() + if nc.is_recording(): nc.cancel_recording() + nc.logout() + data_manager.request_shutdown() + quest_reader.stop() + robot_controller.cleanup() diff --git a/scripts/foot_pedal/README.md b/scripts/foot_pedal/README.md new file mode 100644 index 0000000..82ce39e --- /dev/null +++ b/scripts/foot_pedal/README.md @@ -0,0 +1,51 @@ +# Foot Pedal Control System + +The Foot Pedal system allows you to control the robot and data collection using a 3-pedal USB device (or any keyboard-mapped input). + +## Actions +- **ACTIVATE**: Enables the robot and aligns targets to current joint positions. +- **HOME**: Moves the robot arm to its predefined home position. +- **RECORD**: Toggles Neuracore data collection (Start/Stop recording). + +## Setup and Configuration + +### Remapping Keys +To map or change which pedal triggers which action, run the configuration script: + +```bash +./scripts/foot_pedal/run_pedal_config.sh +``` + +Follow the prompts in the terminal: +1. When prompted for 'ACTIVATE', press the first pedal. +2. When prompted for 'HOME', press the second pedal. +3. When prompted for 'RECORD', press the third pedal. + +The configuration is saved to `~/.neuracore/foot_pedal.json`. + +### Running the Demo +To start controlling the robot with the pedals, use the demo script: + +```bash +python3 scripts/foot_pedal/pedal_demo.py +``` + +## Developer Usage + +The pedal system is integrated into the Neuracore core library. You can use it in your own scripts as follows: + +```python +from neuracore.core.input_devices.foot_pedal import FootPedal + +pedal = FootPedal() +pedal.on("activate", your_activate_function) +pedal.on("home", your_home_function) +pedal.on("record", your_record_function) + +pedal.start() +``` + +## Troubleshooting +- If pedals are not detected, ensure the terminal window has focus. +- The `readchar` library must be installed for key detection. +- If using a serial-based pedal, use `list_potential_ports()` from `foot_pedal.py` to identify the device path. diff --git a/scripts/foot_pedal/pedal_demo.py b/scripts/foot_pedal/pedal_demo.py new file mode 100755 index 0000000..6a1c5d2 --- /dev/null +++ b/scripts/foot_pedal/pedal_demo.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +"""Example of using the Foot Pedal to control the Piper robot.""" + +import sys +import threading +import time +from pathlib import Path + +# Add paths +sys.path.insert(0, str(Path(__file__).parent.parent.parent)) + +import neuracore as nc +from neuracore.core.input_devices.foot_pedal import FootPedal +from piper_controller import PiperController + + +def main(): + print("=" * 60) + print("FOOT PEDAL ROBOT CONTROL DEMO") + print("=" * 60) + + # Initialize robot + print("\nšŸ¤– Initializing Piper robot...") + try: + robot = PiperController(can_interface="can0", debug_mode=False) + robot.start_control_loop() + except Exception as e: + print(f"āœ— Failed to initialize robot: {e}") + print("Running in simulation/mock mode (no hardware)...") + robot = None + + # Initialize Foot Pedal + print("\nāŒØļø Initializing Foot Pedal...") + pedal = FootPedal() + + if not any(pedal.mappings.values()): + print("āš ļø Foot pedal mappings are empty. Please run run_pedal_config.sh first.") + # Default fallback for demo + pedal.mappings = {"activate": "1", "home": "2", "record": "3"} + print(f"Using default demo mappings: {pedal.mappings}") + + # Connect to Neuracore (optional, for recording) + try: + nc.login() + print("āœ“ Connected to Neuracore") + except Exception: + print("āš ļø Neuracore connection failed. Recording actions will only print messages.") + + # Define callbacks + def on_activate(): + print("\nšŸš€ Foot Pedal: ACTIVATE pressed") + if robot: + if robot.resume_robot(): + print("āœ“ Robot enabled") + else: + print("āœ— Failed to enable robot") + else: + print("Action: Robot ACTIVATE (MOCK)") + + def on_home(): + print("\nšŸ  Foot Pedal: HOME pressed") + if robot: + robot.move_to_home() + print("āœ“ Robot moving to home") + else: + print("Action: Robot HOME (MOCK)") + + def on_record(): + print("\nšŸ“¹ Foot Pedal: RECORD pressed") + try: + if not nc.is_recording(): + nc.start_recording() + print("āœ“ Recording started") + else: + nc.stop_recording() + print("āœ“ Recording stopped") + except Exception as e: + print(f"āš ļø Recording action failed: {e}") + print(f"Action: Toggle Recording (MOCK)") + + # Register callbacks + pedal.on("activate", on_activate) + pedal.on("home", on_home) + pedal.on("record", on_record) + + # Start pedal listener + pedal.start() + + print("\nāœ… Setup complete!") + print(f"ACTIVATE: Press '{pedal.mappings['activate']}' to enable robot") + print(f"HOME: Press '{pedal.mappings['home']}' to send robot home") + print(f"RECORD: Press '{pedal.mappings['record']}' to toggle recording") + print("\nāš ļø Press Ctrl+C to exit") + + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + print("\nšŸ‘‹ Stopping...") + finally: + pedal.stop() + if robot: + robot.cleanup() + + +if __name__ == "__main__": + main() diff --git a/scripts/foot_pedal/run_pedal_config.sh b/scripts/foot_pedal/run_pedal_config.sh new file mode 100755 index 0000000..39be813 --- /dev/null +++ b/scripts/foot_pedal/run_pedal_config.sh @@ -0,0 +1,9 @@ +#!/bin/bash +# Script to run foot pedal configuration + +# Get the directory where the script is located +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +REPO_ROOT="$( cd "$SCRIPT_DIR/../../.." &> /dev/null && pwd )" + +# Run the python config script +PYTHONPATH="$REPO_ROOT/neuracore:$PYTHONPATH" python3 "$REPO_ROOT/neuracore/neuracore/core/input_devices/pedal_config.py" From 7bb398c82a7ef54649ddddb1c2fca0f964ab35b3 Mon Sep 17 00:00:00 2001 From: Pruthvi-Neuracore Date: Mon, 9 Mar 2026 14:47:13 +0000 Subject: [PATCH 02/31] fix: pre-commits --- example-agilex-dictionary.txt | 2 + examples/10_leader_arm_teleop_agilex.py | 186 ++++++++++++++++-------- examples/7_teleop_with_pedal.py | 76 ++++++---- examples/common/data_manager.py | 48 ++++++ scripts/foot_pedal/pedal_demo.py | 22 ++- 5 files changed, 240 insertions(+), 94 deletions(-) diff --git a/example-agilex-dictionary.txt b/example-agilex-dictionary.txt index 25fbae6..8defd6b 100644 --- a/example-agilex-dictionary.txt +++ b/example-agilex-dictionary.txt @@ -49,3 +49,5 @@ Viser wxyz xyzw yourdfpy +Lerobot +readchar diff --git a/examples/10_leader_arm_teleop_agilex.py b/examples/10_leader_arm_teleop_agilex.py index 74b1e0a..629f4ae 100644 --- a/examples/10_leader_arm_teleop_agilex.py +++ b/examples/10_leader_arm_teleop_agilex.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 """AgileX PiPER Teleoperation using LeRobot Leader Arm and Foot Pedal. -This script maps a physical LeRobot leader arm and a 3-button Foot Pedal +This script maps a physical LeRobot leader arm and a 3-button Foot Pedal to a simulated AgileX PiPER robot in Neuracore. """ @@ -28,24 +28,36 @@ # Add neuracore and other dependencies _neuracore_pkg_root = _ws_root / "neuracore" sys.path.insert(0, str(_neuracore_pkg_root)) +sys.path.insert( + 0, str(_example_agilex_root) +) # Fix for local piper_controller if needed # CRITICAL: Set PYTHONPATH so that spawned subprocesses (like the data daemon) can find neuracore -os.environ["PYTHONPATH"] = str(_neuracore_pkg_root) + os.pathsep + os.environ.get("PYTHONPATH", "") +os.environ["PYTHONPATH"] = ( + str(_neuracore_pkg_root) + os.pathsep + os.environ.get("PYTHONPATH", "") +) -import neuracore as nc +import neuracore as nc # noqa: E402 # Import from example_so101 common components -from common.data_manager import DataManager, RobotActivityState -from common.leader_arm import LerobotSO101LeaderArm -from neuracore.core.input_devices.foot_pedal import FootPedal +from common.data_manager import DataManager, RobotActivityState # noqa: E402 +from common.leader_arm import LerobotSO101LeaderArm # noqa: E402 +from neuracore.core.input_devices.foot_pedal import FootPedal # noqa: E402 # AgileX PiPER Configurations PIPER_JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] # Limits in degrees -PIPER_LIMITS_DEG = np.array([ - (-150.0, 150.0), (0.0, 180.0), (-170.0, 0.0), - (-100.0, 100.0), (-70.0, 70.0), (-120.0, 120.0) -], dtype=np.float64) +PIPER_LIMITS_DEG = np.array( + [ + (-150.0, 150.0), + (0.0, 180.0), + (-170.0, 0.0), + (-100.0, 100.0), + (-70.0, 70.0), + (-120.0, 120.0), + ], + dtype=np.float64, +) PIPER_OFFSETS_DEG = np.zeros(6, dtype=np.float64) PIPER_DIRECTIONS = np.ones(6, dtype=np.float64) @@ -57,33 +69,63 @@ # Leader 3 (Wrist Flex) -> Piper joint5 # Leader 4 (Wrist Roll) -> Piper joint6 LEADER_TO_PIPER_JOINT = {0: 0, 1: 1, 2: 2, 3: 4, 4: 5} -PIPER_FIXED_JOINTS = {3: 0.0} # joint4 fixed at 0 +PIPER_FIXED_JOINTS = {3: 0.0} # joint4 fixed at 0 -URDF_PATH = _example_agilex_root / "piper_description" / "urdf" / "piper_description.urdf" +URDF_PATH = ( + _example_agilex_root / "piper_description" / "urdf" / "piper_description.urdf" +) -def leader_reader_thread(data_manager: DataManager, leader: LerobotSO101LeaderArm, rate: float): + +def leader_reader_thread( + data_manager: DataManager, leader: LerobotSO101LeaderArm, rate: float +) -> None: + """Thread for reading data from the leader arm and updating the shared state. + + Args: + data_manager: Shared state manager. + leader: Initialized leader arm instance. + rate: Frequency in Hz to read the arm. + """ dt = 1.0 / rate while not data_manager.is_shutdown_requested(): try: if leader.is_connected: angles, gripper = leader.read_mapped() - data_manager.set_leader_mapped_state(angles, gripper) + data_manager.set_leader_mapped_state(angles, gripper) # type: ignore[attr-defined] except Exception as e: print(f"Error in leader reader thread: {e}") time.sleep(dt) -def main(): - parser = argparse.ArgumentParser(description="AgileX PiPER Teleop with LeRobot Leader and Foot Pedal") - parser.add_argument("--leader-port", type=str, default="/dev/ttyACM0", help="USB port for leader arm") - parser.add_argument("--leader-id", type=str, default="my_awesome_leader_arm", help="LeRobot calibration id") - parser.add_argument("--leader-rate", type=float, default=50.0, help="Reading rate in Hz") - parser.add_argument("--robot-name", type=str, default="AgileX PiPER", help="Robot name in Neuracore") - - # Pedal keys + +def main() -> None: + """Main execution entry point for teleoperation script.""" + parser = argparse.ArgumentParser( + description="AgileX PiPER Teleop with LeRobot Leader and Foot Pedal" + ) + parser.add_argument( + "--leader-port", + type=str, + default="/dev/ttyACM0", + help="USB port for leader arm", + ) + parser.add_argument( + "--leader-id", + type=str, + default="my_awesome_leader_arm", + help="LeRobot calibration id", + ) + parser.add_argument( + "--leader-rate", type=float, default=50.0, help="Reading rate in Hz" + ) + parser.add_argument( + "--robot-name", type=str, default="AgileX PiPER", help="Robot name in Neuracore" + ) + + # Pedal keys (Matching OpenArm unified mapping) parser.add_argument("--pedal-activate", type=str, default="a") - parser.add_argument("--pedal-home", type=str, default="b") - parser.add_argument("--pedal-record", type=str, default="c") - + parser.add_argument("--pedal-home", type=str, default="h") + parser.add_argument("--pedal-record", type=str, default="r") + args = parser.parse_args() print("=" * 60) @@ -99,20 +141,23 @@ def main(): print(f"āŒ Failed to connect to leader: {e}") print("Tip: Run the robust calibration script first.") sys.exit(1) - + # Adjusted Piper Config for better mapping - local_limits_deg = np.array([ - (-150.0, 150.0), (0.0, 160.0), (-160.0, 0.0), # J1, J2, J3 - (-100.0, 100.0), (-100.0, 100.0), (-180.0, 180.0) # J4, J5, J6 - ], dtype=np.float64) - - # Official Neutral Angles: [-1.0, 80.0, -51.0, -4.0, 16.0, 2.6] - # We use these as offsets so leader 0 -> official neutral + local_limits_deg = np.array( + [ + (-150.0, 150.0), + (0.0, 160.0), + (-160.0, 0.0), # J1, J2, J3 + (-100.0, 100.0), + (-100.0, 100.0), + (-180.0, 180.0), # J4, J5, J6 + ], + dtype=np.float64, + ) + + # Reverting to the previous working offsets as requested + # These worked well for the user in the initial session local_offsets_deg = np.array([-1.0, 80.0, -51.0, 0.0, 16.0, 2.6]) - - # Directions: - # J3 (Elbow) often needs to be flipped if leader fold -> piper fold - # J2 (Lift) leader up -> piper lift up local_directions = np.array([1.0, 1.0, -1.0, 1.0, 1.0, 1.0]) leader.configure_follower( @@ -120,22 +165,34 @@ def main(): follower_offsets_deg=local_offsets_deg, follower_directions=local_directions, leader_to_follower_joint=LEADER_TO_PIPER_JOINT, - fixed_joints=PIPER_FIXED_JOINTS + fixed_joints=PIPER_FIXED_JOINTS, ) - print("āœ“ Leader arm configured with OFFICIAL offsets") + print("āœ“ Leader arm configured with WORKING offsets (Reverted)") # Neutral Pose for Parking/Home (degrees) - Using official values NEUTRAL_POSE_DEG = np.array([-1.0, 80.0, -51.0, 0.0, 16.0, 2.6]) # 2. Initialize Data Manager and Foot Pedal data_manager = DataManager() - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) # Start DISABLED - + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) # Start DISABLED + pedal = FootPedal() - pedal.save_config(activate_key=args.pedal_activate, home_key=args.pedal_home, record_key=args.pedal_record) + + # HARDCODE working keys for the user's specific FootSwitch + # a = ACTIVATE, b = HOME, c = RECORD + activate_key = "a" + home_key = "b" + record_key = "c" + + # Save this config so the FootPedal class picks it up + pedal.save_config( + activate_key=activate_key, home_key=home_key, record_key=record_key + ) + print(f"āœ“ PEDAL MAPPINGS ENFORCED: {pedal.mappings}") # Define callbacks first - def on_activate(): + def on_activate() -> None: + """Toggle the robot enable/disable state.""" state = data_manager.get_robot_activity_state() if state == RobotActivityState.ENABLED: data_manager.set_robot_activity_state(RobotActivityState.DISABLED) @@ -144,13 +201,17 @@ def on_activate(): data_manager.set_robot_activity_state(RobotActivityState.ENABLED) print("āœ“ [PEDAL] Robot ENABLED (Following Leader)") - def on_home(): + def on_home() -> None: + """Trigger the homing procedure.""" print("šŸ  [PEDAL] Moving to home position...") data_manager.set_robot_activity_state(RobotActivityState.HOMING) - def on_record(): + def on_record() -> None: + """Toggle Neuracore recording session.""" print("āŗļø [PEDAL] Toggling recording...") - if not nc.is_recording(): + current_status = nc.is_recording() + print(f"šŸ” [DEBUG] Current Neuracore recording status: {current_status}") + if not current_status: try: nc.start_recording() print("āœ“ [PEDAL] Recording STARTED") @@ -169,14 +230,18 @@ def on_record(): pedal.on("record", on_record) pedal.start() - print(f"āŒØļø Foot Pedal listener started (Keys: {args.pedal_activate}, {args.pedal_home}, {args.pedal_record})") + print( + f"āŒØļø Foot Pedal listener started (Keys: {args.pedal_activate}, {args.pedal_home}, {args.pedal_record})" + ) # 3. Initialize Neuracore print("\nšŸ”§ Initializing Neuracore...") nc.login() print(f"šŸ“¦ Connecting to Robot: {args.robot_name}") - nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH)) - + nc.connect_robot( + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=True + ) + ds_name = f"piper-leader-teleop-{time.strftime('%Y%m%d-%H%M%S')}" print(f"šŸ“‚ Creating Dataset: {ds_name}") nc.create_dataset(name=ds_name) @@ -185,7 +250,7 @@ def on_record(): reader_thread = threading.Thread( target=leader_reader_thread, args=(data_manager, leader, args.leader_rate), - daemon=True + daemon=True, ) reader_thread.start() @@ -195,14 +260,14 @@ def on_record(): print(f"2. Press '{args.pedal_activate}' to ENABLE (Mirror Leader).") print(f"3. Press '{args.pedal_record}' to Toggle Recording.") print("------------------------------------------------------------") - + loop_count = 0 try: while True: t0 = time.time() mapped_angles, mapped_gripper = data_manager.get_leader_mapped_state() state = data_manager.get_robot_activity_state() - + if state == RobotActivityState.HOMING: angles_deg = NEUTRAL_POSE_DEG gripper_val = 0.5 @@ -215,7 +280,7 @@ def on_record(): # ENABLED: Follow Leader if mapped_angles is not None: angles_deg = mapped_angles - gripper_val = mapped_gripper + gripper_val = mapped_gripper if mapped_gripper is not None else 0.5 else: angles_deg = NEUTRAL_POSE_DEG gripper_val = 0.5 @@ -226,9 +291,13 @@ def on_record(): # Debug every 30 loops (~2Hz) if loop_count % 30 == 0: if state == RobotActivityState.DISABLED: - print(f"šŸ’” [REMINDER] Robot is DISABLED. Press '{args.pedal_activate}' to Mirror. [Raw Grip: {gripper_val:.2f}]") + print( + f"šŸ’” [REMINDER] Robot is DISABLED. Press '{args.pedal_activate}' to Mirror. [Raw Grip: {gripper_val:.2f}]" + ) else: - print(f"šŸ“” [MIRRORING] J1-3: {np.round(angles_deg[:3], 1)} | Grip: {gripper_val:.2f}") + print( + f"šŸ“” [MIRRORING] J1-3: {np.round(angles_deg[:3], 1)} | Grip: {gripper_val:.2f}" + ) # Log to Neuracore data_dict = {} @@ -238,7 +307,7 @@ def on_record(): if i == 3 and abs(val) < 0.0001: val = 0.0001 data_dict[f"joint{i+1}"] = val - + # Visual Gripper Joints (joint7, joint8) if gripper_val is not None: # joint7: 0 (closed) to 0.035 (open) @@ -247,11 +316,11 @@ def on_record(): data_dict["joint8"] = float(-gripper_val * 0.035) nc.log_joint_positions(data_dict, timestamp=t0) - + else: if loop_count % 60 == 0: print("āš ļø [WARNING] No leader data received! Is it plugged in?") - + time.sleep(max(0, 1.0 / 60.0 - (time.time() - t0))) except KeyboardInterrupt: @@ -267,5 +336,6 @@ def on_record(): nc.logout() print("šŸ‘‹ Shutdown complete.") + if __name__ == "__main__": main() diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 9f2fd76..4860884 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -1,7 +1,7 @@ #!/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 +This script combines Meta Quest controller input for movement with Foot Pedal control for session management (Activate, Home, Record). """ @@ -10,8 +10,8 @@ import sys import threading import time -import traceback from pathlib import Path +from typing import Any import neuracore as nc import numpy as np @@ -22,26 +22,15 @@ sys.path.insert(0, str(Path(__file__).parent.parent.parent / "neuracore")) 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, GRIPPER_LOGGING_NAME, - IK_SOLVER_RATE, JOINT_NAMES, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, - POSITION_COST, POSTURE_COST_VECTOR, ROBOT_RATE, - SOLVER_DAMPING_VALUE, - SOLVER_NAME, URDF_PATH, ) from common.data_manager import DataManager, RobotActivityState @@ -50,13 +39,22 @@ 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 neuracore.core.input_devices.foot_pedal import FootPedal from pink_ik_solver import PinkIKSolver from piper_controller import PiperController -from neuracore.core.input_devices.foot_pedal import FootPedal -def log_to_neuracore_on_change_callback(name: str, value: float, timestamp: float) -> None: +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)} @@ -65,9 +63,13 @@ def log_to_neuracore_on_change_callback(name: str, value: float, timestamp: floa 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) + 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) + 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: @@ -75,6 +77,7 @@ def log_to_neuracore_on_change_callback(name: str, value: float, timestamp: floa def toggle_robot_state() -> None: + """Toggle the robot's activity state between ENABLED and DISABLED.""" state = data_manager.get_robot_activity_state() if state == RobotActivityState.ENABLED: data_manager.set_robot_activity_state(RobotActivityState.DISABLED) @@ -90,6 +93,7 @@ def toggle_robot_state() -> None: def move_robot_home() -> None: + """Command the robot to move to its neutral/home position.""" state = data_manager.get_robot_activity_state() if state == RobotActivityState.ENABLED: print("šŸ  [ACTION] Moving to home position...") @@ -103,6 +107,7 @@ def move_robot_home() -> None: def toggle_recording() -> None: + """Start or stop a data recording session in Neuracore.""" if not nc.is_recording(): try: nc.start_recording() @@ -132,15 +137,19 @@ def toggle_recording() -> None: # Neuracore Init 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 + ) + 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") # 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_controller_filter_params( + CONTROLLER_MIN_CUTOFF, CONTROLLER_BETA, CONTROLLER_D_CUTOFF + ) # Robot Initialization print("\nšŸ¤– Initializing Piper...") @@ -149,18 +158,28 @@ def toggle_recording() -> None: # Threads print("\nšŸ“Š Starting Threads (JointState, QuestReader, IKSolver, Camera)...") - threading.Thread(target=joint_state_thread, args=(data_manager, robot_controller), daemon=True).start() - + threading.Thread( + target=joint_state_thread, args=(data_manager, robot_controller), daemon=True + ).start() + 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() + threading.Thread( + target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True + ).start() # Sync IK solver to current position - initial_joints = np.radians(data_manager.get_current_joint_angles() or NEUTRAL_JOINT_ANGLES) + 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) + 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() + threading.Thread( + target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True + ).start() threading.Thread(target=camera_thread, args=(data_manager,), daemon=True).start() # Foot Pedal Initialization @@ -184,7 +203,8 @@ def toggle_recording() -> None: print("\nšŸ‘‹ Shutting down...") finally: pedal.stop() - if nc.is_recording(): nc.cancel_recording() + if nc.is_recording(): + nc.cancel_recording() nc.logout() data_manager.request_shutdown() quest_reader.stop() diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index 9e159a1..d2a380c 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -93,6 +93,17 @@ def __init__(self) -> None: self.rgb_image: np.ndarray | None = None +class LeaderMappedState: + """Leader arm mapped state - joint angles and gripper from leader read_mapped().""" + + def __init__(self) -> None: + """Initialize LeaderMappedState with default values.""" + self._lock = threading.Lock() + + self.joint_angles: np.ndarray | None = None + self.gripper_open: float | None = None + + class DataManager: """Main state container coordinating all state groups. @@ -112,6 +123,7 @@ def __init__(self) -> None: self._robot_state = RobotState() self._ik_state = IKState() self._camera_state = CameraState() + self._leader_mapped_state = LeaderMappedState() # System state self._shutdown_event = threading.Event() @@ -530,6 +542,42 @@ def get_ik_success(self) -> bool: with self._ik_state._lock: return self._ik_state.success + # ============================================================================ + # Leader Mapped State Methods + # ============================================================================ + + def set_leader_mapped_state( + self, joint_angles: np.ndarray, gripper_open: float + ) -> None: + """Set leader-mapped joint angles and gripper (thread-safe). + + Args: + joint_angles: Follower-space joint angles from leader read_mapped(). + gripper_open: Gripper open amount in [0, 1]. + """ + with self._leader_mapped_state._lock: + self._leader_mapped_state.joint_angles = ( + joint_angles.copy() if joint_angles is not None else None + ) + self._leader_mapped_state.gripper_open = gripper_open + + def get_leader_mapped_state( + self, + ) -> tuple[np.ndarray | None, float | None]: + """Get leader-mapped joint angles and gripper (thread-safe). + + Returns: + Tuple of (joint_angles, gripper_open); either may be None if not set. + """ + with self._leader_mapped_state._lock: + angles = ( + self._leader_mapped_state.joint_angles.copy() + if self._leader_mapped_state.joint_angles is not None + else None + ) + gripper = self._leader_mapped_state.gripper_open + return (angles, gripper) + # ============================================================================ # System State Methods # ============================================================================ diff --git a/scripts/foot_pedal/pedal_demo.py b/scripts/foot_pedal/pedal_demo.py index 6a1c5d2..3fc87c0 100755 --- a/scripts/foot_pedal/pedal_demo.py +++ b/scripts/foot_pedal/pedal_demo.py @@ -2,7 +2,6 @@ """Example of using the Foot Pedal to control the Piper robot.""" import sys -import threading import time from pathlib import Path @@ -11,10 +10,12 @@ import neuracore as nc from neuracore.core.input_devices.foot_pedal import FootPedal + from piper_controller import PiperController -def main(): +def main() -> None: + """Run a demonstration of foot pedal control for the Piper robot.""" print("=" * 60) print("FOOT PEDAL ROBOT CONTROL DEMO") print("=" * 60) @@ -32,7 +33,7 @@ def main(): # Initialize Foot Pedal print("\nāŒØļø Initializing Foot Pedal...") pedal = FootPedal() - + if not any(pedal.mappings.values()): print("āš ļø Foot pedal mappings are empty. Please run run_pedal_config.sh first.") # Default fallback for demo @@ -44,10 +45,13 @@ def main(): nc.login() print("āœ“ Connected to Neuracore") except Exception: - print("āš ļø Neuracore connection failed. Recording actions will only print messages.") + print( + "āš ļø Neuracore connection failed. Recording actions will only print messages." + ) # Define callbacks - def on_activate(): + def on_activate() -> None: + """Handle the activate/enable action.""" print("\nšŸš€ Foot Pedal: ACTIVATE pressed") if robot: if robot.resume_robot(): @@ -57,7 +61,8 @@ def on_activate(): else: print("Action: Robot ACTIVATE (MOCK)") - def on_home(): + def on_home() -> None: + """Handle the move-to-home action.""" print("\nšŸ  Foot Pedal: HOME pressed") if robot: robot.move_to_home() @@ -65,7 +70,8 @@ def on_home(): else: print("Action: Robot HOME (MOCK)") - def on_record(): + def on_record() -> None: + """Handle the toggle-recording action.""" print("\nšŸ“¹ Foot Pedal: RECORD pressed") try: if not nc.is_recording(): @@ -76,7 +82,7 @@ def on_record(): print("āœ“ Recording stopped") except Exception as e: print(f"āš ļø Recording action failed: {e}") - print(f"Action: Toggle Recording (MOCK)") + print("Action: Toggle Recording (MOCK)") # Register callbacks pedal.on("activate", on_activate) From f78d0859ba63f121c80aebf465b83c2e76acbde8 Mon Sep 17 00:00:00 2001 From: Pruthvi-Neuracore Date: Mon, 16 Mar 2026 14:57:10 +0000 Subject: [PATCH 03/31] Refactor: Move foot pedal logic into example 7 and remove redundant scripts per PR review --- example-agilex-dictionary.txt | 6 + examples/10_leader_arm_teleop_agilex.py | 341 ------------------------ examples/7_teleop_with_pedal.py | 141 +++++++++- examples/common/data_manager.py | 48 ---- examples/common/threads/__init__.py | 1 + scripts/foot_pedal/README.md | 51 ---- scripts/foot_pedal/pedal_demo.py | 113 -------- scripts/foot_pedal/run_pedal_config.sh | 9 - 8 files changed, 140 insertions(+), 570 deletions(-) delete mode 100644 examples/10_leader_arm_teleop_agilex.py create mode 100644 examples/common/threads/__init__.py delete mode 100644 scripts/foot_pedal/README.md delete mode 100755 scripts/foot_pedal/pedal_demo.py delete mode 100755 scripts/foot_pedal/run_pedal_config.sh diff --git a/example-agilex-dictionary.txt b/example-agilex-dictionary.txt index 8defd6b..5a0a7f0 100644 --- a/example-agilex-dictionary.txt +++ b/example-agilex-dictionary.txt @@ -51,3 +51,9 @@ xyzw yourdfpy Lerobot readchar +ecodes +evdev +keystate +pynput +ungrab + diff --git a/examples/10_leader_arm_teleop_agilex.py b/examples/10_leader_arm_teleop_agilex.py deleted file mode 100644 index 629f4ae..0000000 --- a/examples/10_leader_arm_teleop_agilex.py +++ /dev/null @@ -1,341 +0,0 @@ -#!/usr/bin/env python3 -"""AgileX PiPER Teleoperation using LeRobot Leader Arm and Foot Pedal. - -This script maps a physical LeRobot leader arm and a 3-button Foot Pedal -to a simulated AgileX PiPER robot in Neuracore. -""" - -import argparse -import os -import sys -import threading -import time -import traceback -from pathlib import Path - -import numpy as np - -# Resolve workspace paths -_file_path = Path(__file__).resolve() -_example_agilex_root = _file_path.parent.parent -_ws_root = _example_agilex_root.parent - -# Add example_so101 to path for the leader_arm module -_example_so101_root = _ws_root / "example_so101" -sys.path.insert(0, str(_example_so101_root)) -sys.path.insert(0, str(_example_so101_root / "examples")) - -# Add neuracore and other dependencies -_neuracore_pkg_root = _ws_root / "neuracore" -sys.path.insert(0, str(_neuracore_pkg_root)) -sys.path.insert( - 0, str(_example_agilex_root) -) # Fix for local piper_controller if needed - -# CRITICAL: Set PYTHONPATH so that spawned subprocesses (like the data daemon) can find neuracore -os.environ["PYTHONPATH"] = ( - str(_neuracore_pkg_root) + os.pathsep + os.environ.get("PYTHONPATH", "") -) - -import neuracore as nc # noqa: E402 - -# Import from example_so101 common components -from common.data_manager import DataManager, RobotActivityState # noqa: E402 -from common.leader_arm import LerobotSO101LeaderArm # noqa: E402 -from neuracore.core.input_devices.foot_pedal import FootPedal # noqa: E402 - -# AgileX PiPER Configurations -PIPER_JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] -# Limits in degrees -PIPER_LIMITS_DEG = np.array( - [ - (-150.0, 150.0), - (0.0, 180.0), - (-170.0, 0.0), - (-100.0, 100.0), - (-70.0, 70.0), - (-120.0, 120.0), - ], - dtype=np.float64, -) - -PIPER_OFFSETS_DEG = np.zeros(6, dtype=np.float64) -PIPER_DIRECTIONS = np.ones(6, dtype=np.float64) - -# Leader (5 joints) to AgileX (6 joints) mapping -# Leader 0 (Pan) -> Piper joint1 -# Leader 1 (Lift) -> Piper joint2 -# Leader 2 (Elbow) -> Piper joint3 -# Leader 3 (Wrist Flex) -> Piper joint5 -# Leader 4 (Wrist Roll) -> Piper joint6 -LEADER_TO_PIPER_JOINT = {0: 0, 1: 1, 2: 2, 3: 4, 4: 5} -PIPER_FIXED_JOINTS = {3: 0.0} # joint4 fixed at 0 - -URDF_PATH = ( - _example_agilex_root / "piper_description" / "urdf" / "piper_description.urdf" -) - - -def leader_reader_thread( - data_manager: DataManager, leader: LerobotSO101LeaderArm, rate: float -) -> None: - """Thread for reading data from the leader arm and updating the shared state. - - Args: - data_manager: Shared state manager. - leader: Initialized leader arm instance. - rate: Frequency in Hz to read the arm. - """ - dt = 1.0 / rate - while not data_manager.is_shutdown_requested(): - try: - if leader.is_connected: - angles, gripper = leader.read_mapped() - data_manager.set_leader_mapped_state(angles, gripper) # type: ignore[attr-defined] - except Exception as e: - print(f"Error in leader reader thread: {e}") - time.sleep(dt) - - -def main() -> None: - """Main execution entry point for teleoperation script.""" - parser = argparse.ArgumentParser( - description="AgileX PiPER Teleop with LeRobot Leader and Foot Pedal" - ) - parser.add_argument( - "--leader-port", - type=str, - default="/dev/ttyACM0", - help="USB port for leader arm", - ) - parser.add_argument( - "--leader-id", - type=str, - default="my_awesome_leader_arm", - help="LeRobot calibration id", - ) - parser.add_argument( - "--leader-rate", type=float, default=50.0, help="Reading rate in Hz" - ) - parser.add_argument( - "--robot-name", type=str, default="AgileX PiPER", help="Robot name in Neuracore" - ) - - # Pedal keys (Matching OpenArm unified mapping) - parser.add_argument("--pedal-activate", type=str, default="a") - parser.add_argument("--pedal-home", type=str, default="h") - parser.add_argument("--pedal-record", type=str, default="r") - - args = parser.parse_args() - - print("=" * 60) - print("AGILEX PIPER LE ROBOT + FOOT PEDAL TELEOP") - print("=" * 60) - - # 1. Initialize Leader Arm - print(f"\nšŸ¦¾ Connecting to Le Robot leader on {args.leader_port}...") - leader = LerobotSO101LeaderArm(port=args.leader_port, calibration_id=args.leader_id) - try: - leader.connect(calibrate=False) - except Exception as e: - print(f"āŒ Failed to connect to leader: {e}") - print("Tip: Run the robust calibration script first.") - sys.exit(1) - - # Adjusted Piper Config for better mapping - local_limits_deg = np.array( - [ - (-150.0, 150.0), - (0.0, 160.0), - (-160.0, 0.0), # J1, J2, J3 - (-100.0, 100.0), - (-100.0, 100.0), - (-180.0, 180.0), # J4, J5, J6 - ], - dtype=np.float64, - ) - - # Reverting to the previous working offsets as requested - # These worked well for the user in the initial session - local_offsets_deg = np.array([-1.0, 80.0, -51.0, 0.0, 16.0, 2.6]) - local_directions = np.array([1.0, 1.0, -1.0, 1.0, 1.0, 1.0]) - - leader.configure_follower( - follower_limits_deg=local_limits_deg, - follower_offsets_deg=local_offsets_deg, - follower_directions=local_directions, - leader_to_follower_joint=LEADER_TO_PIPER_JOINT, - fixed_joints=PIPER_FIXED_JOINTS, - ) - print("āœ“ Leader arm configured with WORKING offsets (Reverted)") - - # Neutral Pose for Parking/Home (degrees) - Using official values - NEUTRAL_POSE_DEG = np.array([-1.0, 80.0, -51.0, 0.0, 16.0, 2.6]) - - # 2. Initialize Data Manager and Foot Pedal - data_manager = DataManager() - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) # Start DISABLED - - pedal = FootPedal() - - # HARDCODE working keys for the user's specific FootSwitch - # a = ACTIVATE, b = HOME, c = RECORD - activate_key = "a" - home_key = "b" - record_key = "c" - - # Save this config so the FootPedal class picks it up - pedal.save_config( - activate_key=activate_key, home_key=home_key, record_key=record_key - ) - print(f"āœ“ PEDAL MAPPINGS ENFORCED: {pedal.mappings}") - - # Define callbacks first - def on_activate() -> None: - """Toggle the robot enable/disable state.""" - state = data_manager.get_robot_activity_state() - if state == RobotActivityState.ENABLED: - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - print("āœ“ [PEDAL] Robot DISABLED (Parking...)") - else: - data_manager.set_robot_activity_state(RobotActivityState.ENABLED) - print("āœ“ [PEDAL] Robot ENABLED (Following Leader)") - - def on_home() -> None: - """Trigger the homing procedure.""" - print("šŸ  [PEDAL] Moving to home position...") - data_manager.set_robot_activity_state(RobotActivityState.HOMING) - - def on_record() -> None: - """Toggle Neuracore recording session.""" - print("āŗļø [PEDAL] Toggling recording...") - current_status = nc.is_recording() - print(f"šŸ” [DEBUG] Current Neuracore recording status: {current_status}") - if not current_status: - try: - nc.start_recording() - print("āœ“ [PEDAL] Recording STARTED") - except Exception as e: - print(f"āœ— [PEDAL] Failed to start recording: {e}") - else: - try: - nc.stop_recording() - print("āœ“ [PEDAL] Recording STOPPED") - except Exception as e: - print(f"āœ— [PEDAL] Failed to stop recording: {e}") - - # Explicitly register - pedal.on("activate", on_activate) - pedal.on("home", on_home) - pedal.on("record", on_record) - - pedal.start() - print( - f"āŒØļø Foot Pedal listener started (Keys: {args.pedal_activate}, {args.pedal_home}, {args.pedal_record})" - ) - - # 3. Initialize Neuracore - print("\nšŸ”§ Initializing Neuracore...") - nc.login() - print(f"šŸ“¦ Connecting to Robot: {args.robot_name}") - nc.connect_robot( - robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=True - ) - - ds_name = f"piper-leader-teleop-{time.strftime('%Y%m%d-%H%M%S')}" - print(f"šŸ“‚ Creating Dataset: {ds_name}") - nc.create_dataset(name=ds_name) - - # 4. Start Leader Reading Thread - reader_thread = threading.Thread( - target=leader_reader_thread, - args=(data_manager, leader, args.leader_rate), - daemon=True, - ) - reader_thread.start() - - print("\nšŸš€ TELEOP READY") - print("------------------------------------------------------------") - print("1. Robot starts in DISABLED (Neutral) pose.") - print(f"2. Press '{args.pedal_activate}' to ENABLE (Mirror Leader).") - print(f"3. Press '{args.pedal_record}' to Toggle Recording.") - print("------------------------------------------------------------") - - loop_count = 0 - try: - while True: - t0 = time.time() - mapped_angles, mapped_gripper = data_manager.get_leader_mapped_state() - state = data_manager.get_robot_activity_state() - - if state == RobotActivityState.HOMING: - angles_deg = NEUTRAL_POSE_DEG - gripper_val = 0.5 - time.sleep(0.5) - data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - elif state == RobotActivityState.DISABLED: - angles_deg = NEUTRAL_POSE_DEG - gripper_val = 0.5 - else: - # ENABLED: Follow Leader - if mapped_angles is not None: - angles_deg = mapped_angles - gripper_val = mapped_gripper if mapped_gripper is not None else 0.5 - else: - angles_deg = NEUTRAL_POSE_DEG - gripper_val = 0.5 - - # Streaming and Logging - if angles_deg is not None: - loop_count += 1 - # Debug every 30 loops (~2Hz) - if loop_count % 30 == 0: - if state == RobotActivityState.DISABLED: - print( - f"šŸ’” [REMINDER] Robot is DISABLED. Press '{args.pedal_activate}' to Mirror. [Raw Grip: {gripper_val:.2f}]" - ) - else: - print( - f"šŸ“” [MIRRORING] J1-3: {np.round(angles_deg[:3], 1)} | Grip: {gripper_val:.2f}" - ) - - # Log to Neuracore - data_dict = {} - for i, deg in enumerate(angles_deg): - val = float(np.radians(deg)) - # Explicit small offset for joint4 if 0.0 is problematic - if i == 3 and abs(val) < 0.0001: - val = 0.0001 - data_dict[f"joint{i+1}"] = val - - # Visual Gripper Joints (joint7, joint8) - if gripper_val is not None: - # joint7: 0 (closed) to 0.035 (open) - # joint8: 0 (closed) to -0.035 (open) - data_dict["joint7"] = float(gripper_val * 0.035) - data_dict["joint8"] = float(-gripper_val * 0.035) - - nc.log_joint_positions(data_dict, timestamp=t0) - - else: - if loop_count % 60 == 0: - print("āš ļø [WARNING] No leader data received! Is it plugged in?") - - time.sleep(max(0, 1.0 / 60.0 - (time.time() - t0))) - - except KeyboardInterrupt: - print("\nšŸ‘‹ Teleop stopped by user.") - except Exception as e: - print(f"\nāŒ Error: {e}") - traceback.print_exc() - finally: - data_manager.request_shutdown() - pedal.stop() - reader_thread.join(timeout=1.0) - leader.disconnect() - nc.logout() - print("šŸ‘‹ Shutdown complete.") - - -if __name__ == "__main__": - main() diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 4860884..4437644 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -21,6 +21,10 @@ # Add neuracore path for local imports if needed sys.path.insert(0, str(Path(__file__).parent.parent.parent / "neuracore")) +import json +import traceback +from typing import Callable + from common.configs import ( CONTROLLER_BETA, CONTROLLER_D_CUTOFF, @@ -39,11 +43,130 @@ 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 neuracore.core.input_devices.foot_pedal import FootPedal from pink_ik_solver import PinkIKSolver from piper_controller import PiperController +# --------------------------------------------------------------------------- +# Foot Pedal logic (Moved here per PR review) +# --------------------------------------------------------------------------- + +_PEDAL_CONFIG_PATH = Path.home() / ".neuracore" / "foot_pedal.json" +_DEFAULT_MAPPINGS = {"button_a": "a", "button_b": "b", "button_c": "c"} + + +class FootPedal: + """Foot pedal reader class - fires callbacks on key press.""" + + def __init__(self, data_manager: DataManager, config: dict[str, Any] | None = None): + """Initialize FootPedal with data_manager and optional config.""" + self._data_manager = data_manager + self._config = config or self.load_config() + self._mappings = { + "button_a": self._config.get("button_a"), + "button_b": self._config.get("button_b"), + "button_c": self._config.get("button_c"), + } + + # Callbacks + self.on_button_a: Callable[[], None] | None = None + self.on_button_b: Callable[[], None] | None = None + self.on_button_c: Callable[[], None] | None = None + + @staticmethod + def load_config() -> dict[str, Any]: + """Load foot pedal key mappings from JSON file.""" + if _PEDAL_CONFIG_PATH.exists(): + try: + with open(_PEDAL_CONFIG_PATH) as f: + return dict(json.load(f)) + except Exception as e: + print(f"āš ļø Could not load pedal config: {e}") + return dict(_DEFAULT_MAPPINGS) + + def _dispatch(self, char: str) -> None: + """Fire the right callback for the detected key char.""" + if char == self._mappings.get("button_a") and self.on_button_a: + self.on_button_a() + elif char == self._mappings.get("button_b") and self.on_button_b: + self.on_button_b() + elif char == self._mappings.get("button_c") and self.on_button_c: + self.on_button_c() + + def run(self) -> None: + """Main listener loop.""" + print(f"āŒØļø Foot pedal listener started. Mappings: {self._mappings}") + + # -- evdev path (preferred on Linux) ------------------------------------ + try: + import evdev + + devices = [evdev.InputDevice(path) for path in evdev.list_devices()] + pedals = [ + d for d in devices if "PCsensor" in d.name or "FootSwitch" in d.name + ] + + if pedals: + pedal_dev = pedals[0] + for p in pedals: + if "Keyboard" in p.name: + pedal_dev = p + break + + print(f"āŒØļø Foot pedal acquired via evdev: {pedal_dev.name}") + try: + pedal_dev.grab() + for event in pedal_dev.read_loop(): + if self._data_manager.is_shutdown_requested(): + break + if event.type == evdev.ecodes.EV_KEY: + k = evdev.categorize(event) + if k.keystate == k.key_down: + key_str = k.keycode + if isinstance(key_str, list): + key_str = key_str[0] + char = key_str.replace("KEY_", "").lower() + print(f"šŸ” [PEDAL] Key: '{char}'") + self._dispatch(char) + except Exception as e: + print(f"āš ļø evdev read error: {e}") + finally: + try: + pedal_dev.ungrab() + except Exception: + pass + print("āŒØļø Foot pedal thread stopped (evdev).") + return + + except Exception as e: + print(f"āš ļø evdev unavailable: {e} ā€” falling back to pynput") + + # -- pynput fallback ---------------------------------------------------- + try: + from pynput import keyboard + + print("āŒØļø Foot pedal listener (pynput fallback) 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 self._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.") + def log_to_neuracore_on_change_callback( name: str, value: Any, timestamp: float @@ -182,13 +305,15 @@ def toggle_recording() -> None: ).start() threading.Thread(target=camera_thread, args=(data_manager,), daemon=True).start() - # Foot Pedal Initialization + # Foot Pedal ā€“ started as a daemon thread, callbacks wired inline print("\nāŒØļø Initializing Foot Pedals...") - pedal = FootPedal() - pedal.on("activate", toggle_robot_state) - pedal.on("home", move_robot_home) - pedal.on("record", toggle_recording) - pedal.start() + pedal = FootPedal(data_manager) + pedal.on_button_a = toggle_robot_state + pedal.on_button_b = move_robot_home + pedal.on_button_c = toggle_recording + + pedal_thread = threading.Thread(target=pedal.run, daemon=True) + pedal_thread.start() print("\nāœ… SYSTEM ONLINE") print("------------------------------------------------------------") @@ -202,7 +327,7 @@ def toggle_recording() -> None: except KeyboardInterrupt: print("\nšŸ‘‹ Shutting down...") finally: - pedal.stop() + pedal_thread.join(timeout=1.0) if nc.is_recording(): nc.cancel_recording() nc.logout() diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index d2a380c..9e159a1 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -93,17 +93,6 @@ def __init__(self) -> None: self.rgb_image: np.ndarray | None = None -class LeaderMappedState: - """Leader arm mapped state - joint angles and gripper from leader read_mapped().""" - - def __init__(self) -> None: - """Initialize LeaderMappedState with default values.""" - self._lock = threading.Lock() - - self.joint_angles: np.ndarray | None = None - self.gripper_open: float | None = None - - class DataManager: """Main state container coordinating all state groups. @@ -123,7 +112,6 @@ def __init__(self) -> None: self._robot_state = RobotState() self._ik_state = IKState() self._camera_state = CameraState() - self._leader_mapped_state = LeaderMappedState() # System state self._shutdown_event = threading.Event() @@ -542,42 +530,6 @@ def get_ik_success(self) -> bool: with self._ik_state._lock: return self._ik_state.success - # ============================================================================ - # Leader Mapped State Methods - # ============================================================================ - - def set_leader_mapped_state( - self, joint_angles: np.ndarray, gripper_open: float - ) -> None: - """Set leader-mapped joint angles and gripper (thread-safe). - - Args: - joint_angles: Follower-space joint angles from leader read_mapped(). - gripper_open: Gripper open amount in [0, 1]. - """ - with self._leader_mapped_state._lock: - self._leader_mapped_state.joint_angles = ( - joint_angles.copy() if joint_angles is not None else None - ) - self._leader_mapped_state.gripper_open = gripper_open - - def get_leader_mapped_state( - self, - ) -> tuple[np.ndarray | None, float | None]: - """Get leader-mapped joint angles and gripper (thread-safe). - - Returns: - Tuple of (joint_angles, gripper_open); either may be None if not set. - """ - with self._leader_mapped_state._lock: - angles = ( - self._leader_mapped_state.joint_angles.copy() - if self._leader_mapped_state.joint_angles is not None - else None - ) - gripper = self._leader_mapped_state.gripper_open - return (angles, gripper) - # ============================================================================ # System State Methods # ============================================================================ diff --git a/examples/common/threads/__init__.py b/examples/common/threads/__init__.py new file mode 100644 index 0000000..d7d934f --- /dev/null +++ b/examples/common/threads/__init__.py @@ -0,0 +1 @@ +"""Shared thread functions for teleoperation examples.""" diff --git a/scripts/foot_pedal/README.md b/scripts/foot_pedal/README.md deleted file mode 100644 index 82ce39e..0000000 --- a/scripts/foot_pedal/README.md +++ /dev/null @@ -1,51 +0,0 @@ -# Foot Pedal Control System - -The Foot Pedal system allows you to control the robot and data collection using a 3-pedal USB device (or any keyboard-mapped input). - -## Actions -- **ACTIVATE**: Enables the robot and aligns targets to current joint positions. -- **HOME**: Moves the robot arm to its predefined home position. -- **RECORD**: Toggles Neuracore data collection (Start/Stop recording). - -## Setup and Configuration - -### Remapping Keys -To map or change which pedal triggers which action, run the configuration script: - -```bash -./scripts/foot_pedal/run_pedal_config.sh -``` - -Follow the prompts in the terminal: -1. When prompted for 'ACTIVATE', press the first pedal. -2. When prompted for 'HOME', press the second pedal. -3. When prompted for 'RECORD', press the third pedal. - -The configuration is saved to `~/.neuracore/foot_pedal.json`. - -### Running the Demo -To start controlling the robot with the pedals, use the demo script: - -```bash -python3 scripts/foot_pedal/pedal_demo.py -``` - -## Developer Usage - -The pedal system is integrated into the Neuracore core library. You can use it in your own scripts as follows: - -```python -from neuracore.core.input_devices.foot_pedal import FootPedal - -pedal = FootPedal() -pedal.on("activate", your_activate_function) -pedal.on("home", your_home_function) -pedal.on("record", your_record_function) - -pedal.start() -``` - -## Troubleshooting -- If pedals are not detected, ensure the terminal window has focus. -- The `readchar` library must be installed for key detection. -- If using a serial-based pedal, use `list_potential_ports()` from `foot_pedal.py` to identify the device path. diff --git a/scripts/foot_pedal/pedal_demo.py b/scripts/foot_pedal/pedal_demo.py deleted file mode 100755 index 3fc87c0..0000000 --- a/scripts/foot_pedal/pedal_demo.py +++ /dev/null @@ -1,113 +0,0 @@ -#!/usr/bin/env python3 -"""Example of using the Foot Pedal to control the Piper robot.""" - -import sys -import time -from pathlib import Path - -# Add paths -sys.path.insert(0, str(Path(__file__).parent.parent.parent)) - -import neuracore as nc -from neuracore.core.input_devices.foot_pedal import FootPedal - -from piper_controller import PiperController - - -def main() -> None: - """Run a demonstration of foot pedal control for the Piper robot.""" - print("=" * 60) - print("FOOT PEDAL ROBOT CONTROL DEMO") - print("=" * 60) - - # Initialize robot - print("\nšŸ¤– Initializing Piper robot...") - try: - robot = PiperController(can_interface="can0", debug_mode=False) - robot.start_control_loop() - except Exception as e: - print(f"āœ— Failed to initialize robot: {e}") - print("Running in simulation/mock mode (no hardware)...") - robot = None - - # Initialize Foot Pedal - print("\nāŒØļø Initializing Foot Pedal...") - pedal = FootPedal() - - if not any(pedal.mappings.values()): - print("āš ļø Foot pedal mappings are empty. Please run run_pedal_config.sh first.") - # Default fallback for demo - pedal.mappings = {"activate": "1", "home": "2", "record": "3"} - print(f"Using default demo mappings: {pedal.mappings}") - - # Connect to Neuracore (optional, for recording) - try: - nc.login() - print("āœ“ Connected to Neuracore") - except Exception: - print( - "āš ļø Neuracore connection failed. Recording actions will only print messages." - ) - - # Define callbacks - def on_activate() -> None: - """Handle the activate/enable action.""" - print("\nšŸš€ Foot Pedal: ACTIVATE pressed") - if robot: - if robot.resume_robot(): - print("āœ“ Robot enabled") - else: - print("āœ— Failed to enable robot") - else: - print("Action: Robot ACTIVATE (MOCK)") - - def on_home() -> None: - """Handle the move-to-home action.""" - print("\nšŸ  Foot Pedal: HOME pressed") - if robot: - robot.move_to_home() - print("āœ“ Robot moving to home") - else: - print("Action: Robot HOME (MOCK)") - - def on_record() -> None: - """Handle the toggle-recording action.""" - print("\nšŸ“¹ Foot Pedal: RECORD pressed") - try: - if not nc.is_recording(): - nc.start_recording() - print("āœ“ Recording started") - else: - nc.stop_recording() - print("āœ“ Recording stopped") - except Exception as e: - print(f"āš ļø Recording action failed: {e}") - print("Action: Toggle Recording (MOCK)") - - # Register callbacks - pedal.on("activate", on_activate) - pedal.on("home", on_home) - pedal.on("record", on_record) - - # Start pedal listener - pedal.start() - - print("\nāœ… Setup complete!") - print(f"ACTIVATE: Press '{pedal.mappings['activate']}' to enable robot") - print(f"HOME: Press '{pedal.mappings['home']}' to send robot home") - print(f"RECORD: Press '{pedal.mappings['record']}' to toggle recording") - print("\nāš ļø Press Ctrl+C to exit") - - try: - while True: - time.sleep(1) - except KeyboardInterrupt: - print("\nšŸ‘‹ Stopping...") - finally: - pedal.stop() - if robot: - robot.cleanup() - - -if __name__ == "__main__": - main() diff --git a/scripts/foot_pedal/run_pedal_config.sh b/scripts/foot_pedal/run_pedal_config.sh deleted file mode 100755 index 39be813..0000000 --- a/scripts/foot_pedal/run_pedal_config.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/bash -# Script to run foot pedal configuration - -# Get the directory where the script is located -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" -REPO_ROOT="$( cd "$SCRIPT_DIR/../../.." &> /dev/null && pwd )" - -# Run the python config script -PYTHONPATH="$REPO_ROOT/neuracore:$PYTHONPATH" python3 "$REPO_ROOT/neuracore/neuracore/core/input_devices/pedal_config.py" From 2f6154c7093013f56c8b24ff6176c33ed0847d13 Mon Sep 17 00:00:00 2001 From: Pruthvi-Neuracore Date: Mon, 16 Mar 2026 16:15:30 +0000 Subject: [PATCH 04/31] Integrated foot pedal teleop alignment with robust headless testing and dashboard sync - Final Lint Clean --- examples/7_teleop_with_pedal.py | 312 +++++++++++--------------- examples/common/threads/foot_pedal.py | 148 ++++++++++++ 2 files changed, 280 insertions(+), 180 deletions(-) create mode 100644 examples/common/threads/foot_pedal.py diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 4437644..7bd3f4b 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -21,11 +21,8 @@ # Add neuracore path for local imports if needed sys.path.insert(0, str(Path(__file__).parent.parent.parent / "neuracore")) -import json -import traceback -from typing import Callable -from common.configs import ( +from common.configs import ( # noqa: E402 CONTROLLER_BETA, CONTROLLER_D_CUTOFF, CONTROLLER_MIN_CUTOFF, @@ -37,135 +34,27 @@ ROBOT_RATE, URDF_PATH, ) -from common.data_manager import DataManager, RobotActivityState -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 common.data_manager import DataManager, RobotActivityState # noqa: E402 +from common.threads.camera import camera_thread # noqa: E402 +from common.threads.foot_pedal import FootPedal # noqa: E402 +from common.threads.ik_solver import ik_solver_thread # noqa: E402 +from common.threads.joint_state import joint_state_thread # noqa: E402 +from common.threads.quest_reader import quest_reader_thread # noqa: E402 +from meta_quest_teleop.reader import MetaQuestReader # noqa: E402 -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController +from pink_ik_solver import PinkIKSolver # noqa: E402 +from piper_controller import PiperController # noqa: E402 # --------------------------------------------------------------------------- -# Foot Pedal logic (Moved here per PR review) # --------------------------------------------------------------------------- - -_PEDAL_CONFIG_PATH = Path.home() / ".neuracore" / "foot_pedal.json" -_DEFAULT_MAPPINGS = {"button_a": "a", "button_b": "b", "button_c": "c"} - - -class FootPedal: - """Foot pedal reader class - fires callbacks on key press.""" - - def __init__(self, data_manager: DataManager, config: dict[str, Any] | None = None): - """Initialize FootPedal with data_manager and optional config.""" - self._data_manager = data_manager - self._config = config or self.load_config() - self._mappings = { - "button_a": self._config.get("button_a"), - "button_b": self._config.get("button_b"), - "button_c": self._config.get("button_c"), - } - - # Callbacks - self.on_button_a: Callable[[], None] | None = None - self.on_button_b: Callable[[], None] | None = None - self.on_button_c: Callable[[], None] | None = None - - @staticmethod - def load_config() -> dict[str, Any]: - """Load foot pedal key mappings from JSON file.""" - if _PEDAL_CONFIG_PATH.exists(): - try: - with open(_PEDAL_CONFIG_PATH) as f: - return dict(json.load(f)) - except Exception as e: - print(f"āš ļø Could not load pedal config: {e}") - return dict(_DEFAULT_MAPPINGS) - - def _dispatch(self, char: str) -> None: - """Fire the right callback for the detected key char.""" - if char == self._mappings.get("button_a") and self.on_button_a: - self.on_button_a() - elif char == self._mappings.get("button_b") and self.on_button_b: - self.on_button_b() - elif char == self._mappings.get("button_c") and self.on_button_c: - self.on_button_c() - - def run(self) -> None: - """Main listener loop.""" - print(f"āŒØļø Foot pedal listener started. Mappings: {self._mappings}") - - # -- evdev path (preferred on Linux) ------------------------------------ - try: - import evdev - - devices = [evdev.InputDevice(path) for path in evdev.list_devices()] - pedals = [ - d for d in devices if "PCsensor" in d.name or "FootSwitch" in d.name - ] - - if pedals: - pedal_dev = pedals[0] - for p in pedals: - if "Keyboard" in p.name: - pedal_dev = p - break - - print(f"āŒØļø Foot pedal acquired via evdev: {pedal_dev.name}") - try: - pedal_dev.grab() - for event in pedal_dev.read_loop(): - if self._data_manager.is_shutdown_requested(): - break - if event.type == evdev.ecodes.EV_KEY: - k = evdev.categorize(event) - if k.keystate == k.key_down: - key_str = k.keycode - if isinstance(key_str, list): - key_str = key_str[0] - char = key_str.replace("KEY_", "").lower() - print(f"šŸ” [PEDAL] Key: '{char}'") - self._dispatch(char) - except Exception as e: - print(f"āš ļø evdev read error: {e}") - finally: - try: - pedal_dev.ungrab() - except Exception: - pass - print("āŒØļø Foot pedal thread stopped (evdev).") - return - - except Exception as e: - print(f"āš ļø evdev unavailable: {e} ā€” falling back to pynput") - - # -- pynput fallback ---------------------------------------------------- - try: - from pynput import keyboard - - print("āŒØļø Foot pedal listener (pynput fallback) 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 self._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.") +# Foot Pedal Configuration - Edit these to match your hardware +# ENABLE_DISABLE_PEDAL -> Toggle robot ENABLE / DISABLE +# HOME_POSITION_PEDAL -> Move robot to HOME position +# RECORD_TOGGLE_PEDAL -> Toggle data recording (Matches Quest RJ) +# --------------------------------------------------------------------------- +ENABLE_DISABLE_PEDAL = "a" +HOME_POSITION_PEDAL = "b" +RECORD_TOGGLE_PEDAL = "c" def log_to_neuracore_on_change_callback( @@ -201,48 +90,67 @@ def log_to_neuracore_on_change_callback( 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) - robot_controller.graceful_stop() + if robot_controller: + robot_controller.graceful_stop() data_manager.set_teleop_state(False, None, None) - print("āœ“ [ACTION] Robot DISABLED") - elif state == RobotActivityState.DISABLED: + 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("āœ“ [ACTION] Robot 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("šŸ  [ACTION] Moving to home position...") + print("šŸ  Pedal pressed - Moving to home position...") data_manager.set_robot_activity_state(RobotActivityState.HOMING) data_manager.set_teleop_state(False, None, None) - if not robot_controller.move_to_home(): - print("āœ— [ACTION] Homing failed") + + 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("āš ļø [ACTION] Cannot home: robot not enabled") + 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("āœ“ [ACTION] Recording STARTED") + print("āœ“ šŸ”“ Recording started (Pedal)") except Exception as e: - print(f"āœ— [ACTION] Recording failed to start: {e}") + print(f"āœ— Failed to start recording: {e}") else: try: nc.stop_recording() - print("āœ“ [ACTION] Recording STOPPED") + print("āœ“ ā¹ļø Recording stopped (Pedal)") except Exception as e: - print(f"āœ— [ACTION] Recording failed to stop: {e}") + print(f"āœ— Failed to stop recording: {e}") if __name__ == "__main__": @@ -259,13 +167,15 @@ def toggle_recording() -> None: # Neuracore Init print("\nšŸ”§ Initializing Neuracore...") - 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") + 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() @@ -276,38 +186,68 @@ def toggle_recording() -> None: # Robot Initialization print("\nšŸ¤– Initializing Piper...") - robot_controller = PiperController(can_interface="can0", robot_rate=ROBOT_RATE) - robot_controller.start_control_loop() + 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)...") - threading.Thread( - target=joint_state_thread, args=(data_manager, robot_controller), daemon=True - ).start() - - 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() + 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 - 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() - threading.Thread(target=camera_thread, args=(data_manager,), daemon=True).start() + 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(data_manager) + pedal = FootPedal( + data_manager, + key_map={ + "button_a": ENABLE_DISABLE_PEDAL, + "button_b": HOME_POSITION_PEDAL, + "button_c": RECORD_TOGGLE_PEDAL, + }, + ) pedal.on_button_a = toggle_robot_state pedal.on_button_b = move_robot_home pedal.on_button_c = toggle_recording @@ -317,8 +257,8 @@ def toggle_recording() -> None: print("\nāœ… SYSTEM ONLINE") print("------------------------------------------------------------") - print("šŸŽ® QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") - print("āŒØļø PEDAL CONTROLS: ACTIVATE (Enable), HOME (Reset), RECORD") + print("šŸŽ® QUEST CONTROLS: Hold GRIP to move, TRIGGER for gripper") + print("āŒØļø PEDAL CONTROLS: ENABLE/DISABLE (A), HOME (B), RECORD (C)") print("------------------------------------------------------------") try: @@ -327,10 +267,22 @@ def toggle_recording() -> None: except KeyboardInterrupt: print("\nšŸ‘‹ Shutting down...") finally: - pedal_thread.join(timeout=1.0) - if nc.is_recording(): - nc.cancel_recording() - nc.logout() + 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() - quest_reader.stop() - robot_controller.cleanup() + if quest_reader: + try: + quest_reader.stop() + except Exception: + pass + if robot_controller: + try: + robot_controller.cleanup() + except Exception: + pass diff --git a/examples/common/threads/foot_pedal.py b/examples/common/threads/foot_pedal.py new file mode 100644 index 0000000..5007953 --- /dev/null +++ b/examples/common/threads/foot_pedal.py @@ -0,0 +1,148 @@ +"""Foot pedal reader thread ā€“ evdev/pynput listener that fires button callbacks.""" + +import sys +import time +import traceback +from pathlib import Path +from typing import Any, Callable + +from common.data_manager import DataManager + +# Add workspace roots to path for standalone testing +_examples_path = Path(__file__).parent.parent.parent +sys.path.insert(0, str(_examples_path)) # Add 'examples' folder to find 'common' +sys.path.insert(0, str(_examples_path.parent)) # Add 'example_agilex' folder + + +class FootPedal: + """Foot pedal reader ā€“ fires task-agnostic button callbacks on key press. + + Assign callables to ``on_button_a``, ``on_button_b``, ``on_button_c`` + then call ``run()`` in a daemon thread:: + + pedal = FootPedal( + data_manager, + {"button_a": "a", "button_b": "b", "button_c": "c"}, + ) + pedal.on_button_a = my_fn + threading.Thread(target=pedal.run, daemon=True).start() + """ + + def __init__( + self, + data_manager: DataManager, + key_map: dict[str, Any], + ) -> None: + """Initialize FootPedal. + + Args: + data_manager: Shared state manager (used for shutdown signalling). + key_map: Mapping of button names to key chars, e.g. + ``{"button_a": "a", "button_b": "b", "button_c": "c"}``. + """ + self._data_manager = data_manager + self._key_map = key_map + + self.on_button_a: Callable[[], None] | None = None + self.on_button_b: Callable[[], None] | None = None + self.on_button_c: Callable[[], None] | None = None + + def _dispatch(self, char: str) -> None: + """Fire the matching callback for *char*.""" + if char == self._key_map.get("button_a") and self.on_button_a: + self.on_button_a() + elif char == self._key_map.get("button_b") and self.on_button_b: + self.on_button_b() + elif char == self._key_map.get("button_c") and self.on_button_c: + self.on_button_c() + + def run(self) -> None: + """Block and dispatch key events until shutdown is requested.""" + print(f"āŒØļø Foot pedal listener started. Mappings: {self._key_map}") + + # -- evdev (preferred on Linux, exclusive grab) ---------------------- + try: + import evdev # type: ignore[import] + + devices = [evdev.InputDevice(p) for p in evdev.list_devices()] + pedals = [ + d for d in devices if "PCsensor" in d.name or "FootSwitch" in d.name + ] + + if pedals: + dev = next((p for p in pedals if "Keyboard" in p.name), pedals[0]) + print(f"āŒØļø Foot pedal acquired via evdev: {dev.name}") + try: + dev.grab() + for event in dev.read_loop(): + if self._data_manager.is_shutdown_requested(): + break + if event.type == evdev.ecodes.EV_KEY: + k = evdev.categorize(event) + if k.keystate == k.key_down: + key_str = k.keycode + if isinstance(key_str, list): + key_str = key_str[0] + char = key_str.replace("KEY_", "").lower() + print(f"šŸ” [PEDAL] Key: '{char}'") + self._dispatch(char) + except Exception as e: + print(f"āš ļø evdev read error: {e}") + finally: + try: + dev.ungrab() + except Exception: + pass + print("āŒØļø Foot pedal stopped (evdev).") + return + + except Exception as e: + print(f"āš ļø evdev unavailable: {e} ā€” falling back to pynput") + + # -- pynput fallback ------------------------------------------------ + try: + from pynput import keyboard # type: ignore[import] + + print("āŒØļø Foot pedal listener (pynput fallback) started.") + + def on_press(key: object) -> None: + """Forward key press.""" + try: + char = key.char if hasattr(key, "char") else str(key) # type: ignore[union-attr] + self._dispatch(char) + except Exception: + pass + + with keyboard.Listener(on_press=on_press) as listener: + while not self._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__": + # -- Standalone Hardware Test ------------------------------------------- + from common.data_manager import DataManager + + print("šŸš€ Standalone Foot Pedal Hardware Test") + print("--------------------------------------") + dm = DataManager() + # Default mappings + mappings = {"button_a": "a", "button_b": "b", "button_c": "c"} + pedal = FootPedal(dm, mappings) + + pedal.on_button_a = lambda: print("āœ“ šŸŸ” Pedal A (ENABLE/DISABLE) detected") + pedal.on_button_b = lambda: print("āœ“ šŸ  Pedal B (HOME) detected") + pedal.on_button_c = lambda: print("āœ“ šŸ”“ Pedal C (RECORD) detected") + + try: + pedal.run() + except KeyboardInterrupt: + print("\nšŸ‘‹ Test stopped.") From 437c688a249ad66c718f701333f22699dc26590b Mon Sep 17 00:00:00 2001 From: mark Date: Fri, 20 Mar 2026 16:44:46 +0000 Subject: [PATCH 05/31] temp commit --- examples/1_tune_teleop_params.py | 26 ++- .../2_collect_teleop_data_with_neuracore.py | 47 +++--- examples/3_replay_neuracore_episodes.py | 6 +- examples/4_rollout_neuracore_policy.py | 73 +++++---- .../5_rollout_neuracore_policy_minimal.py | 47 +++--- examples/6_visualize_policy_from_dataset.py | 73 ++++++--- examples/7_teleop_with_pedal.py | 47 +++--- examples/common/configs.py | 23 ++- examples/common/data_manager.py | 102 ++++++++---- examples/common/foot_pedal.py | 107 +++++++++++++ examples/common/robot_visualizer.py | 32 ++++ examples/common/threads/camera_usb.py | 75 +++++++++ examples/common/threads/foot_pedal.py | 148 ------------------ examples/common/threads/ik_solver.py | 9 +- examples/common/threads/joint_state.py | 8 +- .../{camera.py => realsense_camera.py} | 5 +- piper_controller.py | 10 +- 17 files changed, 495 insertions(+), 343 deletions(-) create mode 100644 examples/common/foot_pedal.py create mode 100644 examples/common/threads/camera_usb.py delete mode 100644 examples/common/threads/foot_pedal.py rename examples/common/threads/{camera.py => realsense_camera.py} (92%) diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index 47f748a..b439e48 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -22,6 +22,7 @@ from common.configs import ( CAMERA_FRAME_STREAMING_RATE, + CAMERA_NAMES, CONTROLLER_BETA, CONTROLLER_D_CUTOFF, CONTROLLER_DATA_RATE, @@ -32,6 +33,7 @@ IK_SOLVER_RATE, JOINT_STATE_STREAMING_RATE, LM_DAMPING, + META_QUEST_AXIS_MASK, NEUTRAL_JOINT_ANGLES, ORIENTATION_COST, POSITION_COST, @@ -46,10 +48,10 @@ ) from common.data_manager import DataManager, RobotActivityState from common.robot_visualizer import RobotVisualizer -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 common.threads.realsense_camera import camera_thread from meta_quest_teleop.reader import MetaQuestReader from pink_ik_solver import PinkIKSolver @@ -146,7 +148,12 @@ def on_button_b_pressed() -> None: # Initialize Meta Quest reader print("\nšŸŽ® Initializing Meta Quest reader...") -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, +) # Register button callbacks (after state and robot_controller are initialized) quest_reader.on("button_a_pressed", on_button_a_pressed) @@ -206,6 +213,8 @@ def on_button_b_pressed() -> None: 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, @@ -225,7 +234,6 @@ def on_button_b_pressed() -> None: posture_cost_vector=POSTURE_COST_VECTOR, ) visualizer.add_controller_visualization() -visualizer.add_target_frame_visualization() # Set up button callbacks @@ -292,9 +300,10 @@ def on_go_home() -> None: min_cutoff, beta, d_cutoff = visualizer.get_controller_filter_params() data_manager.set_controller_filter_params(min_cutoff, beta, d_cutoff) - # Update scaling factors (module-level variables used by IK thread) - TRANSLATION_SCALE = visualizer.get_translation_scale() - ROTATION_SCALE = visualizer.get_rotation_scale() + # Update scaling factors (shared with IK thread via DataManager) + translation_scale = visualizer.get_translation_scale() + rotation_scale = visualizer.get_rotation_scale() + data_manager.set_teleop_scaling(translation_scale, rotation_scale) # Update Pink parameters (GUI controls) pink_params = visualizer.get_pink_parameters() @@ -312,6 +321,7 @@ def on_go_home() -> None: 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) @@ -332,6 +342,10 @@ def on_go_home() -> None: # 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) diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index fedff4e..0cc16bd 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -13,6 +13,7 @@ import time import traceback from pathlib import Path +from typing import Any import neuracore as nc import numpy as np @@ -29,9 +30,7 @@ DAMPING_COST, FRAME_TASK_GAIN, GRIPPER_FRAME_NAME, - GRIPPER_LOGGING_NAME, IK_SOLVER_RATE, - JOINT_NAMES, JOINT_STATE_STREAMING_RATE, LM_DAMPING, NEUTRAL_JOINT_ANGLES, @@ -44,10 +43,10 @@ URDF_PATH, ) from common.data_manager import DataManager, RobotActivityState -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 common.threads.realsense_camera import camera_thread as realsense_camera_thread from meta_quest_teleop.reader import MetaQuestReader from pink_ik_solver import PinkIKSolver @@ -55,32 +54,22 @@ def log_to_neuracore_on_change_callback( - name: str, value: float, timestamp: float + name: str, payload: dict[str, Any], timestamp: float ) -> None: """Log data to queue on change callback.""" # Call appropriate Neuracore logging function try: if name == "log_joint_positions": - data_value = np.radians(value) - data_dict = { - joint_name: angle for joint_name, angle in zip(JOINT_NAMES, data_value) - } - nc.log_joint_positions(data_dict, timestamp=timestamp) + nc.log_joint_positions(payload, timestamp=timestamp) elif name == "log_joint_target_positions": - data_value = np.radians(value) - data_dict = { - joint_name: angle for joint_name, angle in zip(JOINT_NAMES, data_value) - } - nc.log_joint_target_positions(data_dict, timestamp=timestamp) + nc.log_joint_target_positions(payload, timestamp=timestamp) elif name == "log_parallel_gripper_open_amounts": - data_dict = {GRIPPER_LOGGING_NAME: value} - nc.log_parallel_gripper_open_amounts(data_dict, timestamp=timestamp) + nc.log_parallel_gripper_open_amounts(payload, timestamp=timestamp) elif name == "log_parallel_gripper_target_open_amounts": - data_dict = {GRIPPER_LOGGING_NAME: value} - nc.log_parallel_gripper_target_open_amounts(data_dict, timestamp=timestamp) + nc.log_parallel_gripper_target_open_amounts(payload, timestamp=timestamp) elif name == "log_rgb": - camera_name = "rgb" - image_array = value + 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}") @@ -278,12 +267,17 @@ def on_button_rj_pressed() -> None: ) 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 + # Start cameras threads + print("\nšŸ“· Starting cameras threads...") + realsense_camera_thread_obj = threading.Thread( + target=realsense_camera_thread, args=(data_manager,), daemon=True ) - camera_thread_obj.start() + 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...") @@ -328,7 +322,8 @@ def on_button_rj_pressed() -> None: quest_thread.join() quest_reader.stop() ik_thread.join() - camera_thread_obj.join() + realsense_camera_thread_obj.join() + # usb_camera_thread_obj.join() robot_controller.cleanup() print("\nšŸ‘‹ Demo stopped.") diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index fc06f26..11df770 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -10,7 +10,7 @@ import neuracore as nc import numpy as np from common.configs import ( - GRIPPER_LOGGING_NAME, + GRIPPER_NAME, JOINT_NAMES, NEUTRAL_JOINT_ANGLES, ROBOT_RATE, @@ -140,8 +140,8 @@ def main() -> None: gripper_data = step.data[ DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS ] - if GRIPPER_LOGGING_NAME in gripper_data: - gripper_value = gripper_data[GRIPPER_LOGGING_NAME].open_amount + 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 diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 3841ef9..3a089c2 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -17,6 +17,7 @@ import numpy as np from neuracore_types import ( BatchedJointData, + BatchedNCData, BatchedParallelGripperOpenAmountData, DataType, ) @@ -26,7 +27,7 @@ from common.configs import ( CAMERA_FRAME_STREAMING_RATE, - CAMERA_LOGGING_NAME, + CAMERA_NAMES, CONTROLLER_BETA, CONTROLLER_D_CUTOFF, CONTROLLER_DATA_RATE, @@ -34,7 +35,7 @@ DAMPING_COST, FRAME_TASK_GAIN, GRIPPER_FRAME_NAME, - GRIPPER_LOGGING_NAME, + GRIPPER_NAME, IK_SOLVER_RATE, JOINT_NAMES, JOINT_STATE_STREAMING_RATE, @@ -57,41 +58,42 @@ from common.data_manager import DataManager, RobotActivityState from common.policy_state import PolicyState from common.robot_visualizer import RobotVisualizer -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 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 +# NOTE: JOINT_NAMES is the order used by the URDF / robot controller. +# MODEL_JOINT_NAMES is the order used by the Neuracore model. +MODEL_JOINT_NAMES = ["joint6", "joint4", "joint5", "joint2", "joint1", "joint3"] -def convert_predictions_to_horizon_dict(predictions: dict) -> dict[str, list[float]]: - """Convert predictions dict to horizon dict format.""" - horizon: dict[str, list[float]] = {} - # Extract joint target positions +def convert_predictions_to_horizon( + predictions: dict[DataType, dict[str, BatchedNCData]], +) -> dict[str, list[float]]: + """Convert predictions to horizon dict.""" + 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): - # Extract values: (B, T, 1) -> list[float], taking B=0 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: gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - if GRIPPER_LOGGING_NAME in gripper_data: - batched = gripper_data[GRIPPER_LOGGING_NAME] + if GRIPPER_NAME in gripper_data: + batched = gripper_data[GRIPPER_NAME] if isinstance(batched, BatchedParallelGripperOpenAmountData): - # Extract values: (B, T, 1) -> list[float], taking B=0 values = batched.open_amount[0, :, 0].cpu().numpy().tolist() - horizon[GRIPPER_LOGGING_NAME] = values - + horizon[GRIPPER_NAME] = values return horizon @@ -169,20 +171,24 @@ def run_policy( if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in model_input_order: gripper_open_value = data_manager.get_current_gripper_open_value() if gripper_open_value is not None: - nc.log_parallel_gripper_open_amount( - GRIPPER_LOGGING_NAME, gripper_open_value - ) + 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 model_input_order: - rgb_image = data_manager.get_rgb_image() - if rgb_image is not None: - nc.log_rgb(CAMERA_LOGGING_NAME, rgb_image) - print(" āœ“ Logged RGB image") + # Log all available cameras (payload as-is; reporter = this script) + logged_any_rgb = False + for camera_name in CAMERA_NAMES: + rgb_image = data_manager.get_rgb_image(camera_name) + if rgb_image is not None: + nc.log_rgb(camera_name, rgb_image) + logged_any_rgb = True + if logged_any_rgb: + print(" āœ“ Logged RGB image(s)") else: print(" āš ļø No RGB image available") + rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) # Check if we have at least some data to run the policy if ( @@ -196,8 +202,8 @@ def run_policy( # Get policy prediction try: start_time = time.time() - predictions = policy.predict(timeout=5) - prediction_horizon = convert_predictions_to_horizon_dict(predictions) + predictions = policy.predict(timeout=60) + prediction_horizon = convert_predictions_to_horizon(predictions) end_time = time.time() horizon_length = policy_state.get_prediction_horizon_length() print( @@ -476,10 +482,10 @@ def policy_execution_thread( ) # Send current gripper open value to robot (if available) - if GRIPPER_LOGGING_NAME in locked_horizon: - current_gripper_target_open_value = locked_horizon[ - GRIPPER_LOGGING_NAME - ][execution_index] + 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 ) @@ -568,6 +574,11 @@ def update_visualization( 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() @@ -707,13 +718,13 @@ def update_visualization( # This order is determined by the output_robot_data_spec in the training config. # The order here should match the order in your training config's output_robot_data_spec. model_input_order = { - DataType.JOINT_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME], - DataType.RGB_IMAGES: [CAMERA_LOGGING_NAME], + DataType.JOINT_POSITIONS: MODEL_JOINT_NAMES, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], + DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } model_output_order = { - DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME], + DataType.JOINT_TARGET_POSITIONS: MODEL_JOINT_NAMES, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } print("\nšŸ“‹ Model input order:") diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 8695337..4a029a6 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -27,8 +27,8 @@ sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - CAMERA_LOGGING_NAME, - GRIPPER_LOGGING_NAME, + CAMERA_NAMES, + GRIPPER_NAME, JOINT_NAMES, NEUTRAL_JOINT_ANGLES, POLICY_EXECUTION_RATE, @@ -38,8 +38,8 @@ ) from common.data_manager import DataManager, RobotActivityState from common.policy_state import PolicyState -from common.threads.camera import camera_thread from common.threads.joint_state import joint_state_thread +from common.threads.realsense_camera import camera_thread from piper_controller import PiperController @@ -62,45 +62,42 @@ def convert_predictions_to_horizon_dict(predictions: dict) -> dict[str, list[flo # Extract gripper open amounts if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - if GRIPPER_LOGGING_NAME in gripper_data: - batched = gripper_data[GRIPPER_LOGGING_NAME] + if GRIPPER_NAME in gripper_data: + batched = gripper_data[GRIPPER_NAME] if isinstance(batched, BatchedParallelGripperOpenAmountData): # Extract values: (B, T, 1) -> list[float], taking B=0 values = batched.open_amount[0, :, 0].cpu().numpy().tolist() - horizon[GRIPPER_LOGGING_NAME] = values + horizon[GRIPPER_NAME] = values return horizon def log_current_state(data_manager: DataManager) -> None: - """Log current state to Neuracore.""" + """Log current state to Neuracore. Reporter: build payload (e.g. joints in radians), log as-is.""" current_joint_angles = data_manager.get_current_joint_angles() if current_joint_angles is None: print("āš ļø No joint angles available") return - # Get current gripper open value gripper_open_value = data_manager.get_current_gripper_open_value() if gripper_open_value is None: print("āš ļø No gripper open value available") return - # Get current RGB image - rgb_image = data_manager.get_rgb_image() - if rgb_image is None: - print("āš ļø No RGB image available") - return - - # Prepare data for Neuracore logging + # Reporter: convert to radians for Neuracore joint_angles_rad = np.radians(current_joint_angles) joint_positions_dict = { - joint_name: angle for joint_name, angle in zip(JOINT_NAMES, joint_angles_rad) + joint_name: float(angle) + for joint_name, angle in zip(JOINT_NAMES, joint_angles_rad) } - - # Log joint positions, parallel gripper open amounts, and RGB image to Neuracore nc.log_joint_positions(joint_positions_dict) - nc.log_parallel_gripper_open_amount(GRIPPER_LOGGING_NAME, gripper_open_value) - nc.log_rgb(CAMERA_LOGGING_NAME, rgb_image) + nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_open_value) + + # Log all available cameras as-is + for camera_name in CAMERA_NAMES: + rgb_image = data_manager.get_rgb_image(camera_name) + if rgb_image is not None: + nc.log_rgb(camera_name, rgb_image) def run_policy( @@ -160,8 +157,8 @@ def execute_horizon( robot_controller.set_target_joint_angles(current_joint_target_positions_deg) # Send current gripper open value to robot (if available) - if GRIPPER_LOGGING_NAME in locked_horizon: - current_gripper_target_open_value = locked_horizon[GRIPPER_LOGGING_NAME][i] + if GRIPPER_NAME in locked_horizon: + current_gripper_target_open_value = locked_horizon[GRIPPER_NAME][i] robot_controller.set_gripper_open_value(current_gripper_target_open_value) # Log current state for visualization @@ -230,12 +227,12 @@ def execute_horizon( # The order here should match the order in your training config's output_robot_data_spec. model_input_order = { DataType.JOINT_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME], - DataType.RGB_IMAGES: [CAMERA_LOGGING_NAME], + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], + DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } model_output_order = { DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME], + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } print("\nšŸ“‹ Model input order:") diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index 633f6d9..a780d71 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -29,12 +29,15 @@ sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - CAMERA_LOGGING_NAME, - GRIPPER_LOGGING_NAME, + CAMERA_NAMES, + GRIPPER_NAME, JOINT_NAMES, + POLICY_EXECUTION_RATE, URDF_PATH, ) +MODEL_JOINT_NAMES = ["joint6", "joint4", "joint5", "joint2", "joint1", "joint3"] + # Parse arguments parser = argparse.ArgumentParser( description="Visualize policy predictions from dataset" @@ -54,7 +57,10 @@ help="Name of remote Neuracore policy endpoint to use instead of a local policy.", ) parser.add_argument( - "--frequency", type=int, default=100, help="Frequency of visualization" + "--frequency", + type=int, + default=POLICY_EXECUTION_RATE, + help="Frequency of visualization", ) args = parser.parse_args() @@ -65,13 +71,13 @@ # Load policy model_input_order = { - DataType.JOINT_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME], - DataType.RGB_IMAGES: [CAMERA_LOGGING_NAME], + # DataType.JOINT_POSITIONS: MODEL_JOINT_NAMES, + # DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], + DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } model_output_order = { - DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME], + DataType.JOINT_TARGET_POSITIONS: MODEL_JOINT_NAMES, + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } if args.remote_endpoint_name: @@ -143,6 +149,7 @@ current_horizon = None current_action_idx = 0 playing = False +rgb_gui_handle = None def convert_predictions_to_horizon( @@ -160,17 +167,17 @@ def convert_predictions_to_horizon( horizon[joint_name] = values if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - if GRIPPER_LOGGING_NAME in gripper_data: - batched = gripper_data[GRIPPER_LOGGING_NAME] + 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_LOGGING_NAME] = values + horizon[GRIPPER_NAME] = values return horizon def select_random_state() -> None: """Select random state and run policy.""" - global current_horizon, current_action_idx, playing + global current_horizon, current_action_idx, playing, rgb_gui_handle # Select random episode and step episode_idx = random.randint(0, len(synced_dataset) - 1) @@ -197,31 +204,47 @@ def select_random_state() -> None: gripper_value = 1.0 if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in step.data: gripper_data = step.data[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] - if GRIPPER_LOGGING_NAME in gripper_data: - gripper_value = gripper_data[GRIPPER_LOGGING_NAME].open_amount + if GRIPPER_NAME in gripper_data: + gripper_value = gripper_data[GRIPPER_NAME].open_amount # Log to Neuracore for visualization - nc.log_parallel_gripper_open_amount(GRIPPER_LOGGING_NAME, gripper_value) + nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_value) # Extract RGB image rgb_image = None if DataType.RGB_IMAGES in step.data: rgb_data = step.data[DataType.RGB_IMAGES] - if CAMERA_LOGGING_NAME in rgb_data: - rgb_image = np.array(rgb_data[CAMERA_LOGGING_NAME].frame) - # Save image to file for visualization + if CAMERA_NAMES[0] in rgb_data: + rgb_image = np.array(rgb_data[CAMERA_NAMES[0]].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 # Log to Neuracore for visualization - nc.log_rgb(CAMERA_LOGGING_NAME, rgb_image) - + nc.log_rgb(CAMERA_NAMES[0], rgb_image) # Get policy prediction print("šŸŽÆ Getting policy prediction...") - predictions = policy.predict(timeout=5) + start_time = time.time() + try: + predictions = policy.predict(timeout=60) + except nc.EndpointError as e: + print(f"āœ— Failed to get policy prediction: {e}") + import traceback + + traceback.print_exc() + return + duration = time.time() - start_time current_horizon = convert_predictions_to_horizon(predictions) current_action_idx = 0 playing = True - print("FINISHED PREDICTION") + print(f"FINISHED PREDICTION in {duration:.3f} s") # Update robot to initial pose from first step in the horizon @@ -229,7 +252,7 @@ def select_random_state() -> None: urdf_vis.update_cfg(joint_positions) print( - f"āœ… Prediction received: {len(current_horizon.get(JOINT_NAMES[0], []))} actions" + f"āœ… Prediction received: {len(current_horizon.get(MODEL_JOINT_NAMES[0], []))} actions" ) @@ -289,9 +312,7 @@ def select_random_state() -> None: nc.log_joint_positions(joint_config_dict) # Update gripper value - gripper_value = current_horizon[GRIPPER_LOGGING_NAME][ - current_action_idx - ] + gripper_value = current_horizon[GRIPPER_NAME][current_action_idx] gripper_handle.value = round( gripper_value, 2 ) # Round to 2 decimal places diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 7bd3f4b..4b00a1f 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -16,13 +16,10 @@ import neuracore as nc import numpy as np -# Add parent directory to path +# Add parent directory to path to import pink_ik_solver and piper_controller sys.path.insert(0, str(Path(__file__).parent.parent)) -# Add neuracore path for local imports if needed -sys.path.insert(0, str(Path(__file__).parent.parent.parent / "neuracore")) - -from common.configs import ( # noqa: E402 +from common.configs import ( CONTROLLER_BETA, CONTROLLER_D_CUTOFF, CONTROLLER_MIN_CUTOFF, @@ -34,24 +31,17 @@ ROBOT_RATE, URDF_PATH, ) -from common.data_manager import DataManager, RobotActivityState # noqa: E402 -from common.threads.camera import camera_thread # noqa: E402 -from common.threads.foot_pedal import FootPedal # noqa: E402 -from common.threads.ik_solver import ik_solver_thread # noqa: E402 -from common.threads.joint_state import joint_state_thread # noqa: E402 -from common.threads.quest_reader import quest_reader_thread # noqa: E402 -from meta_quest_teleop.reader import MetaQuestReader # noqa: E402 - -from pink_ik_solver import PinkIKSolver # noqa: E402 -from piper_controller import PiperController # noqa: E402 - -# --------------------------------------------------------------------------- -# --------------------------------------------------------------------------- -# Foot Pedal Configuration - Edit these to match your hardware -# ENABLE_DISABLE_PEDAL -> Toggle robot ENABLE / DISABLE -# HOME_POSITION_PEDAL -> Move robot to HOME position -# RECORD_TOGGLE_PEDAL -> Toggle data recording (Matches Quest RJ) -# --------------------------------------------------------------------------- +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" @@ -241,18 +231,19 @@ def toggle_recording() -> None: # Foot Pedal ā€“ started as a daemon thread, callbacks wired inline print("\nāŒØļø Initializing Foot Pedals...") pedal = FootPedal( - data_manager, key_map={ "button_a": ENABLE_DISABLE_PEDAL, "button_b": HOME_POSITION_PEDAL, "button_c": RECORD_TOGGLE_PEDAL, }, ) - pedal.on_button_a = toggle_robot_state - pedal.on_button_b = move_robot_home - pedal.on_button_c = toggle_recording + 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, daemon=True) + pedal_thread = threading.Thread( + target=pedal.run_loop, args=(data_manager,), daemon=True + ) pedal_thread.start() print("\nāœ… SYSTEM ONLINE") diff --git a/examples/common/configs.py b/examples/common/configs.py index d2ac818..dfe043b 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -43,21 +43,34 @@ JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore CAMERA_FRAME_STREAMING_RATE = 60.0 # Data collection rate for camera frame +# Meta quest axis mask +META_QUEST_AXIS_MASK = [1, 1, 1, 0, 0, 0] # [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 (degrees) -NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619] +# 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] # 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 = 100.0 # Hz +POLICY_EXECUTION_RATE = 20.0 # Hz PREDICTION_HORIZON_EXECUTION_RATIO = ( - 0.8 # percentage of the prediction horizon that is executed + 1.0 # percentage of the prediction horizon that is executed ) MAX_SAFETY_THRESHOLD = 20.0 # degrees MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds -GRIPPER_LOGGING_NAME = "gripper" -CAMERA_LOGGING_NAME = "rgb" +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/data_manager.py b/examples/common/data_manager.py index 9e159a1..44baa15 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -12,6 +12,7 @@ import numpy as np +from .configs import GRIPPER_NAME, JOINT_NAMES from .one_euro_filter import OneEuroFilterTransform @@ -54,6 +55,9 @@ def __init__(self) -> None: 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 class RobotState: @@ -84,13 +88,14 @@ def __init__(self) -> None: class CameraState: - """Camera state - RGB image, depth image, point cloud.""" + """Camera state - RGB images for one or more cameras.""" def __init__(self) -> None: """Initialize CameraState with default values.""" self._lock = threading.Lock() - self.rgb_image: np.ndarray | None = None + # Map from camera name -> latest RGB image + self.rgb_images: dict[str, np.ndarray] = {} class DataManager: @@ -116,12 +121,15 @@ def __init__(self) -> None: # System state self._shutdown_event = threading.Event() - # Callback for state changes (RGB, target joints, current joints) - # the callable takes arguments: (stream_name: str, data: Any, timestamp: float) - self._on_change_callback: Callable[[str, Any, float], None] | None = None + # 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}. + self._on_change_callback: ( + Callable[[str, dict[str, Any], float], None] | None + ) = None def set_on_change_callback( - self, on_change_callback: Callable[[str, Any, float], None] + 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 @@ -130,23 +138,22 @@ def set_on_change_callback( # Camera State Methods # ============================================================================ - def get_rgb_image(self) -> np.ndarray | None: - """Get RGB image (thread-safe).""" + 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: - return ( - self._camera_state.rgb_image.copy() - if self._camera_state.rgb_image is not None - else None - ) + 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) -> None: - """Set RGB image (thread-safe).""" + def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None: + """Set RGB image for a specific camera (thread-safe).""" with self._camera_state._lock: - self._camera_state.rgb_image = image.copy() + self._camera_state.rgb_images[camera_name] = image.copy() if self._on_change_callback: - self._on_change_callback( - "log_rgb", self._camera_state.rgb_image.copy(), time.time() - ) + img_copy = self._camera_state.rgb_images[camera_name].copy() + self._on_change_callback("log_rgb", {camera_name: img_copy}, time.time()) # ============================================================================ # Controller State Methods @@ -284,6 +291,35 @@ def set_teleop_state( robot_initial.copy() if robot_initial is not None else None ) + 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, + self._teleop_state.rotation_scale, + ) + def get_teleop_active(self) -> bool: """Get teleoperation active state (thread-safe).""" with self._teleop_state._lock: @@ -358,11 +394,12 @@ 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: - self._on_change_callback( - "log_joint_positions", - self._robot_state.joint_angles.copy(), - time.time(), - ) + 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._on_change_callback("log_joint_positions", payload, time.time()) def get_current_end_effector_pose(self) -> np.ndarray | None: """Get current end effector pose (thread-safe). @@ -410,7 +447,7 @@ def set_current_gripper_open_value(self, value: float) -> None: if self._on_change_callback: self._on_change_callback( "log_parallel_gripper_open_amounts", - value, + {GRIPPER_NAME: value}, time.time(), ) @@ -434,7 +471,7 @@ def set_target_gripper_open_value(self, value: float) -> None: if self._on_change_callback: self._on_change_callback( "log_parallel_gripper_target_open_amounts", - self._robot_state.target_gripper_open_value, + {GRIPPER_NAME: self._robot_state.target_gripper_open_value}, time.time(), ) @@ -464,11 +501,14 @@ 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: - self._on_change_callback( - "log_joint_target_positions", - self._ik_state.target_joint_angles.copy(), - time.time(), - ) + 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._on_change_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). diff --git a/examples/common/foot_pedal.py b/examples/common/foot_pedal.py new file mode 100644 index 0000000..903b552 --- /dev/null +++ b/examples/common/foot_pedal.py @@ -0,0 +1,107 @@ +"""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.") diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py index e8eb548..0547095 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -82,6 +82,7 @@ def __init__(self, urdf_path: str) -> 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 @@ -133,6 +134,37 @@ def add_gripper_status_controls(self) -> None: "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") diff --git a/examples/common/threads/camera_usb.py b/examples/common/threads/camera_usb.py new file mode 100644 index 0000000..329c11e --- /dev/null +++ b/examples/common/threads/camera_usb.py @@ -0,0 +1,75 @@ +"""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") diff --git a/examples/common/threads/foot_pedal.py b/examples/common/threads/foot_pedal.py deleted file mode 100644 index 5007953..0000000 --- a/examples/common/threads/foot_pedal.py +++ /dev/null @@ -1,148 +0,0 @@ -"""Foot pedal reader thread ā€“ evdev/pynput listener that fires button callbacks.""" - -import sys -import time -import traceback -from pathlib import Path -from typing import Any, Callable - -from common.data_manager import DataManager - -# Add workspace roots to path for standalone testing -_examples_path = Path(__file__).parent.parent.parent -sys.path.insert(0, str(_examples_path)) # Add 'examples' folder to find 'common' -sys.path.insert(0, str(_examples_path.parent)) # Add 'example_agilex' folder - - -class FootPedal: - """Foot pedal reader ā€“ fires task-agnostic button callbacks on key press. - - Assign callables to ``on_button_a``, ``on_button_b``, ``on_button_c`` - then call ``run()`` in a daemon thread:: - - pedal = FootPedal( - data_manager, - {"button_a": "a", "button_b": "b", "button_c": "c"}, - ) - pedal.on_button_a = my_fn - threading.Thread(target=pedal.run, daemon=True).start() - """ - - def __init__( - self, - data_manager: DataManager, - key_map: dict[str, Any], - ) -> None: - """Initialize FootPedal. - - Args: - data_manager: Shared state manager (used for shutdown signalling). - key_map: Mapping of button names to key chars, e.g. - ``{"button_a": "a", "button_b": "b", "button_c": "c"}``. - """ - self._data_manager = data_manager - self._key_map = key_map - - self.on_button_a: Callable[[], None] | None = None - self.on_button_b: Callable[[], None] | None = None - self.on_button_c: Callable[[], None] | None = None - - def _dispatch(self, char: str) -> None: - """Fire the matching callback for *char*.""" - if char == self._key_map.get("button_a") and self.on_button_a: - self.on_button_a() - elif char == self._key_map.get("button_b") and self.on_button_b: - self.on_button_b() - elif char == self._key_map.get("button_c") and self.on_button_c: - self.on_button_c() - - def run(self) -> None: - """Block and dispatch key events until shutdown is requested.""" - print(f"āŒØļø Foot pedal listener started. Mappings: {self._key_map}") - - # -- evdev (preferred on Linux, exclusive grab) ---------------------- - try: - import evdev # type: ignore[import] - - devices = [evdev.InputDevice(p) for p in evdev.list_devices()] - pedals = [ - d for d in devices if "PCsensor" in d.name or "FootSwitch" in d.name - ] - - if pedals: - dev = next((p for p in pedals if "Keyboard" in p.name), pedals[0]) - print(f"āŒØļø Foot pedal acquired via evdev: {dev.name}") - try: - dev.grab() - for event in dev.read_loop(): - if self._data_manager.is_shutdown_requested(): - break - if event.type == evdev.ecodes.EV_KEY: - k = evdev.categorize(event) - if k.keystate == k.key_down: - key_str = k.keycode - if isinstance(key_str, list): - key_str = key_str[0] - char = key_str.replace("KEY_", "").lower() - print(f"šŸ” [PEDAL] Key: '{char}'") - self._dispatch(char) - except Exception as e: - print(f"āš ļø evdev read error: {e}") - finally: - try: - dev.ungrab() - except Exception: - pass - print("āŒØļø Foot pedal stopped (evdev).") - return - - except Exception as e: - print(f"āš ļø evdev unavailable: {e} ā€” falling back to pynput") - - # -- pynput fallback ------------------------------------------------ - try: - from pynput import keyboard # type: ignore[import] - - print("āŒØļø Foot pedal listener (pynput fallback) started.") - - def on_press(key: object) -> None: - """Forward key press.""" - try: - char = key.char if hasattr(key, "char") else str(key) # type: ignore[union-attr] - self._dispatch(char) - except Exception: - pass - - with keyboard.Listener(on_press=on_press) as listener: - while not self._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__": - # -- Standalone Hardware Test ------------------------------------------- - from common.data_manager import DataManager - - print("šŸš€ Standalone Foot Pedal Hardware Test") - print("--------------------------------------") - dm = DataManager() - # Default mappings - mappings = {"button_a": "a", "button_b": "b", "button_c": "c"} - pedal = FootPedal(dm, mappings) - - pedal.on_button_a = lambda: print("āœ“ šŸŸ” Pedal A (ENABLE/DISABLE) detected") - pedal.on_button_b = lambda: print("āœ“ šŸ  Pedal B (HOME) detected") - pedal.on_button_c = lambda: print("āœ“ šŸ”“ Pedal C (RECORD) detected") - - try: - pedal.run() - except KeyboardInterrupt: - print("\nšŸ‘‹ Test stopped.") diff --git a/examples/common/threads/ik_solver.py b/examples/common/threads/ik_solver.py index e15af40..9bd1175 100644 --- a/examples/common/threads/ik_solver.py +++ b/examples/common/threads/ik_solver.py @@ -9,7 +9,7 @@ # 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, ROTATION_SCALE, TRANSLATION_SCALE +from common.configs import IK_SOLVER_RATE from common.data_manager import DataManager, RobotActivityState from common.utils import scale_and_add_delta_transform @@ -83,11 +83,14 @@ def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None 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, + translation_scale, + rotation_scale, robot_initial, ) diff --git a/examples/common/threads/joint_state.py b/examples/common/threads/joint_state.py index b39a50a..e663625 100644 --- a/examples/common/threads/joint_state.py +++ b/examples/common/threads/joint_state.py @@ -45,12 +45,10 @@ def joint_state_thread( print("āœ“ Robot reached home position and is re-enabled") elif robot_activity_state == RobotActivityState.ENABLED: - if ( - target_joint_angles is not None - and trigger_value is not None - and data_manager.get_teleop_active() - ): + 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: target_gripper_open_value = 1.0 - trigger_value data_manager.set_target_gripper_open_value( target_gripper_open_value diff --git a/examples/common/threads/camera.py b/examples/common/threads/realsense_camera.py similarity index 92% rename from examples/common/threads/camera.py rename to examples/common/threads/realsense_camera.py index 8a7b6c6..24e6dfe 100644 --- a/examples/common/threads/camera.py +++ b/examples/common/threads/realsense_camera.py @@ -5,7 +5,7 @@ import numpy as np import pyrealsense2 as rs -from common.configs import CAMERA_FRAME_STREAMING_RATE +from common.configs import CAMERA_FRAME_STREAMING_RATE, CAMERA_NAMES from common.data_manager import DataManager @@ -13,6 +13,7 @@ def camera_thread(data_manager: DataManager) -> None: """Camera thread - captures RGB images from RealSense.""" print("šŸ“· Camera thread started") + camera_name = CAMERA_NAMES[0] dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE pipeline: rs.pipeline | None = None @@ -46,7 +47,7 @@ def camera_thread(data_manager: DataManager) -> None: 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) + data_manager.set_rgb_image(color_image, camera_name) # Sleep to maintain loop rate elapsed = time.time() - iteration_start diff --git a/piper_controller.py b/piper_controller.py index 980725c..ff6cd9c 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -86,8 +86,8 @@ def __init__( ) # Gripper range in degrees (for internal SDK communication) - self.GRIPPER_DEGREES_MIN = -5.000 - self.GRIPPER_DEGREES_MAX = 95.00 + self.GRIPPER_DEGREES_MIN = -10.00 + self.GRIPPER_DEGREES_MAX = 30.00 self.GRIPPER_DEGREES_RANGE = self.GRIPPER_DEGREES_MAX - self.GRIPPER_DEGREES_MIN # Home gripper value in degrees @@ -169,7 +169,9 @@ def _initialize_robot(self) -> None: """Initialize robot connection and enable it.""" print(f"Initializing robot on {self.can_interface}...") self.piper = C_PiperInterface_V2( - self.can_interface, start_sdk_joint_limit=True, start_sdk_gripper_limit=True + self.can_interface, + start_sdk_joint_limit=True, + start_sdk_gripper_limit=False, ) self.piper.ConnectPort() @@ -479,7 +481,7 @@ def control_loop(self) -> None: self._send_end_effector_command(self._target_pose) elif current_mode == PiperController.ControlMode.JOINT_SPACE: # Send joint angles command - self._send_joint_command(self._target_joint_angles) + self._send_joint_command(self._target_joint_angles.copy()) # Always send gripper command regardless of control mode self._send_gripper_command(self._gripper_open_value_degrees) From 96437bbaa683fb6448f062eb03311795203a09c2 Mon Sep 17 00:00:00 2001 From: mark Date: Wed, 1 Apr 2026 19:26:24 +0100 Subject: [PATCH 06/31] added torques as current values --- .../2_collect_teleop_data_with_neuracore.py | 15 +++++++--- examples/4_rollout_neuracore_policy.py | 29 ++++++++++++------- examples/common/configs.py | 6 ++-- examples/common/data_manager.py | 28 ++++++++++++++++++ examples/common/threads/joint_state.py | 7 ++++- piper_controller.py | 25 ++++++++++++++++ 6 files changed, 91 insertions(+), 19 deletions(-) diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 0cc16bd..1e43557 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -61,6 +61,8 @@ def log_to_neuracore_on_change_callback( 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": @@ -293,14 +295,18 @@ def on_button_rj_pressed() -> None: print() try: - while True: + 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...") @@ -315,15 +321,16 @@ def on_button_rj_pressed() -> None: print("Traceback:") traceback.print_exc() - # shutdown threads - nc.logout() + # shutdown threads and data producers before logging out data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - quest_thread.join() 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.") diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 3a089c2..1427a90 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -67,10 +67,6 @@ from pink_ik_solver import PinkIKSolver from piper_controller import PiperController -# NOTE: JOINT_NAMES is the order used by the URDF / robot controller. -# MODEL_JOINT_NAMES is the order used by the Neuracore model. -MODEL_JOINT_NAMES = ["joint6", "joint4", "joint5", "joint2", "joint1", "joint3"] - def convert_predictions_to_horizon( predictions: dict[DataType, dict[str, BatchedNCData]], @@ -702,7 +698,7 @@ def update_visualization( 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(f" šŸ–„ļø Visualization: {VISUALIZATION_RATE} Hz") # Connect to Neuracore print("\nšŸ”§ Initializing Neuracore...") @@ -713,17 +709,28 @@ def update_visualization( overwrite=False, ) - # Load policy from either train run name or model path - # NOTE: The model_output_order MUST match the exact order used during training - # This order is determined by the output_robot_data_spec in the training config. - # The order here should match the order in your training config's output_robot_data_spec. + # Load policy model_input_order = { - DataType.JOINT_POSITIONS: MODEL_JOINT_NAMES, + DataType.JOINT_POSITIONS: [ + "joint2", + "joint5", + "joint4", + "joint3", + "joint1", + "joint6", + ], DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } model_output_order = { - DataType.JOINT_TARGET_POSITIONS: MODEL_JOINT_NAMES, + DataType.JOINT_TARGET_POSITIONS: [ + "joint2", + "joint5", + "joint4", + "joint3", + "joint1", + "joint6", + ], DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } diff --git a/examples/common/configs.py b/examples/common/configs.py index dfe043b..78cb175 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -30,8 +30,8 @@ GRIP_THRESHOLD = 0.9 # Grip value threshold to activate control # Scaling factors for translation and rotation -TRANSLATION_SCALE = 3.0 -ROTATION_SCALE = 2.0 +TRANSLATION_SCALE = 1.0 +ROTATION_SCALE = 1.0 # Thread rates (Hz) CONTROLLER_DATA_RATE = 50.0 # Controller input reading @@ -44,7 +44,7 @@ CAMERA_FRAME_STREAMING_RATE = 60.0 # Data collection rate for camera frame # Meta quest axis mask -META_QUEST_AXIS_MASK = [1, 1, 1, 0, 0, 0] # [x, y, z, roll, pitch, yaw] +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. diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index 44baa15..c87480f 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -68,6 +68,7 @@ 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 @@ -401,6 +402,33 @@ def set_current_joint_angles(self, angles: np.ndarray) -> None: } self._on_change_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() + if self._robot_state.joint_torques is not None + else 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()) + def get_current_end_effector_pose(self) -> np.ndarray | None: """Get current end effector pose (thread-safe). diff --git a/examples/common/threads/joint_state.py b/examples/common/threads/joint_state.py index e663625..322e37b 100644 --- a/examples/common/threads/joint_state.py +++ b/examples/common/threads/joint_state.py @@ -29,6 +29,11 @@ def joint_state_thread( 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: @@ -48,7 +53,7 @@ def joint_state_thread( 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: + 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 diff --git a/piper_controller.py b/piper_controller.py index ff6cd9c..3b0e471 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -710,6 +710,29 @@ def get_current_joint_angles(self) -> np.ndarray | None: print(f"Failed to get current joint angles: {e}") return None + def get_current_joint_currents(self) -> np.ndarray | None: + """Get the current measured motor currents from the robot, if available. + + Returns: + numpy array or None: [m1, m2, m3, m4, m5, m6] in amps, or None if unavailable + """ + if hasattr(self, "piper") and self.piper is not None: + try: + high_spd_msg = self.piper.GetArmHighSpdInfoMsgs() + if high_spd_msg: + # Current is reported in 0.001A units per motor + motors = [] + for i in range(1, 7): + motor = getattr(high_spd_msg, f"motor_{i}", None) + if motor is None: + return None + motors.append(motor.current / 1000.0) + return np.array(motors, dtype=np.float64) + except Exception as e: + if self.debug_mode: + print(f"Failed to get current motor currents: {e}") + return None + def get_current_gripper_open_value(self) -> float | None: """Get the current measured gripper open value from the robot, if available. @@ -746,6 +769,7 @@ def get_robot_status(self) -> dict[str, Any]: "gripper_open_value": self.get_gripper_open_value(), "current_end_pose": self.get_current_end_effector_pose(), "current_joint_angles": self.get_current_joint_angles(), + "current_joint_currents": self.get_current_joint_currents(), "current_gripper_open_value": self.get_current_gripper_open_value(), } return status @@ -760,6 +784,7 @@ def get_robot_status(self) -> dict[str, Any]: "gripper_open_value": None, "current_end_pose": None, "current_joint_angles": None, + "current_joint_currents": None, "current_gripper_open_value": None, } From c59ecf6b0f363107692a6552d7444d20e00b5357 Mon Sep 17 00:00:00 2001 From: mark Date: Wed, 15 Apr 2026 10:48:17 +0100 Subject: [PATCH 07/31] feat: wrist twisting and scaling controls --- environment.yaml | 2 +- examples/1_tune_teleop_params.py | 73 +++++++++++++++++++++- examples/common/configs.py | 6 +- examples/common/data_manager.py | 19 ++++++ examples/common/threads/ik_solver.py | 34 +++++----- pink_ik_solver.py | 30 +++++++-- piper_controller.py | 44 ++++++++----- scripts/piper/piper_gui_control.py | 93 ++++++++++++++++++++++++++++ 8 files changed, 261 insertions(+), 40 deletions(-) diff --git a/environment.yaml b/environment.yaml index 2fc42d5..b6f46f6 100644 --- a/environment.yaml +++ b/environment.yaml @@ -7,7 +7,7 @@ dependencies: - pip: - piper-sdk==0.6.1 - pin - - pin-pink + - pin-pink==3.5.0 - qpsolvers[quadprog] - yourdfpy - pure-python-adb diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index b439e48..3a8ea8e 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -34,17 +34,21 @@ 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 @@ -62,7 +66,6 @@ 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 @@ -94,6 +97,59 @@ def on_button_b_pressed() -> None: 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" ) @@ -132,6 +188,8 @@ def on_button_b_pressed() -> None: 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, ) @@ -158,6 +216,9 @@ def on_button_b_pressed() -> None: # 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...") @@ -284,8 +345,11 @@ def on_go_home() -> None: 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. Release grip to stop") -print(" 7. Use 'Emergency Stop' in GUI if needed") +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() @@ -303,6 +367,9 @@ def on_go_home() -> None: # 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) diff --git a/examples/common/configs.py b/examples/common/configs.py index 78cb175..e8142b0 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -32,6 +32,9 @@ # Scaling factors for translation and rotation TRANSLATION_SCALE = 1.0 ROTATION_SCALE = 1.0 +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 @@ -51,9 +54,10 @@ CAMERA_WIDTH = 640 CAMERA_HEIGHT = 480 -# # Initial neutral pose for robot (degrees) +# # 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_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] diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py index c87480f..fec186d 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -58,6 +58,7 @@ def __init__(self) -> 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: @@ -326,6 +327,24 @@ 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: + """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 + ) + 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]: diff --git a/examples/common/threads/ik_solver.py b/examples/common/threads/ik_solver.py index 9bd1175..e43a33e 100644 --- a/examples/common/threads/ik_solver.py +++ b/examples/common/threads/ik_solver.py @@ -29,20 +29,6 @@ def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None # Get current robot joint angles from state current_joint_angles = data_manager.get_current_joint_angles() - current_ik_joint_angles = np.degrees(ik_solver.get_current_configuration()) - - # Sync IK solver with actual joint angles if close enough - if ( - current_joint_angles is not None - and 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) - ) # Get current end effector pose from IK solver and set in state ik_ee_pose = ik_solver.get_current_end_effector_pose() @@ -53,6 +39,26 @@ def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None 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 diff --git a/pink_ik_solver.py b/pink_ik_solver.py index f4bdcfc..f424fe8 100644 --- a/pink_ik_solver.py +++ b/pink_ik_solver.py @@ -179,6 +179,22 @@ def _setup_tasks( print("āœ… Tasks configured!") + def _rebuild_end_effector_task(self) -> None: + """Recreate the FrameTask using current frame-task parameters. + + Some Pink versions expose FrameTask parameters as read-only attributes. + Rebuilding the task is the safest way to apply runtime parameter updates. + """ + assert self.configuration is not None, "Configuration must be initialized" + self.ee_task = FrameTask( + self.end_effector_frame, + position_cost=self.position_cost, + orientation_cost=self.orientation_cost, + lm_damping=self.lm_damping, + gain=self.frame_task_gain, + ) + self.ee_task.set_target_from_configuration(self.configuration) + def update_task_parameters( self, position_cost: float | None = None, @@ -204,25 +220,26 @@ def update_task_parameters( """ assert self.ee_task is not None, "End effector task must be initialized" assert self.damping_task is not None, "Damping task must be initialized" + should_rebuild_ee_task = False if position_cost is not None: self.position_cost = position_cost - self.ee_task.position_cost = position_cost + should_rebuild_ee_task = True if orientation_cost is not None: self.orientation_cost = orientation_cost - self.ee_task.orientation_cost = orientation_cost + should_rebuild_ee_task = True if frame_task_gain is not None: self.frame_task_gain = frame_task_gain - self.ee_task.gain = frame_task_gain + should_rebuild_ee_task = True if lm_damping is not None: self.lm_damping = lm_damping - self.ee_task.lm_damping = lm_damping + should_rebuild_ee_task = True if damping_cost is not None: self.damping_cost = damping_cost - self.damping_task.cost = damping_cost + self.damping_task = DampingTask(cost=self.damping_cost) if solver_damping_value is not None: self.solver_damping_value = solver_damping_value @@ -240,6 +257,9 @@ def update_task_parameters( assert self.posture_task is not None, "Posture task must be initialized" self.posture_task.cost = self.posture_cost_vector + if should_rebuild_ee_task: + self._rebuild_end_effector_task() + def set_target_pose(self, position: np.ndarray, orientation: np.ndarray) -> None: """Set target pose from position and orientation. diff --git a/piper_controller.py b/piper_controller.py index 3b0e471..68049b3 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -29,8 +29,9 @@ def __init__( can_interface: str = "can0", robot_rate: float = 100.0, control_mode: "PiperController.ControlMode" = ControlMode.JOINT_SPACE, - neutral_joint_angles: np.ndarray | None = None, - neutral_end_effector_pose: np.ndarray | None = None, + enable_joint_angle_limits: bool = True, + neutral_joint_angles: np.ndarray | list[float] | None = None, + neutral_end_effector_pose: np.ndarray | list[float] | None = None, debug_mode: bool = False, ) -> None: """Initialize the robot controller. @@ -39,13 +40,15 @@ def __init__( can_interface: CAN interface for robot communication (default: 'can0') robot_rate: Robot control loop rate in Hz (default: 100.0) control_mode: Initial control mode (END_EFFECTOR or JOINT_SPACE) + enable_joint_angle_limits: Enable SDK + software joint angle limits (default: True) neutral_joint_angles: Neutral joint angles [j1, j2, j3, j4, j5, j6] in degrees (default: None) - neutral_end_effector_pose: Neutral end effector pose as 4x4 transformation matrix (default: None) + neutral_end_effector_pose: Neutral end effector 6D pose [x, y, z, rx, ry, rz] in mm/degrees (default: None) debug_mode: Enable debug logging (default: False) """ self.can_interface = can_interface self.robot_rate = robot_rate self.debug_mode = debug_mode + self.enable_joint_angle_limits = enable_joint_angle_limits # Thread synchronization self.position_lock = threading.Lock() @@ -65,11 +68,12 @@ def __init__( # HOME positions in end effector space and joint space if neutral_end_effector_pose is not None: - if neutral_end_effector_pose.shape == (4, 4): - self.HOME_POSE = neutral_end_effector_pose.copy().astype(np.float64) + neutral_pose_6d = np.array(neutral_end_effector_pose, dtype=np.float64) + if neutral_pose_6d.shape == (6,): + self.HOME_POSE = self._pose_6d_to_4x4(neutral_pose_6d) else: raise ValueError( - "neutral_end_effector_pose must be a 4x4 transformation matrix" + "neutral_end_effector_pose must be a 6D pose [x, y, z, rx, ry, rz]" ) else: # Convert default 6D pose to 4x4 matrix @@ -170,7 +174,7 @@ def _initialize_robot(self) -> None: print(f"Initializing robot on {self.can_interface}...") self.piper = C_PiperInterface_V2( self.can_interface, - start_sdk_joint_limit=True, + start_sdk_joint_limit=self.enable_joint_angle_limits, start_sdk_gripper_limit=False, ) self.piper.ConnectPort() @@ -399,12 +403,15 @@ def set_target_joint_angles(self, joint_angles: np.ndarray) -> None: with self.position_lock: angles = np.array(joint_angles, dtype=np.float64) - # Clamp joint angles to limits using numpy - clamped_angles = np.clip( - angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1] - ) + if self.enable_joint_angle_limits: + # Clamp joint angles to limits using numpy + target_angles = np.clip( + angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1] + ) + else: + target_angles = angles - self._target_joint_angles = clamped_angles + self._target_joint_angles = target_angles if self.debug_mode: print(f"Target joint angles set: {self._target_joint_angles}") @@ -419,10 +426,13 @@ def update_target_joint_angles(self, joint_deltas: np.ndarray) -> None: deltas = np.array(joint_deltas, dtype=np.float64) new_joint_angles = self._target_joint_angles + deltas - # Clamp joint angles to limits using numpy - self._target_joint_angles = np.clip( - new_joint_angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1] - ) + if self.enable_joint_angle_limits: + # Clamp joint angles to limits using numpy + self._target_joint_angles = np.clip( + new_joint_angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1] + ) + else: + self._target_joint_angles = new_joint_angles if self.debug_mode: print(f"Joint angles updated: {self._target_joint_angles}") @@ -764,6 +774,7 @@ def get_robot_status(self) -> dict[str, Any]: status = { "enabled": self.is_robot_enabled(), "control_mode": self.get_control_mode(), + "joint_angle_limits_enabled": self.enable_joint_angle_limits, "target_pose": self.get_target_pose(), "target_joint_angles": self.get_target_joint_angles(), "gripper_open_value": self.get_gripper_open_value(), @@ -779,6 +790,7 @@ def get_robot_status(self) -> dict[str, Any]: return { "enabled": None, "control_mode": None, + "joint_angle_limits_enabled": None, "target_pose": None, "target_joint_angles": None, "gripper_open_value": None, diff --git a/scripts/piper/piper_gui_control.py b/scripts/piper/piper_gui_control.py index 2f4d1fc..b1ab17e 100644 --- a/scripts/piper/piper_gui_control.py +++ b/scripts/piper/piper_gui_control.py @@ -325,6 +325,22 @@ def _create_gui(self) -> None: ) self.home_button.grid(row=0, column=1, padx=10) + # Copy current joint + gripper values to clipboard + self.copy_pose_button = ttk.Button( + button_frame, + text="Copy Joint+Gripper", + command=self._on_copy_joint_gripper_pressed, + ) + self.copy_pose_button.grid(row=0, column=2, padx=10) + + # Copy current end-effector pose to clipboard + self.copy_ee_position_button = ttk.Button( + button_frame, + text="Copy EE Pose", + command=self._on_copy_ee_position_pressed, + ) + self.copy_ee_position_button.grid(row=0, column=3, padx=10) + # Configure grid weights self.root.columnconfigure(0, weight=1) self.root.rowconfigure(0, weight=1) @@ -718,6 +734,83 @@ def _on_stop_resume_pressed(self) -> None: 2000, lambda: self.status_label.config(text="Ready", foreground="green") ) + def _on_copy_joint_gripper_pressed(self) -> None: + """Copy current joint positions and gripper value to clipboard.""" + status = self.robot.get_robot_status() + + joint_angles = status["current_joint_angles"] + if joint_angles is None: + joint_angles = status["target_joint_angles"] + + gripper_value = status["current_gripper_open_value"] + if gripper_value is None: + gripper_value = status["gripper_open_value"] + + if joint_angles is None or gripper_value is None: + self.status_label.config( + text="No joint/gripper data available to copy", foreground="red" + ) + self.root.after( + 2000, lambda: self.status_label.config(text="Ready", foreground="green") + ) + return + + joint_text = ", ".join(f"{float(angle):.3f}" for angle in joint_angles) + clipboard_text = ( + f"joint_positions: {joint_text}\n" + f"gripper_value: {float(gripper_value):.3f}" + ) + + self.root.clipboard_clear() + self.root.clipboard_append(clipboard_text) + self.root.update() + + self.status_label.config( + text="Copied joint positions and gripper to clipboard", foreground="blue" + ) + self.root.after( + 2000, lambda: self.status_label.config(text="Ready", foreground="green") + ) + + def _on_copy_ee_position_pressed(self) -> None: + """Copy current end-effector pose (XYZ + RPY) to clipboard.""" + status = self.robot.get_robot_status() + + pose = status["current_end_pose"] + if pose is None: + pose = status["target_pose"] + + if pose is None: + self.status_label.config( + text="No end-effector pose available to copy", foreground="red" + ) + self.root.after( + 2000, lambda: self.status_label.config(text="Ready", foreground="green") + ) + return + + position = pose[:3, 3] + rotation = Rotation.from_matrix(pose[:3, :3]).as_euler("xyz", degrees=True) + clipboard_text = ( + f"ee_position: x={float(position[0]):.3f}, " + f"y={float(position[1]):.3f}, " + f"z={float(position[2]):.3f}\n" + f"ee_rotation_xyz_deg: rx={float(rotation[0]):.3f}, " + f"ry={float(rotation[1]):.3f}, " + f"rz={float(rotation[2]):.3f}" + ) + + self.root.clipboard_clear() + self.root.clipboard_append(clipboard_text) + self.root.update() + + self.status_label.config( + text="Copied end-effector pose to clipboard", foreground="blue" + ) + self.root.after( + 2000, lambda: self.status_label.config(text="Ready", foreground="green") + ) + def command_handler(self) -> None: """Handle commands from GUI in separate thread.""" dt = 1.0 / self.control_update_rate From be485c5717132d2a6e7842945fb1b2e38111e2a0 Mon Sep 17 00:00:00 2001 From: Pruthvi-Neuracore Date: Wed, 15 Apr 2026 14:08:33 +0100 Subject: [PATCH 08/31] feat: add heartbeat --- piper_controller.py | 12 ++++++++++++ scripts/piper/can_activate.sh | 6 ++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/piper_controller.py b/piper_controller.py index 68049b3..e979bcc 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -477,11 +477,23 @@ def control_loop(self) -> None: Only sends commands when robot is in ENABLED state. """ loop_period = 1.0 / self.robot_rate + # Re-enable heartbeat every 1 second to recover from robot self-disable + reenable_interval = 1.0 + last_reenable_time = time.time() while self.running.is_set(): try: # Only send commands if robot is enabled if self.is_robot_enabled(): + now = time.time() + # Periodically re-send EnablePiper to keep hardware enabled + # (robot firmware can self-disable on faults/limits) + if now - last_reenable_time >= reenable_interval: + if not self.piper.EnablePiper(): + if self.debug_mode: + print("Control loop: EnablePiper re-enable returned False") + last_reenable_time = now + # Get current control mode current_mode = self.get_control_mode() diff --git a/scripts/piper/can_activate.sh b/scripts/piper/can_activate.sh index 1a957d1..bf254fe 100644 --- a/scripts/piper/can_activate.sh +++ b/scripts/piper/can_activate.sh @@ -121,16 +121,18 @@ else # Set the interface bitrate and activate it. sudo ip link set "$INTERFACE_NAME" down - sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE + sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE restart-ms 100 sudo ip link set "$INTERFACE_NAME" up + sudo ip link set "$INTERFACE_NAME" txqueuelen 1000 echo "Interface $INTERFACE_NAME has been reset to bitrate $DEFAULT_BITRATE and activated." - + # Rename the interface to the default name. if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME." sudo ip link set "$INTERFACE_NAME" down sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME" sudo ip link set "$DEFAULT_CAN_NAME" up + sudo ip link set "$DEFAULT_CAN_NAME" txqueuelen 1000 echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated." fi fi From d82694f999189df03943df5845088741ca68021a Mon Sep 17 00:00:00 2001 From: mark Date: Wed, 15 Apr 2026 16:41:40 +0100 Subject: [PATCH 09/31] feat: add wrist rotation and rescaling to example 2 --- .../2_collect_teleop_data_with_neuracore.py | 74 ++++++++++++++++++- 1 file changed, 73 insertions(+), 1 deletion(-) diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 1e43557..2d04bd5 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -33,14 +33,21 @@ 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 @@ -139,6 +146,61 @@ def on_button_rj_pressed() -> None: 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") @@ -196,6 +258,7 @@ def on_button_rj_pressed() -> None: CONTROLLER_BETA, CONTROLLER_D_CUTOFF, ) + data_manager.set_teleop_scaling(TRANSLATION_SCALE, ROTATION_SCALE) # Initialize robot controller print("\nšŸ¤– Initializing Piper robot controller...") @@ -204,6 +267,8 @@ def on_button_rj_pressed() -> None: 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, ) @@ -224,12 +289,16 @@ def on_button_rj_pressed() -> None: 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...") @@ -290,7 +359,10 @@ def on_button_rj_pressed() -> None: 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. Release grip to stop") + 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() From 45bb1404849b73645ff5bc9a96a99451716dbedf Mon Sep 17 00:00:00 2001 From: mark Date: Tue, 19 May 2026 13:00:18 +0100 Subject: [PATCH 10/31] feat: add cross emobdiment fixes --- examples/3_replay_neuracore_episodes.py | 41 ++++-- examples/4_rollout_neuracore_policy.py | 126 +++++++++++++----- .../5_rollout_neuracore_policy_minimal.py | 53 +++++--- examples/6_visualize_policy_from_dataset.py | 70 +++++----- 4 files changed, 193 insertions(+), 97 deletions(-) diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index 11df770..2153bcb 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -15,7 +15,14 @@ NEUTRAL_JOINT_ANGLES, ROBOT_RATE, ) -from neuracore_types import DataType, RobotDataSpec, SynchronizedPoint +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 @@ -53,29 +60,37 @@ def main() -> None: print("\nšŸ” Getting dataset from Neuracore...") dataset = nc.get_dataset(args.dataset_name) - # Build robot_data_spec for synchronization - print("\nšŸ” Building robot data spec for synchronization...") - data_types_to_synchronize = [ + # 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.JOINT_TARGET_POSITIONS, DataType.RGB_IMAGES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, + ] + output_modalities: list[DataType] = [ + DataType.JOINT_TARGET_POSITIONS, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, ] - robot_data_spec: RobotDataSpec = {} - robot_ids_dataset = dataset.robot_ids - for robot_id in robot_ids_dataset: - data_type_to_names = dataset.get_full_data_spec(robot_id) - robot_data_spec[robot_id] = { - data_type: data_type_to_names[data_type] - for data_type in data_types_to_synchronize + 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, - robot_data_spec=robot_data_spec, + cross_embodiment_union=cross_embodiment_union, ) # Determine which episodes to play diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 1427a90..04bbcdb 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -15,11 +15,14 @@ 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 @@ -68,6 +71,13 @@ 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]]: @@ -140,7 +150,7 @@ def run_policy( policy: nc.policy, policy_state: PolicyState, visualizer: RobotVisualizer, - model_input_order: dict[DataType, list[str]], + input_embodiment_description: EmbodimentDescription, ) -> bool: """Handle Run Policy button press to capture state and get policy prediction.""" print("Running policy...") @@ -150,21 +160,25 @@ def run_policy( gripper_open_value = None rgb_image = None - # Only log data types that are in model_input_order - if DataType.JOINT_POSITIONS in model_input_order: + 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 = { - joint_name: angle - for joint_name, angle in zip(JOINT_NAMES, joint_angles_rad) + 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 model_input_order: + 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) @@ -172,19 +186,22 @@ def run_policy( else: print(" āš ļø No gripper open value available") - if DataType.RGB_IMAGES in model_input_order: - # Log all available cameras (payload as-is; reporter = this script) + 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 CAMERA_NAMES: - rgb_image = data_manager.get_rgb_image(camera_name) - if rgb_image is not None: - nc.log_rgb(camera_name, rgb_image) + 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") - rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0]) + 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 ( @@ -309,11 +326,17 @@ def run_and_start_policy_execution( policy: nc.policy, policy_state: PolicyState, visualizer: RobotVisualizer, - model_input_order: dict[DataType, list[str]], + 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, model_input_order) + run_policy( + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, + ) start_policy_execution(data_manager, policy_state) @@ -338,7 +361,7 @@ def play_policy( policy: nc.policy, policy_state: PolicyState, visualizer: RobotVisualizer, - model_input_order: dict[DataType, list[str]], + input_embodiment_description: EmbodimentDescription, ) -> None: """Handle Play Policy button press to start/stop continuous policy execution.""" if not policy_state.get_continuous_play_active(): @@ -347,7 +370,11 @@ def play_policy( # Run policy to get prediction horizon success = run_policy( - data_manager, policy, policy_state, visualizer, model_input_order + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, ) if not success: print("āš ļø Failed to run policy") @@ -391,7 +418,7 @@ def policy_execution_thread( policy_state: PolicyState, robot_controller: PiperController, visualizer: RobotVisualizer, - model_input_order: dict[DataType, list[str]], + input_embodiment_description: EmbodimentDescription, ) -> None: """Policy execution thread.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE @@ -505,7 +532,7 @@ def policy_execution_thread( policy, policy_state, visualizer, - model_input_order, + input_embodiment_description, ) if not success: print("āš ļø Failed to run policy") @@ -687,6 +714,12 @@ def update_visualization( 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) @@ -704,13 +737,13 @@ def update_visualization( print("\nšŸ”§ Initializing Neuracore...") nc.login() nc.connect_robot( - robot_name="AgileX PiPER", + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False, ) - # Load policy - model_input_order = { + # Load policy (cross-embodiment + preprocessing; same pattern as example 6) + input_embodiment_description: EmbodimentDescription = { DataType.JOINT_POSITIONS: [ "joint2", "joint5", @@ -722,7 +755,7 @@ def update_visualization( DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } - model_output_order = { + output_embodiment_description: EmbodimentDescription = { DataType.JOINT_TARGET_POSITIONS: [ "joint2", "joint5", @@ -733,13 +766,16 @@ def update_visualization( ], DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } + input_preprocessing_config: PreprocessingConfiguration = { + DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], + } - print("\nšŸ“‹ Model input order:") - for data_type, names in model_input_order.items(): - print(f" {data_type.name}: {names}") - print("\nšŸ“‹ Model output order:") - for data_type, names in model_output_order.items(): - print(f" {data_type.name}: {names}") + 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( @@ -758,16 +794,20 @@ def update_visualization( policy = nc.policy( train_run_name=args.train_run_name, device="cuda", - model_input_order=model_input_order, - model_output_order=model_output_order, + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + input_preprocessing_config=input_preprocessing_config, + 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", - model_input_order=model_input_order, - model_output_order=model_output_order, + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + input_preprocessing_config=input_preprocessing_config, + robot_name=args.robot_name, ) print(" āœ“ Policy loaded successfully") @@ -873,7 +913,11 @@ def update_visualization( 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, model_input_order + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, ) ) visualizer.set_start_policy_execution_callback( @@ -881,12 +925,20 @@ def update_visualization( ) visualizer.set_run_and_start_policy_execution_callback( lambda: run_and_start_policy_execution( - data_manager, policy, policy_state, visualizer, model_input_order + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, ) ) visualizer.set_play_policy_callback( lambda: play_policy( - data_manager, policy, policy_state, visualizer, model_input_order + data_manager, + policy, + policy_state, + visualizer, + input_embodiment_description, ) ) # Set up execution mode dropdown callback to sync with PolicyState @@ -915,7 +967,7 @@ def update_visualization( policy_state, robot_controller, visualizer, - model_input_order, + input_embodiment_description, ), daemon=True, ) diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 4a029a6..68e60ae 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -17,10 +17,13 @@ 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, BatchedParallelGripperOpenAmountData, DataType, + EmbodimentDescription, ) # Add parent directory to path @@ -44,6 +47,12 @@ from piper_controller import PiperController +def _embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: + if isinstance(spec, dict): + return [spec[i] for i in sorted(spec)] + return list(spec) + + def convert_predictions_to_horizon_dict(predictions: dict) -> dict[str, list[float]]: """Convert predictions dict to horizon dict format.""" horizon: dict[str, list[float]] = {} @@ -194,6 +203,12 @@ def execute_horizon( 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, @@ -216,31 +231,31 @@ def execute_horizon( print("\nšŸ”§ Initializing Neuracore...") nc.login() nc.connect_robot( - robot_name="AgileX PiPER", + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False, ) - # Load policy - # NOTE: The model_output_order MUST match the exact order used during training - # This order is determined by the output_robot_data_spec in the training config. - # The order here should match the order in your training config's output_robot_data_spec. - model_input_order = { + # Load policy (cross-embodiment + preprocessing; same pattern as example 6) + input_embodiment_description: EmbodimentDescription = { DataType.JOINT_POSITIONS: JOINT_NAMES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } - model_output_order = { + output_embodiment_description: EmbodimentDescription = { DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } + input_preprocessing_config: PreprocessingConfiguration = { + DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], + } - print("\nšŸ“‹ Model input order:") - for data_type, names in model_input_order.items(): - print(f" {data_type.name}: {names}") - print("\nšŸ“‹ Model output order:") - for data_type, names in model_output_order.items(): - print(f" {data_type.name}: {names}") + 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( @@ -258,16 +273,20 @@ def execute_horizon( print(f"\nšŸ¤– Loading policy from training run: {args.train_run_name}...") policy = nc.policy( train_run_name=args.train_run_name, - model_input_order=model_input_order, - model_output_order=model_output_order, + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + input_preprocessing_config=input_preprocessing_config, + 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", - model_input_order=model_input_order, - model_output_order=model_output_order, + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + input_preprocessing_config=input_preprocessing_config, + robot_name=args.robot_name, ) print(" āœ“ Policy loaded successfully") diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index a780d71..287d7d3 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -15,12 +15,17 @@ import numpy as np import viser import yourdfpy +from neuracore.core.utils.robot_data_spec_utils import ( + merge_cross_embodiment_description, +) +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, - RobotDataSpec, + EmbodimentDescription, ) from PIL import Image from viser.extras import ViserUrdf @@ -36,8 +41,6 @@ URDF_PATH, ) -MODEL_JOINT_NAMES = ["joint6", "joint4", "joint5", "joint2", "joint1", "joint3"] - # Parse arguments parser = argparse.ArgumentParser( description="Visualize policy predictions from dataset" @@ -62,24 +65,34 @@ default=POLICY_EXECUTION_RATE, help="Frequency of visualization", ) +parser.add_argument( + "--robot-name", + type=str, + default="AgileX PiPER", + help="Name of the robot to use", +) args = parser.parse_args() # Connect to Neuracore print("šŸ”§ Initializing Neuracore...") nc.login() -nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False) +nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) -# Load policy -model_input_order = { - # DataType.JOINT_POSITIONS: MODEL_JOINT_NAMES, - # DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], +input_embodiment_description: EmbodimentDescription = { + DataType.JOINT_POSITIONS: JOINT_NAMES, + DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], DataType.RGB_IMAGES: [CAMERA_NAMES[0]], } -model_output_order = { - DataType.JOINT_TARGET_POSITIONS: MODEL_JOINT_NAMES, +output_embodiment_description: EmbodimentDescription = { + DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], } + +input_preprocessing_config: PreprocessingConfiguration = { + DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], +} + if args.remote_endpoint_name: print(f"šŸ¤– Connecting to remote policy endpoint: {args.remote_endpoint_name}...") try: @@ -95,16 +108,20 @@ policy = nc.policy( train_run_name=args.train_run_name, device="cuda", - model_input_order=model_input_order, - model_output_order=model_output_order, + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + input_preprocessing_config=input_preprocessing_config, + 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", - model_input_order=model_input_order, - model_output_order=model_output_order, + input_embodiment_description=input_embodiment_description, + output_embodiment_description=output_embodiment_description, + input_preprocessing_config=input_preprocessing_config, + robot_name=args.robot_name, ) print(" āœ“ Policy loaded") @@ -113,23 +130,17 @@ dataset = nc.get_dataset(args.dataset_name) print(f" āœ“ Dataset loaded: {len(dataset)} episodes") -# Get data types from model input and output -required_data_types = set(model_input_order.keys()) | set(model_output_order.keys()) - -# Filter data spec to only include required data types -robot_data_spec: RobotDataSpec = { - robot_id: { - data_type: names - for data_type, names in dataset.get_full_data_spec(robot_id).items() - if data_type in required_data_types - } - for robot_id in dataset.robot_ids -} +input_cross_embodiment_description = {args.robot_name: input_embodiment_description} +output_cross_embodiment_description = {args.robot_name: output_embodiment_description} +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, - robot_data_spec=robot_data_spec, + cross_embodiment_union=cross_embodiment_union, prefetch_videos=True, max_prefetch_workers=2, ) @@ -251,9 +262,8 @@ def select_random_state() -> None: joint_positions = np.array([current_horizon[jn][0] for jn in JOINT_NAMES]) urdf_vis.update_cfg(joint_positions) - print( - f"āœ… Prediction received: {len(current_horizon.get(MODEL_JOINT_NAMES[0], []))} actions" - ) + horizon_len = len(current_horizon.get(JOINT_NAMES[0], [])) + print(f"āœ… Prediction received: {horizon_len} actions") # Add button From 069d28947cc01379a4d8c67a0390e86b47acbc25 Mon Sep 17 00:00:00 2001 From: mark Date: Wed, 20 May 2026 15:10:12 +0100 Subject: [PATCH 11/31] feat:fix preproc calls and cross embodiment --- examples/4_rollout_neuracore_policy.py | 216 ++++------------- .../5_rollout_neuracore_policy_minimal.py | 170 ++++---------- examples/6_visualize_policy_from_dataset.py | 217 ++++++------------ examples/common/policy_helpers.py | 214 +++++++++++++++++ 4 files changed, 386 insertions(+), 431 deletions(-) create mode 100644 examples/common/policy_helpers.py diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 04bbcdb..6ceb550 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -15,15 +15,7 @@ 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, -) +from neuracore_types import DataType, EmbodimentDescription # Add parent directory to path to import pink_ik_solver and piper_controller sys.path.insert(0, str(Path(__file__).parent.parent)) @@ -38,7 +30,6 @@ DAMPING_COST, FRAME_TASK_GAIN, GRIPPER_FRAME_NAME, - GRIPPER_NAME, IK_SOLVER_RATE, JOINT_NAMES, JOINT_STATE_STREAMING_RATE, @@ -59,6 +50,15 @@ VISUALIZATION_RATE, ) 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.robot_visualizer import RobotVisualizer from common.threads.ik_solver import ik_solver_thread @@ -71,38 +71,6 @@ 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.""" - 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: - 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, @@ -155,62 +123,18 @@ def run_policy( """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 not log_robot_state_for_policy(data_manager, input_embodiment_description): + print("āœ— No data available to run policy") + return False + rgb_image = None if DataType.RGB_IMAGES in input_embodiment_description: - rgb_names = _embodiment_names_ordered( + 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 + current_joint_angles = data_manager.get_current_joint_angles() # Get policy prediction try: @@ -419,6 +343,7 @@ def policy_execution_thread( robot_controller: PiperController, visualizer: RobotVisualizer, input_embodiment_description: EmbodimentDescription, + output_gripper_names: list[str] | None, ) -> None: """Policy execution thread.""" dt_execution = 1.0 / POLICY_EXECUTION_RATE @@ -457,20 +382,13 @@ def policy_execution_thread( 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 + previous_joint_target_positions_deg = ( + joint_targets_deg_at_index( + locked_horizon, execution_index - 1 + ) ) + if previous_joint_target_positions_deg is None: + break joint_errors = np.abs( current_joint_angles - previous_joint_target_positions_deg ) @@ -478,23 +396,13 @@ def policy_execution_thread( 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 + current_joint_target_positions_deg = joint_targets_deg_at_index( + locked_horizon, execution_index + ) + if current_joint_target_positions_deg is not None: 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 @@ -504,14 +412,13 @@ def policy_execution_thread( 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 - ) + gripper_target = gripper_open_at_index( + locked_horizon, + execution_index, + gripper_names=output_gripper_names, + ) + if gripper_target is not None: + robot_controller.set_gripper_open_value(gripper_target) # Update execution index policy_state.increment_execution_action_index() @@ -742,41 +649,6 @@ def update_visualization( overwrite=False, ) - # Load policy (cross-embodiment + preprocessing; same pattern as example 6) - input_embodiment_description: EmbodimentDescription = { - DataType.JOINT_POSITIONS: [ - "joint2", - "joint5", - "joint4", - "joint3", - "joint1", - "joint6", - ], - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], - DataType.RGB_IMAGES: [CAMERA_NAMES[0]], - } - output_embodiment_description: EmbodimentDescription = { - DataType.JOINT_TARGET_POSITIONS: [ - "joint2", - "joint5", - "joint4", - "joint3", - "joint1", - "joint6", - ], - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [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}..." @@ -794,9 +666,6 @@ def update_visualization( policy = nc.policy( train_run_name=args.train_run_name, device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, robot_name=args.robot_name, ) else: @@ -804,12 +673,22 @@ def update_visualization( policy = nc.policy( model_file=args.model_path, device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, 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 policy state policy_state = PolicyState() @@ -968,6 +847,7 @@ def update_visualization( robot_controller, visualizer, input_embodiment_description, + output_gripper_names, ), daemon=True, ) diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 68e60ae..132cde6 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -16,23 +16,11 @@ 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, - BatchedParallelGripperOpenAmountData, - DataType, - EmbodimentDescription, -) # Add parent directory to path sys.path.insert(0, str(Path(__file__).parent.parent)) from common.configs import ( - CAMERA_NAMES, - GRIPPER_NAME, - JOINT_NAMES, NEUTRAL_JOINT_ANGLES, POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO, @@ -40,89 +28,39 @@ 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 _embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]: - if isinstance(spec, dict): - return [spec[i] for i in sorted(spec)] - return list(spec) - - -def convert_predictions_to_horizon_dict(predictions: dict) -> dict[str, list[float]]: - """Convert predictions dict to horizon dict format.""" - horizon: dict[str, list[float]] = {} - - # Extract joint target positions - 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): - # Extract values: (B, T, 1) -> list[float], taking B=0 - 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: - gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] - if GRIPPER_NAME in gripper_data: - batched = gripper_data[GRIPPER_NAME] - if isinstance(batched, BatchedParallelGripperOpenAmountData): - # Extract values: (B, T, 1) -> list[float], taking B=0 - values = batched.open_amount[0, :, 0].cpu().numpy().tolist() - horizon[GRIPPER_NAME] = values - - return horizon - - -def log_current_state(data_manager: DataManager) -> None: - """Log current state to Neuracore. Reporter: build payload (e.g. joints in radians), log as-is.""" - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is None: - print("āš ļø No joint angles available") - return - - gripper_open_value = data_manager.get_current_gripper_open_value() - if gripper_open_value is None: - print("āš ļø No gripper open value available") - return - - # Reporter: convert to radians for Neuracore - joint_angles_rad = np.radians(current_joint_angles) - joint_positions_dict = { - joint_name: float(angle) - for joint_name, angle in zip(JOINT_NAMES, joint_angles_rad) - } - nc.log_joint_positions(joint_positions_dict) - nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_open_value) - - # Log all available cameras as-is - for camera_name in CAMERA_NAMES: - rgb_image = data_manager.get_rgb_image(camera_name) - if rgb_image is not None: - nc.log_rgb(camera_name, rgb_image) - - def run_policy( data_manager: DataManager, policy: nc.policy, policy_state: PolicyState, + input_embodiment_description: EmbodimentDescription, ) -> bool: """Run policy and get prediction horizon.""" - # Log current state - log_current_state(data_manager) + 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_dict(predictions) + prediction_horizon = convert_predictions_to_horizon(predictions) elapsed = time.time() - start_time # Get horizon length from the first joint (all should have same length) @@ -143,6 +81,8 @@ def execute_horizon( 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() @@ -155,23 +95,17 @@ def execute_horizon( for i in range(horizon_length): start_time = time.time() - # 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][i] for joint_name in JOINT_NAMES] - ) - current_joint_target_positions_deg = np.degrees( - current_joint_target_positions_rad - ) - robot_controller.set_target_joint_angles(current_joint_target_positions_deg) + 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) - # Send current gripper open value to robot (if available) - if GRIPPER_NAME in locked_horizon: - current_gripper_target_open_value = locked_horizon[GRIPPER_NAME][i] - robot_controller.set_gripper_open_value(current_gripper_target_open_value) + 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 current state for visualization - log_current_state(data_manager) + log_robot_state_for_policy(data_manager, input_embodiment_description) # Sleep to maintain rate elapsed = time.time() - start_time @@ -236,27 +170,6 @@ def execute_horizon( overwrite=False, ) - # Load policy (cross-embodiment + preprocessing; same pattern as example 6) - input_embodiment_description: EmbodimentDescription = { - DataType.JOINT_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], - DataType.RGB_IMAGES: [CAMERA_NAMES[0]], - } - output_embodiment_description: EmbodimentDescription = { - DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [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}..." @@ -273,9 +186,7 @@ def execute_horizon( print(f"\nšŸ¤– Loading policy from training run: {args.train_run_name}...") policy = nc.policy( train_run_name=args.train_run_name, - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, + device="cuda", robot_name=args.robot_name, ) else: @@ -283,12 +194,22 @@ def execute_horizon( policy = nc.policy( model_file=args.model_path, device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, 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() @@ -352,14 +273,21 @@ def execute_horizon( while True: # Run policy - if not run_policy(data_manager, policy, policy_state): + 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 + data_manager, + policy_state, + robot_controller, + args.frequency, + input_embodiment_description, + output_gripper_names, ) except KeyboardInterrupt: diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index 287d7d3..64d7f7e 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -9,6 +9,7 @@ import random import sys import time +import traceback from pathlib import Path import neuracore as nc @@ -18,27 +19,24 @@ from neuracore.core.utils.robot_data_spec_utils import ( merge_cross_embodiment_description, ) -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, -) +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 ( - CAMERA_NAMES, - GRIPPER_NAME, - JOINT_NAMES, - POLICY_EXECUTION_RATE, - URDF_PATH, +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 @@ -59,18 +57,18 @@ 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", ) -parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Name of the robot to use", -) args = parser.parse_args() # Connect to Neuracore @@ -78,21 +76,6 @@ nc.login() nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False) -input_embodiment_description: EmbodimentDescription = { - DataType.JOINT_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_NAME], - DataType.RGB_IMAGES: [CAMERA_NAMES[0]], -} -output_embodiment_description: EmbodimentDescription = { - DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES, - DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_NAME], -} - - -input_preprocessing_config: PreprocessingConfiguration = { - DataType.RGB_IMAGES: [ResizePad(size=(224, 224))], -} - if args.remote_endpoint_name: print(f"šŸ¤– Connecting to remote policy endpoint: {args.remote_endpoint_name}...") try: @@ -108,9 +91,6 @@ policy = nc.policy( train_run_name=args.train_run_name, device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, robot_name=args.robot_name, ) else: @@ -118,20 +98,36 @@ policy = nc.policy( model_file=args.model_path, device="cuda", - input_embodiment_description=input_embodiment_description, - output_embodiment_description=output_embodiment_description, - input_preprocessing_config=input_preprocessing_config, 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") -input_cross_embodiment_description = {args.robot_name: input_embodiment_description} -output_cross_embodiment_description = {args.robot_name: output_embodiment_description} +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, @@ -163,34 +159,10 @@ rgb_gui_handle = None -def convert_predictions_to_horizon( - predictions: dict[DataType, dict[str, BatchedNCData]], -) -> dict[str, list[float]]: - """Convert predictions to horizon dict.""" - 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 - if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions: - 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 select_random_state() -> None: """Select random state and run policy.""" global current_horizon, current_action_idx, playing, rgb_gui_handle - # Select random episode and step episode_idx = random.randint(0, len(synced_dataset) - 1) episode = synced_dataset[episode_idx] if len(episode) == 0: @@ -201,31 +173,14 @@ def select_random_state() -> None: step = episode[step_idx] print(f"šŸ“Š Selected episode {episode_idx}, step {step_idx}") - # Extract joint positions - joint_positions_dict = {} - if DataType.JOINT_POSITIONS in step.data: - joint_data = step.data[DataType.JOINT_POSITIONS] - for joint_name in JOINT_NAMES: - if joint_name in joint_data: - joint_positions_dict[joint_name] = joint_data[joint_name].value - # Log to Neuracore for visualization - nc.log_joint_positions(joint_positions_dict) - - # Extract gripper - gripper_value = 1.0 - if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in step.data: - gripper_data = step.data[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS] - if GRIPPER_NAME in gripper_data: - gripper_value = gripper_data[GRIPPER_NAME].open_amount - # Log to Neuracore for visualization - nc.log_parallel_gripper_open_amount(GRIPPER_NAME, gripper_value) - - # Extract RGB image - rgb_image = None - if DataType.RGB_IMAGES in step.data: - rgb_data = step.data[DataType.RGB_IMAGES] - if CAMERA_NAMES[0] in rgb_data: - rgb_image = np.array(rgb_data[CAMERA_NAMES[0]].frame) + 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") @@ -238,17 +193,14 @@ def select_random_state() -> None: ) else: rgb_gui_handle.image = rgb_image - # Log to Neuracore for visualization - nc.log_rgb(CAMERA_NAMES[0], rgb_image) - # Get policy prediction + 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}") - import traceback - traceback.print_exc() return duration = time.time() - start_time @@ -257,13 +209,11 @@ def select_random_state() -> None: playing = True print(f"FINISHED PREDICTION in {duration:.3f} s") - # Update robot to initial pose from first step in the horizon + joint_cfg = urdf_cfg_from_horizon(current_horizon or {}, 0) + if joint_cfg is not None: + urdf_vis.update_cfg(joint_cfg) - joint_positions = np.array([current_horizon[jn][0] for jn in JOINT_NAMES]) - urdf_vis.update_cfg(joint_positions) - - horizon_len = len(current_horizon.get(JOINT_NAMES[0], [])) - print(f"āœ… Prediction received: {horizon_len} actions") + print(f"āœ… Prediction received: {horizon_length(current_horizon or {})} actions") # Add button @@ -277,7 +227,7 @@ def select_random_state() -> None: max=1.0, step=0.01, initial_value=0.0, - disabled=True, # Read-only + disabled=True, ) # Add frequency control @@ -289,50 +239,33 @@ def select_random_state() -> None: step=1.0, ) -# Select initial state select_random_state() -# Main loop try: while True: start_time = time.time() - # Update robot visualization - if ( - playing - and current_horizon - and len(current_horizon.get(JOINT_NAMES[0], [])) > 0 - ): - horizon_length = len(current_horizon[JOINT_NAMES[0]]) - if current_action_idx < horizon_length: - # Get current action - joint_config = np.array( - [ - current_horizon[joint_name][current_action_idx] - for joint_name in JOINT_NAMES - ] + 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, ) - urdf_vis.update_cfg(joint_config) - - # Log to Neuracore for visualization - # NOTE: we log to joint positions instead of joint target positions - # because the latter is not visualized by Neuracore - joint_config_dict = { - jn: joint_config[i] for i, jn in enumerate(JOINT_NAMES) - } - nc.log_joint_positions(joint_config_dict) - - # Update gripper value - gripper_value = current_horizon[GRIPPER_NAME][current_action_idx] - gripper_handle.value = round( - gripper_value, 2 - ) # Round to 2 decimal places - - # Advance to next action - current_action_idx = (current_action_idx + 1) % horizon_length - - # Sleep to control update rate + 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) # Avoid division by zero + frequency = max(frequency_handle.value, 0.1) time.sleep(max(0, 1.0 / frequency - elapsed)) except KeyboardInterrupt: diff --git a/examples/common/policy_helpers.py b/examples/common/policy_helpers.py new file mode 100644 index 0000000..a969105 --- /dev/null +++ b/examples/common/policy_helpers.py @@ -0,0 +1,214 @@ +"""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) From 895900c75f135c71377d17d2f5fe3b7d9cee9bbd Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 20 May 2026 15:46:55 +0100 Subject: [PATCH 12/31] 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 13/31] 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 14/31] 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 15/31] 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 16/31] 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 d43ff9050f9a4bda405fd5b65f085fab863328a0 Mon Sep 17 00:00:00 2001 From: mark Date: Tue, 26 May 2026 15:47:28 +0100 Subject: [PATCH 17/31] fix: fixed target gripper dropping from recordings --- examples/common/threads/joint_state.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/examples/common/threads/joint_state.py b/examples/common/threads/joint_state.py index 322e37b..087f0c1 100644 --- a/examples/common/threads/joint_state.py +++ b/examples/common/threads/joint_state.py @@ -53,12 +53,18 @@ def joint_state_thread( 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 + if data_manager.get_teleop_active(): + target_gripper_open_value = max(0.0, min(1.0, 1.0 - trigger_value)) + robot_controller.set_gripper_open_value(target_gripper_open_value) + elif gripper_open_value is not None: + target_gripper_open_value = gripper_open_value + else: + target_gripper_open_value = None + + if target_gripper_open_value is not None: 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 From fc6ce076b4987162c4e7bafb1c5d2a9c9bf2e4b3 Mon Sep 17 00:00:00 2001 From: Halid Date: Wed, 20 May 2026 15:46:55 +0100 Subject: [PATCH 18/31] 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 19/31] 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 20/31] 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 21/31] 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 22/31] 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 23/31] 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 24/31] 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 25/31] 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 26/31] 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 27/31] 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 28/31] 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 29/31] 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 30/31] 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 31/31] 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 ```